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

@@ -11,30 +11,30 @@ using namespace cv;
 
 class CsvReader {
 public:
-  /**
-  * The default constructor of the CSV reader Class.
-  * The default separator is ' ' (empty space)
-  *
-  * @param path - The path of the file to read
-  * @param separator - The separator character between words per line
-  * @return
-  */
-  CsvReader(const string &path, const char &separator = ' ');
+    /**
+    * The default constructor of the CSV reader Class.
+    * The default separator is ' ' (empty space)
+    *
+    * @param path - The path of the file to read
+    * @param separator - The separator character between words per line
+    * @return
+    */
+    CsvReader(const string &path, char separator = ' ');
 
-  /**
-  * Read a plane text file with .ply format
-  *
-  * @param list_vertex - The container of the vertices list of the mesh
-  * @param list_triangle - The container of the triangles list of the mesh
-  * @return
-  */
-  void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles);
+    /**
+    * Read a plane text file with .ply format
+    *
+    * @param list_vertex - The container of the vertices list of the mesh
+    * @param list_triangle - The container of the triangles list of the mesh
+    * @return
+    */
+    void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles);
 
 private:
-  /** The current stream file for the reader */
-  ifstream _file;
-  /** The separator character between words for each line */
-  char _separator;
+    /** The current stream file for the reader */
+    ifstream _file;
+    /** The separator character between words for each line */
+    char _separator;
 };
 
 #endif

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

 #include "CsvWriter.h"
 
 CsvWriter::CsvWriter(const string &path, const string &separator){
-  _file.open(path.c_str(), ofstream::out);
-  _isFirstTerm = true;
-  _separator = separator;
+    _file.open(path.c_str(), ofstream::out);
+    _isFirstTerm = true;
+    _separator = separator;
 }
 
 CsvWriter::~CsvWriter() {
-  _file.flush();
-  _file.close();
+    _file.flush();
+    _file.close();
 }
 
 void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d)
 {
-  string x, y, z;
-  for(unsigned int 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);
-
-    _file << x << _separator << y << _separator << z << std::endl;
-  }
+    for(size_t i = 0; i < list_points3d.size(); ++i)
+    {
+        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)
-  {
-    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);
-
-    _file << u << _separator << v << _separator << x << _separator << y << _separator << z;
-
-    for(int j = 0; j < 32; ++j)
+    for(size_t i = 0; i < list_points3d.size(); ++i)
     {
-      descriptor_str = FloatToString(descriptors.at<float>(i,j));
-      _file << _separator << descriptor_str;
+        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)
+        {
+            string descriptor_str = FloatToString(descriptors.at<float>((int)i,j));
+            _file << _separator << descriptor_str;
+        }
+        _file << std::endl;
     }
-    _file << std::endl;
-  }
 }

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

 #ifndef CSVWRITER_H
-#define	CSVWRITER_H
+#define CSVWRITER_H
 
 #include <iostream>
 #include <fstream>
@@ -11,15 +11,15 @@ using namespace cv;
 
 class CsvWriter {
 public:
-  CsvWriter(const string &path, const string &separator = " ");
-  ~CsvWriter();
-  void writeXYZ(const vector<Point3f> &list_points3d);
-  void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors);
+    CsvWriter(const string &path, const string &separator = " ");
+    ~CsvWriter();
+    void writeXYZ(const vector<Point3f> &list_points3d);
+    void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors);
 
 private:
-  ofstream _file;
-  string _separator;
-  bool _isFirstTerm;
+    ofstream _file;
+    string _separator;
+    bool _isFirstTerm;
 };
 
 #endif

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

@@ -14,15 +14,15 @@
 // --------------------------------------------------- //
 
 /**  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 */
 Triangle::~Triangle()
 {
-  // TODO Auto-generated destructor stub
+    // TODO Auto-generated destructor stub
 }
 
 
@@ -31,14 +31,15 @@ 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 */
 Ray::~Ray()
 {
-  // TODO Auto-generated destructor stub
+    // TODO Auto-generated destructor stub
 }
 
 
