Skip to content

O
opencv
Commit 3c92d40f authored by catree's avatar catree
Browse files
Options

Fix arguments parsing. Add possibility to choose between different features… 

Fix arguments parsing. Add possibility to choose between different features type. Add keypoints matching visualization. Auto format code.
parent 3c70d966
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 388 additions and 378 deletions
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp
View file @ 3c92d40f
#include "CsvReader.h" #include "CsvReader.h"
/** The default constructor of the CSV reader Class */ /** 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); _file.open(path.c_str(), ifstream::in);
_separator = separator; _separator = separator;
} }
......
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h
View file @ 3c92d40f
...@@ -11,30 +11,30 @@ using namespace cv; ...@@ -11,30 +11,30 @@ using namespace cv;
class CsvReader { class CsvReader {
public: public:
/** /**
* The default constructor of the CSV reader Class. * The default constructor of the CSV reader Class.
* The default separator is ' ' (empty space) * The default separator is ' ' (empty space)
* *
* @param path - The path of the file to read * @param path - The path of the file to read
* @param separator - The separator character between words per line * @param separator - The separator character between words per line
* @return * @return
*/ */
CsvReader(const string &path, const char &separator = ' '); CsvReader(const string &path, char separator = ' ');
/** /**
* Read a plane text file with .ply format * Read a plane text file with .ply format
* *
* @param list_vertex - The container of the vertices list of the mesh * @param list_vertex - The container of the vertices list of the mesh
* @param list_triangle - The container of the triangles list of the mesh * @param list_triangle - The container of the triangles list of the mesh
* @return * @return
*/ */
void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles); void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles);
private: private:
/** The current stream file for the reader */ /** The current stream file for the reader */
ifstream _file; ifstream _file;
/** The separator character between words for each line */ /** The separator character between words for each line */
char _separator; char _separator;
}; };
#endif #endif
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp
View file @ 3c92d40f
#include "CsvWriter.h" #include "CsvWriter.h"
CsvWriter::CsvWriter(const string &path, const string &separator){ CsvWriter::CsvWriter(const string &path, const string &separator){
_file.open(path.c_str(), ofstream::out); _file.open(path.c_str(), ofstream::out);
_isFirstTerm = true; _isFirstTerm = true;
_separator = separator; _separator = separator;
} }
CsvWriter::~CsvWriter() { CsvWriter::~CsvWriter() {
_file.flush(); _file.flush();
_file.close(); _file.close();
} }
void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d) void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d)
{ {
string x, y, z; for(size_t i = 0; i < list_points3d.size(); ++i)
for(unsigned int i = 0; i < list_points3d.size(); ++i) {
{ string x = FloatToString(list_points3d[i].x);
x = FloatToString(list_points3d[i].x); string y = FloatToString(list_points3d[i].y);
y = FloatToString(list_points3d[i].y); string z = FloatToString(list_points3d[i].z);
z = FloatToString(list_points3d[i].z);
_file << x << _separator << y << _separator << z << std::endl;
}
_file << x << _separator << y << _separator << z << std::endl;
}
} }
void CsvWriter::writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors) void CsvWriter::writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors)
{ {
string u, v, x, y, z, descriptor_str; for(size_t i = 0; i < list_points3d.size(); ++i)
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)
{ {
descriptor_str = FloatToString(descriptors.at<float>(i,j)); string u = FloatToString(list_points2d[i].x);
_file << _separator << descriptor_str; string v = FloatToString(list_points2d[i].y);
string x = FloatToString(list_points3d[i].x);
string y = FloatToString(list_points3d[i].y);
string z = FloatToString(list_points3d[i].z);
_file << u << _separator << v << _separator << x << _separator << y << _separator << z;
for(int j = 0; j < 32; ++j)
{
string descriptor_str = FloatToString(descriptors.at<float>((int)i,j));
_file << _separator << descriptor_str;
}
_file << std::endl;
} }
_file << std::endl;
}
} }
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h
View file @ 3c92d40f
#ifndef CSVWRITER_H #ifndef CSVWRITER_H
#define CSVWRITER_H #define CSVWRITER_H
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
...@@ -11,15 +11,15 @@ using namespace cv; ...@@ -11,15 +11,15 @@ using namespace cv;
class CsvWriter { class CsvWriter {
public: public:
CsvWriter(const string &path, const string &separator = " "); CsvWriter(const string &path, const string &separator = " ");
~CsvWriter(); ~CsvWriter();
void writeXYZ(const vector<Point3f> &list_points3d); void writeXYZ(const vector<Point3f> &list_points3d);
void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors); void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors);
private: private:
ofstream _file; ofstream _file;
string _separator; string _separator;
bool _isFirstTerm; bool _isFirstTerm;
}; };
#endif #endif
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp
View file @ 3c92d40f
...@@ -14,15 +14,15 @@ ...@@ -14,15 +14,15 @@
// --------------------------------------------------- // // --------------------------------------------------- //
/** The custom constructor of the Triangle Class */ /** 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 */ /** The default destructor of the Class */
Triangle::~Triangle() Triangle::~Triangle()
{ {
// TODO Auto-generated destructor stub // TODO Auto-generated destructor stub
} }
...@@ -31,14 +31,15 @@ Triangle::~Triangle() ...@@ -31,14 +31,15 @@ Triangle::~Triangle()
// --------------------------------------------------- // // --------------------------------------------------- //
/** The custom constructor of the Ray Class */ /** The custom constructor of the Ray Class */
Ray::Ray(cv::Point3f P0, cv::Point3f P1) { Ray::Ray(const cv::Point3f& P0, const cv::Point3f& P1) :
p0_ = P0; p1_ = P1; p0_(P0), p1_(P1)
{
} }
/** The default destructor of the Class */ /** The default destructor of the Class */
Ray::~Ray() Ray::~Ray()
{ {
// TODO Auto-generated destructor stub // TODO Auto-generated destructor stub
} }
...@@ -47,36 +48,31 @@ Ray::~Ray() ...@@ -47,36 +48,31 @@ Ray::~Ray()
// --------------------------------------------------- // // --------------------------------------------------- //
/** The default constructor of the ObjectMesh Class */ /** 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 */ /** The default destructor of the ObjectMesh Class */
Mesh::~Mesh() Mesh::~Mesh()
{ {
// TODO Auto-generated destructor stub // TODO Auto-generated destructor stub
} }
/** Load a CSV with *.ply format **/ /** 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 // Clear previous data
CsvReader csvReader(path); 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 // Read from .ply file
num_vertexs_ = (int)list_vertex_.size(); csvReader.readPLY(list_vertex_, list_triangles_);
num_triangles_ = (int)list_triangles_.size();
// Update mesh attributes
num_vertices_ = (int)list_vertex_.size();
num_triangles_ = (int)list_triangles_.size();
} }
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h
View file @ 3c92d40f
...@@ -19,18 +19,16 @@ ...@@ -19,18 +19,16 @@
class Triangle { class Triangle {
public: public:
explicit Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2); explicit Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2);
virtual ~Triangle(); virtual ~Triangle();
cv::Point3f getV0() const { return v0_; } cv::Point3f getV0() const { return v0_; }
cv::Point3f getV1() const { return v1_; } cv::Point3f getV1() const { return v1_; }
cv::Point3f getV2() const { return v2_; } cv::Point3f getV2() const { return v2_; }
private: private:
/** The identifier number of the triangle */ /** The three vertices that defines the triangle */
int id_; cv::Point3f v0_, v1_, v2_;
/** The three vertices that defines the triangle */
cv::Point3f v0_, v1_, v2_;
}; };
...@@ -41,15 +39,15 @@ private: ...@@ -41,15 +39,15 @@ private:
class Ray { class Ray {
public: public:
explicit Ray(cv::Point3f P0, cv::Point3f P1); explicit Ray(const cv::Point3f& P0, const cv::Point3f& P1);
virtual ~Ray(); virtual ~Ray();
cv::Point3f getP0() { return p0_; } cv::Point3f getP0() { return p0_; }
cv::Point3f getP1() { return p1_; } cv::Point3f getP1() { return p1_; }
private: private:
/** The two points that defines the ray */ /** The two points that defines the ray */
cv::Point3f p0_, p1_; cv::Point3f p0_, p1_;
}; };
...@@ -61,26 +59,24 @@ class Mesh ...@@ -61,26 +59,24 @@ class Mesh
{ {
public: public:
Mesh(); Mesh();
virtual ~Mesh(); virtual ~Mesh();
std::vector<std::vector<int> > getTrianglesList() const { return list_triangles_; } std::vector<std::vector<int> > getTrianglesList() const { return list_triangles_; }
cv::Point3f getVertex(int pos) const { return list_vertex_[pos]; } cv::Point3f getVertex(int pos) const { return list_vertex_[pos]; }
int getNumVertices() const { return num_vertexs_; } int getNumVertices() const { return num_vertices_; }
void load(const std::string path_file); void load(const std::string& path_file);
private: private:
/** The identification number of the mesh */ /** The current number of vertices in the mesh */
int id_; int num_vertices_;
/** The current number of vertices in the mesh */ /** The current number of triangles in the mesh */
int num_vertexs_; int num_triangles_;
/** The current number of triangles in the mesh */ /* The list of triangles of the mesh */
int num_triangles_; std::vector<cv::Point3f> list_vertex_;
/* The list of triangles of the mesh */ /* The list of triangles of the mesh */
std::vector<cv::Point3f> list_vertex_; std::vector<std::vector<int> > list_triangles_;
/* The list of triangles of the mesh */
std::vector<std::vector<int> > list_triangles_;
}; };
#endif /* OBJECTMESH_H_ */ #endif /* OBJECTMESH_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp
View file @ 3c92d40f
...@@ -8,66 +8,76 @@ ...@@ -8,66 +8,76 @@
#include "Model.h" #include "Model.h"
#include "CsvWriter.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() Model::~Model()
{ {
// TODO Auto-generated destructor stub // TODO Auto-generated destructor stub
} }
void Model::add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d) void Model::add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d)
{ {
list_points2d_in_.push_back(point2d); list_points2d_in_.push_back(point2d);
list_points3d_in_.push_back(point3d); list_points3d_in_.push_back(point3d);
n_correspondences_++; n_correspondences_++;
} }
void Model::add_outlier(const cv::Point2f &point2d) 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) void Model::add_descriptor(const cv::Mat &descriptor)
{ {
descriptors_.push_back(descriptor); descriptors_.push_back(descriptor);
} }
void Model::add_keypoint(const cv::KeyPoint &kp) void Model::add_keypoint(const cv::KeyPoint &kp)
{ {
list_keypoints_.push_back(kp); list_keypoints_.push_back(kp);
} }
void Model::set_trainingImagePath(const std::string &path)
{
training_img_path_ = path;
}
/** Save a CSV file and fill the object mesh */ /** Save a YAML file and fill the object mesh */
void Model::save(const std::string path) void Model::save(const std::string &path)
{ {
cv::Mat points3dmatrix = cv::Mat(list_points3d_in_); cv::Mat points3dmatrix = cv::Mat(list_points3d_in_);
cv::Mat points2dmatrix = cv::Mat(list_points2d_in_); cv::Mat points2dmatrix = cv::Mat(list_points2d_in_);
//cv::Mat keyPointmatrix = cv::Mat(list_keypoints_);
cv::FileStorage storage(path, cv::FileStorage::WRITE); cv::FileStorage storage(path, cv::FileStorage::WRITE);
storage << "points_3d" << points3dmatrix; storage << "points_3d" << points3dmatrix;
storage << "points_2d" << points2dmatrix; storage << "points_2d" << points2dmatrix;
storage << "keypoints" << list_keypoints_; storage << "keypoints" << list_keypoints_;
storage << "descriptors" << descriptors_; storage << "descriptors" << descriptors_;
storage << "training_image_path" << training_img_path_;
storage.release(); storage.release();
} }
/** Load a YAML file using OpenCv functions **/ /** 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); cv::FileStorage storage(path, cv::FileStorage::READ);
storage["points_3d"] >> points3d_mat; storage["points_3d"] >> points3d_mat;
storage["descriptors"] >> descriptors_; storage["descriptors"] >> descriptors_;
if (!storage["keypoints"].empty())
points3d_mat.copyTo(list_points3d_in_); {
storage["keypoints"] >> list_keypoints_;
storage.release(); }
if (!storage["training_image_path"].empty())
{
storage["training_image_path"] >> training_img_path_;
}
points3d_mat.copyTo(list_points3d_in_);
storage.release();
} }
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h
View file @ 3c92d40f
...@@ -15,40 +15,41 @@ ...@@ -15,40 +15,41 @@
class Model class Model
{ {
public: public:
Model(); Model();
virtual ~Model(); virtual ~Model();
std::vector<cv::Point2f> get_points2d_in() const { return list_points2d_in_; } std::vector<cv::Point2f> get_points2d_in() const { return list_points2d_in_; }
std::vector<cv::Point2f> get_points2d_out() const { return list_points2d_out_; } std::vector<cv::Point2f> get_points2d_out() const { return list_points2d_out_; }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_in_; } std::vector<cv::Point3f> get_points3d() const { return list_points3d_in_; }
std::vector<cv::KeyPoint> get_keypoints() const { return list_keypoints_; } std::vector<cv::KeyPoint> get_keypoints() const { return list_keypoints_; }
cv::Mat get_descriptors() const { return descriptors_; } cv::Mat get_descriptors() const { return descriptors_; }
int get_numDescriptors() const { return descriptors_.rows; } 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_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d);
void add_outlier(const cv::Point2f &point2d); void add_outlier(const cv::Point2f &point2d);
void add_descriptor(const cv::Mat &descriptor); void add_descriptor(const cv::Mat &descriptor);
void add_keypoint(const cv::KeyPoint &kp); void add_keypoint(const cv::KeyPoint &kp);
void set_trainingImagePath(const std::string &path);
void save(const std::string path); void save(const std::string &path);
void load(const std::string path); void load(const std::string &path);
private: private:
/** The current number of correspondecnes */ /** The current number of correspondecnes */
int n_correspondences_; int n_correspondences_;
/** The list of 2D points on the model surface */ /** The list of 2D points on the model surface */
std::vector<cv::KeyPoint> list_keypoints_; std::vector<cv::KeyPoint> list_keypoints_;
/** The list of 2D points on the model surface */ /** The list of 2D points on the model surface */
std::vector<cv::Point2f> list_points2d_in_; std::vector<cv::Point2f> list_points2d_in_;
/** The list of 2D points outside the model surface */ /** The list of 2D points outside the model surface */
std::vector<cv::Point2f> list_points2d_out_; std::vector<cv::Point2f> list_points2d_out_;
/** The list of 3D points on the model surface */ /** The list of 3D points on the model surface */
std::vector<cv::Point3f> list_points3d_in_; std::vector<cv::Point3f> list_points3d_in_;
/** The list of 2D points descriptors */ /** The list of 2D points descriptors */
cv::Mat descriptors_; cv::Mat descriptors_;
/** Path to the training image */
std::string training_img_path_;
}; };
#endif /* OBJECTMODEL_H_ */ #endif /* OBJECTMODEL_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.cpp
View file @ 3c92d40f
...@@ -7,29 +7,28 @@ ...@@ -7,29 +7,28 @@
#include "ModelRegistration.h" #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() ModelRegistration::~ModelRegistration()
{ {
// TODO Auto-generated destructor stub // TODO Auto-generated destructor stub
} }
void ModelRegistration::registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d) 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_points2d_.push_back(point2d);
list_points3d_.push_back(point3d); list_points3d_.push_back(point3d);
n_registrations_++; n_registrations_++;
} }
void ModelRegistration::reset() void ModelRegistration::reset()
{ {
n_registrations_ = 0; n_registrations_ = 0;
max_registrations_ = 0; max_registrations_ = 0;
list_points2d_.clear(); list_points2d_.clear();
list_points3d_.clear(); list_points3d_.clear();
} }
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h
View file @ 3c92d40f
...@@ -14,30 +14,29 @@ ...@@ -14,30 +14,29 @@
class ModelRegistration class ModelRegistration
{ {
public: public:
ModelRegistration();
virtual ~ModelRegistration();
ModelRegistration(); void setNumMax(int n) { max_registrations_ = n; }
virtual ~ModelRegistration();
void setNumMax(int n) { max_registrations_ = n; } std::vector<cv::Point2f> get_points2d() const { return list_points2d_; }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_; }
int getNumMax() const { return max_registrations_; }
int getNumRegist() const { return n_registrations_; }
std::vector<cv::Point2f> get_points2d() const { return list_points2d_; } bool is_registrable() const { return (n_registrations_ < max_registrations_); }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_; } void registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d);
int getNumMax() const { return max_registrations_; } void reset();
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();
private: private:
/** The current number of registered points */ /** The current number of registered points */
int n_registrations_; int n_registrations_;
/** The total number of points to register */ /** The total number of points to register */
int max_registrations_; int max_registrations_;
/** The list of 2D points to register the model */ /** The list of 2D points to register the model */
std::vector<cv::Point2f> list_points2d_; std::vector<cv::Point2f> list_points2d_;
/** The list of 3D points to register the model */ /** The list of 3D points to register the model */
std::vector<cv::Point3f> list_points3d_; std::vector<cv::Point3f> list_points3d_;
}; };
#endif /* MODELREGISTRATION_H_ */ #endif /* MODELREGISTRATION_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h
View file @ 3c92d40f
...@@ -18,41 +18,35 @@ ...@@ -18,41 +18,35 @@
class PnPProblem class PnPProblem
{ {
public: public:
explicit PnPProblem(const double param[]); // custom constructor explicit PnPProblem(const double param[]); // custom constructor
virtual ~PnPProblem(); virtual ~PnPProblem();
bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d); bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d);
bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out); bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out);
std::vector<cv::Point2f> verify_points(Mesh *mesh); std::vector<cv::Point2f> verify_points(Mesh *mesh);
cv::Point2f backproject3DPoint(const cv::Point3f &point3d); cv::Point2f backproject3DPoint(const cv::Point3f &point3d);
bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags); bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags);
void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d,
int flags, cv::Mat &inliers, int flags, cv::Mat &inliers,
int iterationsCount, float reprojectionError, double confidence ); int iterationsCount, float reprojectionError, double confidence );
cv::Mat get_A_matrix() const { return _A_matrix; } cv::Mat get_A_matrix() const { return A_matrix_; }
cv::Mat get_R_matrix() const { return _R_matrix; } cv::Mat get_R_matrix() const { return R_matrix_; }
cv::Mat get_t_matrix() const { return _t_matrix; } cv::Mat get_t_matrix() const { return t_matrix_; }
cv::Mat get_P_matrix() const { return _P_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: private:
/** The calibration matrix */ /** The calibration matrix */
cv::Mat _A_matrix; cv::Mat A_matrix_;
/** The computed rotation matrix */ /** The computed rotation matrix */
cv::Mat _R_matrix; cv::Mat R_matrix_;
/** The computed translation matrix */ /** The computed translation matrix */
cv::Mat _t_matrix; cv::Mat t_matrix_;
/** The computed projection matrix */ /** The computed projection matrix */
cv::Mat _P_matrix; cv::Mat P_matrix_;
}; };
// Functions for Möller-Trumbore intersection algorithm
cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
double DOT(cv::Point3f v1, cv::Point3f v2);
cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
#endif /* PNPPROBLEM_H_ */ #endif /* PNPPROBLEM_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp
View file @ 3c92d40f
...@@ -12,141 +12,143 @@ ...@@ -12,141 +12,143 @@
RobustMatcher::~RobustMatcher() RobustMatcher::~RobustMatcher()
{ {
// TODO Auto-generated destructor stub // TODO Auto-generated destructor stub
} }
void RobustMatcher::computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints) void RobustMatcher::computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints)
{ {
detector_->detect(image, keypoints); detector_->detect(image, keypoints);
} }
void RobustMatcher::computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors) void RobustMatcher::computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)
{ {
extractor_->compute(image, keypoints, descriptors); extractor_->compute(image, keypoints, descriptors);
} }
int RobustMatcher::ratioTest(std::vector<std::vector<cv::DMatch> > &matches) int RobustMatcher::ratioTest(std::vector<std::vector<cv::DMatch> > &matches)
{ {
int removed = 0; int removed = 0;
// for all matches // for all matches
for ( std::vector<std::vector<cv::DMatch> >::iterator for ( std::vector<std::vector<cv::DMatch> >::iterator
matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
{
// if 2 NN has been identified
if (matchIterator->size() > 1)
{ {
// check distance ratio // if 2 NN has been identified
if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_) if (matchIterator->size() > 1)
{ {
matchIterator->clear(); // remove match // check distance ratio
removed++; if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_)
} {
} matchIterator->clear(); // remove match
else removed++;
{ // 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<std::vector<cv::DMatch> >& matches1, void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
const std::vector<std::vector<cv::DMatch> >& matches2, const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches ) std::vector<cv::DMatch>& symMatches )
{ {
// for all matches image 1 -> image 2
// for all matches image 1 -> image 2 for (std::vector<std::vector<cv::DMatch> >::const_iterator
for (std::vector<std::vector<cv::DMatch> >::const_iterator matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1)
matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1) {
{
// ignore deleted matches
if (matchIterator1->empty() || matchIterator1->size() < 2)
continue;
// for all matches image 2 -> image 1
for (std::vector<std::vector<cv::DMatch> >::const_iterator
matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
{
// ignore deleted matches // ignore deleted matches
if (matchIterator2->empty() || matchIterator2->size() < 2) if (matchIterator1->empty() || matchIterator1->size() < 2)
continue; continue;
// Match symmetry test // for all matches image 2 -> image 1
if ((*matchIterator1)[0].queryIdx == for (std::vector<std::vector<cv::DMatch> >::const_iterator
(*matchIterator2)[0].trainIdx && matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
(*matchIterator2)[0].queryIdx ==
(*matchIterator1)[0].trainIdx)
{ {
// add symmetrical match // ignore deleted matches
symMatches.push_back( if (matchIterator2->empty() || matchIterator2->size() < 2)
cv::DMatch((*matchIterator1)[0].queryIdx, continue;
(*matchIterator1)[0].trainIdx,
(*matchIterator1)[0].distance)); // Match symmetry test
break; // next match in image 1 -> image 2 if ((*matchIterator1)[0].queryIdx == (*matchIterator2)[0].trainIdx &&
(*matchIterator2)[0].queryIdx == (*matchIterator1)[0].trainIdx)
{
// add symmetrical match
symMatches.push_back(cv::DMatch((*matchIterator1)[0].queryIdx,
(*matchIterator1)[0].trainIdx, (*matchIterator1)[0].distance));
break; // next match in image 1 -> image 2
}
} }
} }
}
} }
void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model ) std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model)
{ {
// 1a. Detection of the ORB features
this->computeKeyPoints(frame, keypoints_frame);
// 1a. Detection of the ORB features // 1b. Extraction of the ORB descriptors
this->computeKeyPoints(frame, keypoints_frame); 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 // 2. Match the two image descriptors
std::vector<std::vector<cv::DMatch> > matches12, matches21; std::vector<std::vector<cv::DMatch> > matches12, matches21;
// 2a. From image 1 to image 2 // 2a. From image 1 to image 2
matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
// 2b. From image 2 to image 1 // 2b. From image 2 to image 1
matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours
// 3. Remove matches for which NN ratio is > than threshold // 3. Remove matches for which NN ratio is > than threshold
// clean image 1 -> image 2 matches // clean image 1 -> image 2 matches
ratioTest(matches12); ratioTest(matches12);
// clean image 2 -> image 1 matches // clean image 2 -> image 1 matches
ratioTest(matches21); ratioTest(matches21);
// 4. Remove non-symmetrical matches // 4. Remove non-symmetrical matches
symmetryTest(matches12, matches21, good_matches); symmetryTest(matches12, matches21, good_matches);
if (!training_img_.empty() && !keypoints_model.empty())
{
cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
}
} }
void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame, std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model ) const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model)
{ {
good_matches.clear(); good_matches.clear();
// 1a. Detection of the ORB features // 1a. Detection of the ORB features
this->computeKeyPoints(frame, keypoints_frame); this->computeKeyPoints(frame, keypoints_frame);
// 1b. Extraction of the ORB descriptors // 1b. Extraction of the ORB descriptors
cv::Mat descriptors_frame; cv::Mat descriptors_frame;
this->computeDescriptors(frame, keypoints_frame, descriptors_frame); this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
// 2. Match the two image descriptors // 2. Match the two image descriptors
std::vector<std::vector<cv::DMatch> > matches; std::vector<std::vector<cv::DMatch> > matches;
matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2); matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2);
// 3. Remove matches for which NN ratio is > than threshold // 3. Remove matches for which NN ratio is > than threshold
ratioTest(matches); ratioTest(matches);
// 4. Fill good matches container // 4. Fill good matches container
for ( std::vector<std::vector<cv::DMatch> >::iterator for ( std::vector<std::vector<cv::DMatch> >::iterator
matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
{ {
if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]); if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]);
} }
if (!training_img_.empty() && !keypoints_model.empty())
{
cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
}
} }
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h
View file @ 3c92d40f
...@@ -16,66 +16,77 @@ ...@@ -16,66 +16,77 @@
class RobustMatcher { class RobustMatcher {
public: public:
RobustMatcher() : ratio_(0.8f) RobustMatcher() : detector_(), extractor_(), matcher_(),
{ ratio_(0.8f), training_img_(), img_matching_()
// ORB is the default feature {
detector_ = cv::ORB::create(); // ORB is the default feature
extractor_ = cv::ORB::create(); detector_ = cv::ORB::create();
extractor_ = cv::ORB::create();
// BruteFroce matcher with Norm Hamming is the default matcher // BruteFroce matcher with Norm Hamming is the default matcher
matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false); matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false);
} }
virtual ~RobustMatcher(); virtual ~RobustMatcher();
// Set the feature detector // Set the feature detector
void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) { detector_ = detect; } void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) { detector_ = detect; }
// Set the descriptor extractor // Set the descriptor extractor
void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) { extractor_ = desc; } void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) { extractor_ = desc; }
// Set the matcher // Set the matcher
void setDescriptorMatcher(const cv::Ptr<cv::DescriptorMatcher>& match) { matcher_ = match; } void setDescriptorMatcher(const cv::Ptr<cv::DescriptorMatcher>& match) { matcher_ = match; }
// Compute the keypoints of an image // Compute the keypoints of an image
void computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints); void computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints);
// Compute the descriptors of an image given its keypoints // Compute the descriptors of an image given its keypoints
void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors); void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
// Set ratio parameter for the ratio test cv::Mat getImageMatching() const { return img_matching_; }
void setRatio( float rat) { ratio_ = rat; }
// Clear matches for which NN ratio is > than threshold // Set ratio parameter for the ratio test
// return the number of removed points void setRatio( float rat) { ratio_ = rat; }
// (corresponding entries being cleared,
// i.e. size will be 0)
int ratioTest(std::vector<std::vector<cv::DMatch> > &matches);
// Insert symmetrical matches in symMatches vector void setTrainingImage(const cv::Mat &img) { training_img_ = img; }
void symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches );
// Match feature points using ratio and symmetry test // Clear matches for which NN ratio is > than threshold
void robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, // return the number of removed points
// (corresponding entries being cleared,
// i.e. size will be 0)
int ratioTest(std::vector<std::vector<cv::DMatch> > &matches);
// Insert symmetrical matches in symMatches vector
void symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches );
// Match feature points using ratio and symmetry test
void robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame, std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model ); const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model);
// Match feature points using ratio test // Match feature points using ratio test
void fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, void fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame, std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model ); const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model);
private: private:
// pointer to the feature point detector object // pointer to the feature point detector object
cv::Ptr<cv::FeatureDetector> detector_; cv::Ptr<cv::FeatureDetector> detector_;
// pointer to the feature descriptor extractor object // pointer to the feature descriptor extractor object
cv::Ptr<cv::DescriptorExtractor> extractor_; cv::Ptr<cv::DescriptorExtractor> extractor_;
// pointer to the matcher object // pointer to the matcher object
cv::Ptr<cv::DescriptorMatcher> matcher_; cv::Ptr<cv::DescriptorMatcher> matcher_;
// max ratio between 1st and 2nd NN // max ratio between 1st and 2nd NN
float ratio_; float ratio_;
// training image
cv::Mat training_img_;
// matching image
cv::Mat img_matching_;
}; };
#endif /* ROBUSTMATCHER_H_ */ #endif /* ROBUSTMATCHER_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h
View file @ 3c92d40f
...@@ -10,6 +10,7 @@ ...@@ -10,6 +10,7 @@
#include <iostream> #include <iostream>
#include <opencv2/features2d.hpp>
#include "PnPProblem.h" #include "PnPProblem.h"
// Draw a text with the question point // Draw a text with the question point
...@@ -66,4 +67,8 @@ std::string FloatToString ( float Number ); ...@@ -66,4 +67,8 @@ std::string FloatToString ( float Number );
// Converts a given integer to a string // Converts a given integer to a string
std::string IntToString ( int Number ); std::string IntToString ( int Number );
void createFeatures(const std::string &featureName, int numKeypoints, cv::Ptr<cv::Feature2D> &detector, cv::Ptr<cv::Feature2D> &descriptor);
cv::Ptr<cv::DescriptorMatcher> createMatcher(const std::string &featureName, bool useFLANN);
#endif /* UTILS_H_ */ #endif /* UTILS_H_ */
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment