Merge pull request opencv#16614 from GFleishman:estimateTranslation3D

added estimateTranslation3D to calib3d/ptsetreg

* added estimateTranslation3D; follows API and implementation structure for estimateAffine3D, but only allows for translation

* void variables in null function to suppress compiler warnings

* added test for estimateTranslation3D

* changed to Matx13d datatype for translation vector in ptsetreg and test; used short license in test

* removed iostream include

* calib3d: code cleanup

modules/calib3d/include/opencv2/calib3d.hpp

@@ -2869,6 +2869,53 @@ CV_EXPORTS_W  int estimateAffine3D(InputArray src, InputArray dst,
                                    OutputArray out, OutputArray inliers,
                                    double ransacThreshold = 3, double confidence = 0.99);
 
+/** @brief Computes an optimal translation between two 3D point sets.
+ *
+ * It computes
+ * \f[
+ * \begin{bmatrix}
+ * x\\
+ * y\\
+ * z\\
+ * \end{bmatrix}
+ * =
+ * \begin{bmatrix}
+ * X\\
+ * Y\\
+ * Z\\
+ * \end{bmatrix}
+ * +
+ * \begin{bmatrix}
+ * b_1\\
+ * b_2\\
+ * b_3\\
+ * \end{bmatrix}
+ * \f]
+ *
+ * @param src First input 3D point set containing \f$(X,Y,Z)\f$.
+ * @param dst Second input 3D point set containing \f$(x,y,z)\f$.
+ * @param out Output 3D translation vector \f$3 \times 1\f$ of the form
+ * \f[
+ * \begin{bmatrix}
+ * b_1 \\
+ * b_2 \\
+ * b_3 \\
+ * \end{bmatrix}
+ * \f]
+ * @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier).
+ * @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as
+ * an inlier.
+ * @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything
+ * between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation
+ * significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation.
+ *
+ * The function estimates an optimal 3D translation between two 3D point sets using the
+ * RANSAC algorithm.
+ *  */
+CV_EXPORTS_W  int estimateTranslation3D(InputArray src, InputArray dst,
+                                        OutputArray out, OutputArray inliers,
+                                        double ransacThreshold = 3, double confidence = 0.99);
+
 /** @brief Computes an optimal affine transformation between two 2D point sets.
 
 It computes

modules/calib3d/src/ptsetreg.cpp

@@ -505,6 +505,86 @@ class Affine3DEstimatorCallback : public PointSetRegistrator::Callback
     }
 };
 
+
+/*
+ * Compute
+ *  x      X     t1
+ *  y  =   Y  +  t2
+ *  z      Z     t3
+ *
+ *  - every element in _m1 contains (X,Y,Z), which are called source points
+ *  - every element in _m2 contains (x,y,z), which are called destination points
+ *  - _model is of size 3x1, which contains
+ *      t1
+ *      t2
+ *      t3
+ */
+class Translation3DEstimatorCallback CV_FINAL : public PointSetRegistrator::Callback
+{
+public:
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
+    {
+
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        const Point3f* from = m1.ptr<Point3f>();
+        const Point3f* to   = m2.ptr<Point3f>();
+
+        Matx13d T;
+
+        // The optimal translation is the mean of the pointwise displacements
+        for(int i = 0; i < 4; i++)
+        {
+            const Point3f& f = from[i];
+            const Point3f& t = to[i];
+
+            T(0, 0) = T(0, 0) + t.x - f.x;
+            T(0, 1) = T(0, 1) + t.y - f.y;
+            T(0, 2) = T(0, 2) + t.z - f.z;
+        }
+        T *= (1.0f / 4);
+        Mat(T, false).copyTo(_model);
+        return 1;
+    }
+
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
+        const Point3f* from = m1.ptr<Point3f>();
+        const Point3f* to   = m2.ptr<Point3f>();
+        const double* F = model.ptr<double>();
+
+        int count = m1.checkVector(3);
+        CV_Assert( count > 0 );
+
+        _err.create(count, 1, CV_32F);
+        Mat err = _err.getMat();
+        float* errptr = err.ptr<float>();
+
+        for(int i = 0; i < count; i++ )
+        {
+            const Point3f& f = from[i];
+            const Point3f& t = to[i];
+
+            double a = F[0] + f.x - t.x;
+            double b = F[1] + f.y - t.y;
+            double c = F[2] + f.z - t.z;
+
+            errptr[i] = (float)(a*a + b*b + c*c);
+        }
+    }
+
+    // not doing SVD, no degeneracy concerns, can simply return true
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
+    {
+        // voids to suppress compiler warnings
+        (void)_ms1;
+        (void)_ms2;
+        (void)count;
+        return true;
+    }
+};
+
+
 /*
  * Compute
  *  x     a  b   X    c
@@ -818,6 +898,30 @@ int estimateAffine3D(InputArray _from, InputArray _to,
     return createRANSACPointSetRegistrator(makePtr<Affine3DEstimatorCallback>(), 4, ransacThreshold, confidence)->run(dFrom, dTo, _out, _inliers);
 }
 
+int estimateTranslation3D(InputArray _from, InputArray _to,
+                          OutputArray _out, OutputArray _inliers,
+                          double ransacThreshold, double confidence)
+{
+    CV_INSTRUMENT_REGION();
+
+    Mat from = _from.getMat(), to = _to.getMat();
+    int count = from.checkVector(3);
+
+    CV_Assert( count >= 0 && to.checkVector(3) == count );
+
+    Mat dFrom, dTo;
+    from.convertTo(dFrom, CV_32F);
+    to.convertTo(dTo, CV_32F);
+    dFrom = dFrom.reshape(3, count);
+    dTo = dTo.reshape(3, count);
+
+    const double epsilon = DBL_EPSILON;
+    ransacThreshold = ransacThreshold <= 0 ? 3 : ransacThreshold;
+    confidence = (confidence < epsilon) ? 0.99 : (confidence > 1 - epsilon) ? 0.99 : confidence;
+
+    return createRANSACPointSetRegistrator(makePtr<Translation3DEstimatorCallback>(), 4, ransacThreshold, confidence)->run(dFrom, dTo, _out, _inliers);
+}
+
 Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers,
                      const int method, const double ransacReprojThreshold,
                      const size_t maxIters, const double confidence,

modules/calib3d/test/test_translation3d_estimator.cpp

@@ -0,0 +1,100 @@
+// 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 "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+TEST(Calib3d_EstimateTranslation3D, test4Points)
+{
+    Matx13d trans;
+    cv::randu(trans, Scalar(1), Scalar(3));
+
+    // setting points that are no in the same line
+
+    Mat fpts(1, 4, CV_32FC3);
+    Mat tpts(1, 4, CV_32FC3);
+
+    RNG& rng = theRNG();
+    fpts.at<Point3f>(0) = Point3f(rng.uniform(1.0f, 2.0f), rng.uniform(1.0f, 2.0f), rng.uniform(5.0f, 6.0f));
+    fpts.at<Point3f>(1) = Point3f(rng.uniform(3.0f, 4.0f), rng.uniform(3.0f, 4.0f), rng.uniform(5.0f, 6.0f));
+    fpts.at<Point3f>(2) = Point3f(rng.uniform(1.0f, 2.0f), rng.uniform(3.0f, 4.0f), rng.uniform(5.0f, 6.0f));
+    fpts.at<Point3f>(3) = Point3f(rng.uniform(3.0f, 4.0f), rng.uniform(1.0f, 2.0f), rng.uniform(5.0f, 6.0f));
+
+    std::transform(fpts.ptr<Point3f>(), fpts.ptr<Point3f>() + 4, tpts.ptr<Point3f>(),
+        [&] (const Point3f& p) -> Point3f
+        {
+            return Point3f((float)(p.x + trans(0, 0)),
+                           (float)(p.y + trans(0, 1)),
+                           (float)(p.z + trans(0, 2)));
+        }
+    );
+
+    Matx13d trans_est;
+    vector<uchar> outliers;
+    int res = estimateTranslation3D(fpts, tpts, trans_est, outliers);
+    EXPECT_GT(res, 0);
+
+    const double thres = 1e-3;
+
+    EXPECT_LE(cvtest::norm(trans_est, trans, NORM_INF), thres)
+        << "aff est: " << trans_est << endl
+        << "aff ref: " << trans;
+}
+
+TEST(Calib3d_EstimateTranslation3D, testNPoints)
+{
+    Matx13d trans;
+    cv::randu(trans, Scalar(-2), Scalar(2));
+
+    // setting points that are no in the same line
+
+    const int n = 100;
+    const int m = 3*n/5;
+    const Point3f shift_outl = Point3f(15, 15, 15);
+    const float noise_level = 20.f;
+
+    Mat fpts(1, n, CV_32FC3);
+    Mat tpts(1, n, CV_32FC3);
+
+    randu(fpts, Scalar::all(0), Scalar::all(100));
+    std::transform(fpts.ptr<Point3f>(), fpts.ptr<Point3f>() + n, tpts.ptr<Point3f>(),
+        [&] (const Point3f& p) -> Point3f
+        {
+            return Point3f((float)(p.x + trans(0, 0)),
+                           (float)(p.y + trans(0, 1)),
+                           (float)(p.z + trans(0, 2)));
+        }
+    );
+
+    /* adding noise*/
+    std::transform(tpts.ptr<Point3f>() + m, tpts.ptr<Point3f>() + n, tpts.ptr<Point3f>() + m,
+        [&] (const Point3f& pt) -> Point3f
+        {
+            Point3f p = pt + shift_outl;
+            RNG& rng = theRNG();
+            return Point3f(p.x + noise_level * (float)rng,
+                           p.y + noise_level * (float)rng,
+                           p.z + noise_level * (float)rng);
+        }
+    );
+
+    Matx13d trans_est;
+    vector<uchar> outl;
+    int res = estimateTranslation3D(fpts, tpts, trans_est, outl);
+    EXPECT_GT(res, 0);
+
+    const double thres = 1e-4;
+    EXPECT_LE(cvtest::norm(trans_est, trans, NORM_INF), thres)
+        << "aff est: " << trans_est << endl
+        << "aff ref: " << trans;
+
+    bool outl_good = count(outl.begin(), outl.end(), 1) == m &&
+        m == accumulate(outl.begin(), outl.begin() + m, 0);
+
+    EXPECT_TRUE(outl_good);
+}
+
+}} // namespace