@@ -47,36 +48,31 @@ 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 */
 Mesh::~Mesh()
 {
-  // TODO Auto-generated destructor stub
+    // 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);
 
-  // Create the reader
-  CsvReader csvReader(path);
-
-  // Clear previous data
-  list_vertex_.clear();
-  list_triangles_.clear();
-
-  // Read from .ply file
-  csvReader.readPLY(list_vertex_, list_triangles_);
+    // Clear previous data
+    list_vertex_.clear();
+    list_triangles_.clear();
 
-  // Update mesh attributes
-  num_vertexs_ = (int)list_vertex_.size();
-  num_triangles_ = (int)list_triangles_.size();
+    // Read from .ply file
+    csvReader.readPLY(list_vertex_, list_triangles_);
 
+    // Update mesh attributes
+    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,18 +19,16 @@
 class Triangle {
 public:
 
-  explicit Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2);
-  virtual ~Triangle();
+    explicit Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2);
+    virtual ~Triangle();
 
-  cv::Point3f getV0() const { return v0_; }
-  cv::Point3f getV1() const { return v1_; }
-  cv::Point3f getV2() const { return v2_; }
+    cv::Point3f getV0() const { return v0_; }
+    cv::Point3f getV1() const { return v1_; }
+    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_;
+    /** The three vertices that defines the triangle */
+    cv::Point3f v0_, v1_, v2_;
 };
 
 
@@ -41,15 +39,15 @@ private:
 class Ray {
 public:
 
-  explicit Ray(cv::Point3f P0, cv::Point3f P1);
-  virtual ~Ray();
+    explicit Ray(const cv::Point3f& P0, const cv::Point3f& P1);
+    virtual ~Ray();
 
-  cv::Point3f getP0() { return p0_; }
-  cv::Point3f getP1() { return p1_; }
+    cv::Point3f getP0() { return p0_; }
+    cv::Point3f getP1() { return p1_; }
 
 private:
-  /** The two points that defines the ray */
-  cv::Point3f p0_, p1_;
+    /** The two points that defines the ray */
+    cv::Point3f p0_, p1_;
 };
 
 
@@ -61,26 +59,24 @@ class Mesh
 {
 public:
 
-  Mesh();
-  virtual ~Mesh();
+    Mesh();
+    virtual ~Mesh();
 
-  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_; }
+    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_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_;
-  /** The current number of triangles in the mesh */
-  int num_triangles_;
-  /* The list of triangles of the mesh */
-  std::vector<cv::Point3f> list_vertex_;
-  /* The list of triangles of the mesh */
-  std::vector<std::vector<int> > list_triangles_;
+    /** The current number of vertices in the mesh */
+    int num_vertices_;
+    /** The current number of triangles in the mesh */
+    int num_triangles_;
+    /* The list of triangles of the mesh */
+    std::vector<cv::Point3f> list_vertex_;
+    /* The list of triangles of the mesh */
+    std::vector<std::vector<int> > list_triangles_;
 };
 
 #endif /* OBJECTMESH_H_ */

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

