diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index a7756e27e3..e08f5f4b3f 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -2076,6 +2076,78 @@ CV_EXPORTS_W cv::Mat estimateAffinePartial2D(InputArray from, InputArray to, Out size_t maxIters = 2000, double confidence = 0.99, size_t refineIters = 10); +/** @brief Computes a pure 2D translation between two 2D point sets. + +It computes +\f[ +\begin{bmatrix} +x\\ +y +\end{bmatrix} += +\begin{bmatrix} +1 & 0\\ +0 & 1 +\end{bmatrix} +\begin{bmatrix} +X\\ +Y +\end{bmatrix} ++ +\begin{bmatrix} +t_x\\ +t_y +\end{bmatrix}. +\f] + +@param from First input 2D point set containing \f$(X,Y)\f$. +@param to Second input 2D point set containing \f$(x,y)\f$. +@param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier). +@param method Robust method used to compute the transformation. The following methods are possible: +- @ref RANSAC - RANSAC-based robust method +- @ref LMEDS - Least-Median robust method +RANSAC is the default method. +@param ransacReprojThreshold Maximum reprojection error in the RANSAC algorithm to consider +a point as an inlier. Applies only to RANSAC. +@param maxIters The maximum number of robust method iterations. +@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. +@param refineIters Maximum number of iterations of the refining algorithm. For pure translation +the least-squares solution on inliers is closed-form, so passing 0 is recommended (no additional refine). + +@return A 2D translation vector \f$[t_x, t_y]^T\f$ as `cv::Vec2d`. If the translation could not be +estimated, both components are set to NaN and, if @p inliers is provided, the mask is filled with zeros. + +\par Converting to a 2x3 transformation matrix: +\f[ +\begin{bmatrix} +1 & 0 & t_x\\ +0 & 1 & t_y +\end{bmatrix} +\f] + +@code{.cpp} +cv::Vec2d t = cv::estimateTranslation2D(from, to, inliers); +cv::Mat T = (cv::Mat_(2,3) << 1,0,t[0], 0,1,t[1]); +@endcode + +The function estimates a pure 2D translation between two 2D point sets using the selected robust +algorithm. Inliers are determined by the reprojection error threshold. + +@note +The RANSAC method can handle practically any ratio of outliers but needs a threshold to +distinguish inliers from outliers. The method LMeDS does not need any threshold but works +correctly only when there are more than 50% inliers. + +@sa estimateAffine2D, estimateAffinePartial2D, getAffineTransform +*/ +CV_EXPORTS_W cv::Vec2d estimateTranslation2D(InputArray from, InputArray to, OutputArray inliers = noArray(), + int method = RANSAC, + double ransacReprojThreshold = 3, + size_t maxIters = 2000, double confidence = 0.99, + size_t refineIters = 0); + /** @example samples/cpp/tutorial_code/features/Homography/decompose_homography.cpp An example program with homography decomposition. diff --git a/modules/3d/perf/perf_translation2d.cpp b/modules/3d/perf/perf_translation2d.cpp new file mode 100644 index 0000000000..702325faf4 --- /dev/null +++ b/modules/3d/perf/perf_translation2d.cpp @@ -0,0 +1,124 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// (3-clause BSD License) +// +// Copyright (C) 2015-2016, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the names of the copyright holders nor the names of the contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall copyright holders or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "perf_precomp.hpp" +#include +#include + +namespace opencv_test +{ +using namespace perf; + +CV_ENUM(Method, RANSAC, LMEDS) +typedef tuple TranslationParams; +typedef TestBaseWithParam EstimateTranslation2DPerf; +#define ESTIMATE_PARAMS Combine(Values(1000), Values(0.95), Method::all(), Values(10, 0)) + +static float rngIn(float from, float to) { return from + (to - from) * (float)theRNG(); } + +static cv::Mat rngTranslationMat() +{ + double tx = rngIn(-2.f, 2.f); + double ty = rngIn(-2.f, 2.f); + double t[2*3] = { 1.0, 0.0, tx, + 0.0, 1.0, ty }; + return cv::Mat(2, 3, CV_64F, t).clone(); +} + +PERF_TEST_P(EstimateTranslation2DPerf, EstimateTranslation2D, ESTIMATE_PARAMS) +{ + TranslationParams params = GetParam(); + const int n = get<0>(params); + const double confidence = get<1>(params); + const int method = get<2>(params); + const size_t refining = get<3>(params); + + //fixed seed so the generated data are deterministic + cv::theRNG().state = 0x12345678; + // ground-truth pure translation + cv::Mat T = rngTranslationMat(); + + // LMEDS can't handle more than 50% outliers (by design) + int m; + if (method == LMEDS) + m = 3*n/5; + else + m = 2*n/5; + + const float shift_outl = 15.f; + const float noise_level = 20.f; + + cv::Mat fpts(1, n, CV_32FC2); + cv::Mat tpts(1, n, CV_32FC2); + + randu(fpts, 0.f, 100.f); + transform(fpts, tpts, T); + + // add outliers to the tail [m, n) + cv::Mat outliers = tpts.colRange(m, n); + outliers.reshape(1) += shift_outl; + + cv::Mat noise(outliers.size(), outliers.type()); + randu(noise, 0.f, noise_level); + outliers += noise; + + cv::Vec2d T_est; + std::vector inliers(n); + + warmup(inliers, WARMUP_WRITE); + warmup(fpts, WARMUP_READ); + warmup(tpts, WARMUP_READ); + + TEST_CYCLE() + { + T_est = estimateTranslation2D(fpts, tpts, inliers, method, + /*ransacReprojThreshold=*/3.0, + /*maxIters=*/2000, + /*confidence=*/confidence, + /*refineIters=*/refining); + } + + // Convert to Mat for SANITY_CHECK consistency + cv::Mat T_est_mat = (cv::Mat_(2,1) << T_est[0], T_est[1]); + SANITY_CHECK(T_est_mat, 1e-6); +} + +} // namespace opencv_test \ No newline at end of file diff --git a/modules/3d/src/ptsetreg.cpp b/modules/3d/src/ptsetreg.cpp index 3be9fa76e0..4fff6a3454 100644 --- a/modules/3d/src/ptsetreg.cpp +++ b/modules/3d/src/ptsetreg.cpp @@ -761,6 +761,73 @@ public: } }; +/* + * Compute + * x 1 0 X t_x + * = * + + * y 0 1 Y t_y + * + * - every element in _m1 contains (X, Y), which are called source points + * - every element in _m2 contains (x, y), which are called destination points + * - _model is of size 2x3, which contains + * 1 0 t_x + * 0 1 t_y + * + * Minimal sample size: 1 + * - For a single correspondence, the optimal translation equals the pointwise + * displacement (to - from). The robust framework (RANSAC/LMEDS) selects + * inlier sets using the reprojection error and refits the model. + */ +class Translation2DEstimatorCallback : public PointSetRegistrator::Callback +{ +public: + // Fit translation using the minimal subset (1 sample): displacement of the first pair. + // The robust framework ensures consensus on larger sets; a closed-form LS refit + // (mean displacement) can be applied after inlier selection. + int runKernel(InputArray _m1, InputArray _m2, OutputArray _model) const CV_OVERRIDE + { + Mat m1 = _m1.getMat(), m2 = _m2.getMat(); + const auto* from = m1.ptr(); + const auto* to = m2.ptr(); + const int n = m1.checkVector(2); + if (n < 1) return 0; + + // Minimal model from a single correspondence + const double tx = (double)to[0].x - (double)from[0].x; + const double ty = (double)to[0].y - (double)from[0].y; + + _model.create(2, 3, CV_64F); + double* H = _model.getMat().ptr(); + H[0]=1.0; H[1]=0.0; H[2]=tx; + H[3]=0.0; H[4]=1.0; H[5]=ty; + return 1; + } + + // Return squared L2 reprojection error (pixels^2), as used by affine estimators. + void computeError(InputArray _m1, InputArray _m2, + InputArray _model, OutputArray _err) const CV_OVERRIDE + { + Mat m1 = _m1.getMat(), m2 = _m2.getMat(); + const auto* from = m1.ptr(); + const auto* to = m2.ptr(); + const int n = m1.checkVector(2); + + const double* H = _model.getMat().ptr(); // [1 0 tx; 0 1 ty] + // Cast once to float for inner loop speed + const float tx = (float)H[2]; + const float ty = (float)H[5]; + + _err.create(n, 1, CV_32F); + float* e = _err.getMat().ptr(); + + for (int i = 0; i < n; ++i) { + // residual = (from + t) - to + const float rx = (float)from[i].x + tx - (float)to[i].x; + const float ry = (float)from[i].y + ty - (float)to[i].y; + e[i] = rx*rx + ry*ry; // squared L2 (pixels^2) + } + } +}; int estimateAffine3D(InputArray _from, InputArray _to, OutputArray _out, OutputArray _inliers, @@ -1167,4 +1234,165 @@ Mat estimateAffinePartial2D(InputArray _from, InputArray _to, OutputArray _inlie return H; } +Vec2d estimateTranslation2D(InputArray _from, InputArray _to, + OutputArray _inliers, + int method, + double ransacReprojThreshold, + size_t maxIters, double confidence, + size_t refineIters) +{ + using std::numeric_limits; + const double NaN = numeric_limits::quiet_NaN(); + Vec2d tvec(NaN, NaN); + + // Normalize input layout and type: + // - Accepts various shapes (Nx2, 1xN, etc.); force to CV_32FC2 for the registrator. + // - Keep local copies to allow reshaping into N x 1 vectors of Point2f. + Mat from = _from.getMat(), to = _to.getMat(); + int count = from.checkVector(2); + bool result = false; + CV_Assert(count >= 0 && to.checkVector(2) == count); + + if (from.type() != CV_32FC2 || to.type() != CV_32FC2) { + Mat tmp1, tmp2; + from.convertTo(tmp1, CV_32FC2); from = tmp1; + to.convertTo(tmp2, CV_32FC2); to = tmp2; + } else { + from = from.clone(); + to = to.clone(); + } + + // Convert to N x 1 vectors of Point2f (matches registrator expectations). + from = from.reshape(2, count); + to = to.reshape(2, count); + + // Optional inlier mask (1-inlier, 0-outlier). Only allocate if requested. + Mat inliers; + if (_inliers.needed()) { + _inliers.create(count, 1, CV_8U, -1, true); + inliers = _inliers.getMat(); + } + + // Build translation model callback. Minimal sample size is 1. + Mat T; // 2x3 output (CV_64F) + Ptr cb = makePtr(); + + // Create robust estimators with the same semantics as affine functions. + if (method == RANSAC) + result = createRANSACPointSetRegistrator(cb, 1, + ransacReprojThreshold, confidence, (int)maxIters)->run(from, to, T, inliers); + else if (method == LMEDS) + result = createLMeDSPointSetRegistrator(cb, 1, + confidence, (int)maxIters)->run(from, to, T, inliers); + else + CV_Error(Error::StsBadArg, "Unknown or unsupported robust estimation method"); + + // Estimation failure: return NaNs and zero inlier mask (if requested). + if (!result) { + if (_inliers.needed()) + inliers.setTo(Scalar(0)); + return tvec; + } + + // Post-process: compress inliers to the front (same pattern as affine), + // then optionally refine or compute closed-form LS (mean displacement) on inliers. + if (count > 0) { + // Reorder: pack inliers to the front + compressElems(from.ptr(), inliers.ptr(), 1, count); + int nin = compressElems(to.ptr(), inliers.ptr(), 1, count); + + if (nin > 0) { + Mat src = from.rowRange(0, nin); + Mat dst = to.rowRange(0, nin); + + if (refineIters > 0) { + if (T.empty()) + T = (Mat_(2,3) << 1,0,0, 0,1,0); + // LM refine on translation only. + // T is: + // [1 0 tx] + // [0 1 ty] + // We optimize the 2-parameter vector (tx, ty). + double* Hptr = T.ptr(); + double Hvec_buf[2] = { Hptr[2], Hptr[5] }; // (tx, ty) + Mat Hvec(2, 1, CV_64F, Hvec_buf); + + auto translation2dRefineCallback = [src, dst](InputOutputArray _param, OutputArray _err, OutputArray _Jac) -> bool + { + int i, errCount = src.checkVector(2); + Mat param = _param.getMat(); + _err.create(errCount * 2, 1, CV_64F); + Mat err = _err.getMat(), J; + if (_Jac.needed()) + { + _Jac.create(errCount * 2, param.rows, CV_64F); + J = _Jac.getMat(); + CV_Assert(J.isContinuous() && J.cols == 2); + } + + const Point2f* M = src.ptr(); + const Point2f* m = dst.ptr(); + const double* h = param.ptr(); + double* errptr = err.ptr(); + double* Jptr = J.data ? J.ptr() : 0; + + for (i = 0; i < errCount; i++) + { + double Mx = M[i].x, My = M[i].y; + double xi = Mx + h[0]; + double yi = My + h[1]; + errptr[i * 2] = xi - m[i].x; + errptr[i * 2 + 1] = yi - m[i].y; + + /* + Jacobian should be: + {1, 0} + {0, 1} + */ + if (Jptr) + { + Jptr[0] = 1; Jptr[1] = 0; + Jptr[2] = 0; Jptr[3] = 1; + + Jptr += 4; + } + } + + return true; + }; + LevMarq solver(Hvec, translation2dRefineCallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)refineIters) + .setGeodesic(true)); + solver.optimize(); + + // Update H with refined parameters + Hptr[2] = Hvec_buf[0]; // refined tx + Hptr[5] = Hvec_buf[1]; // refined ty + } else { + // Closed-form LS on inliers: mean displacement (optimal in L2 sense). + const auto* f = src.ptr(); + const auto* t = dst.ptr(); + double sx = 0.0, sy = 0.0; + for (int i = 0; i < nin; ++i) { + sx += (double)t[i].x - (double)f[i].x; + sy += (double)t[i].y - (double)f[i].y; + } + if (T.empty()) + T = (Mat_(2,3) << 1,0,0, 0,1,0); + double* H = T.ptr(); + H[2] = sx / nin; // t_x + H[5] = sy / nin; // t_y + } + } + + // Extract translation components + if (!T.empty()) { + tvec[0] = T.at(0, 2); + tvec[1] = T.at(1, 2); + } + } + + return tvec; +} } // namespace cv diff --git a/modules/3d/test/test_translation2d_estimator.cpp b/modules/3d/test/test_translation2d_estimator.cpp new file mode 100644 index 0000000000..b007695252 --- /dev/null +++ b/modules/3d/test/test_translation2d_estimator.cpp @@ -0,0 +1,211 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// (3-clause BSD License) +// +// Copyright (C) 2015-2016, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the names of the copyright holders nor the names of the contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall copyright holders or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +CV_ENUM(Method, RANSAC, LMEDS) +typedef TestWithParam EstimateTranslation2D; + +static float rngIn(float from, float to) { return from + (to - from) * (float)theRNG(); } + +// build a pure translation 2x3 matrix +static cv::Mat rngTranslationMat() +{ + double tx = rngIn(-20.f, 20.f); + double ty = rngIn(-20.f, 20.f); + double t[2*3] = { 1.0, 0.0, tx, + 0.0, 1.0, ty }; + return cv::Mat(2, 3, CV_64F, t).clone(); +} + +static inline cv::Vec2d getTxTy(const cv::Mat& T) +{ + CV_Assert(T.rows == 2 && T.cols == 3 && T.type() == CV_64F); + return cv::Vec2d(T.at(0,2), T.at(1,2)); +} + +TEST_P(EstimateTranslation2D, test1Point) +{ + // minimal sample is 1 point + for (size_t i = 0; i < 500; ++i) + { + cv::Mat T = rngTranslationMat(); + cv::Vec2d T_ref = getTxTy(T); + + cv::Mat fpts(1, 1, CV_32FC2); + cv::Mat tpts(1, 1, CV_32FC2); + + fpts.at(0) = cv::Point2f(rngIn(1,2), rngIn(5,6)); + transform(fpts, tpts, T); + + std::vector inliers; + cv::Vec2d T_est = estimateTranslation2D(fpts, tpts, inliers, GetParam() /* method */); + + EXPECT_NEAR(T_est[0], T_ref[0], 1e-6); + EXPECT_NEAR(T_est[1], T_ref[1], 1e-6); + EXPECT_EQ((int)inliers.size(), 1); + EXPECT_EQ((int)inliers[0], 1); + } +} + +TEST_P(EstimateTranslation2D, testNPoints) +{ + for (size_t i = 0; i < 500; ++i) + { + cv::Mat T = rngTranslationMat(); + cv::Vec2d T_ref = getTxTy(T); + + const int method = GetParam(); + const int n = 100; + int m; + // LMEDS can't handle more than 50% outliers (by design) + if (method == LMEDS) + m = 3*n/5; + else + m = 2*n/5; + + const float shift_outl = 15.f; + const float noise_level = 20.f; + + cv::Mat fpts(1, n, CV_32FC2); + cv::Mat tpts(1, n, CV_32FC2); + + randu(fpts, 0.f, 100.f); + transform(fpts, tpts, T); + + /* adding noise to some points (make last n-m points outliers) */ + cv::Mat outliers = tpts.colRange(m, n); + outliers.reshape(1) += shift_outl; + + cv::Mat noise(outliers.size(), outliers.type()); + randu(noise, 0.f, noise_level); + outliers += noise; + + std::vector inliers; + cv::Vec2d T_est = estimateTranslation2D(fpts, tpts, inliers, method); + + // Check estimation produced finite values + ASSERT_TRUE(std::isfinite(T_est[0]) && std::isfinite(T_est[1])); + + EXPECT_NEAR(T_est[0], T_ref[0], 1e-4); + EXPECT_NEAR(T_est[1], T_ref[1], 1e-4); + + bool inliers_good = std::count(inliers.begin(), inliers.end(), 1) == m && + m == std::accumulate(inliers.begin(), inliers.begin() + m, 0); + EXPECT_TRUE(inliers_good); + } +} + +// test conversion from other datatypes than float +TEST_P(EstimateTranslation2D, testConversion) +{ + cv::Mat T = rngTranslationMat(); + T.convertTo(T, CV_32S); // convert to int to transform ints properly + + std::vector fpts(3); + std::vector tpts(3); + + fpts[0] = cv::Point2f(rngIn(1,2), rngIn(5,6)); + fpts[1] = cv::Point2f(rngIn(3,4), rngIn(3,4)); + fpts[2] = cv::Point2f(rngIn(1,2), rngIn(3,4)); + + transform(fpts, tpts, T); + + std::vector inliers; + cv::Vec2d T_est = estimateTranslation2D(fpts, tpts, inliers, GetParam() /* method */); + + ASSERT_TRUE(std::isfinite(T_est[0]) && std::isfinite(T_est[1])); + + T.convertTo(T, CV_64F); // convert back for reference extraction + cv::Vec2d T_ref = getTxTy(T); + + EXPECT_NEAR(T_est[0], T_ref[0], 1e-3); + EXPECT_NEAR(T_est[1], T_ref[1], 1e-3); + + // all must be inliers + EXPECT_EQ(countNonZero(inliers), 3); +} + +INSTANTIATE_TEST_CASE_P(Calib3d, EstimateTranslation2D, Method::all()); + +// "don't change inputs" regression, mirroring affine partial test +TEST(EstimateTranslation2D, dont_change_inputs) +{ + /*const static*/ float pts0_[10] = { + 0.0f, 0.0f, + 0.0f, 8.0f, + 4.0f, 0.0f, // outlier + 8.0f, 8.0f, + 8.0f, 0.0f + }; + /*const static*/ float pts1_[10] = { + 0.1f, 0.1f, + 0.1f, 8.1f, + 0.0f, 4.0f, // outlier + 8.1f, 8.1f, + 8.1f, 0.1f + }; + + cv::Mat pts0(cv::Size(1, 5), CV_32FC2, (void*)pts0_); + cv::Mat pts1(cv::Size(1, 5), CV_32FC2, (void*)pts1_); + + cv::Mat pts0_copy = pts0.clone(); + cv::Mat pts1_copy = pts1.clone(); + + cv::Mat inliers; + + cv::Vec2d T = cv::estimateTranslation2D(pts0, pts1, inliers); + + for (int i = 0; i < pts0.rows; ++i) + EXPECT_EQ(pts0_copy.at(i), pts0.at(i)) << "pts0: i=" << i; + + for (int i = 0; i < pts1.rows; ++i) + EXPECT_EQ(pts1_copy.at(i), pts1.at(i)) << "pts1: i=" << i; + + EXPECT_EQ(0, (int)inliers.at(2)); + + // sanity: estimated translation should be finite + EXPECT_TRUE(std::isfinite(T[0]) && std::isfinite(T[1])); +} + +}} // namespace \ No newline at end of file