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.cpp

@@ -13,122 +13,112 @@
 
 #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;
-  tmp_p.y =  v1.z*v2.x - v1.x*v2.z;
-  tmp_p.z =  v1.x*v2.y - v1.y*v2.x;
-  return tmp_p;
+    cv::Point3f tmp_p;
+    tmp_p.x =  v1.y*v2.z - v1.z*v2.y;
+    tmp_p.y =  v1.z*v2.x - v1.x*v2.z;
+    tmp_p.z =  v1.x*v2.y - v1.y*v2.x;
+    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;
+    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;
-  tmp_p.y =  v1.y - v2.y;
-  tmp_p.z =  v1.z - v2.z;
-  return tmp_p;
+    cv::Point3f tmp_p;
+    tmp_p.x =  v1.x - v2.x;
+    tmp_p.y =  v1.y - v2.y;
+    tmp_p.z =  v1.z - v2.z;
+    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];
-
-  double d1 = std::sqrt( std::pow(p1.x-origin.x, 2) + std::pow(p1.y-origin.y, 2) + std::pow(p1.z-origin.z, 2) );
-  double d2 = std::sqrt( std::pow(p2.x-origin.x, 2) + std::pow(p2.y-origin.y, 2) + std::pow(p2.z-origin.z, 2) );
-
-  if(d1 < d2)
-  {
-      return p1;
-  }
-  else
-  {
-      return p2;
-  }
+    cv::Point3f p1 = points_list[0];
+    cv::Point3f p2 = points_list[1];
+
+    double d1 = std::sqrt( std::pow(p1.x-origin.x, 2) + std::pow(p1.y-origin.y, 2) + std::pow(p1.z-origin.z, 2) );
+    double d2 = std::sqrt( std::pow(p2.x-origin.x, 2) + std::pow(p2.y-origin.y, 2) + std::pow(p2.z-origin.z, 2) );
+
+    if(d1 < d2)
+    {
+        return p1;
+    }
+    else
+    {
+        return p2;
+    }
 }
 
 // Custom constructor given the intrinsic camera parameters
 
 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
 
 }
 
 PnPProblem::~PnPProblem()
 {
-  // TODO Auto-generated destructor stub
+    // TODO Auto-generated destructor stub
 }
 
 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);
+    // 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);
 }
 
-
 // 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,
                                int flags)
 {
-  cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
-  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
-  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
+    cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
+    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
+    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
 
-  bool useExtrinsicGuess = false;
+    bool useExtrinsicGuess = false;
 
-  // Pose estimation
-  bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
-                                      useExtrinsicGuess, flags);
+    // Pose estimation
+    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;
+    // Transforms Rotation Vector to Matrix
+    Rodrigues(rvec, R_matrix_);
+    t_matrix_ = tvec;
 
-  // Set projection matrix
-  this->set_P_matrix(_R_matrix, _t_matrix);
+    // Set projection matrix
+    this->set_P_matrix(R_matrix_, t_matrix_);
 
-  return correspondence;
+    return correspondence;
 }
 
 // Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use
@@ -138,182 +128,180 @@ void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points
                                      int flags, cv::Mat &inliers, int iterationsCount,  // PnP method; inliers container
                                      float reprojectionError, double confidence )    // Ransac parameters
 {
-  cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);  // vector of distortion coefficients
-  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);          // output rotation vector
-  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);    // output translation vector
+    cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);  // vector of distortion coefficients
+    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);          // output rotation vector
+    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);    // output translation vector
 
-  bool useExtrinsicGuess = false;   // if true the function uses the provided rvec and tvec values as
-            // initial approximations of the rotation and translation vectors
+    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,
-                useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
-                inliers, flags );
+    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
 
 }
 
 // Given the mesh, backproject the 3D points to 2D to verify the pose estimation
 std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
 {
-  std::vector<cv::Point2f> verified_points_2d;
-  for( int i = 0; i < mesh->getNumVertices(); i++)
-  {
-    cv::Point3f point3d = mesh->getVertex(i);
-    cv::Point2f point2d = this->backproject3DPoint(point3d);
-    verified_points_2d.push_back(point2d);
-  }
-
-  return verified_points_2d;
-}
+    std::vector<cv::Point2f> verified_points_2d;
+    for( int i = 0; i < mesh->getNumVertices(); i++)
+    {
+        cv::Point3f point3d = mesh->getVertex(i);
+        cv::Point2f point2d = this->backproject3DPoint(point3d);
+        verified_points_2d.push_back(point2d);
+    }
 
+    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]'
-  cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
-  point3d_vec.at<double>(0) = point3d.x;
-  point3d_vec.at<double>(1) = point3d.y;
-  point3d_vec.at<double>(2) = point3d.z;
-  point3d_vec.at<double>(3) = 1;
-
-  // 2D point vector [u v 1]'
-  cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
-  point2d_vec = _A_matrix * _P_matrix * point3d_vec;
-
-  // Normalization of [u v]'
-  cv::Point2f point2d;
-  point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));
-  point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));
-
-  return point2d;
+    // 3D point vector [x y z 1]'
+    cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
+    point3d_vec.at<double>(0) = point3d.x;
+    point3d_vec.at<double>(1) = point3d.y;
+    point3d_vec.at<double>(2) = point3d.z;
+    point3d_vec.at<double>(3) = 1;
+
+    // 2D point vector [u v 1]'
+    cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
+    point2d_vec = A_matrix_ * P_matrix_ * point3d_vec;
+
+    // Normalization of [u v]'
+    cv::Point2f point2d;
+    point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));
+    point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));
+
+    return point2d;
 }
 
 // Back project a 2D point to 3D and returns if it's on the object surface
 bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d)
 {
-  // Triangles list of the object mesh
-  std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList();
+    // Triangles list of the object mesh
+    std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList();
 
-  double lambda = 8;
-  double u = point2d.x;
-  double v = point2d.y;
+    double lambda = 8;
+    double u = point2d.x;
+    double v = point2d.y;
 
-  // Point in vector form
-  cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1
-  point2d_vec.at<double>(0) = u * lambda;
-  point2d_vec.at<double>(1) = v * lambda;
-  point2d_vec.at<double>(2) = lambda;
+    // Point in vector form
+    cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1
+    point2d_vec.at<double>(0) = u * lambda;
+    point2d_vec.at<double>(1) = v * lambda;
+    point2d_vec.at<double>(2) = lambda;
 
-  // Point in camera coordinates
-  cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1
+    // Point in camera coordinates
+    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
+    // Point in world coordinates
+    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
+    // Center of projection
+    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
-  ray = ray / cv::norm(ray); // 3x1
+    // Ray direction vector
+    cv::Mat ray = X_w - C_op; // 3x1
+    ray = ray / cv::norm(ray); // 3x1
 
-  // Set up Ray
-  Ray R((cv::Point3f)C_op, (cv::Point3f)ray);
+    // Set up Ray
+    Ray R((cv::Point3f)C_op, (cv::Point3f)ray);
 
-  // A vector to store the intersections found
-  std::vector<cv::Point3f> intersections_list;
+    // A vector to store the intersections found
+    std::vector<cv::Point3f> intersections_list;
 
-  // Loop for all the triangles and check the intersection
-  for (unsigned int i = 0; i < triangles_list.size(); i++)
-  {
-    cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]);
-    cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
-    cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
-
-    Triangle T(i, V0, V1, V2);
+    // Loop for all the triangles and check the intersection
+    for (unsigned int i = 0; i < triangles_list.size(); i++)
+    {
+        cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]);
+        cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
+        cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
+
+        Triangle T(V0, V1, V2);
+
+        double out;
+        if(this->intersect_MollerTrumbore(R, T, &out))
+        {
+            cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
+            intersections_list.push_back(tmp_pt);
+        }
+    }
 
-    double out;
-    if(this->intersect_MollerTrumbore(R, T, &out))
+    // If there are intersection, find the nearest one
+    if (!intersections_list.empty())
     {
-      cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
-      intersections_list.push_back(tmp_pt);
+        point3d = get_nearest_3D_point(intersections_list, R.getP0());
+        return true;
+    }
+    else
+    {
+        return false;
     }
