diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp index c69751740b..1461877ef6 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp @@ -1,7 +1,7 @@ #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; } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h index 3e8c498960..cf54cb740c 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h +++ b/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 &list_vertex, vector > &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 &list_vertex, vector > &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 diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp index a64fb69130..10d0df479e 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp @@ -1,48 +1,45 @@ #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 &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 &list_points3d, const vector &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(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((int)i,j)); + _file << _separator << descriptor_str; + } + _file << std::endl; } - _file << std::endl; - } } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h index 06314bbd89..499fb115ec 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h @@ -1,5 +1,5 @@ #ifndef CSVWRITER_H -#define CSVWRITER_H +#define CSVWRITER_H #include #include @@ -11,15 +11,15 @@ using namespace cv; class CsvWriter { public: - CsvWriter(const string &path, const string &separator = " "); - ~CsvWriter(); - void writeXYZ(const vector &list_points3d); - void writeUVXYZ(const vector &list_points3d, const vector &list_points2d, const Mat &descriptors); + CsvWriter(const string &path, const string &separator = " "); + ~CsvWriter(); + void writeXYZ(const vector &list_points3d); + void writeUVXYZ(const vector &list_points3d, const vector &list_points2d, const Mat &descriptors); private: - ofstream _file; - string _separator; - bool _isFirstTerm; + ofstream _file; + string _separator; + bool _isFirstTerm; }; #endif diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp index e6c84c47a7..2228b6e9d9 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp +++ b/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(); - // Clear previous data - list_vertex_.clear(); - list_triangles_.clear(); - - // Read from .ply file - csvReader.readPLY(list_vertex_, list_triangles_); - - // 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(); } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h index 2ca625d3c3..40f8b7d492 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h +++ b/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 > getTrianglesList() const { return list_triangles_; } - cv::Point3f getVertex(int pos) const { return list_vertex_[pos]; } - int getNumVertices() const { return num_vertexs_; } + std::vector > 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 list_vertex_; - /* The list of triangles of the mesh */ - std::vector > 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 list_vertex_; + /* The list of triangles of the mesh */ + std::vector > list_triangles_; }; #endif /* OBJECTMESH_H_ */ diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp index 3256cef057..e2dc2a8a37 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp +++ b/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); } - -/** Save a CSV file and fill the object mesh */ -void Model::save(const std::string path) +void Model::set_trainingImagePath(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_); + training_img_path_ = path; +} - cv::FileStorage storage(path, cv::FileStorage::WRITE); - storage << "points_3d" << points3dmatrix; - storage << "points_2d" << points2dmatrix; - storage << "keypoints" << list_keypoints_; - storage << "descriptors" << descriptors_; +/** 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_); - storage.release(); + cv::FileStorage storage(path, cv::FileStorage::WRITE); + storage << "points_3d" << points3dmatrix; + storage << "points_2d" << points2dmatrix; + storage << "keypoints" << list_keypoints_; + storage << "descriptors" << descriptors_; + storage << "training_image_path" << training_img_path_; + + storage.release(); } /** Load a YAML file using OpenCv functions **/ -void Model::load(const std::string path) +void Model::load(const std::string &path) { - cv::Mat points3d_mat; + cv::Mat points3d_mat; - cv::FileStorage storage(path, cv::FileStorage::READ); - storage["points_3d"] >> points3d_mat; - storage["descriptors"] >> descriptors_; + 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(); + points3d_mat.copyTo(list_points3d_in_); + storage.release(); } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h index 79af18f0fb..7380af5d0e 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h @@ -15,40 +15,41 @@ class Model { public: - Model(); - virtual ~Model(); + Model(); + virtual ~Model(); - std::vector get_points2d_in() const { return list_points2d_in_; } - std::vector get_points2d_out() const { return list_points2d_out_; } - std::vector get_points3d() const { return list_points3d_in_; } - std::vector get_keypoints() const { return list_keypoints_; } - cv::Mat get_descriptors() const { return descriptors_; } - int get_numDescriptors() const { return descriptors_.rows; } + std::vector get_points2d_in() const { return list_points2d_in_; } + std::vector get_points2d_out() const { return list_points2d_out_; } + std::vector get_points3d() const { return list_points3d_in_; } + std::vector 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 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); - + 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 list_keypoints_; - /** The list of 2D points on the model surface */ - std::vector list_points2d_in_; - /** The list of 2D points outside the model surface */ - std::vector list_points2d_out_; - /** The list of 3D points on the model surface */ - std::vector 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 list_keypoints_; + /** The list of 2D points on the model surface */ + std::vector list_points2d_in_; + /** The list of 2D points outside the model surface */ + std::vector list_points2d_out_; + /** The list of 3D points on the model surface */ + std::vector 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_ */ diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.cpp index e8da885685..8690a345b2 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.cpp +++ b/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(); } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h index 7491ede45a..350f56d871 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h +++ b/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 get_points2d() const { return list_points2d_; } + std::vector get_points3d() const { return list_points3d_; } + int getNumMax() const { return max_registrations_; } + int getNumRegist() const { return n_registrations_; } - std::vector get_points2d() const { return list_points2d_; } - std::vector 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 list_points2d_; -/** The list of 3D points to register the model */ -std::vector 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 list_points2d_; + /** The list of 3D points to register the model */ + std::vector list_points3d_; }; #endif /* MODELREGISTRATION_H_ */ diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp index a53250ab86..c99b0eca05 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp @@ -13,122 +13,112 @@ #include -/* 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 &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 &points_list, cv::Point3f origin) +static cv::Point3f get_nearest_3D_point(std::vector &points_list, cv::Point3f origin) { - cv::Point3f p1 = points_list[0]; - cv::Point3f p2 = points_list[1]; + 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) ); + 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; - } + 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(0, 0) = params[0]; // [ fx 0 cx ] - _A_matrix.at(1, 1) = params[1]; // [ 0 fy cy ] - _A_matrix.at(0, 2) = params[2]; // [ 0 0 1 ] - _A_matrix.at(1, 2) = params[3]; - _A_matrix.at(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(0, 0) = params[0]; // [ fx 0 cx ] + A_matrix_.at(1, 1) = params[1]; // [ 0 fy cy ] + A_matrix_.at(0, 2) = params[2]; // [ 0 0 1 ] + A_matrix_.at(1, 2) = params[3]; + A_matrix_.at(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(0,0) = R_matrix.at(0,0); - _P_matrix.at(0,1) = R_matrix.at(0,1); - _P_matrix.at(0,2) = R_matrix.at(0,2); - _P_matrix.at(1,0) = R_matrix.at(1,0); - _P_matrix.at(1,1) = R_matrix.at(1,1); - _P_matrix.at(1,2) = R_matrix.at(1,2); - _P_matrix.at(2,0) = R_matrix.at(2,0); - _P_matrix.at(2,1) = R_matrix.at(2,1); - _P_matrix.at(2,2) = R_matrix.at(2,2); - _P_matrix.at(0,3) = t_matrix.at(0); - _P_matrix.at(1,3) = t_matrix.at(1); - _P_matrix.at(2,3) = t_matrix.at(2); + // Rotation-Translation Matrix Definition + P_matrix_.at(0,0) = R_matrix.at(0,0); + P_matrix_.at(0,1) = R_matrix.at(0,1); + P_matrix_.at(0,2) = R_matrix.at(0,2); + P_matrix_.at(1,0) = R_matrix.at(1,0); + P_matrix_.at(1,1) = R_matrix.at(1,1); + P_matrix_.at(1,2) = R_matrix.at(1,2); + P_matrix_.at(2,0) = R_matrix.at(2,0); + P_matrix_.at(2,1) = R_matrix.at(2,1); + P_matrix_.at(2,2) = R_matrix.at(2,2); + P_matrix_.at(0,3) = t_matrix.at(0); + P_matrix_.at(1,3) = t_matrix.at(1); + P_matrix_.at(2,3) = t_matrix.at(2); } - // Estimate the pose given a list of 2D/3D correspondences and the method to use bool PnPProblem::estimatePose( const std::vector &list_points3d, const std::vector &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 &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 PnPProblem::verify_points(Mesh *mesh) { - std::vector 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); - } + std::vector 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; + 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(0) = point3d.x; - point3d_vec.at(1) = point3d.y; - point3d_vec.at(2) = point3d.z; - point3d_vec.at(3) = 1; + // 3D point vector [x y z 1]' + cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1); + point3d_vec.at(0) = point3d.x; + point3d_vec.at(1) = point3d.y; + point3d_vec.at(2) = point3d.z; + point3d_vec.at(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; + // 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(0) / point2d_vec.at(2)); - point2d.y = (float)(point2d_vec.at(1) / point2d_vec.at(2)); + // Normalization of [u v]' + cv::Point2f point2d; + point2d.x = (float)(point2d_vec.at(0) / point2d_vec.at(2)); + point2d.y = (float)(point2d_vec.at(1) / point2d_vec.at(2)); - return point2d; + 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 > triangles_list = mesh->getTrianglesList(); + // Triangles list of the object mesh + std::vector > 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(0) = u * lambda; - point2d_vec.at(1) = v * lambda; - point2d_vec.at(2) = lambda; + // Point in vector form + cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1 + point2d_vec.at(0) = u * lambda; + point2d_vec.at(1) = v * lambda; + point2d_vec.at(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 intersections_list; + // A vector to store the intersections found + std::vector 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); - - double out; - if(this->intersect_MollerTrumbore(R, T, &out)) + // Loop for all the triangles and check the intersection + for (unsigned int i = 0; i < triangles_list.size(); i++) { - cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D - intersections_list.push_back(tmp_pt); - } - } + 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]); - // 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; - } + 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); + } + } + + // 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; } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h index 63d47c4993..b0bb31f2c7 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h +++ b/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 verify_points(Mesh *mesh); - cv::Point2f backproject3DPoint(const cv::Point3f &point3d); - bool estimatePose(const std::vector &list_points3d, const std::vector &list_points2d, int flags); - void estimatePoseRANSAC( const std::vector &list_points3d, const std::vector &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 verify_points(Mesh *mesh); + cv::Point2f backproject3DPoint(const cv::Point3f &point3d); + bool estimatePose(const std::vector &list_points3d, const std::vector &list_points2d, int flags); + void estimatePoseRANSAC( const std::vector &list_points3d, const std::vector &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_ */ diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp index 862bdd18b2..2cded40667 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp +++ b/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& keypoints) { - detector_->detect(image, keypoints); + detector_->detect(image, keypoints); } void RobustMatcher::computeDescriptors( const cv::Mat& image, std::vector& keypoints, cv::Mat& descriptors) { - extractor_->compute(image, keypoints, descriptors); + extractor_->compute(image, keypoints, descriptors); } int RobustMatcher::ratioTest(std::vector > &matches) { - int removed = 0; - // for all matches - for ( std::vector >::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 >::iterator + matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) { - // check distance ratio - if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_) - { - 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++; + } } - else - { // does not have 2 neighbours - matchIterator->clear(); // remove match - removed++; - } - } - return removed; + return removed; } void RobustMatcher::symmetryTest( const std::vector >& matches1, - const std::vector >& matches2, - std::vector& symMatches ) + const std::vector >& matches2, + std::vector& symMatches ) { - - // for all matches image 1 -> image 2 - for (std::vector >::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 >::const_iterator - matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2) - { + // for all matches image 1 -> image 2 + for (std::vector >::const_iterator + matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1) + { // ignore deleted matches - if (matchIterator2->empty() || matchIterator2->size() < 2) - continue; + if (matchIterator1->empty() || matchIterator1->size() < 2) + continue; - // Match symmetry test - if ((*matchIterator1)[0].queryIdx == - (*matchIterator2)[0].trainIdx && - (*matchIterator2)[0].queryIdx == - (*matchIterator1)[0].trainIdx) + // for all matches image 2 -> image 1 + for (std::vector >::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& good_matches, - std::vector& keypoints_frame, const cv::Mat& descriptors_model ) + std::vector& keypoints_frame, const cv::Mat& descriptors_model, + const std::vector& 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 > matches12, matches21; - // 2. Match the two image descriptors - std::vector > 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& good_matches, - std::vector& keypoints_frame, - const cv::Mat& descriptors_model ) + std::vector& keypoints_frame, + const cv::Mat& descriptors_model, + const std::vector& 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 > matches; - matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2); + // 2. Match the two image descriptors + std::vector > 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 >::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 >::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_); + } } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h index 82aab2008e..152dd7cdc9 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h +++ b/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((int)cv::NORM_HAMMING, false); + // BruteFroce matcher with Norm Hamming is the default matcher + matcher_ = cv::makePtr((int)cv::NORM_HAMMING, false); - } - virtual ~RobustMatcher(); + } + virtual ~RobustMatcher(); - // Set the feature detector - void setFeatureDetector(const cv::Ptr& detect) { detector_ = detect; } + // Set the feature detector + void setFeatureDetector(const cv::Ptr& detect) { detector_ = detect; } - // Set the descriptor extractor - void setDescriptorExtractor(const cv::Ptr& desc) { extractor_ = desc; } + // Set the descriptor extractor + void setDescriptorExtractor(const cv::Ptr& desc) { extractor_ = desc; } - // Set the matcher - void setDescriptorMatcher(const cv::Ptr& match) { matcher_ = match; } + // Set the matcher + void setDescriptorMatcher(const cv::Ptr& match) { matcher_ = match; } - // Compute the keypoints of an image - void computeKeyPoints( const cv::Mat& image, std::vector& keypoints); + // Compute the keypoints of an image + void computeKeyPoints( const cv::Mat& image, std::vector& keypoints); - // Compute the descriptors of an image given its keypoints - void computeDescriptors( const cv::Mat& image, std::vector& keypoints, cv::Mat& descriptors); + // Compute the descriptors of an image given its keypoints + void computeDescriptors( const cv::Mat& image, std::vector& 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 > &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 >& matches1, - const std::vector >& matches2, - std::vector& 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& 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 > &matches); + + // Insert symmetrical matches in symMatches vector + void symmetryTest( const std::vector >& matches1, + const std::vector >& matches2, + std::vector& symMatches ); + + // Match feature points using ratio and symmetry test + void robustMatch( const cv::Mat& frame, std::vector& good_matches, std::vector& keypoints_frame, - const cv::Mat& descriptors_model ); + const cv::Mat& descriptors_model, + const std::vector& keypoints_model); - // Match feature points using ratio test - void fastRobustMatch( const cv::Mat& frame, std::vector& good_matches, - std::vector& keypoints_frame, - const cv::Mat& descriptors_model ); + // Match feature points using ratio test + void fastRobustMatch( const cv::Mat& frame, std::vector& good_matches, + std::vector& keypoints_frame, + const cv::Mat& descriptors_model, + const std::vector& keypoints_model); private: - // pointer to the feature point detector object - cv::Ptr detector_; - // pointer to the feature descriptor extractor object - cv::Ptr extractor_; - // pointer to the matcher object - cv::Ptr matcher_; - // max ratio between 1st and 2nd NN - float ratio_; + // pointer to the feature point detector object + cv::Ptr detector_; + // pointer to the feature descriptor extractor object + cv::Ptr extractor_; + // pointer to the matcher object + cv::Ptr 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_ */ diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp index 1945a2eb8b..23ea221eb4 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp @@ -11,178 +11,180 @@ #include "ModelRegistration.h" #include "Utils.h" +#include #include #include +#include +#if defined (HAVE_OPENCV_XFEATURES2D) +#include +#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 &list_points_2d, std::vector &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]; + 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 ); + // 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 +")"; + 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); - } + 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 &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 &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::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::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 > list_triangles = mesh->getTrianglesList(); - for( size_t i = 0; i < list_triangles.size(); i++) - { - std::vector tmp_triangle = list_triangles.at(i); + std::vector > list_triangles = mesh->getTrianglesList(); + for( size_t i = 0; i < list_triangles.size(); i++) + { + std::vector 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::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::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); - } + 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); + cv::Mat euler(3,1,CV_64F); - double m00 = rotationMatrix.at(0,0); - double m02 = rotationMatrix.at(0,2); - double m10 = rotationMatrix.at(1,0); - double m11 = rotationMatrix.at(1,1); - double m12 = rotationMatrix.at(1,2); - double m20 = rotationMatrix.at(2,0); - double m22 = rotationMatrix.at(2,2); + double m00 = rotationMatrix.at(0,0); + double m02 = rotationMatrix.at(0,2); + double m10 = rotationMatrix.at(1,0); + double m11 = rotationMatrix.at(1,1); + double m12 = rotationMatrix.at(1,2); + double m20 = rotationMatrix.at(2,0); + double m22 = rotationMatrix.at(2,2); - double bank, attitude, heading; + 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); - } + // 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(0) = bank; - euler.at(1) = attitude; - euler.at(2) = heading; + euler.at(0) = bank; + euler.at(1) = attitude; + euler.at(2) = heading; - return euler; + 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); + cv::Mat rotationMatrix(3,3,CV_64F); - double bank = euler.at(0); - double attitude = euler.at(1); - double heading = euler.at(2); + double bank = euler.at(0); + double attitude = euler.at(1); + double heading = euler.at(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); + // 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; + 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; + 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(0,0) = m00; - rotationMatrix.at(0,1) = m01; - rotationMatrix.at(0,2) = m02; - rotationMatrix.at(1,0) = m10; - rotationMatrix.at(1,1) = m11; - rotationMatrix.at(1,2) = m12; - rotationMatrix.at(2,0) = m20; - rotationMatrix.at(2,1) = m21; - rotationMatrix.at(2,2) = m22; + rotationMatrix.at(0,0) = m00; + rotationMatrix.at(0,1) = m01; + rotationMatrix.at(0,2) = m02; + rotationMatrix.at(1,0) = m10; + rotationMatrix.at(1,1) = m11; + rotationMatrix.at(1,2) = m12; + rotationMatrix.at(2,0) = m20; + rotationMatrix.at(2,1) = m21; + rotationMatrix.at(2,2) = m22; - return rotationMatrix; + 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 &detector, cv::Ptr &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 createMatcher(const std::string &featureName, bool useFLANN) +{ + if (featureName == "ORB" || featureName == "BRISK" || featureName == "AKAZE" || featureName == "BINBOOST") + { + if (useFLANN) + { + cv::Ptr indexParams = cv::makePtr(6, 12, 1); // instantiate LSH index parameters + cv::Ptr searchParams = cv::makePtr(50); // instantiate flann search parameters + return cv::makePtr(indexParams, searchParams); + } + else + { + return cv::DescriptorMatcher::create("BruteForce-Hamming"); + } + + } + else + { + if (useFLANN) + { + return cv::DescriptorMatcher::create("FlannBased"); + } + else + { + return cv::DescriptorMatcher::create("BruteForce"); + } + } } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h index 8a3763a569..18a5985548 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h @@ -10,6 +10,7 @@ #include +#include #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 &detector, cv::Ptr &descriptor); + +cv::Ptr createMatcher(const std::string &featureName, bool useFLANN); + #endif /* UTILS_H_ */ diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp index 7d6ab999fe..8313d56c76 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp @@ -1,9 +1,8 @@ // C++ #include -#include // OpenCV #include -#include +#include #include #include #include @@ -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(); - 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); - 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); + 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 - if (parser.has("help")) - { - parser.printMessage(); - return 0; - } - else - { - video_read_path = parser.get("video").size() > 0 ? parser.get("video") : video_read_path; - yml_read_path = parser.get("model").size() > 0 ? parser.get("model") : yml_read_path; - ply_read_path = parser.get("mesh").size() > 0 ? parser.get("mesh") : ply_read_path; - numKeyPoints = !parser.has("keypoints") ? parser.get("keypoints") : numKeyPoints; - ratioTest = !parser.has("ratio") ? parser.get("ratio") : ratioTest; - fast_match = !parser.has("fast") ? parser.get("fast") : fast_match; - iterationsCount = !parser.has("iterations") ? parser.get("iterations") : iterationsCount; - reprojectionError = !parser.has("error") ? parser.get("error") : reprojectionError; - confidence = !parser.has("confidence") ? parser.get("confidence") : confidence; - minInliersKalman = !parser.has("inliers") ? parser.get("inliers") : minInliersKalman; - pnpMethod = !parser.has("method") ? parser.get("method") : pnpMethod; - } + // 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 - PnPProblem pnp_detection(params_WEBCAM); - PnPProblem pnp_detection_est(params_WEBCAM); + double params_WEBCAM[] = { width*f/sx, // fx + height*f/sy, // fy + width/2, // cx + height/2}; // cy - Model model; // instantiate Model object - model.load(yml_read_path); // load a 3D textured object model + // Some basic colors + Scalar red(0, 0, 255); + Scalar green(0,255,0); + Scalar blue(255,0,0); + Scalar yellow(0,255,255); - Mesh mesh; // instantiate Mesh object - mesh.load(ply_read_path); // load an object mesh + // Robust Matcher parameters + int numKeyPoints = 2000; // number of detected keypoints + float ratioTest = 0.70f; // ratio test + bool fast_match = true; // fastRobustMatch() or robustMatch() - RobustMatcher rmatcher; // instantiate RobustMatcher + // 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. - Ptr orb = ORB::create(); + // Kalman Filter parameters + int minInliersKalman = 30; // Kalman threshold updating - rmatcher.setFeatureDetector(orb); // set feature detector - rmatcher.setDescriptorExtractor(orb); // set descriptor extractor + // PnP parameters + int pnpMethod = SOLVEPNP_ITERATIVE; + string featureName = "ORB"; + bool useFLANN = false; - Ptr indexParams = makePtr(6, 12, 1); // instantiate LSH index parameters - Ptr searchParams = makePtr(50); // instantiate flann search parameters + // Save results + string saveDirectory = ""; + Mat frameSave; + int frameCount = 0; - // instantiate FlannBased matcher - Ptr matcher = makePtr(indexParams, searchParams); - rmatcher.setDescriptorMatcher(matcher); // set matcher - rmatcher.setRatio(ratioTest); // set ratio test parameter + bool displayFilteredPose = false; - 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 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 good_matches; // to obtain the 3D points of the model - vector keypoints_scene; // to obtain the 2D points of the scene - - - if(fast_match) + 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("video").size() > 0 ? parser.get("video") : video_read_path; + yml_read_path = parser.get("model").size() > 0 ? parser.get("model") : yml_read_path; + ply_read_path = parser.get("mesh").size() > 0 ? parser.get("mesh") : ply_read_path; + numKeyPoints = parser.has("keypoints") ? parser.get("keypoints") : numKeyPoints; + ratioTest = parser.has("ratio") ? parser.get("ratio") : ratioTest; + fast_match = parser.has("fast") ? parser.get("fast") : fast_match; + iterationsCount = parser.has("iterations") ? parser.get("iterations") : iterationsCount; + reprojectionError = parser.has("error") ? parser.get("error") : reprojectionError; + confidence = parser.has("confidence") ? parser.get("confidence") : confidence; + minInliersKalman = parser.has("inliers") ? parser.get("inliers") : minInliersKalman; + pnpMethod = parser.has("method") ? parser.get("method") : pnpMethod; + featureName = parser.has("feature") ? parser.get("feature") : featureName; + useFLANN = parser.has("FLANN") ? parser.get("FLANN") : useFLANN; + saveDirectory = parser.has("save") ? parser.get("save") : saveDirectory; + displayFilteredPose = parser.has("displayFiltered") ? parser.get("displayFiltered") : displayFilteredPose; } + 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; - // -- Step 2: Find out the 2D/3D correspondences + PnPProblem pnp_detection(params_WEBCAM); + PnPProblem pnp_detection_est(params_WEBCAM); - vector list_points3d_model_match; // container for the model 3D coordinates found in the scene - vector list_points2d_scene_match; // container for the model 2D coordinates found in the scene + Model model; // instantiate Model object + model.load(yml_read_path); // load a 3D textured object model - for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index) + Mesh mesh; // instantiate Mesh object + mesh.load(ply_read_path); // load an object mesh + + RobustMatcher rmatcher; // instantiate RobustMatcher + + Ptr 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); + 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; - Mat inliers_idx; - vector list_points2d_inliers; + // Get the MODEL INFO + vector 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 keypoints_model = model.get_keypoints(); - if(good_matches.size() >= 4) // OpenCV requires solvePnPRANSAC to minimally have 4 set of points + // 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 { - - // -- 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(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 = false; - - // GOOD MEASUREMENT - if( inliers_idx.rows >= minInliersKalman ) - { - - // 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); - + cout << "Could not open the camera device" << endl; + return -1; } - // -- Step X: Draw pose - - if(good_measurement) + if (!saveDirectory.empty()) { - drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose + if (!cv::utils::fs::exists(saveDirectory)) + { + std::cout << "Create directory: " << saveDirectory << std::endl; + cv::utils::fs::createDirectories(saveDirectory); + } } - else + + // Measure elapsed time + TickMeter tm; + + Mat frame, frame_vis, frame_matching; + while(cap.read(frame) && (char)waitKey(30) != 27) // capture frame until ESC is pressed { - drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose + tm.reset(); + tm.start(); + frame_vis = frame.clone(); // refresh visualisation frame + + // -- Step 1: Robust matching between model descriptors and scene descriptors + vector good_matches; // to obtain the 3D points of the model + vector 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 list_points3d_model_match; // container for the model 3D coordinates found in the scene + vector 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 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(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 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++; + } } - float l = 5; - vector 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); - - - // -- 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"); - - 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 - 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 - 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(0,3) = dt; + KF.transitionMatrix.at(1,4) = dt; + KF.transitionMatrix.at(2,5) = dt; + KF.transitionMatrix.at(3,6) = dt; + KF.transitionMatrix.at(4,7) = dt; + KF.transitionMatrix.at(5,8) = dt; + KF.transitionMatrix.at(0,6) = 0.5*pow(dt,2); + KF.transitionMatrix.at(1,7) = 0.5*pow(dt,2); + KF.transitionMatrix.at(2,8) = 0.5*pow(dt,2); + + // orientation + KF.transitionMatrix.at(9,12) = dt; + KF.transitionMatrix.at(10,13) = dt; + KF.transitionMatrix.at(11,14) = dt; + KF.transitionMatrix.at(12,15) = dt; + KF.transitionMatrix.at(13,16) = dt; + KF.transitionMatrix.at(14,17) = dt; + KF.transitionMatrix.at(9,15) = 0.5*pow(dt,2); + KF.transitionMatrix.at(10,16) = 0.5*pow(dt,2); + KF.transitionMatrix.at(11,17) = 0.5*pow(dt,2); - /** DYNAMIC MODEL **/ + /** MEASUREMENT 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(0,3) = dt; - KF.transitionMatrix.at(1,4) = dt; - KF.transitionMatrix.at(2,5) = dt; - KF.transitionMatrix.at(3,6) = dt; - KF.transitionMatrix.at(4,7) = dt; - KF.transitionMatrix.at(5,8) = dt; - KF.transitionMatrix.at(0,6) = 0.5*pow(dt,2); - KF.transitionMatrix.at(1,7) = 0.5*pow(dt,2); - KF.transitionMatrix.at(2,8) = 0.5*pow(dt,2); - - // orientation - KF.transitionMatrix.at(9,12) = dt; - KF.transitionMatrix.at(10,13) = dt; - KF.transitionMatrix.at(11,14) = dt; - KF.transitionMatrix.at(12,15) = dt; - KF.transitionMatrix.at(13,16) = dt; - KF.transitionMatrix.at(14,17) = dt; - KF.transitionMatrix.at(9,15) = 0.5*pow(dt,2); - KF.transitionMatrix.at(10,16) = 0.5*pow(dt,2); - KF.transitionMatrix.at(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(0,0) = 1; // x - KF.measurementMatrix.at(1,1) = 1; // y - KF.measurementMatrix.at(2,2) = 1; // z - KF.measurementMatrix.at(3,9) = 1; // roll - KF.measurementMatrix.at(4,10) = 1; // pitch - KF.measurementMatrix.at(5,11) = 1; // yaw + // [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(0,0) = 1; // x + KF.measurementMatrix.at(1,1) = 1; // y + KF.measurementMatrix.at(2,2) = 1; // z + KF.measurementMatrix.at(3,9) = 1; // roll + KF.measurementMatrix.at(4,10) = 1; // pitch + KF.measurementMatrix.at(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); - // The "correct" phase that is going to use the predicted value and our measurement - Mat estimated = KF.correct(measurement); + // Estimated translation + translation_estimated.at(0) = estimated.at(0); + translation_estimated.at(1) = estimated.at(1); + translation_estimated.at(2) = estimated.at(2); - // Estimated translation - translation_estimated.at(0) = estimated.at(0); - translation_estimated.at(1) = estimated.at(1); - translation_estimated.at(2) = estimated.at(2); - - // Estimated euler angles - Mat eulers_estimated(3, 1, CV_64F); - eulers_estimated.at(0) = estimated.at(9); - eulers_estimated.at(1) = estimated.at(10); - eulers_estimated.at(2) = estimated.at(11); - - // Convert estimated quaternion to rotation matrix - rotation_estimated = euler2rot(eulers_estimated); + // Estimated euler angles + Mat eulers_estimated(3, 1, CV_64F); + eulers_estimated.at(0) = estimated.at(9); + eulers_estimated.at(1) = estimated.at(10); + eulers_estimated.at(2) = estimated.at(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); + // Convert rotation matrix to euler angles + Mat measured_eulers(3, 1, CV_64F); + measured_eulers = rot2euler(rotation_measured); - // Set measurement to predict - measurements.at(0) = translation_measured.at(0); // x - measurements.at(1) = translation_measured.at(1); // y - measurements.at(2) = translation_measured.at(2); // z - measurements.at(3) = measured_eulers.at(0); // roll - measurements.at(4) = measured_eulers.at(1); // pitch - measurements.at(5) = measured_eulers.at(2); // yaw + // Set measurement to predict + measurements.at(0) = translation_measured.at(0); // x + measurements.at(1) = translation_measured.at(1); // y + measurements.at(2) = translation_measured.at(2); // z + measurements.at(3) = measured_eulers.at(0); // roll + measurements.at(4) = measured_eulers.at(1); // pitch + measurements.at(5) = measured_eulers.at(2); // yaw } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp index 5ddb83f0da..31ffb2e01e 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp +++ b/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]; + 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); + 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; - } - } + 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(); - 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); - // load a mesh given the *.ply file path - mesh.load(ply_read_path); + 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"; - // set parameters - int numKeyPoints = 10000; - - //Instantiate robust matcher: detector, extractor, matcher - RobustMatcher rmatcher; - Ptr 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 list_points2d = registration.get_points2d(); - vector 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) + 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("image").size() > 0 ? parser.get("image") : img_path; + ply_read_path = parser.get("mesh").size() > 0 ? parser.get("mesh") : ply_read_path; + write_path = parser.get("model").size() > 0 ? parser.get("model") : write_path; + numKeyPoints = parser.has("keypoints") ? parser.get("keypoints") : numKeyPoints; + featureName = parser.has("feature") ? parser.get("feature") : featureName; + } + + std::cout << "Input image: " << img_path << std::endl; + std::cout << "CAD model: " << ply_read_path << std::endl; + std::cout << "Output training file: " << write_path << std::endl; + std::cout << "Feature: " << featureName << std::endl; + std::cout << "Number of keypoints for ORB: " << numKeyPoints << std::endl; + + // load a mesh given the *.ply file path + mesh.load(ply_read_path); + + //Instantiate robust matcher: detector, extractor, matcher + RobustMatcher rmatcher; + Ptr detector, descriptor; + createFeatures(featureName, numKeyPoints, detector, descriptor); + rmatcher.setFeatureDetector(detector); + rmatcher.setDescriptorExtractor(descriptor); + + + /** 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; + + if (img_in.empty()) { + 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; + + // Some basic colors + const Scalar red(0, 0, 255); + const Scalar green(0,255,0); + const Scalar blue(255,0,0); + const Scalar yellow(0,255,255); + + // Loop until all the points are registered + while ( waitKey(30) < 0 ) + { + // Refresh debug image + img_vis = img_in.clone(); + + // Current registered points + vector list_points2d = registration.get_points2d(); + vector 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); + } + + /** COMPUTE CAMERA POSE **/ + + cout << "COMPUTING POSE ..." << endl; + + // The list of registered points + vector list_points2d = registration.get_points2d(); + vector 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 ) + { + cout << "Correspondence found" << endl; + + // Compute all the 2D points of the mesh to verify the algorithm and draw it + vector list_points2d_mesh = pnp_registration.verify_points(&mesh); + draw2DPoints(img_vis, list_points2d_mesh, green); + } else { + cout << "Correspondence not found" << endl << endl; } // Show the image imshow("MODEL REGISTRATION", img_vis); - } - /** COMPUTE CAMERA POSE **/ - - cout << "COMPUTING POSE ..." << endl; - - // The list of registered points - vector list_points2d = registration.get_points2d(); - vector 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 ) - { - cout << "Correspondence found" << endl; - - // Compute all the 2D points of the mesh to verify the algorithm and draw it - vector list_points2d_mesh = pnp_registration.verify_points(&mesh); - draw2DPoints(img_vis, list_points2d_mesh, green); - - } else { - cout << "Correspondence not found" << endl << endl; - } - - // Show the image - imshow("MODEL REGISTRATION", img_vis); - - // Show image until ESC pressed - waitKey(0); + // Show image until ESC pressed + waitKey(0); - /** COMPUTE 3D of the image Keypoints **/ + /** COMPUTE 3D of the image Keypoints **/ - // Containers for keypoints and descriptors of the model - vector keypoints_model; - Mat descriptors; + // Containers for keypoints and descriptors of the model + vector keypoints_model; + Mat descriptors; - // Compute keypoints and descriptors - rmatcher.computeKeyPoints(img_in, keypoints_model); - rmatcher.computeDescriptors(img_in, keypoints_model, 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]); + // 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); + } } - else - { - model.add_outlier(point2d); - } - } - // save the model into a *.yaml file - model.save(write_path); + model.set_trainingImagePath(img_path); + // save the model into a *.yaml file + model.save(write_path); - // Out image - img_vis = img_in.clone(); + // Out image + img_vis = img_in.clone(); - // The list of the points2d of the model - vector list_points_in = model.get_points2d_in(); - vector list_points_out = model.get_points2d_out(); + // The list of the points2d of the model + vector list_points_in = model.get_points2d_in(); + vector list_points_out = model.get_points2d_out(); - // Draw some debug text - string num = IntToString((int)list_points_in.size()); - string text = "There are " + num + " inliers"; - drawText(img_vis, text, green); + // Draw some debug text + string num = IntToString((int)list_points_in.size()); + string text = "There are " + num + " inliers"; + drawText(img_vis, text, green); - // Draw some debug text - num = IntToString((int)list_points_out.size()); - text = "There are " + num + " outliers"; - drawText2(img_vis, text, red); + // Draw some debug text + num = IntToString((int)list_points_out.size()); + text = "There are " + num + " outliers"; + drawText2(img_vis, text, red); - // Draw the object mesh - drawObjectMesh(img_vis, &mesh, &pnp_registration, blue); + // Draw the object mesh + drawObjectMesh(img_vis, &mesh, &pnp_registration, blue); - // 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 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); - // Show the image - imshow("MODEL REGISTRATION", img_vis); + // Show the image + imshow("MODEL REGISTRATION", img_vis); - // Wait until ESC pressed - waitKey(0); + // Wait until ESC pressed + waitKey(0); - // Close and Destroy Window - destroyWindow("MODEL REGISTRATION"); - - cout << "GOODBYE" << endl; + // 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; }