Skip to content

O
opencv
Commit 9b71f5fd authored by Alexander Alekhin's avatar Alexander Alekhin
Browse files
Options

Merge pull request #13835 from catree:real_time_pose_tutorial_keypoints_matching

parents 428720f4 3c92d40f
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 @ 9b71f5fd
#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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
#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 @ 9b71f5fd
#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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
......@@ -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 @ 9b71f5fd
// 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 @ 9b71f5fd
......@@ -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