Merge pull request #13835 from catree:real_time_pose_tutorial_keypoints_matching

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp

 #include "CsvReader.h"
 
 /** The default constructor of the CSV reader Class */
-CsvReader::CsvReader(const string &path, const char &separator){
+CsvReader::CsvReader(const string &path, char separator){
     _file.open(path.c_str(), ifstream::in);
     _separator = separator;
 }

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h

@@ -19,7 +19,7 @@ public:
     * @param separator - The separator character between words per line
     * @return
     */
-  CsvReader(const string &path, const char &separator = ' ');
+    CsvReader(const string &path, char separator = ' ');
 
     /**
     * Read a plane text file with .ply format

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp

@@ -13,34 +13,31 @@ CsvWriter::~CsvWriter() {
 
 void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d)
 {
-  string x, y, z;
-  for(unsigned int i = 0; i < list_points3d.size(); ++i)
+    for(size_t i = 0; i < list_points3d.size(); ++i)
     {
-    x = FloatToString(list_points3d[i].x);
-    y = FloatToString(list_points3d[i].y);
-    z = FloatToString(list_points3d[i].z);
+        string x = FloatToString(list_points3d[i].x);
+        string y = FloatToString(list_points3d[i].y);
+        string z = FloatToString(list_points3d[i].z);
 
         _file << x << _separator << y << _separator << z << std::endl;
     }
-
 }
 
 void CsvWriter::writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors)
 {
-  string u, v, x, y, z, descriptor_str;
-  for(unsigned int i = 0; i < list_points3d.size(); ++i)
+    for(size_t i = 0; i < list_points3d.size(); ++i)
     {
-    u = FloatToString(list_points2d[i].x);
-    v = FloatToString(list_points2d[i].y);
-    x = FloatToString(list_points3d[i].x);
-    y = FloatToString(list_points3d[i].y);
-    z = FloatToString(list_points3d[i].z);
+        string u = FloatToString(list_points2d[i].x);
+        string v = FloatToString(list_points2d[i].y);
+        string x = FloatToString(list_points3d[i].x);
+        string y = FloatToString(list_points3d[i].y);
+        string z = FloatToString(list_points3d[i].z);
 
         _file << u << _separator << v << _separator << x << _separator << y << _separator << z;
 
         for(int j = 0; j < 32; ++j)
         {
-      descriptor_str = FloatToString(descriptors.at<float>(i,j));
+            string descriptor_str = FloatToString(descriptors.at<float>((int)i,j));
             _file << _separator << descriptor_str;
         }
         _file << std::endl;

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp

@@ -14,9 +14,9 @@
 // --------------------------------------------------- //
 
 /**  The custom constructor of the Triangle Class */
-Triangle::Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2)
+Triangle::Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2) :
+    v0_(V0), v1_(V1), v2_(V2)
 {
-  id_ = id; v0_ = V0; v1_ = V1; v2_ = V2;
 }
 
 /**  The default destructor of the Class */
@@ -31,8 +31,9 @@ Triangle::~Triangle()
 // --------------------------------------------------- //
 
 /**  The custom constructor of the Ray Class */
-Ray::Ray(cv::Point3f P0, cv::Point3f P1) {
-  p0_ = P0; p1_ = P1;
+Ray::Ray(const cv::Point3f& P0, const cv::Point3f& P1) :
+    p0_(P0), p1_(P1)
+{
 }
 
 /**  The default destructor of the Class */
@@ -47,11 +48,9 @@ Ray::~Ray()
 // --------------------------------------------------- //
 
 /** The default constructor of the ObjectMesh Class */
-Mesh::Mesh() : list_vertex_(0) , list_triangles_(0)
+Mesh::Mesh() : num_vertices_(0), num_triangles_(0),
+    list_vertex_(0) , list_triangles_(0)
 {
-  id_ = 0;
-  num_vertexs_ = 0;
-  num_triangles_ = 0;
 }
 
 /** The default destructor of the ObjectMesh Class */
@@ -60,11 +59,9 @@ Mesh::~Mesh()
     // TODO Auto-generated destructor stub
 }
 
-
 /** Load a CSV with *.ply format **/
-void Mesh::load(const std::string path)
+void Mesh::load(const std::string& path)
 {
-
     // Create the reader
     CsvReader csvReader(path);
 
@@ -76,7 +73,6 @@ void Mesh::load(const std::string path)
     csvReader.readPLY(list_vertex_, list_triangles_);
 
     // Update mesh attributes
-  num_vertexs_ = (int)list_vertex_.size();
+    num_vertices_ = (int)list_vertex_.size();
     num_triangles_ = (int)list_triangles_.size();
-
 }

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h

@@ -19,7 +19,7 @@
 class Triangle {
 public:
 
-  explicit Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2);
+    explicit Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2);
     virtual ~Triangle();
 
     cv::Point3f getV0() const { return v0_; }
