14 #include <Eigen/Dense> 16 #include <mrpt/otherlibs/do_opencv_includes.h> 43 p3p(Eigen::MatrixXd cam_intrinsic);
47 p3p(cv::Mat cameraMatrix);
58 cv::Mat&
R, cv::Mat& tvec,
const cv::Mat& opoints,
59 const cv::Mat& ipoints);
62 Eigen::Ref<Eigen::Matrix3d>
R, Eigen::Ref<Eigen::Vector3d>
t,
63 Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts);
87 double R[4][3][3],
double t[4][3],
double mu0,
double mv0,
double X0,
88 double Y0,
double Z0,
double mu1,
double mv1,
double X1,
double Y1,
89 double Z1,
double mu2,
double mv2,
double X2,
double Y2,
double Z2);
118 double R[3][3],
double t[3],
double mu0,
double mv0,
double X0,
119 double Y0,
double Z0,
double mu1,
double mv1,
double X1,
double Y1,
120 double Z1,
double mu2,
double mv2,
double X2,
double Y2,
double Z2,
121 double mu3,
double mv3,
double X3,
double Y3,
double Z3);
129 template <
typename T>
130 void init_camera_parameters(
const cv::Mat& cameraMatrix)
132 cx = cameraMatrix.at<T>(0, 2);
133 cy = cameraMatrix.at<T>(1, 2);
134 fx = cameraMatrix.at<T>(0, 0);
135 fy = cameraMatrix.at<T>(1, 1);
144 template <
typename Opo
intType,
typename Ipo
intType>
146 const cv::Mat& opoints,
const cv::Mat& ipoints,
147 std::vector<double>&
points)
151 for (
int i = 0; i < 4; i++)
153 points[i * 5] = ipoints.at<IpointType>(i).
x *
fx +
cx;
154 points[i * 5 + 1] = ipoints.at<IpointType>(i).
y *
fy +
cy;
155 points[i * 5 + 2] = opoints.at<OpointType>(i).
x;
156 points[i * 5 + 3] = opoints.at<OpointType>(i).
y;
157 points[i * 5 + 4] = opoints.at<OpointType>(i).
z;
169 Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts,
170 std::vector<double>&
points)
174 for (
int i = 0; i < 4; i++)
178 points[i * 5 + 2] = obj_pts(i, 0);
179 points[i * 5 + 3] = obj_pts(i, 1);
180 points[i * 5 + 4] = obj_pts(i, 2);
196 double lengths[4][3],
double distances[3],
double cosines[3]);
198 double M_start[3][3],
double X0,
double Y0,
double Z0,
double X1,
199 double Y1,
double Z1,
double X2,
double Y2,
double Z2,
double R[3][3],
208 bool jacobi_4x4(
double* A,
double* D,
double* U);
void extract_points(Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts, std::vector< double > &points)
Eigen wrapper for extracting object and image points.
bool jacobi_4x4(double *A, double *D, double *U)
Function used to compute the SVD.
GLsizei const GLfloat * points
double inv_fy
Inverse of focal length x.
bool align(double M_start[3][3], double X0, double Y0, double Z0, double X1, double Y1, double Z1, double X2, double Y2, double Z2, double R[3][3], double T[3])
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
Helper function to solve()
double cy_fy
Inverse of image center point x.
void init_inverse_parameters()
Function to compute inverse parameters of camera intrinsic matrix.
bool solve(Eigen::Ref< Eigen::Matrix3d > R, Eigen::Ref< Eigen::Vector3d > t, Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts)
p3p(double fx, double fy, double cx, double cy)
Constructor for p3p class using C.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double cx_fx
Inverse of focal length y.
double inv_fx
Image center y.