| 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> |
|---|
| 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> |
|---|
| 18 | void (const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) { |
|---|
| 19 | points.clear(); |
|---|
| 20 | int npoints = std::max(a: opoints.checkVector(elemChannels: 3, CV_32F), b: opoints.checkVector(elemChannels: 3, CV_64F)); |
|---|
| 21 | points.resize(new_size: 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: |
|---|
| 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 | |
|---|