@@ -27,8 +27,6 @@ public:
     cv::Point3f getV2() const { return v2_; }
 
 private:
-  /** The identifier number of the triangle */
-  int id_;
     /** The three vertices that defines the triangle */
     cv::Point3f v0_, v1_, v2_;
 };
@@ -41,7 +39,7 @@ private:
 class Ray {
 public:
 
-  explicit Ray(cv::Point3f P0, cv::Point3f P1);
+    explicit Ray(const cv::Point3f& P0, const cv::Point3f& P1);
     virtual ~Ray();
 
     cv::Point3f getP0() { return p0_; }
@@ -66,15 +64,13 @@ public:
 
     std::vector<std::vector<int> > getTrianglesList() const { return list_triangles_; }
     cv::Point3f getVertex(int pos) const { return list_vertex_[pos]; }
-  int getNumVertices() const { return num_vertexs_; }
+    int getNumVertices() const { return num_vertices_; }
 
-  void load(const std::string path_file);
+    void load(const std::string& path_file);
 
 private:
-  /** The identification number of the mesh */
-  int id_;
     /** The current number of vertices in the mesh */
-  int num_vertexs_;
+    int num_vertices_;
     /** The current number of triangles in the mesh */
     int num_triangles_;
     /* The list of triangles of the mesh */

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp

@@ -8,9 +8,8 @@
 #include "Model.h"
 #include "CsvWriter.h"
 
-Model::Model() : list_points2d_in_(0), list_points2d_out_(0), list_points3d_in_(0)
+Model::Model() : n_correspondences_(0), list_points2d_in_(0), list_points2d_out_(0), list_points3d_in_(0), training_img_path_()
 {
-  n_correspondences_ = 0;
 }
 
 Model::~Model()
@@ -40,34 +39,45 @@ void Model::add_keypoint(const cv::KeyPoint &kp)
     list_keypoints_.push_back(kp);
 }
 
+void Model::set_trainingImagePath(const std::string &path)
+{
+    training_img_path_ = path;
+}
 
-/** Save a CSV file and fill the object mesh */
-void Model::save(const std::string path)
+/** Save a YAML file and fill the object mesh */
+void Model::save(const std::string &path)
 {
     cv::Mat points3dmatrix = cv::Mat(list_points3d_in_);
     cv::Mat points2dmatrix = cv::Mat(list_points2d_in_);
-  //cv::Mat keyPointmatrix = cv::Mat(list_keypoints_);
 
     cv::FileStorage storage(path, cv::FileStorage::WRITE);
     storage << "points_3d" << points3dmatrix;
     storage << "points_2d" << points2dmatrix;
     storage << "keypoints" << list_keypoints_;
     storage << "descriptors" << descriptors_;
+    storage << "training_image_path" << training_img_path_;
 
     storage.release();
 }
 
 /** Load a YAML file using OpenCv functions **/
-void Model::load(const std::string path)
+void Model::load(const std::string &path)
 {
     cv::Mat points3d_mat;
 
     cv::FileStorage storage(path, cv::FileStorage::READ);
     storage["points_3d"] >> points3d_mat;
     storage["descriptors"] >> descriptors_;
+    if (!storage["keypoints"].empty())
+    {
+        storage["keypoints"] >> list_keypoints_;
+    }
+    if (!storage["training_image_path"].empty())
+    {
+        storage["training_image_path"] >> training_img_path_;
+    }
 
     points3d_mat.copyTo(list_points3d_in_);
 
     storage.release();
-
 }

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h

@@ -24,17 +24,16 @@ public:
     std::vector<cv::KeyPoint> get_keypoints() const { return list_keypoints_; }
     cv::Mat get_descriptors() const { return descriptors_; }
     int get_numDescriptors() const { return descriptors_.rows; }
-
+    std::string get_trainingImagePath() const { return training_img_path_; }
 
     void add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d);
     void add_outlier(const cv::Point2f &point2d);
     void add_descriptor(const cv::Mat &descriptor);
     void add_keypoint(const cv::KeyPoint &kp);
+    void set_trainingImagePath(const std::string &path);
 
-
-  void save(const std::string path);
-  void load(const std::string path);
-
+    void save(const std::string &path);
+    void load(const std::string &path);
 
 private:
     /** The current number of correspondecnes */
@@ -49,6 +48,8 @@ private:
     std::vector<cv::Point3f> list_points3d_in_;
     /** The list of 2D points descriptors */
     cv::Mat descriptors_;
+    /** Path to the training image */
+    std::string training_img_path_;
 };
 
 #endif /* OBJECTMODEL_H_ */

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.cpp

@@ -7,10 +7,9 @@
 
 #include "ModelRegistration.h"
 
-ModelRegistration::ModelRegistration()
+ModelRegistration::ModelRegistration() : n_registrations_(0), max_registrations_(0),
+    list_points2d_(), list_points3d_()
 {
-  n_registrations_ = 0;
-  max_registrations_ = 0;
 }
 
 ModelRegistration::~ModelRegistration()
