1 #ifndef P3P_P3P_H 2 #define P3P_P3P_H 3 4 #include <opencv2/core.hpp> 5 6 namespace cv { 7 class ap3p { 8 private: 9 template<typename T> init_camera_parameters(const cv::Mat & cameraMatrix)10 void init_camera_parameters(const cv::Mat &cameraMatrix) { 11 cx = cameraMatrix.at<T>(0, 2); 12 cy = cameraMatrix.at<T>(1, 2); 13 fx = cameraMatrix.at<T>(0, 0); 14 fy = cameraMatrix.at<T>(1, 1); 15 } 16 17 template<typename OpointType, typename IpointType> extract_points(const cv::Mat & opoints,const cv::Mat & ipoints,std::vector<double> & points)18 void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) { 19 points.clear(); 20 int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); 21 points.resize(5*4); //resize vector to fit for p4p case 22 for (int i = 0; i < npoints; i++) { 23 points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx; 24 points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy; 25 points[i * 5 + 2] = opoints.at<OpointType>(i).x; 26 points[i * 5 + 3] = opoints.at<OpointType>(i).y; 27 points[i * 5 + 4] = opoints.at<OpointType>(i).z; 28 } 29 //Fill vectors with unused values for p3p case 30 for (int i = npoints; i < 4; i++) { 31 for (int j = 0; j < 5; j++) { 32 points[i * 5 + j] = 0; 33 } 34 } 35 } 36 37 void init_inverse_parameters(); 38 39 double fx, fy, cx, cy; 40 double inv_fx, inv_fy, cx_fx, cy_fy; 41 public: ap3p()42 ap3p() : fx(0), fy(0), cx(0), cy(0), inv_fx(0), inv_fy(0), cx_fx(0), cy_fy(0) {} 43 44 ap3p(double fx, double fy, double cx, double cy); 45 46 ap3p(cv::Mat cameraMatrix); 47 48 bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints); 49 int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints); 50 51 int solve(double R[4][3][3], double t[4][3], 52 double mu0, double mv0, double X0, double Y0, double Z0, 53 double mu1, double mv1, double X1, double Y1, double Z1, 54 double mu2, double mv2, double X2, double Y2, double Z2, 55 double mu3, double mv3, double X3, double Y3, double Z3, 56 bool p4p); 57 58 bool solve(double R[3][3], double t[3], 59 double mu0, double mv0, double X0, double Y0, double Z0, 60 double mu1, double mv1, double X1, double Y1, double Z1, 61 double mu2, double mv2, double X2, double Y2, double Z2, 62 double mu3, double mv3, double X3, double Y3, double Z3); 63 64 // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017) 65 // See https://arxiv.org/pdf/1701.08237.pdf 66 // featureVectors: 3 bearing measurements (normalized) stored as column vectors 67 // worldPoints: Positions of the 3 feature points stored as column vectors 68 // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame 69 // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame 70 int computePoses(const double featureVectors[3][4], const double worldPoints[3][4], double solutionsR[4][3][3], 71 double solutionsT[4][3], bool p4p); 72 73 }; 74 } 75 #endif //P3P_P3P_H 76