13 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <Eigen/Dense>
39 p3p(Eigen::MatrixXd cam_intrinsic);
43 p3p(cv::Mat cameraMatrix);
54 cv::Mat&
R, cv::Mat& tvec,
const cv::Mat& opoints,
55 const cv::Mat& ipoints);
58 Eigen::Ref<Eigen::Matrix3d>
R, Eigen::Ref<Eigen::Vector3d> t,
59 Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts);
83 double R[4][3][3],
double t[4][3],
double mu0,
double mv0,
double X0,
84 double Y0,
double Z0,
double mu1,
double mv1,
double X1,
double Y1,
85 double Z1,
double mu2,
double mv2,
double X2,
double Y2,
double Z2);
114 double R[3][3],
double t[3],
double mu0,
double mv0,
double X0,
115 double Y0,
double Z0,
double mu1,
double mv1,
double X1,
double Y1,
116 double Z1,
double mu2,
double mv2,
double X2,
double Y2,
double Z2,
117 double mu3,
double mv3,
double X3,
double Y3,
double Z3);
125 template <
typename T>
128 cx = cameraMatrix.at<T>(0, 2);
129 cy = cameraMatrix.at<T>(1, 2);
130 fx = cameraMatrix.at<T>(0, 0);
131 fy = cameraMatrix.at<T>(1, 1);
140 template <
typename Opo
intType,
typename Ipo
intType>
142 const cv::Mat& opoints,
const cv::Mat& ipoints,
143 std::vector<double>& points)
147 for (
int i = 0; i < 4; i++)
149 points[i * 5] = ipoints.at<IpointType>(i).x *
fx +
cx;
150 points[i * 5 + 1] = ipoints.at<IpointType>(i).y *
fy +
cy;
151 points[i * 5 + 2] = opoints.at<OpointType>(i).x;
152 points[i * 5 + 3] = opoints.at<OpointType>(i).y;
153 points[i * 5 + 4] = opoints.at<OpointType>(i).z;
165 Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts,
166 std::vector<double>& points)
170 for (
int i = 0; i < 4; i++)
172 points[i * 5] = img_pts(i, 0) *
fx +
cx;
173 points[i * 5 + 1] = img_pts(i, 1) *
fy +
cy;
174 points[i * 5 + 2] = obj_pts(i, 0);
175 points[i * 5 + 3] = obj_pts(i, 1);
176 points[i * 5 + 4] = obj_pts(i, 2);
192 double lengths[4][3],
double distances[3],
double cosines[3]);
194 double M_start[3][3],
double X0,
double Y0,
double Z0,
double X1,
195 double Y1,
double Z1,
double X2,
double Y2,
double Z2,
double R[3][3],