@@ -19,12 +18,12 @@ ModelRegistration::~ModelRegistration()
 }
 
 void ModelRegistration::registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d)
- {
+{
     // add correspondence at the end of the vector
     list_points2d_.push_back(point2d);
     list_points3d_.push_back(point3d);
     n_registrations_++;
- }
+}
 
 void ModelRegistration::reset()
 {

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h

@@ -14,7 +14,6 @@
 class ModelRegistration
 {
 public:
-
     ModelRegistration();
     virtual ~ModelRegistration();
 
@@ -30,14 +29,14 @@ public:
     void reset();
 
 private:
-/** The current number of registered points */
-int n_registrations_;
-/** The total number of points to register */
-int max_registrations_;
-/** The list of 2D points to register the model */
-std::vector<cv::Point2f> list_points2d_;
-/** The list of 3D points to register the model */
-std::vector<cv::Point3f> list_points3d_;
+    /** The current number of registered points */
+    int n_registrations_;
+    /** The total number of points to register */
+    int max_registrations_;
+    /** The list of 2D points to register the model */
+    std::vector<cv::Point2f> list_points2d_;
+    /** The list of 3D points to register the model */
+    std::vector<cv::Point3f> list_points3d_;
 };
 
 #endif /* MODELREGISTRATION_H_ */

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp

@@ -13,16 +13,8 @@
 
 #include <opencv2/calib3d/calib3d.hpp>
 
-/* Functions headers */
-cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
-double DOT(cv::Point3f v1, cv::Point3f v2);
-cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
-cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin);
-
-
 /* Functions for Möller-Trumbore intersection algorithm */
-
-cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
+static cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
 {
     cv::Point3f tmp_p;
     tmp_p.x =  v1.y*v2.z - v1.z*v2.y;
@@ -31,12 +23,12 @@ cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
     return tmp_p;
 }
 
-double DOT(cv::Point3f v1, cv::Point3f v2)
+static double DOT(cv::Point3f v1, cv::Point3f v2)
 {
     return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
 }
 
-cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
+static cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
 {
     cv::Point3f tmp_p;
     tmp_p.x =  v1.x - v2.x;
@@ -45,11 +37,10 @@ cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
     return tmp_p;
 }
 
-/* End functions for Möller-Trumbore intersection algorithm
- *  */
+/* End functions for Möller-Trumbore intersection algorithm */
 
 // Function to get the nearest 3D point to the Ray origin
-cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin)
+static cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin)
 {
     cv::Point3f p1 = points_list[0];
     cv::Point3f p2 = points_list[1];
@@ -71,15 +62,15 @@ cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Poin
 
 PnPProblem::PnPProblem(const double params[])
 {
-  _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1);   // intrinsic camera parameters
-  _A_matrix.at<double>(0, 0) = params[0];       //      [ fx   0  cx ]
-  _A_matrix.at<double>(1, 1) = params[1];       //      [  0  fy  cy ]
-  _A_matrix.at<double>(0, 2) = params[2];       //      [  0   0   1 ]
-  _A_matrix.at<double>(1, 2) = params[3];
-  _A_matrix.at<double>(2, 2) = 1;
-  _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1);   // rotation matrix
-  _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1);   // translation matrix
-  _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1);   // rotation-translation matrix
+    A_matrix_ = cv::Mat::zeros(3, 3, CV_64FC1);   // intrinsic camera parameters
+    A_matrix_.at<double>(0, 0) = params[0];       //      [ fx   0  cx ]
+    A_matrix_.at<double>(1, 1) = params[1];       //      [  0  fy  cy ]
+    A_matrix_.at<double>(0, 2) = params[2];       //      [  0   0   1 ]
+    A_matrix_.at<double>(1, 2) = params[3];
+    A_matrix_.at<double>(2, 2) = 1;
+    R_matrix_ = cv::Mat::zeros(3, 3, CV_64FC1);   // rotation matrix
+    t_matrix_ = cv::Mat::zeros(3, 1, CV_64FC1);   // translation matrix
+    P_matrix_ = cv::Mat::zeros(3, 4, CV_64FC1);   // rotation-translation matrix
 
 }
 
