rapid: make correspondencies explicit for simpler data handling

modules/rapid/include/opencv2/rapid.hpp

@@ -24,12 +24,12 @@ namespace rapid
 /**
  * Debug draw markers of matched correspondences onto a lineBundle
  * @param bundle the lineBundle
- * @param srcLocations the according source locations
- * @param newLocations matched source locations
+ * @param cols column coordinates in the line bundle
  * @param colors colors for the markers. Defaults to white.
  */
-CV_EXPORTS_W void drawCorrespondencies(InputOutputArray bundle, InputArray srcLocations,
-                                       InputArray newLocations, InputArray colors = noArray());
+CV_EXPORTS_W void drawCorrespondencies(InputOutputArray bundle, InputArray cols,
+                                       InputArray colors = noArray());
+
 /**
  * Debug draw search lines onto an image
  * @param img the output image
@@ -83,20 +83,22 @@ CV_EXPORTS_W void extractLineBundle(int len, InputArray ctl2d, InputArray img, O
  * Find corresponding image locations by searching for a maximal sobel edge along the search line (a single
  * row in the bundle)
  * @param bundle the line bundle
- * @param srcLocations the according source image location
- * @param newLocations image locations with maximal edge along the search line
+ * @param cols correspondence-position per line in line-bundle-space
  * @param response the sobel response for the selected point
  */
-CV_EXPORTS_W void findCorrespondencies(InputArray bundle, InputArray srcLocations, OutputArray newLocations,
+CV_EXPORTS_W void findCorrespondencies(InputArray bundle, OutputArray cols,
                                        OutputArray response = noArray());
 
 /**
- * Filter corresponding 2d and 3d points based on mask
+ * Collect corresponding 2d and 3d points based on correspondencies and mask
+ * @param cols correspondence-position per line in line-bundle-space
+ * @param srcLocations the source image location
  * @param pts2d 2d points
  * @param pts3d 3d points
  * @param mask mask containing non-zero values for the elements to be retained
  */
-CV_EXPORTS_W void filterCorrespondencies(InputOutputArray pts2d, InputOutputArray pts3d, InputArray mask);
+CV_EXPORTS_W void convertCorrespondencies(InputArray cols, InputArray srcLocations, OutputArray pts2d,
+                                          InputOutputArray pts3d = noArray(), InputArray mask = noArray());
 
 /**
  * High level function to execute a single rapid @cite harris1990rapid iteration
@@ -104,7 +106,7 @@ CV_EXPORTS_W void filterCorrespondencies(InputOutputArray pts2d, InputOutputArra
  * 1. @ref extractControlPoints
  * 2. @ref extractLineBundle
  * 3. @ref findCorrespondencies
- * 4. @ref filterCorrespondencies
+ * 4. @ref convertCorrespondencies
  * 5. @ref solvePnPRefineLM
  *
  * @param img the video frame
@@ -115,10 +117,11 @@ CV_EXPORTS_W void filterCorrespondencies(InputOutputArray pts2d, InputOutputArra
  * @param K camera matrix
  * @param rvec rotation between mesh and camera. Input values are used as an initial solution.
  * @param tvec translation between mesh and camera. Input values are used as an initial solution.
+ * @param rmsd the 2d reprojection difference
  * @return ratio of search lines that could be extracted and matched
  */
 CV_EXPORTS_W float rapid(InputArray img, int num, int len, InputArray pts3d, InputArray tris, InputArray K,
-                         InputOutputArray rvec, InputOutputArray tvec);
+                         InputOutputArray rvec, InputOutputArray tvec, CV_OUT double* rmsd = 0);
 //! @}
 } /* namespace rapid */
 } /* namespace cv */

modules/rapid/samples/track_marker.py

@@ -35,7 +35,7 @@ while cv.waitKey(1) != 27:
     # tracking and refinement with rapid
     if rot is not None:
         for i in range(5):  # multiple iterations
