mirror of
https://github.com/opencv/opencv.git
synced 2024-12-15 09:49:13 +08:00
79 lines
2.8 KiB
C++
79 lines
2.8 KiB
C++
// This file is part of OpenCV project.
|
|
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
|
// of this distribution and at http://opencv.org/license.html
|
|
|
|
#ifndef P3P_H
|
|
#define P3P_H
|
|
|
|
#include "precomp.hpp"
|
|
|
|
namespace cv {
|
|
|
|
class p3p
|
|
{
|
|
public:
|
|
p3p(double fx, double fy, double cx, double cy);
|
|
p3p(cv::Mat cameraMatrix);
|
|
|
|
bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
|
|
int solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints);
|
|
int solve(double R[4][3][3], double t[4][3],
|
|
double mu0, double mv0, double X0, double Y0, double Z0,
|
|
double mu1, double mv1, double X1, double Y1, double Z1,
|
|
double mu2, double mv2, double X2, double Y2, double Z2,
|
|
double mu3, double mv3, double X3, double Y3, double Z3,
|
|
bool p4p);
|
|
bool solve(double R[3][3], double t[3],
|
|
double mu0, double mv0, double X0, double Y0, double Z0,
|
|
double mu1, double mv1, double X1, double Y1, double Z1,
|
|
double mu2, double mv2, double X2, double Y2, double Z2,
|
|
double mu3, double mv3, double X3, double Y3, double Z3);
|
|
|
|
private:
|
|
template <typename T>
|
|
void init_camera_parameters(const cv::Mat& cameraMatrix)
|
|
{
|
|
cx = cameraMatrix.at<T> (0, 2);
|
|
cy = cameraMatrix.at<T> (1, 2);
|
|
fx = cameraMatrix.at<T> (0, 0);
|
|
fy = cameraMatrix.at<T> (1, 1);
|
|
}
|
|
template <typename OpointType, typename IpointType>
|
|
void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
|
|
{
|
|
points.clear();
|
|
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
|
|
points.resize(5*4); //resize vector to fit for p4p case
|
|
for(int i = 0; i < npoints; i++)
|
|
{
|
|
points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
|
|
points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;
|
|
points[i*5+2] = opoints.at<OpointType>(i).x;
|
|
points[i*5+3] = opoints.at<OpointType>(i).y;
|
|
points[i*5+4] = opoints.at<OpointType>(i).z;
|
|
}
|
|
//Fill vectors with unused values for p3p case
|
|
for (int i = npoints; i < 4; i++) {
|
|
for (int j = 0; j < 5; j++) {
|
|
points[i * 5 + j] = 0;
|
|
}
|
|
}
|
|
}
|
|
void init_inverse_parameters();
|
|
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]);
|
|
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]);
|
|
|
|
bool jacobi_4x4(double * A, double * D, double * U);
|
|
|
|
double fx, fy, cx, cy;
|
|
double inv_fx, inv_fy, cx_fx, cy_fy;
|
|
};
|
|
|
|
}
|
|
|
|
#endif // P3P_H
|