potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-31 00:03:03 +04:00
Code Packages Releases Wiki Activity

Merge branch 4.x

Browse Source
This commit is contained in:
Alexander Alekhin
2022-08-21 14:55:48 +00:00
parent 99d216c8d4 925ff6241f
commit c25f776151
538 changed files with 84703 additions and 5374 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/3d/include/opencv2/3d.hpp
+29 -15
View File
@@ -1557,7 +1557,7 @@ unit length.
*/
CV_EXPORTS_W void decomposeEssentialMat( InputArray E, OutputArray R1, OutputArray R2, OutputArray t );
/** @brief Recovers the relative camera rotation and the translation from corresponding points in two images from two different cameras, using cheirality check. Returns the number of
/** @brief Recovers the relative camera rotation and the translation from corresponding points in two images from two different cameras, using chirality check. Returns the number of
inliers that pass the check.
@param points1 Array of N 2D points from the first image. The point coordinates should be
@@ -1589,11 +1589,11 @@ line in pixels, beyond which the point is considered an outlier and is not used
final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the
point localization, image resolution, and the image noise.
@param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the cheirality check.
inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the chirality check.
This function decomposes an essential matrix using @ref decomposeEssentialMat and then verifies
possible pose hypotheses by doing cheirality check. The cheirality check means that the
possible pose hypotheses by doing chirality check. The chirality check means that the
triangulated 3D points should have positive depth. Some details can be found in @cite Nister03.
This function can be used to process the output E and mask from @ref findEssentialMat. In this
@@ -1628,7 +1628,7 @@ CV_EXPORTS_W int recoverPose( InputArray points1, InputArray points2,
InputOutputArray mask = noArray());
/** @brief Recovers the relative camera rotation and the translation from an estimated essential
matrix and the corresponding points in two images, using cheirality check. Returns the number of
matrix and the corresponding points in two images, using chirality check. Returns the number of
inliers that pass the check.
@param E The input essential matrix.
@@ -1646,11 +1646,11 @@ described below.
therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit
length.
@param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the cheirality check.
inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the chirality check.
This function decomposes an essential matrix using @ref decomposeEssentialMat and then verifies
possible pose hypotheses by doing cheirality check. The cheirality check means that the
possible pose hypotheses by doing chirality check. The chirality check means that the
triangulated 3D points should have positive depth. Some details can be found in @cite Nister03.
This function can be used to process the output E and mask from @ref findEssentialMat. In this
@@ -1697,8 +1697,8 @@ length.
are feature points from cameras with same focal length and principal point.
@param pp principal point of the camera.
@param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the cheirality check.
inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the chirality check.
This function differs from the one above that it computes camera intrinsic matrix from focal length and
principal point:
@@ -1733,12 +1733,12 @@ length.
@param distanceThresh threshold distance which is used to filter out far away points (i.e. infinite
points).
@param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the cheirality check.
inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to
recover pose. In the output mask only inliers which pass the chirality check.
@param triangulatedPoints 3D points which were reconstructed by triangulation.
This function differs from the one above that it outputs the triangulated 3D point that are used for
the cheirality check.
the chirality check.
*/
CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2,
InputArray cameraMatrix, OutputArray R, OutputArray t,
@@ -2097,7 +2097,7 @@ Check @ref tutorial_homography "the corresponding tutorial" for more details.
This function extracts relative camera motion between two views of a planar object and returns up to
four mathematical solution tuples of rotation, translation, and plane normal. The decomposition of
the homography matrix H is described in detail in @cite Malis.
the homography matrix H is described in detail in @cite Malis2007.
If the homography H, induced by the plane, gives the constraint
\f[s_i \vecthree{x'_i}{y'_i}{1} \sim H \vecthree{x_i}{y_i}{1}\f] on the source image points
@@ -2125,7 +2125,7 @@ CV_EXPORTS_W int decomposeHomographyMat(InputArray H,
@param pointsMask optional Mat/Vector of 8u type representing the mask for the inliers as given by the #findHomography function
This function is intended to filter the output of the #decomposeHomographyMat based on additional
information as described in @cite Malis . The summary of the method: the #decomposeHomographyMat function
information as described in @cite Malis2007 . The summary of the method: the #decomposeHomographyMat function
returns 2 unique solutions and their "opposites" for a total of 4 solutions. If we have access to the
sets of points visible in the camera frame before and after the homography transformation is applied,
we can determine which are the true potential solutions and which are the opposites by verifying which
@@ -2465,6 +2465,20 @@ void undistortPoints(InputArray src, OutputArray dst,
TermCriteria criteria=TermCriteria(TermCriteria::MAX_ITER, 5, 0.01));
/**
* @brief Compute undistorted image points position
*
* @param src Observed points position, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel (CV_32FC2 or CV_64FC2) (or vector\<Point2f\> ).
* @param dst Output undistorted points position (1xN/Nx1 2-channel or vector\<Point2f\> ).
* @param cameraMatrix Camera matrix \f$\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
* @param distCoeffs Distortion coefficients
*/
CV_EXPORTS_W
void undistortImagePoints(InputArray src, OutputArray dst, InputArray cameraMatrix,
InputArray distCoeffs,
TermCriteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 5, 0.01));
/** @brief Octree for 3D vision.
*
* In 3D vision filed, the Octree is used to process and accelerate the pointcloud data. The class Octree represents
modules/3d/src/five-point.cpp
+1 -1
View File
@@ -583,7 +583,7 @@ int recoverPose( InputArray E, InputArray _points1, InputArray _points2,
P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;
P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;
// Do the cheirality check.
// Do the chirality check.
// Notice here a threshold dist is used to filter
// out far away points (i.e. infinite points) since
// their depth may vary between positive and negative.
modules/3d/src/sqpnp.cpp
+91 -41
View File
@@ -116,7 +116,7 @@ void PoseSolver::solve(InputArray objectPoints, InputArray imagePoints, OutputAr
num_solutions_ = 0;
computeOmega(_objectPoints, _imagePoints);
solveInternal();
solveInternal(_objectPoints);
int depthRot = rvecs.fixedType() ? rvecs.depth() : CV_64F;
int depthTrans = tvecs.fixedType() ? tvecs.depth() : CV_64F;
@@ -192,37 +192,41 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints)
omega_(7, 7) += sq_norm * Y2; omega_(7, 8) += sq_norm * YZ;
omega_(8, 8) += sq_norm * Z2;
//Compute qa_sum
//Compute qa_sum. Certain pairs of elements are equal, so filling them outside the loop saves some operations
qa_sum(0, 0) += X; qa_sum(0, 1) += Y; qa_sum(0, 2) += Z;
qa_sum(1, 3) += X; qa_sum(1, 4) += Y; qa_sum(1, 5) += Z;
qa_sum(0, 6) += -x * X; qa_sum(0, 7) += -x * Y; qa_sum(0, 8) += -x * Z;
qa_sum(1, 6) += -y * X; qa_sum(1, 7) += -y * Y; qa_sum(1, 8) += -y * Z;
qa_sum(2, 0) += -x * X; qa_sum(2, 1) += -x * Y; qa_sum(2, 2) += -x * Z;
qa_sum(2, 3) += -y * X; qa_sum(2, 4) += -y * Y; qa_sum(2, 5) += -y * Z;
qa_sum(2, 6) += sq_norm * X; qa_sum(2, 7) += sq_norm * Y; qa_sum(2, 8) += sq_norm * Z;
}
//Complete qa_sum
qa_sum(1, 3) = qa_sum(0, 0); qa_sum(1, 4) = qa_sum(0, 1); qa_sum(1, 5) = qa_sum(0, 2);
qa_sum(2, 0) = qa_sum(0, 6); qa_sum(2, 1) = qa_sum(0, 7); qa_sum(2, 2) = qa_sum(0, 8);
qa_sum(2, 3) = qa_sum(1, 6); qa_sum(2, 4) = qa_sum(1, 7); qa_sum(2, 5) = qa_sum(1, 8);
//lower triangles of omega_'s off-diagonal blocks (0:2, 6:8), (3:5, 6:8) and (6:8, 6:8)
omega_(1, 6) = omega_(0, 7); omega_(2, 6) = omega_(0, 8); omega_(2, 7) = omega_(1, 8);
omega_(4, 6) = omega_(3, 7); omega_(5, 6) = omega_(3, 8); omega_(5, 7) = omega_(4, 8);
omega_(7, 6) = omega_(6, 7); omega_(8, 6) = omega_(6, 8); omega_(8, 7) = omega_(7, 8);
//upper triangle of omega_'s block (3:5, 3:5)
omega_(3, 3) = omega_(0, 0); omega_(3, 4) = omega_(0, 1); omega_(3, 5) = omega_(0, 2);
omega_(4, 4) = omega_(1, 1); omega_(4, 5) = omega_(1, 2);
omega_(5, 5) = omega_(2, 2);
omega_(4, 4) = omega_(1, 1); omega_(4, 5) = omega_(1, 2);
omega_(5, 5) = omega_(2, 2);
//Mirror upper triangle to lower triangle
for (int r = 0; r < 9; r++)
{
for (int c = 0; c < r; c++)
{
omega_(r, c) = omega_(c, r);
}
}
//Mirror omega_'s upper triangle to lower triangle
//Note that elements (7, 6), (8, 6) & (8, 7) have already been assigned above
omega_(1, 0) = omega_(0, 1);
omega_(2, 0) = omega_(0, 2); omega_(2, 1) = omega_(1, 2);
omega_(3, 0) = omega_(0, 3); omega_(3, 1) = omega_(1, 3); omega_(3, 2) = omega_(2, 3);
omega_(4, 0) = omega_(0, 4); omega_(4, 1) = omega_(1, 4); omega_(4, 2) = omega_(2, 4); omega_(4, 3) = omega_(3, 4);
omega_(5, 0) = omega_(0, 5); omega_(5, 1) = omega_(1, 5); omega_(5, 2) = omega_(2, 5); omega_(5, 3) = omega_(3, 5); omega_(5, 4) = omega_(4, 5);
omega_(6, 0) = omega_(0, 6); omega_(6, 1) = omega_(1, 6); omega_(6, 2) = omega_(2, 6); omega_(6, 3) = omega_(3, 6); omega_(6, 4) = omega_(4, 6); omega_(6, 5) = omega_(5, 6);
omega_(7, 0) = omega_(0, 7); omega_(7, 1) = omega_(1, 7); omega_(7, 2) = omega_(2, 7); omega_(7, 3) = omega_(3, 7); omega_(7, 4) = omega_(4, 7); omega_(7, 5) = omega_(5, 7);
omega_(8, 0) = omega_(0, 8); omega_(8, 1) = omega_(1, 8); omega_(8, 2) = omega_(2, 8); omega_(8, 3) = omega_(3, 8); omega_(8, 4) = omega_(4, 8); omega_(8, 5) = omega_(5, 8);
cv::Matx<double, 3, 3> q;
q(0, 0) = n; q(0, 1) = 0; q(0, 2) = -sum_img.x;
@@ -245,6 +249,11 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints)
cv::SVD omega_svd(omega_, cv::SVD::FULL_UV);
s_ = omega_svd.w;
u_ = cv::Mat(omega_svd.vt.t());
#if 0
// EVD equivalent of the SVD; less accurate
cv::eigen(omega_, s_, u_);
u_ = u_.t(); // eigenvectors were returned as rows
#endif
CV_Assert(s_(0) >= 1e-7);
@@ -255,7 +264,7 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints)
point_mean_ = cv::Vec3d(sum_obj.x / n, sum_obj.y / n, sum_obj.z / n);
}
void PoseSolver::solveInternal()
void PoseSolver::solveInternal(InputArray objectPoints)
{
double min_sq_err = std::numeric_limits<double>::max();
int num_eigen_points = num_null_vectors_ > 0 ? num_null_vectors_ : 1;
@@ -272,42 +281,39 @@ void PoseSolver::solveInternal()
{
solutions[0].r_hat = det3x3(e) * e;
solutions[0].t = p_ * solutions[0].r_hat;
checkSolution(solutions[0], min_sq_err);
checkSolution(solutions[0], objectPoints, min_sq_err);
}
else
{
Matx<double, 9, 1> r;
nearestRotationMatrix(e, r);
nearestRotationMatrixFOAM(e, r);
solutions[0] = runSQP(r);
solutions[0].t = p_ * solutions[0].r_hat;
checkSolution(solutions[0], min_sq_err);
checkSolution(solutions[0], objectPoints, min_sq_err);
nearestRotationMatrix(-e, r);
nearestRotationMatrixFOAM(-e, r);
solutions[1] = runSQP(r);
solutions[1].t = p_ * solutions[1].r_hat;
checkSolution(solutions[1], min_sq_err);
checkSolution(solutions[1], objectPoints, min_sq_err);
}
}
int c = 1;
while (min_sq_err > 3 * s_[9 - num_eigen_points - c] && 9 - num_eigen_points - c > 0)
int index, c = 1;
while ((index = 9 - num_eigen_points - c) > 0 && min_sq_err > 3 * s_[index])
{
int index = 9 - num_eigen_points - c;
const cv::Matx<double, 9, 1> e = u_.col(index);
SQPSolution solutions[2];
Matx<double, 9, 1> r;
nearestRotationMatrix(e, r);
nearestRotationMatrixFOAM(e, r);
solutions[0] = runSQP(r);
solutions[0].t = p_ * solutions[0].r_hat;
checkSolution(solutions[0], min_sq_err);
checkSolution(solutions[0], objectPoints, min_sq_err);
nearestRotationMatrix(-e, r);
nearestRotationMatrixFOAM(-e, r);
solutions[1] = runSQP(r);
solutions[1].t = p_ * solutions[1].r_hat;
checkSolution(solutions[1], min_sq_err);
checkSolution(solutions[1], objectPoints, min_sq_err);
c++;
}
@@ -339,7 +345,7 @@ PoseSolver::SQPSolution PoseSolver::runSQP(const cv::Matx<double, 9, 1>& r0)
if (det_r > SQP_DET_THRESHOLD)
{
nearestRotationMatrix(r, solution.r_hat);
nearestRotationMatrixFOAM(r, solution.r_hat);
}
else
{
@@ -613,12 +619,26 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx<double, 9, 1>& r,
}
// faster nearest rotation computation based on FOAM (see: http://users.ics.forth.gr/~lourakis/publ/2018_iros.pdf )
// if e = u*w*vt then r=u*diag([1, 1, det(u)*det(v)])*vt
void PoseSolver::nearestRotationMatrixSVD(const cv::Matx<double, 9, 1>& e,
cv::Matx<double, 9, 1>& r)
{
cv::Matx<double, 3, 3> e33 = e.reshape<3, 3>();
cv::SVD e33_svd(e33, cv::SVD::FULL_UV);
double detuv = cv::determinant(e33_svd.u)*cv::determinant(e33_svd.vt);
cv::Matx<double, 3, 3> diag = cv::Matx33d::eye();
diag(2, 2) = detuv;
cv::Matx<double, 3, 3> r33 = cv::Mat(e33_svd.u*diag*e33_svd.vt);
r = r33.reshape<9, 1>();
}
// Faster nearest rotation computation based on FOAM. See M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016
// and M. Lourakis, G. Terzakis: "Efficient Absolute Orientation Revisited", IROS 2018.
/* Solve the nearest orthogonal approximation problem
* i.e., given e, find R minimizing ||R-e||_F
*
* The computation borrows from Markley's FOAM algorithm
* "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. Astronaut. Sci.
* "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. Astronaut. Sci. 1993.
*
* See also M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016
*
@@ -626,24 +646,32 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx<double, 9, 1>& r,
* Institute of Computer Science, Foundation for Research & Technology - Hellas
* Heraklion, Crete, Greece.
*/
void PoseSolver::nearestRotationMatrix(const cv::Matx<double, 9, 1>& e,
void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx<double, 9, 1>& e,
cv::Matx<double, 9, 1>& r)
{
int i;
double l, lprev, det_e, e_sq, adj_e_sq, adj_e[9];
// det(e)
det_e = e(0) * e(4) * e(8) - e(0) * e(5) * e(7) - e(1) * e(3) * e(8) + e(2) * e(3) * e(7) + e(1) * e(6) * e(5) - e(2) * e(6) * e(4);
if (fabs(det_e) < 1E-04) { // singular, handle it with SVD
PoseSolver::nearestRotationMatrixSVD(e, r);
return;
}
// e's adjoint
adj_e[0] = e(4) * e(8) - e(5) * e(7); adj_e[1] = e(2) * e(7) - e(1) * e(8); adj_e[2] = e(1) * e(5) - e(2) * e(4);
adj_e[3] = e(5) * e(6) - e(3) * e(8); adj_e[4] = e(0) * e(8) - e(2) * e(6); adj_e[5] = e(2) * e(3) - e(0) * e(5);
adj_e[6] = e(3) * e(7) - e(4) * e(6); adj_e[7] = e(1) * e(6) - e(0) * e(7); adj_e[8] = e(0) * e(4) - e(1) * e(3);
// det(e), ||e||^2, ||adj(e)||^2
det_e = e(0) * e(4) * e(8) - e(0) * e(5) * e(7) - e(1) * e(3) * e(8) + e(2) * e(3) * e(7) + e(1) * e(6) * e(5) - e(2) * e(6) * e(4);
// ||e||^2, ||adj(e)||^2
e_sq = e(0) * e(0) + e(1) * e(1) + e(2) * e(2) + e(3) * e(3) + e(4) * e(4) + e(5) * e(5) + e(6) * e(6) + e(7) * e(7) + e(8) * e(8);
adj_e_sq = adj_e[0] * adj_e[0] + adj_e[1] * adj_e[1] + adj_e[2] * adj_e[2] + adj_e[3] * adj_e[3] + adj_e[4] * adj_e[4] + adj_e[5] * adj_e[5] + adj_e[6] * adj_e[6] + adj_e[7] * adj_e[7] + adj_e[8] * adj_e[8];
// compute l_max with Newton-Raphson from FOAM's characteristic polynomial, i.e. eq.(23) - (26)
for (i = 200, l = 2.0, lprev = 0.0; fabs(l - lprev) > 1E-12 * fabs(lprev) && i > 0; --i) {
l = 0.5*(e_sq + 3.0); // 1/2*(trace(mat(e)*mat(e)') + trace(eye(3)))
if (det_e < 0.0) l = -l;
for (i = 15, lprev = 0.0; fabs(l - lprev) > 1E-12 * fabs(lprev) && i > 0; --i) {
double tmp, p, pp;
tmp = (l * l - e_sq);
@@ -717,9 +745,31 @@ inline bool PoseSolver::positiveDepth(const SQPSolution& solution) const
return (r(6) * mean(0) + r(7) * mean(1) + r(8) * mean(2) + t(2) > 0);
}
void PoseSolver::checkSolution(SQPSolution& solution, double& min_error)
inline bool PoseSolver::positiveMajorityDepths(const SQPSolution& solution, InputArray objectPoints) const
{
if (positiveDepth(solution))
const cv::Matx<double, 9, 1>& r = solution.r_hat;
const cv::Matx<double, 3, 1>& t = solution.t;
int npos = 0, nneg = 0;
Mat _objectPoints = objectPoints.getMat();
int n = _objectPoints.cols * _objectPoints.rows;
for (int i = 0; i < n; i++)
{
const cv::Point3d& obj_pt = _objectPoints.at<cv::Point3d>(i);
if (r(6) * obj_pt.x + r(7) * obj_pt.y + r(8) * obj_pt.z + t(2) > 0) ++npos;
else ++nneg;
}
return npos >= nneg;
}
void PoseSolver::checkSolution(SQPSolution& solution, InputArray objectPoints, double& min_error)
{
bool cheirok = positiveDepth(solution) || positiveMajorityDepths(solution, objectPoints); // check the majority if the check with centroid fails
if (cheirok)
{
solution.sq_error = (omega_ * solution.r_hat).ddot(solution.r_hat);
if (fabs(min_error - solution.sq_error) > EQUAL_SQUARED_ERRORS_DIFF)
modules/3d/src/sqpnp.hpp
+33 -14
View File
@@ -85,13 +85,14 @@ private:
/*
* @brief Computes the 9x9 PSD Omega matrix and supporting matrices.
* @param objectPoints The 3D points in object coordinates.
*/
void solveInternal();
void solveInternal(InputArray objectPoints);
/*
* @brief Produces the distance from being orthogonal for a given 3x3 matrix
* in row-major form.
* @param e The vector to test representing a 3x3 matrix in row major form.
* in row-major order.
* @param e The vector to test representing a 3x3 matrix in row-major order.
* @return The distance the matrix is from being orthogonal.
*/
static double orthogonalityError(const cv::Matx<double, 9, 1>& e);
@@ -99,31 +100,49 @@ private:
/*
* @brief Processes a solution and sorts it by error.
* @param solution The solution to evaluate.
* @param min_error The current minimum error.
* @param objectPoints The 3D points in object coordinates.
* @param min_error The current minimum error.
*/
void checkSolution(SQPSolution& solution, double& min_error);
void checkSolution(SQPSolution& solution, InputArray objectPoints, double& min_error);
/*
* @brief Computes the determinant of a matrix stored in row-major format.
* @param e Vector representing a 3x3 matrix stored in row-major format.
* @brief Computes the determinant of a matrix stored in row-major order.
* @param e Vector representing a 3x3 matrix stored in row-major order.
* @return The determinant of the matrix.
*/
static double det3x3(const cv::Matx<double, 9, 1>& e);
/*
* @brief Tests the cheirality for a given solution.
* @brief Tests the cheirality on the mean object point for a given solution.
* @param solution The solution to evaluate.
*/
inline bool positiveDepth(const SQPSolution& solution) const;
/*
* @brief Determines the nearest rotation matrix to a given rotaiton matrix.
* Input and output are 9x1 vector representing a vector stored in row-major
* form.
* @param e The input 3x3 matrix stored in a vector in row-major form.
* @param r The nearest rotation matrix to the input e (again in row-major form).
* @brief Tests the cheirality on all object points for a given solution.
* @param solution The solution to evaluate.
* @param objectPoints The 3D points in object coordinates.
*/
static void nearestRotationMatrix(const cv::Matx<double, 9, 1>& e,
inline bool positiveMajorityDepths(const SQPSolution& solution, InputArray objectPoints) const;
/*
* @brief Determines the nearest rotation matrix to a given rotation matrix using SVD.
* Input and output are 9x1 vector representing a matrix stored in row-major
* order.
* @param e The input 3x3 matrix stored in a vector in row-major order.
* @param r The nearest rotation matrix to the input e (again in row-major order).
*/
static void nearestRotationMatrixSVD(const cv::Matx<double, 9, 1>& e,
cv::Matx<double, 9, 1>& r);
/*
* @brief Determines the nearest rotation matrix to a given rotation matrix using the FOAM algorithm.
* Input and output are 9x1 vector representing a matrix stored in row-major
* order.
* @param e The input 3x3 matrix stored in a vector in row-major order.
* @param r The nearest rotation matrix to the input e (again in row-major order).
*/
static void nearestRotationMatrixFOAM(const cv::Matx<double, 9, 1>& e,
cv::Matx<double, 9, 1>& r);
/*
modules/3d/src/undistort.dispatch.cpp
+6
View File
@@ -40,6 +40,7 @@
//
//M*/
#include "opencv2/core/types.hpp"
#include "precomp.hpp"
#include "distortion_model.hpp"
@@ -523,6 +524,11 @@ void undistortPoints(InputArray _src, OutputArray _dst,
undistortPointsInternal(src, dst, cameraMatrix, distCoeffs, R, P, criteria);
}
void undistortImagePoints(InputArray src, OutputArray dst, InputArray cameraMatrix, InputArray distCoeffs, TermCriteria termCriteria)
{
undistortPoints(src, dst, cameraMatrix, distCoeffs, noArray(), cameraMatrix, termCriteria);
}
static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, enum UndistortTypes projType)
{
double x = p.x, y = p.y;
modules/3d/test/test_undistort_points.cpp
+63 -10
View File
@@ -3,6 +3,7 @@
// of this distribution and at http://opencv.org/license.html.
#include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
#include "opencv2/core/types.hpp"
#include "test_precomp.hpp"
namespace opencv_test { namespace {
@@ -14,6 +15,7 @@ protected:
-1, 5), Point3f pmax = Point3f(1, 1, 10));
void generateCameraMatrix(Mat& cameraMatrix);
void generateDistCoeffs(Mat& distCoeffs, int count);
cv::Mat generateRotationVector();
double thresh = 1.0e-2;
};
@@ -50,6 +52,14 @@ void UndistortPointsTest::generateDistCoeffs(Mat& distCoeffs, int count)
distCoeffs.at<double>(i,0) = theRNG().uniform(-0.1, 0.1);
}
cv::Mat UndistortPointsTest::generateRotationVector()
{
Mat rvec(1, 3, CV_64F);
theRNG().fill(rvec, RNG::UNIFORM, -0.2, 0.2);
return rvec;
}
TEST_F(UndistortPointsTest, accuracy)
{
Mat intrinsics, distCoeffs;
@@ -58,34 +68,73 @@ TEST_F(UndistortPointsTest, accuracy)
vector<Point3f> points(500);
generate3DPointCloud(points);
vector<Point2f> projectedPoints;
projectedPoints.resize(points.size());
Mat rvec = generateRotationVector();
Mat R;
cv::Rodrigues(rvec, R);
int modelMembersCount[] = {4,5,8};
for (int idx = 0; idx < 3; idx++)
{
generateDistCoeffs(distCoeffs, modelMembersCount[idx]);
/* Project points with distortion */
vector<Point2f> projectedPoints;
projectPoints(Mat(points), Mat::zeros(3,1,CV_64FC1),
Mat::zeros(3,1,CV_64FC1), intrinsics,
distCoeffs, projectedPoints);
/* Project points without distortion */
vector<Point2f> realUndistortedPoints;
projectPoints(Mat(points), Mat::zeros(3,1,CV_64FC1),
projectPoints(Mat(points), rvec,
Mat::zeros(3,1,CV_64FC1), intrinsics,
Mat::zeros(4,1,CV_64FC1), realUndistortedPoints);
/* Undistort points */
Mat undistortedPoints;
undistortPoints(Mat(projectedPoints), undistortedPoints, intrinsics, distCoeffs);
Mat p;
perspectiveTransform(undistortedPoints, p, intrinsics);
undistortedPoints = p;
undistortPoints(Mat(projectedPoints), undistortedPoints, intrinsics, distCoeffs, R, intrinsics);
EXPECT_MAT_NEAR(realUndistortedPoints, undistortedPoints.t(), thresh);
}
}
TEST_F(UndistortPointsTest, undistortImagePointsAccuracy)
{
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics);
vector<Point3f> points(500);
generate3DPointCloud(points);
int modelMembersCount[] = {4,5,8};
for (int idx = 0; idx < 3; idx++)
{
generateDistCoeffs(distCoeffs, modelMembersCount[idx]);
/* Project points with distortion */
vector<Point2f> projectedPoints;
projectPoints(Mat(points), Mat::zeros(3,1,CV_64FC1),
Mat::zeros(3,1,CV_64FC1), intrinsics,
distCoeffs, projectedPoints);
/* Project points without distortion */
vector<Point2f> realUndistortedPoints;
projectPoints(Mat(points), Mat::zeros(3, 1, CV_64FC1),
Mat::zeros(3,1,CV_64FC1), intrinsics,
Mat::zeros(4,1,CV_64FC1), realUndistortedPoints);
/* Undistort points */
Mat undistortedPoints;
TermCriteria termCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 5, thresh / 2);
undistortImagePoints(Mat(projectedPoints), undistortedPoints, intrinsics, distCoeffs,
termCriteria);
EXPECT_MAT_NEAR(realUndistortedPoints, undistortedPoints.t(), thresh);
}
}
TEST_F(UndistortPointsTest, stop_criteria)
{
Mat cameraMatrix = (Mat_<double>(3,3,CV_64F) << 857.48296979, 0, 968.06224829,
@@ -103,13 +152,17 @@ TEST_F(UndistortPointsTest, stop_criteria)
TermCriteria criteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 100, maxError);
std::vector<Point2d> pt_undist_vec;
undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, noArray(), noArray(), criteria);
Mat rVec = Mat(Matx31d(0.1, -0.2, 0.2));
Mat R;
cv::Rodrigues(rVec, R);
undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, R, noArray(), criteria);
std::vector<Point3d> pt_undist_vec_homogeneous;
pt_undist_vec_homogeneous.emplace_back(pt_undist_vec[0].x, pt_undist_vec[0].y, 1.0 );
std::vector<Point2d> pt_redistorted_vec;
projectPoints(pt_undist_vec_homogeneous, Mat::zeros(3,1,CV_64F),
projectPoints(pt_undist_vec_homogeneous, -rVec,
Mat::zeros(3,1,CV_64F), cameraMatrix, distCoeffs, pt_redistorted_vec);
const double obtainedError = sqrt( pow(pt_distorted.x - pt_redistorted_vec[0].x, 2) + pow(pt_distorted.y - pt_redistorted_vec[0].y, 2) );