mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-08-06 14:36:36 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #21166 from rayonnant14:issue_21105

Browse Source 
Fix intrinsics processing in case USAC parameters

* fixed USAC intrinsics processing

* change mat to matx33d, added test
This commit is contained in:
Polina Smolnikova 2021-12-13 20:12:49 +03:00 committed by GitHub
parent d24befa0bc
commit 456137fa24
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 82 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
modules/calib3d/src/usac.hpp
View File

@ -116,7 +116,7 @@ public:
class P3PSolver : public MinimalSolver {
public:
static Ptr<P3PSolver> create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K);
static Ptr<P3PSolver> create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K);
};
//-------------------------- AFFINE -----------------------
@ -164,7 +164,7 @@ public:
class DLSPnP : public NonMinimalSolver {
public:
static Ptr<DLSPnP> create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K);
static Ptr<DLSPnP> create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K);
};
//-------------------------- AFFINE -----------------------
@ -571,11 +571,11 @@ namespace Utils {
* @points is matrix N x 4.
* @norm_points is output matrix N x 4 with calibrated points.
*/
void calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &norm_points);
void calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts);
void normalizeAndDecalibPointsPnP (const Mat &K, Mat &pts, Mat &calib_norm_pts);
void decomposeProjection (const Mat &P, Mat &K_, Mat &R, Mat &t, bool same_focal=false);
double getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2);
void calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &points, Mat &norm_points);
void calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Mat &calib_norm_pts);
void normalizeAndDecalibPointsPnP (const Matx33d &K, Mat &pts, Mat &calib_norm_pts);
void decomposeProjection (const Mat &P, Matx33d &K_, Mat &R, Mat &t, bool same_focal=false);
double getCalibratedThreshold (double threshold, const Matx33d &K1, const Matx33d &K2);
float findMedian (std::vector<float> &array);
}
namespace Math {

9
modules/calib3d/src/usac/dls_solver.cpp
View File

@ -49,13 +49,14 @@ namespace cv { namespace usac {
// This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented
class DLSPnPImpl : public DLSPnP {
private:
const Mat * points_mat, * calib_norm_points_mat, * K_mat;
const Mat * points_mat, * calib_norm_points_mat;
const Matx33d * K_mat;
#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
const Mat &K;
const Matx33d &K;
const float * const calib_norm_points, * const points;
#endif
public:
explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_) :
explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Matx33d &K_) :
points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_)
#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
, K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data)
@ -870,7 +871,7 @@ protected:
2 * D[74] - 2 * D[78]); // s1^3
}
};
Ptr<DLSPnP> DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
Ptr<DLSPnP> DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) {
return makePtr<DLSPnPImpl>(points_, calib_norm_pts, K);
}
}}

9
modules/calib3d/src/usac/pnp_solver.cpp
View File

@ -263,7 +263,8 @@ private:
* calibrated normalized points
* K^-1 [u v 1]^T / ||K^-1 [u v 1]^T||
*/
const Mat * points_mat, * calib_norm_points_mat, * K_mat, &K;
const Mat * points_mat, * calib_norm_points_mat;
const Matx33d * K_mat, &K;
const float * const calib_norm_points, * const points;
const double VAL_THR = 1e-4;
public:
@ -271,7 +272,7 @@ public:
* @points_ is matrix N x 5
* u v x y z. (u,v) is image point, (x y z) is world point
*/
P3PSolverImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_) :
P3PSolverImpl (const Mat &points_, const Mat &calib_norm_points_, const Matx33d &K_) :
points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_),
K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data) {}
@ -362,7 +363,7 @@ public:
const Matx33d R = Math::rotVec2RotMat(Math::rotMat2RotVec(Z * Zw.inv()));
Mat P, KR = K * R;
Mat P, KR = Mat(K * R);
hconcat(KR, -KR * (X1 - R.t() * nX1), P);
models.emplace_back(P);
}
@ -374,7 +375,7 @@ public:
return makePtr<P3PSolverImpl>(*points_mat, *calib_norm_points_mat, *K_mat);
}
};
Ptr<P3PSolver> P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
Ptr<P3PSolver> P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) {
return makePtr<P3PSolverImpl>(points_, calib_norm_pts, K);
}
}}

11
modules/calib3d/src/usac/ransac_solvers.cpp
View File

