1 /* 2 * PnPProblem.h 3 * 4 * Created on: Mar 28, 2014 5 * Author: Edgar Riba 6 */ 7 8 #ifndef PNPPROBLEM_H_ 9 #define PNPPROBLEM_H_ 10 11 #include <iostream> 12 13 #include <opencv2/core/core.hpp> 14 #include <opencv2/highgui/highgui.hpp> 15 16 #include "Mesh.h" 17 #include "ModelRegistration.h" 18 19 class PnPProblem 20 { 21 22 public: 23 explicit PnPProblem(const double param[]); // custom constructor 24 virtual ~PnPProblem(); 25 26 bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d); 27 bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out); 28 std::vector<cv::Point2f> verify_points(Mesh *mesh); 29 cv::Point2f backproject3DPoint(const cv::Point3f &point3d); 30 bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags); 31 void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, 32 int flags, cv::Mat &inliers, 33 int iterationsCount, float reprojectionError, double confidence ); 34 get_A_matrix()35 cv::Mat get_A_matrix() const { return _A_matrix; } get_R_matrix()36 cv::Mat get_R_matrix() const { return _R_matrix; } get_t_matrix()37 cv::Mat get_t_matrix() const { return _t_matrix; } get_P_matrix()38 cv::Mat get_P_matrix() const { return _P_matrix; } 39 40 void set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix); 41 42 private: 43 /** The calibration matrix */ 44 cv::Mat _A_matrix; 45 /** The computed rotation matrix */ 46 cv::Mat _R_matrix; 47 /** The computed translation matrix */ 48 cv::Mat _t_matrix; 49 /** The computed projection matrix */ 50 cv::Mat _P_matrix; 51 }; 52 53 // Functions for Möller–Trumbore intersection algorithm 54 cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2); 55 double DOT(cv::Point3f v1, cv::Point3f v2); 56 cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2); 57 58 #endif /* PNPPROBLEM_H_ */ 59