@@ -91,21 +82,20 @@ PnPProblem::~PnPProblem()
 void PnPProblem::set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix)
 {
     // Rotation-Translation Matrix Definition
-  _P_matrix.at<double>(0,0) = R_matrix.at<double>(0,0);
-  _P_matrix.at<double>(0,1) = R_matrix.at<double>(0,1);
-  _P_matrix.at<double>(0,2) = R_matrix.at<double>(0,2);
-  _P_matrix.at<double>(1,0) = R_matrix.at<double>(1,0);
-  _P_matrix.at<double>(1,1) = R_matrix.at<double>(1,1);
-  _P_matrix.at<double>(1,2) = R_matrix.at<double>(1,2);
-  _P_matrix.at<double>(2,0) = R_matrix.at<double>(2,0);
-  _P_matrix.at<double>(2,1) = R_matrix.at<double>(2,1);
-  _P_matrix.at<double>(2,2) = R_matrix.at<double>(2,2);
-  _P_matrix.at<double>(0,3) = t_matrix.at<double>(0);
-  _P_matrix.at<double>(1,3) = t_matrix.at<double>(1);
-  _P_matrix.at<double>(2,3) = t_matrix.at<double>(2);
+    P_matrix_.at<double>(0,0) = R_matrix.at<double>(0,0);
+    P_matrix_.at<double>(0,1) = R_matrix.at<double>(0,1);
+    P_matrix_.at<double>(0,2) = R_matrix.at<double>(0,2);
+    P_matrix_.at<double>(1,0) = R_matrix.at<double>(1,0);
+    P_matrix_.at<double>(1,1) = R_matrix.at<double>(1,1);
+    P_matrix_.at<double>(1,2) = R_matrix.at<double>(1,2);
+    P_matrix_.at<double>(2,0) = R_matrix.at<double>(2,0);
+    P_matrix_.at<double>(2,1) = R_matrix.at<double>(2,1);
+    P_matrix_.at<double>(2,2) = R_matrix.at<double>(2,2);
+    P_matrix_.at<double>(0,3) = t_matrix.at<double>(0);
+    P_matrix_.at<double>(1,3) = t_matrix.at<double>(1);
+    P_matrix_.at<double>(2,3) = t_matrix.at<double>(2);
 }
 
-
 // Estimate the pose given a list of 2D/3D correspondences and the method to use
 bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
                                const std::vector<cv::Point2f> &list_points2d,
@@ -118,15 +108,15 @@ bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
     bool useExtrinsicGuess = false;
 
     // Pose estimation
-  bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
+    bool correspondence = cv::solvePnP( list_points3d, list_points2d, A_matrix_, distCoeffs, rvec, tvec,
                                         useExtrinsicGuess, flags);
 
     // Transforms Rotation Vector to Matrix
-  Rodrigues(rvec,_R_matrix);
-  _t_matrix = tvec;
+    Rodrigues(rvec, R_matrix_);
+    t_matrix_ = tvec;
 
     // Set projection matrix
-  this->set_P_matrix(_R_matrix, _t_matrix);
+    this->set_P_matrix(R_matrix_, t_matrix_);
 
     return correspondence;
 }
@@ -145,14 +135,14 @@ void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points
     bool useExtrinsicGuess = false;   // if true the function uses the provided rvec and tvec values as
     // initial approximations of the rotation and translation vectors
 
-  cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
+    cv::solvePnPRansac( list_points3d, list_points2d, A_matrix_, distCoeffs, rvec, tvec,
                         useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
                         inliers, flags );
 
-  Rodrigues(rvec,_R_matrix);      // converts Rotation Vector to Matrix
-  _t_matrix = tvec;       // set translation matrix
+    Rodrigues(rvec, R_matrix_); // converts Rotation Vector to Matrix
+    t_matrix_ = tvec;           // set translation matrix
 
-  this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix
+    this->set_P_matrix(R_matrix_, t_matrix_); // set rotation-translation matrix
 
 }
 
@@ -170,9 +160,7 @@ std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
     return verified_points_2d;
 }
 
-
 // Backproject a 3D point to 2D using the estimated pose parameters
-
 cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
 {
     // 3D point vector [x y z 1]'
@@ -184,7 +172,7 @@ cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
 
     // 2D point vector [u v 1]'
     cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
-  point2d_vec = _A_matrix * _P_matrix * point3d_vec;
+    point2d_vec = A_matrix_ * P_matrix_ * point3d_vec;
 
     // Normalization of [u v]'
     cv::Point2f point2d;
@@ -211,13 +199,13 @@ bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d
     point2d_vec.at<double>(2) = lambda;
 
     // Point in camera coordinates
-  cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1
+    cv::Mat X_c = A_matrix_.inv() * point2d_vec ; // 3x1
 
     // Point in world coordinates
-  cv::Mat X_w = _R_matrix.inv() * ( X_c - _t_matrix ); // 3x1
+    cv::Mat X_w = R_matrix_.inv() * ( X_c - t_matrix_ ); // 3x1
 
     // Center of projection
-  cv::Mat C_op = cv::Mat(_R_matrix.inv()).mul(-1) * _t_matrix; // 3x1
+    cv::Mat C_op = cv::Mat(R_matrix_.inv()).mul(-1) * t_matrix_; // 3x1
 
     // Ray direction vector
     cv::Mat ray = X_w - C_op; // 3x1
@@ -236,7 +224,7 @@ bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d
         cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
         cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
 
-    Triangle T(i, V0, V1, V2);
+        Triangle T(V0, V1, V2);
 
         double out;
         if(this->intersect_MollerTrumbore(R, T, &out))

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h

@@ -18,7 +18,6 @@
 
 class PnPProblem
 {
-
 public:
     explicit PnPProblem(const double param[]);  // custom constructor
     virtual ~PnPProblem();
@@ -32,27 +31,22 @@ public:
                              int flags, cv::Mat &inliers,
                              int iterationsCount, float reprojectionError, double confidence );
 
-  cv::Mat get_A_matrix() const { return _A_matrix; }
-  cv::Mat get_R_matrix() const { return _R_matrix; }
-  cv::Mat get_t_matrix() const { return _t_matrix; }
-  cv::Mat get_P_matrix() const { return _P_matrix; }
+    cv::Mat get_A_matrix() const { return A_matrix_; }
+    cv::Mat get_R_matrix() const { return R_matrix_; }
+    cv::Mat get_t_matrix() const { return t_matrix_; }
+    cv::Mat get_P_matrix() const { return P_matrix_; }
 
     void set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix);
 
 private:
     /** The calibration matrix */
