/*! * \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 #include #include /*! \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 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 mapLengthToWorld(const vector& 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 mapAreaToWorld(const vector& 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 mapAngleToWorld(const vector& 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 mCharucoIds; std::vector mAssistImgPoints; std::vector mAssistWorldPoints; std::string mErrorMsg; friend class StereoCalibrator; CameraCalibrator::Ptr mDistortCalibPtr; cv::Ptr 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& 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& worldPointsVec, const vector& 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 Rect2MatMap; Rect2MatMap mPTransMatMap; // for query and maintain perspective transform matrix map CyclopsLock mPMapLock; CyclopsLock mLock; private: Mat mImg; }; #endif // CameraCalibrator_h_