newValue/3part/Cyclops/include/CameraCalibrator.h

/*!
 * \file CameraCalibrator.h
 * \date 2018/09/04
 *
 * \author Lin, Chi
 * Contact: lin.chi@hzleaper.com
 *
 *
 * \note
*/

#ifndef __CameraCalibrator_h_
#define __CameraCalibrator_h_

#include "StdUtils.h"
#include "CVUtils.h"
#include "CyclopsEnums.h"
#include "CyclopsModules.h"
#include <memory>
#include <unordered_map>
#include <opencv2/aruco/charuco.hpp>

/*! \brief Calibrate camera, find its intrinsic and extrinsic parameters
*
* By design, we assume the real world is on a flat plane. So the calibration could be a 3D-to-2D model as its common concept,
* or a 2D-to-2D model fixing the world Z value all o 0.
* We support several calibration model in this class, you could either manually set the corresponding image and world positions, using CalibModel::NPoints,
* or use grid calibration board, then we'll detect patterns (corners or circles) and use predefined world positions.
*
* It also support insufficient calibration using only a few points:
* a. (Not implemented yet) only 2 points, with specified assumption, like fix rotation, fix transform.
* b. 4 points, without lens distortion optimization
*
* Example:
* \code{.cpp}
* CameraCalibrator cc;
* double err = cc.calibrate(img, &worldPnts, &imgPnts);
* Mat undistortImg = cc.undistort(img); // get undistorted and transformed image
* 
* \\ do you detection on the undistorted image, like find a circle
* Point2f circleCenter;
* Point2f circleCenter_world = cc.mapFromWorld2D(circleCenter, CameraCalibrator::PixelStage::PerspectiveTransform);
*
* \\ if you prefer to do the detection on original image (don't want to introduce much interpolation to pixels)
* Point2f lineMidPointOnOriImg;
* Point2f lineMidPoint_world = cc.mapFromWorld2D(lineMidPointOnOriImg, CameraCalibrator::PixelStage::Original);
* \endcode
*
* \\ an experiment shows the accuracy of charuco is approaching
* ++++++++++++++++++++++++++++++++++++++++++++++
* +  method   ++  RMS    ++    error         +++
* +  Charuco  ++  0.460  ++    0.083/100mm   +++
* +  chess 1  ++  0.457  ++    0.0825/100mm  +++
* +  chess 2  ++  0.473  ++    0.015/100mm   +++
* ++++++++++++++++++++++++++++++++++++++++++++++
* chess 1 use the same board(13*8*20mm) as charuco, chess 2 use another board(130*80*2mm)
*
* See CameraCalibratorTest for unite test
*
*/
class StereoCalibrator;
class CameraCalibrator : public ICyclopsModuleInstance
{
public:
	/*! \fn getIsCalibrated
	* Get whether this calibrator is calibrated, call calibrate() if it's not.
	*/
	DECLARE_PARAMETER_GET(bool, IsCalibrated)
	/*! \fn setCalibModel
	* Define the calibration model, it could be some built-in model or NPoints
	* (then you have to manually setup the world positions), default to CalibModel::NPoints, see also getCalibModel()
	* \fn getCalibModel
	* Get the calibration model, see also setCalibModel()
	*/
	DECLARE_PARAMETER2(CalibModel, CalibModel, CalibModel::NPoints, CalibModel::Charuco)
	/*! \fn setCameraModel
	 * Define the camera model, default to CameraModel::Normal, see also getCameraModel()
	 * \fn getCameraModel
	 * Get the camera model, see also setCameraModel()
	 */
	DECLARE_PARAMETER2(CameraModel, CameraModel, CameraModel::Normal, CameraModel::FishEye)
	/*! \fn setDistortionModel
	 * Define the distortion model, default to DistortionModel::Default, see also getDistortionModel()
	 * \fn getDistortionModel
	 * Get value of the distortion model, see also setDistortionModel()
	 */
	DECLARE_PARAMETER2(DistortionModel, DistortionModel, DistortionModel::Default, DistortionModel::All)
	/*! \fn setSquareSize
	 * Define square size(mm) of calibration board, default to 10,
	 * Only works for CalibModel::Charuco or CalibModel::AsymGrid model
	 * see also getSquareSize()
	 * \fn getSquareSize
	 * Get square size(mm) of calibration board, see also setSquareSize()
	 */
	DECLARE_PARAMETER2(double, SquareSize, 0, 200)
	/*! \fn setMarkerSize
	 * Define marker size(mm) of calibration board, default to 8,
	 * Only works for CalibModel::Charuco model
	 * see also getMarkerSize()
	 * \fn getMarkerSize
	 * Get square size(mm) of calibration board, see also setSquareSize()
	*/
	DECLARE_PARAMETER2(double, MarkerSize, 0, 200)
	/*! \fn setGridWidth
	 * Only works for CalibModel::SymGrid or CalibModel::AsymGrid model
	 * Define how many square/circle/corners there is horizontally, default to 10, see also getGridWidth()
	 * \fn getGridWidth
	 * Get how many square/circle/corners there is horizontally, see also setGridWidth()
	 */
	DECLARE_PARAMETER(int, GridWidth)
	/*! \fn setGridHeight
	 * Only works for CalibModel::SymGrid or CalibModel::AsymGrid model
	 * Define how many square/circle/corners there is vertically, default to 7, see also getGridHeight()
	 * \fn getGridHeight
	 * Get how many square/circle/corners there is vertically, see also setGridHeight()
	 */
	DECLARE_PARAMETER(int, GridHeight)
	/*! \fn getRMSErr
	 * Get the overall RMS re-projection error.
	 */
	DECLARE_PARAMETER_GET(double, RMSErr)
	/*! \fn getPerspectiveScale
	 * Get value of scale when apply undistortion and perspective transform, see also setPerspectiveScaleShift()
	 */
	DECLARE_PARAMETER_GET(double, PerspectiveScale)
	/*! \fn getPerspectiveShift
	 * Get shift value when apply perspective transform, see also setPerspectiveScaleShift()
	 */
	DECLARE_PARAMETER_GET(Point2f, PerspectiveShift)
	/*! \fn getIs2D
	 * Get whether this is a 2D calibrated (without z value in world points)
	*/
	DECLARE_PARAMETER_GET(bool, Is2D)
	/*! \fn setPixelStage
	 * Define the level of undistorted image's pixels, see UndistortPixelStage,
	 * default to UndistortPixelStage::Origina, see also getPixelStage()
	 * \fn getPixelStage
	 * Get value of the level of undistorted image's pixels, see also setPixelStage()
	 */
	DECLARE_PARAMETER2(UndistortPixelStage, PixelStage, UndistortPixelStage::Original, UndistortPixelStage::PerspectiveTransform)
	/*! \fn getOriginPosition
	 * Get value of world position of origin, it's usually used for align several calibrated camera, defualt to (0, 0), see also setOriginPosition()
	 */
	DECLARE_PARAMETER_GET(Point3f, OriginPosition)
	/*! \fn getOriginAngle
	 * Get value of angle of world origin (around x, y, z axis), it's usually used for align several calibrated camera, default to 0, see also setOriginAngle()
	 */
	DECLARE_PARAMETER_GET(Point3f, OriginAngle)
	/*! \fn setTermCount
	 * Define the maximum iteration count to do calibration optimization, default to 30, see also getTermCount()
	 * \fn getTermCount
	 * Get value of the maximum iteration count to do calibration optimization, see also setTermCount()
	 */
	DECLARE_PARAMETER(int, TermCount)