-  }
-
-  // If there are intersection, find the nearest one
-  if (!intersections_list.empty())
-  {
-    point3d = get_nearest_3D_point(intersections_list, R.getP0());
-    return true;
-  }
-  else
-  {
-    return false;
-  }
 }
 
 // Möller-Trumbore intersection algorithm
 bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out)
 {
-  const double EPSILON = 0.000001;
+    const double EPSILON = 0.000001;
 
-  cv::Point3f e1, e2;
-  cv::Point3f P, Q, T;
-  double det, inv_det, u, v;
-  double t;
+    cv::Point3f e1, e2;
+    cv::Point3f P, Q, T;
+    double det, inv_det, u, v;
+    double t;
 
-  cv::Point3f V1 = Triangle.getV0();  // Triangle vertices
-  cv::Point3f V2 = Triangle.getV1();
-  cv::Point3f V3 = Triangle.getV2();
+    cv::Point3f V1 = Triangle.getV0();  // Triangle vertices
+    cv::Point3f V2 = Triangle.getV1();
+    cv::Point3f V3 = Triangle.getV2();
 
-  cv::Point3f O = Ray.getP0(); // Ray origin
-  cv::Point3f D = Ray.getP1(); // Ray direction
+    cv::Point3f O = Ray.getP0(); // Ray origin
+    cv::Point3f D = Ray.getP1(); // Ray direction
 
-  //Find vectors for two edges sharing V1
-  e1 = SUB(V2, V1);
-  e2 = SUB(V3, V1);
+    //Find vectors for two edges sharing V1
+    e1 = SUB(V2, V1);
+    e2 = SUB(V3, V1);
 
-  // Begin calculation determinant - also used to calculate U parameter
-  P = CROSS(D, e2);
+    // Begin calculation determinant - also used to calculate U parameter
+    P = CROSS(D, e2);
 
-  // If determinant is near zero, ray lie in plane of triangle
-  det = DOT(e1, P);
+    // If determinant is near zero, ray lie in plane of triangle
+    det = DOT(e1, P);
 
-  //NOT CULLING
-  if(det > -EPSILON && det < EPSILON) return false;
-  inv_det = 1.f / det;
+    //NOT CULLING
+    if(det > -EPSILON && det < EPSILON) return false;
+    inv_det = 1.f / det;
 
-  //calculate distance from V1 to ray origin
-  T = SUB(O, V1);
+    //calculate distance from V1 to ray origin
+    T = SUB(O, V1);
 
-  //Calculate u parameter and test bound
-  u = DOT(T, P) * inv_det;
+    //Calculate u parameter and test bound
+    u = DOT(T, P) * inv_det;
 
-  //The intersection lies outside of the triangle
-  if(u < 0.f || u > 1.f) return false;
+    //The intersection lies outside of the triangle
+    if(u < 0.f || u > 1.f) return false;
 
-  //Prepare to test v parameter
-  Q = CROSS(T, e1);
+    //Prepare to test v parameter
+    Q = CROSS(T, e1);
 
-  //Calculate V parameter and test bound
-  v = DOT(D, Q) * inv_det;
+    //Calculate V parameter and test bound
+    v = DOT(D, Q) * inv_det;
 
-  //The intersection lies outside of the triangle
-  if(v < 0.f || u + v  > 1.f) return false;
+    //The intersection lies outside of the triangle
+    if(v < 0.f || u + v  > 1.f) return false;
 
-  t = DOT(e2, Q) * inv_det;
+    t = DOT(e2, Q) * inv_det;
 
-  if(t > EPSILON) { //ray intersection
-    *out = t;
-    return true;
-  }
+    if(t > EPSILON) { //ray intersection
+        *out = t;
+        return true;
+    }
 
-  // No hit, no win
-  return false;
+    // No hit, no win
+    return false;
 }

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.cpp

@@ -11,178 +11,180 @@
 #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)
 {
-  std::string x = IntToString((int)point.x);
-  std::string y = IntToString((int)point.y);
-  std::string z = IntToString((int)point.z);
+    std::string x = IntToString((int)point.x);
+    std::string y = IntToString((int)point.y);
+    std::string z = IntToString((int)point.z);
 
-  std::string text = " Where is point (" + x + ","  + y + "," + z + ") ?";
-  cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
+    std::string text = " Where is point (" + x + ","  + y + "," + z + ") ?";
+    cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
 }
 
 // Draw a text with the number of entered points
 void drawText(cv::Mat image, std::string text, cv::Scalar color)
 {
-  cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
+    cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
 }
 
 // Draw a text with the number of entered points
 void drawText2(cv::Mat image, std::string text, cv::Scalar color)
 {
-  cv::putText(image, text, cv::Point(25,75), fontFace, fontScale, color, thickness_font, 8);
+    cv::putText(image, text, cv::Point(25,75), fontFace, fontScale, color, thickness_font, 8);
 }
 
 // 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
 void drawConfidence(cv::Mat image, double confidence, cv::Scalar color)
 {
-  std::string conf_str = IntToString((int)confidence);
-  std::string text = conf_str + " %";
-  cv::putText(image, text, cv::Point(500,75), fontFace, fontScale, color, thickness_font, 8);
+    std::string conf_str = IntToString((int)confidence);
+    std::string text = conf_str + " %";
+    cv::putText(image, text, cv::Point(500,75), fontFace, fontScale, color, thickness_font, 8);
 }
 
 // Draw a text with the number of entered points
 void drawCounter(cv::Mat image, int n, int n_max, cv::Scalar color)
 {
-  std::string n_str = IntToString(n);
-  std::string n_max_str = IntToString(n_max);
-  std::string text = n_str + " of " + n_max_str + " points";
-  cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
+    std::string n_str = IntToString(n);
+    std::string n_max_str = IntToString(n_max);
+    std::string text = n_str + " of " + n_max_str + " points";
+    cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
 }
 
 // Draw the points and the coordinates
 void drawPoints(cv::Mat image, std::vector<cv::Point2f> &list_points_2d, std::vector<cv::Point3f> &list_points_3d, cv::Scalar color)
 {
-  for (unsigned int i = 0; i < list_points_2d.size(); ++i)
-  {
-    cv::Point2f point_2d = list_points_2d[i];
-    cv::Point3f point_3d = list_points_3d[i];
-
-    // Draw Selected points
-    cv::circle(image, point_2d, radius, color, -1, lineType );
-
-    std::string idx = IntToString(i+1);
-    std::string x = IntToString((int)point_3d.x);
-    std::string y = IntToString((int)point_3d.y);
-    std::string z = IntToString((int)point_3d.z);
-    std::string text = "P" + idx + " (" + x + "," + y + "," + z +")";
-
-    point_2d.x = point_2d.x + 10;
-    point_2d.y = point_2d.y - 10;
-    cv::putText(image, text, point_2d, fontFace, fontScale*0.5, color, thickness_font, 8);
-  }
+    for (unsigned int i = 0; i < list_points_2d.size(); ++i)
+    {
+        cv::Point2f point_2d = list_points_2d[i];
+        cv::Point3f point_3d = list_points_3d[i];
+
+        // Draw Selected points
+        cv::circle(image, point_2d, radius, color, -1, lineType );
+
+        std::string idx = IntToString(i+1);
+        std::string x = IntToString((int)point_3d.x);
+        std::string y = IntToString((int)point_3d.y);
+        std::string z = IntToString((int)point_3d.z);
+        std::string text = "P" + idx + " (" + x + "," + y + "," + z +")";
+
+        point_2d.x = point_2d.x + 10;
+        point_2d.y = point_2d.y - 10;
+        cv::putText(image, text, point_2d, fontFace, fontScale*0.5, color, thickness_font, 8);
+    }
 }
 
 // Draw only the 2D points
 void draw2DPoints(cv::Mat image, std::vector<cv::Point2f> &list_points, cv::Scalar color)
 {
-  for( size_t i = 0; i < list_points.size(); i++)
-  {
-    cv::Point2f point_2d = list_points[i];
+    for( size_t i = 0; i < list_points.size(); i++)
+    {
+        cv::Point2f point_2d = list_points[i];
 
-    // Draw Selected points
-    cv::circle(image, point_2d, radius, color, -1, lineType );
-  }
+        // Draw Selected points
+        cv::circle(image, point_2d, radius, color, -1, lineType );
+    }
 }
 
 // Draw an arrow into the image
 void drawArrow(cv::Mat image, cv::Point2i p, cv::Point2i q, cv::Scalar color, int arrowMagnitude, int thickness, int line_type, int shift)
 {
-  //Draw the principle line
-  cv::line(image, p, q, color, thickness, line_type, shift);
-  const double PI = CV_PI;
-  //compute the angle alpha
-  double angle = atan2((double)p.y-q.y, (double)p.x-q.x);
-  //compute the coordinates of the first segment
-  p.x = (int) ( q.x +  arrowMagnitude * cos(angle + PI/4));
-  p.y = (int) ( q.y +  arrowMagnitude * sin(angle + PI/4));
-  //Draw the first segment
-  cv::line(image, p, q, color, thickness, line_type, shift);
-  //compute the coordinates of the second segment
-  p.x = (int) ( q.x +  arrowMagnitude * cos(angle - PI/4));
-  p.y = (int) ( q.y +  arrowMagnitude * sin(angle - PI/4));
-  //Draw the second segment
-  cv::line(image, p, q, color, thickness, line_type, shift);
+    //Draw the principle line
+    cv::line(image, p, q, color, thickness, line_type, shift);
+    const double PI = CV_PI;
+    //compute the angle alpha
+    double angle = atan2((double)p.y-q.y, (double)p.x-q.x);
+    //compute the coordinates of the first segment
+    p.x = (int) ( q.x +  arrowMagnitude * cos(angle + PI/4));
+    p.y = (int) ( q.y +  arrowMagnitude * sin(angle + PI/4));
+    //Draw the first segment
+    cv::line(image, p, q, color, thickness, line_type, shift);
+    //compute the coordinates of the second segment
+    p.x = (int) ( q.x +  arrowMagnitude * cos(angle - PI/4));
+    p.y = (int) ( q.y +  arrowMagnitude * sin(angle - PI/4));
+    //Draw the second segment
+    cv::line(image, p, q, color, thickness, line_type, shift);
 }
 
 // Draw the 3D coordinate axes
 void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_points2d)
 {
-  cv::Scalar red(0, 0, 255);
-  cv::Scalar green(0,255,0);
-  cv::Scalar blue(255,0,0);
-  cv::Scalar black(0,0,0);
-
-  cv::Point2i origin = list_points2d[0];
-  cv::Point2i pointX = list_points2d[1];
-  cv::Point2i pointY = list_points2d[2];
-  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);
-  cv::circle(image, origin, radius/2, black, -1, lineType );
-
+    cv::Scalar red(0, 0, 255);
+    cv::Scalar green(0,255,0);
+    cv::Scalar blue(255,0,0);
+    cv::Scalar black(0,0,0);
+
+    cv::Point2i origin = list_points2d[0];
+    cv::Point2i pointX = list_points2d[1];
+    cv::Point2i pointY = list_points2d[2];
+    cv::Point2i pointZ = list_points2d[3];
+
+    drawArrow(image, origin, pointX, red, 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
 void drawObjectMesh(cv::Mat image, const Mesh *mesh, PnPProblem *pnpProblem, cv::Scalar color)
 {
-  std::vector<std::vector<int> > list_triangles = mesh->getTrianglesList();
-  for( size_t i = 0; i < list_triangles.size(); i++)
-  {
-    std::vector<int> tmp_triangle = list_triangles.at(i);
-
-    cv::Point3f point_3d_0 = mesh->getVertex(tmp_triangle[0]);
-    cv::Point3f point_3d_1 = mesh->getVertex(tmp_triangle[1]);
-    cv::Point3f point_3d_2 = mesh->getVertex(tmp_triangle[2]);
-
-    cv::Point2f point_2d_0 = pnpProblem->backproject3DPoint(point_3d_0);
-    cv::Point2f point_2d_1 = pnpProblem->backproject3DPoint(point_3d_1);
-    cv::Point2f point_2d_2 = pnpProblem->backproject3DPoint(point_3d_2);
-
-    cv::line(image, point_2d_0, point_2d_1, color, 1);
-    cv::line(image, point_2d_1, point_2d_2, color, 1);
-    cv::line(image, point_2d_2, point_2d_0, color, 1);
-  }
+    std::vector<std::vector<int> > list_triangles = mesh->getTrianglesList();
+    for( size_t i = 0; i < list_triangles.size(); i++)
+    {
+        std::vector<int> tmp_triangle = list_triangles.at(i);
+
+        cv::Point3f point_3d_0 = mesh->getVertex(tmp_triangle[0]);
+        cv::Point3f point_3d_1 = mesh->getVertex(tmp_triangle[1]);
+        cv::Point3f point_3d_2 = mesh->getVertex(tmp_triangle[2]);
+
+        cv::Point2f point_2d_0 = pnpProblem->backproject3DPoint(point_3d_0);
+        cv::Point2f point_2d_1 = pnpProblem->backproject3DPoint(point_3d_1);
+        cv::Point2f point_2d_2 = pnpProblem->backproject3DPoint(point_3d_2);
+
+        cv::line(image, point_2d_0, point_2d_1, color, 1);
+        cv::line(image, point_2d_1, point_2d_2, color, 1);
+        cv::line(image, point_2d_2, point_2d_0, color, 1);
+    }
 }
 
 // Computes the norm of the translation error
 double get_translation_error(const cv::Mat &t_true, const cv::Mat &t)
 {
-  return cv::norm( t_true - t );
+    return cv::norm( t_true - t );
 }
 
 // Computes the norm of the rotation error
 double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R)
 {
-  cv::Mat error_vec, error_mat;
-  error_mat = -R_true * R.t();
-  cv::Rodrigues(error_mat, error_vec);
+    cv::Mat error_vec, error_mat;
+    error_mat = -R_true * R.t();
+    cv::Rodrigues(error_mat, error_vec);
 
-  return cv::norm(error_vec);
+    return cv::norm(error_vec);
 }
 
 // Converts a given Rotation Matrix to Euler angles
