Merge pull request opencv#3174 from AleksandrPanov:fix_gridboard_objPoints

Fix objPoints order in GridBoard and CharucoBoard

* fix gridBoard

* fix charucoBoard

* add rightBottomBorder

* add test_aruco_utils and code refactoring/fix tests

* fix axes and add charuco dict

* add axes test, remove drawAxis(), update tutorial

Author: Alexander Panov

modules/aruco/include/opencv2/aruco.hpp

@@ -272,8 +272,9 @@ CV_EXPORTS_W void detectMarkers(InputArray image, const Ptr<Dictionary> &diction
  * 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:
- * (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0),
- * (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0)
+ * (0, 0, 0), (markerLength, 0, 0),
+ * (markerLength, markerLength, 0), (0, markerLength, 0)
+ * @sa use cv::drawFrameAxes to get world coordinate system axis for object points
  */
 CV_EXPORTS_W void estimatePoseSingleMarkers(InputArrayOfArrays corners, float markerLength,
                                             InputArray cameraMatrix, InputArray distCoeffs,
@@ -316,7 +317,14 @@ class CV_EXPORTS_W Board {
     CV_WRAP void setIds(InputArray ids);
 
     /// array of object points of all the marker corners in the board
-    /// each marker include its 4 corners in CCW order. For M markers, the size is Mx4.
+    /// each marker include its 4 corners in this order:
+    ///-   objPoints[i][0] - left-top point of i-th marker
+    ///-   objPoints[i][1] - right-top point of i-th marker
+    ///-   objPoints[i][2] - right-bottom point of i-th marker
+    ///-   objPoints[i][3] - left-bottom point of i-th marker
+    ///
+    /// Markers are placed in a certain order - row by row, left to right in every row.
+    /// For M markers, the size is Mx4.
     CV_PROP std::vector< std::vector< Point3f > > objPoints;
 
     /// the dictionary of markers employed for this board
@@ -325,6 +333,9 @@ class CV_EXPORTS_W Board {
     /// vector of the identifiers of the markers in the board (same size than objPoints)
     /// The identifiers refers to the board dictionary
     CV_PROP_RW std::vector< int > ids;
+
+    /// coordinate of the bottom right corner of the board, is set when calling the function create()
+    CV_PROP Point3f rightBottomBorder;
 };
 
 
@@ -424,6 +435,7 @@ class CV_EXPORTS_W GridBoard : public Board {
  * Input markers that are not included in the board layout are ignored.
  * The function returns the number of markers from the input employed for the board pose estimation.
  * Note that returning a 0 means the pose has not been estimated.
+ * @sa use cv::drawFrameAxes to get world coordinate system axis for object points
  */
 CV_EXPORTS_W int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr<Board> &board,
                                    InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec,
@@ -488,36 +500,14 @@ CV_EXPORTS_W void refineDetectedMarkers(
  * Given an array of detected marker corners and its corresponding ids, this functions draws
  * the markers in the image. The marker borders are painted and the markers identifiers if provided.
  * Useful for debugging purposes.
+ *
  */
 CV_EXPORTS_W void drawDetectedMarkers(InputOutputArray image, InputArrayOfArrays corners,
                                       InputArray ids = noArray(),
                                       Scalar borderColor = Scalar(0, 255, 0));
 
 
 
-/**
- * @brief Draw coordinate system axis from pose estimation
- *
- * @param image input/output image. It must have 1 or 3 channels. The number of channels is not
- * altered.
- * @param cameraMatrix input 3x3 floating-point camera matrix
- * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
- * @param distCoeffs vector of distortion coefficients
- * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements
- * @param rvec rotation vector of the coordinate system that will be drawn. (@sa Rodrigues).
- * @param tvec translation vector of the coordinate system that will be drawn.
- * @param length length of the painted axis in the same unit than tvec (usually in meters)
- *
- * Given the pose estimation of a marker or board, this function draws the axis of the world
- * coordinate system, i.e. the system centered on the marker/board. Useful for debugging purposes.
- *
- * @deprecated use cv::drawFrameAxes
- */
-CV_EXPORTS_W void drawAxis(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
-                           InputArray rvec, InputArray tvec, float length);
-
-
-
 /**
  * @brief Draw a canonical marker image
  *

modules/aruco/include/opencv2/aruco/charuco.hpp

@@ -181,6 +181,7 @@ CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, Inp
  * This function estimates a Charuco board pose from some detected corners.
  * The function checks if the input corners are enough and valid to perform pose estimation.
  * If pose estimation is valid, returns true, else returns false.
+ * @sa use cv::drawFrameAxes to get world coordinate system axis for object points
  */
 CV_EXPORTS_W bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds,
                                            const Ptr<CharucoBoard> &board, InputArray cameraMatrix,
@@ -275,6 +276,7 @@ CV_EXPORTS_W double calibrateCameraCharuco(
  * diamond.
  * @param cameraMatrix Optional camera calibration matrix.
  * @param distCoeffs Optional camera distortion coefficients.
+ * @param dictionary dictionary of markers indicating the type of markers.
  *
  * This function detects Diamond markers from the previous detected ArUco markers. The diamonds
  * are returned in the diamondCorners and diamondIds parameters. If camera calibration parameters
@@ -285,7 +287,8 @@ CV_EXPORTS_W void detectCharucoDiamond(InputArray image, InputArrayOfArrays mark
                                        InputArray markerIds, float squareMarkerLengthRate,
                                        OutputArrayOfArrays diamondCorners, OutputArray diamondIds,
                                        InputArray cameraMatrix = noArray(),
-                                       InputArray distCoeffs = noArray());
+                                       InputArray distCoeffs = noArray(),
+                                       Ptr<Dictionary> dictionary = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0)));
 
 
 

modules/aruco/samples/calibrate_camera.cpp

@@ -101,7 +101,7 @@ int main(int argc, char *argv[]) {
     if(parser.get<bool>("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST;
     if(parser.get<bool>("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT;
 
-    Ptr<aruco::DetectorParameters> detectorParams;
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     if(parser.has("dp")) {
         FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
         bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);

modules/aruco/samples/calibrate_camera_charuco.cpp

@@ -102,7 +102,7 @@ int main(int argc, char *argv[]) {
     if(parser.get<bool>("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST;
     if(parser.get<bool>("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT;
 
-    Ptr<aruco::DetectorParameters> detectorParams;
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     if(parser.has("dp")) {
         FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
         bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);

modules/aruco/samples/detect_board.cpp

@@ -97,7 +97,7 @@ int main(int argc, char *argv[]) {
         }
     }
 
-    Ptr<aruco::DetectorParameters> detectorParams;
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     if(parser.has("dp")) {
         FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
         bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
@@ -199,7 +199,7 @@ int main(int argc, char *argv[]) {
             aruco::drawDetectedMarkers(imageCopy, rejected, noArray(), Scalar(100, 0, 255));
 
         if(markersOfBoardDetected > 0)
-            aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
+            cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
 
         imshow("out", imageCopy);
         char key = (char)waitKey(waitTime);

modules/aruco/samples/detect_board_charuco.cpp

@@ -99,7 +99,7 @@ int main(int argc, char *argv[]) {
         }
     }
 
-    Ptr<aruco::DetectorParameters> detectorParams;
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     if(parser.has("dp")) {
         FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
         bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
@@ -213,7 +213,7 @@ int main(int argc, char *argv[]) {
         }
 
         if(validPose)
-            aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
+            cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
 
         imshow("out", imageCopy);
         char key = (char)waitKey(waitTime);

modules/aruco/samples/detect_diamonds.cpp

@@ -87,7 +87,7 @@ int main(int argc, char *argv[]) {
     bool autoScale = parser.has("as");
     float autoScaleFactor = autoScale ? parser.get<float>("as") : 1.f;
 
-    Ptr<aruco::DetectorParameters> detectorParams;
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     if(parser.has("dp")) {
         FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
         bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
@@ -219,8 +219,8 @@ int main(int argc, char *argv[]) {
 
             if(estimatePose) {
                 for(unsigned int i = 0; i < diamondIds.size(); i++)
-                    aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
-                                    squareLength * 0.5f);
+                    cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
+                                      squareLength * 1.1f);
             }
         }
 

modules/aruco/samples/detect_markers.cpp

@@ -80,7 +80,7 @@ int main(int argc, char *argv[]) {
     bool estimatePose = parser.has("c");
     float markerLength = parser.get<float>("l");
 
-    Ptr<aruco::DetectorParameters> detectorParams;
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     if(parser.has("dp")) {
         FileStorage fs(parser.get<string>("dp"), FileStorage::READ);
         bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams);
@@ -179,8 +179,7 @@ int main(int argc, char *argv[]) {
 
             if(estimatePose) {
                 for(unsigned int i = 0; i < ids.size(); i++)
-                    aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
-                                    markerLength * 0.5f);
+                    cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength * 1.5f, 2);
             }
         }
 

modules/aruco/samples/tutorial_charuco_create_detect.cpp

@@ -72,7 +72,7 @@ static inline void detectCharucoBoardWithCalibrationPose()
                     //! [pose]
                     // if charuco pose is valid
                     if (valid)
-                        cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1f);
+                        cv::drawFrameAxes(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1f);
                 }
             }
             cv::imshow("out", imageCopy);

modules/aruco/src/aruco.cpp

@@ -785,11 +785,11 @@ static void _getSingleMarkerObjectPoints(float markerLength, OutputArray _objPoi
 
     _objPoints.create(4, 1, CV_32FC3);
     Mat objPoints = _objPoints.getMat();
-    // set coordinate system in the middle of the marker, with Z pointing out
-    objPoints.ptr< Vec3f >(0)[0] = Vec3f(-markerLength / 2.f, markerLength / 2.f, 0);
-    objPoints.ptr< Vec3f >(0)[1] = Vec3f(markerLength / 2.f, markerLength / 2.f, 0);
-    objPoints.ptr< Vec3f >(0)[2] = Vec3f(markerLength / 2.f, -markerLength / 2.f, 0);
-    objPoints.ptr< Vec3f >(0)[3] = Vec3f(-markerLength / 2.f, -markerLength / 2.f, 0);
+    // 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);
 }
 
 /**
@@ -1593,6 +1593,7 @@ Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Ptr<Dictionary> &di
     CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1);
 
     std::vector< std::vector< Point3f > > obj_points_vector;
+    Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
     for (unsigned int i = 0; i < objPoints.total(); i++) {
         std::vector<Point3f> corners;
         Mat corners_mat = objPoints.getMat(i);
@@ -1602,7 +1603,11 @@ Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Ptr<Dictionary> &di
         CV_Assert(corners_mat.total() == 4);
 
         for (int j = 0; j < 4; j++) {
-            corners.push_back(corners_mat.at<Point3f>(j));
+            const Point3f& corner = corners_mat.at<Point3f>(j);
+            corners.push_back(corner);
+            rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x);
+            rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y);
+            rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z);
         }
         obj_points_vector.push_back(corners);
     }
@@ -1611,6 +1616,7 @@ Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Ptr<Dictionary> &di
     ids.copyTo(res->ids);
     res->objPoints = obj_points_vector;
     res->dictionary = cv::makePtr<Dictionary>(dictionary);
+    res->rightBottomBorder = rightBottomBorder;
     return res;
 }
 
@@ -1646,20 +1652,19 @@ Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength,
     }
 
     // calculate Board objPoints
-    float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparation;
     for(int y = 0; y < markersY; y++) {
         for(int x = 0; x < markersX; x++) {
-            vector< Point3f > corners;
-            corners.resize(4);
+            vector<Point3f> corners(4);
             corners[0] = Point3f(x * (markerLength + markerSeparation),
-                                 maxY - y * (markerLength + markerSeparation), 0);
+                                 y * (markerLength + markerSeparation), 0);
             corners[1] = corners[0] + Point3f(markerLength, 0, 0);
-            corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0);
-            corners[3] = corners[0] + Point3f(0, -markerLength, 0);
+            corners[2] = corners[0] + Point3f(markerLength, markerLength, 0);
+            corners[3] = corners[0] + Point3f(0, markerLength, 0);
             res->objPoints.push_back(corners);
         }
     }
-
+    res->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1),
+                                     markersY * markerLength + markerSeparation * (markersY - 1), 0.f);
     return res;
 }
 
@@ -1711,15 +1716,6 @@ void drawDetectedMarkers(InputOutputArray _image, InputArrayOfArrays _corners,
 }
 
 
-
-/**
- */
-void drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec,
-              InputArray _tvec, float length)
-{
-    drawFrameAxes(_image, _cameraMatrix, _distCoeffs, _rvec, _tvec, length, 3);
-}
-
 /**
  */
 void drawMarker(const Ptr<Dictionary> &dictionary, int id, int sidePixels, OutputArray _img, int borderBits) {
@@ -1784,7 +1780,7 @@ void _drawPlanarBoardImpl(Board *_board, Size outSize, OutputArray _img, int mar
             // move top left to 0, 0
             pf -= Point2f(minX, minY);
             pf.x = pf.x / sizeX * float(out.cols);
-            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+            pf.y = pf.y / sizeY * float(out.rows);
             outCorners[j] = pf;
         }