	// Following parameters are from camera and len device, or physical metrics from working scenario,
	// we should move them to a CalibrationContext class for those extra informations that helps improve calibration optimization.

	/*! \fn setBoardThickness
	 * Define the thickness of calibration board, it will help compensate the calibration error due to mismatch of plane of calibration and detection.
	 * Works for calibration model except CalibModel::NPoints
	 * default to 0, see also getBoardThickness()
	 * \fn getBoardThickness
	 * Get value of the thickness of calibration board, see also setBoardThickness()
	 */
	DECLARE_PARAMETER(double, BoardThickness)
	/*! \fn setCellSize
	 * Define cell size of camera sensor(in μm), which is used in setting up initial value for calibration, default to 0.
	 * It should be available in datasheet of your device, we use same cell size for both x and y axis as in most cases they are same.
	 * see also getCellSize()
	 * \fn getCellSize
	 * Get value of cell size of camera sensor(in μm), which is used as initial value in calibration, see also setCellSize()
	 */
	DECLARE_PARAMETER(double, CellSize)
	/*! \fn setFocusLength
	 * Define focus length of camera sensor(in mm), which is used in setting up initial value for calibration, default to 0.
	 * It should be available in datasheet of your device, see also getFocusLength()
	 * \fn getFocusLength
	 * Get value of focus length of camera sensor, see also setFocusLength()
	 */
	DECLARE_PARAMETER(double, FocusLength)
	/*! \fn setZ
	 * Define distance from world plane to image plane(in mm), which is used in setting up initial value for calibration,, default to 0, see also getZ()
	 * \fn getZ
	 * Get value of distance from world plane to image plane, see also setZ()
	 */
	DECLARE_PARAMETER(double, Z)
	/*! \fn setUseLFSubPixCorner
	 * Define line-fitting for sub-pix chessboard corner detection, default to false, see also getUseLFSubPixCorner()
	 * \fn getUseLFSubPixCorner
	 * Get value of line-fitting for sub-pix chessboard corner detection, see also setUseLFSubPixCorner()
	 */
	DECLARE_PARAMETER(bool, UseLFSubPixCorner)
	/*! \fn setAutoRemoveBad
	 * Define whether to remove bad calibration point pair automatically, default to false, see also getAutoRemoveBad()
	 * \fn getAutoRemoveBad
	 * Get value of whether to remove bad calibration point pair automatically, see also setAutoRemoveBad()
	 */
	DECLARE_PARAMETER(bool, AutoRemoveBad)

public:
	CameraCalibrator(CalibModel calibModel) :
		mCalibModel(calibModel),
		mIsCalibrated(false), mCameraModel(CameraModel::Normal), mDistortionModel(DistortionModel::Default),
		mSquareSize(10), mMarkerSize(8), mGridWidth(10), mGridHeight(7), mBoardThickness(0),
		mRMSErr(-1), mPTransMat(Mat::eye(3, 3, CV_64FC1)), mPerspectiveScale(1.), mPerspectiveShift(0, 0), mIs2D(false), mNoDistort(false),
		mPixelStage(UndistortPixelStage::Original),
		mOriginPosition(0, 0, 0), mOriginAngle(0), mTermCount(30), mCellSize(0), mFocusLength(0), mZ(0),
		mUseLFSubPixCorner(false), mAutoRemoveBad(false)
	{}
	CameraCalibrator() : CameraCalibrator(CalibModel::NPoints) {}
	virtual ~CameraCalibrator() {}