@@ -191,41 +193,41 @@ double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R)
 // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
 cv::Mat rot2euler(const cv::Mat & rotationMatrix)
 {
-  cv::Mat euler(3,1,CV_64F);
-
-  double m00 = rotationMatrix.at<double>(0,0);
-  double m02 = rotationMatrix.at<double>(0,2);
-  double m10 = rotationMatrix.at<double>(1,0);
-  double m11 = rotationMatrix.at<double>(1,1);
-  double m12 = rotationMatrix.at<double>(1,2);
-  double m20 = rotationMatrix.at<double>(2,0);
-  double m22 = rotationMatrix.at<double>(2,2);
-
-  double bank, attitude, heading;
-
-  // Assuming the angles are in radians.
-  if (m10 > 0.998) { // singularity at north pole
-    bank = 0;
-    attitude = CV_PI/2;
-    heading = atan2(m02,m22);
-  }
-  else if (m10 < -0.998) { // singularity at south pole
-    bank = 0;
-    attitude = -CV_PI/2;
-    heading = atan2(m02,m22);
-  }
-  else
-  {
-    bank = atan2(-m12,m11);
-    attitude = asin(m10);
-    heading = atan2(-m20,m00);
-  }
-
-  euler.at<double>(0) = bank;
-  euler.at<double>(1) = attitude;
-  euler.at<double>(2) = heading;
-
-  return euler;
+    cv::Mat euler(3,1,CV_64F);
+
+    double m00 = rotationMatrix.at<double>(0,0);
+    double m02 = rotationMatrix.at<double>(0,2);
+    double m10 = rotationMatrix.at<double>(1,0);
+    double m11 = rotationMatrix.at<double>(1,1);
+    double m12 = rotationMatrix.at<double>(1,2);
+    double m20 = rotationMatrix.at<double>(2,0);
+    double m22 = rotationMatrix.at<double>(2,2);
+
+    double bank, attitude, heading;
+
+    // Assuming the angles are in radians.
+    if (m10 > 0.998) { // singularity at north pole
+        bank = 0;
+        attitude = CV_PI/2;
+        heading = atan2(m02,m22);
+    }
+    else if (m10 < -0.998) { // singularity at south pole
+        bank = 0;
+        attitude = -CV_PI/2;
+        heading = atan2(m02,m22);
+    }
+    else
+    {
+        bank = atan2(-m12,m11);
+        attitude = asin(m10);
+        heading = atan2(-m20,m00);
+    }
+
+    euler.at<double>(0) = bank;
+    euler.at<double>(1) = attitude;
+    euler.at<double>(2) = heading;
+
+    return euler;
 }
 
 // Converts a given Euler angles to Rotation Matrix
