add getDistanceVector

Author: AleksandrPanov

modules/aruco/perf/perf_aruco.cpp

@@ -6,16 +6,16 @@
 namespace opencv_test {
 using namespace perf;
 
-typedef tuple<bool, int> ArucoTestParams;
-typedef TestBaseWithParam<ArucoTestParams> EstimateAruco;
+typedef tuple<bool, int> UseArucoParams;
+typedef TestBaseWithParam<UseArucoParams> EstimateAruco;
 #define ESTIMATE_PARAMS Combine(Values(false, true), Values(-1))
 
 static double deg2rad(double deg) { return deg * CV_PI / 180.; }
 
 class MarkerPainter
 {
 private:
-    int imgMarkerSize;
+    int imgMarkerSize = 0;
     Mat cameraMatrix;
 public:
     MarkerPainter(const int size)
@@ -28,8 +28,8 @@ class MarkerPainter
         imgMarkerSize = size;
         cameraMatrix = Mat::eye(3, 3, CV_64FC1);
         cameraMatrix.at<double>(0, 0) = cameraMatrix.at<double>(1, 1) = imgMarkerSize;
-        cameraMatrix.at<double>(0, 2) = imgMarkerSize / 2;
-        cameraMatrix.at<double>(1, 2) = imgMarkerSize / 2;
+        cameraMatrix.at<double>(0, 2) = imgMarkerSize / 2.0;
+        cameraMatrix.at<double>(1, 2) = imgMarkerSize / 2.0;
     }
 
     static std::pair<Mat, Mat> getSyntheticRT(double yaw, double pitch, double distance)
@@ -99,20 +99,20 @@ class MarkerPainter
 
         const float markerLength = 0.05f;
         vector<Point3f> markerObjPoints;
-        markerObjPoints.push_back(Point3f(-markerLength / 2.f, +markerLength / 2.f, 0));
-        markerObjPoints.push_back(markerObjPoints[0] + Point3f(markerLength, 0, 0));
-        markerObjPoints.push_back(markerObjPoints[0] + Point3f(markerLength, -markerLength, 0));
-        markerObjPoints.push_back(markerObjPoints[0] + Point3f(0, -markerLength, 0));
+        markerObjPoints.emplace_back(Point3f(-markerLength / 2.f, +markerLength / 2.f, 0));
+        markerObjPoints.emplace_back(markerObjPoints[0] + Point3f(markerLength, 0, 0));
+        markerObjPoints.emplace_back(markerObjPoints[0] + Point3f(markerLength, -markerLength, 0));
+        markerObjPoints.emplace_back(markerObjPoints[0] + Point3f(0, -markerLength, 0));
 
         // project markers and draw them
         Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0));
         projectPoints(markerObjPoints, rvec, tvec, cameraMatrix, distCoeffs, corners);
 
         vector<Point2f> originalCorners;
-        originalCorners.push_back(Point2f(0.f, 0.f));
-        originalCorners.push_back(originalCorners[0]+Point2f((float)markerSizePixels, 0));
-        originalCorners.push_back(originalCorners[0]+Point2f((float)markerSizePixels, (float)markerSizePixels));
-        originalCorners.push_back(originalCorners[0]+Point2f(0, (float)markerSizePixels));
+        originalCorners.emplace_back(Point2f(0.f, 0.f));
+        originalCorners.emplace_back(originalCorners[0]+Point2f((float)markerSizePixels, 0));
+        originalCorners.emplace_back(originalCorners[0]+Point2f((float)markerSizePixels, (float)markerSizePixels));
+        originalCorners.emplace_back(originalCorners[0]+Point2f(0, (float)markerSizePixels));
 
         Mat transformation = getPerspectiveTransform(originalCorners, corners);
 
@@ -135,12 +135,12 @@ class MarkerPainter
             {
                 int currentId = iter;
                 auto marker_corners = getProjectMarker(currentId, deg2rad(70+yaw), deg2rad(pitch), params, dictionary);
-                Point2f startPoint(j*imgMarkerSize, i*imgMarkerSize);
+                Point2i startPoint(j*imgMarkerSize, i*imgMarkerSize);
                 Mat tmp_roi = tileImage(Rect(startPoint.x, startPoint.y, imgMarkerSize, imgMarkerSize));
                 marker_corners.first.copyTo(tmp_roi);
 
                 for (Point2f& point: marker_corners.second)
-                    point += startPoint;
+                    point += static_cast<Point2f>(startPoint);
                 idCorners[currentId] = marker_corners.second;
                 auto test = idCorners[currentId];
                 yaw = (yaw + 10) % 51; // 70+yaw >= 70 && 70+yaw <= 120
@@ -152,9 +152,27 @@ class MarkerPainter
     }
 };
 
+static inline vector<double> getDistanceVector(map<int, vector<Point2f> > &golds, const vector<int>& ids,
+                                               const vector<vector<Point2f> >& corners)
+{
+    vector<double> distVector(ids.size(), numeric_limits<double>::max());
+    for (size_t i = 0; i < ids.size(); i++)
+    {
+        int id = ids[i];
+        const auto gold_corners = golds.find(id);
+        if (gold_corners != golds.end())
+        for (int c = 0; c < 4; c++)
+        {
+            double distance = cv::norm(gold_corners->second[c] - corners[i][c]);
+            distVector[i] = distance;
+        }
+    }
+    return distVector;
+}
+
 PERF_TEST_P(EstimateAruco, ArucoFirst, ESTIMATE_PARAMS)
 {
-    ArucoTestParams testParams = GetParam();
+    UseArucoParams testParams = GetParam();
     Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
     Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     detectorParams->minDistanceToBorder = 1;
@@ -180,22 +198,13 @@ PERF_TEST_P(EstimateAruco, ArucoFirst, ESTIMATE_PARAMS)
         aruco::detectMarkers(image_map.first, dictionary, corners, ids, detectorParams);
     }
     ASSERT_EQ(numMarkersInRow*numMarkersInRow, ids.size());
-    for (size_t i = 0; i < ids.size(); i++)
-    {
-        int id = ids[i];
-        for (int c = 0; c < 4; c++)
-            {
-                std::vector<Point2f> test = image_map.second[id];
-                double dist = cv::norm(image_map.second[id][c] - corners[i][c]);
-                EXPECT_LE(dist, 5.0);
-            }
-    }
-    SANITY_CHECK_NOTHING();
+    auto distVector = getDistanceVector(image_map.second, ids, corners);
+    SANITY_CHECK(distVector, 3.0*4.0, ERROR_ABSOLUTE);
 }
 
 PERF_TEST_P(EstimateAruco, ArucoSecond, ESTIMATE_PARAMS)
 {
-    ArucoTestParams testParams = GetParam();
+    UseArucoParams testParams = GetParam();
     Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
     Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     detectorParams->minDistanceToBorder = 1;
@@ -221,41 +230,50 @@ PERF_TEST_P(EstimateAruco, ArucoSecond, ESTIMATE_PARAMS)
         aruco::detectMarkers(image_map.first, dictionary, corners, ids, detectorParams);
     }
     ASSERT_EQ(numMarkersInRow*numMarkersInRow, ids.size());
-    for (size_t i = 0; i < ids.size(); i++)
+    auto distVector = getDistanceVector(image_map.second, ids, corners);
+    SANITY_CHECK(distVector, 3.0*4.0, ERROR_ABSOLUTE);
+}
+
+struct Aruco3Params
+{
+    bool useAruco3Detection = false;
+    float minMarkerLengthRatioOriginalImg = 0.f;
+    int minSideLengthCanonicalImg = 0;
+
+    Aruco3Params(bool useAruco3, float minMarkerLen, int minSideLen): useAruco3Detection(useAruco3),
+                                                                      minMarkerLengthRatioOriginalImg(minMarkerLen),
+                                                                      minSideLengthCanonicalImg(minSideLen) {}
+    friend std::ostream& operator<<(std::ostream& os, const Aruco3Params& d)
     {
-        int id = ids[i];
-        for (int c = 0; c < 4; c++)
-            {
-                std::vector<Point2f> test = image_map.second[id];
-                double dist = cv::norm(image_map.second[id][c] - corners[i][c]);
-                EXPECT_LE(dist, 5.0);
-            }
+        os << d.useAruco3Detection << " " << d.minMarkerLengthRatioOriginalImg << " " << d.minSideLengthCanonicalImg;
+        return os;
     }
-    SANITY_CHECK_NOTHING();
-}
+};
+typedef tuple<Aruco3Params, pair<int, int>> ArucoTestParams;
 