	//! Smart pointer to hold an instance of CameraCalibrator
	typedef std::shared_ptr<CameraCalibrator> Ptr;
	DECL_GET_INSTANCE(CameraCalibrator::Ptr)

	enum ErrorCode {
		NoError = 1,
		// negative values for error
		ErrTooFewWorldPoints = -1,
		ErrUnequalImageWorldPoints = -2,
		ErrFailDetectImagePoints = -3,
		ErrFailCalibration = -4,
		ErrUnexpect = -5,
	};

	/*! Set calibration grid size, how many grid there's horizontally and vertically.
	 * Mean nothing if calibration mode is set to CalibModel::NPoints or CalibModel::Custom.
	 * @param w width
	 * @param h height
	*/
	void setGridSize(unsigned int w, unsigned int h) {
		mGridWidth = w; mGridHeight = h;
	}
	
	/*! Add one frame for multi-frame calibration.
	 * Do nothing if calibration mode is set to CalibModel::NPoints or CalibModel::Custom.
	 * We'll detect and record corner points from provided image with grid and calibration mode setting
	 * @param img image used for calibration
	 * @return NoError for succeed, or error code if fail
	*/
	virtual int addCalibrateFrame(const Mat& img);
	int getFrameCount() const {
		return mAssistImgPoints.size();
	}
	virtual bool removeAssistFrame(int idx);
	/*! Run calibration with provided image.
	 * If calibration mode is set to CalibModel::NPoints (Default),
	 * you need also provide the corresponding points in world and image coordinates;
	 * If it's in other mode (Grid calibration),
	 * we'll detect the pattern (either corner or circle) on image, and use predefined world positions
	 * @param img image used for calibration
	 *        for non-custom calibration, this image will be used as main plane
	 * @param worldPoints input and output, used as NPoints input world point positions,
	 *        and report the real positions used in algorithm
	 * @param imgPoints input and output, used as NPoints input image point positions,
	 *        and report the real positions used in algorithm
	 * @return NoError for succeed, or error code if fail
	*/
	virtual int calibrate(const Mat& img,
		Point3fVec* worldPoints = nullptr,
		Point2fVec* imgPoints = nullptr);

