You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
253 lines
9.8 KiB
C++
253 lines
9.8 KiB
C++
/*!
|
|
* \file StereoCalibrator.h
|
|
* \date 2020/03/13 9:40
|
|
*
|
|
* \author lucas.ma
|
|
* Contact: lucas.ma@hzleaper.com
|
|
*
|
|
* TODO: null
|
|
*
|
|
* \note
|
|
*/
|
|
#ifndef _StereoCalibrator_H__
|
|
#define _StereoCalibrator_H__
|
|
#include "StdUtils.h"
|
|
#include "CVUtils.h"
|
|
#include "CyclopsEnums.h"
|
|
#include "CyclopsModules.h"
|
|
#include <memory>
|
|
#include <unordered_map>
|
|
#include"CameraCalibrator.h"
|
|
|
|
class StereoCalibrator : public CameraCalibrator
|
|
{
|
|
public:
|
|
using MultiImgPnts = vector<Point2fVec>;
|
|
using MultiWorldPnts = vector<Point3fVec>;
|
|
|
|
enum ErrorCode
|
|
{
|
|
NoError = 1,
|
|
// negative values for error
|
|
ErrTooFewWorldPoints = -1,
|
|
ErrUnequalImageWorldPoints = -2,
|
|
ErrFailDetectImagePoints = -3,
|
|
ErrFailCalibration = -4,
|
|
ErrUnexpect = -5,
|
|
|
|
ErrUnequalLRImagePoints = -6,
|
|
ErrUnequalLRWorldPoints = -7,
|
|
ErrFailDetectLeftImagePoints = -8,
|
|
ErrFailDetectEightImagePoints = -9,
|
|
ErrFailStereoCalibration = -10,
|
|
ErrFailDetectLRInstance = -11,
|
|
ErrFailUpdateCameraMatAndCalibration = -12
|
|
};
|
|
|
|
enum CamCalibrator
|
|
{
|
|
LeftCamCalibrator,
|
|
RightCamCalibrator
|
|
};
|
|
|
|
|
|
/*! \fn setUpdateL
|
|
*description: set value to decide if update camera matrix & distcoeff matrix of left camera
|
|
* \fn getUpdateL
|
|
*description: get value to check if left camera matrix & distcoeff matrix have been updated
|
|
*/
|
|
DECLARE_PARAMETER(bool, UpdateL)
|
|
/*! \fn setUpdateR
|
|
*description: set value to decide if update camera matrix & distcoeff matrix of left camera
|
|
* \fn getUpdateR
|
|
*description: get value to check if right camera matrix & distcoeff matrix have been updated
|
|
*/
|
|
DECLARE_PARAMETER(bool, UpdateR)
|
|
/*! \fn setCellSizeL
|
|
*description: set cell size for left camera
|
|
* \fn getCellSizeL
|
|
*description: get cell size from left camera
|
|
*/
|
|
DECLARE_PARAMETER(double, CellSizeL)
|
|
/*! \fn setCellSizeR
|
|
*description: set cell size for right camera
|
|
* \fn getCellSizeR
|
|
*description: get cell size from right camera
|
|
*/
|
|
DECLARE_PARAMETER(double, CellSizeR)
|
|
/*! \fn setFocalLengthL
|
|
*description: set focal length for left camera
|
|
* \fn getFocalLengthL
|
|
*description: get focal length from left camera
|
|
*/
|
|
DECLARE_PARAMETER(double, FocalLengthL)
|
|
|
|
/*! \fn setFocalLengthR
|
|
*description: set focal length for right camera
|
|
* \fn getFocalLengthR
|
|
*description: get focal length from right camera
|
|
*/
|
|
DECLARE_PARAMETER(double, FocalLengthR)
|
|
|
|
|
|
void setImageSize(cv::Size size) {
|
|
mImgSize = size;
|
|
}
|
|
|
|
public:
|
|
|
|
/*! \fn serializeToMemory
|
|
* Serialize the camera calibrator (including calibration and distortion matrixes) into a in-memory string, see also deserializeFromMemory()
|
|
* @param str used to take the output serialization result
|
|
* @return true for succeed, false for fail
|
|
* \fn serializeToFile
|
|
* Serialize the camera calibrator (including calibration and distortion matrixes) into a text file, see also deserializeFromFile()
|
|
* @param filename file name (full path) where we will write the data
|
|
* @return true for succeed, false for fail
|
|
* \fn deserializeFromMemory
|
|
* Deserialize the camera calibrator from in-memory string, see also serializeToMemory()
|
|
* @param str in-memory string
|
|
* @return true for succeed, false for fail
|
|
* \fn deserializeFromFile
|
|
* Deserialize the camera calibrator from a text file, see also serializeToFile()
|
|
* @param filename file name (full path) where we will read the data
|
|
* @return true for succeed, false for fail
|
|
*/
|
|
DECL_SERIALIZE_FUNCS
|
|
/*! Run calibration with provided images. This function is used for creating relative R &T as well as camera matrix & distort matrix of both of left camera and right camera. It should be noticed that calibrated left and right camera instances should be first passed (see setCameraCalibrator) before calibrating so that camera matrix can be well received from both cameras. Be careful use it since it could update the camera matrix & distort matrix after calibrating, which if has already been calibrated before. Set if update or not before use (see func setUpdateL & setUpdateR)
|
|
* @param image group from left camera
|
|
* @param image group from right camera
|
|
* @return calibration 1 for succeed, or error code if it fails
|
|
*/
|
|
virtual int stereo3dCalibrate(const vector<Mat>& imgLeftVec, const vector<Mat>& imgRightVec);
|
|
|
|
/*! Run calibration with provided image points. This function is used for creating relative R &T as well as camera matrix & distort matrix of both of left camera and right camera. It should be noticed that calibrated left and right camera instances should be first passed (see setCameraCalibrator) before calibrating so that images points can be well received. Be careful use it since it could update the camera matrix & distort matrix after calibrating, which if has already been calibrated before. Set if update or not before use (see func setUpdateL & setUpdateR)
|
|
* @return calibration 1 for succeed, or error code if it fails
|
|
*/
|
|
virtual int stereo3dCalibrate();
|
|
|
|
/*! Run calibration with provided images. This function is used for creating relative R &T as well as camera matrix & distort matrix of both of left camera and right camera. It should be noticed that calibrated left and right camera instances should be first passed (see setCameraCalibrator) before calibrating so that camera matrix can be well received from both cameras. Be careful use it since it could update the camera matrix & distort matrix after calibrating, which if has already been calibrated before. Set if update or not before use (see func setUpdateL & setUpdateR)
|
|
* @param image from left camera
|
|
* @param image from right camera
|
|
* @return calibration 1 for succeed, or error code if it fails
|
|
*/
|
|
virtual int stereo3dCalibrate(const Mat& imgLeft, const Mat& imgRight);
|
|
|
|
/*! Run setCameraCalibrator with provided left and right cameras instances. This function is used for receiving camera matrix, distCoeff matrix etc that could be used during calibration. It should be noticed that both cameras should be calibrated before
|
|
* @param instance from left camera
|
|
* @param instance from right camera
|
|
* @return 1 for succeed, or error code if it fails
|
|
*/
|
|
virtual void setCameraCalibrator(CameraCalibrator::Ptr camL, CameraCalibrator::Ptr camR);
|
|
|
|
/*! Run mapToWorld with a pair of provided image points. This function is used for mapping image points from 2d to 3d
|
|
* @param a image point from left camera
|
|
* @param a image point from right camera
|
|
* @return a 3d point
|
|
*/
|
|
virtual Point3f mapToWorld(
|
|
const Point2f& leftPnt,
|
|
const Point2f&rightPnt);
|
|
|
|
/*! Run mapToWorld with provided left and right image points. This function is used for mapping image points from 2d to 3d
|
|
* @param image points from left camera
|
|
* @param image points from right camera
|
|
* @return 3d points
|
|
*/
|
|
virtual Point3fVec mapToWorld(
|
|
const Point2fVec& leftPnts,
|
|
const Point2fVec& rightPnts);
|
|
|
|
/*! Run mapFromWorld with a provided 3d point. This function is used for mapping a 3d world point to 2d with its corresponding left and right camera
|
|
* @param a image point from left camera
|
|
* @param a image point from right camera
|
|
* @return 3d points
|
|
*/
|
|
virtual bool mapFromWorld(
|
|
const Point3f& worldPoint,
|
|
Point2f& pntLeft, Point2f& pntRight);
|
|
|
|
/*! Run mapFromWorld with provided 3d points. This function is used for mapping 3d world points to 2d with its corresponding left and right camera
|
|
* @param image points from left camera
|
|
* @param image points from right camera
|
|
* @return true if success otherwise false
|
|
*/
|
|
virtual bool mapFromWorld(
|
|
const Point3fVec& worldPoint,
|
|
Point2fVec& pntLeftVec, Point2fVec& pntRightVec);
|
|
|
|
/*! Run addMuiltFrame with a pair of provided images. This function is used for detecting and recording corner points from a pair of provided images with grid and calibration mode setting
|
|
* @param image from left camera
|
|
* @param image from right camera
|
|
* @return 1 success otherwise error code
|
|
*/
|
|
virtual int addMuiltFrame(const Mat& leftMat, const Mat& rightMat);
|
|
virtual int getFrameCount();
|
|
|
|
/*! Run addAssistPnt with provided assistant image points from multi-frames. This function is used for adding detected corner points as stereo assistant points.
|
|
* @param assistant points from left camera
|
|
* @param assistant points from right camera
|
|
* @return 1 success otherwise 0
|
|
*/
|
|
virtual int addAssistPnt(const MultiImgPnts& assLeftPnt,
|
|
const MultiImgPnts& assRightPnt, const MultiWorldPnts& assWorldPnt);
|
|
/*! Clear calibration result */
|
|
virtual void clear(bool clearAllFrame = false);
|
|
|
|
/*! Get error message when calibration failed */
|
|
virtual std::string getErrorStr(int errorCode) const;
|
|
|
|
typedef std::shared_ptr<StereoCalibrator> Ptr;
|
|
DECL_GET_INSTANCE(StereoCalibrator::Ptr);
|
|
|
|
|
|
StereoCalibrator(CalibModel calibModel = CalibModel::Chessboard) :CameraCalibrator(calibModel), mUpdateL(false),
|
|
mUpdateR(false),mCellSizeL(0.), mCellSizeR(0.), mFocalLengthL(0.),
|
|
mFocalLengthR(0.){}
|
|
|
|
virtual ~StereoCalibrator() {};
|
|
protected:
|
|
void matrixCopy(CameraCalibrator::Ptr pCamL, CameraCalibrator::Ptr pCamR
|
|
);
|
|
private:
|
|
bool _doStereoCalibration(
|
|
const MultiWorldPnts & worldPntsVec,
|
|
const MultiImgPnts & imgPntsVecL,
|
|
const MultiImgPnts & imgPntsVecR,
|
|
Mat & camMatL, Mat &distCoeffMatL,
|
|
Mat & camMatR, Mat & distCoeffMatR,
|
|
cv::Size size,
|
|
Mat & R, Mat & T,
|
|
Mat & E, Mat & F,
|
|
Mat & perViewErrors,
|
|
double & rmsErr,
|
|
int flag = CALIB_USE_INTRINSIC_GUESS
|
|
);
|
|
|
|
bool doStereoCalibration(
|
|
const MultiWorldPnts & worldPntsVec,
|
|
const MultiImgPnts & imgPntsVecL,
|
|
const MultiImgPnts & imgPntsVecR
|
|
);
|
|
|
|
void createProjection();
|
|
virtual bool serialize(FileStorage& fs);
|
|
virtual bool deserialize(const FileNode& fs);
|
|
CameraCalibrator::Ptr m_pCamL;
|
|
CameraCalibrator::Ptr m_pCamR;
|
|
Mat mCameraMatrixL, mCameraMatrixR;
|
|
Mat mDistCoeffL, mDistCoeffR;
|
|
Mat mRelativeR, mRelativeT, mE, mF, mPerViewErrorMat;
|
|
Mat mProjMatL, mProjMatR;
|
|
Mat mRL, mRR, mTL, mTR;
|
|
MultiImgPnts mLeftImgPnts, mRightImgPnts;
|
|
MultiWorldPnts mWorldPnts;
|
|
MultiWorldPnts mAssistWorldPnts;
|
|
MultiImgPnts mAssistLeftPnts, mAssisRightPnts;
|
|
Mat mRefRMat, mRefTMat;
|
|
};
|
|
|
|
|
|
|
|
#endif // _StereoCalibrator_H__
|