@@ -234,65 +236,166 @@ cv::Mat rot2euler(const cv::Mat & rotationMatrix)
 // https://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm
 cv::Mat euler2rot(const cv::Mat & euler)
 {
-  cv::Mat rotationMatrix(3,3,CV_64F);
-
-  double bank = euler.at<double>(0);
-  double attitude = euler.at<double>(1);
-  double heading = euler.at<double>(2);
-
-  // Assuming the angles are in radians.
-  double ch = cos(heading);
-  double sh = sin(heading);
-  double ca = cos(attitude);
-  double sa = sin(attitude);
-  double cb = cos(bank);
-  double sb = sin(bank);
-
-  double m00, m01, m02, m10, m11, m12, m20, m21, m22;
-
-  m00 = ch * ca;
-  m01 = sh*sb - ch*sa*cb;
-  m02 = ch*sa*sb + sh*cb;
-  m10 = sa;
-  m11 = ca*cb;
-  m12 = -ca*sb;
-  m20 = -sh*ca;
-  m21 = sh*sa*cb + ch*sb;
-  m22 = -sh*sa*sb + ch*cb;
-
-  rotationMatrix.at<double>(0,0) = m00;
-  rotationMatrix.at<double>(0,1) = m01;
-  rotationMatrix.at<double>(0,2) = m02;
-  rotationMatrix.at<double>(1,0) = m10;
-  rotationMatrix.at<double>(1,1) = m11;
-  rotationMatrix.at<double>(1,2) = m12;
-  rotationMatrix.at<double>(2,0) = m20;
-  rotationMatrix.at<double>(2,1) = m21;
-  rotationMatrix.at<double>(2,2) = m22;
-
-  return rotationMatrix;
+    cv::Mat rotationMatrix(3,3,CV_64F);
+
+    double bank = euler.at<double>(0);
+    double attitude = euler.at<double>(1);
+    double heading = euler.at<double>(2);
+
+    // Assuming the angles are in radians.
+    double ch = cos(heading);
+    double sh = sin(heading);
+    double ca = cos(attitude);
+    double sa = sin(attitude);
+    double cb = cos(bank);
+    double sb = sin(bank);
+
+    double m00, m01, m02, m10, m11, m12, m20, m21, m22;
+
+    m00 = ch * ca;
+    m01 = sh*sb - ch*sa*cb;
+    m02 = ch*sa*sb + sh*cb;
+    m10 = sa;
+    m11 = ca*cb;
+    m12 = -ca*sb;
+    m20 = -sh*ca;
+    m21 = sh*sa*cb + ch*sb;
+    m22 = -sh*sa*sb + ch*cb;
+
+    rotationMatrix.at<double>(0,0) = m00;
+    rotationMatrix.at<double>(0,1) = m01;
+    rotationMatrix.at<double>(0,2) = m02;
+    rotationMatrix.at<double>(1,0) = m10;
+    rotationMatrix.at<double>(1,1) = m11;
+    rotationMatrix.at<double>(1,2) = m12;
+    rotationMatrix.at<double>(2,0) = m20;
+    rotationMatrix.at<double>(2,1) = m21;
+    rotationMatrix.at<double>(2,2) = m22;
+
+    return rotationMatrix;
 }
 
 // Converts a given string to an integer
 int StringToInt ( const std::string &Text )
 {
-   std::istringstream ss(Text);
-   int result;
-   return ss >> result ? result : 0;
+    std::istringstream ss(Text);
+    int result;
+    return ss >> result ? result : 0;
 }
 
 // Converts a given float to a string
 std::string FloatToString ( float Number )
 {
-  std::ostringstream ss;
-  ss << Number;
-  return ss.str();
+    std::ostringstream ss;
+    ss << Number;
+    return ss.str();
 }
 
 // Converts a given integer to a string
 std::string IntToString ( int Number )
 {
-  std::ostringstream ss;
-  ss << Number;
-  return ss.str();
+    std::ostringstream ss;
+    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_detection.cpp

 // C++
 #include <iostream>
-#include <time.h>
 // OpenCV
 #include <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
+#include <opencv2/core/utils/filesystem.hpp>
 #include <opencv2/highgui.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/calib3d.hpp>
@@ -21,451 +20,482 @@
 using namespace cv;
 using namespace std;
 
-string tutorial_path = "../../samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/"; // path to tutorial
-
-string video_read_path = tutorial_path + "Data/box.mp4";       // recorded video
-string yml_read_path = tutorial_path + "Data/cookies_ORB.yml"; // 3dpts + descriptors
-string ply_read_path = tutorial_path + "Data/box.ply";         // mesh
-
-// Intrinsic camera parameters: UVC WEBCAM
-double f = 55;                           // focal length in mm
-double sx = 22.3, sy = 14.9;             // sensor size
-double width = 640, height = 480;        // image size
-
-double params_WEBCAM[] = { width*f/sx,   // fx
-                           height*f/sy,  // fy
-                           width/2,      // cx
-                           height/2};    // cy
-
-// Some basic colors
-Scalar red(0, 0, 255);
-Scalar green(0,255,0);
-Scalar blue(255,0,0);
-Scalar yellow(0,255,255);
-
-
-// Robust Matcher parameters
-int numKeyPoints = 2000;      // number of detected keypoints
-float ratioTest = 0.70f;          // ratio test
-bool fast_match = true;       // fastRobustMatch() or robustMatch()
-
-// RANSAC parameters
-int iterationsCount = 500;      // number of Ransac iterations.
-float reprojectionError = 2.0;  // maximum allowed distance to consider it an inlier.
-double confidence = 0.95;        // ransac successful confidence.
-
-// Kalman Filter parameters
-int minInliersKalman = 30;    // Kalman threshold updating
-
-// PnP parameters
-int pnpMethod = SOLVEPNP_ITERATIVE;
-
-
 /**  Functions headers  **/
 void help();
 void initKalmanFilter( KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt);
+void predictKalmanFilter( KalmanFilter &KF, Mat &translation_predicted, Mat &rotation_predicted );
 void updateKalmanFilter( KalmanFilter &KF, Mat &measurements,
                          Mat &translation_estimated, Mat &rotation_estimated );
 void fillMeasurements( Mat &measurements,
                        const Mat &translation_measured, const Mat &rotation_measured);
 
-
 /**  Main program  **/
 int main(int argc, char *argv[])
 {
-
-  help();
-
-  const String keys =
-      "{help h        |      | print this message                   }"
-      "{video v       |      | path to recorded video               }"
-      "{model         |      | path to yml model                    }"
-      "{mesh          |      | path to ply mesh                     }"
-      "{keypoints k   |2000  | number of keypoints to detect        }"
-      "{ratio r       |0.7   | threshold for ratio test             }"
-      "{iterations it |500   | RANSAC maximum iterations count      }"
-      "{error e       |2.0   | RANSAC reprojection error            }"
-      "{confidence c  |0.95  | RANSAC confidence                    }"
-      "{inliers in    |30    | minimum inliers for Kalman update    }"
-      "{method  pnp   |0     | PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS}"
-      "{fast f        |true  | use of robust fast match             }"
-      ;
-  CommandLineParser parser(argc, argv, keys);
-
-  if (parser.has("help"))
-  {
-      parser.printMessage();
-      return 0;
-  }
-  else
-  {
-    video_read_path = parser.get<string>("video").size() > 0 ? parser.get<string>("video") : video_read_path;
-    yml_read_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : yml_read_path;
-    ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
-    numKeyPoints = !parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
-    ratioTest = !parser.has("ratio") ? parser.get<float>("ratio") : ratioTest;
-    fast_match = !parser.has("fast") ? parser.get<bool>("fast") : fast_match;
-    iterationsCount = !parser.has("iterations") ? parser.get<int>("iterations") : iterationsCount;
-    reprojectionError = !parser.has("error") ? parser.get<float>("error") : reprojectionError;
-    confidence = !parser.has("confidence") ? parser.get<float>("confidence") : confidence;
-    minInliersKalman = !parser.has("inliers") ? parser.get<int>("inliers") : minInliersKalman;
-    pnpMethod = !parser.has("method") ? parser.get<int>("method") : pnpMethod;
-  }
-
-  PnPProblem pnp_detection(params_WEBCAM);
-  PnPProblem pnp_detection_est(params_WEBCAM);
-
-  Model model;               // instantiate Model object
-  model.load(yml_read_path); // load a 3D textured object model
-
-  Mesh mesh;                 // instantiate Mesh object
-  mesh.load(ply_read_path);  // load an object mesh
-
-  RobustMatcher rmatcher;                                                     // instantiate RobustMatcher
-
-  Ptr<FeatureDetector> orb = ORB::create();
-
-  rmatcher.setFeatureDetector(orb);                                      // set feature detector
-  rmatcher.setDescriptorExtractor(orb);                                 // set descriptor extractor
-
-  Ptr<flann::IndexParams> indexParams = makePtr<flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters
-  Ptr<flann::SearchParams> searchParams = makePtr<flann::SearchParams>(50);       // instantiate flann search parameters
-
-  // instantiate FlannBased matcher
-  Ptr<DescriptorMatcher> matcher = makePtr<FlannBasedMatcher>(indexParams, searchParams);
-  rmatcher.setDescriptorMatcher(matcher);                                                         // set matcher
-  rmatcher.setRatio(ratioTest); // set ratio test parameter
-
-  KalmanFilter KF;         // instantiate Kalman Filter
-  int nStates = 18;            // the number of states
-  int nMeasurements = 6;       // the number of measured states
-  int nInputs = 0;             // the number of control actions
-  double dt = 0.125;           // time between measurements (1/FPS)
-
-  initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt);    // init function
-  Mat measurements(nMeasurements, 1, CV_64F); measurements.setTo(Scalar(0));
-  bool good_measurement = false;
-
-
-  // Get the MODEL INFO
-  vector<Point3f> list_points3d_model = model.get_points3d();  // list with model 3D coordinates
-  Mat descriptors_model = model.get_descriptors();                  // list with descriptors of each 3D coordinate
-
-
-  // Create & Open Window
-  namedWindow("REAL TIME DEMO", WINDOW_KEEPRATIO);
-
-
-  VideoCapture cap;                           // instantiate VideoCapture
-  cap.open(video_read_path);                      // open a recorded video
-
-  if(!cap.isOpened())   // check if we succeeded
-  {
-    cout << "Could not open the camera device" << endl;
-    return -1;
-  }
-
-  // start and end times
-  time_t start, end;
-
-  // fps calculated using number of frames / seconds
-  // floating point seconds elapsed since start
-  double fps, sec;
-
-  // frame counter
-  int counter = 0;
-
-  // start the clock
-  time(&start);
-
-  Mat frame, frame_vis;
-
-  while(cap.read(frame) && (char)waitKey(30) != 27) // capture frame until ESC is pressed
-  {
-
-    frame_vis = frame.clone();    // refresh visualisation frame
-
-
-    // -- Step 1: Robust matching between model descriptors and scene descriptors
-
-    vector<DMatch> good_matches;       // to obtain the 3D points of the model
-    vector<KeyPoint> keypoints_scene;  // to obtain the 2D points of the scene
-
-
-    if(fast_match)
+    help();
+
+    const String keys =
+            "{help h            |      | print this message                                                 }"
+            "{video v           |      | path to recorded video                                             }"
+            "{model             |      | path to yml model                                                  }"
+            "{mesh              |      | path to ply mesh                                                   }"
+            "{keypoints k       |2000  | number of keypoints to detect                                      }"
+            "{ratio r           |0.7   | threshold for ratio test                                           }"
+            "{iterations it     |500   | RANSAC maximum iterations count                                    }"
+            "{error e           |6.0   | RANSAC reprojection error                                          }"
+            "{confidence c      |0.99  | RANSAC confidence                                                  }"
+            "{inliers in        |30    | minimum inliers for Kalman update                                  }"
+            "{method  pnp       |0     | PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS - (5) AP3P}"
+            "{fast f            |true  | use of robust fast match                                           }"
+            "{feature           |ORB   | feature name (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)  }"
+            "{FLANN             |false | use FLANN library for descriptors matching                         }"
+            "{save              |      | path to the directory where to save the image results              }"
+            "{displayFiltered   |false | display filtered pose (from Kalman filter)                         }"
+            ;
+    CommandLineParser parser(argc, argv, keys);
+
+    string video_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.mp4");       // recorded video
+    string yml_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml"); // 3dpts + descriptors
+    string ply_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply");         // mesh
+
+    // Intrinsic camera parameters: UVC WEBCAM
+    double f = 55;                           // focal length in mm
+    double sx = 22.3, sy = 14.9;             // sensor size
+    double width = 640, height = 480;        // image size
+
+    double params_WEBCAM[] = { width*f/sx,   // fx
+                               height*f/sy,  // fy
+                               width/2,      // cx
+                               height/2};    // cy
+
+    // Some basic colors
+    Scalar red(0, 0, 255);
+    Scalar green(0,255,0);
+    Scalar blue(255,0,0);
+    Scalar yellow(0,255,255);
+
+    // Robust Matcher parameters
+    int numKeyPoints = 2000;      // number of detected keypoints
+    float ratioTest = 0.70f;      // ratio test
+    bool fast_match = true;       // fastRobustMatch() or robustMatch()
+
+    // RANSAC parameters
+    int iterationsCount = 500;      // number of Ransac iterations.
+    float reprojectionError = 6.0;  // maximum allowed distance to consider it an inlier.
+    double confidence = 0.99;       // ransac successful confidence.
+
+    // Kalman Filter parameters
+    int minInliersKalman = 30;    // Kalman threshold updating
+
+    // PnP parameters
+    int pnpMethod = SOLVEPNP_ITERATIVE;
+    string featureName = "ORB";
+    bool useFLANN = false;
+
+    // Save results
+    string saveDirectory = "";
+    Mat frameSave;
+    int frameCount = 0;
+
+    bool displayFilteredPose = false;
+
+    if (parser.has("help"))
     {
-      rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);
+        parser.printMessage();
+        return 0;
     }
     else
     {
-      rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model);
+        video_read_path = parser.get<string>("video").size() > 0 ? parser.get<string>("video") : video_read_path;
+        yml_read_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : yml_read_path;
+        ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
+        numKeyPoints = parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
+        ratioTest = parser.has("ratio") ? parser.get<float>("ratio") : ratioTest;
+        fast_match = parser.has("fast") ? parser.get<bool>("fast") : fast_match;
+        iterationsCount = parser.has("iterations") ? parser.get<int>("iterations") : iterationsCount;
+        reprojectionError = parser.has("error") ? parser.get<float>("error") : reprojectionError;
+        confidence = parser.has("confidence") ? parser.get<float>("confidence") : confidence;
+        minInliersKalman = parser.has("inliers") ? parser.get<int>("inliers") : minInliersKalman;
+        pnpMethod = parser.has("method") ? parser.get<int>("method") : pnpMethod;
+        featureName = parser.has("feature") ? parser.get<string>("feature") : featureName;
+        useFLANN = parser.has("FLANN") ? parser.get<bool>("FLANN") : useFLANN;
+        saveDirectory = parser.has("save") ? parser.get<string>("save") : saveDirectory;
+        displayFilteredPose = parser.has("displayFiltered") ? parser.get<bool>("displayFiltered") : displayFilteredPose;
     }
 