	/*! Run calibration with provided image. This is a more convenient function if you are calibrating 2D world positions
	 * @param img image used for calibration
	 *        for non-custom calibration, this image will be used as main plane
	 * @param worldPoints input and output, used as NPoints input world point positions,
	 *        and report the real positions used in algorithm
	 * @param imgPoints input and output, used as NPoints input image point positions,
	 *        and report the real positions used in algorithm
	 * @return NoError for succeed, or error code if fail
	*/
	virtual int calibrate2D(const Mat& img,
		Point2fVec* worldPoints = nullptr,
		Point2fVec* imgPoints = nullptr);

	/*! Run calibration with provided points, only for NPoints calibration.
	 * @param imgSize image size
	 * @param worldPoints input and output, used as NPoints input world point positions
	 * @param imgPoints input and output, used as NPoints input image point positions
	 * @return NoError for succeed, or error code if fail
	*/
	virtual int calibrate2D(const Size& imgSize,
		Point2fVec* worldPoints = nullptr,
		Point2fVec* imgPoints = nullptr);

	/*! Run calibration with provided numeric, only for Custom calibration.
	 * @param sx scale factor used for scaling numeric over x-axis
	 * @param sy scale factor used for scaling numeric over y-axis
	 * @param cx a shift value over x-axis for principal point 
	 * @param cy a shift value over y-axis for principal point
	 * @param theta an angle value for determining coordinate system
	*/
	virtual void calibrate2D(double sx, double sy,
		double cx, double cy, double theta);

	/*! Calibration adaptation for slight camera rotation/translation changes,
	    it may be caused by temperature changes, vibration, etc.
	 * @param imgPoints positions of marks in current image
	 * @param worldPoints known world positions of marks
	 * @param roi Roi
	 * @return true if adaptation succeed
	*/
	virtual bool adapt(const Point2fVec& imgPoints, const Point3fVec& worldPoints, Rect roi = Rect());

	/*! Reset adaptation, rotation/translation matrix are reset to original value */
	virtual void resetAdapt();

	/*! Evaluate the calibration result using grid broads, by comparing the distance of mapping result of image points
	    detected from the provided image and generated world points
	 * @param img image of calibration board
	 * @param err distance mis-matching range(relative error), -1 ~ 1, smaller range means better calibration
	 * @param skipNear skip evaluation of point nearby in 2 square
	 * @return NoError for succeed, or error code if fail
	*/
	virtual int evaluate(const Mat& img, Rangef& err, bool skipNear = true);

	/*! Transforms an image to compensate for lens distortion, and also for camera tilt if required
	 * @param img input image
	 * @param roi Roi
	 * @return the transformed image
	*/
	virtual Mat undistort(const Mat& img, Rect roi = Rect());

	/*! Map a point in image coordinates to world coordinates
	 * @param imgPoint position in image coordinates
	 * @param roi optional, set it if the image is in some roi, then the input position will be treated as located in that roi
	 * @param z optional, set it if the returned world points are required to be on z-planar, otherwise it's on the defined thickness-planar
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return 3D position in world coordinates
	*/
	virtual Point3f mapToWorld(const Point2f& imgPoint, Rect roi = Rect(), double z = DBL_MAX, bool postTrans = true);
	virtual Point3fVec mapToWorld(const Point2fVec& imgPoint, Rect roi = Rect(), double z = DBL_MAX, bool postTrans = true);

	/*! Mat a point in world coordinates to image coordinates
	 * @param worldPoint 3D position in world coordinates
	 * @param roi optional, set it if the image is in some roi, then the returned position will located in that roi
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return position in image coordinates
	*/
	virtual Point2f mapFromWorld(const Point3f& worldPoint, Rect roi = Rect(), bool postTrans = true);
	virtual Point2fVec mapFromWorld(const Point3fVec& worldPoint, Rect roi = Rect(), bool postTrans = true);

	/*! Map a point in image coordinates to world coordinates
	 * @param imgPoint position in image coordinates
	 * @param fromStage specify where the point is, aka. how the image is generated
	 * @param roi optional, set it if the image is in some roi, then the input position will be treated as located in that roi
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return 2D position in world coordinates
	*/
	virtual Point2f mapToWorld2D(const Point2f& imgPoint, Rect roi = Rect(), bool postTrans = true);
	virtual Point2fVec mapToWorld2D(const Point2fVec& imgPoint, Rect roi = Rect(), bool postTrans = true);

