From a1fe9c871fb436edb39c78cf8f92bb32e3ae5936 Mon Sep 17 00:00:00 2001
From: Andreas Franek <itfranek@gmail.com>
Date: Fri, 17 Sep 2021 14:08:06 +0200
Subject: [PATCH] Merge pull request #3024 from
 andy-held:charuco-selectAndRefineChessboardCorners

aruco: fix subpixel coordinates in interpolateCornersCharuco

* adjust sub-pixel coordinates before cornerSubPix

* add test for ChArUco board subpixel accuracy
---
 modules/aruco/src/charuco.cpp                |  4 +-
 modules/aruco/test/test_charucodetection.cpp | 56 ++++++++++++++++++++
 2 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/modules/aruco/src/charuco.cpp b/modules/aruco/src/charuco.cpp
index 3cdd350dc..8397e916c 100644
--- a/modules/aruco/src/charuco.cpp
+++ b/modules/aruco/src/charuco.cpp
@@ -315,7 +315,7 @@ static int _selectAndRefineChessboardCorners(InputArray _allCorners, InputArray
 
         for (int i = begin; i < end; i++) {
             vector<Point2f> in;
-            in.push_back(filteredChessboardImgPoints[i]);
+            in.push_back(filteredChessboardImgPoints[i] - Point2f(0.5, 0.5)); // adjust sub-pixel coordinates for cornerSubPix
             Size winSize = filteredWinSizes[i];
             if (winSize.height == -1 || winSize.width == -1)
                 winSize = Size(params->cornerRefinementWinSize, params->cornerRefinementWinSize);
@@ -325,7 +325,7 @@ static int _selectAndRefineChessboardCorners(InputArray _allCorners, InputArray
                                       params->cornerRefinementMaxIterations,
                                       params->cornerRefinementMinAccuracy));
 
-            filteredChessboardImgPoints[i] = in[0];
+            filteredChessboardImgPoints[i] = in[0] + Point2f(0.5, 0.5);
         }
     });
 
diff --git a/modules/aruco/test/test_charucodetection.cpp b/modules/aruco/test/test_charucodetection.cpp
index e803a031d..a39fae699 100644
--- a/modules/aruco/test/test_charucodetection.cpp
+++ b/modules/aruco/test/test_charucodetection.cpp
@@ -677,4 +677,60 @@ TEST(Charuco, testCharucoCornersCollinear_false)
     EXPECT_FALSE(result);
 }
 
+// test that ChArUco board detection is subpixel accurate
+TEST(Charuco, testBoardSubpixelCoords)
+{
+    cv::Size res{500, 500};
+    cv::Mat K = (cv::Mat_<double>(3,3) <<
+        0.5*res.width, 0, 0.5*res.width,
+        0, 0.5*res.height, 0.5*res.height,
+        0, 0, 1);
+
+    // load board image with corners at round values
+    cv::String testImagePath = cvtest::TS::ptr()->get_data_path() + "aruco/" + "trivial_board_detection.png";
+    Mat img = imread(testImagePath);
+    cv::Mat expected_corners = (cv::Mat_<float>(9,2) <<
+        200, 300,
+        250, 300,
+        300, 300,
+        200, 250,
+        250, 250,
+        300, 250,
+        200, 200,
+        250, 200,
+        300, 200
+    );
+
+    cv::Mat gray;
+    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
+
+    auto dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_36h11);
+    auto board = cv::aruco::CharucoBoard::create(4, 4, 1.f, .8f, dict);
+
+    auto params = cv::aruco::DetectorParameters::create();
+    params->cornerRefinementMethod = cv::aruco::CORNER_REFINE_APRILTAG;
+
+    std::vector<int> ids;
+    std::vector<std::vector<cv::Point2f>> corners, rejected;
+
+    cv::aruco::detectMarkers(gray, dict, corners, ids, params, rejected, K);
+
+    ASSERT_EQ(ids.size(), size_t(8));
+
+    cv::Mat c_ids, c_corners;
+    cv::aruco::interpolateCornersCharuco(corners, ids, gray, board, c_corners, c_ids, K);
+    cv::Mat corners_reshaped = c_corners.reshape(1);
+
+    ASSERT_EQ(c_corners.rows, expected_corners.rows);
+    EXPECT_NEAR(0, cvtest::norm(expected_corners, c_corners.reshape(1), NORM_INF), 1e-3);
+
+    c_ids = cv::Mat();
+    c_corners = cv::Mat();
+    cv::aruco::interpolateCornersCharuco(corners, ids, gray, board, c_corners, c_ids);
+    corners_reshaped = c_corners.reshape(1);
+
+    ASSERT_EQ(c_corners.rows, expected_corners.rows);
+    EXPECT_NEAR(0, cvtest::norm(expected_corners, c_corners.reshape(1), NORM_INF), 1e-3);
+}
+
 }} // namespace