-
-    // -- Step 2: Find out the 2D/3D correspondences
-
-    vector<Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
-    vector<Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
-
-    for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
+    std::cout << "Video: " << video_read_path << std::endl;
+    std::cout << "Training data: " << yml_read_path << std::endl;
+    std::cout << "CAD model: " << ply_read_path << std::endl;
+    std::cout << "Ratio test threshold: " << ratioTest << std::endl;
+    std::cout << "Fast match(no symmetry test)?: " << fast_match << std::endl;
+    std::cout << "RANSAC number of iterations: " << iterationsCount << std::endl;
+    std::cout << "RANSAC reprojection error: " << reprojectionError << std::endl;
+    std::cout << "RANSAC confidence threshold: " << confidence << std::endl;
+    std::cout << "Kalman number of inliers: " << minInliersKalman << std::endl;
+    std::cout << "PnP method: " << pnpMethod << std::endl;
+    std::cout << "Feature: " << featureName << std::endl;
+    std::cout << "Number of keypoints for ORB: " << numKeyPoints << std::endl;
+    std::cout << "Use FLANN-based matching? " << useFLANN << std::endl;
+    std::cout << "Save directory: " << saveDirectory << std::endl;
+    std::cout << "Display filtered pose from Kalman filter? " << displayFilteredPose << std::endl;
+
+    PnPProblem pnp_detection(params_WEBCAM);
+    PnPProblem pnp_detection_est(params_WEBCAM);
+
+    Model model;               // instantiate Model object
+    model.load(yml_read_path); // load a 3D textured object model
+
+    Mesh mesh;                 // instantiate Mesh object
+    mesh.load(ply_read_path);  // load an object mesh
+
+    RobustMatcher rmatcher;                                                     // instantiate RobustMatcher
+
+    Ptr<FeatureDetector> detector, descriptor;
+    createFeatures(featureName, numKeyPoints, detector, descriptor);
+    rmatcher.setFeatureDetector(detector);                                      // set feature detector
+    rmatcher.setDescriptorExtractor(descriptor);                                // set descriptor extractor
+    rmatcher.setDescriptorMatcher(createMatcher(featureName, useFLANN));        // set matcher
+    rmatcher.setRatio(ratioTest); // set ratio test parameter
+    if (!model.get_trainingImagePath().empty())
     {
-      Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ];  // 3D point from model
-      Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 2D point from the scene
-      list_points3d_model_match.push_back(point3d_model);         // add 3D point
-      list_points2d_scene_match.push_back(point2d_scene);         // add 2D point
+        Mat trainingImg = imread(model.get_trainingImagePath());
+        rmatcher.setTrainingImage(trainingImg);
     }
 
-    // Draw outliers
-    draw2DPoints(frame_vis, list_points2d_scene_match, red);
-
-
-    Mat inliers_idx;
-    vector<Point2f> list_points2d_inliers;
-
-    if(good_matches.size() >= 4) // OpenCV requires solvePnPRANSAC to minimally have 4 set of points
-    {
-
-      // -- Step 3: Estimate the pose using RANSAC approach
-      pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match,
-                                        pnpMethod, inliers_idx,
-                                        iterationsCount, reprojectionError, confidence );
-
-      // -- Step 4: Catch the inliers keypoints to draw
-      for(int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index)
-      {
-        int n = inliers_idx.at<int>(inliers_index);         // i-inlier
-        Point2f point2d = list_points2d_scene_match[n]; // i-inlier point 2D
-        list_points2d_inliers.push_back(point2d);           // add i-inlier to list
-      }
-
-      // Draw inliers points 2D
-      draw2DPoints(frame_vis, list_points2d_inliers, blue);
+    KalmanFilter KF;             // instantiate Kalman Filter
+    int nStates = 18;            // the number of states
+    int nMeasurements = 6;       // the number of measured states
+    int nInputs = 0;             // the number of control actions
+    double dt = 0.125;           // time between measurements (1/FPS)
 
+    initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt);    // init function
+    Mat measurements(nMeasurements, 1, CV_64FC1); measurements.setTo(Scalar(0));
+    bool good_measurement = false;
 
-      // -- Step 5: Kalman Filter
+    // Get the MODEL INFO
+    vector<Point3f> list_points3d_model = model.get_points3d();  // list with model 3D coordinates
+    Mat descriptors_model = model.get_descriptors();             // list with descriptors of each 3D coordinate
+    vector<KeyPoint> keypoints_model = model.get_keypoints();
 
-      good_measurement = false;
+    // Create & Open Window
+    namedWindow("REAL TIME DEMO", WINDOW_KEEPRATIO);
 
-      // GOOD MEASUREMENT
-      if( inliers_idx.rows >= minInliersKalman )
-      {
+    VideoCapture cap;                           // instantiate VideoCapture
+    cap.open(video_read_path);                  // open a recorded video
 
-        // Get the measured translation
-        Mat translation_measured(3, 1, CV_64F);
-        translation_measured = pnp_detection.get_t_matrix();
-
-        // Get the measured rotation
-        Mat rotation_measured(3, 3, CV_64F);
-        rotation_measured = pnp_detection.get_R_matrix();
-
-        // fill the measurements vector
-        fillMeasurements(measurements, translation_measured, rotation_measured);
-
-        good_measurement = true;
-
-      }
-
-      // Instantiate estimated translation and rotation
-      Mat translation_estimated(3, 1, CV_64F);
-      Mat rotation_estimated(3, 3, CV_64F);
-
-      // update the Kalman filter with good measurements
-      updateKalmanFilter( KF, measurements,
-                          translation_estimated, rotation_estimated);
-
-
-      // -- Step 6: Set estimated projection matrix
-      pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
-
-    }
-
-    // -- Step X: Draw pose
-
-    if(good_measurement)
+    if(!cap.isOpened())   // check if we succeeded
     {
-      drawObjectMesh(frame_vis, &mesh, &pnp_detection, green);  // draw current pose
+        cout << "Could not open the camera device" << endl;
+        return -1;
     }
-    else
+
+    if (!saveDirectory.empty())
     {
-      drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
+        if (!cv::utils::fs::exists(saveDirectory))
+        {
+            std::cout << "Create directory: " << saveDirectory << std::endl;
+            cv::utils::fs::createDirectories(saveDirectory);
+        }
     }
 
-    float l = 5;
-    vector<Point2f> pose_points2d;
-    pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,0)));  // axis center
-    pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(l,0,0)));  // axis x
-    pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,l,0)));  // axis y
-    pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,l)));  // axis z
-    draw3DCoordinateAxes(frame_vis, pose_points2d);           // draw axes
-
-    // FRAME RATE
-
-    // see how much time has elapsed
-    time(&end);
-
-    // calculate current FPS
-    ++counter;
-    sec = difftime (end, start);
-
-    fps = counter / sec;
-
-    drawFPS(frame_vis, fps, yellow); // frame ratio
-    double detection_ratio = ((double)inliers_idx.rows/(double)good_matches.size())*100;
-    drawConfidence(frame_vis, detection_ratio, yellow);
-
+    // Measure elapsed time
+    TickMeter tm;
 