-            ratio, rot, trans = cv.rapid.rapid(img, 40, line_len, obj_points, tris, K, rot, trans)
+            ratio, rot, trans = cv.rapid.rapid(img, 40, line_len, obj_points, tris, K, rot, trans)[:3]
             if ratio < 0.8:
                 # bad quality, force re-detect
                 rot, trans = None, None

modules/rapid/src/rapid.cpp

@@ -223,19 +223,13 @@ static void compute1DSobel(const Mat& src, Mat& dst)
     }
 }
 
-void findCorrespondencies(InputArray bundle, InputArray _srcLocations, OutputArray _newLocations,
-                          OutputArray _response)
+void findCorrespondencies(InputArray bundle, OutputArray _cols, OutputArray _response)
 {
-    CV_Assert(bundle.size() == _srcLocations.size());
-    CV_CheckTypeEQ(_srcLocations.type(), CV_16SC2, "Vec2s data type expected");
-
     Mat_<uchar> sobel;
     compute1DSobel(bundle.getMat(), sobel);
 
-    _newLocations.create(sobel.rows, 1, CV_16SC2);
-
-    Mat newLocations = _newLocations.getMat();
-    Mat srcLocations = _srcLocations.getMat();
+    _cols.create(sobel.rows, 1, CV_32S);
+    Mat_<int> cols = _cols.getMat();
 
     Mat_<uchar> response;
     if (_response.needed()) {
@@ -267,64 +261,75 @@ void findCorrespondencies(InputArray bundle, InputArray _srcLocations, OutputArr
         if (!response.empty())
             response(i) = mx;
 
-        newLocations.at<Vec2s>(i, 0) = srcLocations.at<Vec2s>(i, pos);
+        cols(i) = pos;
     }
 }
 
-void drawCorrespondencies(InputOutputArray _bundle, InputArray _srcLocations, InputArray _newLocations,
-                          InputArray _colors)
+void drawCorrespondencies(InputOutputArray _bundle, InputArray _cols, InputArray _colors)
 {
-    CV_CheckTypeEQ(_srcLocations.type(), CV_16SC2, "Vec2s data type expected");
-    CV_CheckTypeEQ(_newLocations.type(), CV_16SC2, "Vec2s data type expected");
-    CV_Assert(_bundle.size() == _srcLocations.size());
-    CV_Assert(_colors.empty() || _colors.rows() == _srcLocations.rows());
+    CV_CheckTypeEQ(_cols.type(), CV_32S, "cols must be of int type");
+    CV_Assert(_bundle.rows() == _cols.rows());
+    CV_Assert(_colors.empty() || _colors.rows() == _cols.rows());
 
     Mat bundle = _bundle.getMat();
-    Mat_<Vec2s> srcLocations = _srcLocations.getMat();
-    Mat_<Vec2s> newLocations = _newLocations.getMat();
+    Mat_<int> cols = _cols.getMat();
     Mat_<Vec4d> colors = _colors.getMat();
 
     for (int i = 0; i < bundle.rows; i++) {
-        const Vec2s& ref = newLocations(i);
-        for (int j = 1; j < bundle.cols - 1; j++) {
-            if (ref == srcLocations(i, j)) {
-                bundle(Rect(Point(j, i), Size(1, 1))) = colors.empty() ? Scalar::all(255) : colors(i);
-            }
-        }
+        bundle(Rect(Point(cols(i), i), Size(1, 1))) = colors.empty() ? Scalar::all(255) : colors(i);
     }
 }
 
-void filterCorrespondencies(InputOutputArray _pts2d, InputOutputArray _pts3d, InputArray _mask)
+void convertCorrespondencies(InputArray _cols, InputArray _srcLocations, OutputArray _pts2d,
+                             InputOutputArray _pts3d, InputArray _mask)
 {
-    CV_CheckTypeEQ(_mask.type(), CV_8UC1, "mask must be of uchar type");
-    CV_Assert(_pts2d.rows() == _pts3d.rows() && _pts2d.rows() == _mask.rows());
+    CV_CheckTypeEQ(_cols.type(), CV_32S, "cols must be of int type");
+    CV_CheckTypeEQ(_srcLocations.type(), CV_16SC2, "Vec2s data type expected");
+    CV_Assert(_srcLocations.rows() == _cols.rows());
 
-    Mat pts2d = _pts2d.getMat();
-    Mat pts3d = _pts3d.getMat();
-    Mat_<uchar> mask = _mask.getMat();
+    Mat_<cv::Vec2s> srcLocations = _srcLocations.getMat();
+    Mat_<int> cols = _cols.getMat();
 
-    Mat opts3d(0, 1, pts3d.type());
-    opts3d.reserve(mask.rows);
-    Mat opts2d(0, 1, pts2d.type());
-    opts2d.reserve(mask.rows);
+    Mat pts2d = Mat(0, 1, CV_16SC2);
+    pts2d.reserve(cols.rows);
 
-    for (int i = 0; i < mask.rows; i++) {
-        if (!mask(i))
-            continue;
-
-        opts2d.push_back(pts2d.row(i));
-        opts3d.push_back(pts3d.row(i));
+    Mat_<uchar> mask;
+    if (!_mask.empty())
+    {
+        CV_CheckTypeEQ(_mask.type(), CV_8UC1, "mask must be of uchar type");
+        CV_Assert(_cols.rows() == _mask.rows());
+        mask = _mask.getMat();
     }
 
-    Mat(opts3d).copyTo(_pts3d);
-    Mat(opts2d).copyTo(_pts2d);
+    Mat pts3d;
+    Mat opts3d;
+    if(!_pts3d.empty())
+    {
+        CV_Assert(_cols.rows() == _pts3d.rows());
+        pts3d = _pts3d.getMat();
+        opts3d.create(0, 1, pts3d.type());
+        opts3d.reserve(cols.rows);
+    }
+
+    for (int i = 0; i < cols.rows; i++) {
+        if (!mask.empty() && !mask(i))
+            continue;
+
+        pts2d.push_back(srcLocations(i, cols(i)));
+        if(!pts3d.empty())
+            opts3d.push_back(pts3d.row(i));
+    }
+
+    pts2d.copyTo(_pts2d);
+    if(!pts3d.empty())
+        opts3d.copyTo(_pts3d);
 }
 
 float rapid(InputArray img, int num, int len, InputArray vtx, InputArray tris, InputArray K,
-            InputOutputArray rvec, InputOutputArray tvec)
+            InputOutputArray rvec, InputOutputArray tvec, double* rmsd)
 {
     CV_Assert(num >= 3);
-    Mat pts2d, pts3d, correspondencies;
+    Mat pts2d, pts3d;
     extractControlPoints(num, len, vtx, rvec, tvec, K, img.size(), tris, pts2d, pts3d);
     if (pts2d.empty())
         return 0;
@@ -332,18 +337,26 @@ float rapid(InputArray img, int num, int len, InputArray vtx, InputArray tris, I
     Mat lineBundle, imgLoc;
     extractLineBundle(len, pts2d, img, lineBundle, imgLoc);
 
-    Mat response;
-    findCorrespondencies(lineBundle, imgLoc, correspondencies, response);
+    Mat cols, response;
+    findCorrespondencies(lineBundle, cols, response);
 
     const uchar sobel_thresh = 20;
-    filterCorrespondencies(correspondencies, pts3d, response > sobel_thresh);
+    Mat mask = response > sobel_thresh;
+    convertCorrespondencies(cols, imgLoc, pts2d, pts3d, mask);
 
-    if (correspondencies.rows < 3)
+    if(rmsd)
+    {
+        cols.copyTo(cols, mask);
+        cols -= len + 1;
+        *rmsd = std::sqrt(norm(cols, NORM_L2SQR) / cols.rows);
+    }
+
+    if (pts2d.rows < 3)
         return 0;
 
-    solvePnPRefineLM(pts3d, correspondencies, K, cv::noArray(), rvec, tvec);
+    solvePnPRefineLM(pts3d, pts2d, K, cv::noArray(), rvec, tvec);
 
-    return float(correspondencies.rows) / num;
+    return float(pts2d.rows) / num;
 }
 
 } /* namespace rapid */