mirror of
https://github.com/opencv/opencv.git
synced 2024-11-29 13:47:32 +08:00
Merge pull request #13835 from catree:real_time_pose_tutorial_keypoints_matching
This commit is contained in:
Alexander Alekhin
commit
9b71f5fd54
@ -1,7 +1,7 @@
|
||||
#include "CsvReader.h"
|
||||
|
||||
/** The default constructor of the CSV reader Class */
|
||||
CsvReader::CsvReader(const string &path, const char &separator){
|
||||
CsvReader::CsvReader(const string &path, char separator){
|
||||
_file.open(path.c_str(), ifstream::in);
|
||||
_separator = separator;
|
||||
}
|
||||
|
||||
@ -19,7 +19,7 @@ public:
|
||||
* @param separator - The separator character between words per line
|
||||
* @return
|
||||
*/
|
||||
CsvReader(const string &path, const char &separator = ' ');
|
||||
CsvReader(const string &path, char separator = ' ');
|
||||
|
||||
/**
|
||||
* Read a plane text file with .ply format
|
||||
|
||||
@ -13,34 +13,31 @@ CsvWriter::~CsvWriter() {
|
||||
|
||||
void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d)
|
||||
{
|
||||
string x, y, z;
|
||||
for(unsigned int i = 0; i < list_points3d.size(); ++i)
|
||||
for(size_t 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);
|
||||
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)
|
||||
for(size_t 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);
|
||||
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)
|
||||
{
|
||||
descriptor_str = FloatToString(descriptors.at<float>(i,j));
|
||||
string descriptor_str = FloatToString(descriptors.at<float>((int)i,j));
|
||||
_file << _separator << descriptor_str;
|
||||
}
|
||||
_file << std::endl;
|
||||
|
||||
@ -14,9 +14,9 @@
|
||||
// --------------------------------------------------- //
|
||||
|
||||
/** 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 */
|
||||
@ -31,8 +31,9 @@ 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 */
|
||||
@ -47,11 +48,9 @@ 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 */
|
||||
@ -60,11 +59,9 @@ Mesh::~Mesh()
|
||||
// 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);
|
||||
|
||||
@ -76,7 +73,6 @@ void Mesh::load(const std::string path)
|
||||
csvReader.readPLY(list_vertex_, list_triangles_);
|
||||
|
||||
// Update mesh attributes
|
||||
num_vertexs_ = (int)list_vertex_.size();
|
||||
num_vertices_ = (int)list_vertex_.size();
|
||||
num_triangles_ = (int)list_triangles_.size();
|
||||
|
||||
}
|
||||
|
||||
@ -19,7 +19,7 @@
|
||||
class Triangle {
|
||||
public:
|
||||
|
||||
explicit Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2);
|
||||
explicit Triangle(const cv::Point3f& V0, const cv::Point3f& V1, const cv::Point3f& V2);
|
||||
virtual ~Triangle();
|
||||
|
||||
cv::Point3f getV0() const { return v0_; }
|
||||
@ -27,8 +27,6 @@ public:
|
||||
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_;
|
||||
};
|
||||
@ -41,7 +39,7 @@ private:
|
||||
class Ray {
|
||||
public:
|
||||
|
||||
explicit Ray(cv::Point3f P0, cv::Point3f P1);
|
||||
explicit Ray(const cv::Point3f& P0, const cv::Point3f& P1);
|
||||
virtual ~Ray();
|
||||
|
||||
cv::Point3f getP0() { return p0_; }
|
||||
@ -66,15 +64,13 @@ public:
|
||||
|
||||
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_; }
|
||||
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_;
|
||||
int num_vertices_;
|
||||
/** The current number of triangles in the mesh */
|
||||
int num_triangles_;
|
||||
/* The list of triangles of the mesh */
|
||||
|
||||
@ -8,9 +8,8 @@
|
||||
#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()
|
||||
@ -40,34 +39,45 @@ void Model::add_keypoint(const cv::KeyPoint &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::FileStorage storage(path, cv::FileStorage::WRITE);
|
||||
storage << "points_3d" << points3dmatrix;
|
||||
storage << "points_2d" << points2dmatrix;
|
||||
storage << "keypoints" << list_keypoints_;
|
||||
storage << "descriptors" << descriptors_;
|
||||
storage << "training_image_path" << training_img_path_;
|
||||
|
||||
storage.release();
|
||||
}
|
||||
|
||||
/** Load a YAML file using OpenCv functions **/
|
||||
void Model::load(const std::string path)
|
||||
void Model::load(const std::string &path)
|
||||
{
|
||||
cv::Mat points3d_mat;
|
||||
|
||||
cv::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();
|
||||
|
||||
}
|
||||
|
||||
@ -24,17 +24,16 @@ public:
|
||||
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);
|
||||
|
||||
void save(const std::string &path);
|
||||
void load(const std::string &path);
|
||||
|
||||
private:
|
||||
/** The current number of correspondecnes */
|
||||
@ -49,6 +48,8 @@ private:
|
||||
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_ */
|
||||
|
||||
@ -7,10 +7,9 @@
|
||||
|
||||
#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()
|
||||
@ -19,12 +18,12 @@ ModelRegistration::~ModelRegistration()
|
||||
}
|
||||
|
||||
void ModelRegistration::registerPoint(const cv::Point2f &point2d, const cv::Point3f &point3d)
|
||||
{
|
||||
{
|
||||
// add correspondence at the end of the vector
|
||||
list_points2d_.push_back(point2d);
|
||||
list_points3d_.push_back(point3d);
|
||||
n_registrations_++;
|
||||
}
|
||||
}
|
||||
|
||||
void ModelRegistration::reset()
|
||||
{
|
||||
|
||||
@ -14,7 +14,6 @@
|
||||
class ModelRegistration
|
||||
{
|
||||
public:
|
||||
|
||||
ModelRegistration();
|
||||
virtual ~ModelRegistration();
|
||||
|
||||
@ -30,14 +29,14 @@ public:
|
||||
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_ */
|
||||
|
||||
@ -13,16 +13,8 @@
|
||||
|
||||
#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;
|
||||
@ -31,12 +23,12 @@ cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
@ -45,11 +37,10 @@ cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
|
||||
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];
|
||||
@ -71,15 +62,15 @@ cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Poin
|
||||
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
@ -91,21 +82,20 @@ PnPProblem::~PnPProblem()
|
||||
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);
|
||||
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,
|
||||
@ -118,15 +108,15 @@ bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
|
||||
bool useExtrinsicGuess = false;
|
||||
|
||||
// Pose estimation
|
||||
bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
|
||||
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;
|
||||
Rodrigues(rvec, R_matrix_);
|
||||
t_matrix_ = tvec;
|
||||
|
||||
// Set projection matrix
|
||||
this->set_P_matrix(_R_matrix, _t_matrix);
|
||||
this->set_P_matrix(R_matrix_, t_matrix_);
|
||||
|
||||
return correspondence;
|
||||
}
|
||||
@ -145,14 +135,14 @@ void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points
|
||||
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,
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
@ -170,9 +160,7 @@ std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
|
||||
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]'
|
||||
@ -184,7 +172,7 @@ cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
|
||||
|
||||
// 2D point vector [u v 1]'
|
||||
cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
|
||||
point2d_vec = _A_matrix * _P_matrix * point3d_vec;
|
||||
point2d_vec = A_matrix_ * P_matrix_ * point3d_vec;
|
||||
|
||||
// Normalization of [u v]'
|
||||
cv::Point2f point2d;
|
||||
@ -211,13 +199,13 @@ bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d
|
||||
point2d_vec.at<double>(2) = lambda;
|
||||
|
||||
// Point in camera coordinates
|
||||
cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1
|
||||
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
|
||||
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
|
||||
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
|
||||
@ -236,7 +224,7 @@ bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d
|
||||
cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
|
||||
cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
|
||||
|
||||
Triangle T(i, V0, V1, V2);
|
||||
Triangle T(V0, V1, V2);
|
||||
|
||||
double out;
|
||||
if(this->intersect_MollerTrumbore(R, T, &out))
|
||||
|
||||
@ -18,7 +18,6 @@
|
||||
|
||||
class PnPProblem
|
||||
{
|
||||
|
||||
public:
|
||||
explicit PnPProblem(const double param[]); // custom constructor
|
||||
virtual ~PnPProblem();
|
||||
@ -32,27 +31,22 @@ public:
|
||||
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);
|
||||
|
||||
private:
|
||||
/** The calibration matrix */
|
||||
cv::Mat _A_matrix;
|
||||
cv::Mat A_matrix_;
|
||||
/** The computed rotation matrix */
|
||||
cv::Mat _R_matrix;
|
||||
cv::Mat R_matrix_;
|
||||
/** The computed translation matrix */
|
||||
cv::Mat _t_matrix;
|
||||
cv::Mat t_matrix_;
|
||||
/** The computed projection matrix */
|
||||
cv::Mat _P_matrix;
|
||||
cv::Mat P_matrix_;
|
||||
};
|
||||
|
||||
// Functions for Möller-Trumbore intersection algorithm
|
||||
cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
|
||||
double DOT(cv::Point3f v1, cv::Point3f v2);
|
||||
cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
|
||||
|
||||
#endif /* PNPPROBLEM_H_ */
|
||||
|
||||
@ -55,12 +55,10 @@ void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& m
|
||||
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;
|
||||
@ -74,27 +72,22 @@ void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& m
|
||||
continue;
|
||||
|
||||
// Match symmetry test
|
||||
if ((*matchIterator1)[0].queryIdx ==
|
||||
(*matchIterator2)[0].trainIdx &&
|
||||
(*matchIterator2)[0].queryIdx ==
|
||||
(*matchIterator1)[0].trainIdx)
|
||||
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));
|
||||
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);
|
||||
|
||||
@ -120,11 +113,16 @@ void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>&
|
||||
// 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 )
|
||||
const cv::Mat& descriptors_model,
|
||||
const std::vector<cv::KeyPoint>& keypoints_model)
|
||||
{
|
||||
good_matches.clear();
|
||||
|
||||
@ -149,4 +147,8 @@ void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatc
|
||||
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_);
|
||||
}
|
||||
}
|
||||
|
||||
@ -16,7 +16,8 @@
|
||||
|
||||
class RobustMatcher {
|
||||
public:
|
||||
RobustMatcher() : ratio_(0.8f)
|
||||
RobustMatcher() : detector_(), extractor_(), matcher_(),
|
||||
ratio_(0.8f), training_img_(), img_matching_()
|
||||
{
|
||||
// ORB is the default feature
|
||||
detector_ = cv::ORB::create();
|
||||
@ -43,9 +44,13 @@ public:
|
||||
// Compute the descriptors of an image given its keypoints
|
||||
void computeDescriptors( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
|
||||
|
||||
cv::Mat getImageMatching() const { return img_matching_; }
|
||||
|
||||
// Set ratio parameter for the ratio test
|
||||
void setRatio( float rat) { ratio_ = rat; }
|
||||
|
||||
void setTrainingImage(const cv::Mat &img) { training_img_ = img; }
|
||||
|
||||
// Clear matches for which NN ratio is > than threshold
|
||||
// return the number of removed points
|
||||
// (corresponding entries being cleared,
|
||||
@ -60,12 +65,14 @@ public:
|
||||
// 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 );
|
||||
const cv::Mat& descriptors_model,
|
||||
const std::vector<cv::KeyPoint>& keypoints_model);
|
||||
|
||||
private:
|
||||
// pointer to the feature point detector object
|
||||
@ -76,6 +83,10 @@ private:
|
||||
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_ */
|
||||
|
||||
@ -11,18 +11,22 @@
|
||||
#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)
|
||||
@ -50,9 +54,8 @@ void drawText2(cv::Mat image, std::string text, cv::Scalar color)
|
||||
// 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
|
||||
@ -141,10 +144,9 @@ void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_po
|
||||
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);
|
||||
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
|
||||
@ -296,3 +298,104 @@ std::string IntToString ( int Number )
|
||||
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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -1,9 +1,8 @@
|
||||
// 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,59 +20,18 @@
|
||||
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 =
|
||||
@ -84,14 +42,63 @@ int main(int argc, char *argv[])
|
||||
"{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 }"
|
||||
"{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}"
|
||||
"{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"))
|
||||
{
|
||||
parser.printMessage();
|
||||
@ -102,16 +109,36 @@ int main(int argc, char *argv[])
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
@ -123,18 +150,17 @@ int main(int argc, char *argv[])
|
||||
|
||||
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
|
||||
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())
|
||||
{
|
||||
Mat trainingImg = imread(model.get_trainingImagePath());
|
||||
rmatcher.setTrainingImage(trainingImg);
|
||||
}
|
||||
|
||||
KalmanFilter KF; // instantiate Kalman Filter
|
||||
int nStates = 18; // the number of states
|
||||
@ -143,19 +169,17 @@ int main(int argc, char *argv[])
|
||||
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));
|
||||
Mat measurements(nMeasurements, 1, CV_64FC1); 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
|
||||
|
||||
vector<KeyPoint> keypoints_model = model.get_keypoints();
|
||||
|
||||
// Create & Open Window
|
||||
namedWindow("REAL TIME DEMO", WINDOW_KEEPRATIO);
|
||||
|
||||
|
||||
VideoCapture cap; // instantiate VideoCapture
|
||||
cap.open(video_read_path); // open a recorded video
|
||||
|
||||
@ -165,45 +189,45 @@ int main(int argc, char *argv[])
|
||||
return -1;
|
||||
}
|
||||
|
||||
// start and end times
|
||||
time_t start, end;
|
||||
if (!saveDirectory.empty())
|
||||
{
|
||||
if (!cv::utils::fs::exists(saveDirectory))
|
||||
{
|
||||
std::cout << "Create directory: " << saveDirectory << std::endl;
|
||||
cv::utils::fs::createDirectories(saveDirectory);
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
// Measure elapsed time
|
||||
TickMeter tm;
|
||||
|
||||
Mat frame, frame_vis, frame_matching;
|
||||
while(cap.read(frame) && (char)waitKey(30) != 27) // capture frame until ESC is pressed
|
||||
{
|
||||
|
||||
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);
|
||||
rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model, keypoints_model);
|
||||
}
|
||||
else
|
||||
{
|
||||
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model);
|
||||
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
|
||||
|
||||
@ -218,13 +242,14 @@ int main(int argc, char *argv[])
|
||||
// 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,
|
||||
@ -241,81 +266,68 @@ int main(int argc, char *argv[])
|
||||
// Draw inliers points 2D
|
||||
draw2DPoints(frame_vis, list_points2d_inliers, blue);
|
||||
|
||||
|
||||
// -- Step 5: Kalman Filter
|
||||
|
||||
good_measurement = false;
|
||||
|
||||
// GOOD MEASUREMENT
|
||||
if( inliers_idx.rows >= minInliersKalman )
|
||||
{
|
||||
|
||||
// Get the measured translation
|
||||
Mat translation_measured(3, 1, CV_64F);
|
||||
translation_measured = pnp_detection.get_t_matrix();
|
||||
Mat 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();
|
||||
Mat 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
|
||||
// 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
|
||||
|
||||
if(good_measurement)
|
||||
{
|
||||
drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose
|
||||
}
|
||||
else
|
||||
{
|
||||
drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
|
||||
}
|
||||
|
||||
// -- 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
|
||||
time(&end);
|
||||
tm.stop();
|
||||
|
||||
// calculate current FPS
|
||||
++counter;
|
||||
sec = difftime (end, start);
|
||||
|
||||
fps = counter / sec;
|
||||
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;
|
||||
@ -329,41 +341,62 @@ int main(int argc, char *argv[])
|
||||
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++;
|
||||
}
|
||||
}
|
||||
|
||||
// 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]
|
||||
@ -423,14 +456,12 @@ void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInp
|
||||
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();
|
||||
|
||||
@ -450,7 +481,6 @@ void updateKalmanFilter( KalmanFilter &KF, Mat &measurement,
|
||||
|
||||
// Convert estimated quaternion to rotation matrix
|
||||
rotation_estimated = euler2rot(eulers_estimated);
|
||||
|
||||
}
|
||||
|
||||
/**********************************************************************************************************/
|
||||
|
||||
@ -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
|
||||
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,13 +46,25 @@ 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 )
|
||||
{
|
||||
bool is_registrable = registration.is_registrable();
|
||||
if (is_registrable)
|
||||
{
|
||||
int n_regist = registration.getNumRegist();
|
||||
int n_vertex = pts[n_regist];
|
||||
@ -72,9 +72,6 @@ static void onMouseModelRegistration( int event, int x, int y, int, void* )
|
||||
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;
|
||||
}
|
||||
@ -82,21 +79,56 @@ static void onMouseModelRegistration( int event, int x, int y, int, void* )
|
||||
}
|
||||
|
||||
/** Main program **/
|
||||
int main()
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
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"))
|
||||
{
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
std::cout << "Input image: " << img_path << std::endl;
|
||||
std::cout << "CAD model: " << ply_read_path << std::endl;
|
||||
std::cout << "Output training file: " << write_path << std::endl;
|
||||
std::cout << "Feature: " << featureName << std::endl;
|
||||
std::cout << "Number of keypoints for ORB: " << numKeyPoints << std::endl;
|
||||
|
||||
// load a mesh given the *.ply file path
|
||||
mesh.load(ply_read_path);
|
||||
|
||||
// set parameters
|
||||
int numKeyPoints = 10000;
|
||||
|
||||
//Instantiate robust matcher: detector, extractor, matcher
|
||||
RobustMatcher rmatcher;
|
||||
Ptr<FeatureDetector> detector = ORB::create(numKeyPoints);
|
||||
Ptr<Feature2D> detector, descriptor;
|
||||
createFeatures(featureName, numKeyPoints, detector, descriptor);
|
||||
rmatcher.setFeatureDetector(detector);
|
||||
rmatcher.setDescriptorExtractor(descriptor);
|
||||
|
||||
|
||||
/** GROUND TRUTH OF THE FIRST IMAGE **/
|
||||
|
||||
@ -104,13 +136,13 @@ int main()
|
||||
namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);
|
||||
|
||||
// Set up the mouse events
|
||||
setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 );
|
||||
setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0);
|
||||
|
||||
// Open the image to register
|
||||
Mat img_in = imread(img_path, IMREAD_COLOR);
|
||||
Mat img_vis = img_in.clone();
|
||||
Mat img_vis;
|
||||
|
||||
if (!img_in.data) {
|
||||
if (img_in.empty()) {
|
||||
cout << "Could not open or find the image" << endl;
|
||||
return -1;
|
||||
}
|
||||
@ -122,6 +154,12 @@ int main()
|
||||
cout << "Click the box corners ..." << endl;
|
||||
cout << "Waiting ..." << endl;
|
||||
|
||||
// Some basic colors
|
||||
const Scalar red(0, 0, 255);
|
||||
const Scalar green(0,255,0);
|
||||
const Scalar blue(255,0,0);
|
||||
const Scalar yellow(0,255,255);
|
||||
|
||||
// Loop until all the points are registered
|
||||
while ( waitKey(30) < 0 )
|
||||
{
|
||||
@ -176,7 +214,6 @@ int main()
|
||||
// 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;
|
||||
}
|
||||
@ -215,6 +252,7 @@ int main()
|
||||
}
|
||||
}
|
||||
|
||||
model.set_trainingImagePath(img_path);
|
||||
// save the model into a *.yaml file
|
||||
model.save(write_path);
|
||||
|
||||
@ -252,17 +290,4 @@ int main()
|
||||
destroyWindow("MODEL REGISTRATION");
|
||||
|
||||
cout << "GOODBYE" << endl;
|
||||
|
||||
}
|
||||
|
||||
/**********************************************************************************************************/
|
||||
void help()
|
||||
{
|
||||
cout
|
||||
<< "--------------------------------------------------------------------------" << endl
|
||||
<< "This program shows how to create your 3D textured model. " << endl
|
||||
<< "Usage:" << endl
|
||||
<< "./cpp-tutorial-pnp_registration" << endl
|
||||
<< "--------------------------------------------------------------------------" << endl
|
||||
<< endl;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user