mirror of
https://github.com/opencv/opencv.git
synced 2026-07-29 15:23:05 +04:00
Merge pull request #28023 from ismailabouzeidx:feat/estimate-translation2d-5x
[3d] Port estimateTranslation2D into 5.x #28023 Merge with opencv_extra PR: opencv/opencv_extra#1290 This PR ports the newly added function `estimateTranslation2D` that was merged into 4.x under this PR: opencv/opencv#27950 The implementation is now adapted to the 5.x branch and integrates cleanly with the updated refinement API. Specifically, this 5.x version makes use of the improved LevMarq interface to support optional non-linear refinement of the translation parameters. Functionality, tests, and documentation follow the same design and behavior as the original 4.x PR. Further details can be found in the linked PR above. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Ismail Abou Zeid
GitHub
@@ -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_<double>(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.
|
||||
|
||||
|
||||
@@ -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 <algorithm>
|
||||
#include <functional>
|
||||
|
||||
namespace opencv_test
|
||||
{
|
||||
using namespace perf;
|
||||
|
||||
CV_ENUM(Method, RANSAC, LMEDS)
|
||||
typedef tuple<int, double, Method, size_t> TranslationParams;
|
||||
typedef TestBaseWithParam<TranslationParams> 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<uchar> 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_<double>(2,1) << T_est[0], T_est[1]);
|
||||
SANITY_CHECK(T_est_mat, 1e-6);
|
||||
}
|
||||
|
||||
} // namespace opencv_test
|
||||
@@ -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<Point2f>();
|
||||
const auto* to = m2.ptr<Point2f>();
|
||||
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<double>();
|
||||
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<Point2f>();
|
||||
const auto* to = m2.ptr<Point2f>();
|
||||
const int n = m1.checkVector(2);
|
||||
|
||||
const double* H = _model.getMat().ptr<double>(); // [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<float>();
|
||||
|
||||
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<double>::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<PointSetRegistrator::Callback> cb = makePtr<Translation2DEstimatorCallback>();
|
||||
|
||||
// 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<Point2f>(), inliers.ptr<uchar>(), 1, count);
|
||||
int nin = compressElems(to.ptr<Point2f>(), inliers.ptr<uchar>(), 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_<double>(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>();
|
||||
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<Point2f>();
|
||||
const Point2f* m = dst.ptr<Point2f>();
|
||||
const double* h = param.ptr<double>();
|
||||
double* errptr = err.ptr<double>();
|
||||
double* Jptr = J.data ? J.ptr<double>() : 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<Point2f>();
|
||||
const auto* t = dst.ptr<Point2f>();
|
||||
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_<double>(2,3) << 1,0,0, 0,1,0);
|
||||
double* H = T.ptr<double>();
|
||||
H[2] = sx / nin; // t_x
|
||||
H[5] = sy / nin; // t_y
|
||||
}
|
||||
}
|
||||
|
||||
// Extract translation components
|
||||
if (!T.empty()) {
|
||||
tvec[0] = T.at<double>(0, 2);
|
||||
tvec[1] = T.at<double>(1, 2);
|
||||
}
|
||||
}
|
||||
|
||||
return tvec;
|
||||
}
|
||||
} // namespace cv
|
||||
|
||||
@@ -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<Method> 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<double>(0,2), T.at<double>(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<cv::Point2f>(0) = cv::Point2f(rngIn(1,2), rngIn(5,6));
|
||||
transform(fpts, tpts, T);
|
||||
|
||||
std::vector<uchar> 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<uchar> 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<cv::Point> fpts(3);
|
||||
std::vector<cv::Point> 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<uchar> 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<cv::Vec2f>(i), pts0.at<cv::Vec2f>(i)) << "pts0: i=" << i;
|
||||
|
||||
for (int i = 0; i < pts1.rows; ++i)
|
||||
EXPECT_EQ(pts1_copy.at<cv::Vec2f>(i), pts1.at<cv::Vec2f>(i)) << "pts1: i=" << i;
|
||||
|
||||
EXPECT_EQ(0, (int)inliers.at<uchar>(2));
|
||||
|
||||
// sanity: estimated translation should be finite
|
||||
EXPECT_TRUE(std::isfinite(T[0]) && std::isfinite(T[1]));
|
||||
}
|
||||
|
||||
}} // namespace
|
||||
Reference in New Issue
Block a user