	/*! Map a point in world coordinates to image coordinates
	 * @param worldPoint 2D position in world coordinates
	 * @param roi optional, set it if the image is in some roi, then the returned position will located in that roi
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return position in image coordinates
	*/
	virtual Point2f mapFromWorld2D(const Point2f& worldPoint, Rect roi = Rect(), bool postTrans = true);
	virtual Point2fVec mapFromWorld2D(const Point2fVec& worldPoint, Rect roi = Rect(), bool postTrans = true);

	/*! Map a length (numeric) value in image coordinates to world coordinates.
	 * Note this function doesn't give accurate result when there's strong distortion or camera tilt.
	 * It's recommend to fix them in the very beginning before detection and inspection and transformation, via undistort().
	 * @param length numeric length in image coordinates
	 * @param anchorPnt the reference point to do a relatively accurate mapping
	 * @param roi optional, set it if the image is in some roi, then the input position will be treated as located in that roi
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return length in world coordinates
	*/
	virtual double mapLengthToWorld(const double& length, const Point2f& anchorPnt, Rect roi = Rect(), bool postTrans = true);
	virtual vector<double> mapLengthToWorld(const vector<double>& lengths, const Point2fVec& anchorPnts, Rect roi = Rect(), bool postTrans = true);
	
	/*! Map an area (numeric) value in image coordinates to world coordinates.
	 * Note this function doesn't give accurate result when there's strong distortion or camera tilt.
	 * It's recommend to fix them in the very beginning before detection and inspection and transformation, via undistort().
	 * @param area numeric area in image coordinates
	 * @param anchorPnt the reference point to do a relatively accurate mapping
	 * @param roi optional, set it if the image is in some roi, then the input position will be treated as located in that roi
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return area in world coordinates
	*/
	virtual double mapAreaToWorld(const double& area, const Point2f& anchorPnt, Rect roi = Rect(), bool postTrans = true);
	virtual vector<double> mapAreaToWorld(const vector<double>& areas, const Point2fVec& anchorPnts, Rect roi = Rect(), bool postTrans = true);

	/*! Map an angle (numeric) value in image coordinates to world coordinates.
	 * Note this function doesn't give accurate result when there's strong distortion or camera tilt.
	 * It's recommend to fix them in the very beginning before detection and inspection and transformation, via undistort().
	 * @param angle numeric angle in image coordinates
	 * @param anchorPnt the reference point to do a relatively accurate mapping
	 * @param roi optional, set it if the image is in some roi, then the input position will be treated as located in that roi
	 * @param postTrans optional, whether post-transform and origin point transform is applied in this mapping, default to true
	 * @return angle in world coordinates
	*/
	virtual double mapAngleToWorld(const double& angle, const Point2f& anchorPnt, Rect roi = Rect(), bool postTrans = true);
	virtual vector<double> mapAngleToWorld(const vector<double>& angles, const Point2fVec& anchorPnts, Rect roi = Rect(), bool postTrans = true);

	/*! \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

	/*! Clear calibration result */
	virtual void clear(bool clearAllFrame = false);

	/*! Define scale when apply undistortion and perspective transform, 0 to 10, default to 1 means no scale, see also getPerspectiveScale() */
	virtual void setPerspectiveScale(double s);

	/*! Define scale and additional shift when apply perspective transform*/
	virtual void setPerspectiveScaleShift(double s, Point2f shift);

	/*! Get mapping from image to world point in a descriptive string */
	virtual void getMappingInStr(std::string& str) const;

	/*! Get error message when calibration failed */
	virtual std::string getErrorStr(int errorCode) const;

	/*! Set world origin pose for 2D calibration */
	virtual void setOriginPose2D(const Point2f& position, double angle);

	/*! Set world origin pose for 3D calibration
	 *  Note: this and related functions are implemented by not tested, please test (and fix bugs) before use
	 */
	virtual void setOriginPose3D(const Point3f& position, const Point3f& angle);

	/*! Set world customized affine matrix for 2D calibration */
	virtual bool setCustomMatrix2D(const Mat& custMat);

	/*! Set world customized affine matrix for 3D calibration */
	virtual bool setCustomMatrix3D(const Mat& custMat);

	/*! Set preceding distortion calibrator */
	virtual bool setDistortCalibrator(CameraCalibrator::Ptr ccPtr);
	
	/*! Get preceding distortion calibrator */
	CameraCalibrator::Ptr getDistortCalibrator() const {
		return mDistortCalibPtr;
	}

	/*! Get image points used for calibration
	 * @param idx negative value for get image point on 0-planar, otherwise, for assistant planar
	*/
	Point2fVec getImgPnts(int idx = -1) const {
		if (idx < 0 || idx >= mAssistImgPoints.size()) return mImgPoints;
		else return mAssistImgPoints[idx];
	}

