Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues 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
pull/21253/head
Polina Smolnikova 3 years ago committed by GitHub
parent d24befa0bc
commit 456137fa24
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 83 additions and 59 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      modules/calib3d/src/usac.hpp
  2. 9
      modules/calib3d/src/usac/dls_solver.cpp
  3. 9
      modules/calib3d/src/usac/pnp_solver.cpp
  4. 11
      modules/calib3d/src/usac/ransac_solvers.cpp
  5. 72
      modules/calib3d/src/usac/utils.cpp
  6. 27
      modules/calib3d/test/test_usac.cpp

14
modules/calib3d/src/usac.hpp
Unescape 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
Unescape 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
Unescape 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
Unescape 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);

72
modules/calib3d/src/usac/utils.cpp
Unescape 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 * 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 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 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
Unescape 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

Write Preview
Loading…
Cancel
Save