@@ -8,66 +8,76 @@
 #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()
 {
-  // TODO Auto-generated destructor stub
+    // TODO Auto-generated destructor stub
 }
 
 void Model::add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d)
 {
-  list_points2d_in_.push_back(point2d);
-  list_points3d_in_.push_back(point3d);
-  n_correspondences_++;
+    list_points2d_in_.push_back(point2d);
+    list_points3d_in_.push_back(point3d);
+    n_correspondences_++;
 }
 
 void Model::add_outlier(const cv::Point2f &point2d)
 {
-  list_points2d_out_.push_back(point2d);
+    list_points2d_out_.push_back(point2d);
 }
 
 void Model::add_descriptor(const cv::Mat &descriptor)
 {
-  descriptors_.push_back(descriptor);
+    descriptors_.push_back(descriptor);
 }
 
 void Model::add_keypoint(const cv::KeyPoint &kp)
 {
-  list_keypoints_.push_back(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::Mat points3dmatrix = cv::Mat(list_points3d_in_);
+    cv::Mat points2dmatrix = cv::Mat(list_points2d_in_);
 
-  cv::FileStorage storage(path, cv::FileStorage::WRITE);
-  storage << "points_3d" << points3dmatrix;
-  storage << "points_2d" << points2dmatrix;
-  storage << "keypoints" << list_keypoints_;
-  storage << "descriptors" << descriptors_;
+    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();
+    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_;
-
-  points3d_mat.copyTo(list_points3d_in_);
-
-  storage.release();
-
+    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

@@ -15,40 +15,41 @@
 class Model
 {
 public:
-  Model();
-  virtual ~Model();
-
-  std::vector<cv::Point2f> get_points2d_in() const { return list_points2d_in_; }
-  std::vector<cv::Point2f> get_points2d_out() const { return list_points2d_out_; }
-  std::vector<cv::Point3f> get_points3d() const { return list_points3d_in_; }
-  std::vector<cv::KeyPoint> get_keypoints() const { return list_keypoints_; }
-  cv::Mat get_descriptors() const { return descriptors_; }
-  int get_numDescriptors() const { return descriptors_.rows; }
-
-
-  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 save(const std::string path);
-  void load(const std::string path);
-
+    Model();
+    virtual ~Model();
+
+    std::vector<cv::Point2f> get_points2d_in() const { return list_points2d_in_; }
+    std::vector<cv::Point2f> get_points2d_out() const { return list_points2d_out_; }
+    std::vector<cv::Point3f> get_points3d() const { return list_points3d_in_; }
+    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);
 
 private:
-  /** The current number of correspondecnes */
-  int n_correspondences_;
-  /** The list of 2D points on the model surface */
-  std::vector<cv::KeyPoint> list_keypoints_;
-  /** The list of 2D points on the model surface */
-  std::vector<cv::Point2f> list_points2d_in_;
-  /** The list of 2D points outside the model surface */
-  std::vector<cv::Point2f> list_points2d_out_;
-  /** The list of 3D points on the model surface */
-  std::vector<cv::Point3f> list_points3d_in_;
-  /** The list of 2D points descriptors */
-  cv::Mat descriptors_;
+    /** The current number of correspondecnes */
+    int n_correspondences_;
+    /** The list of 2D points on the model surface */
+    std::vector<cv::KeyPoint> list_keypoints_;
+    /** The list of 2D points on the model surface */
+    std::vector<cv::Point2f> list_points2d_in_;
+    /** The list of 2D points outside the model surface */
+    std::vector<cv::Point2f> list_points2d_out_;
+    /** The list of 3D points on the model surface */
+    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,29 +7,28 @@
 
 #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()
 {
-  // TODO Auto-generated destructor stub
+    // TODO Auto-generated destructor stub
 }
 
 void ModelRegistration::registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d)
- {
-   // add correspondence at the end of the vector
+{
+    // add correspondence at the end of the vector
     list_points2d_.push_back(point2d);
     list_points3d_.push_back(point3d);
     n_registrations_++;
- }
+}
 
 void ModelRegistration::reset()
 {
-  n_registrations_ = 0;
-  max_registrations_ = 0;
-  list_points2d_.clear();
-  list_points3d_.clear();
+    n_registrations_ = 0;
+    max_registrations_ = 0;
+    list_points2d_.clear();
+    list_points3d_.clear();
 }

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

@@ -14,30 +14,29 @@
 class ModelRegistration
 {
 public:
+    ModelRegistration();
+    virtual ~ModelRegistration();
 
-  ModelRegistration();
-  virtual ~ModelRegistration();
+    void setNumMax(int n) { max_registrations_ = n; }
 
-  void setNumMax(int n) { max_registrations_ = n; }
+    std::vector<cv::Point2f> get_points2d() const { return list_points2d_; }
+    std::vector<cv::Point3f> get_points3d() const { return list_points3d_; }
+    int getNumMax() const { return max_registrations_; }
+    int getNumRegist() const { return n_registrations_; }
 
-  std::vector<cv::Point2f> get_points2d() const { return list_points2d_; }
-  std::vector<cv::Point3f> get_points3d() const { return list_points3d_; }
-  int getNumMax() const { return max_registrations_; }
-  int getNumRegist() const { return n_registrations_; }
-
-  bool is_registrable() const { return (n_registrations_ < max_registrations_); }
-  void registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d);
-  void reset();
+    bool is_registrable() const { return (n_registrations_ < max_registrations_); }
+    void registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d);
+    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.h