-typedef tuple<tuple<bool, float>, int> ArucoLargeTestParams;
-typedef TestBaseWithParam<ArucoLargeTestParams> EstimateLargeAruco;
-#define ESTIMATE_LARGE_PARAMS Combine(Values(make_tuple(false, 0.f), make_tuple(true, 0.015f)), Values(-1))
+typedef TestBaseWithParam<ArucoTestParams> EstimateLargeAruco;
+#define ESTIMATE_FHD_PARAMS Combine(Values(Aruco3Params(false, 0.f, 0), Aruco3Params(true, 0.f, 32), \
+Aruco3Params(true, 0.015f, 32), Aruco3Params(true, 0.f, 16), Aruco3Params(true, 0.008, 16)),         \
+Values(std::make_pair(1440, 1), std::make_pair(480, 3), std::make_pair(144, 10)))
 
-PERF_TEST_P(EstimateLargeAruco, ArucoFHD, ESTIMATE_LARGE_PARAMS)
+PERF_TEST_P(EstimateLargeAruco, ArucoFHD, ESTIMATE_FHD_PARAMS)
 {
-    ArucoLargeTestParams testParams = GetParam();
+    ArucoTestParams testParams = GetParam();
     Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
     Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
     detectorParams->minDistanceToBorder = 1;
     detectorParams->markerBorderBits = 1;
     detectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
 
     //USE_ARUCO3
-    detectorParams->useAruco3Detection = get<0>(get<0>(testParams));
+    detectorParams->useAruco3Detection = get<0>(testParams).useAruco3Detection;
     if (detectorParams->useAruco3Detection) {
-        detectorParams->minSideLengthCanonicalImg = 32;
-        detectorParams->minMarkerLengthRatioOriginalImg = get<1>(get<0>(testParams));
+        detectorParams->minSideLengthCanonicalImg = get<0>(testParams).minSideLengthCanonicalImg;
+        detectorParams->minMarkerLengthRatioOriginalImg = get<0>(testParams).minMarkerLengthRatioOriginalImg;
     }
-    const int markerSize = 144;
-    const size_t numMarkersInRow = 10; // 1440*1440 = 1920*1080
-    MarkerPainter painter(markerSize);
+    const int markerSize = get<1>(testParams).first;       // 1440 or 480 or 144
+    const int numMarkersInRow = get<1>(testParams).second; // 1 or 3 or 144
+    MarkerPainter painter(markerSize);                     // num pixels is 1440x1440 as in FHD 1920x1080
     auto image_map = painter.getProjectMarkersTile(numMarkersInRow, detectorParams, dictionary);
 
     // detect markers
@@ -265,78 +283,9 @@ PERF_TEST_P(EstimateLargeAruco, ArucoFHD, ESTIMATE_LARGE_PARAMS)
     {
         aruco::detectMarkers(image_map.first, dictionary, corners, ids, detectorParams);
     }
-    ASSERT_EQ(numMarkersInRow*numMarkersInRow, ids.size());
-    for (size_t i = 0; i < ids.size(); i++)
-    {
-        int id = ids[i];
-        for (int c = 0; c < 4; c++)
-            {
-                std::vector<Point2f> test = image_map.second[id];
-                double dist = cv::norm(image_map.second[id][c] - corners[i][c]);
-                EXPECT_LE(dist, 5.0);
-            }
-    }
-    SANITY_CHECK_NOTHING();
-}
-
-// TODO: delete this test
-TEST(EstimateAruco, ArucoTryTest)
-{
-    Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
-    Ptr<aruco::DetectorParameters> params = aruco::DetectorParameters::create();
-    params->minDistanceToBorder = 1;
-
-    //USE_ARUCO3
-    params->useAruco3Detection = true;
-    params->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
-    params->minSideLengthCanonicalImg = 16;
-    params->markerBorderBits = 1;
-    MarkerPainter painter(100);
-    int iter = 0;
-    // detect from different positions
-    for(int pitch = 0; pitch < 360; pitch += 70) {
-        for (int yaw = 70; yaw <= 120; yaw += 40) {
-            int currentId = iter % 250;
-            auto marker_corners = painter.getProjectMarker(currentId, deg2rad(yaw), deg2rad(pitch), params, dictionary);
-
-            // detect markers
-            vector<vector<Point2f> > corners;
-            vector<int> ids;
-            aruco::detectMarkers(marker_corners.first, dictionary, corners, ids, params);
-            // check results
-            ASSERT_EQ(1ull, ids.size());
-            ASSERT_EQ(currentId, ids[0]);
-            for (int c = 0; c < 4; c++)
-            {
-                double dist = cv::norm(marker_corners.second[c] - corners[0][c]);
-                EXPECT_LE(dist, 5.0);
-            }
-            iter++;
-        }
-    }
-    params->minMarkerLengthRatioOriginalImg = 0.1f;
-    iter = 0;
-    // detect from different positions
-    for(int pitch = 0; pitch < 360; pitch += 70) {
-        for (int yaw = 70; yaw <= 120; yaw += 40) {
-            int currentId = iter % 250;
-            auto marker_corners = painter.getProjectMarker(currentId, deg2rad(yaw), deg2rad(pitch), params, dictionary);
-
-            // detect markers
-            vector<vector<Point2f> > corners;
-            vector<int> ids;
-            aruco::detectMarkers(marker_corners.first, dictionary, corners, ids, params);
-            // check results
-            ASSERT_EQ(1ull, ids.size());
-            ASSERT_EQ(currentId, ids[0]);
-            for (int c = 0; c < 4; c++)
-            {
-                double dist = cv::norm(marker_corners.second[c] - corners[0][c]);
-                EXPECT_LE(dist, 5.0);
-            }
-            iter++;
-        }
-    }
+    ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(ids.size()));
+    auto distVector = getDistanceVector(image_map.second, ids, corners);
+    SANITY_CHECK(distVector, 3.0*4.0, ERROR_ABSOLUTE);
 }
 
 }

