123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195 |
- #ifndef PNPSOLVER_H
- #define PNPSOLVER_H
- #include <opencv2/core/core.hpp>
- #include "MapPoint.h"
- #include "Frame.h"
- namespace ORB_SLAM3
- {
- class PnPsolver {
- public:
- PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches);
- ~PnPsolver();
- void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4,
- float th2 = 5.991);
- cv::Mat find(vector<bool> &vbInliers, int &nInliers);
- cv::Mat iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers);
- private:
- void CheckInliers();
- bool Refine();
-
- void set_maximum_number_of_correspondences(const int n);
- void reset_correspondences(void);
- void add_correspondence(const double X, const double Y, const double Z,
- const double u, const double v);
- double compute_pose(double R[3][3], double T[3]);
- void relative_error(double & rot_err, double & transl_err,
- const double Rtrue[3][3], const double ttrue[3],
- const double Rest[3][3], const double test[3]);
- void print_pose(const double R[3][3], const double t[3]);
- double reprojection_error(const double R[3][3], const double t[3]);
- void choose_control_points(void);
- void compute_barycentric_coordinates(void);
- void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
- void compute_ccs(const double * betas, const double * ut);
- void compute_pcs(void);
- void solve_for_sign(void);
- void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
- void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
- void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
- void qr_solve(CvMat * A, CvMat * b, CvMat * X);
- double dot(const double * v1, const double * v2);
- double dist2(const double * p1, const double * p2);
- void compute_rho(double * rho);
- void compute_L_6x10(const double * ut, double * l_6x10);
- void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
- void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
- double cb[4], CvMat * A, CvMat * b);
- double compute_R_and_t(const double * ut, const double * betas,
- double R[3][3], double t[3]);
- void estimate_R_and_t(double R[3][3], double t[3]);
- void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
- double R_src[3][3], double t_src[3]);
- void mat_to_quat(const double R[3][3], double q[4]);
- double uc, vc, fu, fv;
- double * pws, * us, * alphas, * pcs;
- int maximum_number_of_correspondences;
- int number_of_correspondences;
- double cws[4][3], ccs[4][3];
- double cws_determinant;
- vector<MapPoint*> mvpMapPointMatches;
-
- vector<cv::Point2f> mvP2D;
- vector<float> mvSigma2;
-
- vector<cv::Point3f> mvP3Dw;
-
- vector<size_t> mvKeyPointIndices;
-
- double mRi[3][3];
- double mti[3];
- cv::Mat mTcwi;
- vector<bool> mvbInliersi;
- int mnInliersi;
-
- int mnIterations;
- vector<bool> mvbBestInliers;
- int mnBestInliers;
- cv::Mat mBestTcw;
-
- cv::Mat mRefinedTcw;
- vector<bool> mvbRefinedInliers;
- int mnRefinedInliers;
-
- int N;
-
- vector<size_t> mvAllIndices;
-
- double mRansacProb;
-
- int mRansacMinInliers;
-
- int mRansacMaxIts;
-
- float mRansacEpsilon;
-
- float mRansacTh;
-
- int mRansacMinSet;
-
- vector<float> mvMaxError;
- };
- }
- #endif
|