From 31ec9b2aa723e36ef1f9280b9ba074e6264f51e6 Mon Sep 17 00:00:00 2001 From: GFleishman Date: Tue, 7 Apr 2020 15:58:25 -0400 Subject: [PATCH] Merge pull request #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 | 47 ++++++++++ modules/calib3d/src/ptsetreg.cpp | 104 +++++++++++++++++++++ .../calib3d/test/test_translation3d_estimator.cpp | 100 ++++++++++++++++++++ 3 files changed, 251 insertions(+) create mode 100644 modules/calib3d/test/test_translation3d_estimator.cpp diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 92d8dbc..63e82fd 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/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 diff --git a/modules/calib3d/src/ptsetreg.cpp b/modules/calib3d/src/ptsetreg.cpp index cb7de1b..e8ae3a6 100644 --- a/modules/calib3d/src/ptsetreg.cpp +++ b/modules/calib3d/src/ptsetreg.cpp @@ -505,6 +505,86 @@ public: } }; + +/* + * 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(); + const Point3f* to = m2.ptr(); + + 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(); + const Point3f* to = m2.ptr(); + const double* F = model.ptr(); + + int count = m1.checkVector(3); + CV_Assert( count > 0 ); + + _err.create(count, 1, CV_32F); + Mat err = _err.getMat(); + float* errptr = err.ptr(); + + 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(), 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(), 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, diff --git a/modules/calib3d/test/test_translation3d_estimator.cpp b/modules/calib3d/test/test_translation3d_estimator.cpp new file mode 100644 index 0000000..88ad40e --- /dev/null +++ b/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(0) = Point3f(rng.uniform(1.0f, 2.0f), rng.uniform(1.0f, 2.0f), rng.uniform(5.0f, 6.0f)); + fpts.at(1) = Point3f(rng.uniform(3.0f, 4.0f), rng.uniform(3.0f, 4.0f), rng.uniform(5.0f, 6.0f)); + fpts.at(2) = Point3f(rng.uniform(1.0f, 2.0f), rng.uniform(3.0f, 4.0f), rng.uniform(5.0f, 6.0f)); + fpts.at(3) = Point3f(rng.uniform(3.0f, 4.0f), rng.uniform(1.0f, 2.0f), rng.uniform(5.0f, 6.0f)); + + std::transform(fpts.ptr(), fpts.ptr() + 4, tpts.ptr(), + [&] (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 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(), fpts.ptr() + n, tpts.ptr(), + [&] (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() + m, tpts.ptr() + n, tpts.ptr() + 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 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 -- 2.7.4