Feature/aruco speedup (rebased with 4.x)

modules/aruco/include/opencv2/aruco.hpp

@@ -146,6 +146,24 @@ enum CornerRefineMethod{
  *   Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8). (default 0.0)
  * - detectInvertedMarker: to check if there is a white marker. In order to generate a "white" marker just
  *   invert a normal marker by using a tilde, ~markerImage. (default false)
+ * - useAruco3Detection: to enable the new and faster Aruco detection strategy. The most important observation from the authors of
+ *   Romero-Ramirez et al: Speeded up detection of squared fiducial markers (2018) is, that the binary
+ *   code of a marker can be reliably detected if the canonical image (that is used to extract the binary code)
+ *   has a size of minSideLengthCanonicalImg (in practice tau_c=16-32 pixels).
+ *   Link to article: https://www.researchgate.net/publication/325787310_Speeded_Up_Detection_of_Squared_Fiducial_Markers
+ *   In addition, very small markers are barely useful for pose estimation and thus a we can define a minimum marker size that we
+ *   still want to be able to detect (e.g. 50x50 pixel).
+ *   To decouple this from the initial image size they propose to resize the input image
+ *   to (I_w_r, I_h_r) = (tau_c / tau_dot_i) * (I_w, I_h), with tau_dot_i = tau_c + max(I_w,I_h) * tau_i.
+ *   Here tau_i (parameter: minMarkerLengthRatioOriginalImg) is a ratio in the range [0,1].
+ *   If we set this to 0, the smallest marker we can detect
+ *   has a side length of tau_c. If we set it to 1 the marker would fill the entire image.
+ *   For a FullHD video a good value to start with is 0.1.
+ * - minSideLengthCanonicalImg: minimum side length of a marker in the canonical image.
+ *   Latter is the binarized image in which contours are searched.
+ *   So all contours with a size smaller than minSideLengthCanonicalImg*minSideLengthCanonicalImg will omitted from the search.
+ * - minMarkerLengthRatioOriginalImg:  range [0,1], eq (2) from paper
+ *   The parameter tau_i has a direct influence on the processing speed.
  */
 struct CV_EXPORTS_W DetectorParameters {
 
@@ -188,6 +206,12 @@ struct CV_EXPORTS_W DetectorParameters {
 
     // to detect white (inverted) markers
     CV_PROP_RW bool detectInvertedMarker;
+
+    // New Aruco functionality proposed in the paper:
+    // Romero-Ramirez et al: Speeded up detection of squared fiducial markers (2018)
+    CV_PROP_RW bool useAruco3Detection;
+    CV_PROP_RW int minSideLengthCanonicalImg;
+    CV_PROP_RW float minMarkerLengthRatioOriginalImg;
 };
 
 

modules/aruco/perf/perf_aruco.cpp

@@ -0,0 +1,297 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+#include "perf_precomp.hpp"
+
+namespace opencv_test {
+using namespace perf;
+
+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 = 0;
+    Mat cameraMatrix;
+public:
+    MarkerPainter(const int size)
+    {
+        setImgMarkerSize(size);
+    }
+
+    void setImgMarkerSize(const int size)
+    {
+        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.0;
+        cameraMatrix.at<double>(1, 2) = imgMarkerSize / 2.0;
+    }
+
+    static std::pair<Mat, Mat> getSyntheticRT(double yaw, double pitch, double distance)
+    {
+        auto rvec_tvec = std::make_pair(Mat(3, 1, CV_64FC1), Mat(3, 1, CV_64FC1));
+        Mat& rvec = rvec_tvec.first;
+        Mat& tvec = rvec_tvec.second;
+
+        // Rvec
+        // first put the Z axis aiming to -X (like the camera axis system)
+        Mat rotZ(3, 1, CV_64FC1);
+        rotZ.ptr<double>(0)[0] = 0;
+        rotZ.ptr<double>(0)[1] = 0;
+        rotZ.ptr<double>(0)[2] = -0.5 * CV_PI;
+
+        Mat rotX(3, 1, CV_64FC1);
+        rotX.ptr<double>(0)[0] = 0.5 * CV_PI;
+        rotX.ptr<double>(0)[1] = 0;
+        rotX.ptr<double>(0)[2] = 0;
+
+        Mat camRvec, camTvec;
+        composeRT(rotZ, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotX, Mat(3, 1, CV_64FC1, Scalar::all(0)),
+                  camRvec, camTvec);
+
+        // now pitch and yaw angles
+        Mat rotPitch(3, 1, CV_64FC1);
+        rotPitch.ptr<double>(0)[0] = 0;
+        rotPitch.ptr<double>(0)[1] = pitch;
+        rotPitch.ptr<double>(0)[2] = 0;
+
+        Mat rotYaw(3, 1, CV_64FC1);
+        rotYaw.ptr<double>(0)[0] = yaw;
+        rotYaw.ptr<double>(0)[1] = 0;
+        rotYaw.ptr<double>(0)[2] = 0;
+
+        composeRT(rotPitch, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotYaw,
+                  Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec);
+
+        // compose both rotations
+        composeRT(camRvec, Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec,
+                  Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec);
+
+        // Tvec, just move in z (camera) direction the specific distance
+        tvec.ptr<double>(0)[0] = 0.;
+        tvec.ptr<double>(0)[1] = 0.;
+        tvec.ptr<double>(0)[2] = distance;
+        return rvec_tvec;
+}
+
+    std::pair<Mat, vector<Point2f> > getProjectMarker(int id, double yaw, double pitch,
+                                                      const Ptr<aruco::DetectorParameters>& parameters,
+                                                      const Ptr<aruco::Dictionary>& dictionary)
+    {
+        auto marker_corners = std::make_pair(Mat(imgMarkerSize, imgMarkerSize, CV_8UC1, Scalar::all(255)), vector<Point2f>());
+        Mat& img = marker_corners.first;
+        vector<Point2f>& corners = marker_corners.second;
+
+        // canonical image
+        const int markerSizePixels = static_cast<int>(imgMarkerSize/sqrt(2.f));
+        aruco::drawMarker(dictionary, id, markerSizePixels, img, parameters->markerBorderBits);
+
+        // get rvec and tvec for the perspective
+        const double distance = 0.1;
+        auto rvec_tvec = MarkerPainter::getSyntheticRT(yaw, pitch, distance);
+        Mat& rvec = rvec_tvec.first;
+        Mat& tvec = rvec_tvec.second;
+
+        const float markerLength = 0.05f;
+        vector<Point3f> markerObjPoints;
+        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.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);
+
+        warpPerspective(img, img, transformation, Size(imgMarkerSize, imgMarkerSize), INTER_NEAREST, BORDER_CONSTANT,
+                        Scalar::all(255));
+        return marker_corners;
+    }
+
+    std::pair<Mat, map<int, vector<Point2f> > > getProjectMarkersTile(const int numMarkers,
+                                                                      const Ptr<aruco::DetectorParameters>& params,
+                                                                      const Ptr<aruco::Dictionary>& dictionary)
+    {
+        Mat tileImage(imgMarkerSize*numMarkers, imgMarkerSize*numMarkers, CV_8UC1, Scalar::all(255));
+        map<int, vector<Point2f> > idCorners;
+
+        int iter = 0, pitch = 0, yaw = 0;
+        for (int i = 0; i < numMarkers; i++)
+        {
+            for (int j = 0; j < numMarkers; j++)
+            {
+                int currentId = iter;
+                auto marker_corners = getProjectMarker(currentId, deg2rad(70+yaw), deg2rad(pitch), params, dictionary);
+                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 += static_cast<Point2f>(startPoint);
+                idCorners[currentId] = marker_corners.second;
+                auto test = idCorners[currentId];
+                yaw = (yaw + 10) % 51; // 70+yaw >= 70 && 70+yaw <= 120
+                iter++;
+            }
+            pitch = (pitch + 60) % 360;
+        }
+        return std::make_pair(tileImage, idCorners);
+    }
+};
+
+static inline double getMaxDistance(map<int, vector<Point2f> > &golds, const vector<int>& ids,
+                                               const vector<vector<Point2f> >& corners)
+{
+    std::map<int, double> mapDist;
+    for (const auto& el : golds)
+        mapDist[el.first] = std::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()) {
+            double distance = 0.;
+            for (int c = 0; c < 4; c++)
+                distance = std::max(distance, cv::norm(gold_corners->second[c] - corners[i][c]));
+            mapDist[id] = distance;
+        }
+    }
+    return std::max_element(std::begin(mapDist), std::end(mapDist),
+           [](const pair<int, double>& p1, const pair<int, double>& p2){return p1.second < p2.second;})->second;
+}
+
+PERF_TEST_P(EstimateAruco, ArucoFirst, ESTIMATE_PARAMS)
+{
+    UseArucoParams 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;
+
+    const int markerSize = 100;
+    const int numMarkersInRow = 9;
+    //USE_ARUCO3
+    detectorParams->useAruco3Detection = get<0>(testParams);
+    if (detectorParams->useAruco3Detection) {
+        detectorParams->minSideLengthCanonicalImg = 32;
+        detectorParams->minMarkerLengthRatioOriginalImg = 0.04f / numMarkersInRow;
+    }
+    MarkerPainter painter(markerSize);
+    auto image_map = painter.getProjectMarkersTile(numMarkersInRow, detectorParams, dictionary);
+
+    // detect markers
+    vector<vector<Point2f> > corners;
+    vector<int> ids;
+    TEST_CYCLE()
+    {
+        aruco::detectMarkers(image_map.first, dictionary, corners, ids, detectorParams);
+    }
+    ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(ids.size()));
+    double maxDistance = getMaxDistance(image_map.second, ids, corners);
+    ASSERT_LT(maxDistance, 3.);
+    SANITY_CHECK_NOTHING();
+}
+
+PERF_TEST_P(EstimateAruco, ArucoSecond, ESTIMATE_PARAMS)
+{
+    UseArucoParams 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>(testParams);
+    if (detectorParams->useAruco3Detection) {
+        detectorParams->minSideLengthCanonicalImg = 64;
+        detectorParams->minMarkerLengthRatioOriginalImg = 0.f;
+    }
+    const int markerSize = 200;
+    const int numMarkersInRow = 11;
+    MarkerPainter painter(markerSize);
+    auto image_map = painter.getProjectMarkersTile(numMarkersInRow, detectorParams, dictionary);
+
+    // detect markers
+    vector<vector<Point2f> > corners;
+    vector<int> ids;
+    TEST_CYCLE()
+    {
+        aruco::detectMarkers(image_map.first, dictionary, corners, ids, detectorParams);
+    }
+    ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(ids.size()));
+    double maxDistance = getMaxDistance(image_map.second, ids, corners);
+    ASSERT_LT(maxDistance, 3.);
+    SANITY_CHECK_NOTHING();
+}
+
+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)
+    {
+        os << d.useAruco3Detection << " " << d.minMarkerLengthRatioOriginalImg << " " << d.minSideLengthCanonicalImg;
+        return os;
+    }
+};
+typedef tuple<Aruco3Params, pair<int, int>> ArucoTestParams;
+
+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.0069f, 16)),       \
+Values(std::make_pair(1440, 1), std::make_pair(480, 3), std::make_pair(144, 10)))
+
+PERF_TEST_P(EstimateLargeAruco, ArucoFHD, ESTIMATE_FHD_PARAMS)
+{
+    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>(testParams).useAruco3Detection;
+    if (detectorParams->useAruco3Detection) {
+        detectorParams->minSideLengthCanonicalImg = get<0>(testParams).minSideLengthCanonicalImg;
+        detectorParams->minMarkerLengthRatioOriginalImg = get<0>(testParams).minMarkerLengthRatioOriginalImg;
+    }
+    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
+    vector<vector<Point2f> > corners;
+    vector<int> ids;
+    TEST_CYCLE()
+    {
+        aruco::detectMarkers(image_map.first, dictionary, corners, ids, detectorParams);
+    }
+    ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(ids.size()));
+    double maxDistance = getMaxDistance(image_map.second, ids, corners);
+    ASSERT_LT(maxDistance, 3.);
+    SANITY_CHECK_NOTHING();
+}
+
+}

modules/aruco/perf/perf_main.cpp

@@ -0,0 +1,3 @@
+#include "perf_precomp.hpp"
+
+CV_PERF_TEST_MAIN(aruco)

modules/aruco/perf/perf_precomp.hpp

@@ -0,0 +1,11 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+#ifndef __OPENCV_PERF_PRECOMP_HPP__
+#define __OPENCV_PERF_PRECOMP_HPP__
+
+#include "opencv2/ts.hpp"
+#include "opencv2/aruco.hpp"
+#include "opencv2/calib3d.hpp"
+
+#endif

modules/aruco/samples/detector_params.yml

@@ -21,3 +21,10 @@ perspectiveRemoveIgnoredMarginPerCell: 0.13
 maxErroneousBitsInBorderRate: 0.04
 minOtsuStdDev: 5.0
 errorCorrectionRate: 0.6
+
+# new aruco 3 functionality
+useAruco3Detection: 0
+minSideLengthCanonicalImg: 32 # 16, 32, 64 --> tau_c from the paper
+minMarkerLengthRatioOriginalImg: 0.02 # range [0,0.2] --> tau_i from the paper
+cameraMotionSpeed: 0.1 # range [0,1) --> tau_s from the paper
+useGlobalThreshold: 0