modules/aruco/src/aruco.cpp

@@ -738,6 +738,7 @@ static size_t _findOptPyrImageForCanonicalImg(
 /**
  * @brief Identify square candidates according to a marker dictionary
  */
+
 static void _identifyCandidates(InputArray _image,
                                 const std::vector<cv::Mat>& _image_pyr,
                                 const std::vector<cv::Size>& _image_pyr_sizes,
@@ -771,8 +772,8 @@ static void _identifyCandidates(InputArray _image,
 
             // implements equation (4)
             if (params->useAruco3Detection) {
-                const int perimeter_in_seg_img = (int)contourS[i].size();
-                const size_t n = _findOptPyrImageForCanonicalImg(_image_pyr_sizes, _image.size(), perimeter_in_seg_img, min_perimeter);
+                const int perimeter_scaled_img = (int)contourS[i].size();
+                const size_t n = _findOptPyrImageForCanonicalImg(_image_pyr_sizes, _image.size(), perimeter_scaled_img, min_perimeter);
                 const Mat& pyr_img = _image_pyr[n];
                 const float scale = (float)_image_pyr_sizes[n].width / _image.cols();
                 validCandidates[i] = _identifyOneCandidate(_dictionary, pyr_img, candidates[i], currId, params, rotated[i], scale);
@@ -1154,8 +1155,8 @@ float detectMarkers(InputArray _image, const Ptr<Dictionary> &_dictionary, Outpu
     const float scale_pyr = 2.f;
     //// Step 1.1: resize image with equation (1) from paper [1]
     if (_params->useAruco3Detection) {
-        const float img_area = grey.rows * grey.cols;
-        const float min_area_marker = _params->minSideLengthCanonicalImg * _params->minSideLengthCanonicalImg;
+        const float img_area = static_cast<float>(grey.rows*grey.cols);
+        const float min_area_marker = static_cast<float>(_params->minSideLengthCanonicalImg*_params->minSideLengthCanonicalImg);
         // find max level
         num_levels = static_cast<int>(log2(img_area / min_area_marker)/scale_pyr);
         // the closest pyramid image to the downsampled segmentation image