@@ -18,41 +18,35 @@
 
 class PnPProblem
 {
-
 public:
-  explicit PnPProblem(const double param[]);  // custom constructor
-  virtual ~PnPProblem();
+    explicit PnPProblem(const double param[]);  // custom constructor
+    virtual ~PnPProblem();
 
-  bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d);
-  bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out);
-  std::vector<cv::Point2f> verify_points(Mesh *mesh);
-  cv::Point2f backproject3DPoint(const cv::Point3f &point3d);
-  bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags);
-  void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d,
-                           int flags, cv::Mat &inliers,
-                           int iterationsCount, float reprojectionError, double confidence );
+    bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d);
+    bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out);
+    std::vector<cv::Point2f> verify_points(Mesh *mesh);
+    cv::Point2f backproject3DPoint(const cv::Point3f &point3d);
+    bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags);
+    void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d,
+                             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);
+    void set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix);
 
 private:
-  /** The calibration matrix */
-  cv::Mat _A_matrix;
-  /** The computed rotation matrix */
-  cv::Mat _R_matrix;
-  /** The computed translation matrix */
-  cv::Mat _t_matrix;
-  /** The computed projection matrix */
-  cv::Mat _P_matrix;
+    /** The calibration matrix */
+    cv::Mat A_matrix_;
+    /** The computed rotation matrix */
+    cv::Mat R_matrix_;
+    /** The computed translation matrix */
+    cv::Mat t_matrix_;
+    /** The computed projection 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

@@ -12,141 +12,143 @@
 
 RobustMatcher::~RobustMatcher()
 {
-  // TODO Auto-generated destructor stub
+    // TODO Auto-generated destructor stub
 }
 
 void RobustMatcher::computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints)
 {
-  detector_->detect(image, keypoints);
+    detector_->detect(image, keypoints);
 }
 
 void RobustMatcher::computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)
 {
-  extractor_->compute(image, keypoints, descriptors);
+    extractor_->compute(image, keypoints, descriptors);
 }
 
 int RobustMatcher::ratioTest(std::vector<std::vector<cv::DMatch> > &matches)
 {
-  int removed = 0;
-  // for all matches
-  for ( std::vector<std::vector<cv::DMatch> >::iterator
-        matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
-  {
-    // if 2 NN has been identified
-    if (matchIterator->size() > 1)
+    int removed = 0;
+    // for all matches
+    for ( std::vector<std::vector<cv::DMatch> >::iterator
+          matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
     {
-      // check distance ratio
-      if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_)
-      {
-        matchIterator->clear(); // remove match
-        removed++;
-      }
-    }
-    else
-    { // does not have 2 neighbours
-      matchIterator->clear(); // remove match
-      removed++;
+        // if 2 NN has been identified
+        if (matchIterator->size() > 1)
+        {
+            // check distance ratio
+            if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_)
+            {
+                matchIterator->clear(); // remove match
+                removed++;
+            }
+        }
+        else
+        { // does not have 2 neighbours
+            matchIterator->clear(); // remove match
+            removed++;
+        }
     }
-  }
-  return removed;
+    return removed;
 }
 
 void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
-                     const std::vector<std::vector<cv::DMatch> >& matches2,
-                     std::vector<cv::DMatch>& symMatches )
+                                  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;
-
-      // for all matches image 2 -> image 1
-      for (std::vector<std::vector<cv::DMatch> >::const_iterator
-          matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
-      {
+    // 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 (matchIterator2->empty() || matchIterator2->size() < 2)
-           continue;
-
-        // Match symmetry test
-        if ((*matchIterator1)[0].queryIdx ==
-            (*matchIterator2)[0].trainIdx &&
-            (*matchIterator2)[0].queryIdx ==
-            (*matchIterator1)[0].trainIdx)
+        if (matchIterator1->empty() || matchIterator1->size() < 2)
+            continue;
+
+        // for all matches image 2 -> image 1
+        for (std::vector<std::vector<cv::DMatch> >::const_iterator
+             matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
         {
-            // add symmetrical match
-            symMatches.push_back(
-              cv::DMatch((*matchIterator1)[0].queryIdx,
-                         (*matchIterator1)[0].trainIdx,
-                         (*matchIterator1)[0].distance));
-            break; // next match in image 1 -> image 2
+            // ignore deleted matches
+            if (matchIterator2->empty() || matchIterator2->size() < 2)
+                continue;
+
+            // Match symmetry test
+            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));
+                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);
 
-  // 1a. Detection of the ORB features
-  this->computeKeyPoints(frame, keypoints_frame);
-
-  // 1b. Extraction of the ORB descriptors
-  cv::Mat descriptors_frame;
-  this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
+    // 1b. Extraction of the ORB descriptors
+    cv::Mat descriptors_frame;
+    this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
 
-  // 2. Match the two image descriptors
-  std::vector<std::vector<cv::DMatch> > matches12, matches21;
+    // 2. Match the two image descriptors
+    std::vector<std::vector<cv::DMatch> > matches12, matches21;
 
-  // 2a. From image 1 to image 2
-  matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
+    // 2a. From image 1 to image 2
+    matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
 
-  // 2b. From image 2 to image 1
-  matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours
+    // 2b. From image 2 to image 1
+    matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours
 
-  // 3. Remove matches for which NN ratio is > than threshold
-  // clean image 1 -> image 2 matches
-  ratioTest(matches12);
-  // clean image 2 -> image 1 matches
-  ratioTest(matches21);
+    // 3. Remove matches for which NN ratio is > than threshold
+    // clean image 1 -> image 2 matches
+    ratioTest(matches12);
+    // clean image 2 -> image 1 matches
+    ratioTest(matches21);
 
-  // 4. Remove non-symmetrical matches
-  symmetryTest(matches12, matches21, good_matches);
+    // 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 )
+                                     std::vector<cv::KeyPoint>& keypoints_frame,
+                                     const cv::Mat& descriptors_model,
+                                     const std::vector<cv::KeyPoint>& keypoints_model)
 {
-  good_matches.clear();
+    good_matches.clear();
 
-  // 1a. Detection of the ORB features
-  this->computeKeyPoints(frame, keypoints_frame);
+    // 1a. Detection of the ORB features
+    this->computeKeyPoints(frame, keypoints_frame);
 
-  // 1b. Extraction of the ORB descriptors
-  cv::Mat descriptors_frame;
-  this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
+    // 1b. Extraction of the ORB descriptors
+    cv::Mat descriptors_frame;
+    this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
 
-  // 2. Match the two image descriptors
-  std::vector<std::vector<cv::DMatch> > matches;
-  matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2);
+    // 2. Match the two image descriptors
+    std::vector<std::vector<cv::DMatch> > matches;
+    matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2);
 
-  // 3. Remove matches for which NN ratio is > than threshold
-  ratioTest(matches);
+    // 3. Remove matches for which NN ratio is > than threshold
+    ratioTest(matches);
 
-  // 4. Fill good matches container
-  for ( std::vector<std::vector<cv::DMatch> >::iterator
-         matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
-  {
-    if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]);
-  }
+    // 4. Fill good matches container
+    for ( std::vector<std::vector<cv::DMatch> >::iterator
+          matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
+    {
+        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,66 +16,77 @@
 
 class RobustMatcher {
 public:
-  RobustMatcher() : ratio_(0.8f)
-  {
-    // ORB is the default feature
-    detector_ = cv::ORB::create();
-    extractor_ = cv::ORB::create();
+    RobustMatcher() : detector_(), extractor_(), matcher_(),
+        ratio_(0.8f), training_img_(), img_matching_()
+    {
+        // ORB is the default feature
+        detector_ = cv::ORB::create();
+        extractor_ = cv::ORB::create();
 
-    // BruteFroce matcher with Norm Hamming is the default matcher
-    matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false);
+        // BruteFroce matcher with Norm Hamming is the default matcher
+        matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false);
 
-  }
-  virtual ~RobustMatcher();
+    }
+    virtual ~RobustMatcher();
 
-  // Set the feature detector
-  void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) {  detector_ = detect; }
+    // Set the feature detector
+    void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) {  detector_ = detect; }
 
-  // Set the descriptor extractor
-  void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) { extractor_ = desc; }
+    // Set the descriptor extractor
+    void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) { extractor_ = desc; }
 
-  // Set the matcher
-  void setDescriptorMatcher(const cv::Ptr<cv::DescriptorMatcher>& match) {  matcher_ = match; }
+    // Set the matcher
+    void setDescriptorMatcher(const cv::Ptr<cv::DescriptorMatcher>& match) {  matcher_ = match; }
 
-  // Compute the keypoints of an image
-  void computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints);
+    // Compute the keypoints of an image
+    void computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints);
 
-  // Compute the descriptors of an image given its keypoints
-  void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
+    // Compute the descriptors of an image given its keypoints
+    void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
 
-  // Set ratio parameter for the ratio test
-  void setRatio( float rat) { ratio_ = rat; }
+    cv::Mat getImageMatching() const { return img_matching_; }
 
-  // Clear matches for which NN ratio is > than threshold
-  // return the number of removed points
-  // (corresponding entries being cleared,
-  // i.e. size will be 0)
-  int ratioTest(std::vector<std::vector<cv::DMatch> > &matches);
+    // Set ratio parameter for the ratio test
+    void setRatio( float rat) { ratio_ = rat; }
 
-  // Insert symmetrical matches in symMatches vector
-  void symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
-                     const std::vector<std::vector<cv::DMatch> >& matches2,
-                     std::vector<cv::DMatch>& symMatches );
+    void setTrainingImage(const cv::Mat &img) { training_img_ = img; }
 
-  // Match feature points using ratio and symmetry test
-  void robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
+    // Clear matches for which NN ratio is > than threshold
+    // return the number of removed points
+    // (corresponding entries being cleared,
+    // i.e. size will be 0)
+    int ratioTest(std::vector<std::vector<cv::DMatch> > &matches);
+
+    // Insert symmetrical matches in symMatches vector
+    void symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
+                       const std::vector<std::vector<cv::DMatch> >& matches2,
+                       std::vector<cv::DMatch>& symMatches );
+
+    // 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 );
+    // 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 std::vector<cv::KeyPoint>& keypoints_model);
 
 private:
-  // pointer to the feature point detector object
-  cv::Ptr<cv::FeatureDetector> detector_;
-  // pointer to the feature descriptor extractor object
-  cv::Ptr<cv::DescriptorExtractor> extractor_;
-  // pointer to the matcher object
-  cv::Ptr<cv::DescriptorMatcher> matcher_;
-  // max ratio between 1st and 2nd NN
-  float ratio_;
+    // pointer to the feature point detector object
+    cv::Ptr<cv::FeatureDetector> detector_;
+    // pointer to the feature descriptor extractor object
+    cv::Ptr<cv::DescriptorExtractor> extractor_;
+    // pointer to the matcher object
+    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.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_ */