	/*! Get world points used for calibration
	 * @param idx negative value for get world point on 0-planar, otherwise, for assistant planar
	*/
	Point3fVec getWorldPnts(int idx = -1) const {
		if (idx < 0 || idx >= mAssistWorldPoints.size()) return mWorldPoints;
		else return mAssistWorldPoints[idx];
	}

	/*! Get image size */
	Size getImageSize() const { return mImgSize; }
	
protected:
	Mat mCameraMatrix;
	Mat mDistCoeffs;
	bool mNoDistort;
	Mat mTVec, mRVec, mInvTVec;
	Mat mTVecBackup, mRVecBackup;
	Mat mPTransMat;
	Mat mScaledCamearMatrix;
	Size mImgSize;
	Mat mRMat, mInvRMat;
	Mat mTransMat, mInvTransMat;
	Mat mCustMat;
	Mat mUndistrotMap;
	Point2fVec mImgPoints;
	Point3fVec mWorldPoints;
	std::vector<int> mCharucoIds;

	std::vector<Point2fVec> mAssistImgPoints;
	std::vector<Point3fVec> mAssistWorldPoints;

	std::string mErrorMsg;

	friend class StereoCalibrator;

	CameraCalibrator::Ptr mDistortCalibPtr;
	cv::Ptr<cv::aruco::CharucoBoard> mCharucoBoardPtr;
	void genGridBoardCornerPnts(Point3fVec& corners, const Point2fVec& imgPnts);
	void ensureCharucoBoard();
	void genCharucoCornerPnts(Point3fVec& corners);
	bool detectGridBoardImagePnts(const Mat& img, Point2fVec& corners);
	bool detectBoardPnts(const Mat& img, Point2fVec& imgPoints, Point3fVec& worldPoints, bool removeBad = false);
	bool detectCharucoBoardImagePnts(const Mat& img, Point2fVec& corners, std::vector<int>& cornerIds);
    bool updateIEMatrix(
		const Mat& camMatrix, const Mat& distCoeffs);
	virtual bool serialize(FileStorage& fs);
	virtual bool deserialize(const FileNode& fs);
	bool updateMatrix(const Mat& cameraMat, const Mat& distCoeff, const Mat& R, const Mat& T);
	void customizeCamMatrix(Mat & camMatrix, double cellSize, double focalLength, int &flag);
	void genHomoMat(const Mat& custMat, const Mat& originMat, Mat& homoCustMat, Mat& homoOriginMat, bool is2D);
	void setCustMat(const Mat& custMat);
	void genTransMat();
	int getCalibFlagForDistortion(bool fix);

private:
	bool doCalibration(const Point3fVec& worldPoints, const Point2fVec& imgPoints, double& rmsErr);
	bool doUndistort(const Point2fVec& imgPoints, Point2fVec& undistortImgPoints, const Mat& cameraMatrix);
	void project2WorldPlane(const Point2fVec& imgPoints, Point3fVec& worldPoints, 
		UndistortPixelStage pixStage, double z = DBL_MAX);
	bool genPTransMat();
	Mat guessCameraMatrix(const Point3fVec& worldPoints, const Point2fVec& imgPoints);
	void getIntrisincParam(double* fx, double* fy = nullptr, double* cx = nullptr, double* cy = nullptr);
	void prepareMatrix();

	void resetFullSizeRoi(Rect& roi);
	Rect getUndistortRoi(const Rect& roi);
	Mat adjPMatrixForRoi(const Rect& roi, const Rect& undistortRoi);
	bool _doCalibration(const vector<Point3fVec>& worldPointsVec, const vector<Point2fVec>& imgPointsVec,
		double& rmsErr, Mat& camMatrix, Mat& distCoeffs, Mat& tVec, Mat& rVec, int flag);

	void pntsAroundAnchor(double halfLength, const Point2f& pnt, Point2fVec& aroundPnts);
	void anglePntsAroundAnchor(double angle, const Point2f& pnt, Point2fVec& aroundPnts);

	typedef std::unordered_map<Rect, Mat, RectHasher, RectEqualFunc> Rect2MatMap;
	Rect2MatMap mPTransMatMap; // for query and maintain perspective transform matrix map
	CyclopsLock mPMapLock;

	CyclopsLock mLock;

private:
	Mat mImg;
};

#endif // CameraCalibrator_h_