From 02b991a43329fa6521e6c41f79b908740e3e84ef Mon Sep 17 00:00:00 2001
From: gmedan <gmedan@gmail.com>
Date: Mon, 2 Jul 2018 13:53:39 +0300
Subject: [PATCH] Merge pull request #1677 from gmedan:fix-charuco-topology

* fix #1665 ChAruco board topology (nearestMarkerIdx) is sensitive to scale
---
 modules/aruco/src/charuco.cpp                |  2 +-
 modules/aruco/test/test_charucodetection.cpp | 47 +++++++++++++++++++-
 2 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/modules/aruco/src/charuco.cpp b/modules/aruco/src/charuco.cpp
index e2b547704..475d7c3e7 100644
--- a/modules/aruco/src/charuco.cpp
+++ b/modules/aruco/src/charuco.cpp
@@ -194,7 +194,7 @@ void CharucoBoard::_getNearestMarkerCorners() {
             double sqDistance;
             Point3f distVector = charucoCorner - center;
             sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
-            if(j == 0 || fabs(sqDistance - minDist) < 0.0001) {
+            if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * _squareLength, 2)) {
                 // if same minimum distance (or first iteration), add to nearestMarkerIdx vector
                 nearestMarkerIdx[i].push_back(j);
                 minDist = sqDistance;
diff --git a/modules/aruco/test/test_charucodetection.cpp b/modules/aruco/test/test_charucodetection.cpp
index 5cb47d8fd..44b6f3f88 100644
--- a/modules/aruco/test/test_charucodetection.cpp
+++ b/modules/aruco/test/test_charucodetection.cpp
@@ -538,7 +538,48 @@ void CV_CharucoDiamondDetection::run(int) {
     }
 }
 
+/**
+* @brief Check charuco board creation
+*/
+class CV_CharucoBoardCreation : public cvtest::BaseTest {
+public:
+    CV_CharucoBoardCreation();
 
+protected:
+    void run(int);
+};
+
+CV_CharucoBoardCreation::CV_CharucoBoardCreation() {}
+
+void CV_CharucoBoardCreation::run(int)
+{
+    Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_5X5_250);
+    int n = 6;
+
+    float markerSizeFactor = 0.5f;
+
+    for (float squareSize_mm = 5.0f; squareSize_mm < 35.0f; squareSize_mm += 0.1f)
+    {
+        Ptr<aruco::CharucoBoard> board_meters = aruco::CharucoBoard::create(
+            n, n, squareSize_mm*1e-3f, squareSize_mm * markerSizeFactor * 1e-3f, dictionary);
+
+        Ptr<aruco::CharucoBoard> board_millimeters = aruco::CharucoBoard::create(
+            n, n, squareSize_mm, squareSize_mm * markerSizeFactor, dictionary);
+
+        for (size_t i = 0; i < board_meters->nearestMarkerIdx.size(); i++)
+        {
+            if (board_meters->nearestMarkerIdx[i].size() != board_millimeters->nearestMarkerIdx[i].size() ||
+                board_meters->nearestMarkerIdx[i][0] != board_millimeters->nearestMarkerIdx[i][0])
+            {
+                ts->printf(cvtest::TS::LOG,
+                    cv::format("Charuco board topology is sensitive to scale with squareSize=%.1f\n",
+                        squareSize_mm).c_str());
+                ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT);
+                break;
+            }
+        }
+    }
+}
 
 
 TEST(CV_CharucoDetection, accuracy) {
@@ -546,7 +587,6 @@ TEST(CV_CharucoDetection, accuracy) {
     test.safe_run();
 }
 
-
 TEST(CV_CharucoPoseEstimation, accuracy) {
     CV_CharucoPoseEstimation test;
     test.safe_run();
@@ -557,4 +597,9 @@ TEST(CV_CharucoDiamondDetection, accuracy) {
     test.safe_run();
 }
 
+TEST(CV_CharucoBoardCreation, accuracy) {
+    CV_CharucoBoardCreation test;
+    test.safe_run();
+}
+
 }} // namespace