-  cv::Mat _A_matrix;
+    cv::Mat A_matrix_;
     /** The computed rotation matrix */
-  cv::Mat _R_matrix;
+    cv::Mat R_matrix_;
     /** The computed translation matrix */
-  cv::Mat _t_matrix;
+    cv::Mat t_matrix_;
     /** The computed projection matrix */
-  cv::Mat _P_matrix;
+    cv::Mat P_matrix_;
 };
 
-// Functions for Möller-Trumbore intersection algorithm
-cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
-double DOT(cv::Point3f v1, cv::Point3f v2);
-cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
-
 #endif /* PNPPROBLEM_H_ */

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp

@@ -55,12 +55,10 @@ void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& m
                                   const std::vector<std::vector<cv::DMatch> >& matches2,
                                   std::vector<cv::DMatch>& symMatches )
 {
-
     // for all matches image 1 -> image 2
     for (std::vector<std::vector<cv::DMatch> >::const_iterator
          matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1)
     {
-
         // ignore deleted matches
         if (matchIterator1->empty() || matchIterator1->size() < 2)
             continue;
@@ -74,27 +72,22 @@ void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& m
                 continue;
 
             // Match symmetry test
-        if ((*matchIterator1)[0].queryIdx ==
-            (*matchIterator2)[0].trainIdx &&
-            (*matchIterator2)[0].queryIdx ==
-            (*matchIterator1)[0].trainIdx)
+            if ((*matchIterator1)[0].queryIdx == (*matchIterator2)[0].trainIdx &&
+                (*matchIterator2)[0].queryIdx == (*matchIterator1)[0].trainIdx)
             {
                 // add symmetrical match
-            symMatches.push_back(
-              cv::DMatch((*matchIterator1)[0].queryIdx,
-                         (*matchIterator1)[0].trainIdx,
-                         (*matchIterator1)[0].distance));
+                symMatches.push_back(cv::DMatch((*matchIterator1)[0].queryIdx,
+                                     (*matchIterator1)[0].trainIdx, (*matchIterator1)[0].distance));
                 break; // next match in image 1 -> image 2
             }
         }
     }
-
 }
 
 void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
-              std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model )
+                                 std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model,
+                                 const std::vector<cv::KeyPoint>& keypoints_model)
 {
-
     // 1a. Detection of the ORB features
     this->computeKeyPoints(frame, keypoints_frame);
 
@@ -120,11 +113,16 @@ void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>&
     // 4. Remove non-symmetrical matches
     symmetryTest(matches12, matches21, good_matches);
 
+    if (!training_img_.empty() && !keypoints_model.empty())
+    {
+        cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
+    }
 }
 
 void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
                                      std::vector<cv::KeyPoint>& keypoints_frame,
-                                 const cv::Mat& descriptors_model )
+                                     const cv::Mat& descriptors_model,
+                                     const std::vector<cv::KeyPoint>& keypoints_model)
 {
     good_matches.clear();
 
@@ -149,4 +147,8 @@ void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatc
         if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]);
     }
 
+    if (!training_img_.empty() && !keypoints_model.empty())
+    {
+        cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
+    }
 }

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h

@@ -16,7 +16,8 @@
 
 class RobustMatcher {
 public:
-  RobustMatcher() : ratio_(0.8f)
+    RobustMatcher() : detector_(), extractor_(), matcher_(),
+        ratio_(0.8f), training_img_(), img_matching_()
     {
         // ORB is the default feature
         detector_ = cv::ORB::create();
@@ -43,9 +44,13 @@ public:
     // Compute the descriptors of an image given its keypoints
     void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
 
+    cv::Mat getImageMatching() const { return img_matching_; }
+
     // Set ratio parameter for the ratio test
     void setRatio( float rat) { ratio_ = rat; }
 
+    void setTrainingImage(const cv::Mat &img) { training_img_ = img; }
+
     // Clear matches for which NN ratio is > than threshold
     // return the number of removed points
     // (corresponding entries being cleared,
@@ -60,12 +65,14 @@ public:
     // Match feature points using ratio and symmetry test
     void robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
                       std::vector<cv::KeyPoint>& keypoints_frame,
-                      const cv::Mat& descriptors_model );
+                      const cv::Mat& descriptors_model,
+                      const std::vector<cv::KeyPoint>& keypoints_model);
 
     // Match feature points using ratio test
     void fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
                           std::vector<cv::KeyPoint>& keypoints_frame,