-    // -- Step X: Draw some debugging text
-
-    // Draw some debug text
-    int inliers_int = inliers_idx.rows;
-    int outliers_int = (int)good_matches.size() - inliers_int;
-    string inliers_str = IntToString(inliers_int);
-    string outliers_str = IntToString(outliers_int);
-    string n = IntToString((int)good_matches.size());
-    string text = "Found " + inliers_str + " of " + n + " matches";
-    string text2 = "Inliers: " + inliers_str + " - Outliers: " + outliers_str;
-
-    drawText(frame_vis, text, green);
-    drawText2(frame_vis, text2, red);
-
-    imshow("REAL TIME DEMO", frame_vis);
-  }
-
-  // Close and Destroy Window
-  destroyWindow("REAL TIME DEMO");
+    Mat frame, frame_vis, frame_matching;
+    while(cap.read(frame) && (char)waitKey(30) != 27) // capture frame until ESC is pressed
+    {
+        tm.reset();
+        tm.start();
+        frame_vis = frame.clone();    // refresh visualisation frame
+
+        // -- Step 1: Robust matching between model descriptors and scene descriptors
+        vector<DMatch> good_matches;       // to obtain the 3D points of the model
+        vector<KeyPoint> keypoints_scene;  // to obtain the 2D points of the scene
+
+        if(fast_match)
+        {
+            rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model, keypoints_model);
+        }
+        else
+        {
+            rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model, keypoints_model);
+        }
+
+        frame_matching = rmatcher.getImageMatching();
+        if (!frame_matching.empty())
+        {
+            imshow("Keypoints matching", frame_matching);
+        }
+
+        // -- Step 2: Find out the 2D/3D correspondences
+        vector<Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
+        vector<Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
+
+        for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
+        {
+            Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ];  // 3D point from model
+            Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 2D point from the scene
+            list_points3d_model_match.push_back(point3d_model);         // add 3D point
+            list_points2d_scene_match.push_back(point2d_scene);         // add 2D point
+        }
+
+        // Draw outliers
+        draw2DPoints(frame_vis, list_points2d_scene_match, red);
+
+        Mat inliers_idx;
+        vector<Point2f> list_points2d_inliers;
+
+        // Instantiate estimated translation and rotation
+        good_measurement = false;
+
+        if(good_matches.size() >= 4) // OpenCV requires solvePnPRANSAC to minimally have 4 set of points
+        {
+            // -- Step 3: Estimate the pose using RANSAC approach
+            pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match,
+                                              pnpMethod, inliers_idx,
+                                              iterationsCount, reprojectionError, confidence );
+
+            // -- Step 4: Catch the inliers keypoints to draw
+            for(int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index)
+            {
+                int n = inliers_idx.at<int>(inliers_index);         // i-inlier
+                Point2f point2d = list_points2d_scene_match[n];     // i-inlier point 2D
+                list_points2d_inliers.push_back(point2d);           // add i-inlier to list
+            }
+
+            // Draw inliers points 2D
+            draw2DPoints(frame_vis, list_points2d_inliers, blue);
+
+            // -- Step 5: Kalman Filter
+
+            // GOOD MEASUREMENT
+            if( inliers_idx.rows >= minInliersKalman )
+            {
+                // Get the measured translation
+                Mat translation_measured = pnp_detection.get_t_matrix();
+
+                // Get the measured rotation
+                Mat rotation_measured = pnp_detection.get_R_matrix();
+
+                // fill the measurements vector
+                fillMeasurements(measurements, translation_measured, rotation_measured);
+                good_measurement = true;
+            }
+
+            // update the Kalman filter with good measurements, otherwise with previous valid measurements
+            Mat translation_estimated(3, 1, CV_64FC1);
+            Mat rotation_estimated(3, 3, CV_64FC1);
+            updateKalmanFilter( KF, measurements,
+                                translation_estimated, rotation_estimated);
+
+            // -- Step 6: Set estimated projection matrix
+            pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
+        }
+
+        // -- Step X: Draw pose and coordinate frame
+        float l = 5;
+        vector<Point2f> pose_points2d;
+        if (!good_measurement || displayFilteredPose)
+        {
+            drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
+
+            pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,0)));  // axis center
+            pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(l,0,0)));  // axis x
+            pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,l,0)));  // axis y
+            pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,l)));  // axis z
+            draw3DCoordinateAxes(frame_vis, pose_points2d);           // draw axes
+        }
+        else
+        {
+            drawObjectMesh(frame_vis, &mesh, &pnp_detection, green);  // draw current pose
+
+            pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,0,0)));  // axis center
+            pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(l,0,0)));  // axis x
+            pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,l,0)));  // axis y
+            pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,0,l)));  // axis z
+            draw3DCoordinateAxes(frame_vis, pose_points2d);           // draw axes
+        }
+
+        // FRAME RATE
+        // see how much time has elapsed
+        tm.stop();
+
+        // calculate current FPS
+        double fps = 1.0 / tm.getTimeSec();
+
+        drawFPS(frame_vis, fps, yellow); // frame ratio
+        double detection_ratio = ((double)inliers_idx.rows/(double)good_matches.size())*100;
+        drawConfidence(frame_vis, detection_ratio, yellow);
+
+        // -- Step X: Draw some debugging text
+        // Draw some debug text
+        int inliers_int = inliers_idx.rows;
+        int outliers_int = (int)good_matches.size() - inliers_int;
+        string inliers_str = IntToString(inliers_int);
+        string outliers_str = IntToString(outliers_int);
+        string n = IntToString((int)good_matches.size());
+        string text = "Found " + inliers_str + " of " + n + " matches";
+        string text2 = "Inliers: " + inliers_str + " - Outliers: " + outliers_str;
+
+        drawText(frame_vis, text, green);
+        drawText2(frame_vis, text2, red);
+
+        imshow("REAL TIME DEMO", frame_vis);
+
+        if (!saveDirectory.empty())
+        {
+            const int widthSave = !frame_matching.empty() ? frame_matching.cols : frame_vis.cols;
+            const int heightSave = !frame_matching.empty() ? frame_matching.rows + frame_vis.rows : frame_vis.rows;
+            frameSave = Mat::zeros(heightSave, widthSave, CV_8UC3);
+            if (!frame_matching.empty())
+            {
+                int startX = (int)((widthSave - frame_vis.cols) / 2.0);
+                Mat roi = frameSave(Rect(startX, 0, frame_vis.cols, frame_vis.rows));
+                frame_vis.copyTo(roi);
+
+                roi = frameSave(Rect(0, frame_vis.rows, frame_matching.cols, frame_matching.rows));
+                frame_matching.copyTo(roi);
+            }
+            else
+            {
+                frame_vis.copyTo(frameSave);
+            }
+
+            string saveFilename = format(string(saveDirectory + "/image_%04d.png").c_str(), frameCount);
+            imwrite(saveFilename, frameSave);
+            frameCount++;
+        }
+    }
 
-  cout << "GOODBYE ..." << endl;
+    // Close and Destroy Window
+    destroyWindow("REAL TIME DEMO");
 
