mrpt::vision::pnp::epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints)
{
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
pws.resize(3 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
alphas.resize(4 * number_of_correspondences);
pcs.resize(3 * number_of_correspondences);
max_nr = 0;
A1 = NULL;
A2 = NULL;
}
int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
{
CV_INSTRUMENT_REGION();
double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19],
p4p);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}