-                       const cv::Mat& descriptors_model );
+                          const cv::Mat& descriptors_model,
+                          const std::vector<cv::KeyPoint>& keypoints_model);
 
 private:
     // pointer to the feature point detector object
@@ -76,6 +83,10 @@ private:
     cv::Ptr<cv::DescriptorMatcher> matcher_;
     // max ratio between 1st and 2nd NN
     float ratio_;
+    // training image
+    cv::Mat training_img_;
+    // matching image
+    cv::Mat img_matching_;
 };
 
 #endif /* ROBUSTMATCHER_H_ */

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp

@@ -11,18 +11,22 @@
 #include "ModelRegistration.h"
 #include "Utils.h"
 
+#include <opencv2/opencv_modules.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/calib3d.hpp>
+#include <opencv2/flann.hpp>
+#if defined (HAVE_OPENCV_XFEATURES2D)
+#include <opencv2/xfeatures2d.hpp>
+#endif
 
 // For text
-int fontFace = cv::FONT_ITALIC;
-double fontScale = 0.75;
-int thickness_font = 2;
+const int fontFace = cv::FONT_ITALIC;
+const double fontScale = 0.75;
+const int thickness_font = 2;
 
 // For circles
-int lineType = 8;
-int radius = 4;
-double thickness_circ = -1;
+const int lineType = 8;
+const int radius = 4;
 
 // Draw a text with the question point
 void drawQuestion(cv::Mat image, cv::Point3f point, cv::Scalar color)
@@ -50,9 +54,8 @@ void drawText2(cv::Mat image, std::string text, cv::Scalar color)
 // Draw a text with the frame ratio
 void drawFPS(cv::Mat image, double fps, cv::Scalar color)
 {
-  std::string fps_str = IntToString((int)fps);
-  std::string text = fps_str + " FPS";
-  cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
+    std::string fps_str = cv::format("%.2f FPS", fps);
+    cv::putText(image, fps_str, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
 }
 
 // Draw a text with the frame ratio
@@ -141,10 +144,9 @@ void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_po
     cv::Point2i pointZ = list_points2d[3];
 
     drawArrow(image, origin, pointX, red, 9, 2);
-  drawArrow(image, origin, pointY, blue, 9, 2);
-  drawArrow(image, origin, pointZ, green, 9, 2);
+    drawArrow(image, origin, pointY, green, 9, 2);
+    drawArrow(image, origin, pointZ, blue, 9, 2);
     cv::circle(image, origin, radius/2, black, -1, lineType );
-
 }
 
 // Draw the object mesh
@@ -296,3 +298,104 @@ std::string IntToString ( int Number )
     ss << Number;
     return ss.str();
 }