@ -774,13 +774,14 @@ bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2
Ptr<MinimalSolver> min_solver;
Ptr<NonMinimalSolver> non_min_solver;
Mat points, K1, K2, calib_points, undist_points1, undist_points2;
Mat points, calib_points, undist_points1, undist_points2;
Matx33d K1, K2;
int points_size;
double threshold = params->getThreshold(), max_thr = params->getMaximumThreshold();
const int min_sample_size = params->getSampleSize();
if (params->isPnP()) {
if (! K1_.empty()) {
K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
K1 = K1_.getMat();
if (! dist_coeff1.empty()) {
// undistortPoints also calibrate points using K
if (points1.isContinuous())
@ -797,8 +798,8 @@ bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2
} else {
if (params->isEssential()) {
CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!");
K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
K2 = K2_.getMat(); K2.convertTo(K2, CV_64F);
K1 = K1_.getMat();
K2 = K2_.getMat();
if (! dist_coeff1.empty() || ! dist_coeff2.empty()) {
// undistortPoints also calibrate points using K
if (points1.isContinuous())
@ -1011,7 +1012,7 @@ bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2
// convert R to rodrigues and back and recalculate inliers which due to numerical
// issues can differ
Mat out, R, newR, newP, t, rvec;
if (K1.empty()) {
if (K1_.empty()) {
usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t);
Rodrigues(R, rvec);
hconcat(rvec, t, out);

70
modules/calib3d/src/usac/utils.cpp
View File

@ -8,9 +8,9 @@
#include <map>
namespace cv { namespace usac {
double Utils::getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2) {
return threshold / ((K1.at<double>(0, 0) + K1.at<double>(1, 1) +
K2.at<double>(0, 0) + K2.at<double>(1, 1)) / 4.0);
double Utils::getCalibratedThreshold (double threshold, const Matx33d &K1, const Matx33d &K2) {
return threshold / ((K1(0, 0) + K1(1, 1) +
K2(0, 0) + K2(1, 1)) / 4.0);
}
/*
@ -20,22 +20,20 @@ double Utils::getCalibratedThreshold (double threshold, const Mat &K1, const Mat
* 0 k22 k23
* 0 0 1]
*/
void Utils::calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &calib_points) {
const auto * const points_ = (float *) points.data;
const auto * const k1 = (double *) K1.data;
const auto inv1_k11 = float(1 / k1[0]); // 1 / k11
const auto inv1_k12 = float(-k1[1] / (k1[0]*k1[4])); // -k12 / (k11*k22)
void Utils::calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &points, Mat &calib_points) {
const auto inv1_k11 = float(1 / K1(0, 0)); // 1 / k11
const auto inv1_k12 = float(-K1(0, 1) / (K1(0, 0)*K1(1, 1))); // -k12 / (k11*k22)
// (-k13*k22 + k12*k23) / (k11*k22)
const auto inv1_k13 = float((-k1[2]*k1[4] + k1[1]*k1[5]) / (k1[0]*k1[4]));
const auto inv1_k22 = float(1 / k1[4]); // 1 / k22
const auto inv1_k23 = float(-k1[5] / k1[4]); // -k23 / k22
const auto inv1_k13 = float((-K1(0, 2)*K1(1, 1) + K1(0, 1)*K1(1, 2)) / (K1(0, 0)*K1(1, 1)));
const auto inv1_k22 = float(1 / K1(1, 1)); // 1 / k22
const auto inv1_k23 = float(-K1(1, 2) / K1(1, 1)); // -k23 / k22
const auto * const k2 = (double *) K2.data;
const auto inv2_k11 = float(1 / k2[0]);
const auto inv2_k12 = float(-k2[1] / (k2[0]*k2[4]));
const auto inv2_k13 = float((-k2[2]*k2[4] + k2[1]*k2[5]) / (k2[0]*k2[4]));
const auto inv2_k22 = float(1 / k2[4]);
const auto inv2_k23 = float(-k2[5] / k2[4]);
const auto inv2_k11 = float(1 / K2(0, 0));
const auto inv2_k12 = float(-K2(0, 1) / (K2(0, 0)*K2(1, 1)));
const auto inv2_k13 = float((-K2(0, 2)*K2(1, 1) + K2(0, 1)*K2(1, 2)) / (K2(0, 0)*K2(1, 1)));
const auto inv2_k22 = float(1 / K2(1, 1));
const auto inv2_k23 = float(-K2(1, 2) / K2(1, 1));
const auto * const points_ = (float *) points.data;
calib_points = Mat ( points.rows, 4, points.type());
auto * calib_points_ = (float *) calib_points.data;
@ -54,15 +52,13 @@ void Utils::calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Ma
* points is matrix of size |N| x 5, first two columns are image points [u_i, v_i]
* calib_norm_pts are K^-1 [u v 1]^T / ||K^-1 [u v 1]^T||
*/
void Utils::calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts) {
void Utils::calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Mat &calib_norm_pts) {
const auto * const points = (float *) pts.data;
const auto * const k = (double *) K.data;
const auto inv_k11 = float(1 / k[0]);
const auto inv_k12 = float(-k[1] / (k[0]*k[4]));
const auto inv_k13 = float((-k[2]*k[4] + k[1]*k[5]) / (k[0]*k[4]));
const auto inv_k22 = float(1 / k[4]);
const auto inv_k23 = float(-k[5] / k[4]);
const auto inv_k11 = float(1 / K(0, 0));
const auto inv_k12 = float(-K(0, 1) / (K(0, 0)*K(1, 1)));
const auto inv_k13 = float((-K(0, 2)*K(1, 1) + K(0, 1)*K(1, 2)) / (K(0, 0)*K(1, 1)));
const auto inv_k22 = float(1 / K(1, 1));
const auto inv_k23 = float(-K(1, 2) / K(1, 1));
calib_norm_pts = Mat (pts.rows, 3, pts.type());
auto * calib_norm_pts_ = (float *) calib_norm_pts.data;
@ -77,10 +73,9 @@ void Utils::calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &c
}
}
void Utils::normalizeAndDecalibPointsPnP (const Mat &K_, Mat &pts, Mat &calib_norm_pts) {
const auto * const K = (double *) K_.data;
const auto k11 = (float)K[0], k12 = (float)K[1], k13 = (float)K[2],
k22 = (float)K[4], k23 = (float)K[5];
void Utils::normalizeAndDecalibPointsPnP (const Matx33d &K_, Mat &pts, Mat &calib_norm_pts) {
const auto k11 = (float)K_(0, 0), k12 = (float)K_(0, 1), k13 = (float)K_(0, 2),
k22 = (float)K_(1, 1), k23 = (float)K_(1, 2);
calib_norm_pts = Mat (pts.rows, 3, pts.type());
auto * points = (float *) pts.data;
auto * calib_norm_pts_ = (float *) calib_norm_pts.data;
@ -103,20 +98,19 @@ void Utils::normalizeAndDecalibPointsPnP (const Mat &K_, Mat &pts, Mat &calib_no
* 0 fy ty
* 0 0 1]
*/
void Utils::decomposeProjection (const Mat &P, Mat &K_, Mat &R, Mat &t, bool same_focal) {
void Utils::decomposeProjection (const Mat &P, Matx33d &K_, Mat &R, Mat &t, bool same_focal) {
const Mat M = P.colRange(0,3);
double scale = norm(M.row(2)); scale *= scale;
Matx33d K = Matx33d::eye();
K(1,2) = M.row(1).dot(M.row(2)) / scale;
K(0,2) = M.row(0).dot(M.row(2)) / scale;
K(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K(1,2)*K(1,2));
K(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K(0,2)*K(0,2));
K_ = Matx33d::eye();
K_(1,2) = M.row(1).dot(M.row(2)) / scale;
K_(0,2) = M.row(0).dot(M.row(2)) / scale;
K_(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K_(1,2)*K_(1,2));
K_(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K_(0,2)*K_(0,2));
if (same_focal)
K(0,0) = K(1,1) = (K(0,0) + K(1,1)) / 2;
R = K.inv() * M / sqrt(scale);
K_(0,0) = K_(1,1) = (K_(0,0) + K_(1,1)) / 2;
R = K_.inv() * M / sqrt(scale);
if (determinant(M) < 0) R *= -1;
t = R * M.inv() * P.col(3);
K_ = Mat(K);
}
Matx33d Math::getSkewSymmetric(const Vec3d &v) {

27
modules/calib3d/test/test_usac.cpp
View File

@ -452,5 +452,32 @@ TEST(usac_testUsacParams, accuracy) {
checkInliersMask(TestSolver::Homogr, inl_size, usac_params.threshold, pts1, pts2, model, mask);
}
TEST(usac_solvePnPRansac, regression_21105) {
std::vector<int> gt_inliers;
const int pts_size = 100;
double inl_ratio = 0.1;
cv::Mat img_pts, obj_pts, K1, K2;
cv::RNG &rng = cv::theRNG();
generatePoints(rng, img_pts, obj_pts, K1, K2, false /*two calib*/,
pts_size, TestSolver ::PnP, inl_ratio, 0.15 /*noise std*/, gt_inliers);
const double conf = 0.99, thr = 2., max_iters = 1.3 * log(1 - conf) /
log(1 - pow(inl_ratio, 3 /* sample size */));
const int flag = USAC_DEFAULT;
std::vector<int> inliers;
cv::Matx31d rvec, tvec;
CV_Assert(cv::solvePnPRansac(obj_pts, img_pts, K1, cv::noArray(), rvec, tvec,
false, (int)max_iters, (float)thr, conf, inliers, flag));
cv::Mat zero_column = cv::Mat::zeros(3, 1, K1.type());
cv::hconcat(K1, zero_column, K1);
cv::Mat K1_copy = K1.colRange(0, 3);
std::vector<int> inliers_copy;
cv::Matx31d rvec_copy, tvec_copy;
CV_Assert(cv::solvePnPRansac(obj_pts, img_pts, K1_copy, cv::noArray(), rvec_copy, tvec_copy,
false, (int)max_iters, (float)thr, conf, inliers_copy, flag));
EXPECT_EQ(rvec, rvec_copy);
EXPECT_EQ(tvec, tvec_copy);
EXPECT_EQ(inliers, inliers_copy);
}
}} // namespace