diff --git a/modules/aruco/include/opencv2/aruco/aruco_calib_pose.hpp b/modules/aruco/include/opencv2/aruco/aruco_calib_pose.hpp index fe21116631c..5ce84253685 100644 --- a/modules/aruco/include/opencv2/aruco/aruco_calib_pose.hpp +++ b/modules/aruco/include/opencv2/aruco/aruco_calib_pose.hpp @@ -4,6 +4,7 @@ #ifndef __OPENCV_ARUCO_CALIB_POSE_HPP__ #define __OPENCV_ARUCO_CALIB_POSE_HPP__ #include +#include namespace cv { namespace aruco { @@ -11,6 +12,57 @@ namespace aruco { //! @addtogroup aruco //! @{ +/** @brief + * rvec/tvec define the right handed coordinate system of the marker. + * PatternPos defines center this system and axes direction. + * Axis X (red color) - first coordinate, axis Y (green color) - second coordinate, + * axis Z (blue color) - third coordinate. + * @sa estimatePoseSingleMarkers(), @ref tutorial_aruco_detection + */ +enum PatternPos { + /** @brief The marker coordinate system is centered on the middle of the marker. + * The coordinates of the four corners (CCW order) of the marker in its own coordinate system are: + * (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0), + * (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0). + * + * These pattern points define this coordinate system: + * ![Image with axes drawn](images/singlemarkersaxes.jpg) + */ + CCW_center, + /** @brief The marker coordinate system is centered on the top-left corner of the marker. + * The coordinates of the four corners (CW order) of the marker in its own coordinate system are: + * (0, 0, 0), (markerLength, 0, 0), + * (markerLength, markerLength, 0), (0, markerLength, 0). + * + * These pattern points define this coordinate system: + * ![Image with axes drawn](images/singlemarkersaxes2.jpg) + */ + CW_top_left_corner +}; + +/** @brief + * Pose estimation parameters + * @param pattern Defines center this system and axes direction (default PatternPos::CCW_center). + * @param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses the provided + * rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further + * optimizes them (default false). + * @param solvePnPMethod Method for solving a PnP problem: see @ref calib3d_solvePnP_flags (default SOLVEPNP_ITERATIVE). + * @sa PatternPos, solvePnP(), @ref tutorial_aruco_detection + */ +struct CV_EXPORTS_W EstimateParameters { + CV_PROP_RW PatternPos pattern; + CV_PROP_RW bool useExtrinsicGuess; + CV_PROP_RW SolvePnPMethod solvePnPMethod; + + EstimateParameters(): pattern(CCW_center), useExtrinsicGuess(false), + solvePnPMethod(SOLVEPNP_ITERATIVE) {} + + CV_WRAP static Ptr create() { + return makePtr(); + } +}; + + /** * @brief Pose estimation for single markers * @@ -29,21 +81,28 @@ namespace aruco { * @param tvecs array of output translation vectors (e.g. std::vector). * Each element in tvecs corresponds to the specific marker in imgPoints. * @param _objPoints array of object points of all the marker corners + * @param estimateParameters set the origin of coordinate system and the coordinates of the four corners of the marker + * (default estimateParameters.pattern = PatternPos::CCW_center, estimateParameters.useExtrinsicGuess = false, + * estimateParameters.solvePnPMethod = SOLVEPNP_ITERATIVE). * * This function receives the detected markers and returns their pose estimation respect to * the camera individually. So for each marker, one rotation and translation vector is returned. * The returned transformation is the one that transforms points from each marker coordinate system * to the camera coordinate system. - * The marker corrdinate system is centered on the middle of the marker, with the Z axis - * perpendicular to the marker plane. - * The coordinates of the four corners of the marker in its own coordinate system are: - * (0, 0, 0), (markerLength, 0, 0), - * (markerLength, markerLength, 0), (0, markerLength, 0) + * The marker coordinate system is centered on the middle (by default) or on the top-left corner of the marker, + * with the Z axis perpendicular to the marker plane. + * estimateParameters defines the coordinates of the four corners of the marker in its own coordinate system (by default) are: + * (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0), + * (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0) * @sa use cv::drawFrameAxes to get world coordinate system axis for object points + * @sa @ref tutorial_aruco_detection + * @sa EstimateParameters + * @sa PatternPos */ CV_EXPORTS_W void estimatePoseSingleMarkers(InputArrayOfArrays corners, float markerLength, InputArray cameraMatrix, InputArray distCoeffs, - OutputArray rvecs, OutputArray tvecs, OutputArray _objPoints = noArray()); + OutputArray rvecs, OutputArray tvecs, OutputArray _objPoints = noArray(), + Ptr estimateParameters = EstimateParameters::create()); /** * @brief Pose estimation for a board of markers diff --git a/modules/aruco/src/aruco_calib_pose.cpp b/modules/aruco/src/aruco_calib_pose.cpp index 4e17fe52506..7dc7200a503 100644 --- a/modules/aruco/src/aruco_calib_pose.cpp +++ b/modules/aruco/src/aruco_calib_pose.cpp @@ -3,7 +3,6 @@ // of this distribution and at http://opencv.org/license.html #include -#include namespace cv { namespace aruco { @@ -41,24 +40,37 @@ void getBoardObjectAndImagePoints(const Ptr &board, InputArrayOfArrays de } /** - * @brief Return object points for the system centered in a single marker, given the marker length + * @brief Return object points for the system centered in a middle (by default) or in a top left corner of single + * marker, given the marker length */ -static Mat _getSingleMarkerObjectPoints(float markerLength) { +static Mat _getSingleMarkerObjectPoints(float markerLength, const EstimateParameters& estimateParameters) { CV_Assert(markerLength > 0); Mat objPoints(4, 1, CV_32FC3); // set coordinate system in the top-left corner of the marker, with Z pointing out - objPoints.ptr< Vec3f >(0)[0] = Vec3f(0.f, 0.f, 0); - objPoints.ptr< Vec3f >(0)[1] = Vec3f(markerLength, 0.f, 0); - objPoints.ptr< Vec3f >(0)[2] = Vec3f(markerLength, markerLength, 0); - objPoints.ptr< Vec3f >(0)[3] = Vec3f(0.f, markerLength, 0); + if (estimateParameters.pattern == CW_top_left_corner) { + objPoints.ptr(0)[0] = Vec3f(0.f, 0.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength, 0.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength, markerLength, 0); + objPoints.ptr(0)[3] = Vec3f(0.f, markerLength, 0); + } + else if (estimateParameters.pattern == CCW_center) { + objPoints.ptr(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0); + objPoints.ptr(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0); + } + else + CV_Error(Error::StsBadArg, "Unknown estimateParameters pattern"); return objPoints; } -void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength, InputArray _cameraMatrix, - InputArray _distCoeffs, OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints) { +void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints, + Ptr estimateParameters) { CV_Assert(markerLength > 0); - Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength); + Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength, *estimateParameters); int nMarkers = (int)_corners.total(); _rvecs.create(nMarkers, 1, CV_64FC3); _tvecs.create(nMarkers, 1, CV_64FC3); @@ -72,7 +84,7 @@ void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength, for (int i = begin; i < end; i++) { solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs, rvecs.at(i), - tvecs.at(i)); + tvecs.at(i), estimateParameters->useExtrinsicGuess, estimateParameters->solvePnPMethod); } }); diff --git a/modules/aruco/src/aruco_detector.cpp b/modules/aruco/src/aruco_detector.cpp index f24c2c4d5c0..a9dc9af4670 100644 --- a/modules/aruco/src/aruco_detector.cpp +++ b/modules/aruco/src/aruco_detector.cpp @@ -755,7 +755,9 @@ static void _refineCandidateLines(std::vector& nContours, std::vector