Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #23806 from asmorkalov:as/usac_drop_mat_ptr

Browse Source 
Get rid of unsafe raw pointers to Mat object in USAC
pull/23862/head
Alexander Smorkalov 1 year ago committed by GitHub
parent e7501b69ea 0866a135c6
commit ee97dd5211
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 141 additions and 91 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. 30
      modules/calib3d/src/usac/degeneracy.cpp
  2. 13
      modules/calib3d/src/usac/dls_solver.cpp
  3. 11
      modules/calib3d/src/usac/essential_solver.cpp
  4. 71
      modules/calib3d/src/usac/estimator.cpp
  5. 30
      modules/calib3d/src/usac/fundamental_solver.cpp
  6. 41
      modules/calib3d/src/usac/homography_solver.cpp
  7. 36
      modules/calib3d/src/usac/pnp_solver.cpp

30
modules/calib3d/src/usac/degeneracy.cpp
Unescape View File

@ -8,12 +8,14 @@
namespace cv { namespace usac {
class EpipolarGeometryDegeneracyImpl : public EpipolarGeometryDegeneracy {
private:
const Mat * points_mat;
const float * const points; // i-th row xi1 yi1 xi2 yi2
Mat points_mat;
const int min_sample_size;
public:
explicit EpipolarGeometryDegeneracyImpl (const Mat &points_, int sample_size_) :
points_mat(&points_), points ((float*) points_mat->data), min_sample_size (sample_size_) {}
points_mat(points_), min_sample_size (sample_size_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
/*
* Do oriented constraint to verify if epipolar geometry is in front or behind the camera.
* Return: true if all points are in front of the camers w.r.t. tested epipolar geometry - satisfies constraint.
@ -26,6 +28,7 @@ public:
const Vec3d ep = Utils::getRightEpipole(F_);
const auto * const e = ep.val; // of size 3x1
const auto * const F = (double *) F_.data;
const float * points = points_mat.ptr<float>();
// without loss of generality, let the first point in sample be in front of the camera.
int pt = 4*sample[0];
@ -67,16 +70,19 @@ Ptr<EpipolarGeometryDegeneracy> EpipolarGeometryDegeneracy::create (const Mat &p
class HomographyDegeneracyImpl : public HomographyDegeneracy {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
const float TOLERANCE = 2 * FLT_EPSILON; // 2 from area of triangle
public:
explicit HomographyDegeneracyImpl (const Mat &points_) :
points_mat(&points_), points ((float *)points_mat->data) {}
points_mat(points_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline bool isSampleGood (const std::vector<int> &sample) const override {
const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2], smpl4 = 4*sample[3];
// planar correspondences must lie on the same side of any line from two points in sample
const float * points = points_mat.ptr<float>();
const float x1 = points[smpl1], y1 = points[smpl1+1], X1 = points[smpl1+2], Y1 = points[smpl1+3];
const float x2 = points[smpl2], y2 = points[smpl2+1], X2 = points[smpl2+2], Y2 = points[smpl2+3];
const float x3 = points[smpl3], y3 = points[smpl3+1], X3 = points[smpl3+2], Y3 = points[smpl3+3];
@ -188,8 +194,7 @@ private:
const Ptr<Quality> quality;
const Ptr<Error> f_error;
Ptr<Quality> h_repr_quality;
const float * const points;
const Mat * points_mat;
Mat points_mat;
const Ptr<ReprojectionErrorForward> h_reproj_error;
Ptr<EpipolarNonMinimalSolver> f_non_min_solver;
Ptr<HomographyNonMinimalSolver> h_non_min_solver;
@ -215,7 +220,7 @@ public:
FundamentalDegeneracyImpl (int state, const Ptr<Quality> &quality_, const Mat &points_,
int sample_size_, int plane_and_parallax_iters, double homography_threshold_,
double f_inlier_thr_sqr, const Mat true_K1_, const Mat true_K2_) :
rng (state), quality(quality_), f_error(quality_->getErrorFnc()), points((float *) points_.data), points_mat(&points_),
rng (state), quality(quality_), f_error(quality_->getErrorFnc()), points_mat(points_),
h_reproj_error(ReprojectionErrorForward::create(points_)),
ep_deg (points_, sample_size_), homography_threshold (homography_threshold_),
points_size (quality_->getPointsSize()),
@ -330,7 +335,7 @@ public:
bool recoverIfDegenerate (const std::vector<int> &sample, const Mat &F_best, const Score &F_best_score,
Mat &non_degenerate_model, Score &non_degenerate_model_score) override {
const auto swapF = [&] (const Mat &_F, const Score &_score) {
const auto non_min_solver = EpipolarNonMinimalSolver::create(*points_mat, true);
const auto non_min_solver = EpipolarNonMinimalSolver::create(points_mat, true);
if (! optimizeF(_F, _score, non_degenerate_model, non_degenerate_model_score)) {
_F.copyTo(non_degenerate_model);
non_degenerate_model_score = _score;
@ -401,6 +406,7 @@ public:
return false;
num_models_used_so_far = 0; // reset estimation of lambda for plane-and-parallax
int max_iters = max_iters_pl_par, non_planar_support = 0, iters = 0;
const float * points = points_mat.ptr<float>();
std::vector<Matx33d> F_good;
const double CLOSE_THR = 1.0;
for (; iters < max_iters; iters++) {
@ -454,6 +460,7 @@ public:
return true;
}
bool calibDegensac (const Matx33d &H, Mat &F_new, Score &F_new_score, int non_planar_support_degen_F, const Score &F_degen_score) {
const float * points = points_mat.ptr<float>();
if (! is_principal_pt_set) {
// estimate principal points from coordinates
float px1 = 0, py1 = 0, px2 = 0, py2 = 0;
@ -606,6 +613,7 @@ public:
}
private:
bool getH (const Matx33d &A, const Vec3d &e_prime, int smpl1, int smpl2, int smpl3, Matx33d &H) {
const float * points = points_mat.ptr<float>();
Vec3d p1(points[smpl1 ], points[smpl1+1], 1), p2(points[smpl2 ], points[smpl2+1], 1), p3(points[smpl3 ], points[smpl3+1], 1);
Vec3d P1(points[smpl1+2], points[smpl1+3], 1), P2(points[smpl2+2], points[smpl2+3], 1), P3(points[smpl3+2], points[smpl3+3], 1);
const Matx33d M (p1[0], p1[1], 1, p2[0], p2[1], 1, p3[0], p3[1], 1);
@ -684,4 +692,4 @@ public:
Ptr<EssentialDegeneracy> EssentialDegeneracy::create (const Mat &points_, int sample_size_) {
return makePtr<EssentialDegeneracyImpl>(points_, sample_size_);
}
}}
}}

13
modules/calib3d/src/usac/dls_solver.cpp
Unescape View File

@ -49,13 +49,15 @@ namespace cv { namespace usac {
class DLSPnPImpl : public DLSPnP {
#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
private:
const Mat * points_mat, * calib_norm_points_mat;
Mat points_mat, calib_norm_points_mat;
const Matx33d K;
const float * const calib_norm_points, * const points;
public:
explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_)
: points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K(K_),
calib_norm_points((float*)calib_norm_points_mat->data), points((float*)points_mat->data) {}
: points_mat(points_), calib_norm_points_mat(calib_norm_points_), K(K_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
CV_DbgAssert(!calib_norm_points_mat.empty() && calib_norm_points_mat.isContinuous());
}
#else
public:
explicit DLSPnPImpl (const Mat &, const Mat &, const Mat &) {}
@ -88,7 +90,8 @@ public:
// Compute V*W*b with the rotation parameters factored out. This is the
// translation parameterized by the 9 entries of the rotation matrix.
Matx<double, 3, 9> translation_factor = Matx<double, 3, 9>::zeros();
const float * points = points_mat.ptr<float>();
const float * calib_norm_points = calib_norm_points_mat.ptr<float>();
for (int i = 0; i < sample_number; i++) {
const int idx_world = 5 * sample[i], idx_calib = 3 * sample[i];
Vec3d normalized_feature_pos(calib_norm_points[idx_calib],

11
modules/calib3d/src/usac/essential_solver.cpp
Unescape View File

@ -23,14 +23,17 @@ namespace cv { namespace usac {
class EssentialMinimalSolver5ptsImpl : public EssentialMinimalSolver5pts {
private:
// Points must be calibrated K^-1 x
const Mat * points_mat;
const float * const pts;
const Mat points_mat;
const bool use_svd, is_nister;
public:
explicit EssentialMinimalSolver5ptsImpl (const Mat &points_, bool use_svd_=false, bool is_nister_=false) :
points_mat(&points_), pts((float*)points_mat->data), use_svd(use_svd_), is_nister(is_nister_) {}
points_mat(points_), use_svd(use_svd_), is_nister(is_nister_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
const float * pts = points_mat.ptr<float>();
// (1) Extract 4 null vectors from linear equations of epipolar constraint
std::vector<double> coefficients(45); // 5 pts=rows, 9 columns
auto *coefficients_ = &coefficients[0];
@ -343,4 +346,4 @@ Ptr<EssentialMinimalSolver5pts> EssentialMinimalSolver5pts::create
(const Mat &points_, bool use_svd, bool is_nister) {
return makePtr<EssentialMinimalSolver5ptsImpl>(points_, use_svd, is_nister);
}
}}
}}

71
modules/calib3d/src/usac/estimator.cpp
Unescape View File

@ -216,19 +216,18 @@ Ptr<PnPEstimator> PnPEstimator::create (const Ptr<MinimalSolver> &min_solver_,
// Symmetric Reprojection Error
class ReprojectionErrorSymmetricImpl : public ReprojectionErrorSymmetric {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
float m11, m12, m13, m21, m22, m23, m31, m32, m33;
float minv11, minv12, minv13, minv21, minv22, minv23, minv31, minv32, minv33;
std::vector<float> errors;
public:
explicit ReprojectionErrorSymmetricImpl (const Mat &points_)
: points_mat(&points_), points ((float *) points_.data)
: points_mat(points_)
, m11(0), m12(0), m13(0), m21(0), m22(0), m23(0), m31(0), m32(0), m33(0)
, minv11(0), minv12(0), minv13(0), minv21(0), minv22(0), minv23(0), minv31(0), minv32(0), minv33(0)
, errors(points_.rows)
{
CV_DbgAssert(points);
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline void setModelParameters(const Mat& model) override
@ -250,6 +249,7 @@ public:
}
inline float getError (int idx) const override {
idx *= 4;
const float * points = points_mat.ptr<float>();
const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
const float est_z2 = 1 / (m31 * x1 + m32 * y1 + m33),
dx2 = x2 - (m11 * x1 + m12 * y1 + m13) * est_z2,
@ -261,7 +261,8 @@ public:
}
const std::vector<float> &getErrors (const Mat &model) override {
setModelParameters(model);
for (int point_idx = 0; point_idx < points_mat->rows; point_idx++) {
const float * points = points_mat.ptr<float>();
for (int point_idx = 0; point_idx < points_mat.rows; point_idx++) {
const int smpl = 4*point_idx;
const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
const float est_z2 = 1 / (m31 * x1 + m32 * y1 + m33),
@ -283,17 +284,16 @@ ReprojectionErrorSymmetric::create(const Mat &points) {
// Forward Reprojection Error
class ReprojectionErrorForwardImpl : public ReprojectionErrorForward {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
float m11, m12, m13, m21, m22, m23, m31, m32, m33;
std::vector<float> errors;
public:
explicit ReprojectionErrorForwardImpl (const Mat &points_)
: points_mat(&points_), points ((float *)points_.data)
: points_mat(points_)
, m11(0), m12(0), m13(0), m21(0), m22(0), m23(0), m31(0), m32(0), m33(0)
, errors(points_.rows)
{
CV_DbgAssert(points);
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline void setModelParameters(const Mat& model) override
@ -308,6 +308,7 @@ public:
}
inline float getError (int idx) const override {
idx *= 4;
const float * points = points_mat.ptr<float>();
const float x1 = points[idx], y1 = points[idx+1], x2 = points[idx+2], y2 = points[idx+3];
const float est_z2 = 1 / (m31 * x1 + m32 * y1 + m33),
dx2 = x2 - (m11 * x1 + m12 * y1 + m13) * est_z2,
@ -316,7 +317,8 @@ public:
}
const std::vector<float> &getErrors (const Mat &model) override {
setModelParameters(model);
for (int point_idx = 0; point_idx < points_mat->rows; point_idx++) {
const float * points = points_mat.ptr<float>();
for (int point_idx = 0; point_idx < points_mat.rows; point_idx++) {
const int smpl = 4*point_idx;
const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
const float est_z2 = 1 / (m31 * x1 + m32 * y1 + m33),
@ -334,17 +336,16 @@ ReprojectionErrorForward::create(const Mat &points) {
class SampsonErrorImpl : public SampsonError {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
float m11, m12, m13, m21, m22, m23, m31, m32, m33;
std::vector<float> errors;
public:
explicit SampsonErrorImpl (const Mat &points_)
: points_mat(&points_), points ((float *) points_.data)
: points_mat(points_)
, m11(0), m12(0), m13(0), m21(0), m22(0), m23(0), m31(0), m32(0), m33(0)
, errors(points_.rows)
{
CV_DbgAssert(points);
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline void setModelParameters(const Mat& model) override
@ -370,6 +371,7 @@ public:
*/
inline float getError (int idx) const override {
idx *= 4;
const float * points = points_mat.ptr<float>();
const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
const float F_pt1_x = m11 * x1 + m12 * y1 + m13,
F_pt1_y = m21 * x1 + m22 * y1 + m23;
@ -381,7 +383,8 @@ public:
}
const std::vector<float> &getErrors (const Mat &model) override {
setModelParameters(model);
for (int point_idx = 0; point_idx < points_mat->rows; point_idx++) {
const float * points = points_mat.ptr<float>();
for (int point_idx = 0; point_idx < points_mat.rows; point_idx++) {
const int smpl = 4*point_idx;
const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
const float F_pt1_x = m11 * x1 + m12 * y1 + m13,
@ -402,17 +405,16 @@ SampsonError::create(const Mat &points) {
class SymmetricGeometricDistanceImpl : public SymmetricGeometricDistance {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
float m11, m12, m13, m21, m22, m23, m31, m32, m33;
std::vector<float> errors;
public:
explicit SymmetricGeometricDistanceImpl (const Mat &points_)
: points_mat(&points_), points ((float *) points_.data)
: points_mat(points_)
, m11(0), m12(0), m13(0), m21(0), m22(0), m23(0), m31(0), m32(0), m33(0)
, errors(points_.rows)
{
CV_DbgAssert(points);
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline void setModelParameters(const Mat& model) override
@ -428,6 +430,7 @@ public:
inline float getError (int idx) const override {
idx *= 4;
const float * points = points_mat.ptr<float>();
const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
// pt2^T * E, line 1 = [l1 l2]
const float l1 = x2 * m11 + y2 * m21 + m31,
@ -443,7 +446,8 @@ public:
}
const std::vector<float> &getErrors (const Mat &model) override {
setModelParameters(model);
for (int point_idx = 0; point_idx < points_mat->rows; point_idx++) {
const float * points = points_mat.ptr<float>();
for (int point_idx = 0; point_idx < points_mat.rows; point_idx++) {
const int smpl = 4*point_idx;
const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
const float l1 = x2 * m11 + y2 * m21 + m31, t1 = m11 * x1 + m12 * y1 + m13,
@ -462,17 +466,16 @@ SymmetricGeometricDistance::create(const Mat &points) {
class ReprojectionErrorPmatrixImpl : public ReprojectionErrorPmatrix {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
float p11, p12, p13, p14, p21, p22, p23, p24, p31, p32, p33, p34;
std::vector<float> errors;
public:
explicit ReprojectionErrorPmatrixImpl (const Mat &points_)
: points_mat(&points_), points ((float *) points_.data)
: points_mat(points_)
, p11(0), p12(0), p13(0), p14(0), p21(0), p22(0), p23(0), p24(0), p31(0), p32(0), p33(0), p34(0)
, errors(points_.rows)
{
CV_DbgAssert(points);
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
@ -489,6 +492,7 @@ public:
inline float getError (int idx) const override {
idx *= 5;
const float * points = points_mat.ptr<float>();
const float u = points[idx ], v = points[idx+1],
x = points[idx+2], y = points[idx+3], z = points[idx+4];
const float depth = 1 / (p31 * x + p32 * y + p33 * z + p34);
@ -498,7 +502,8 @@ public:
}
const std::vector<float> &getErrors (const Mat &model) override {
setModelParameters(model);
for (int point_idx = 0; point_idx < points_mat->rows; point_idx++) {
const float * points = points_mat.ptr<float>();
for (int point_idx = 0; point_idx < points_mat.rows; point_idx++) {
const int smpl = 5*point_idx;
const float u = points[smpl ], v = points[smpl+1],
x = points[smpl+2], y = points[smpl+3], z = points[smpl+4];
@ -523,17 +528,16 @@ private:
* m21 m22 m23
* 0 0 1
*/
const Mat * points_mat;
const float * const points;
Mat points_mat;
float m11, m12, m13, m21, m22, m23;
std::vector<float> errors;
public:
explicit ReprojectionDistanceAffineImpl (const Mat &points_)
: points_mat(&points_), points ((float *) points_.data)
: points_mat(points_)
, m11(0), m12(0), m13(0), m21(0), m22(0), m23(0)
, errors(points_.rows)
{
CV_DbgAssert(points);
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline void setModelParameters(const Mat& model) override
@ -547,13 +551,15 @@ public:
}
inline float getError (int idx) const override {
idx *= 4;
const float * points = points_mat.ptr<float>();
const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
const float dx2 = x2 - (m11 * x1 + m12 * y1 + m13), dy2 = y2 - (m21 * x1 + m22 * y1 + m23);
return dx2 * dx2 + dy2 * dy2;
}
const std::vector<float> &getErrors (const Mat &model) override {
setModelParameters(model);
for (int point_idx = 0; point_idx < points_mat->rows; point_idx++) {
const float * points = points_mat.ptr<float>();
for (int point_idx = 0; point_idx < points_mat.rows; point_idx++) {
const int smpl = 4*point_idx;
const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
const float dx2 = x2 - (m11 * x1 + m12 * y1 + m13), dy2 = y2 - (m21 * x1 + m22 * y1 + m23);
@ -570,13 +576,14 @@ ReprojectionErrorAffine::create(const Mat &points) {
////////////////////////////////////// NORMALIZING TRANSFORMATION /////////////////////////
class NormTransformImpl : public NormTransform {
private:
const float * const points;
Mat points_mat;
public:
explicit NormTransformImpl (const Mat &points_) : points((float*)points_.data) {}
explicit NormTransformImpl (const Mat &points_) : points_mat(points_) {}
// Compute normalized points and transformation matrices.
void getNormTransformation (Mat& norm_points, const std::vector<int> &sample,
int sample_size, Matx33d &T1, Matx33d &T2) const override {
const float * points = points_mat.ptr<float>();
double mean_pts1_x = 0, mean_pts1_y = 0, mean_pts2_x = 0, mean_pts2_y = 0;
// find average of each coordinate of points.

30
modules/calib3d/src/usac/fundamental_solver.cpp
Unescape View File

@ -12,17 +12,20 @@
namespace cv { namespace usac {
class FundamentalMinimalSolver7ptsImpl: public FundamentalMinimalSolver7pts {
private:
const Mat * points_mat; // pointer to OpenCV Mat
const float * const points; // pointer to points_mat->data for faster data access
Mat points_mat;
const bool use_ge;
public:
explicit FundamentalMinimalSolver7ptsImpl (const Mat &points_, bool use_ge_) :
points_mat (&points_), points ((float *) points_mat->data), use_ge(use_ge_) {}
points_mat (points_), use_ge(use_ge_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
const int m = 7, n = 9; // rows, cols
std::vector<double> a(63); // m*n
auto * a_ = &a[0];
const float * points = points_mat.ptr<float>();
for (int i = 0; i < m; i++ ) {
const int smpl = 4*sample[i];
@ -158,17 +161,19 @@ Ptr<FundamentalMinimalSolver7pts> FundamentalMinimalSolver7pts::create(const Mat
class FundamentalMinimalSolver8ptsImpl : public FundamentalMinimalSolver8pts {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
public:
explicit FundamentalMinimalSolver8ptsImpl (const Mat &points_) :
points_mat (&points_), points ((float*) points_mat->data)
{ CV_DbgAssert(points); }
points_mat (points_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
const int m = 8, n = 9; // rows, cols
std::vector<double> a(72); // m*n
auto * a_ = &a[0];
const float * points = points_mat.ptr<float>();
for (int i = 0; i < m; i++ ) {
const int smpl = 4*sample[i];
@ -233,18 +238,19 @@ Ptr<FundamentalMinimalSolver8pts> FundamentalMinimalSolver8pts::create(const Mat
class EpipolarNonMinimalSolverImpl : public EpipolarNonMinimalSolver {
private:
const Mat * points_mat;
Mat points_mat;
const bool do_norm;
Matx33d _T1, _T2;
Ptr<NormTransform> normTr = nullptr;
bool enforce_rank = true, is_fundamental, use_ge;
public:
explicit EpipolarNonMinimalSolverImpl (const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge_)
: points_mat(&points_), do_norm(false), _T1(T1), _T2(T2), use_ge(use_ge_) {
is_fundamental = true;
: points_mat(points_), do_norm(false), _T1(T1), _T2(T2), is_fundamental(true), use_ge(use_ge_) {
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
explicit EpipolarNonMinimalSolverImpl (const Mat &points_, bool is_fundamental_) :
points_mat(&points_), do_norm(is_fundamental_), use_ge(false) {
points_mat(points_), do_norm(is_fundamental_), use_ge(false) {
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
is_fundamental = is_fundamental_;
if (is_fundamental)
normTr = NormTransform::create(points_);
@ -259,7 +265,7 @@ public:
Mat norm_points;
if (do_norm)
normTr->getNormTransformation(norm_points, sample, sample_size, T1, T2);
const auto * const norm_pts = do_norm ? (float *) norm_points.data : (float *) points_mat->data;
const float * const norm_pts = do_norm ? norm_points.ptr<const float>() : points_mat.ptr<float>();
if (use_ge) {
double a[8];

41
modules/calib3d/src/usac/homography_solver.cpp
Unescape View File

@ -11,14 +11,17 @@
namespace cv { namespace usac {
class HomographyMinimalSolver4ptsImpl : public HomographyMinimalSolver4pts {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
const bool use_ge;
public:
explicit HomographyMinimalSolver4ptsImpl (const Mat &points_, bool use_ge_) :
points_mat(&points_), points ((float*) points_mat->data), use_ge(use_ge_) {}
points_mat(points_), use_ge(use_ge_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
int estimate (const std::vector<int>& sample, std::vector<Mat> &models) const override {
const float * points = points_mat.ptr<float>();
int m = 8, n = 9;
std::vector<double> A(72, 0);
int cnt = 0;
@ -80,15 +83,21 @@ Ptr<HomographyMinimalSolver4pts> HomographyMinimalSolver4pts::create(const Mat &
class HomographyNonMinimalSolverImpl : public HomographyNonMinimalSolver {
private:
const Mat * points_mat;
Mat points_mat;
const bool do_norm, use_ge;
Ptr<NormTransform> normTr;
Matx33d _T1, _T2;
public:
explicit HomographyNonMinimalSolverImpl (const Mat &norm_points_, const Matx33d &T1, const Matx33d &T2, bool use_ge_) :
points_mat(&norm_points_), do_norm(false), use_ge(use_ge_), _T1(T1), _T2(T2) {}
points_mat(norm_points_), do_norm(false), use_ge(use_ge_), _T1(T1), _T2(T2)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
explicit HomographyNonMinimalSolverImpl (const Mat &points_, bool use_ge_) :
points_mat(&points_), do_norm(true), use_ge(use_ge_), normTr (NormTransform::create(points_)) {}
points_mat(points_), do_norm(true), use_ge(use_ge_), normTr (NormTransform::create(points_))
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
int estimate (const std::vector<int> &sample, int sample_size, std::vector<Mat> &models,
const std::vector<double> &weights) const override {
@ -99,7 +108,7 @@ public:
Mat norm_points_;
if (do_norm)
normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2);
const auto * const npts = do_norm ? (float *) norm_points_.data : (float *) points_mat->data;
const float * const npts = do_norm ? norm_points_.ptr<float>() : points_mat.ptr<float>();
Mat H;
if (use_ge) {
@ -388,11 +397,13 @@ Ptr<CovarianceHomographySolver> CovarianceHomographySolver::create (const Mat &p
class AffineMinimalSolverImpl : public AffineMinimalSolver {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
public:
explicit AffineMinimalSolverImpl (const Mat &points_) :
points_mat(&points_), points((float *) points_mat->data) {}
points_mat(points_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
/*
Affine transformation
x1 y1 1 0 0 0 a u1
@ -404,6 +415,7 @@ public:
*/
int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2];
const float * points = points_mat.ptr<float>();
const auto
x1 = points[smpl1], y1 = points[smpl1+1], u1 = points[smpl1+2], v1 = points[smpl1+3],
x2 = points[smpl2], y2 = points[smpl2+1], u2 = points[smpl2+2], v2 = points[smpl2+3],
@ -435,13 +447,14 @@ Ptr<AffineMinimalSolver> AffineMinimalSolver::create(const Mat &points_) {
class AffineNonMinimalSolverImpl : public AffineNonMinimalSolver {
private:
const Mat * points_mat;
Mat points_mat;
Ptr<NormTransform> normTr;
Matx33d _T1, _T2;
bool do_norm;
public:
explicit AffineNonMinimalSolverImpl (const Mat &points_, InputArray T1, InputArray T2) :
points_mat(&points_) {
points_mat(points_) {
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
if (!T1.empty() && !T2.empty()) {
do_norm = false;
_T1 = T1.getMat();
@ -460,7 +473,7 @@ public:
Mat norm_points_;
if (do_norm)
normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2);
const auto * const pts = normTr ? (float *) norm_points_.data : (float *) points_mat->data;
const float * const pts = normTr ? norm_points_.ptr<float>() : points_mat.ptr<float>();
// do Least Squares
// Ax = b -> A^T Ax = A^T b
@ -640,4 +653,4 @@ Ptr<CovarianceAffineSolver> CovarianceAffineSolver::create (const Mat &points, c
Ptr<CovarianceAffineSolver> CovarianceAffineSolver::create (const Mat &points) {
return makePtr<CovarianceAffineSolverImpl>(points);
}
}}
}}

36
modules/calib3d/src/usac/pnp_solver.cpp
Unescape View File

@ -15,15 +15,17 @@
namespace cv { namespace usac {
class PnPMinimalSolver6PtsImpl : public PnPMinimalSolver6Pts {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
public:
// linear 6 points required (11 equations)
int getSampleSize() const override { return 6; }
int getMaxNumberOfSolutions () const override { return 1; }
explicit PnPMinimalSolver6PtsImpl (const Mat &points_) :
points_mat(&points_), points ((float*)points_mat->data) {}
points_mat(points_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
/*
DLT:
d x = P X, x = (u, v, 1), X = (X, Y, Z, 1), P = K[R t]
@ -72,7 +74,7 @@ public:
int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
std::vector<double> A1 (60, 0), A2(56, 0); // 5x12, 7x8
const float * points = points_mat.ptr<float>();
// std::vector<double> A_all(11*12, 0);
// int cnt3 = 0;
@ -154,14 +156,17 @@ Ptr<PnPMinimalSolver6Pts> PnPMinimalSolver6Pts::create(const Mat &points_) {
class PnPNonMinimalSolverImpl : public PnPNonMinimalSolver {
private:
const Mat * points_mat;
const float * const points;
Mat points_mat;
public:
explicit PnPNonMinimalSolverImpl (const Mat &points_) :
points_mat(&points_), points ((float*)points_mat->data){}
points_mat(points_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
int estimate (const std::vector<int> &sample, int sample_size,
std::vector<Mat> &models, const std::vector<double> &weights) const override {
std::vector<Mat> &models, const std::vector<double> &weights) const override {
const float * points = points_mat.ptr<float>();
if (sample_size < 6)
return 0;
@ -284,9 +289,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;
const Mat points_mat, calib_norm_points_mat;
const Matx33d K;
const float * const calib_norm_points, * const points;
const double VAL_THR = 1e-4;
public:
/*
@ -294,8 +298,11 @@ public:
* 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_) :
points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K(K_),
calib_norm_points((float*)calib_norm_points_mat->data), points((float*)points_mat->data) {}
points_mat(points_), calib_norm_points_mat(calib_norm_points_), K(K_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
CV_DbgAssert(!calib_norm_points_mat.empty() && calib_norm_points_mat.isContinuous());
}
int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
/*
@ -303,6 +310,9 @@ public:
* http://cmp.felk.cvut.cz/~pajdla/gvg/GVG-2016-Lecture.pdf
* pages: 51-59
*/
const float * points = points_mat.ptr<float>();
const float * calib_norm_points = calib_norm_points_mat.ptr<float>();
const int idx1 = 5*sample[0], idx2 = 5*sample[1], idx3 = 5*sample[2];
const Vec3d X1 (points[idx1+2], points[idx1+3], points[idx1+4]);
const Vec3d X2 (points[idx2+2], points[idx2+3], points[idx2+4]);
@ -396,4 +406,4 @@ public:
Ptr<P3PSolver> P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
return makePtr<P3PSolverImpl>(points_, calib_norm_pts, K);
}
}}
}}

Write Preview
Loading…
Cancel
Save