Merge pull request #2514 from amy-tabb:testCharucoCornersCollinear3.4

aruco: new feature testCharucoCornersCollinear() in charuco.hpp/cpp

* feature declaration + implementation

* Update charuco.hpp

Fixed parameter listing for Doxygen.

* Updated charuco.cpp

-indentation fixes
-switched order of the test for nCharucoCorners > 2 as suggested

* tests added for testCharucoCornersCollinear T/F

* fixed test to be consistent with C++98

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

@@ -334,7 +334,18 @@ CV_EXPORTS void drawCharucoDiamond(const Ptr<Dictionary> &dictionary, Vec4i ids,
                                    int borderBits = 1);
 
 
-
+/**
+ * @brief test whether the ChArUco markers are collinear
+ *
+ * @param _board layout of ChArUco board.
+ * @param _charucoIds list of identifiers for each corner in charucoCorners per frame.
+ * @return bool value, 1 (true) if detected corners form a line, 0 (false) if they do not.
+      solvePnP, calibration functions will fail if the corners are collinear (true).
+ *
+ * The number of ids in charucoIDs should be <= the number of chessboard corners in the board.  This functions checks whether the charuco corners are on a straight line (returns true, if so), or not (false).  Axis parallel, as well as diagonal and other straight lines detected.  Degenerate cases: for number of charucoIDs <= 2, the function returns true.
+ */
+CV_EXPORTS_W bool testCharucoCornersCollinear(const Ptr<CharucoBoard> &_board,
+                                              InputArray _charucoIds);
 
 //! @}
 }

modules/aruco/src/charuco.cpp

@@ -938,5 +938,60 @@ void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners,
         }
     }
 }
+
+/**
+   @param board layout of ChArUco board.
+ * @param image charucoIds list of identifiers for each corner in charucoCorners.
+ * @return bool value, 1 (true) for detected corners form a line, 0 for non-linear.
+      solvePnP will fail if the corners are collinear (true).
+  * Check that the set of charuco markers in _charucoIds does not identify a straight line on
+    the charuco board.  Axis parallel, as well as diagonal and other straight lines detected.
+  */
+ bool testCharucoCornersCollinear(const Ptr<CharucoBoard> &_board, InputArray _charucoIds){
+
+    unsigned int nCharucoCorners = (unsigned int)_charucoIds.getMat().total();
+
+    if (nCharucoCorners <= 2)
+        return true;
+
+    // only test if there are 3 or more corners
+    CV_Assert( _board->chessboardCorners.size() >= _charucoIds.getMat().total());
+
+    Vec<double, 3> point0( _board->chessboardCorners[_charucoIds.getMat().at< int >(0)].x,
+            _board->chessboardCorners[_charucoIds.getMat().at< int >(0)].y,
+            1);
+
+    Vec<double, 3> point1( _board->chessboardCorners[_charucoIds.getMat().at< int >(1)].x,
+            _board->chessboardCorners[_charucoIds.getMat().at< int >(1)].y,
+            1);
+
+    // create a line from the first two points.
+    Vec<double, 3> testLine = point0.cross(point1);
+
+    Vec<double, 3> testPoint(0, 0, 1);
+
+    double divisor = sqrt(testLine[0]*testLine[0] + testLine[1]*testLine[1]);
+
+    CV_Assert( divisor != 0);
+
+    // normalize the line with normal
+    testLine /= divisor;
+
+    double dotProduct;
+    for (unsigned int i = 2; i < nCharucoCorners; i++){
+        testPoint(0) = _board->chessboardCorners[_charucoIds.getMat().at< int >(i)].x;
+        testPoint(1) = _board->chessboardCorners[_charucoIds.getMat().at< int >(i)].y;
+
+        // if testPoint is on testLine, dotProduct will be zero (or very, very close)
+        dotProduct = testPoint.dot(testLine);
+
+        if (std::abs(dotProduct) > 1e-6){
+            return false;
+        }
+    }
+
+    // no points found that were off of testLine, return true that all points collinear.
+    return true;
+}
 }
 }

modules/aruco/test/test_charucodetection.cpp

@@ -199,9 +199,6 @@ static Mat projectCharucoBoard(Ptr<aruco::CharucoBoard> &board, Mat cameraMatrix
     return img;
 }
 
-
-
-
 /**
  * @brief Check Charuco detection
  */
@@ -602,4 +599,82 @@ TEST(CV_CharucoBoardCreation, accuracy) {
     test.safe_run();
 }
 
+TEST(Charuco, testCharucoCornersCollinear_true)
+{
+    int squaresX = 13;
+    int squaresY = 28;
+    float squareLength = 300;
+    float markerLength = 150;
+    int dictionaryId = 11;
+
+
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
+
+    Ptr<aruco::Dictionary> dictionary =
+            aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
+
+    Ptr<aruco::CharucoBoard> charucoBoard =
+            aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, dictionary);
+
+    // consistency with C++98
+    const int arrLine[9] = {192, 204, 216, 228, 240, 252, 264, 276, 288};
+    vector<int> charucoIdsAxisLine(9, 0);
+
+    for (int i = 0; i < 9; i++){
+        charucoIdsAxisLine[i] = arrLine[i];
+    }
+
+    const int arrDiag[7] = {198, 209, 220, 231, 242, 253, 264};
+
+    vector<int> charucoIdsDiagonalLine(7, 0);
+
+    for (int i = 0; i < 7; i++){
+        charucoIdsDiagonalLine[i] = arrDiag[i];
+    }
+
+    bool resultAxisLine = cv::aruco::testCharucoCornersCollinear(charucoBoard, charucoIdsAxisLine);
+
+    bool resultDiagonalLine = cv::aruco::testCharucoCornersCollinear(charucoBoard, charucoIdsDiagonalLine);
+
+    EXPECT_TRUE(resultAxisLine);
+
+    EXPECT_TRUE(resultDiagonalLine);
+}
+
+TEST(Charuco, testCharucoCornersCollinear_false)
+{
+    int squaresX = 13;
+    int squaresY = 28;
+    float squareLength = 300;
+    float markerLength = 150;
+    int dictionaryId = 11;
+
+
+    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
+
+    Ptr<aruco::Dictionary> dictionary =
+            aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
+
+    Ptr<aruco::CharucoBoard> charucoBoard =
+            aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, dictionary);
+
+    // consistency with C++98
+    const int arr[63] = {192, 193, 194, 195, 196, 197, 198, 204, 205, 206, 207, 208,
+                                209, 210, 216, 217, 218, 219, 220, 221, 222, 228, 229, 230,
+                                231, 232, 233, 234, 240, 241, 242, 243, 244, 245, 246, 252,
+                                253, 254, 255, 256, 257, 258, 264, 265, 266, 267, 268, 269,
+                                270, 276, 277, 278, 279, 280, 281, 282, 288, 289, 290, 291,
+                                292, 293, 294};
+
+    vector<int> charucoIds(63, 0);
+    for (int i = 0; i < 63; i++){
+        charucoIds[i] = arr[i];
+    }
+
+
+    bool result = cv::aruco::testCharucoCornersCollinear(charucoBoard, charucoIds);
+
+    EXPECT_FALSE(result);
+}
+
 }} // namespace