1#ifndef P3P_P3P_H
2#define P3P_P3P_H
3
4#include <opencv2/core.hpp>
5
6namespace cv {
7class ap3p {
8private:
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 extract_points(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;
41public:
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

source code of opencv/modules/calib3d/src/ap3p.h