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
Hide whitespace changes
Inline Side-by-side
Showing with 1490 additions and 1334 deletions
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp
View file @ 3c92d40f
#include "CsvReader.h"
/** The default constructor of the CSV reader Class */
CsvReader::CsvReader(const string &path, const char &separator){
CsvReader::CsvReader(const string &path, char separator){
_file.open(path.c_str(), ifstream::in);
_separator = separator;
}
......
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h
View file @ 3c92d40f
......@@ -11,30 +11,30 @@ using namespace cv;
class CsvReader {
public:
/**
* The default constructor of the CSV reader Class.
* The default separator is ' ' (empty space)
*
* @param path - The path of the file to read
* @param separator - The separator character between words per line
* @return
*/
CsvReader(const string &path, const char &separator = ' ');
/**
* The default constructor of the CSV reader Class.
* The default separator is ' ' (empty space)
*
* @param path - The path of the file to read
* @param separator - The separator character between words per line
* @return
*/
CsvReader(const string &path, char separator = ' ');
/**
* Read a plane text file with .ply format
*
* @param list_vertex - The container of the vertices list of the mesh
* @param list_triangle - The container of the triangles list of the mesh
* @return
*/
void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles);
/**
* Read a plane text file with .ply format
*
* @param list_vertex - The container of the vertices list of the mesh
* @param list_triangle - The container of the triangles list of the mesh
* @return
*/
void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles);
private:
/** The current stream file for the reader */
ifstream _file;
/** The separator character between words for each line */
char _separator;
/** The current stream file for the reader */
ifstream _file;
/** The separator character between words for each line */
char _separator;
};
#endif
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp
View file @ 3c92d40f
#include "CsvWriter.h"
CsvWriter::CsvWriter(const string &path, const string &separator){
_file.open(path.c_str(), ofstream::out);
_isFirstTerm = true;
_separator = separator;
_file.open(path.c_str(), ofstream::out);
_isFirstTerm = true;
_separator = separator;
}
CsvWriter::~CsvWriter() {
_file.flush();
_file.close();
_file.flush();
_file.close();
}
void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d)
{
string x, y, z;
for(unsigned int i = 0; i < list_points3d.size(); ++i)
{
x = FloatToString(list_points3d[i].x);
y = FloatToString(list_points3d[i].y);
z = FloatToString(list_points3d[i].z);
_file << x << _separator << y << _separator << z << std::endl;
}
for(size_t i = 0; i < list_points3d.size(); ++i)
{
string x = FloatToString(list_points3d[i].x);
string y = FloatToString(list_points3d[i].y);
string z = FloatToString(list_points3d[i].z);
_file << x << _separator << y << _separator << z << std::endl;
}
}
void CsvWriter::writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors)
{
string u, v, x, y, z, descriptor_str;
for(unsigned int i = 0; i < list_points3d.size(); ++i)
{
u = FloatToString(list_points2d[i].x);
v = FloatToString(list_points2d[i].y);
x = FloatToString(list_points3d[i].x);
y = FloatToString(list_points3d[i].y);
z = FloatToString(list_points3d[i].z);
_file << u << _separator << v << _separator << x << _separator << y << _separator << z;
for(int j = 0; j < 32; ++j)
for(size_t i = 0; i < list_points3d.size(); ++i)
{
descriptor_str = FloatToString(descriptors.at<float>(i,j));
_file << _separator << descriptor_str;
string u = FloatToString(list_points2d[i].x);
string v = FloatToString(list_points2d[i].y);
string x = FloatToString(list_points3d[i].x);
string y = FloatToString(list_points3d[i].y);
string z = FloatToString(list_points3d[i].z);
_file << u << _separator << v << _separator << x << _separator << y << _separator << z;
for(int j = 0; j < 32; ++j)
{
string descriptor_str = FloatToString(descriptors.at<float>((int)i,j));
_file << _separator << descriptor_str;
}
_file << std::endl;
}
_file << std::endl;
}
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h
View file @ 3c92d40f
#ifndef CSVWRITER_H
#define CSVWRITER_H
#define CSVWRITER_H
#include <iostream>
#include <fstream>
......@@ -11,15 +11,15 @@ using namespace cv;
class CsvWriter {
public:
CsvWriter(const string &path, const string &separator = " ");
~CsvWriter();
void writeXYZ(const vector<Point3f> &list_points3d);
void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors);
CsvWriter(const string &path, const string &separator = " ");
~CsvWriter();
void writeXYZ(const vector<Point3f> &list_points3d);
void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors);
private:
ofstream _file;
string _separator;
bool _isFirstTerm;
ofstream _file;
string _separator;
bool _isFirstTerm;
};
#endif
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp
View file @ 3c92d40f
......@@ -14,15 +14,15 @@
// --------------------------------------------------- //
/** The custom constructor of the Triangle Class */
Triangle::Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2)
Triangle::Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2) :
v0_(V0), v1_(V1), v2_(V2)
{
id_ = id; v0_ = V0; v1_ = V1; v2_ = V2;
}
/** The default destructor of the Class */
Triangle::~Triangle()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
......@@ -31,14 +31,15 @@ Triangle::~Triangle()
// --------------------------------------------------- //
/** The custom constructor of the Ray Class */
Ray::Ray(cv::Point3f P0, cv::Point3f P1) {
p0_ = P0; p1_ = P1;
Ray::Ray(const cv::Point3f& P0, const cv::Point3f& P1) :
p0_(P0), p1_(P1)
{
}
/** The default destructor of the Class */
Ray::~Ray()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
......@@ -47,36 +48,31 @@ Ray::~Ray()
// --------------------------------------------------- //
/** The default constructor of the ObjectMesh Class */
Mesh::Mesh() : list_vertex_(0) , list_triangles_(0)
Mesh::Mesh() : num_vertices_(0), num_triangles_(0),
list_vertex_(0) , list_triangles_(0)
{
id_ = 0;
num_vertexs_ = 0;
num_triangles_ = 0;
}
/** The default destructor of the ObjectMesh Class */
Mesh::~Mesh()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
/** Load a CSV with *.ply format **/
void Mesh::load(const std::string path)
void Mesh::load(const std::string& path)
{
// Create the reader
CsvReader csvReader(path);
// Create the reader
CsvReader csvReader(path);
// Clear previous data
list_vertex_.clear();
list_triangles_.clear();
// Read from .ply file
csvReader.readPLY(list_vertex_, list_triangles_);
// Clear previous data
list_vertex_.clear();
list_triangles_.clear();
// Update mesh attributes
num_vertexs_ = (int)list_vertex_.size();
num_triangles_ = (int)list_triangles_.size();
// Read from .ply file
csvReader.readPLY(list_vertex_, list_triangles_);
// Update mesh attributes
num_vertices_ = (int)list_vertex_.size();
num_triangles_ = (int)list_triangles_.size();
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h
View file @ 3c92d40f
......@@ -19,18 +19,16 @@
class Triangle {
public:
explicit Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2);
virtual ~Triangle();
explicit Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2);
virtual ~Triangle();
cv::Point3f getV0() const { return v0_; }
cv::Point3f getV1() const { return v1_; }
cv::Point3f getV2() const { return v2_; }
cv::Point3f getV0() const { return v0_; }
cv::Point3f getV1() const { return v1_; }
cv::Point3f getV2() const { return v2_; }
private:
/** The identifier number of the triangle */
int id_;
/** The three vertices that defines the triangle */
cv::Point3f v0_, v1_, v2_;
/** The three vertices that defines the triangle */
cv::Point3f v0_, v1_, v2_;
};
......@@ -41,15 +39,15 @@ private:
class Ray {
public:
explicit Ray(cv::Point3f P0, cv::Point3f P1);
virtual ~Ray();
explicit Ray(const cv::Point3f& P0, const cv::Point3f& P1);
virtual ~Ray();
cv::Point3f getP0() { return p0_; }
cv::Point3f getP1() { return p1_; }
cv::Point3f getP0() { return p0_; }
cv::Point3f getP1() { return p1_; }
private:
/** The two points that defines the ray */
cv::Point3f p0_, p1_;
/** The two points that defines the ray */
cv::Point3f p0_, p1_;
};
......@@ -61,26 +59,24 @@ class Mesh
{
public:
Mesh();
virtual ~Mesh();
Mesh();
virtual ~Mesh();
std::vector<std::vector<int> > getTrianglesList() const { return list_triangles_; }
cv::Point3f getVertex(int pos) const { return list_vertex_[pos]; }
int getNumVertices() const { return num_vertexs_; }
std::vector<std::vector<int> > getTrianglesList() const { return list_triangles_; }
cv::Point3f getVertex(int pos) const { return list_vertex_[pos]; }
int getNumVertices() const { return num_vertices_; }
void load(const std::string path_file);
void load(const std::string& path_file);
private:
/** The identification number of the mesh */
int id_;
/** The current number of vertices in the mesh */
int num_vertexs_;
/** The current number of triangles in the mesh */
int num_triangles_;
/* The list of triangles of the mesh */
std::vector<cv::Point3f> list_vertex_;
/* The list of triangles of the mesh */
std::vector<std::vector<int> > list_triangles_;
/** The current number of vertices in the mesh */
int num_vertices_;
/** The current number of triangles in the mesh */
int num_triangles_;
/* The list of triangles of the mesh */
std::vector<cv::Point3f> list_vertex_;
/* The list of triangles of the mesh */
std::vector<std::vector<int> > list_triangles_;
};
#endif /* OBJECTMESH_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp
View file @ 3c92d40f
......@@ -8,66 +8,76 @@
#include "Model.h"
#include "CsvWriter.h"
Model::Model() : list_points2d_in_(0), list_points2d_out_(0), list_points3d_in_(0)
Model::Model() : n_correspondences_(0), list_points2d_in_(0), list_points2d_out_(0), list_points3d_in_(0), training_img_path_()
{
n_correspondences_ = 0;
}
Model::~Model()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
void Model::add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d)
{
list_points2d_in_.push_back(point2d);
list_points3d_in_.push_back(point3d);
n_correspondences_++;
list_points2d_in_.push_back(point2d);
list_points3d_in_.push_back(point3d);
n_correspondences_++;
}
void Model::add_outlier(const cv::Point2f &point2d)
{
list_points2d_out_.push_back(point2d);
list_points2d_out_.push_back(point2d);
}
void Model::add_descriptor(const cv::Mat &descriptor)
{
descriptors_.push_back(descriptor);
descriptors_.push_back(descriptor);
}
void Model::add_keypoint(const cv::KeyPoint &kp)
{
list_keypoints_.push_back(kp);
list_keypoints_.push_back(kp);
}
void Model::set_trainingImagePath(const std::string &path)
{
training_img_path_ = path;
}
/** Save a CSV file and fill the object mesh */
void Model::save(const std::string path)
/** Save a YAML file and fill the object mesh */
void Model::save(const std::string &path)
{
cv::Mat points3dmatrix = cv::Mat(list_points3d_in_);
cv::Mat points2dmatrix = cv::Mat(list_points2d_in_);
//cv::Mat keyPointmatrix = cv::Mat(list_keypoints_);
cv::Mat points3dmatrix = cv::Mat(list_points3d_in_);
cv::Mat points2dmatrix = cv::Mat(list_points2d_in_);
cv::FileStorage storage(path, cv::FileStorage::WRITE);
storage << "points_3d" << points3dmatrix;
storage << "points_2d" << points2dmatrix;
storage << "keypoints" << list_keypoints_;
storage << "descriptors" << descriptors_;
cv::FileStorage storage(path, cv::FileStorage::WRITE);
storage << "points_3d" << points3dmatrix;
storage << "points_2d" << points2dmatrix;
storage << "keypoints" << list_keypoints_;
storage << "descriptors" << descriptors_;
storage << "training_image_path" << training_img_path_;
storage.release();
storage.release();
}
/** Load a YAML file using OpenCv functions **/
void Model::load(const std::string path)
void Model::load(const std::string &path)
{
cv::Mat points3d_mat;
cv::FileStorage storage(path, cv::FileStorage::READ);
storage["points_3d"] >> points3d_mat;
storage["descriptors"] >> descriptors_;
points3d_mat.copyTo(list_points3d_in_);
storage.release();
cv::Mat points3d_mat;
cv::FileStorage storage(path, cv::FileStorage::READ);
storage["points_3d"] >> points3d_mat;
storage["descriptors"] >> descriptors_;
if (!storage["keypoints"].empty())
{
storage["keypoints"] >> list_keypoints_;
}
if (!storage["training_image_path"].empty())
{
storage["training_image_path"] >> training_img_path_;
}
points3d_mat.copyTo(list_points3d_in_);
storage.release();
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h
View file @ 3c92d40f
......@@ -15,40 +15,41 @@
class Model
{
public:
Model();
virtual ~Model();
std::vector<cv::Point2f> get_points2d_in() const { return list_points2d_in_; }
std::vector<cv::Point2f> get_points2d_out() const { return list_points2d_out_; }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_in_; }
std::vector<cv::KeyPoint> get_keypoints() const { return list_keypoints_; }
cv::Mat get_descriptors() const { return descriptors_; }
int get_numDescriptors() const { return descriptors_.rows; }
void add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d);
void add_outlier(const cv::Point2f &point2d);
void add_descriptor(const cv::Mat &descriptor);
void add_keypoint(const cv::KeyPoint &kp);
void save(const std::string path);
void load(const std::string path);
Model();
virtual ~Model();
std::vector<cv::Point2f> get_points2d_in() const { return list_points2d_in_; }
std::vector<cv::Point2f> get_points2d_out() const { return list_points2d_out_; }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_in_; }
std::vector<cv::KeyPoint> get_keypoints() const { return list_keypoints_; }
cv::Mat get_descriptors() const { return descriptors_; }
int get_numDescriptors() const { return descriptors_.rows; }
std::string get_trainingImagePath() const { return training_img_path_; }
void add_correspondence(const cv::Point2f &point2d, const cv::Point3f &point3d);
void add_outlier(const cv::Point2f &point2d);
void add_descriptor(const cv::Mat &descriptor);
void add_keypoint(const cv::KeyPoint &kp);
void set_trainingImagePath(const std::string &path);
void save(const std::string &path);
void load(const std::string &path);
private:
/** The current number of correspondecnes */
int n_correspondences_;
/** The list of 2D points on the model surface */
std::vector<cv::KeyPoint> list_keypoints_;
/** The list of 2D points on the model surface */
std::vector<cv::Point2f> list_points2d_in_;
/** The list of 2D points outside the model surface */
std::vector<cv::Point2f> list_points2d_out_;
/** The list of 3D points on the model surface */
std::vector<cv::Point3f> list_points3d_in_;
/** The list of 2D points descriptors */
cv::Mat descriptors_;
/** The current number of correspondecnes */
int n_correspondences_;
/** The list of 2D points on the model surface */
std::vector<cv::KeyPoint> list_keypoints_;
/** The list of 2D points on the model surface */
std::vector<cv::Point2f> list_points2d_in_;
/** The list of 2D points outside the model surface */
std::vector<cv::Point2f> list_points2d_out_;
/** The list of 3D points on the model surface */
std::vector<cv::Point3f> list_points3d_in_;
/** The list of 2D points descriptors */
cv::Mat descriptors_;
/** Path to the training image */
std::string training_img_path_;
};
#endif /* OBJECTMODEL_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.cpp
View file @ 3c92d40f
......@@ -7,29 +7,28 @@
#include "ModelRegistration.h"
ModelRegistration::ModelRegistration()
ModelRegistration::ModelRegistration() : n_registrations_(0), max_registrations_(0),
list_points2d_(), list_points3d_()
{
n_registrations_ = 0;
max_registrations_ = 0;
}
ModelRegistration::~ModelRegistration()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
void ModelRegistration::registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d)
{
// add correspondence at the end of the vector
{
// add correspondence at the end of the vector
list_points2d_.push_back(point2d);
list_points3d_.push_back(point3d);
n_registrations_++;
}
}
void ModelRegistration::reset()
{
n_registrations_ = 0;
max_registrations_ = 0;
list_points2d_.clear();
list_points3d_.clear();
n_registrations_ = 0;
max_registrations_ = 0;
list_points2d_.clear();
list_points3d_.clear();
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h
View file @ 3c92d40f
......@@ -14,30 +14,29 @@
class ModelRegistration
{
public:
ModelRegistration();
virtual ~ModelRegistration();
ModelRegistration();
virtual ~ModelRegistration();
void setNumMax(int n) { max_registrations_ = n; }
void setNumMax(int n) { max_registrations_ = n; }
std::vector<cv::Point2f> get_points2d() const { return list_points2d_; }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_; }
int getNumMax() const { return max_registrations_; }
int getNumRegist() const { return n_registrations_; }
std::vector<cv::Point2f> get_points2d() const { return list_points2d_; }
std::vector<cv::Point3f> get_points3d() const { return list_points3d_; }
int getNumMax() const { return max_registrations_; }
int getNumRegist() const { return n_registrations_; }
bool is_registrable() const { return (n_registrations_ < max_registrations_); }
void registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d);
void reset();
bool is_registrable() const { return (n_registrations_ < max_registrations_); }
void registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d);
void reset();
private:
/** The current number of registered points */
int n_registrations_;
/** The total number of points to register */
int max_registrations_;
/** The list of 2D points to register the model */
std::vector<cv::Point2f> list_points2d_;
/** The list of 3D points to register the model */
std::vector<cv::Point3f> list_points3d_;
/** The current number of registered points */
int n_registrations_;
/** The total number of points to register */
int max_registrations_;
/** The list of 2D points to register the model */
std::vector<cv::Point2f> list_points2d_;
/** The list of 3D points to register the model */
std::vector<cv::Point3f> list_points3d_;
};
#endif /* MODELREGISTRATION_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp
View file @ 3c92d40f
......@@ -13,122 +13,112 @@
#include <opencv2/calib3d/calib3d.hpp>
/* Functions headers */
cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
double DOT(cv::Point3f v1, cv::Point3f v2);
cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin);
/* Functions for Möller-Trumbore intersection algorithm */
cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
static cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
{
cv::Point3f tmp_p;
tmp_p.x = v1.y*v2.z - v1.z*v2.y;
tmp_p.y = v1.z*v2.x - v1.x*v2.z;
tmp_p.z = v1.x*v2.y - v1.y*v2.x;
return tmp_p;
cv::Point3f tmp_p;
tmp_p.x = v1.y*v2.z - v1.z*v2.y;
tmp_p.y = v1.z*v2.x - v1.x*v2.z;
tmp_p.z = v1.x*v2.y - v1.y*v2.x;
return tmp_p;
}
double DOT(cv::Point3f v1, cv::Point3f v2)
static double DOT(cv::Point3f v1, cv::Point3f v2)
{
return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}
cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
static cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
{
cv::Point3f tmp_p;
tmp_p.x = v1.x - v2.x;
tmp_p.y = v1.y - v2.y;
tmp_p.z = v1.z - v2.z;
return tmp_p;
cv::Point3f tmp_p;
tmp_p.x = v1.x - v2.x;
tmp_p.y = v1.y - v2.y;
tmp_p.z = v1.z - v2.z;
return tmp_p;
}
/* End functions for Möller-Trumbore intersection algorithm
* */
/* End functions for Möller-Trumbore intersection algorithm */
// Function to get the nearest 3D point to the Ray origin
cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin)
static cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin)
{
cv::Point3f p1 = points_list[0];
cv::Point3f p2 = points_list[1];
double d1 = std::sqrt( std::pow(p1.x-origin.x, 2) + std::pow(p1.y-origin.y, 2) + std::pow(p1.z-origin.z, 2) );
double d2 = std::sqrt( std::pow(p2.x-origin.x, 2) + std::pow(p2.y-origin.y, 2) + std::pow(p2.z-origin.z, 2) );
if(d1 < d2)
{
return p1;
}
else
{
return p2;
}
cv::Point3f p1 = points_list[0];
cv::Point3f p2 = points_list[1];
double d1 = std::sqrt( std::pow(p1.x-origin.x, 2) + std::pow(p1.y-origin.y, 2) + std::pow(p1.z-origin.z, 2) );
double d2 = std::sqrt( std::pow(p2.x-origin.x, 2) + std::pow(p2.y-origin.y, 2) + std::pow(p2.z-origin.z, 2) );
if(d1 < d2)
{
return p1;
}
else
{
return p2;
}
}
// Custom constructor given the intrinsic camera parameters
PnPProblem::PnPProblem(const double params[])
{
_A_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // intrinsic camera parameters
_A_matrix.at<double>(0, 0) = params[0]; // [ fx 0 cx ]
_A_matrix.at<double>(1, 1) = params[1]; // [ 0 fy cy ]
_A_matrix.at<double>(0, 2) = params[2]; // [ 0 0 1 ]
_A_matrix.at<double>(1, 2) = params[3];
_A_matrix.at<double>(2, 2) = 1;
_R_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // rotation matrix
_t_matrix = cv::Mat::zeros(3, 1, CV_64FC1); // translation matrix
_P_matrix = cv::Mat::zeros(3, 4, CV_64FC1); // rotation-translation matrix
A_matrix_ = cv::Mat::zeros(3, 3, CV_64FC1); // intrinsic camera parameters
A_matrix_.at<double>(0, 0) = params[0]; // [ fx 0 cx ]
A_matrix_.at<double>(1, 1) = params[1]; // [ 0 fy cy ]
A_matrix_.at<double>(0, 2) = params[2]; // [ 0 0 1 ]
A_matrix_.at<double>(1, 2) = params[3];
A_matrix_.at<double>(2, 2) = 1;
R_matrix_ = cv::Mat::zeros(3, 3, CV_64FC1); // rotation matrix
t_matrix_ = cv::Mat::zeros(3, 1, CV_64FC1); // translation matrix
P_matrix_ = cv::Mat::zeros(3, 4, CV_64FC1); // rotation-translation matrix
}
PnPProblem::~PnPProblem()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
void PnPProblem::set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix)
{
// Rotation-Translation Matrix Definition
_P_matrix.at<double>(0,0) = R_matrix.at<double>(0,0);
_P_matrix.at<double>(0,1) = R_matrix.at<double>(0,1);
_P_matrix.at<double>(0,2) = R_matrix.at<double>(0,2);
_P_matrix.at<double>(1,0) = R_matrix.at<double>(1,0);
_P_matrix.at<double>(1,1) = R_matrix.at<double>(1,1);
_P_matrix.at<double>(1,2) = R_matrix.at<double>(1,2);
_P_matrix.at<double>(2,0) = R_matrix.at<double>(2,0);
_P_matrix.at<double>(2,1) = R_matrix.at<double>(2,1);
_P_matrix.at<double>(2,2) = R_matrix.at<double>(2,2);
_P_matrix.at<double>(0,3) = t_matrix.at<double>(0);
_P_matrix.at<double>(1,3) = t_matrix.at<double>(1);
_P_matrix.at<double>(2,3) = t_matrix.at<double>(2);
// Rotation-Translation Matrix Definition
P_matrix_.at<double>(0,0) = R_matrix.at<double>(0,0);
P_matrix_.at<double>(0,1) = R_matrix.at<double>(0,1);
P_matrix_.at<double>(0,2) = R_matrix.at<double>(0,2);
P_matrix_.at<double>(1,0) = R_matrix.at<double>(1,0);
P_matrix_.at<double>(1,1) = R_matrix.at<double>(1,1);
P_matrix_.at<double>(1,2) = R_matrix.at<double>(1,2);
P_matrix_.at<double>(2,0) = R_matrix.at<double>(2,0);
P_matrix_.at<double>(2,1) = R_matrix.at<double>(2,1);
P_matrix_.at<double>(2,2) = R_matrix.at<double>(2,2);
P_matrix_.at<double>(0,3) = t_matrix.at<double>(0);
P_matrix_.at<double>(1,3) = t_matrix.at<double>(1);
P_matrix_.at<double>(2,3) = t_matrix.at<double>(2);
}
// Estimate the pose given a list of 2D/3D correspondences and the method to use
bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
const std::vector<cv::Point2f> &list_points2d,
int flags)
{
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
bool useExtrinsicGuess = false;
bool useExtrinsicGuess = false;
// Pose estimation
bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
useExtrinsicGuess, flags);
// Pose estimation
bool correspondence = cv::solvePnP( list_points3d, list_points2d, A_matrix_, distCoeffs, rvec, tvec,
useExtrinsicGuess, flags);
// Transforms Rotation Vector to Matrix
Rodrigues(rvec,_R_matrix);
_t_matrix = tvec;
// Transforms Rotation Vector to Matrix
Rodrigues(rvec, R_matrix_);
t_matrix_ = tvec;
// Set projection matrix
this->set_P_matrix(_R_matrix, _t_matrix);
// Set projection matrix
this->set_P_matrix(R_matrix_, t_matrix_);
return correspondence;
return correspondence;
}
// Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use
......@@ -138,182 +128,180 @@ void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points
int flags, cv::Mat &inliers, int iterationsCount, // PnP method; inliers container
float reprojectionError, double confidence ) // Ransac parameters
{
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1); // vector of distortion coefficients
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1); // vector of distortion coefficients
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector
bool useExtrinsicGuess = false; // if true the function uses the provided rvec and tvec values as
// initial approximations of the rotation and translation vectors
bool useExtrinsicGuess = false; // if true the function uses the provided rvec and tvec values as
// initial approximations of the rotation and translation vectors
cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
inliers, flags );
cv::solvePnPRansac( list_points3d, list_points2d, A_matrix_, distCoeffs, rvec, tvec,
useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
inliers, flags );
Rodrigues(rvec,_R_matrix); // converts Rotation Vector to Matrix
_t_matrix = tvec; // set translation matrix
Rodrigues(rvec, R_matrix_); // converts Rotation Vector to Matrix
t_matrix_ = tvec; // set translation matrix
this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix
this->set_P_matrix(R_matrix_, t_matrix_); // set rotation-translation matrix
}
// Given the mesh, backproject the 3D points to 2D to verify the pose estimation
std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
{
std::vector<cv::Point2f> verified_points_2d;
for( int i = 0; i < mesh->getNumVertices(); i++)
{
cv::Point3f point3d = mesh->getVertex(i);
cv::Point2f point2d = this->backproject3DPoint(point3d);
verified_points_2d.push_back(point2d);
}
return verified_points_2d;
}
std::vector<cv::Point2f> verified_points_2d;
for( int i = 0; i < mesh->getNumVertices(); i++)
{
cv::Point3f point3d = mesh->getVertex(i);
cv::Point2f point2d = this->backproject3DPoint(point3d);
verified_points_2d.push_back(point2d);
}
return verified_points_2d;
}
// Backproject a 3D point to 2D using the estimated pose parameters
cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
{
// 3D point vector [x y z 1]'
cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
point3d_vec.at<double>(0) = point3d.x;
point3d_vec.at<double>(1) = point3d.y;
point3d_vec.at<double>(2) = point3d.z;
point3d_vec.at<double>(3) = 1;
// 2D point vector [u v 1]'
cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
point2d_vec = _A_matrix * _P_matrix * point3d_vec;
// Normalization of [u v]'
cv::Point2f point2d;
point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));
point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));
return point2d;
// 3D point vector [x y z 1]'
cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
point3d_vec.at<double>(0) = point3d.x;
point3d_vec.at<double>(1) = point3d.y;
point3d_vec.at<double>(2) = point3d.z;
point3d_vec.at<double>(3) = 1;
// 2D point vector [u v 1]'
cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
point2d_vec = A_matrix_ * P_matrix_ * point3d_vec;
// Normalization of [u v]'
cv::Point2f point2d;
point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));
point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));
return point2d;
}
// Back project a 2D point to 3D and returns if it's on the object surface
bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d)
{
// Triangles list of the object mesh
std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList();
// Triangles list of the object mesh
std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList();
double lambda = 8;
double u = point2d.x;
double v = point2d.y;
double lambda = 8;
double u = point2d.x;
double v = point2d.y;
// Point in vector form
cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1
point2d_vec.at<double>(0) = u * lambda;
point2d_vec.at<double>(1) = v * lambda;
point2d_vec.at<double>(2) = lambda;
// Point in vector form
cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1
point2d_vec.at<double>(0) = u * lambda;
point2d_vec.at<double>(1) = v * lambda;
point2d_vec.at<double>(2) = lambda;
// Point in camera coordinates
cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1
// Point in camera coordinates
cv::Mat X_c = A_matrix_.inv() * point2d_vec ; // 3x1
// Point in world coordinates
cv::Mat X_w = _R_matrix.inv() * ( X_c - _t_matrix ); // 3x1
// Point in world coordinates
cv::Mat X_w = R_matrix_.inv() * ( X_c - t_matrix_ ); // 3x1
// Center of projection
cv::Mat C_op = cv::Mat(_R_matrix.inv()).mul(-1) * _t_matrix; // 3x1
// Center of projection
cv::Mat C_op = cv::Mat(R_matrix_.inv()).mul(-1) * t_matrix_; // 3x1
// Ray direction vector
cv::Mat ray = X_w - C_op; // 3x1
ray = ray / cv::norm(ray); // 3x1
// Ray direction vector
cv::Mat ray = X_w - C_op; // 3x1
ray = ray / cv::norm(ray); // 3x1
// Set up Ray
Ray R((cv::Point3f)C_op, (cv::Point3f)ray);
// Set up Ray
Ray R((cv::Point3f)C_op, (cv::Point3f)ray);
// A vector to store the intersections found
std::vector<cv::Point3f> intersections_list;
// A vector to store the intersections found
std::vector<cv::Point3f> intersections_list;
// Loop for all the triangles and check the intersection
for (unsigned int i = 0; i < triangles_list.size(); i++)
{
cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]);
cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
Triangle T(i, V0, V1, V2);
// Loop for all the triangles and check the intersection
for (unsigned int i = 0; i < triangles_list.size(); i++)
{
cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]);
cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
Triangle T(V0, V1, V2);
double out;
if(this->intersect_MollerTrumbore(R, T, &out))
{
cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
intersections_list.push_back(tmp_pt);
}
}
double out;
if(this->intersect_MollerTrumbore(R, T, &out))
// If there are intersection, find the nearest one
if (!intersections_list.empty())
{
cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
intersections_list.push_back(tmp_pt);
point3d = get_nearest_3D_point(intersections_list, R.getP0());
return true;
}
else
{
return false;
}
}
// If there are intersection, find the nearest one
if (!intersections_list.empty())
{
point3d = get_nearest_3D_point(intersections_list, R.getP0());
return true;
}
else
{
return false;
}
}
// Möller-Trumbore intersection algorithm
bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out)
{
const double EPSILON = 0.000001;
const double EPSILON = 0.000001;
cv::Point3f e1, e2;
cv::Point3f P, Q, T;
double det, inv_det, u, v;
double t;
cv::Point3f e1, e2;
cv::Point3f P, Q, T;
double det, inv_det, u, v;
double t;
cv::Point3f V1 = Triangle.getV0(); // Triangle vertices
cv::Point3f V2 = Triangle.getV1();
cv::Point3f V3 = Triangle.getV2();
cv::Point3f V1 = Triangle.getV0(); // Triangle vertices
cv::Point3f V2 = Triangle.getV1();
cv::Point3f V3 = Triangle.getV2();
cv::Point3f O = Ray.getP0(); // Ray origin
cv::Point3f D = Ray.getP1(); // Ray direction
cv::Point3f O = Ray.getP0(); // Ray origin
cv::Point3f D = Ray.getP1(); // Ray direction
//Find vectors for two edges sharing V1
e1 = SUB(V2, V1);
e2 = SUB(V3, V1);
//Find vectors for two edges sharing V1
e1 = SUB(V2, V1);
e2 = SUB(V3, V1);
// Begin calculation determinant - also used to calculate U parameter
P = CROSS(D, e2);
// Begin calculation determinant - also used to calculate U parameter
P = CROSS(D, e2);
// If determinant is near zero, ray lie in plane of triangle
det = DOT(e1, P);
// If determinant is near zero, ray lie in plane of triangle
det = DOT(e1, P);
//NOT CULLING
if(det > -EPSILON && det < EPSILON) return false;
inv_det = 1.f / det;
//NOT CULLING
if(det > -EPSILON && det < EPSILON) return false;
inv_det = 1.f / det;
//calculate distance from V1 to ray origin
T = SUB(O, V1);
//calculate distance from V1 to ray origin
T = SUB(O, V1);
//Calculate u parameter and test bound
u = DOT(T, P) * inv_det;
//Calculate u parameter and test bound
u = DOT(T, P) * inv_det;
//The intersection lies outside of the triangle
if(u < 0.f || u > 1.f) return false;
//The intersection lies outside of the triangle
if(u < 0.f || u > 1.f) return false;
//Prepare to test v parameter
Q = CROSS(T, e1);
//Prepare to test v parameter
Q = CROSS(T, e1);
//Calculate V parameter and test bound
v = DOT(D, Q) * inv_det;
//Calculate V parameter and test bound
v = DOT(D, Q) * inv_det;
//The intersection lies outside of the triangle
if(v < 0.f || u + v > 1.f) return false;
//The intersection lies outside of the triangle
if(v < 0.f || u + v > 1.f) return false;
t = DOT(e2, Q) * inv_det;
t = DOT(e2, Q) * inv_det;
if(t > EPSILON) { //ray intersection
*out = t;
return true;
}
if(t > EPSILON) { //ray intersection
*out = t;
return true;
}
// No hit, no win
return false;
// No hit, no win
return false;
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h
View file @ 3c92d40f
......@@ -18,41 +18,35 @@
class PnPProblem
{
public:
explicit PnPProblem(const double param[]); // custom constructor
virtual ~PnPProblem();
explicit PnPProblem(const double param[]); // custom constructor
virtual ~PnPProblem();
bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d);
bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out);
std::vector<cv::Point2f> verify_points(Mesh *mesh);
cv::Point2f backproject3DPoint(const cv::Point3f &point3d);
bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags);
void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d,
int flags, cv::Mat &inliers,
int iterationsCount, float reprojectionError, double confidence );
bool backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d);
bool intersect_MollerTrumbore(Ray &R, Triangle &T, double *out);
std::vector<cv::Point2f> verify_points(Mesh *mesh);
cv::Point2f backproject3DPoint(const cv::Point3f &point3d);
bool estimatePose(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags);
void estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d,
int flags, cv::Mat &inliers,
int iterationsCount, float reprojectionError, double confidence );
cv::Mat get_A_matrix() const { return _A_matrix; }
cv::Mat get_R_matrix() const { return _R_matrix; }
cv::Mat get_t_matrix() const { return _t_matrix; }
cv::Mat get_P_matrix() const { return _P_matrix; }
cv::Mat get_A_matrix() const { return A_matrix_; }
cv::Mat get_R_matrix() const { return R_matrix_; }
cv::Mat get_t_matrix() const { return t_matrix_; }
cv::Mat get_P_matrix() const { return P_matrix_; }
void set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix);
void set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix);
private:
/** The calibration matrix */
cv::Mat _A_matrix;
/** The computed rotation matrix */
cv::Mat _R_matrix;
/** The computed translation matrix */
cv::Mat _t_matrix;
/** The computed projection matrix */
cv::Mat _P_matrix;
/** The calibration matrix */
cv::Mat A_matrix_;
/** The computed rotation matrix */
cv::Mat R_matrix_;
/** The computed translation matrix */
cv::Mat t_matrix_;
/** The computed projection matrix */
cv::Mat P_matrix_;
};
// Functions for Möller-Trumbore intersection algorithm
cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
double DOT(cv::Point3f v1, cv::Point3f v2);
cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
#endif /* PNPPROBLEM_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp
View file @ 3c92d40f
......@@ -12,141 +12,143 @@
RobustMatcher::~RobustMatcher()
{
// TODO Auto-generated destructor stub
// TODO Auto-generated destructor stub
}
void RobustMatcher::computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints)
{
detector_->detect(image, keypoints);
detector_->detect(image, keypoints);
}
void RobustMatcher::computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)
{
extractor_->compute(image, keypoints, descriptors);
extractor_->compute(image, keypoints, descriptors);
}
int RobustMatcher::ratioTest(std::vector<std::vector<cv::DMatch> > &matches)
{
int removed = 0;
// for all matches
for ( std::vector<std::vector<cv::DMatch> >::iterator
matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
{
// if 2 NN has been identified
if (matchIterator->size() > 1)
int removed = 0;
// for all matches
for ( std::vector<std::vector<cv::DMatch> >::iterator
matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
{
// check distance ratio
if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_)
{
matchIterator->clear(); // remove match
removed++;
}
}
else
{ // does not have 2 neighbours
matchIterator->clear(); // remove match
removed++;
// if 2 NN has been identified
if (matchIterator->size() > 1)
{
// check distance ratio
if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_)
{
matchIterator->clear(); // remove match
removed++;
}
}
else
{ // does not have 2 neighbours
matchIterator->clear(); // remove match
removed++;
}
}
}
return removed;
return removed;
}
void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches )
const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches )
{
// for all matches image 1 -> image 2
for (std::vector<std::vector<cv::DMatch> >::const_iterator
matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1)
{
// ignore deleted matches
if (matchIterator1->empty() || matchIterator1->size() < 2)
continue;
// for all matches image 2 -> image 1
for (std::vector<std::vector<cv::DMatch> >::const_iterator
matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
{
// for all matches image 1 -> image 2
for (std::vector<std::vector<cv::DMatch> >::const_iterator
matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1)
{
// ignore deleted matches
if (matchIterator2->empty() || matchIterator2->size() < 2)
continue;
// Match symmetry test
if ((*matchIterator1)[0].queryIdx ==
(*matchIterator2)[0].trainIdx &&
(*matchIterator2)[0].queryIdx ==
(*matchIterator1)[0].trainIdx)
if (matchIterator1->empty() || matchIterator1->size() < 2)
continue;
// for all matches image 2 -> image 1
for (std::vector<std::vector<cv::DMatch> >::const_iterator
matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
{
// add symmetrical match
symMatches.push_back(
cv::DMatch((*matchIterator1)[0].queryIdx,
(*matchIterator1)[0].trainIdx,
(*matchIterator1)[0].distance));
break; // next match in image 1 -> image 2
// ignore deleted matches
if (matchIterator2->empty() || matchIterator2->size() < 2)
continue;
// Match symmetry test
if ((*matchIterator1)[0].queryIdx == (*matchIterator2)[0].trainIdx &&
(*matchIterator2)[0].queryIdx == (*matchIterator1)[0].trainIdx)
{
// add symmetrical match
symMatches.push_back(cv::DMatch((*matchIterator1)[0].queryIdx,
(*matchIterator1)[0].trainIdx, (*matchIterator1)[0].distance));
break; // next match in image 1 -> image 2
}
}
}
}
}
}
void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model )
std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model)
{
// 1a. Detection of the ORB features
this->computeKeyPoints(frame, keypoints_frame);
// 1a. Detection of the ORB features
this->computeKeyPoints(frame, keypoints_frame);
// 1b. Extraction of the ORB descriptors
cv::Mat descriptors_frame;
this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
// 1b. Extraction of the ORB descriptors
cv::Mat descriptors_frame;
this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
// 2. Match the two image descriptors
std::vector<std::vector<cv::DMatch> > matches12, matches21;
// 2. Match the two image descriptors
std::vector<std::vector<cv::DMatch> > matches12, matches21;
// 2a. From image 1 to image 2
matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
// 2a. From image 1 to image 2
matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
// 2b. From image 2 to image 1
matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours
// 2b. From image 2 to image 1
matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours
// 3. Remove matches for which NN ratio is > than threshold
// clean image 1 -> image 2 matches
ratioTest(matches12);
// clean image 2 -> image 1 matches
ratioTest(matches21);
// 3. Remove matches for which NN ratio is > than threshold
// clean image 1 -> image 2 matches
ratioTest(matches12);
// clean image 2 -> image 1 matches
ratioTest(matches21);
// 4. Remove non-symmetrical matches
symmetryTest(matches12, matches21, good_matches);
// 4. Remove non-symmetrical matches
symmetryTest(matches12, matches21, good_matches);
if (!training_img_.empty() && !keypoints_model.empty())
{
cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
}
}
void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model )
std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model)
{
good_matches.clear();
good_matches.clear();
// 1a. Detection of the ORB features
this->computeKeyPoints(frame, keypoints_frame);
// 1a. Detection of the ORB features
this->computeKeyPoints(frame, keypoints_frame);
// 1b. Extraction of the ORB descriptors
cv::Mat descriptors_frame;
this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
// 1b. Extraction of the ORB descriptors
cv::Mat descriptors_frame;
this->computeDescriptors(frame, keypoints_frame, descriptors_frame);
// 2. Match the two image descriptors
std::vector<std::vector<cv::DMatch> > matches;
matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2);
// 2. Match the two image descriptors
std::vector<std::vector<cv::DMatch> > matches;
matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2);
// 3. Remove matches for which NN ratio is > than threshold
ratioTest(matches);
// 3. Remove matches for which NN ratio is > than threshold
ratioTest(matches);
// 4. Fill good matches container
for ( std::vector<std::vector<cv::DMatch> >::iterator
matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
{
if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]);
}
// 4. Fill good matches container
for ( std::vector<std::vector<cv::DMatch> >::iterator
matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator)
{
if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]);
}
if (!training_img_.empty() && !keypoints_model.empty())
{
cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
}
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h
View file @ 3c92d40f
......@@ -16,66 +16,77 @@
class RobustMatcher {
public:
RobustMatcher() : ratio_(0.8f)
{
// ORB is the default feature
detector_ = cv::ORB::create();
extractor_ = cv::ORB::create();
RobustMatcher() : detector_(), extractor_(), matcher_(),
ratio_(0.8f), training_img_(), img_matching_()
{
// ORB is the default feature
detector_ = cv::ORB::create();
extractor_ = cv::ORB::create();
// BruteFroce matcher with Norm Hamming is the default matcher
matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false);
// BruteFroce matcher with Norm Hamming is the default matcher
matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false);
}
virtual ~RobustMatcher();
}
virtual ~RobustMatcher();
// Set the feature detector
void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) { detector_ = detect; }
// Set the feature detector
void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) { detector_ = detect; }
// Set the descriptor extractor
void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) { extractor_ = desc; }
// Set the descriptor extractor
void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) { extractor_ = desc; }
// Set the matcher
void setDescriptorMatcher(const cv::Ptr<cv::DescriptorMatcher>& match) { matcher_ = match; }
// Set the matcher
void setDescriptorMatcher(const cv::Ptr<cv::DescriptorMatcher>& match) { matcher_ = match; }
// Compute the keypoints of an image
void computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints);
// Compute the keypoints of an image
void computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints);
// Compute the descriptors of an image given its keypoints
void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
// Compute the descriptors of an image given its keypoints
void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
// Set ratio parameter for the ratio test
void setRatio( float rat) { ratio_ = rat; }
cv::Mat getImageMatching() const { return img_matching_; }
// Clear matches for which NN ratio is > than threshold
// return the number of removed points
// (corresponding entries being cleared,
// i.e. size will be 0)
int ratioTest(std::vector<std::vector<cv::DMatch> > &matches);
// Set ratio parameter for the ratio test
void setRatio( float rat) { ratio_ = rat; }
// Insert symmetrical matches in symMatches vector
void symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches );
void setTrainingImage(const cv::Mat &img) { training_img_ = img; }
// Match feature points using ratio and symmetry test
void robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
// Clear matches for which NN ratio is > than threshold
// return the number of removed points
// (corresponding entries being cleared,
// i.e. size will be 0)
int ratioTest(std::vector<std::vector<cv::DMatch> > &matches);
// Insert symmetrical matches in symMatches vector
void symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1,
const std::vector<std::vector<cv::DMatch> >& matches2,
std::vector<cv::DMatch>& symMatches );
// Match feature points using ratio and symmetry test
void robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model );
const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model);
// Match feature points using ratio test
void fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model );
// Match feature points using ratio test
void fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame,
const cv::Mat& descriptors_model,
const std::vector<cv::KeyPoint>& keypoints_model);
private:
// pointer to the feature point detector object
cv::Ptr<cv::FeatureDetector> detector_;
// pointer to the feature descriptor extractor object
cv::Ptr<cv::DescriptorExtractor> extractor_;
// pointer to the matcher object
cv::Ptr<cv::DescriptorMatcher> matcher_;
// max ratio between 1st and 2nd NN
float ratio_;
// pointer to the feature point detector object
cv::Ptr<cv::FeatureDetector> detector_;
// pointer to the feature descriptor extractor object
cv::Ptr<cv::DescriptorExtractor> extractor_;
// pointer to the matcher object
cv::Ptr<cv::DescriptorMatcher> matcher_;
// max ratio between 1st and 2nd NN
float ratio_;
// training image
cv::Mat training_img_;
// matching image
cv::Mat img_matching_;
};
#endif /* ROBUSTMATCHER_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp
View file @ 3c92d40f
......@@ -11,178 +11,180 @@
#include "ModelRegistration.h"
#include "Utils.h"
#include <opencv2/opencv_modules.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/flann.hpp>
#if defined (HAVE_OPENCV_XFEATURES2D)
#include <opencv2/xfeatures2d.hpp>
#endif
// For text
int fontFace = cv::FONT_ITALIC;
double fontScale = 0.75;
int thickness_font = 2;
const int fontFace = cv::FONT_ITALIC;
const double fontScale = 0.75;
const int thickness_font = 2;
// For circles
int lineType = 8;
int radius = 4;
double thickness_circ = -1;
const int lineType = 8;
const int radius = 4;
// Draw a text with the question point
void drawQuestion(cv::Mat image, cv::Point3f point, cv::Scalar color)
{
std::string x = IntToString((int)point.x);
std::string y = IntToString((int)point.y);
std::string z = IntToString((int)point.z);
std::string x = IntToString((int)point.x);
std::string y = IntToString((int)point.y);
std::string z = IntToString((int)point.z);
std::string text = " Where is point (" + x + "," + y + "," + z + ") ?";
cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
std::string text = " Where is point (" + x + "," + y + "," + z + ") ?";
cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
}
// Draw a text with the number of entered points
void drawText(cv::Mat image, std::string text, cv::Scalar color)
{
cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
}
// Draw a text with the number of entered points
void drawText2(cv::Mat image, std::string text, cv::Scalar color)
{
cv::putText(image, text, cv::Point(25,75), fontFace, fontScale, color, thickness_font, 8);
cv::putText(image, text, cv::Point(25,75), fontFace, fontScale, color, thickness_font, 8);
}
// Draw a text with the frame ratio
void drawFPS(cv::Mat image, double fps, cv::Scalar color)
{
std::string fps_str = IntToString((int)fps);
std::string text = fps_str + " FPS";
cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
std::string fps_str = cv::format("%.2f FPS", fps);
cv::putText(image, fps_str, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
}
// Draw a text with the frame ratio
void drawConfidence(cv::Mat image, double confidence, cv::Scalar color)
{
std::string conf_str = IntToString((int)confidence);
std::string text = conf_str + " %";
cv::putText(image, text, cv::Point(500,75), fontFace, fontScale, color, thickness_font, 8);
std::string conf_str = IntToString((int)confidence);
std::string text = conf_str + " %";
cv::putText(image, text, cv::Point(500,75), fontFace, fontScale, color, thickness_font, 8);
}
// Draw a text with the number of entered points
void drawCounter(cv::Mat image, int n, int n_max, cv::Scalar color)
{
std::string n_str = IntToString(n);
std::string n_max_str = IntToString(n_max);
std::string text = n_str + " of " + n_max_str + " points";
cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
std::string n_str = IntToString(n);
std::string n_max_str = IntToString(n_max);
std::string text = n_str + " of " + n_max_str + " points";
cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
}
// Draw the points and the coordinates
void drawPoints(cv::Mat image, std::vector<cv::Point2f> &list_points_2d, std::vector<cv::Point3f> &list_points_3d, cv::Scalar color)
{
for (unsigned int i = 0; i < list_points_2d.size(); ++i)
{
cv::Point2f point_2d = list_points_2d[i];
cv::Point3f point_3d = list_points_3d[i];
// Draw Selected points
cv::circle(image, point_2d, radius, color, -1, lineType );
std::string idx = IntToString(i+1);
std::string x = IntToString((int)point_3d.x);
std::string y = IntToString((int)point_3d.y);
std::string z = IntToString((int)point_3d.z);
std::string text = "P" + idx + " (" + x + "," + y + "," + z +")";
point_2d.x = point_2d.x + 10;
point_2d.y = point_2d.y - 10;
cv::putText(image, text, point_2d, fontFace, fontScale*0.5, color, thickness_font, 8);
}
for (unsigned int i = 0; i < list_points_2d.size(); ++i)
{
cv::Point2f point_2d = list_points_2d[i];
cv::Point3f point_3d = list_points_3d[i];
// Draw Selected points
cv::circle(image, point_2d, radius, color, -1, lineType );
std::string idx = IntToString(i+1);
std::string x = IntToString((int)point_3d.x);
std::string y = IntToString((int)point_3d.y);
std::string z = IntToString((int)point_3d.z);
std::string text = "P" + idx + " (" + x + "," + y + "," + z +")";
point_2d.x = point_2d.x + 10;
point_2d.y = point_2d.y - 10;
cv::putText(image, text, point_2d, fontFace, fontScale*0.5, color, thickness_font, 8);
}
}
// Draw only the 2D points
void draw2DPoints(cv::Mat image, std::vector<cv::Point2f> &list_points, cv::Scalar color)
{
for( size_t i = 0; i < list_points.size(); i++)
{
cv::Point2f point_2d = list_points[i];
for( size_t i = 0; i < list_points.size(); i++)
{
cv::Point2f point_2d = list_points[i];
// Draw Selected points
cv::circle(image, point_2d, radius, color, -1, lineType );
}
// Draw Selected points
cv::circle(image, point_2d, radius, color, -1, lineType );
}
}
// Draw an arrow into the image
void drawArrow(cv::Mat image, cv::Point2i p, cv::Point2i q, cv::Scalar color, int arrowMagnitude, int thickness, int line_type, int shift)
{
//Draw the principle line
cv::line(image, p, q, color, thickness, line_type, shift);
const double PI = CV_PI;
//compute the angle alpha
double angle = atan2((double)p.y-q.y, (double)p.x-q.x);
//compute the coordinates of the first segment
p.x = (int) ( q.x + arrowMagnitude * cos(angle + PI/4));
p.y = (int) ( q.y + arrowMagnitude * sin(angle + PI/4));
//Draw the first segment
cv::line(image, p, q, color, thickness, line_type, shift);
//compute the coordinates of the second segment
p.x = (int) ( q.x + arrowMagnitude * cos(angle - PI/4));
p.y = (int) ( q.y + arrowMagnitude * sin(angle - PI/4));
//Draw the second segment
cv::line(image, p, q, color, thickness, line_type, shift);
//Draw the principle line
cv::line(image, p, q, color, thickness, line_type, shift);
const double PI = CV_PI;
//compute the angle alpha
double angle = atan2((double)p.y-q.y, (double)p.x-q.x);
//compute the coordinates of the first segment
p.x = (int) ( q.x + arrowMagnitude * cos(angle + PI/4));
p.y = (int) ( q.y + arrowMagnitude * sin(angle + PI/4));
//Draw the first segment
cv::line(image, p, q, color, thickness, line_type, shift);
//compute the coordinates of the second segment
p.x = (int) ( q.x + arrowMagnitude * cos(angle - PI/4));
p.y = (int) ( q.y + arrowMagnitude * sin(angle - PI/4));
//Draw the second segment
cv::line(image, p, q, color, thickness, line_type, shift);
}
// Draw the 3D coordinate axes
void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_points2d)
{
cv::Scalar red(0, 0, 255);
cv::Scalar green(0,255,0);
cv::Scalar blue(255,0,0);
cv::Scalar black(0,0,0);
cv::Point2i origin = list_points2d[0];
cv::Point2i pointX = list_points2d[1];
cv::Point2i pointY = list_points2d[2];
cv::Point2i pointZ = list_points2d[3];
drawArrow(image, origin, pointX, red, 9, 2);
drawArrow(image, origin, pointY, blue, 9, 2);
drawArrow(image, origin, pointZ, green, 9, 2);
cv::circle(image, origin, radius/2, black, -1, lineType );
cv::Scalar red(0, 0, 255);
cv::Scalar green(0,255,0);
cv::Scalar blue(255,0,0);
cv::Scalar black(0,0,0);
cv::Point2i origin = list_points2d[0];
cv::Point2i pointX = list_points2d[1];
cv::Point2i pointY = list_points2d[2];
cv::Point2i pointZ = list_points2d[3];
drawArrow(image, origin, pointX, red, 9, 2);
drawArrow(image, origin, pointY, green, 9, 2);
drawArrow(image, origin, pointZ, blue, 9, 2);
cv::circle(image, origin, radius/2, black, -1, lineType );
}
// Draw the object mesh
void drawObjectMesh(cv::Mat image, const Mesh *mesh, PnPProblem *pnpProblem, cv::Scalar color)
{
std::vector<std::vector<int> > list_triangles = mesh->getTrianglesList();
for( size_t i = 0; i < list_triangles.size(); i++)
{
std::vector<int> tmp_triangle = list_triangles.at(i);
cv::Point3f point_3d_0 = mesh->getVertex(tmp_triangle[0]);
cv::Point3f point_3d_1 = mesh->getVertex(tmp_triangle[1]);
cv::Point3f point_3d_2 = mesh->getVertex(tmp_triangle[2]);
cv::Point2f point_2d_0 = pnpProblem->backproject3DPoint(point_3d_0);
cv::Point2f point_2d_1 = pnpProblem->backproject3DPoint(point_3d_1);
cv::Point2f point_2d_2 = pnpProblem->backproject3DPoint(point_3d_2);
cv::line(image, point_2d_0, point_2d_1, color, 1);
cv::line(image, point_2d_1, point_2d_2, color, 1);
cv::line(image, point_2d_2, point_2d_0, color, 1);
}
std::vector<std::vector<int> > list_triangles = mesh->getTrianglesList();
for( size_t i = 0; i < list_triangles.size(); i++)
{
std::vector<int> tmp_triangle = list_triangles.at(i);
cv::Point3f point_3d_0 = mesh->getVertex(tmp_triangle[0]);
cv::Point3f point_3d_1 = mesh->getVertex(tmp_triangle[1]);
cv::Point3f point_3d_2 = mesh->getVertex(tmp_triangle[2]);
cv::Point2f point_2d_0 = pnpProblem->backproject3DPoint(point_3d_0);
cv::Point2f point_2d_1 = pnpProblem->backproject3DPoint(point_3d_1);
cv::Point2f point_2d_2 = pnpProblem->backproject3DPoint(point_3d_2);
cv::line(image, point_2d_0, point_2d_1, color, 1);
cv::line(image, point_2d_1, point_2d_2, color, 1);
cv::line(image, point_2d_2, point_2d_0, color, 1);
}
}
// Computes the norm of the translation error
double get_translation_error(const cv::Mat &t_true, const cv::Mat &t)
{
return cv::norm( t_true - t );
return cv::norm( t_true - t );
}
// Computes the norm of the rotation error
double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R)
{
cv::Mat error_vec, error_mat;
error_mat = -R_true * R.t();
cv::Rodrigues(error_mat, error_vec);
cv::Mat error_vec, error_mat;
error_mat = -R_true * R.t();
cv::Rodrigues(error_mat, error_vec);
return cv::norm(error_vec);
return cv::norm(error_vec);
}
// Converts a given Rotation Matrix to Euler angles
......@@ -191,41 +193,41 @@ double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R)
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
cv::Mat rot2euler(const cv::Mat & rotationMatrix)
{
cv::Mat euler(3,1,CV_64F);
double m00 = rotationMatrix.at<double>(0,0);
double m02 = rotationMatrix.at<double>(0,2);
double m10 = rotationMatrix.at<double>(1,0);
double m11 = rotationMatrix.at<double>(1,1);
double m12 = rotationMatrix.at<double>(1,2);
double m20 = rotationMatrix.at<double>(2,0);
double m22 = rotationMatrix.at<double>(2,2);
double bank, attitude, heading;
// Assuming the angles are in radians.
if (m10 > 0.998) { // singularity at north pole
bank = 0;
attitude = CV_PI/2;
heading = atan2(m02,m22);
}
else if (m10 < -0.998) { // singularity at south pole
bank = 0;
attitude = -CV_PI/2;
heading = atan2(m02,m22);
}
else
{
bank = atan2(-m12,m11);
attitude = asin(m10);
heading = atan2(-m20,m00);
}
euler.at<double>(0) = bank;
euler.at<double>(1) = attitude;
euler.at<double>(2) = heading;
return euler;
cv::Mat euler(3,1,CV_64F);
double m00 = rotationMatrix.at<double>(0,0);
double m02 = rotationMatrix.at<double>(0,2);
double m10 = rotationMatrix.at<double>(1,0);
double m11 = rotationMatrix.at<double>(1,1);
double m12 = rotationMatrix.at<double>(1,2);
double m20 = rotationMatrix.at<double>(2,0);
double m22 = rotationMatrix.at<double>(2,2);
double bank, attitude, heading;
// Assuming the angles are in radians.
if (m10 > 0.998) { // singularity at north pole
bank = 0;
attitude = CV_PI/2;
heading = atan2(m02,m22);
}
else if (m10 < -0.998) { // singularity at south pole
bank = 0;
attitude = -CV_PI/2;
heading = atan2(m02,m22);
}
else
{
bank = atan2(-m12,m11);
attitude = asin(m10);
heading = atan2(-m20,m00);
}
euler.at<double>(0) = bank;
euler.at<double>(1) = attitude;
euler.at<double>(2) = heading;
return euler;
}
// Converts a given Euler angles to Rotation Matrix
......@@ -234,65 +236,166 @@ cv::Mat rot2euler(const cv::Mat & rotationMatrix)
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm
cv::Mat euler2rot(const cv::Mat & euler)
{
cv::Mat rotationMatrix(3,3,CV_64F);
double bank = euler.at<double>(0);
double attitude = euler.at<double>(1);
double heading = euler.at<double>(2);
// Assuming the angles are in radians.
double ch = cos(heading);
double sh = sin(heading);
double ca = cos(attitude);
double sa = sin(attitude);
double cb = cos(bank);
double sb = sin(bank);
double m00, m01, m02, m10, m11, m12, m20, m21, m22;
m00 = ch * ca;
m01 = sh*sb - ch*sa*cb;
m02 = ch*sa*sb + sh*cb;
m10 = sa;
m11 = ca*cb;
m12 = -ca*sb;
m20 = -sh*ca;
m21 = sh*sa*cb + ch*sb;
m22 = -sh*sa*sb + ch*cb;
rotationMatrix.at<double>(0,0) = m00;
rotationMatrix.at<double>(0,1) = m01;
rotationMatrix.at<double>(0,2) = m02;
rotationMatrix.at<double>(1,0) = m10;
rotationMatrix.at<double>(1,1) = m11;
rotationMatrix.at<double>(1,2) = m12;
rotationMatrix.at<double>(2,0) = m20;
rotationMatrix.at<double>(2,1) = m21;
rotationMatrix.at<double>(2,2) = m22;
return rotationMatrix;
cv::Mat rotationMatrix(3,3,CV_64F);
double bank = euler.at<double>(0);
double attitude = euler.at<double>(1);
double heading = euler.at<double>(2);
// Assuming the angles are in radians.
double ch = cos(heading);
double sh = sin(heading);
double ca = cos(attitude);
double sa = sin(attitude);
double cb = cos(bank);
double sb = sin(bank);
double m00, m01, m02, m10, m11, m12, m20, m21, m22;
m00 = ch * ca;
m01 = sh*sb - ch*sa*cb;
m02 = ch*sa*sb + sh*cb;
m10 = sa;
m11 = ca*cb;
m12 = -ca*sb;
m20 = -sh*ca;
m21 = sh*sa*cb + ch*sb;
m22 = -sh*sa*sb + ch*cb;
rotationMatrix.at<double>(0,0) = m00;
rotationMatrix.at<double>(0,1) = m01;
rotationMatrix.at<double>(0,2) = m02;
rotationMatrix.at<double>(1,0) = m10;
rotationMatrix.at<double>(1,1) = m11;
rotationMatrix.at<double>(1,2) = m12;
rotationMatrix.at<double>(2,0) = m20;
rotationMatrix.at<double>(2,1) = m21;
rotationMatrix.at<double>(2,2) = m22;
return rotationMatrix;
}
// Converts a given string to an integer
int StringToInt ( const std::string &Text )
{
std::istringstream ss(Text);
int result;
return ss >> result ? result : 0;
std::istringstream ss(Text);
int result;
return ss >> result ? result : 0;
}
// Converts a given float to a string
std::string FloatToString ( float Number )
{
std::ostringstream ss;
ss << Number;
return ss.str();
std::ostringstream ss;
ss << Number;
return ss.str();
}
// Converts a given integer to a string
std::string IntToString ( int Number )
{
std::ostringstream ss;
ss << Number;
return ss.str();
std::ostringstream ss;
ss << Number;
return ss.str();
}
void createFeatures(const std::string &featureName, int numKeypoints, cv::Ptr<cv::Feature2D> &detector, cv::Ptr<cv::Feature2D> &descriptor)
{
if (featureName == "ORB")
{
detector = cv::ORB::create(numKeypoints);
descriptor = cv::ORB::create(numKeypoints);
}
else if (featureName == "KAZE")
{
detector = cv::KAZE::create();
descriptor = cv::KAZE::create();
}
else if (featureName == "AKAZE")
{
detector = cv::AKAZE::create();
descriptor = cv::AKAZE::create();
}
else if (featureName == "BRISK")
{
detector = cv::BRISK::create();
descriptor = cv::BRISK::create();
}
else if (featureName == "SIFT")
{
#if defined (OPENCV_ENABLE_NONFREE) && defined (HAVE_OPENCV_XFEATURES2D)
detector = cv::xfeatures2d::SIFT::create();
descriptor = cv::xfeatures2d::SIFT::create();
#else
std::cout << "xfeatures2d module is not available or nonfree is not enabled." << std::endl;
std::cout << "Default to ORB." << std::endl;
detector = cv::ORB::create(numKeypoints);
descriptor = cv::ORB::create(numKeypoints);
#endif
}
else if (featureName == "SURF")
{
#if defined (OPENCV_ENABLE_NONFREE) && defined (HAVE_OPENCV_XFEATURES2D)
detector = cv::xfeatures2d::SURF::create(100, 4, 3, true); //extended=true
descriptor = cv::xfeatures2d::SURF::create(100, 4, 3, true); //extended=true
#else
std::cout << "xfeatures2d module is not available or nonfree is not enabled." << std::endl;
std::cout << "Default to ORB." << std::endl;
detector = cv::ORB::create(numKeypoints);
descriptor = cv::ORB::create(numKeypoints);
#endif
}
else if (featureName == "BINBOOST")
{
#if defined (HAVE_OPENCV_XFEATURES2D)
detector = cv::KAZE::create();
descriptor = cv::xfeatures2d::BoostDesc::create();
#else
std::cout << "xfeatures2d module is not available." << std::endl;
std::cout << "Default to ORB." << std::endl;
detector = cv::ORB::create(numKeypoints);
descriptor = cv::ORB::create(numKeypoints);
#endif
}
else if (featureName == "VGG")
{
#if defined (HAVE_OPENCV_XFEATURES2D)
detector = cv::KAZE::create();
descriptor = cv::xfeatures2d::VGG::create();
#else
std::cout << "xfeatures2d module is not available." << std::endl;
std::cout << "Default to ORB." << std::endl;
detector = cv::ORB::create(numKeypoints);
descriptor = cv::ORB::create(numKeypoints);
#endif
}
}
cv::Ptr<cv::DescriptorMatcher> createMatcher(const std::string &featureName, bool useFLANN)
{
if (featureName == "ORB" || featureName == "BRISK" || featureName == "AKAZE" || featureName == "BINBOOST")
{
if (useFLANN)
{
cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters
cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>(50); // instantiate flann search parameters
return cv::makePtr<cv::FlannBasedMatcher>(indexParams, searchParams);
}
else
{
return cv::DescriptorMatcher::create("BruteForce-Hamming");
}
}
else
{
if (useFLANN)
{
return cv::DescriptorMatcher::create("FlannBased");
}
else
{
return cv::DescriptorMatcher::create("BruteForce");
}
}
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h
View file @ 3c92d40f
......@@ -10,6 +10,7 @@
#include <iostream>
#include <opencv2/features2d.hpp>
#include "PnPProblem.h"
// Draw a text with the question point
......@@ -66,4 +67,8 @@ std::string FloatToString ( float Number );
// Converts a given integer to a string
std::string IntToString ( int Number );
void createFeatures(const std::string &featureName, int numKeypoints, cv::Ptr<cv::Feature2D> &detector, cv::Ptr<cv::Feature2D> &descriptor);
cv::Ptr<cv::DescriptorMatcher> createMatcher(const std::string &featureName, bool useFLANN);
#endif /* UTILS_H_ */
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp
View file @ 3c92d40f
// C++
#include <iostream>
#include <time.h>
// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/core/utils/filesystem.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
......@@ -21,451 +20,482 @@
using namespace cv;
using namespace std;
string tutorial_path = "../../samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/"; // path to tutorial
string video_read_path = tutorial_path + "Data/box.mp4"; // recorded video
string yml_read_path = tutorial_path + "Data/cookies_ORB.yml"; // 3dpts + descriptors
string ply_read_path = tutorial_path + "Data/box.ply"; // mesh
// Intrinsic camera parameters: UVC WEBCAM
double f = 55; // focal length in mm
double sx = 22.3, sy = 14.9; // sensor size
double width = 640, height = 480; // image size
double params_WEBCAM[] = { width*f/sx, // fx
height*f/sy, // fy
width/2, // cx
height/2}; // cy
// Some basic colors
Scalar red(0, 0, 255);
Scalar green(0,255,0);
Scalar blue(255,0,0);
Scalar yellow(0,255,255);
// Robust Matcher parameters
int numKeyPoints = 2000; // number of detected keypoints
float ratioTest = 0.70f; // ratio test
bool fast_match = true; // fastRobustMatch() or robustMatch()
// RANSAC parameters
int iterationsCount = 500; // number of Ransac iterations.
float reprojectionError = 2.0; // maximum allowed distance to consider it an inlier.
double confidence = 0.95; // ransac successful confidence.
// Kalman Filter parameters
int minInliersKalman = 30; // Kalman threshold updating
// PnP parameters
int pnpMethod = SOLVEPNP_ITERATIVE;
/** Functions headers **/
void help();
void initKalmanFilter( KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt);
void predictKalmanFilter( KalmanFilter &KF, Mat &translation_predicted, Mat &rotation_predicted );
void updateKalmanFilter( KalmanFilter &KF, Mat &measurements,
Mat &translation_estimated, Mat &rotation_estimated );
void fillMeasurements( Mat &measurements,
const Mat &translation_measured, const Mat &rotation_measured);
/** Main program **/
int main(int argc, char *argv[])
{
help();
const String keys =
"{help h | | print this message }"
"{video v | | path to recorded video }"
"{model | | path to yml model }"
"{mesh | | path to ply mesh }"
"{keypoints k |2000 | number of keypoints to detect }"
"{ratio r |0.7 | threshold for ratio test }"
"{iterations it |500 | RANSAC maximum iterations count }"
"{error e |2.0 | RANSAC reprojection error }"
"{confidence c |0.95 | RANSAC confidence }"
"{inliers in |30 | minimum inliers for Kalman update }"
"{method pnp |0 | PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS}"
"{fast f |true | use of robust fast match }"
;
CommandLineParser parser(argc, argv, keys);
if (parser.has("help"))
{
parser.printMessage();
return 0;
}
else
{
video_read_path = parser.get<string>("video").size() > 0 ? parser.get<string>("video") : video_read_path;
yml_read_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : yml_read_path;
ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
numKeyPoints = !parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
ratioTest = !parser.has("ratio") ? parser.get<float>("ratio") : ratioTest;
fast_match = !parser.has("fast") ? parser.get<bool>("fast") : fast_match;
iterationsCount = !parser.has("iterations") ? parser.get<int>("iterations") : iterationsCount;
reprojectionError = !parser.has("error") ? parser.get<float>("error") : reprojectionError;
confidence = !parser.has("confidence") ? parser.get<float>("confidence") : confidence;
minInliersKalman = !parser.has("inliers") ? parser.get<int>("inliers") : minInliersKalman;
pnpMethod = !parser.has("method") ? parser.get<int>("method") : pnpMethod;
}
PnPProblem pnp_detection(params_WEBCAM);
PnPProblem pnp_detection_est(params_WEBCAM);
Model model; // instantiate Model object
model.load(yml_read_path); // load a 3D textured object model
Mesh mesh; // instantiate Mesh object
mesh.load(ply_read_path); // load an object mesh
RobustMatcher rmatcher; // instantiate RobustMatcher
Ptr<FeatureDetector> orb = ORB::create();
rmatcher.setFeatureDetector(orb); // set feature detector
rmatcher.setDescriptorExtractor(orb); // set descriptor extractor
Ptr<flann::IndexParams> indexParams = makePtr<flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters
Ptr<flann::SearchParams> searchParams = makePtr<flann::SearchParams>(50); // instantiate flann search parameters
// instantiate FlannBased matcher
Ptr<DescriptorMatcher> matcher = makePtr<FlannBasedMatcher>(indexParams, searchParams);
rmatcher.setDescriptorMatcher(matcher); // set matcher
rmatcher.setRatio(ratioTest); // set ratio test parameter
KalmanFilter KF; // instantiate Kalman Filter
int nStates = 18; // the number of states
int nMeasurements = 6; // the number of measured states
int nInputs = 0; // the number of control actions
double dt = 0.125; // time between measurements (1/FPS)
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function
Mat measurements(nMeasurements, 1, CV_64F); measurements.setTo(Scalar(0));
bool good_measurement = false;
// Get the MODEL INFO
vector<Point3f> list_points3d_model = model.get_points3d(); // list with model 3D coordinates
Mat descriptors_model = model.get_descriptors(); // list with descriptors of each 3D coordinate
// Create & Open Window
namedWindow("REAL TIME DEMO", WINDOW_KEEPRATIO);
VideoCapture cap; // instantiate VideoCapture
cap.open(video_read_path); // open a recorded video
if(!cap.isOpened()) // check if we succeeded
{
cout << "Could not open the camera device" << endl;
return -1;
}
// start and end times
time_t start, end;
// fps calculated using number of frames / seconds
// floating point seconds elapsed since start
double fps, sec;
// frame counter
int counter = 0;
// start the clock
time(&start);
Mat frame, frame_vis;
while(cap.read(frame) && (char)waitKey(30) != 27) // capture frame until ESC is pressed
{
frame_vis = frame.clone(); // refresh visualisation frame
// -- Step 1: Robust matching between model descriptors and scene descriptors
vector<DMatch> good_matches; // to obtain the 3D points of the model
vector<KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
if(fast_match)
help();
const String keys =
"{help h | | print this message }"
"{video v | | path to recorded video }"
"{model | | path to yml model }"
"{mesh | | path to ply mesh }"
"{keypoints k |2000 | number of keypoints to detect }"
"{ratio r |0.7 | threshold for ratio test }"
"{iterations it |500 | RANSAC maximum iterations count }"
"{error e |6.0 | RANSAC reprojection error }"
"{confidence c |0.99 | RANSAC confidence }"
"{inliers in |30 | minimum inliers for Kalman update }"
"{method pnp |0 | PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS - (5) AP3P}"
"{fast f |true | use of robust fast match }"
"{feature |ORB | feature name (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG) }"
"{FLANN |false | use FLANN library for descriptors matching }"
"{save | | path to the directory where to save the image results }"
"{displayFiltered |false | display filtered pose (from Kalman filter) }"
;
CommandLineParser parser(argc, argv, keys);
string video_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.mp4"); // recorded video
string yml_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml"); // 3dpts + descriptors
string ply_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply"); // mesh
// Intrinsic camera parameters: UVC WEBCAM
double f = 55; // focal length in mm
double sx = 22.3, sy = 14.9; // sensor size
double width = 640, height = 480; // image size
double params_WEBCAM[] = { width*f/sx, // fx
height*f/sy, // fy
width/2, // cx
height/2}; // cy
// Some basic colors
Scalar red(0, 0, 255);
Scalar green(0,255,0);
Scalar blue(255,0,0);
Scalar yellow(0,255,255);
// Robust Matcher parameters
int numKeyPoints = 2000; // number of detected keypoints
float ratioTest = 0.70f; // ratio test
bool fast_match = true; // fastRobustMatch() or robustMatch()
// RANSAC parameters
int iterationsCount = 500; // number of Ransac iterations.
float reprojectionError = 6.0; // maximum allowed distance to consider it an inlier.
double confidence = 0.99; // ransac successful confidence.
// Kalman Filter parameters
int minInliersKalman = 30; // Kalman threshold updating
// PnP parameters
int pnpMethod = SOLVEPNP_ITERATIVE;
string featureName = "ORB";
bool useFLANN = false;
// Save results
string saveDirectory = "";
Mat frameSave;
int frameCount = 0;
bool displayFilteredPose = false;
if (parser.has("help"))
{
rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);
parser.printMessage();
return 0;
}
else
{
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model);
video_read_path = parser.get<string>("video").size() > 0 ? parser.get<string>("video") : video_read_path;
yml_read_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : yml_read_path;
ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
numKeyPoints = parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
ratioTest = parser.has("ratio") ? parser.get<float>("ratio") : ratioTest;
fast_match = parser.has("fast") ? parser.get<bool>("fast") : fast_match;
iterationsCount = parser.has("iterations") ? parser.get<int>("iterations") : iterationsCount;
reprojectionError = parser.has("error") ? parser.get<float>("error") : reprojectionError;
confidence = parser.has("confidence") ? parser.get<float>("confidence") : confidence;
minInliersKalman = parser.has("inliers") ? parser.get<int>("inliers") : minInliersKalman;
pnpMethod = parser.has("method") ? parser.get<int>("method") : pnpMethod;
featureName = parser.has("feature") ? parser.get<string>("feature") : featureName;
useFLANN = parser.has("FLANN") ? parser.get<bool>("FLANN") : useFLANN;
saveDirectory = parser.has("save") ? parser.get<string>("save") : saveDirectory;
displayFilteredPose = parser.has("displayFiltered") ? parser.get<bool>("displayFiltered") : displayFilteredPose;
}
// -- Step 2: Find out the 2D/3D correspondences
vector<Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
vector<Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
std::cout << "Video: " << video_read_path << std::endl;
std::cout << "Training data: " << yml_read_path << std::endl;
std::cout << "CAD model: " << ply_read_path << std::endl;
std::cout << "Ratio test threshold: " << ratioTest << std::endl;
std::cout << "Fast match(no symmetry test)?: " << fast_match << std::endl;
std::cout << "RANSAC number of iterations: " << iterationsCount << std::endl;
std::cout << "RANSAC reprojection error: " << reprojectionError << std::endl;
std::cout << "RANSAC confidence threshold: " << confidence << std::endl;
std::cout << "Kalman number of inliers: " << minInliersKalman << std::endl;
std::cout << "PnP method: " << pnpMethod << std::endl;
std::cout << "Feature: " << featureName << std::endl;
std::cout << "Number of keypoints for ORB: " << numKeyPoints << std::endl;
std::cout << "Use FLANN-based matching? " << useFLANN << std::endl;
std::cout << "Save directory: " << saveDirectory << std::endl;
std::cout << "Display filtered pose from Kalman filter? " << displayFilteredPose << std::endl;
PnPProblem pnp_detection(params_WEBCAM);
PnPProblem pnp_detection_est(params_WEBCAM);
Model model; // instantiate Model object
model.load(yml_read_path); // load a 3D textured object model
Mesh mesh; // instantiate Mesh object
mesh.load(ply_read_path); // load an object mesh
RobustMatcher rmatcher; // instantiate RobustMatcher
Ptr<FeatureDetector> detector, descriptor;
createFeatures(featureName, numKeyPoints, detector, descriptor);
rmatcher.setFeatureDetector(detector); // set feature detector
rmatcher.setDescriptorExtractor(descriptor); // set descriptor extractor
rmatcher.setDescriptorMatcher(createMatcher(featureName, useFLANN)); // set matcher
rmatcher.setRatio(ratioTest); // set ratio test parameter
if (!model.get_trainingImagePath().empty())
{
Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 3D point from model
Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 2D point from the scene
list_points3d_model_match.push_back(point3d_model); // add 3D point
list_points2d_scene_match.push_back(point2d_scene); // add 2D point
Mat trainingImg = imread(model.get_trainingImagePath());
rmatcher.setTrainingImage(trainingImg);
}
// Draw outliers
draw2DPoints(frame_vis, list_points2d_scene_match, red);
Mat inliers_idx;
vector<Point2f> list_points2d_inliers;
if(good_matches.size() >= 4) // OpenCV requires solvePnPRANSAC to minimally have 4 set of points
{
// -- Step 3: Estimate the pose using RANSAC approach
pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match,
pnpMethod, inliers_idx,
iterationsCount, reprojectionError, confidence );
// -- Step 4: Catch the inliers keypoints to draw
for(int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index)
{
int n = inliers_idx.at<int>(inliers_index); // i-inlier
Point2f point2d = list_points2d_scene_match[n]; // i-inlier point 2D
list_points2d_inliers.push_back(point2d); // add i-inlier to list
}
// Draw inliers points 2D
draw2DPoints(frame_vis, list_points2d_inliers, blue);
KalmanFilter KF; // instantiate Kalman Filter
int nStates = 18; // the number of states
int nMeasurements = 6; // the number of measured states
int nInputs = 0; // the number of control actions
double dt = 0.125; // time between measurements (1/FPS)
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function
Mat measurements(nMeasurements, 1, CV_64FC1); measurements.setTo(Scalar(0));
bool good_measurement = false;
// -- Step 5: Kalman Filter
// Get the MODEL INFO
vector<Point3f> list_points3d_model = model.get_points3d(); // list with model 3D coordinates
Mat descriptors_model = model.get_descriptors(); // list with descriptors of each 3D coordinate
vector<KeyPoint> keypoints_model = model.get_keypoints();
good_measurement = false;
// Create & Open Window
namedWindow("REAL TIME DEMO", WINDOW_KEEPRATIO);
// GOOD MEASUREMENT
if( inliers_idx.rows >= minInliersKalman )
{
VideoCapture cap; // instantiate VideoCapture
cap.open(video_read_path); // open a recorded video
// Get the measured translation
Mat translation_measured(3, 1, CV_64F);
translation_measured = pnp_detection.get_t_matrix();
// Get the measured rotation
Mat rotation_measured(3, 3, CV_64F);
rotation_measured = pnp_detection.get_R_matrix();
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
good_measurement = true;
}
// Instantiate estimated translation and rotation
Mat translation_estimated(3, 1, CV_64F);
Mat rotation_estimated(3, 3, CV_64F);
// update the Kalman filter with good measurements
updateKalmanFilter( KF, measurements,
translation_estimated, rotation_estimated);
// -- Step 6: Set estimated projection matrix
pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
}
// -- Step X: Draw pose
if(good_measurement)
if(!cap.isOpened()) // check if we succeeded
{
drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose
cout << "Could not open the camera device" << endl;
return -1;
}
else
if (!saveDirectory.empty())
{
drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
if (!cv::utils::fs::exists(saveDirectory))
{
std::cout << "Create directory: " << saveDirectory << std::endl;
cv::utils::fs::createDirectories(saveDirectory);
}
}
float l = 5;
vector<Point2f> pose_points2d;
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,0))); // axis center
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(l,0,0))); // axis x
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,l,0))); // axis y
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,l))); // axis z
draw3DCoordinateAxes(frame_vis, pose_points2d); // draw axes
// FRAME RATE
// see how much time has elapsed
time(&end);
// calculate current FPS
++counter;
sec = difftime (end, start);
fps = counter / sec;
drawFPS(frame_vis, fps, yellow); // frame ratio
double detection_ratio = ((double)inliers_idx.rows/(double)good_matches.size())*100;
drawConfidence(frame_vis, detection_ratio, yellow);
// Measure elapsed time
TickMeter tm;
// -- Step X: Draw some debugging text
// Draw some debug text
int inliers_int = inliers_idx.rows;
int outliers_int = (int)good_matches.size() - inliers_int;
string inliers_str = IntToString(inliers_int);
string outliers_str = IntToString(outliers_int);
string n = IntToString((int)good_matches.size());
string text = "Found " + inliers_str + " of " + n + " matches";
string text2 = "Inliers: " + inliers_str + " - Outliers: " + outliers_str;
drawText(frame_vis, text, green);
drawText2(frame_vis, text2, red);
imshow("REAL TIME DEMO", frame_vis);
}
// Close and Destroy Window
destroyWindow("REAL TIME DEMO");
Mat frame, frame_vis, frame_matching;
while(cap.read(frame) && (char)waitKey(30) != 27) // capture frame until ESC is pressed
{
tm.reset();
tm.start();
frame_vis = frame.clone(); // refresh visualisation frame
// -- Step 1: Robust matching between model descriptors and scene descriptors
vector<DMatch> good_matches; // to obtain the 3D points of the model
vector<KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
if(fast_match)
{
rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model, keypoints_model);
}
else
{
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model, keypoints_model);
}
frame_matching = rmatcher.getImageMatching();
if (!frame_matching.empty())
{
imshow("Keypoints matching", frame_matching);
}
// -- Step 2: Find out the 2D/3D correspondences
vector<Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
vector<Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
{
Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 3D point from model
Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 2D point from the scene
list_points3d_model_match.push_back(point3d_model); // add 3D point
list_points2d_scene_match.push_back(point2d_scene); // add 2D point
}
// Draw outliers
draw2DPoints(frame_vis, list_points2d_scene_match, red);
Mat inliers_idx;
vector<Point2f> list_points2d_inliers;
// Instantiate estimated translation and rotation
good_measurement = false;
if(good_matches.size() >= 4) // OpenCV requires solvePnPRANSAC to minimally have 4 set of points
{
// -- Step 3: Estimate the pose using RANSAC approach
pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match,
pnpMethod, inliers_idx,
iterationsCount, reprojectionError, confidence );
// -- Step 4: Catch the inliers keypoints to draw
for(int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index)
{
int n = inliers_idx.at<int>(inliers_index); // i-inlier
Point2f point2d = list_points2d_scene_match[n]; // i-inlier point 2D
list_points2d_inliers.push_back(point2d); // add i-inlier to list
}
// Draw inliers points 2D
draw2DPoints(frame_vis, list_points2d_inliers, blue);
// -- Step 5: Kalman Filter
// GOOD MEASUREMENT
if( inliers_idx.rows >= minInliersKalman )
{
// Get the measured translation
Mat translation_measured = pnp_detection.get_t_matrix();
// Get the measured rotation
Mat rotation_measured = pnp_detection.get_R_matrix();
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
good_measurement = true;
}
// update the Kalman filter with good measurements, otherwise with previous valid measurements
Mat translation_estimated(3, 1, CV_64FC1);
Mat rotation_estimated(3, 3, CV_64FC1);
updateKalmanFilter( KF, measurements,
translation_estimated, rotation_estimated);
// -- Step 6: Set estimated projection matrix
pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
}
// -- Step X: Draw pose and coordinate frame
float l = 5;
vector<Point2f> pose_points2d;
if (!good_measurement || displayFilteredPose)
{
drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,0))); // axis center
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(l,0,0))); // axis x
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,l,0))); // axis y
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,l))); // axis z
draw3DCoordinateAxes(frame_vis, pose_points2d); // draw axes
}
else
{
drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose
pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,0,0))); // axis center
pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(l,0,0))); // axis x
pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,l,0))); // axis y
pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,0,l))); // axis z
draw3DCoordinateAxes(frame_vis, pose_points2d); // draw axes
}
// FRAME RATE
// see how much time has elapsed
tm.stop();
// calculate current FPS
double fps = 1.0 / tm.getTimeSec();
drawFPS(frame_vis, fps, yellow); // frame ratio
double detection_ratio = ((double)inliers_idx.rows/(double)good_matches.size())*100;
drawConfidence(frame_vis, detection_ratio, yellow);
// -- Step X: Draw some debugging text
// Draw some debug text
int inliers_int = inliers_idx.rows;
int outliers_int = (int)good_matches.size() - inliers_int;
string inliers_str = IntToString(inliers_int);
string outliers_str = IntToString(outliers_int);
string n = IntToString((int)good_matches.size());
string text = "Found " + inliers_str + " of " + n + " matches";
string text2 = "Inliers: " + inliers_str + " - Outliers: " + outliers_str;
drawText(frame_vis, text, green);
drawText2(frame_vis, text2, red);
imshow("REAL TIME DEMO", frame_vis);
if (!saveDirectory.empty())
{
const int widthSave = !frame_matching.empty() ? frame_matching.cols : frame_vis.cols;
const int heightSave = !frame_matching.empty() ? frame_matching.rows + frame_vis.rows : frame_vis.rows;
frameSave = Mat::zeros(heightSave, widthSave, CV_8UC3);
if (!frame_matching.empty())
{
int startX = (int)((widthSave - frame_vis.cols) / 2.0);
Mat roi = frameSave(Rect(startX, 0, frame_vis.cols, frame_vis.rows));
frame_vis.copyTo(roi);
roi = frameSave(Rect(0, frame_vis.rows, frame_matching.cols, frame_matching.rows));
frame_matching.copyTo(roi);
}
else
{
frame_vis.copyTo(frameSave);
}
string saveFilename = format(string(saveDirectory + "/image_%04d.png").c_str(), frameCount);
imwrite(saveFilename, frameSave);
frameCount++;
}
}
cout << "GOODBYE ..." << endl;
// Close and Destroy Window
destroyWindow("REAL TIME DEMO");
cout << "GOODBYE ..." << endl;
}
/**********************************************************************************************************/
void help()
{
cout
<< "--------------------------------------------------------------------------" << endl
<< "This program shows how to detect an object given its 3D textured model. You can choose to "
<< "use a recorded video or the webcam." << endl
<< "Usage:" << endl
<< "./cpp-tutorial-pnp_detection -help" << endl
<< "Keys:" << endl
<< "'esc' - to quit." << endl
<< "--------------------------------------------------------------------------" << endl
<< endl;
cout
<< "--------------------------------------------------------------------------" << endl
<< "This program shows how to detect an object given its 3D textured model. You can choose to "
<< "use a recorded video or the webcam." << endl
<< "Usage:" << endl
<< "./cpp-tutorial-pnp_detection -help" << endl
<< "Keys:" << endl
<< "'esc' - to quit." << endl
<< "--------------------------------------------------------------------------" << endl
<< endl;
}
/**********************************************************************************************************/
void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt)
{
KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter
setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); // set process noise
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-2)); // set measurement noise
setIdentity(KF.errorCovPost, Scalar::all(1)); // error covariance
/** DYNAMIC MODEL **/
// [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0]
// [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
// [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
// position
KF.transitionMatrix.at<double>(0,3) = dt;
KF.transitionMatrix.at<double>(1,4) = dt;
KF.transitionMatrix.at<double>(2,5) = dt;
KF.transitionMatrix.at<double>(3,6) = dt;
KF.transitionMatrix.at<double>(4,7) = dt;
KF.transitionMatrix.at<double>(5,8) = dt;
KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2);
// orientation
KF.transitionMatrix.at<double>(9,12) = dt;
KF.transitionMatrix.at<double>(10,13) = dt;
KF.transitionMatrix.at<double>(11,14) = dt;
KF.transitionMatrix.at<double>(12,15) = dt;
KF.transitionMatrix.at<double>(13,16) = dt;
KF.transitionMatrix.at<double>(14,17) = dt;
KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2);
/** MEASUREMENT MODEL **/
// [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
KF.measurementMatrix.at<double>(0,0) = 1; // x
KF.measurementMatrix.at<double>(1,1) = 1; // y
KF.measurementMatrix.at<double>(2,2) = 1; // z
KF.measurementMatrix.at<double>(3,9) = 1; // roll
KF.measurementMatrix.at<double>(4,10) = 1; // pitch
KF.measurementMatrix.at<double>(5,11) = 1; // yaw
KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter
setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); // set process noise
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-2)); // set measurement noise
setIdentity(KF.errorCovPost, Scalar::all(1)); // error covariance
/** DYNAMIC MODEL **/
// [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0]
// [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
// [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
// position
KF.transitionMatrix.at<double>(0,3) = dt;
KF.transitionMatrix.at<double>(1,4) = dt;
KF.transitionMatrix.at<double>(2,5) = dt;
KF.transitionMatrix.at<double>(3,6) = dt;
KF.transitionMatrix.at<double>(4,7) = dt;
KF.transitionMatrix.at<double>(5,8) = dt;
KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2);
// orientation
KF.transitionMatrix.at<double>(9,12) = dt;
KF.transitionMatrix.at<double>(10,13) = dt;
KF.transitionMatrix.at<double>(11,14) = dt;
KF.transitionMatrix.at<double>(12,15) = dt;
KF.transitionMatrix.at<double>(13,16) = dt;
KF.transitionMatrix.at<double>(14,17) = dt;
KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2);
KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2);
/** MEASUREMENT MODEL **/
// [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
KF.measurementMatrix.at<double>(0,0) = 1; // x
KF.measurementMatrix.at<double>(1,1) = 1; // y
KF.measurementMatrix.at<double>(2,2) = 1; // z
KF.measurementMatrix.at<double>(3,9) = 1; // roll
KF.measurementMatrix.at<double>(4,10) = 1; // pitch
KF.measurementMatrix.at<double>(5,11) = 1; // yaw
}
/**********************************************************************************************************/
void updateKalmanFilter( KalmanFilter &KF, Mat &measurement,
Mat &translation_estimated, Mat &rotation_estimated )
{
// First predict, to update the internal statePre variable
Mat prediction = KF.predict();
// First predict, to update the internal statePre variable
Mat prediction = KF.predict();
// The "correct" phase that is going to use the predicted value and our measurement
Mat estimated = KF.correct(measurement);
// Estimated translation
translation_estimated.at<double>(0) = estimated.at<double>(0);
translation_estimated.at<double>(1) = estimated.at<double>(1);
translation_estimated.at<double>(2) = estimated.at<double>(2);
// The "correct" phase that is going to use the predicted value and our measurement
Mat estimated = KF.correct(measurement);
// Estimated euler angles
Mat eulers_estimated(3, 1, CV_64F);
eulers_estimated.at<double>(0) = estimated.at<double>(9);
eulers_estimated.at<double>(1) = estimated.at<double>(10);
eulers_estimated.at<double>(2) = estimated.at<double>(11);
// Estimated translation
translation_estimated.at<double>(0) = estimated.at<double>(0);
translation_estimated.at<double>(1) = estimated.at<double>(1);
translation_estimated.at<double>(2) = estimated.at<double>(2);
// Convert estimated quaternion to rotation matrix
rotation_estimated = euler2rot(eulers_estimated);
// Estimated euler angles
Mat eulers_estimated(3, 1, CV_64F);
eulers_estimated.at<double>(0) = estimated.at<double>(9);
eulers_estimated.at<double>(1) = estimated.at<double>(10);
eulers_estimated.at<double>(2) = estimated.at<double>(11);
// Convert estimated quaternion to rotation matrix
rotation_estimated = euler2rot(eulers_estimated);
}
/**********************************************************************************************************/
void fillMeasurements( Mat &measurements,
const Mat &translation_measured, const Mat &rotation_measured)
{
// Convert rotation matrix to euler angles
Mat measured_eulers(3, 1, CV_64F);
measured_eulers = rot2euler(rotation_measured);
// Set measurement to predict
measurements.at<double>(0) = translation_measured.at<double>(0); // x
measurements.at<double>(1) = translation_measured.at<double>(1); // y
measurements.at<double>(2) = translation_measured.at<double>(2); // z
measurements.at<double>(3) = measured_eulers.at<double>(0); // roll
measurements.at<double>(4) = measured_eulers.at<double>(1); // pitch
measurements.at<double>(5) = measured_eulers.at<double>(2); // yaw
// Convert rotation matrix to euler angles
Mat measured_eulers(3, 1, CV_64F);
measured_eulers = rot2euler(rotation_measured);
// Set measurement to predict
measurements.at<double>(0) = translation_measured.at<double>(0); // x
measurements.at<double>(1) = translation_measured.at<double>(1); // y
measurements.at<double>(2) = translation_measured.at<double>(2); // z
measurements.at<double>(3) = measured_eulers.at<double>(0); // roll
measurements.at<double>(4) = measured_eulers.at<double>(1); // pitch
measurements.at<double>(5) = measured_eulers.at<double>(2); // yaw
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp
View file @ 3c92d40f
......@@ -18,34 +18,22 @@ using namespace std;
/** GLOBAL VARIABLES **/
string tutorial_path = "../../samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/"; // path to tutorial
string img_path = tutorial_path + "Data/resized_IMG_3875.JPG"; // image to register
string ply_read_path = tutorial_path + "Data/box.ply"; // object mesh
string write_path = tutorial_path + "Data/cookies_ORB.yml"; // output file
// Boolean the know if the registration it's done
bool end_registration = false;
// Intrinsic camera parameters: UVC WEBCAM
double f = 45; // focal length in mm
double sx = 22.3, sy = 14.9;
double width = 2592, height = 1944;
double params_CANON[] = { width*f/sx, // fx
height*f/sy, // fy
width/2, // cx
height/2}; // cy
const double f = 45; // focal length in mm
const double sx = 22.3, sy = 14.9;
const double width = 2592, height = 1944;
const double params_CANON[] = { width*f/sx, // fx
height*f/sy, // fy
width/2, // cx
height/2}; // cy
// Setup the points to register in the image
// In the order of the *.ply file and starting at 1
int n = 8;
int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
// Some basic colors
Scalar red(0, 0, 255);
Scalar green(0,255,0);
Scalar blue(255,0,0);
Scalar yellow(0,255,255);
const int n = 8;
const int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
/*
* CREATE MODEL REGISTRATION OBJECT
......@@ -58,211 +46,248 @@ Model model;
Mesh mesh;
PnPProblem pnp_registration(params_CANON);
/** Functions headers **/
void help();
/**********************************************************************************************************/
static void help()
{
cout
<< "--------------------------------------------------------------------------" << endl
<< "This program shows how to create your 3D textured model. " << endl
<< "Usage:" << endl
<< "./cpp-tutorial-pnp_registration" << endl
<< "--------------------------------------------------------------------------" << endl
<< endl;
}
// Mouse events for model registration
static void onMouseModelRegistration( int event, int x, int y, int, void* )
{
if ( event == EVENT_LBUTTONUP )
{
int n_regist = registration.getNumRegist();
int n_vertex = pts[n_regist];
Point2f point_2d = Point2f((float)x,(float)y);
Point3f point_3d = mesh.getVertex(n_vertex-1);
bool is_registrable = registration.is_registrable();
if (is_registrable)
{
registration.registerPoint(point_2d, point_3d);
if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
}
}
if ( event == EVENT_LBUTTONUP )
{
bool is_registrable = registration.is_registrable();
if (is_registrable)
{
int n_regist = registration.getNumRegist();
int n_vertex = pts[n_regist];
Point2f point_2d = Point2f((float)x,(float)y);
Point3f point_3d = mesh.getVertex(n_vertex-1);
registration.registerPoint(point_2d, point_3d);
if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
}
}
}
/** Main program **/
int main()
int main(int argc, char *argv[])
{
help();
// load a mesh given the *.ply file path
mesh.load(ply_read_path);
// set parameters
int numKeyPoints = 10000;
//Instantiate robust matcher: detector, extractor, matcher
RobustMatcher rmatcher;
Ptr<FeatureDetector> detector = ORB::create(numKeyPoints);
rmatcher.setFeatureDetector(detector);
/** GROUND TRUTH OF THE FIRST IMAGE **/
// Create & Open Window
namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);
// Set up the mouse events
setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 );
// Open the image to register
Mat img_in = imread(img_path, IMREAD_COLOR);
Mat img_vis = img_in.clone();
if (!img_in.data) {
cout << "Could not open or find the image" << endl;
return -1;
}
// Set the number of points to register
int num_registrations = n;
registration.setNumMax(num_registrations);
cout << "Click the box corners ..." << endl;
cout << "Waiting ..." << endl;
// Loop until all the points are registered
while ( waitKey(30) < 0 )
{
// Refresh debug image
img_vis = img_in.clone();
// Current registered points
vector<Point2f> list_points2d = registration.get_points2d();
vector<Point3f> list_points3d = registration.get_points3d();
// Draw current registered points
drawPoints(img_vis, list_points2d, list_points3d, red);
// If the registration is not finished, draw which 3D point we have to register.
// If the registration is finished, breaks the loop.
if (!end_registration)
help();
const String keys =
"{help h | | print this message }"
"{image i | | path to input image }"
"{model | | path to output yml model }"
"{mesh | | path to ply mesh }"
"{keypoints k |2000 | number of keypoints to detect (only for ORB) }"
"{feature |ORB | feature name (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG) }"
;
CommandLineParser parser(argc, argv, keys);
string img_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/resized_IMG_3875.JPG"); // image to register
string ply_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply"); // object mesh
string write_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml"); // output file
int numKeyPoints = 2000;
string featureName = "ORB";
if (parser.has("help"))
{
// Draw debug text
int n_regist = registration.getNumRegist();
int n_vertex = pts[n_regist];
Point3f current_poin3d = mesh.getVertex(n_vertex-1);
drawQuestion(img_vis, current_poin3d, green);
drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);
parser.printMessage();
return 0;
}
else
{
// Draw debug text
drawText(img_vis, "END REGISTRATION", green);
drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
break;
img_path = parser.get<string>("image").size() > 0 ? parser.get<string>("image") : img_path;
ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
write_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : write_path;
numKeyPoints = parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
featureName = parser.has("feature") ? parser.get<string>("feature") : featureName;
}
// Show the image
imshow("MODEL REGISTRATION", img_vis);
}
/** COMPUTE CAMERA POSE **/
std::cout << "Input image: " << img_path << std::endl;
std::cout << "CAD model: " << ply_read_path << std::endl;
std::cout << "Output training file: " << write_path << std::endl;
std::cout << "Feature: " << featureName << std::endl;
std::cout << "Number of keypoints for ORB: " << numKeyPoints << std::endl;
cout << "COMPUTING POSE ..." << endl;
// load a mesh given the *.ply file path
mesh.load(ply_read_path);
// The list of registered points
vector<Point2f> list_points2d = registration.get_points2d();
vector<Point3f> list_points3d = registration.get_points3d();
//Instantiate robust matcher: detector, extractor, matcher
RobustMatcher rmatcher;
Ptr<Feature2D> detector, descriptor;
createFeatures(featureName, numKeyPoints, detector, descriptor);
rmatcher.setFeatureDetector(detector);
rmatcher.setDescriptorExtractor(descriptor);
// Estimate pose given the registered points
bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, SOLVEPNP_ITERATIVE);
if ( is_correspondence )
{
cout << "Correspondence found" << endl;
// Compute all the 2D points of the mesh to verify the algorithm and draw it
vector<Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh);
draw2DPoints(img_vis, list_points2d_mesh, green);
/** GROUND TRUTH OF THE FIRST IMAGE **/
} else {
cout << "Correspondence not found" << endl << endl;
}
// Create & Open Window
namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);
// Show the image
imshow("MODEL REGISTRATION", img_vis);
// Set up the mouse events
setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0);
// Show image until ESC pressed
waitKey(0);
// Open the image to register
Mat img_in = imread(img_path, IMREAD_COLOR);
Mat img_vis;
if (img_in.empty()) {
cout << "Could not open or find the image" << endl;
return -1;
}
/** COMPUTE 3D of the image Keypoints **/
// Set the number of points to register
int num_registrations = n;
registration.setNumMax(num_registrations);
// Containers for keypoints and descriptors of the model
vector<KeyPoint> keypoints_model;
Mat descriptors;
cout << "Click the box corners ..." << endl;
cout << "Waiting ..." << endl;
// Compute keypoints and descriptors
rmatcher.computeKeyPoints(img_in, keypoints_model);
rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);
// Some basic colors
const Scalar red(0, 0, 255);
const Scalar green(0,255,0);
const Scalar blue(255,0,0);
const Scalar yellow(0,255,255);
// Check if keypoints are on the surface of the registration image and add to the model
for (unsigned int i = 0; i < keypoints_model.size(); ++i) {
Point2f point2d(keypoints_model[i].pt);
Point3f point3d;
bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
if (on_surface)
// Loop until all the points are registered
while ( waitKey(30) < 0 )
{
model.add_correspondence(point2d, point3d);
model.add_descriptor(descriptors.row(i));
model.add_keypoint(keypoints_model[i]);
// Refresh debug image
img_vis = img_in.clone();
// Current registered points
vector<Point2f> list_points2d = registration.get_points2d();
vector<Point3f> list_points3d = registration.get_points3d();
// Draw current registered points
drawPoints(img_vis, list_points2d, list_points3d, red);
// If the registration is not finished, draw which 3D point we have to register.
// If the registration is finished, breaks the loop.
if (!end_registration)
{
// Draw debug text
int n_regist = registration.getNumRegist();
int n_vertex = pts[n_regist];
Point3f current_poin3d = mesh.getVertex(n_vertex-1);
drawQuestion(img_vis, current_poin3d, green);
drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);
}
else
{
// Draw debug text
drawText(img_vis, "END REGISTRATION", green);
drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
break;
}
// Show the image
imshow("MODEL REGISTRATION", img_vis);
}
else
/** COMPUTE CAMERA POSE **/
cout << "COMPUTING POSE ..." << endl;
// The list of registered points
vector<Point2f> list_points2d = registration.get_points2d();
vector<Point3f> list_points3d = registration.get_points3d();
// Estimate pose given the registered points
bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, SOLVEPNP_ITERATIVE);
if ( is_correspondence )
{
model.add_outlier(point2d);
cout << "Correspondence found" << endl;
// Compute all the 2D points of the mesh to verify the algorithm and draw it
vector<Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh);
draw2DPoints(img_vis, list_points2d_mesh, green);
} else {
cout << "Correspondence not found" << endl << endl;
}
}
// save the model into a *.yaml file
model.save(write_path);
// Show the image
imshow("MODEL REGISTRATION", img_vis);
// Out image
img_vis = img_in.clone();
// Show image until ESC pressed
waitKey(0);
/** COMPUTE 3D of the image Keypoints **/
// Containers for keypoints and descriptors of the model
vector<KeyPoint> keypoints_model;
Mat descriptors;
// Compute keypoints and descriptors
rmatcher.computeKeyPoints(img_in, keypoints_model);
rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);
// Check if keypoints are on the surface of the registration image and add to the model
for (unsigned int i = 0; i < keypoints_model.size(); ++i) {
Point2f point2d(keypoints_model[i].pt);
Point3f point3d;
bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
if (on_surface)
{
model.add_correspondence(point2d, point3d);
model.add_descriptor(descriptors.row(i));
model.add_keypoint(keypoints_model[i]);
}
else
{
model.add_outlier(point2d);
}
}
// The list of the points2d of the model
vector<Point2f> list_points_in = model.get_points2d_in();
vector<Point2f> list_points_out = model.get_points2d_out();
model.set_trainingImagePath(img_path);
// save the model into a *.yaml file
model.save(write_path);
// Draw some debug text
string num = IntToString((int)list_points_in.size());
string text = "There are " + num + " inliers";
drawText(img_vis, text, green);
// Out image
img_vis = img_in.clone();
// Draw some debug text
num = IntToString((int)list_points_out.size());
text = "There are " + num + " outliers";
drawText2(img_vis, text, red);
// The list of the points2d of the model
vector<Point2f> list_points_in = model.get_points2d_in();
vector<Point2f> list_points_out = model.get_points2d_out();
// Draw the object mesh
drawObjectMesh(img_vis, &mesh, &pnp_registration, blue);
// Draw some debug text
string num = IntToString((int)list_points_in.size());
string text = "There are " + num + " inliers";
drawText(img_vis, text, green);
// Draw found keypoints depending on if are or not on the surface
draw2DPoints(img_vis, list_points_in, green);
draw2DPoints(img_vis, list_points_out, red);
// Draw some debug text
num = IntToString((int)list_points_out.size());
text = "There are " + num + " outliers";
drawText2(img_vis, text, red);
// Show the image
imshow("MODEL REGISTRATION", img_vis);
// Draw the object mesh
drawObjectMesh(img_vis, &mesh, &pnp_registration, blue);
// Wait until ESC pressed
waitKey(0);
// Draw found keypoints depending on if are or not on the surface
draw2DPoints(img_vis, list_points_in, green);
draw2DPoints(img_vis, list_points_out, red);
// Close and Destroy Window
destroyWindow("MODEL REGISTRATION");
// Show the image
imshow("MODEL REGISTRATION", img_vis);
cout << "GOODBYE" << endl;
// Wait until ESC pressed
waitKey(0);
}
// Close and Destroy Window
destroyWindow("MODEL REGISTRATION");
/**********************************************************************************************************/
void help()
{
cout
<< "--------------------------------------------------------------------------" << endl
<< "This program shows how to create your 3D textured model. " << endl
<< "Usage:" << endl
<< "./cpp-tutorial-pnp_registration" << endl
<< "--------------------------------------------------------------------------" << endl
<< endl;
cout << "GOODBYE" << endl;
}
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