+    cout << "GOODBYE ..." << endl;
 }
 
 /**********************************************************************************************************/
 void help()
 {
-cout
-<< "--------------------------------------------------------------------------"   << endl
-<< "This program shows how to detect an object given its 3D textured model. You can choose to "
-<< "use a recorded video or the webcam."                                          << endl
-<< "Usage:"                                                                       << endl
-<< "./cpp-tutorial-pnp_detection -help"                                           << endl
-<< "Keys:"                                                                        << endl
-<< "'esc' - to quit."                                                             << endl
-<< "--------------------------------------------------------------------------"   << endl
-<< endl;
+    cout
+            << "--------------------------------------------------------------------------"   << endl
+            << "This program shows how to detect an object given its 3D textured model. You can choose to "
+            << "use a recorded video or the webcam."                                          << endl
+            << "Usage:"                                                                       << endl
+            << "./cpp-tutorial-pnp_detection -help"                                           << endl
+            << "Keys:"                                                                        << endl
+            << "'esc' - to quit."                                                             << endl
+            << "--------------------------------------------------------------------------"   << endl
+            << endl;
 }
 
 /**********************************************************************************************************/
 void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt)
 {
-
-  KF.init(nStates, nMeasurements, nInputs, CV_64F);                 // init Kalman Filter
-
-  setIdentity(KF.processNoiseCov, Scalar::all(1e-5));       // set process noise
-  setIdentity(KF.measurementNoiseCov, Scalar::all(1e-2));   // set measurement noise
-  setIdentity(KF.errorCovPost, Scalar::all(1));             // error covariance
-
-
-                     /** DYNAMIC MODEL **/
-
-  //  [1 0 0 dt  0  0 dt2   0   0 0 0 0  0  0  0   0   0   0]
-  //  [0 1 0  0 dt  0   0 dt2   0 0 0 0  0  0  0   0   0   0]
-  //  [0 0 1  0  0 dt   0   0 dt2 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  1  0  0  dt   0   0 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  0  1  0   0  dt   0 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  0  0  1   0   0  dt 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  0  0  0   1   0   0 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  0  0  0   0   1   0 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  0  0  0   0   0   1 0 0 0  0  0  0   0   0   0]
-  //  [0 0 0  0  0  0   0   0   0 1 0 0 dt  0  0 dt2   0   0]
-  //  [0 0 0  0  0  0   0   0   0 0 1 0  0 dt  0   0 dt2   0]
-  //  [0 0 0  0  0  0   0   0   0 0 0 1  0  0 dt   0   0 dt2]
-  //  [0 0 0  0  0  0   0   0   0 0 0 0  1  0  0  dt   0   0]
-  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  1  0   0  dt   0]
-  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  1   0   0  dt]
-  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   1   0   0]
-  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   1   0]
-  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   0   1]
-
-  // position
-  KF.transitionMatrix.at<double>(0,3) = dt;
-  KF.transitionMatrix.at<double>(1,4) = dt;
-  KF.transitionMatrix.at<double>(2,5) = dt;
-  KF.transitionMatrix.at<double>(3,6) = dt;
-  KF.transitionMatrix.at<double>(4,7) = dt;
-  KF.transitionMatrix.at<double>(5,8) = dt;
-  KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2);
-  KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2);
-  KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2);
-
-  // orientation
-  KF.transitionMatrix.at<double>(9,12) = dt;
-  KF.transitionMatrix.at<double>(10,13) = dt;
-  KF.transitionMatrix.at<double>(11,14) = dt;
-  KF.transitionMatrix.at<double>(12,15) = dt;
-  KF.transitionMatrix.at<double>(13,16) = dt;
-  KF.transitionMatrix.at<double>(14,17) = dt;
-  KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2);
-  KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2);
-  KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2);
-
-
-           /** MEASUREMENT MODEL **/
-
-  //  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
-  //  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
-  //  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
-  //  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
-  //  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
-  //  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
-
-  KF.measurementMatrix.at<double>(0,0) = 1;  // x
-  KF.measurementMatrix.at<double>(1,1) = 1;  // y
-  KF.measurementMatrix.at<double>(2,2) = 1;  // z
-  KF.measurementMatrix.at<double>(3,9) = 1;  // roll
-  KF.measurementMatrix.at<double>(4,10) = 1; // pitch
-  KF.measurementMatrix.at<double>(5,11) = 1; // yaw
-
+    KF.init(nStates, nMeasurements, nInputs, CV_64F);                 // init Kalman Filter
+
+    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));       // set process noise
+    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-2));   // set measurement noise
+    setIdentity(KF.errorCovPost, Scalar::all(1));             // error covariance
+
+    /** DYNAMIC MODEL **/
+
+    //  [1 0 0 dt  0  0 dt2   0   0 0 0 0  0  0  0   0   0   0]
+    //  [0 1 0  0 dt  0   0 dt2   0 0 0 0  0  0  0   0   0   0]
+    //  [0 0 1  0  0 dt   0   0 dt2 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  1  0  0  dt   0   0 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  0  1  0   0  dt   0 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  0  0  1   0   0  dt 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  0  0  0   1   0   0 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  0  0  0   0   1   0 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  0  0  0   0   0   1 0 0 0  0  0  0   0   0   0]
+    //  [0 0 0  0  0  0   0   0   0 1 0 0 dt  0  0 dt2   0   0]
+    //  [0 0 0  0  0  0   0   0   0 0 1 0  0 dt  0   0 dt2   0]
+    //  [0 0 0  0  0  0   0   0   0 0 0 1  0  0 dt   0   0 dt2]
+    //  [0 0 0  0  0  0   0   0   0 0 0 0  1  0  0  dt   0   0]
+    //  [0 0 0  0  0  0   0   0   0 0 0 0  0  1  0   0  dt   0]
+    //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  1   0   0  dt]
+    //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   1   0   0]
+    //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   1   0]
+    //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   0   1]
+
+    // position
+    KF.transitionMatrix.at<double>(0,3) = dt;
+    KF.transitionMatrix.at<double>(1,4) = dt;
+    KF.transitionMatrix.at<double>(2,5) = dt;
+    KF.transitionMatrix.at<double>(3,6) = dt;
+    KF.transitionMatrix.at<double>(4,7) = dt;
+    KF.transitionMatrix.at<double>(5,8) = dt;
+    KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2);
+    KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2);
+    KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2);
+
+    // orientation
+    KF.transitionMatrix.at<double>(9,12) = dt;
+    KF.transitionMatrix.at<double>(10,13) = dt;
+    KF.transitionMatrix.at<double>(11,14) = dt;
+    KF.transitionMatrix.at<double>(12,15) = dt;
+    KF.transitionMatrix.at<double>(13,16) = dt;
+    KF.transitionMatrix.at<double>(14,17) = dt;
+    KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2);
+    KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2);
+    KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2);
+
+
+    /** MEASUREMENT MODEL **/
+
+    //  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+    //  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+    //  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+    //  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
+    //  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
+    //  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
+
+    KF.measurementMatrix.at<double>(0,0) = 1;  // x
+    KF.measurementMatrix.at<double>(1,1) = 1;  // y
+    KF.measurementMatrix.at<double>(2,2) = 1;  // z
+    KF.measurementMatrix.at<double>(3,9) = 1;  // roll
+    KF.measurementMatrix.at<double>(4,10) = 1; // pitch
+    KF.measurementMatrix.at<double>(5,11) = 1; // yaw
 }
 
 /**********************************************************************************************************/
 void updateKalmanFilter( KalmanFilter &KF, Mat &measurement,
                          Mat &translation_estimated, Mat &rotation_estimated )
 {
+    // First predict, to update the internal statePre variable
+    Mat prediction = KF.predict();
 
-  // First predict, to update the internal statePre variable
-  Mat prediction = KF.predict();
-
-  // The "correct" phase that is going to use the predicted value and our measurement
-  Mat estimated = KF.correct(measurement);
-
-  // Estimated translation
-  translation_estimated.at<double>(0) = estimated.at<double>(0);
-  translation_estimated.at<double>(1) = estimated.at<double>(1);
-  translation_estimated.at<double>(2) = estimated.at<double>(2);
+    // The "correct" phase that is going to use the predicted value and our measurement
+    Mat estimated = KF.correct(measurement);
 
-  // Estimated euler angles
-  Mat eulers_estimated(3, 1, CV_64F);
-  eulers_estimated.at<double>(0) = estimated.at<double>(9);
-  eulers_estimated.at<double>(1) = estimated.at<double>(10);
-  eulers_estimated.at<double>(2) = estimated.at<double>(11);
+    // Estimated translation
+    translation_estimated.at<double>(0) = estimated.at<double>(0);
+    translation_estimated.at<double>(1) = estimated.at<double>(1);
+    translation_estimated.at<double>(2) = estimated.at<double>(2);
 
-  // Convert estimated quaternion to rotation matrix
-  rotation_estimated = euler2rot(eulers_estimated);
+    // Estimated euler angles
+    Mat eulers_estimated(3, 1, CV_64F);
+    eulers_estimated.at<double>(0) = estimated.at<double>(9);
+    eulers_estimated.at<double>(1) = estimated.at<double>(10);
+    eulers_estimated.at<double>(2) = estimated.at<double>(11);
 
+    // Convert estimated quaternion to rotation matrix
+    rotation_estimated = euler2rot(eulers_estimated);
 }
 
 /**********************************************************************************************************/
 void fillMeasurements( Mat &measurements,
                        const Mat &translation_measured, const Mat &rotation_measured)
 {
-  // Convert rotation matrix to euler angles
-  Mat measured_eulers(3, 1, CV_64F);
-  measured_eulers = rot2euler(rotation_measured);
-
-  // Set measurement to predict
-  measurements.at<double>(0) = translation_measured.at<double>(0); // x
-  measurements.at<double>(1) = translation_measured.at<double>(1); // y
-  measurements.at<double>(2) = translation_measured.at<double>(2); // z
-  measurements.at<double>(3) = measured_eulers.at<double>(0);      // roll
-  measurements.at<double>(4) = measured_eulers.at<double>(1);      // pitch
-  measurements.at<double>(5) = measured_eulers.at<double>(2);      // yaw
+    // Convert rotation matrix to euler angles
+    Mat measured_eulers(3, 1, CV_64F);
+    measured_eulers = rot2euler(rotation_measured);
+
+    // Set measurement to predict
+    measurements.at<double>(0) = translation_measured.at<double>(0); // x
+    measurements.at<double>(1) = translation_measured.at<double>(1); // y
+    measurements.at<double>(2) = translation_measured.at<double>(2); // z
+    measurements.at<double>(3) = measured_eulers.at<double>(0);      // roll
+    measurements.at<double>(4) = measured_eulers.at<double>(1);      // pitch
+    measurements.at<double>(5) = measured_eulers.at<double>(2);      // yaw
 }

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
-                          height*f/sy,  // fy
-                          width/2,      // cx
-                          height/2};    // cy
+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,211 +46,248 @@ 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 )
-  {
-      int n_regist = registration.getNumRegist();
-      int n_vertex = pts[n_regist];
-
-      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;
-      }
-  }
+    if  ( event == EVENT_LBUTTONUP )
+    {
+        bool is_registrable = registration.is_registrable();
+        if (is_registrable)
+        {
+            int n_regist = registration.getNumRegist();
+            int n_vertex = pts[n_regist];
+
+            Point2f point_2d = Point2f((float)x,(float)y);
+            Point3f point_3d = mesh.getVertex(n_vertex-1);
+
+            registration.registerPoint(point_2d, point_3d);
+            if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
+        }
+    }
 }
 
 /**  Main program  **/