+
+void createFeatures(const std::string &featureName, int numKeypoints, cv::Ptr<cv::Feature2D> &detector, cv::Ptr<cv::Feature2D> &descriptor)
+{
+    if (featureName == "ORB")
+    {
+        detector = cv::ORB::create(numKeypoints);
+        descriptor = cv::ORB::create(numKeypoints);
+    }
+    else if (featureName == "KAZE")
+    {
+        detector = cv::KAZE::create();
+        descriptor = cv::KAZE::create();
+    }
+    else if (featureName == "AKAZE")
+    {
+        detector = cv::AKAZE::create();
+        descriptor = cv::AKAZE::create();
+    }
+    else if (featureName == "BRISK")
+    {
+        detector = cv::BRISK::create();
+        descriptor = cv::BRISK::create();
+    }
+    else if (featureName == "SIFT")
+    {
+#if defined (OPENCV_ENABLE_NONFREE) && defined (HAVE_OPENCV_XFEATURES2D)
+        detector = cv::xfeatures2d::SIFT::create();
+        descriptor = cv::xfeatures2d::SIFT::create();
+#else
+        std::cout << "xfeatures2d module is not available or nonfree is not enabled." << std::endl;
+        std::cout << "Default to ORB." << std::endl;
+        detector = cv::ORB::create(numKeypoints);
+        descriptor = cv::ORB::create(numKeypoints);
+#endif
+    }
+    else if (featureName == "SURF")
+    {
+#if defined (OPENCV_ENABLE_NONFREE) && defined (HAVE_OPENCV_XFEATURES2D)
+        detector = cv::xfeatures2d::SURF::create(100, 4, 3, true);   //extended=true
+        descriptor = cv::xfeatures2d::SURF::create(100, 4, 3, true); //extended=true
+#else
+        std::cout << "xfeatures2d module is not available or nonfree is not enabled." << std::endl;
+        std::cout << "Default to ORB." << std::endl;
+        detector = cv::ORB::create(numKeypoints);
+        descriptor = cv::ORB::create(numKeypoints);
+#endif
+    }
+    else if (featureName == "BINBOOST")
+    {
+#if defined (HAVE_OPENCV_XFEATURES2D)
+        detector = cv::KAZE::create();
+        descriptor = cv::xfeatures2d::BoostDesc::create();
+#else
+        std::cout << "xfeatures2d module is not available." << std::endl;
+        std::cout << "Default to ORB." << std::endl;
+        detector = cv::ORB::create(numKeypoints);
+        descriptor = cv::ORB::create(numKeypoints);
+#endif
+    }
+    else if (featureName == "VGG")
+    {
+#if defined (HAVE_OPENCV_XFEATURES2D)
+        detector = cv::KAZE::create();
+        descriptor = cv::xfeatures2d::VGG::create();
+#else
+        std::cout << "xfeatures2d module is not available." << std::endl;
+        std::cout << "Default to ORB." << std::endl;
+        detector = cv::ORB::create(numKeypoints);
+        descriptor = cv::ORB::create(numKeypoints);
+#endif
+    }
+}
+
+cv::Ptr<cv::DescriptorMatcher> createMatcher(const std::string &featureName, bool useFLANN)
+{
+    if (featureName == "ORB" || featureName == "BRISK" || featureName == "AKAZE" || featureName == "BINBOOST")
+    {
+        if (useFLANN)
+        {
+            cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters
+            cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>(50);       // instantiate flann search parameters
+            return cv::makePtr<cv::FlannBasedMatcher>(indexParams, searchParams);
+        }
+        else
+        {
+            return cv::DescriptorMatcher::create("BruteForce-Hamming");
+        }
+
+    }
+    else
+    {
+        if (useFLANN)
+        {
+            return cv::DescriptorMatcher::create("FlannBased");
+        }
+        else
+        {
+            return cv::DescriptorMatcher::create("BruteForce");
+        }
+    }
+}

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h

@@ -10,6 +10,7 @@
 
 #include <iostream>
 
+#include <opencv2/features2d.hpp>
 #include "PnPProblem.h"
 
 // Draw a text with the question point
@@ -66,4 +67,8 @@ std::string FloatToString ( float Number );
 // Converts a given integer to a string
 std::string IntToString ( int Number );
 
+void createFeatures(const std::string &featureName, int numKeypoints, cv::Ptr<cv::Feature2D> &detector, cv::Ptr<cv::Feature2D> &descriptor);
+
+cv::Ptr<cv::DescriptorMatcher> createMatcher(const std::string &featureName, bool useFLANN);
+
 #endif /* UTILS_H_ */

samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp

@@ -18,34 +18,22 @@ using namespace std;
 
 /**  GLOBAL VARIABLES  **/
 
-string tutorial_path = "../../samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/"; // path to tutorial
-
-string img_path = tutorial_path + "Data/resized_IMG_3875.JPG";  // image to register
-string ply_read_path = tutorial_path + "Data/box.ply";          // object mesh
-string write_path = tutorial_path + "Data/cookies_ORB.yml";     // output file
-
 // Boolean the know if the registration it's done
 bool end_registration = false;
 
 // Intrinsic camera parameters: UVC WEBCAM
-double f = 45; // focal length in mm
-double sx = 22.3, sy = 14.9;
-double width = 2592, height = 1944;
-double params_CANON[] = { width*f/sx,   // fx
+const double f = 45; // focal length in mm
+const double sx = 22.3, sy = 14.9;
+const double width = 2592, height = 1944;
+const double params_CANON[] = { width*f/sx,   // fx
                                 height*f/sy,  // fy
                                 width/2,      // cx
                                 height/2};    // cy
 
 // Setup the points to register in the image
 // In the order of the *.ply file and starting at 1
-int n = 8;
-int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
-
-// Some basic colors
-Scalar red(0, 0, 255);
-Scalar green(0,255,0);
-Scalar blue(255,0,0);
-Scalar yellow(0,255,255);
+const int n = 8;
+const int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
 
 /*
  * CREATE MODEL REGISTRATION OBJECT
@@ -58,13 +46,25 @@ Model model;
 Mesh mesh;
 PnPProblem pnp_registration(params_CANON);
 
-/**  Functions headers  **/
-void help();
+/**********************************************************************************************************/
+static void help()
+{
+    cout
+            << "--------------------------------------------------------------------------"   << endl
+            << "This program shows how to create your 3D textured model. "                    << endl
+            << "Usage:"                                                                       << endl
+            << "./cpp-tutorial-pnp_registration"                                              << endl
+            << "--------------------------------------------------------------------------"   << endl
+            << endl;
+}
 
 // Mouse events for model registration
 static void onMouseModelRegistration( int event, int x, int y, int, void* )
 {
     if  ( event == EVENT_LBUTTONUP )
+    {
+        bool is_registrable = registration.is_registrable();
+        if (is_registrable)
         {
             int n_regist = registration.getNumRegist();
             int n_vertex = pts[n_regist];
@@ -72,9 +72,6 @@ static void onMouseModelRegistration( int event, int x, int y, int, void* )
             Point2f point_2d = Point2f((float)x,(float)y);
             Point3f point_3d = mesh.getVertex(n_vertex-1);
 
-      bool is_registrable = registration.is_registrable();
-      if (is_registrable)
-      {
             registration.registerPoint(point_2d, point_3d);
             if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
         }
@@ -82,21 +79,56 @@ static void onMouseModelRegistration( int event, int x, int y, int, void* )
 }
 
 /**  Main program  **/
-int main()
+int main(int argc, char *argv[])
 {
-
     help();
 
+    const String keys =
+            "{help h        |      | print this message                                                 }"
+            "{image i       |      | path to input image                                                }"
+            "{model         |      | path to output yml model                                           }"
+            "{mesh          |      | path to ply mesh                                                   }"
+            "{keypoints k   |2000  | number of keypoints to detect (only for ORB)                       }"
+            "{feature       |ORB   | feature name (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)  }"
+            ;
+    CommandLineParser parser(argc, argv, keys);
+
+    string img_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/resized_IMG_3875.JPG");  // image to register
+    string ply_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply");          // object mesh
+    string write_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml");     // output file
+    int numKeyPoints = 2000;
+    string featureName = "ORB";
+
+    if (parser.has("help"))
+    {
+        parser.printMessage();
+        return 0;
+    }
+    else
+    {
+        img_path = parser.get<string>("image").size() > 0 ? parser.get<string>("image") : img_path;
+        ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
+        write_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : write_path;
+        numKeyPoints = parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
+        featureName = parser.has("feature") ? parser.get<string>("feature") : featureName;
+    }
+
+    std::cout << "Input image: " << img_path << std::endl;
+    std::cout << "CAD model: " << ply_read_path << std::endl;
+    std::cout << "Output training file: " << write_path << std::endl;
+    std::cout << "Feature: " << featureName << std::endl;
+    std::cout << "Number of keypoints for ORB: " << numKeyPoints << std::endl;
+
     // load a mesh given the *.ply file path
     mesh.load(ply_read_path);
 
-  // set parameters
-  int numKeyPoints = 10000;
-
     //Instantiate robust matcher: detector, extractor, matcher
     RobustMatcher rmatcher;
-  Ptr<FeatureDetector> detector = ORB::create(numKeyPoints);
+    Ptr<Feature2D> detector, descriptor;
+    createFeatures(featureName, numKeyPoints, detector, descriptor);
     rmatcher.setFeatureDetector(detector);
+    rmatcher.setDescriptorExtractor(descriptor);
+
 
     /**  GROUND TRUTH OF THE FIRST IMAGE  **/
 
@@ -104,13 +136,13 @@ int main()
     namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);
 
     // Set up the mouse events
-  setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 );
+    setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0);
 
     // Open the image to register
     Mat img_in = imread(img_path, IMREAD_COLOR);
-  Mat img_vis = img_in.clone();
+    Mat img_vis;
 
-  if (!img_in.data) {
+    if (img_in.empty()) {
         cout << "Could not open or find the image" << endl;
         return -1;
     }
@@ -122,6 +154,12 @@ int main()
     cout << "Click the box corners ..." << endl;
     cout << "Waiting ..." << endl;
 
+    // Some basic colors
+    const Scalar red(0, 0, 255);
+    const Scalar green(0,255,0);
+    const Scalar blue(255,0,0);
+    const Scalar yellow(0,255,255);
+
     // Loop until all the points are registered
     while ( waitKey(30) < 0 )
     {
@@ -176,7 +214,6 @@ int main()
         // Compute all the 2D points of the mesh to verify the algorithm and draw it
         vector<Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh);
         draw2DPoints(img_vis, list_points2d_mesh, green);
-
     } else {
         cout << "Correspondence not found" << endl << endl;
     }
@@ -215,6 +252,7 @@ int main()
         }
     }
 
+    model.set_trainingImagePath(img_path);
     // save the model into a *.yaml file
     model.save(write_path);
 
@@ -252,17 +290,4 @@ int main()
     destroyWindow("MODEL REGISTRATION");
 
     cout << "GOODBYE" << endl;
-
-}
-
-/**********************************************************************************************************/
-void help()
-{
-  cout
-  << "--------------------------------------------------------------------------"   << endl
-  << "This program shows how to create your 3D textured model. "                    << endl
-  << "Usage:"                                                                       << endl
-  << "./cpp-tutorial-pnp_registration"                                              << endl
-  << "--------------------------------------------------------------------------"   << endl
-  << endl;
 }