-int main()
+int main(int argc, char *argv[])
 {
-
-  help();
-
-  // 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);
-  rmatcher.setFeatureDetector(detector);
-
-  /**  GROUND TRUTH OF THE FIRST IMAGE  **/
-
-  // Create & Open Window
-  namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);
-
-  // Set up the mouse events
-  setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 );
-
-  // Open the image to register
-  Mat img_in = imread(img_path, IMREAD_COLOR);
-  Mat img_vis = img_in.clone();
-
-  if (!img_in.data) {
-    cout << "Could not open or find the image" << endl;
-    return -1;
-  }
-
-  // Set the number of points to register
-  int num_registrations = n;
-  registration.setNumMax(num_registrations);
-
-  cout << "Click the box corners ..." << endl;
-  cout << "Waiting ..." << endl;
-
-  // Loop until all the points are registered
-  while ( waitKey(30) < 0 )
-  {
-    // Refresh debug image
-    img_vis = img_in.clone();
-
-    // Current registered points
-    vector<Point2f> list_points2d = registration.get_points2d();
-    vector<Point3f> list_points3d = registration.get_points3d();
-
-    // Draw current registered points
-    drawPoints(img_vis, list_points2d, list_points3d, red);
-
-    // If the registration is not finished, draw which 3D point we have to register.
-    // If the registration is finished, breaks the loop.
-    if (!end_registration)
+    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"))
     {
-      // Draw debug text
-      int n_regist = registration.getNumRegist();
-      int n_vertex = pts[n_regist];
-      Point3f current_poin3d = mesh.getVertex(n_vertex-1);
-
-      drawQuestion(img_vis, current_poin3d, green);
-      drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);
+        parser.printMessage();
+        return 0;
     }
     else
     {
-      // Draw debug text
-      drawText(img_vis, "END REGISTRATION", green);
-      drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
-      break;
+        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;
     }
 
-    // Show the image
-    imshow("MODEL REGISTRATION", img_vis);
-  }
-
-  /** COMPUTE CAMERA POSE **/
+    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;
 
-  cout << "COMPUTING POSE ..." << endl;
+    // load a mesh given the *.ply file path
+    mesh.load(ply_read_path);
 
-  // The list of registered points
-  vector<Point2f> list_points2d = registration.get_points2d();
-  vector<Point3f> list_points3d = registration.get_points3d();
+    //Instantiate robust matcher: detector, extractor, matcher
+    RobustMatcher rmatcher;
+    Ptr<Feature2D> detector, descriptor;
+    createFeatures(featureName, numKeyPoints, detector, descriptor);
+    rmatcher.setFeatureDetector(detector);
+    rmatcher.setDescriptorExtractor(descriptor);
 
-  // Estimate pose given the registered points
-  bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, SOLVEPNP_ITERATIVE);
-  if ( is_correspondence )
-  {
-    cout << "Correspondence found" << endl;
 
-    // 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);
+    /**  GROUND TRUTH OF THE FIRST IMAGE  **/
 
-  } else {
-    cout << "Correspondence not found" << endl << endl;
-  }
+    // Create & Open Window
+    namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);
 
-  // Show the image
-  imshow("MODEL REGISTRATION", img_vis);
+    // Set up the mouse events
+    setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0);
 
-  // Show image until ESC pressed
-  waitKey(0);
+    // Open the image to register
+    Mat img_in = imread(img_path, IMREAD_COLOR);
+    Mat img_vis;
 
+    if (img_in.empty()) {
+        cout << "Could not open or find the image" << endl;
+        return -1;
+    }
 
-   /** COMPUTE 3D of the image Keypoints **/
+    // Set the number of points to register
+    int num_registrations = n;
+    registration.setNumMax(num_registrations);
 
-  // Containers for keypoints and descriptors of the model
-  vector<KeyPoint> keypoints_model;
-  Mat descriptors;
+    cout << "Click the box corners ..." << endl;
+    cout << "Waiting ..." << endl;
 
-  // Compute keypoints and descriptors
-  rmatcher.computeKeyPoints(img_in, keypoints_model);
-  rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);
+    // 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);
 
-  // Check if keypoints are on the surface of the registration image and add to the model
-  for (unsigned int i = 0; i < keypoints_model.size(); ++i) {
-    Point2f point2d(keypoints_model[i].pt);
-    Point3f point3d;
-    bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
-    if (on_surface)
+    // Loop until all the points are registered
+    while ( waitKey(30) < 0 )
     {
-        model.add_correspondence(point2d, point3d);
-        model.add_descriptor(descriptors.row(i));
-        model.add_keypoint(keypoints_model[i]);
+        // Refresh debug image
+        img_vis = img_in.clone();
+
+        // Current registered points
+        vector<Point2f> list_points2d = registration.get_points2d();
+        vector<Point3f> list_points3d = registration.get_points3d();
+
+        // Draw current registered points
+        drawPoints(img_vis, list_points2d, list_points3d, red);
+
+        // If the registration is not finished, draw which 3D point we have to register.
+        // If the registration is finished, breaks the loop.
+        if (!end_registration)
+        {
+            // Draw debug text
+            int n_regist = registration.getNumRegist();
+            int n_vertex = pts[n_regist];
+            Point3f current_poin3d = mesh.getVertex(n_vertex-1);
+
+            drawQuestion(img_vis, current_poin3d, green);
+            drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);
+        }
+        else
+        {
+            // Draw debug text
+            drawText(img_vis, "END REGISTRATION", green);
+            drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
+            break;
+        }
+
+        // Show the image
+        imshow("MODEL REGISTRATION", img_vis);
     }
-    else
+
+    /** COMPUTE CAMERA POSE **/
+
+    cout << "COMPUTING POSE ..." << endl;
+
+    // The list of registered points
+    vector<Point2f> list_points2d = registration.get_points2d();
+    vector<Point3f> list_points3d = registration.get_points3d();
+
+    // Estimate pose given the registered points
+    bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, SOLVEPNP_ITERATIVE);
+    if ( is_correspondence )
     {
-        model.add_outlier(point2d);
+        cout << "Correspondence found" << endl;
+
+        // 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;
     }
-  }
 
-  // save the model into a *.yaml file
-  model.save(write_path);
+    // Show the image
+    imshow("MODEL REGISTRATION", img_vis);
 
-  // Out image
-  img_vis = img_in.clone();
+    // Show image until ESC pressed
+    waitKey(0);
+
+
+    /** COMPUTE 3D of the image Keypoints **/
+
+    // Containers for keypoints and descriptors of the model
+    vector<KeyPoint> keypoints_model;
+    Mat descriptors;
+
+    // Compute keypoints and descriptors
+    rmatcher.computeKeyPoints(img_in, keypoints_model);
+    rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);
+
+    // Check if keypoints are on the surface of the registration image and add to the model
+    for (unsigned int i = 0; i < keypoints_model.size(); ++i) {
+        Point2f point2d(keypoints_model[i].pt);
+        Point3f point3d;
+        bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
+        if (on_surface)
+        {
+            model.add_correspondence(point2d, point3d);
+            model.add_descriptor(descriptors.row(i));
+            model.add_keypoint(keypoints_model[i]);
+        }
+        else
+        {
+            model.add_outlier(point2d);
+        }
+    }
 
-  // The list of the points2d of the model
-  vector<Point2f> list_points_in = model.get_points2d_in();
-  vector<Point2f> list_points_out = model.get_points2d_out();
+    model.set_trainingImagePath(img_path);
+    // save the model into a *.yaml file
+    model.save(write_path);
 
-  // Draw some debug text
-  string num = IntToString((int)list_points_in.size());
-  string text = "There are " + num + " inliers";
-  drawText(img_vis, text, green);
+    // Out image
+    img_vis = img_in.clone();
 
-  // Draw some debug text
-  num = IntToString((int)list_points_out.size());
-  text = "There are " + num + " outliers";
-  drawText2(img_vis, text, red);
+    // The list of the points2d of the model
+    vector<Point2f> list_points_in = model.get_points2d_in();
+    vector<Point2f> list_points_out = model.get_points2d_out();
 
-  // Draw the object mesh
-  drawObjectMesh(img_vis, &mesh, &pnp_registration, blue);
+    // Draw some debug text
+    string num = IntToString((int)list_points_in.size());
+    string text = "There are " + num + " inliers";
+    drawText(img_vis, text, green);
 
-  // Draw found keypoints depending on if are or not on the surface
-  draw2DPoints(img_vis, list_points_in, green);
-  draw2DPoints(img_vis, list_points_out, red);
+    // Draw some debug text
+    num = IntToString((int)list_points_out.size());
+    text = "There are " + num + " outliers";
+    drawText2(img_vis, text, red);
 
-  // Show the image
-  imshow("MODEL REGISTRATION", img_vis);
+    // Draw the object mesh
+    drawObjectMesh(img_vis, &mesh, &pnp_registration, blue);
 
-  // Wait until ESC pressed
-  waitKey(0);
+    // Draw found keypoints depending on if are or not on the surface
+    draw2DPoints(img_vis, list_points_in, green);
+    draw2DPoints(img_vis, list_points_out, red);
 
-  // Close and Destroy Window
-  destroyWindow("MODEL REGISTRATION");
+    // Show the image
+    imshow("MODEL REGISTRATION", img_vis);
 
-  cout << "GOODBYE" << endl;
+    // Wait until ESC pressed
+    waitKey(0);
 
-}
+    // Close and Destroy Window
+    destroyWindow("MODEL REGISTRATION");
 
-/**********************************************************************************************************/
-void help()
-{
-  cout
-  << "--------------------------------------------------------------------------"   << endl
-  << "This program shows how to create your 3D textured model. "                    << endl
-  << "Usage:"                                                                       << endl
-  << "./cpp-tutorial-pnp_registration"                                              << endl
-  << "--------------------------------------------------------------------------"   << endl
-  << endl;
+    cout << "GOODBYE" << endl;
 }