edgarriba
11 years ago
parent
57d2cb8962
commit
ddf5c86f3a
20 changed files with 2767 additions and 0 deletions
@ -0,0 +1,80 @@ |
||||
#include "CsvReader.h" |
||||
#include "Utils.h" |
||||
|
||||
/** The default constructor of the CSV reader Class */ |
||||
CsvReader::CsvReader(const std::string &path, const char &separator){ |
||||
_file.open(path.c_str(), ifstream::in); |
||||
_separator = separator; |
||||
} |
||||
|
||||
/* Read a plane text file with .ply format */ |
||||
void CsvReader::readPLY(std::vector<cv::Point3f> &list_vertex, std::vector<std::vector<int> > &list_triangles) |
||||
{ |
||||
std::string line, tmp_str, n; |
||||
int num_vertex, num_triangles; |
||||
int count = 0; |
||||
bool end_header = false; |
||||
bool end_vertex = false; |
||||
|
||||
// Read the whole *.ply file
|
||||
while (getline(_file, line)) { |
||||
stringstream liness(line); |
||||
|
||||
// read header
|
||||
if(!end_header) |
||||
{ |
||||
getline(liness, tmp_str, _separator); |
||||
if( tmp_str == "element" ) |
||||
{ |
||||
getline(liness, tmp_str, _separator); |
||||
getline(liness, n); |
||||
if(tmp_str == "vertex") num_vertex = StringToInt(n); |
||||
if(tmp_str == "face") num_triangles = StringToInt(n); |
||||
} |
||||
if(tmp_str == "end_header") end_header = true; |
||||
} |
||||
|
||||
// read file content
|
||||
else if(end_header) |
||||
{ |
||||
// read vertex and add into 'list_vertex'
|
||||
if(!end_vertex && count < num_vertex) |
||||
{ |
||||
string x, y, z; |
||||
getline(liness, x, _separator); |
||||
getline(liness, y, _separator); |
||||
getline(liness, z); |
||||
|
||||
cv::Point3f tmp_p; |
||||
tmp_p.x = StringToInt(x); |
||||
tmp_p.y = StringToInt(y); |
||||
tmp_p.z = StringToInt(z); |
||||
list_vertex.push_back(tmp_p); |
||||
|
||||
count++; |
||||
if(count == num_vertex) |
||||
{ |
||||
count = 0; |
||||
end_vertex = !end_vertex; |
||||
} |
||||
} |
||||
// read faces and add into 'list_triangles'
|
||||
else if(end_vertex && count < num_triangles) |
||||
{ |
||||
std::string num_pts_per_face, id0, id1, id2; |
||||
getline(liness, num_pts_per_face, _separator); |
||||
getline(liness, id0, _separator); |
||||
getline(liness, id1, _separator); |
||||
getline(liness, id2); |
||||
|
||||
std::vector<int> tmp_triangle(3); |
||||
tmp_triangle[0] = StringToInt(id0); |
||||
tmp_triangle[1] = StringToInt(id1); |
||||
tmp_triangle[2] = StringToInt(id2); |
||||
list_triangles.push_back(tmp_triangle); |
||||
|
||||
count++; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,40 @@ |
||||
#ifndef CSVREADER_H |
||||
#define CSVREADER_H |
||||
|
||||
#include <iostream> |
||||
#include <fstream> |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
|
||||
using namespace std; |
||||
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 std::string &path, const 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(std::vector<cv::Point3f> &list_vertex, std::vector<std::vector<int> > &list_triangles); |
||||
|
||||
private: |
||||
/** The current stream file for the reader */ |
||||
ifstream _file; |
||||
/** The separator character between words for each line */ |
||||
char _separator; |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,50 @@ |
||||
#include "CsvWriter.h" |
||||
#include "Utils.h" |
||||
|
||||
CsvWriter::CsvWriter(const std::string &path, const std::string &separator){ |
||||
_file.open(path.c_str(), std::ofstream::out); |
||||
_isFirstTerm = true; |
||||
_separator = separator; |
||||
} |
||||
|
||||
CsvWriter::~CsvWriter() { |
||||
_file.flush(); |
||||
_file.close(); |
||||
} |
||||
|
||||
void CsvWriter::writeXYZ(const std::vector<cv::Point3f> &list_points3d) |
||||
{ |
||||
std::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; |
||||
} |
||||
|
||||
} |
||||
|
||||
void CsvWriter::writeUVXYZ(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, const cv::Mat &descriptors) |
||||
{ |
||||
std::string u, v, x, y, z, descriptor_str; |
||||
for(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) |
||||
{ |
||||
std::cout << descriptors.at<float>(i,j) << std::endl; |
||||
descriptor_str = FloatToString(descriptors.at<float>(i,j)); |
||||
_file << _separator << descriptor_str; |
||||
} |
||||
_file << std::endl; |
||||
} |
||||
} |
||||
@ -0,0 +1,22 @@ |
||||
#ifndef CSVWRITER_H |
||||
#define CSVWRITER_H |
||||
|
||||
#include <fstream> |
||||
#include <iostream> |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
|
||||
class CsvWriter { |
||||
public: |
||||
CsvWriter(const std::string &path, const std::string &separator = " "); |
||||
~CsvWriter(); |
||||
void writeXYZ(const std::vector<cv::Point3f> &list_points3d); |
||||
void writeUVXYZ(const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, const cv::Mat &descriptors); |
||||
|
||||
private: |
||||
std::ofstream _file; |
||||
std::string _separator; |
||||
bool _isFirstTerm; |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,82 @@ |
||||
/*
|
||||
* Mesh.cpp |
||||
* |
||||
* Created on: Apr 9, 2014 |
||||
* Author: edgar |
||||
*/ |
||||
|
||||
#include "Mesh.h" |
||||
#include "CsvReader.h" |
||||
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// TRIANGLE CLASS //
|
||||
// --------------------------------------------------- //
|
||||
|
||||
/** The custom constructor of the Triangle Class */ |
||||
Triangle::Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2) |
||||
{ |
||||
id_ = id; v0_ = V0; v1_ = V1; v2_ = V2; |
||||
} |
||||
|
||||
/** The default destructor of the Class */ |
||||
Triangle::~Triangle() |
||||
{ |
||||
// TODO Auto-generated destructor stub
|
||||
} |
||||
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// RAY CLASS //
|
||||
// --------------------------------------------------- //
|
||||
|
||||
/** The custom constructor of the Ray Class */ |
||||
Ray::Ray(cv::Point3f P0, cv::Point3f P1) { |
||||
p0_ = P0; p1_ = P1; |
||||
} |
||||
|
||||
/** The default destructor of the Class */ |
||||
Ray::~Ray() |
||||
{ |
||||
// TODO Auto-generated destructor stub
|
||||
} |
||||
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// OBJECT MESH CLASS //
|
||||
// --------------------------------------------------- //
|
||||
|
||||
/** The default constructor of the ObjectMesh Class */ |
||||
Mesh::Mesh() : 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
|
||||
} |
||||
|
||||
|
||||
/** Load a CSV with *.ply format **/ |
||||
void Mesh::load(const std::string 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_); |
||||
|
||||
// Update mesh attributes
|
||||
num_vertexs_ = list_vertex_.size(); |
||||
num_triangles_ = list_triangles_.size(); |
||||
|
||||
} |
||||
@ -0,0 +1,86 @@ |
||||
/*
|
||||
* Mesh.h |
||||
* |
||||
* Created on: Apr 9, 2014 |
||||
* Author: edgar |
||||
*/ |
||||
|
||||
#ifndef MESH_H_ |
||||
#define MESH_H_ |
||||
|
||||
#include <iostream> |
||||
#include <opencv2/core/core.hpp> |
||||
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// TRIANGLE CLASS //
|
||||
// --------------------------------------------------- //
|
||||
|
||||
class Triangle { |
||||
public: |
||||
|
||||
explicit Triangle(int id, cv::Point3f V0, cv::Point3f V1, cv::Point3f V2); |
||||
virtual ~Triangle(); |
||||
|
||||
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_; |
||||
}; |
||||
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// RAY CLASS //
|
||||
// --------------------------------------------------- //
|
||||
|
||||
class Ray { |
||||
public: |
||||
|
||||
explicit Ray(cv::Point3f P0, cv::Point3f P1); |
||||
virtual ~Ray(); |
||||
|
||||
cv::Point3f getP0() { return p0_; } |
||||
cv::Point3f getP1() { return p1_; } |
||||
|
||||
private: |
||||
/** The two points that defines the ray */ |
||||
cv::Point3f p0_, p1_; |
||||
}; |
||||
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// OBJECT MESH CLASS //
|
||||
// --------------------------------------------------- //
|
||||
|
||||
class Mesh |
||||
{ |
||||
public: |
||||
|
||||
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_; } |
||||
|
||||
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_; |
||||
}; |
||||
|
||||
#endif /* OBJECTMESH_H_ */ |
||||
@ -0,0 +1,73 @@ |
||||
/*
|
||||
* Model.cpp |
||||
* |
||||
* Created on: Apr 9, 2014 |
||||
* Author: edgar |
||||
*/ |
||||
|
||||
#include "Model.h" |
||||
#include "CsvWriter.h" |
||||
|
||||
Model::Model() : list_points2d_in_(0), list_points2d_out_(0), list_points3d_in_(0) |
||||
{ |
||||
n_correspondences_ = 0; |
||||
} |
||||
|
||||
Model::~Model() |
||||
{ |
||||
// 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_++; |
||||
} |
||||
|
||||
void Model::add_outlier(const cv::Point2f &point2d) |
||||
{ |
||||
list_points2d_out_.push_back(point2d); |
||||
} |
||||
|
||||
void Model::add_descriptor(const cv::Mat &descriptor) |
||||
{ |
||||
descriptors_.push_back(descriptor); |
||||
} |
||||
|
||||
void Model::add_keypoint(const cv::KeyPoint &kp) |
||||
{ |
||||
list_keypoints_.push_back(kp); |
||||
} |
||||
|
||||
|
||||
/** Save a CSV 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.release(); |
||||
} |
||||
|
||||
/** Load a YAML file using OpenCv functions **/ |
||||
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(); |
||||
|
||||
} |
||||
@ -0,0 +1,54 @@ |
||||
/*
|
||||
* Model.h |
||||
* |
||||
* Created on: Apr 9, 2014 |
||||
* Author: edgar |
||||
*/ |
||||
|
||||
#ifndef MODEL_H_ |
||||
#define MODEL_H_ |
||||
|
||||
#include <iostream> |
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/features2d/features2d.hpp> |
||||
|
||||
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); |
||||
|
||||
|
||||
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_; |
||||
}; |
||||
|
||||
#endif /* OBJECTMODEL_H_ */ |
||||
@ -0,0 +1,35 @@ |
||||
/*
|
||||
* ModelRegistration.cpp |
||||
* |
||||
* Created on: Apr 18, 2014 |
||||
* Author: edgar |
||||
*/ |
||||
|
||||
#include "ModelRegistration.h" |
||||
|
||||
ModelRegistration::ModelRegistration() |
||||
{ |
||||
n_registrations_ = 0; |
||||
max_registrations_ = 0; |
||||
} |
||||
|
||||
ModelRegistration::~ModelRegistration() |
||||
{ |
||||
// TODO Auto-generated destructor stub
|
||||
} |
||||
|
||||
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() |
||||
{ |
||||
n_registrations_ = 0; |
||||
max_registrations_ = 0; |
||||
list_points2d_.clear(); |
||||
list_points3d_.clear(); |
||||
} |
||||
@ -0,0 +1,43 @@ |
||||
/*
|
||||
* ModelRegistration.h |
||||
* |
||||
* Created on: Apr 18, 2014 |
||||
* Author: edgar |
||||
*/ |
||||
|
||||
#ifndef MODELREGISTRATION_H_ |
||||
#define MODELREGISTRATION_H_ |
||||
|
||||
#include <iostream> |
||||
#include <opencv2/core/core.hpp> |
||||
|
||||
class ModelRegistration |
||||
{ |
||||
public: |
||||
|
||||
ModelRegistration(); |
||||
virtual ~ModelRegistration(); |
||||
|
||||
void setNumMax(int n) { max_registrations_ = n; } |
||||
|
||||
std::vector<cv::Point2f> get_points2d() const { return list_points2d_; } |
||||
std::vector<cv::Point3f> get_points3d() const { return list_points3d_; } |
||||
int getNumMax() const { return max_registrations_; } |
||||
int getNumRegist() const { return n_registrations_; } |
||||
|
||||
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_; |
||||
}; |
||||
|
||||
#endif /* MODELREGISTRATION_H_ */ |
||||
@ -0,0 +1,312 @@ |
||||
/*
|
||||
* PnPProblem.cpp |
||||
* |
||||
* Created on: Mar 28, 2014 |
||||
* Author: Edgar Riba |
||||
*/ |
||||
|
||||
#include <iostream> |
||||
#include <sstream> |
||||
|
||||
#include "PnPProblem.h" |
||||
#include "Mesh.h" |
||||
|
||||
#include <opencv2/calib3d/calib3d.hpp> |
||||
|
||||
|
||||
/* Functions for Möller–Trumbore intersection algorithm
|
||||
* */ |
||||
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; |
||||
} |
||||
|
||||
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) |
||||
{ |
||||
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
|
||||
* */ |
||||
|
||||
// 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) |
||||
{ |
||||
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
|
||||
|
||||
} |
||||
|
||||
PnPProblem::~PnPProblem() |
||||
{ |
||||
// 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>(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); |
||||
|
||||
bool useExtrinsicGuess = false; |
||||
|
||||
// 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; |
||||
|
||||
// Set projection matrix
|
||||
this->set_P_matrix(_R_matrix, _t_matrix); |
||||
|
||||
return correspondence; |
||||
} |
||||
|
||||
// Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use
|
||||
|
||||
void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, // list with model 3D coordinates
|
||||
const std::vector<cv::Point2f> &list_points2d, // list with scene 2D coordinates
|
||||
int flags, cv::Mat &inliers, int iterationsCount, // PnP method; inliers container
|
||||
float reprojectionError, float 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
|
||||
|
||||
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 ); |
||||
|
||||
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
|
||||
|
||||
} |
||||
|
||||
// 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; |
||||
} |
||||
|
||||
|
||||
// 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 = point2d_vec.at<double>(0) / point2d_vec.at<double>(2); |
||||
point2d.y = 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(); |
||||
|
||||
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 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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; |
||||
|
||||
// Loop for all the triangles and check the intersection
|
||||
for (unsigned int i = 0; i < triangles_list.size(); i++) |
||||
{ |
||||
cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]); |
||||
cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]); |
||||
cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]); |
||||
|
||||
Triangle T(i, V0, V1, V2); |
||||
|
||||
double out; |
||||
if(this->intersect_MollerTrumbore(R, T, &out)) |
||||
{ |
||||
cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
|
||||
intersections_list.push_back(tmp_pt); |
||||
} |
||||
} |
||||
|
||||
// If there are intersection, find the nearest one
|
||||
if (!intersections_list.empty()) |
||||
{ |
||||
point3d = get_nearest_3D_point(intersections_list, R.getP0()); |
||||
return true; |
||||
} |
||||
else |
||||
{ |
||||
return false; |
||||
} |
||||
} |
||||
|
||||
// Möller–Trumbore intersection algorithm
|
||||
bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out) |
||||
{ |
||||
const double EPSILON = 0.000001; |
||||
|
||||
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 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); |
||||
|
||||
// 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); |
||||
|
||||
//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 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; |
||||
|
||||
//Prepare to test v parameter
|
||||
Q = CROSS(T, e1); |
||||
|
||||
//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; |
||||
|
||||
t = DOT(e2, Q) * inv_det; |
||||
|
||||
if(t > EPSILON) { //ray intersection
|
||||
*out = t; |
||||
return true; |
||||
} |
||||
|
||||
// No hit, no win
|
||||
return false; |
||||
} |
||||
@ -0,0 +1,53 @@ |
||||
/*
|
||||
* PnPProblem.h |
||||
* |
||||
* Created on: Mar 28, 2014 |
||||
* Author: Edgar Riba |
||||
*/ |
||||
|
||||
#ifndef PNPPROBLEM_H_ |
||||
#define PNPPROBLEM_H_ |
||||
|
||||
#include <iostream> |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/highgui/highgui.hpp> |
||||
|
||||
#include "Mesh.h" |
||||
#include "ModelRegistration.h" |
||||
|
||||
class PnPProblem |
||||
{ |
||||
|
||||
public: |
||||
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, float 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; } |
||||
|
||||
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; |
||||
}; |
||||
|
||||
#endif /* PNPPROBLEM_H_ */ |
||||
@ -0,0 +1,152 @@ |
||||
/*
|
||||
* RobustMatcher.cpp |
||||
* |
||||
* Created on: Jun 4, 2014 |
||||
* Author: eriba |
||||
*/ |
||||
|
||||
#include "RobustMatcher.h" |
||||
#include <time.h> |
||||
|
||||
#include <opencv2/features2d/features2d.hpp> |
||||
|
||||
RobustMatcher::~RobustMatcher() |
||||
{ |
||||
// TODO Auto-generated destructor stub
|
||||
} |
||||
|
||||
void RobustMatcher::computeKeyPoints( const cv::Mat& image, std::vector<cv::KeyPoint>& 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); |
||||
} |
||||
|
||||
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) |
||||
{ |
||||
// 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; |
||||
} |
||||
|
||||
void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1, |
||||
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) |
||||
{ |
||||
// 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 ) |
||||
{ |
||||
|
||||
// 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); |
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
int removed1 = ratioTest(matches12); |
||||
// clean image 2 -> image 1 matches
|
||||
int removed2 = ratioTest(matches21); |
||||
|
||||
// 4. Remove non-symmetrical matches
|
||||
symmetryTest(matches12, matches21, good_matches); |
||||
|
||||
} |
||||
|
||||
void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, |
||||
std::vector<cv::KeyPoint>& keypoints_frame, |
||||
const cv::Mat& descriptors_model ) |
||||
{ |
||||
good_matches.clear(); |
||||
|
||||
// 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); |
||||
|
||||
// 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
|
||||
int removed = 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]); |
||||
} |
||||
|
||||
} |
||||
@ -0,0 +1,83 @@ |
||||
/*
|
||||
* RobustMatcher.h |
||||
* |
||||
* Created on: Jun 4, 2014 |
||||
* Author: eriba |
||||
*/ |
||||
|
||||
#ifndef ROBUSTMATCHER_H_ |
||||
#define ROBUSTMATCHER_H_ |
||||
|
||||
#include <iostream> |
||||
#include <boost/shared_ptr.hpp> |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/highgui/highgui.hpp> |
||||
#include <opencv2/features2d/features2d.hpp> |
||||
#include <opencv2/nonfree/nonfree.hpp> |
||||
|
||||
class RobustMatcher { |
||||
public: |
||||
RobustMatcher() : ratio_(0.8f) |
||||
{ |
||||
// ORB is the default feature
|
||||
detector_ = new cv::OrbFeatureDetector(); |
||||
extractor_ = new cv::OrbDescriptorExtractor(); |
||||
|
||||
// BruteFroce matcher with Norm Hamming is the default matcher
|
||||
matcher_ = new cv::BFMatcher(cv::NORM_HAMMING, false); |
||||
|
||||
} |
||||
virtual ~RobustMatcher(); |
||||
|
||||
// Set the feature detector
|
||||
void setFeatureDetector(cv::FeatureDetector * detect) { detector_ = detect; } |
||||
|
||||
// Set the descriptor extractor
|
||||
void setDescriptorExtractor(cv::DescriptorExtractor * desc) { extractor_ = desc; } |
||||
|
||||
// Set the matcher
|
||||
void setDescriptorMatcher(cv::DescriptorMatcher * match) { matcher_ = match; } |
||||
|
||||
// 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); |
||||
|
||||
// Set ratio parameter for the ratio test
|
||||
void setRatio( float rat) { ratio_ = rat; } |
||||
|
||||
// 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 ); |
||||
|
||||
// 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 ); |
||||
|
||||
private: |
||||
// pointer to the feature point detector object
|
||||
cv::FeatureDetector * detector_; |
||||
// pointer to the feature descriptor extractor object
|
||||
cv::DescriptorExtractor * extractor_; |
||||
// pointer to the matcher object
|
||||
cv::DescriptorMatcher * matcher_; |
||||
// max ratio between 1st and 2nd NN
|
||||
float ratio_; |
||||
}; |
||||
|
||||
#endif /* ROBUSTMATCHER_H_ */ |
||||
@ -0,0 +1,295 @@ |
||||
/*
|
||||
* Utils.cpp |
||||
* |
||||
* Created on: Mar 28, 2014 |
||||
* Author: Edgar Riba |
||||
*/ |
||||
|
||||
#include <iostream> |
||||
|
||||
#include "PnPProblem.h" |
||||
#include "ModelRegistration.h" |
||||
#include "Utils.h" |
||||
|
||||
#include <opencv2/imgproc/imgproc.hpp> |
||||
#include <opencv2/calib3d/calib3d.hpp> |
||||
#include <opencv2/nonfree/features2d.hpp> |
||||
|
||||
|
||||
// For text
|
||||
int fontFace = cv::FONT_ITALIC; |
||||
double fontScale = 0.75; |
||||
double thickness_font = 2; |
||||
|
||||
// For circles
|
||||
int lineType = 8; |
||||
int radius = 4; |
||||
double thickness_circ = -1; |
||||
|
||||
// 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 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); |
||||
} |
||||
|
||||
// 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); |
||||
} |
||||
|
||||
// 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); |
||||
} |
||||
|
||||
// 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); |
||||
} |
||||
|
||||
// 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); |
||||
} |
||||
|
||||
// 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); |
||||
} |
||||
} |
||||
|
||||
// Draw only the 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]; |
||||
|
||||
// Draw Selected points
|
||||
cv::circle(image, point_2d, radius, color, -1, lineType ); |
||||
} |
||||
} |
||||
|
||||
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 = 3.141592653; |
||||
//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); |
||||
} |
||||
|
||||
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); |
||||
|
||||
const double PI = 3.141592653; |
||||
int length = 50; |
||||
|
||||
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 ); |
||||
|
||||
} |
||||
|
||||
// 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); |
||||
} |
||||
} |
||||
|
||||
bool equal_point(const cv::Point2f &p1, const cv::Point2f &p2) |
||||
{ |
||||
return ( (p1.x == p2.x) && (p1.y == p2.y) ); |
||||
} |
||||
|
||||
double get_translation_error(const cv::Mat &t_true, const cv::Mat &t) |
||||
{ |
||||
return cv::norm( t_true - t ); |
||||
} |
||||
|
||||
double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R) |
||||
{ |
||||
cv::Mat error_vec, error_mat; |
||||
error_mat = R_true * cv::Mat(R.inv()).mul(-1); |
||||
cv::Rodrigues(error_mat, error_vec); |
||||
|
||||
return cv::norm(error_vec); |
||||
} |
||||
|
||||
cv::Mat rot2euler(const cv::Mat & rotationMatrix) |
||||
{ |
||||
cv::Mat euler(3,1,CV_64F); |
||||
|
||||
double m00 = rotationMatrix.at<double>(0,0); |
||||
double m01 = rotationMatrix.at<double>(0,1); |
||||
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 m21 = rotationMatrix.at<double>(2,1); |
||||
double m22 = rotationMatrix.at<double>(2,2); |
||||
|
||||
double x, y, z; |
||||
|
||||
// Assuming the angles are in radians.
|
||||
if (m10 > 0.998) { // singularity at north pole
|
||||
x = 0; |
||||
y = CV_PI/2; |
||||
z = atan2(m02,m22); |
||||
} |
||||
else if (m10 < -0.998) { // singularity at south pole
|
||||
x = 0; |
||||
y = -CV_PI/2; |
||||
z = atan2(m02,m22); |
||||
} |
||||
else |
||||
{ |
||||
x = atan2(-m12,m11); |
||||
y = asin(m10); |
||||
z = atan2(-m20,m00); |
||||
} |
||||
|
||||
euler.at<double>(0) = x; |
||||
euler.at<double>(1) = y; |
||||
euler.at<double>(2) = z; |
||||
|
||||
return euler; |
||||
} |
||||
|
||||
cv::Mat euler2rot(const cv::Mat & euler) |
||||
{ |
||||
cv::Mat rotationMatrix(3,3,CV_64F); |
||||
|
||||
double x = euler.at<double>(0); |
||||
double y = euler.at<double>(1); |
||||
double z = euler.at<double>(2); |
||||
|
||||
// Assuming the angles are in radians.
|
||||
double ch = cos(z); |
||||
double sh = sin(z); |
||||
double ca = cos(y); |
||||
double sa = sin(y); |
||||
double cb = cos(x); |
||||
double sb = sin(x); |
||||
|
||||
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; |
||||
} |
||||
|
||||
int StringToInt ( const std::string &Text ) |
||||
{ |
||||
std::istringstream ss(Text); |
||||
int result; |
||||
return ss >> result ? result : 0; |
||||
} |
||||
|
||||
std::string FloatToString ( float Number ) |
||||
{ |
||||
std::ostringstream ss; |
||||
ss << Number; |
||||
return ss.str(); |
||||
} |
||||
|
||||
std::string IntToString ( int Number ) |
||||
{ |
||||
std::ostringstream ss; |
||||
ss << Number; |
||||
return ss.str(); |
||||
} |
||||
@ -0,0 +1,75 @@ |
||||
/*
|
||||
* Utils.h |
||||
* |
||||
* Created on: Mar 28, 2014 |
||||
* Author: Edgar Riba |
||||
*/ |
||||
|
||||
#ifndef UTILS_H_ |
||||
#define UTILS_H_ |
||||
|
||||
#include <iostream> |
||||
#include <cv.h> |
||||
|
||||
#include "PnPProblem.h" |
||||
|
||||
// Draw a text with the question point
|
||||
void drawQuestion(cv::Mat image, cv::Point3f point, cv::Scalar color); |
||||
|
||||
// Draw a text in the left of the image
|
||||
void drawText(cv::Mat image, std::string text, cv::Scalar color); |
||||
void drawText2(cv::Mat image, std::string text, cv::Scalar color); |
||||
void drawFPS(cv::Mat image, double fps, cv::Scalar color); |
||||
void drawConfidence(cv::Mat image, double confidence, cv::Scalar color); |
||||
|
||||
// Draw a text with the number of registered points
|
||||
void drawCounter(cv::Mat image, int n, int n_max, cv::Scalar color); |
||||
|
||||
// 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); |
||||
|
||||
// Draw only the points
|
||||
void draw2DPoints(cv::Mat image, std::vector<cv::Point2f> &list_points, cv::Scalar color); |
||||
|
||||
// Draw the object mesh
|
||||
void drawObjectMesh(cv::Mat image, const Mesh *mesh, PnPProblem *pnpProblem, cv::Scalar color); |
||||
|
||||
// Draw an arrow into the image
|
||||
void drawArrow(cv::Mat image, cv::Point2i p, cv::Point2i q, cv::Scalar color, int arrowMagnitude = 9, int thickness=1, int line_type=8, int shift=0); |
||||
|
||||
// Draw the 3D coordinate axes
|
||||
void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_points2d); |
||||
|
||||
// Compute the 2D points with the esticv::Mated pose
|
||||
std::vector<cv::Point2f> verification_points(PnPProblem *p); |
||||
|
||||
bool equal_point(const cv::Point2f &p1, const cv::Point2f &p2); |
||||
|
||||
double get_translation_error(const cv::Mat &t_true, const cv::Mat &t); |
||||
double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R); |
||||
double get_variance(const std::vector<cv::Point3f> list_points3d); |
||||
cv::Mat rot2quat(cv::Mat &rotationMatrix); |
||||
cv::Mat quat2rot(cv::Mat &q); |
||||
cv::Mat euler2rot(const cv::Mat & euler); |
||||
cv::Mat rot2euler(const cv::Mat & rotationMatrix); |
||||
cv::Mat quat2euler(const cv::Mat & q); |
||||
int StringToInt ( const std::string &Text ); |
||||
std::string FloatToString ( float Number ); |
||||
std::string IntToString ( int Number ); |
||||
|
||||
class Timer |
||||
{ |
||||
public: |
||||
Timer(); |
||||
void start() { tstart = (double)clock()/CLOCKS_PER_SEC; } |
||||
void stop() { tstop = (double)clock()/CLOCKS_PER_SEC; } |
||||
void reset() { tstart = 0; tstop = 0;} |
||||
|
||||
double getTimeMilli() const { return (tstop-tstart)*1000; } |
||||
double getTimeSec() const { return tstop-tstart; } |
||||
|
||||
private: |
||||
double tstart, tstop, ttime; |
||||
}; |
||||
|
||||
#endif /* UTILS_H_ */ |
||||
@ -0,0 +1,472 @@ |
||||
#include <iostream> |
||||
#include <time.h> |
||||
|
||||
#include "cv.h" |
||||
#include "highgui.h" |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/highgui/highgui.hpp> |
||||
#include <opencv2/imgproc/imgproc.hpp> |
||||
#include <opencv2/calib3d/calib3d.hpp> |
||||
#include <opencv2/video/tracking.hpp> |
||||
|
||||
#include "Mesh.h" |
||||
#include "Model.h" |
||||
#include "PnPProblem.h" |
||||
#include "RobustMatcher.h" |
||||
#include "ModelRegistration.h" |
||||
#include "Utils.h" |
||||
|
||||
// COOKIES BOX - ORB
|
||||
std::string yml_read_path = "../Data/cookies_ORB.yml"; // 3dpts + descriptors
|
||||
|
||||
// COOKIES BOX MESH
|
||||
std::string ply_read_path = "../Data/box.ply"; // mesh
|
||||
|
||||
void help() |
||||
{ |
||||
std::cout |
||||
<< "--------------------------------------------------------------------------" << std::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." << std::endl |
||||
<< "Usage:" << std::endl |
||||
<< "./pnp_detection ~/path_to_video/box.mp4" << std::endl |
||||
<< "./pnp_detection " << std::endl |
||||
<< "--------------------------------------------------------------------------" << std::endl |
||||
<< std::endl; |
||||
} |
||||
|
||||
|
||||
/*
|
||||
* Set up the 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
|
||||
|
||||
|
||||
/*
|
||||
* Set up some basic colors |
||||
*/ |
||||
cv::Scalar red(0, 0, 255); |
||||
cv::Scalar green(0,255,0); |
||||
cv::Scalar blue(255,0,0); |
||||
cv::Scalar yellow(0,255,255); |
||||
|
||||
|
||||
// Robust Matcher parameters
|
||||
|
||||
int numKeyPoints = 2000; // number of detected keypoints
|
||||
float ratio = 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.
|
||||
float confidence = 0.95; // ransac successful confidence.
|
||||
|
||||
|
||||
// Kalman Filter parameters
|
||||
|
||||
int minInliersKalman = 30; // Kalman threshold updating
|
||||
|
||||
|
||||
/**********************************************************************************************************/ |
||||
|
||||
void initKalmanFilter( cv::KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt); |
||||
|
||||
|
||||
/**********************************************************************************************************/ |
||||
|
||||
|
||||
void updateKalmanFilter( cv::KalmanFilter &KF, cv::Mat &measurements, |
||||
cv::Mat &translation_estimated, cv::Mat &rotation_estimated ); |
||||
|
||||
/**********************************************************************************************************/ |
||||
|
||||
|
||||
void fillMeasurements( cv::Mat &measurements, |
||||
const cv::Mat &translation_measured, const cv::Mat &rotation_measured); |
||||
|
||||
|
||||
/**********************************************************************************************************/ |
||||
|
||||
|
||||
int main(int argc, char *argv[]) |
||||
{ |
||||
|
||||
help(); |
||||
|
||||
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
|
||||
|
||||
cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints); // instatiate ORB feature detector
|
||||
cv::DescriptorExtractor * extractor = new cv::OrbDescriptorExtractor(); // instatiate ORB descriptor extractor
|
||||
|
||||
rmatcher.setFeatureDetector(detector); // set feature detector
|
||||
rmatcher.setDescriptorExtractor(extractor); // set descriptor extractor
|
||||
|
||||
|
||||
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
|
||||
|
||||
cv::DescriptorMatcher * matcher = new cv::FlannBasedMatcher(indexParams, searchParams); // instantiate FlannBased matcher
|
||||
rmatcher.setDescriptorMatcher(matcher); // set matcher
|
||||
|
||||
|
||||
rmatcher.setRatio(ratio); // set ratio test parameter
|
||||
|
||||
|
||||
cv::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 action control
|
||||
|
||||
double dt = 0.125; // time between measurements (1/FPS)
|
||||
|
||||
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function
|
||||
|
||||
cv::Mat measurements(nMeasurements, 1, CV_64F); measurements.setTo(cv::Scalar(0)); |
||||
bool good_measurement = false; |
||||
|
||||
|
||||
// Get the MODEL INFO
|
||||
|
||||
std::vector<cv::Point3f> list_points3d_model = model.get_points3d(); // list with model 3D coordinates
|
||||
cv::Mat descriptors_model = model.get_descriptors(); // list with descriptors of each 3D coordinate
|
||||
|
||||
|
||||
// Create & Open Window
|
||||
cv::namedWindow("REAL TIME DEMO", CV_WINDOW_KEEPRATIO); |
||||
|
||||
|
||||
cv::VideoCapture cap; // instantiate VideoCapture
|
||||
(argc < 2) ? cap.open(1) : cap.open(argv[1]); // open the default camera device
|
||||
// or a recorder video
|
||||
|
||||
if(!cap.isOpened()) // check if we succeeded
|
||||
{ |
||||
std::cout << "Could not open the camera device" << std::endl; |
||||
return -1; |
||||
} |
||||
|
||||
|
||||
// start and end times
|
||||
time_t start, end; |
||||
|
||||
// fps calculated using number of frames / seconds
|
||||
double fps; |
||||
|
||||
// frame counter
|
||||
int counter = 0; |
||||
|
||||
// floating point seconds elapsed since start
|
||||
double sec; |
||||
|
||||
// start the clock
|
||||
time(&start); |
||||
|
||||
double tstart2, tstop2, ttime2; // algorithm metrics
|
||||
double tstart, tstop, ttime; // algorithm metrics
|
||||
|
||||
cv::Mat frame, frame_vis; |
||||
|
||||
while(cap.read(frame) && cv::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
|
||||
|
||||
std::vector<cv::DMatch> good_matches; // to obtain the 3D points of the model
|
||||
std::vector<cv::KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
|
||||
|
||||
|
||||
if(fast_match) |
||||
{ |
||||
rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model); |
||||
} |
||||
else |
||||
{ |
||||
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model); |
||||
} |
||||
|
||||
|
||||
// -- Step 2: Find out the 2D/3D correspondences
|
||||
|
||||
std::vector<cv::Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
|
||||
std::vector<cv::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) |
||||
{ |
||||
cv::Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 3D point from model
|
||||
cv::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); |
||||
|
||||
|
||||
cv::Mat inliers_idx; |
||||
std::vector<cv::Point2f> list_points2d_inliers; |
||||
|
||||
if(good_matches.size() > 0) // None matches, then RANSAC crashes
|
||||
{ |
||||
|
||||
|
||||
// -- Step 3: Estimate the pose using RANSAC approach
|
||||
pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match, |
||||
cv::ITERATIVE, 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
|
||||
cv::Point2f point2d = list_points2d_scene_match[n]; // i-inlier point 2D
|
||||
list_points2d_inliers.push_back(point2d); // add i-inlier to list
|
||||
} |
||||
|
||||
// Draw inliers points 2D
|
||||
draw2DPoints(frame_vis, list_points2d_inliers, blue); |
||||
|
||||
|
||||
// -- Step 5: Kalman Filter
|
||||
|
||||
good_measurement = false; |
||||
|
||||
// GOOD MEASUREMENT
|
||||
if( inliers_idx.rows >= minInliersKalman ) |
||||
{ |
||||
|
||||
// Get the measured translation
|
||||
cv::Mat translation_measured(3, 1, CV_64F); |
||||
translation_measured = pnp_detection.get_t_matrix(); |
||||
|
||||
// Get the measured rotation
|
||||
cv::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
|
||||
cv::Mat translation_estimated(3, 1, CV_64F); |
||||
cv::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) |
||||
{ |
||||
drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose
|
||||
} |
||||
else |
||||
{ |
||||
drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
|
||||
} |
||||
|
||||
double l = 5; |
||||
std::vector<cv::Point2f> pose_points2d; |
||||
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,0,0))); // axis center
|
||||
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(l,0,0))); // axis x
|
||||
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,l,0))); // axis y
|
||||
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::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 ratio = ((double)inliers_idx.rows/(double)good_matches.size())*100; |
||||
drawConfidence(frame_vis, ratio, yellow); |
||||
|
||||
|
||||
// -- Step X: Draw some debugging text
|
||||
|
||||
// Draw some debug text
|
||||
int inliers_int = inliers_idx.rows; |
||||
int outliers_int = good_matches.size() - inliers_int; |
||||
std::string inliers_str = IntToString(inliers_int); |
||||
std::string outliers_str = IntToString(outliers_int); |
||||
std::string n = IntToString(good_matches.size()); |
||||
std::string text = "Found " + inliers_str + " of " + n + " matches"; |
||||
std::string text2 = "Inliers: " + inliers_str + " - Outliers: " + outliers_str; |
||||
|
||||
drawText(frame_vis, text, green); |
||||
drawText2(frame_vis, text2, red); |
||||
|
||||
cv::imshow("REAL TIME DEMO", frame_vis); |
||||
|
||||
//cv::waitKey(0);
|
||||
|
||||
} |
||||
|
||||
// Close and Destroy Window
|
||||
cv::destroyWindow("REAL TIME DEMO"); |
||||
|
||||
std::cout << "GOODBYE ..." << std::endl; |
||||
|
||||
} |
||||
|
||||
/**********************************************************************************************************/ |
||||
void initKalmanFilter(cv::KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt) |
||||
{ |
||||
|
||||
KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter
|
||||
|
||||
cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5)); // set process noise
|
||||
cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-2)); // set measurement noise
|
||||
cv::setIdentity(KF.errorCovPost, cv::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
|
||||
|
||||
//std::cout << "A " << std::endl << KF.transitionMatrix << std::endl;
|
||||
//std::cout << "C " << std::endl << KF.measurementMatrix << std::endl;
|
||||
|
||||
} |
||||
|
||||
/**********************************************************************************************************/ |
||||
void updateKalmanFilter( cv::KalmanFilter &KF, cv::Mat &measurement, |
||||
cv::Mat &translation_estimated, cv::Mat &rotation_estimated ) |
||||
{ |
||||
|
||||
// First predict, to update the internal statePre variable
|
||||
cv::Mat prediction = KF.predict(); |
||||
|
||||
// The "correct" phase that is going to use the predicted value and our measurement
|
||||
cv::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); |
||||
|
||||
// Estimated euler angles
|
||||
cv::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( cv::Mat &measurements, |
||||
const cv::Mat &translation_measured, const cv::Mat &rotation_measured) |
||||
{ |
||||
// Convert rotation matrix to euler angles
|
||||
cv::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
|
||||
} |
||||
@ -0,0 +1,290 @@ |
||||
#include <iostream> |
||||
|
||||
#include "Mesh.h" |
||||
#include "Model.h" |
||||
#include "PnPProblem.h" |
||||
#include "RobustMatcher.h" |
||||
#include "ModelRegistration.h" |
||||
#include "Utils.h" |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/imgproc/imgproc.hpp> |
||||
#include <opencv2/calib3d/calib3d.hpp> |
||||
#include <opencv2/features2d/features2d.hpp> |
||||
#include <opencv2/nonfree/features2d.hpp> |
||||
|
||||
/*
|
||||
* Set up the images paths |
||||
*/ |
||||
|
||||
// COOKIES BOX [718x480]
|
||||
std::string img_path = "../Data/resized_IMG_3875.JPG"; // f 55
|
||||
|
||||
// COOKIES BOX MESH
|
||||
std::string ply_read_path = "../Data/box.ply"; |
||||
|
||||
// YAML writting path
|
||||
std::string write_path = "../Data/cookies_ORB.yml"; |
||||
|
||||
void help() |
||||
{ |
||||
std::cout |
||||
<< "--------------------------------------------------------------------------" << std::endl |
||||
<< "This program shows how to create your 3D textured model. " << std::endl |
||||
<< "Usage:" << std::endl |
||||
<< "./pnp_registration " << std::endl |
||||
<< "--------------------------------------------------------------------------" << std::endl |
||||
<< std::endl; |
||||
} |
||||
|
||||
// Boolean the know if the registration it's done
|
||||
bool end_registration = false; |
||||
|
||||
/*
|
||||
* Set up the intrinsic camera parameters: CANON |
||||
*/ |
||||
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
|
||||
|
||||
|
||||
// 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
|
||||
|
||||
/*
|
||||
* Set up some basic colors |
||||
*/ |
||||
cv::Scalar red(0, 0, 255); |
||||
cv::Scalar green(0,255,0); |
||||
cv::Scalar blue(255,0,0); |
||||
cv::Scalar yellow(0,255,255); |
||||
|
||||
/*
|
||||
* CREATE MODEL REGISTRATION OBJECT |
||||
* CREATE OBJECT MESH |
||||
* CREATE OBJECT MODEL |
||||
* CREATE PNP OBJECT |
||||
*/ |
||||
ModelRegistration registration; |
||||
Model model; |
||||
Mesh mesh; |
||||
PnPProblem pnp_registration(params_CANON); |
||||
|
||||
|
||||
// Mouse events for model registration
|
||||
static void onMouseModelRegistration( int event, int x, int y, int, void* ) |
||||
{ |
||||
if ( event == cv::EVENT_LBUTTONUP ) |
||||
{ |
||||
int n_regist = registration.getNumRegist(); |
||||
int n_vertex = pts[n_regist]; |
||||
|
||||
cv::Point2f point_2d = cv::Point2f(x,y); |
||||
cv::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; |
||||
} |
||||
} |
||||
} |
||||
|
||||
/*
|
||||
* MAIN PROGRAM |
||||
* |
||||
*/ |
||||
|
||||
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; |
||||
cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints); |
||||
rmatcher.setFeatureDetector(detector); |
||||
|
||||
/*
|
||||
* GROUND TRUTH OF THE FIRST IMAGE |
||||
* |
||||
* by the moment it is the reference image |
||||
*/ |
||||
|
||||
// Create & Open Window
|
||||
cv::namedWindow("MODEL REGISTRATION", cv::WINDOW_KEEPRATIO); |
||||
|
||||
// Set up the mouse events
|
||||
cv::setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 ); |
||||
|
||||
// Open the image to register
|
||||
cv::Mat img_in = cv::imread(img_path, cv::IMREAD_COLOR); |
||||
cv::Mat img_vis = img_in.clone(); |
||||
|
||||
if (!img_in.data) { |
||||
std::cout << "Could not open or find the image" << std::endl; |
||||
return -1; |
||||
} |
||||
|
||||
// Set the number of points to register
|
||||
int num_registrations = n; |
||||
registration.setNumMax(num_registrations); |
||||
|
||||
std::cout << "Click the box corners ..." << std::endl; |
||||
std::cout << "Waiting ..." << std::endl; |
||||
|
||||
// Loop until all the points are registered
|
||||
while ( cv::waitKey(30) < 0 ) |
||||
{ |
||||
// Refresh debug image
|
||||
img_vis = img_in.clone(); |
||||
|
||||
// Current registered points
|
||||
std::vector<cv::Point2f> list_points2d = registration.get_points2d(); |
||||
std::vector<cv::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]; |
||||
cv::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
|
||||
cv::imshow("MODEL REGISTRATION", img_vis); |
||||
} |
||||
|
||||
|
||||
/*
|
||||
* |
||||
* COMPUTE CAMERA POSE |
||||
* |
||||
*/ |
||||
|
||||
std::cout << "COMPUTING POSE ..." << std::endl; |
||||
|
||||
// The list of registered points
|
||||
std::vector<cv::Point2f> list_points2d = registration.get_points2d(); |
||||
std::vector<cv::Point3f> list_points3d = registration.get_points3d(); |
||||
|
||||
// Estimate pose given the registered points
|
||||
bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, cv::ITERATIVE); |
||||
if ( is_correspondence ) |
||||
{ |
||||
std::cout << "Correspondence found" << std::endl; |
||||
|
||||
// Compute all the 2D points of the mesh to verify the algorithm and draw it
|
||||
std::vector<cv::Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh); |
||||
draw2DPoints(img_vis, list_points2d_mesh, green); |
||||
|
||||
} else { |
||||
std::cout << "Correspondence not found" << std::endl << std::endl; |
||||
} |
||||
|
||||
// Show the image
|
||||
cv::imshow("MODEL REGISTRATION", img_vis); |
||||
|
||||
// Show image until ESC pressed
|
||||
cv::waitKey(0); |
||||
|
||||
|
||||
/*
|
||||
* |
||||
* COMPUTE 3D of the image Keypoints |
||||
* |
||||
*/ |
||||
|
||||
|
||||
// Containers for keypoints and descriptors of the model
|
||||
std::vector<cv::KeyPoint> keypoints_model; |
||||
cv::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) { |
||||
cv::Point2f point2d(keypoints_model[i].pt); |
||||
cv::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); |
||||
} |
||||
} |
||||
|
||||
// save the model into a *.yaml file
|
||||
model.save(write_path); |
||||
|
||||
// Out image
|
||||
img_vis = img_in.clone(); |
||||
|
||||
// The list of the points2d of the model
|
||||
std::vector<cv::Point2f> list_points_in = model.get_points2d_in(); |
||||
std::vector<cv::Point2f> list_points_out = model.get_points2d_out(); |
||||
|
||||
// Draw some debug text
|
||||
std::string num = IntToString(list_points_in.size()); |
||||
std::string text = "There are " + num + " inliers"; |
||||
drawText(img_vis, text, green); |
||||
|
||||
// Draw some debug text
|
||||
num = IntToString(list_points_out.size()); |
||||
text = "There are " + num + " outliers"; |
||||
drawText2(img_vis, text, red); |
||||
|
||||
// Draw the object mesh
|
||||
drawObjectMesh(img_vis, &mesh, &pnp_registration, blue); |
||||
|
||||
// Draw found keypoints depending on if are or not on the surface
|
||||
draw2DPoints(img_vis, list_points_in, green); |
||||
draw2DPoints(img_vis, list_points_out, red); |
||||
|
||||
// Show the image
|
||||
cv::imshow("MODEL REGISTRATION", img_vis); |
||||
|
||||
// Wait until ESC pressed
|
||||
cv::waitKey(0); |
||||
|
||||
// Close and Destroy Window
|
||||
cv::destroyWindow("MODEL REGISTRATION"); |
||||
|
||||
std::cout << "GOODBYE" << std::endl; |
||||
|
||||
} |
||||
@ -0,0 +1,327 @@ |
||||
#include <iostream> |
||||
|
||||
#include "cv.h" |
||||
#include "highgui.h" |
||||
|
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/imgproc/imgproc.hpp> |
||||
#include <opencv2/nonfree/features2d.hpp> |
||||
#include <opencv2/calib3d/calib3d.hpp> |
||||
|
||||
#include "Mesh.h" |
||||
#include "Model.h" |
||||
#include "PnPProblem.h" |
||||
#include "RobustMatcher.h" |
||||
#include "ModelRegistration.h" |
||||
#include "Utils.h" |
||||
|
||||
#include "CsvWriter.h" |
||||
|
||||
|
||||
/*
|
||||
* Set up the images paths |
||||
*/ |
||||
|
||||
std::string img_verification_path = "../Data/resized_IMG_3872.JPG"; |
||||
std::string ply_read_path = "../Data/box.ply"; |
||||
std::string yml_read_path = "../Data/cookies_ORB.yml"; |
||||
|
||||
// Boolean the know if the registration it's done
|
||||
bool end_registration = false; |
||||
|
||||
// Setup the points to register in the image
|
||||
// In the order of the *.ply file and starting at 1
|
||||
int n = 7; |
||||
int pts[] = {1, 2, 3, 5, 6, 7, 8}; |
||||
|
||||
/*
|
||||
* Set up the intrinsic camera parameters: CANON |
||||
*/ |
||||
double f = 43; |
||||
double sx = 22.3, sy = 14.9; |
||||
double width = 718, height = 480; |
||||
double params_CANON[] = { width*f/sx, // fx
|
||||
height*f/sy, // fy
|
||||
width/2, // cx
|
||||
height/2}; // cy
|
||||
|
||||
/*
|
||||
* Set up some basic colors |
||||
*/ |
||||
cv::Scalar red(0, 0, 255); |
||||
cv::Scalar green(0,255,0); |
||||
cv::Scalar blue(255,0,0); |
||||
cv::Scalar yellow(0,255,255); |
||||
|
||||
/*
|
||||
* CREATE MODEL REGISTRATION OBJECT |
||||
* CREATE OBJECT MESH |
||||
* CREATE OBJECT MODEL |
||||
* CREATE PNP OBJECT |
||||
*/ |
||||
Mesh mesh; |
||||
ModelRegistration registration; |
||||
PnPProblem pnp_verification_epnp(params_CANON); |
||||
PnPProblem pnp_verification_iter(params_CANON); |
||||
PnPProblem pnp_verification_p3p(params_CANON); |
||||
PnPProblem pnp_verification_dls(params_CANON); |
||||
PnPProblem pnp_verification_gt(params_CANON); // groud truth
|
||||
|
||||
|
||||
// Mouse events for model registration
|
||||
static void onMouseModelVerification( int event, int x, int y, int, void* ) |
||||
{ |
||||
if ( event == cv::EVENT_LBUTTONUP ) |
||||
{ |
||||
int n_regist = registration.getNumRegist(); |
||||
int n_vertex = pts[n_regist]; |
||||
|
||||
cv::Point2f point_2d = cv::Point2f(x,y); |
||||
cv::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; |
||||
} |
||||
} |
||||
} |
||||
|
||||
|
||||
/*
|
||||
* MAIN PROGRAM |
||||
* |
||||
*/ |
||||
|
||||
int main(int, char**) |
||||
{ |
||||
|
||||
std::cout << "!!!Hello Verification!!!" << std::endl; // prints !!!Hello World!!!
|
||||
|
||||
// load a mesh given the *.ply file path
|
||||
mesh.load(ply_read_path); |
||||
|
||||
// load the 3D textured object model
|
||||
Model model; |
||||
model.load(yml_read_path); |
||||
|
||||
// set parameters
|
||||
int numKeyPoints = 10000; |
||||
|
||||
//Instantiate robust matcher: detector, extractor, matcher
|
||||
RobustMatcher rmatcher; |
||||
cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints); |
||||
rmatcher.setFeatureDetector(detector); |
||||
rmatcher.setRatio(0.80); |
||||
|
||||
// RANSAC parameters
|
||||
int iterationsCount = 500; |
||||
float reprojectionError = 2.0; |
||||
float confidence = 0.99; |
||||
|
||||
|
||||
/*
|
||||
* GROUND TRUTH SECOND IMAGE |
||||
* |
||||
*/ |
||||
|
||||
cv::Mat img_in, img_vis; |
||||
|
||||
// Setup for new registration
|
||||
registration.setNumMax(n); |
||||
|
||||
// Create & Open Window
|
||||
cv::namedWindow("MODEL GROUND TRUTH", CV_WINDOW_KEEPRATIO); |
||||
|
||||
// Set up the mouse events
|
||||
cv::setMouseCallback("MODEL GROUND TRUTH", onMouseModelVerification, 0 ); |
||||
|
||||
// Open the image to register
|
||||
img_in = cv::imread(img_verification_path, cv::IMREAD_COLOR); |
||||
|
||||
if (!img_in.data) |
||||
{ |
||||
std::cout << "Could not open or find the image" << std::endl; |
||||
return -1; |
||||
} |
||||
|
||||
std::cout << "Click the box corners ..." << std::endl; |
||||
std::cout << "Waiting ..." << std::endl; |
||||
|
||||
// Loop until all the points are registered
|
||||
while ( cv::waitKey(30) < 0 ) |
||||
{ |
||||
// Refresh debug image
|
||||
img_vis = img_in.clone(); |
||||
|
||||
// Current registered points
|
||||
std::vector<cv::Point2f> list_points2d = registration.get_points2d(); |
||||
std::vector<cv::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]; |
||||
cv::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, "GROUND TRUTH", green); |
||||
drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green); |
||||
break; |
||||
} |
||||
|
||||
// Show the image
|
||||
cv::imshow("MODEL GROUND TRUTH", img_vis); |
||||
} |
||||
|
||||
// The list of registered points
|
||||
std::vector<cv::Point2f> list_points2d = registration.get_points2d(); |
||||
std::vector<cv::Point3f> list_points3d = registration.get_points3d(); |
||||
|
||||
// Estimate pose given the registered points
|
||||
bool is_correspondence = pnp_verification_gt.estimatePose(list_points3d, list_points2d, cv::ITERATIVE); |
||||
|
||||
// Compute and draw all mesh object 2D points
|
||||
std::vector<cv::Point2f> pts_2d_ground_truth = pnp_verification_gt.verify_points(&mesh); |
||||
draw2DPoints(img_vis, pts_2d_ground_truth, green); |
||||
|
||||
// Draw the ground truth mesh
|
||||
drawObjectMesh(img_vis, &mesh, &pnp_verification_gt, blue); |
||||
|
||||
// Show the image
|
||||
cv::imshow("MODEL GROUND TRUTH", img_vis); |
||||
|
||||
// Show image until ESC pressed
|
||||
cv::waitKey(0); |
||||
|
||||
|
||||
/*
|
||||
* EXTRACT CORRRESPONDENCES |
||||
* |
||||
*/ |
||||
|
||||
// refresh visualisation image
|
||||
img_vis = img_in.clone(); |
||||
|
||||
// Get the MODEL INFO
|
||||
std::vector<cv::Point2f> list_points2d_model = model.get_points2d_in(); |
||||
std::vector<cv::Point3f> list_points3d_model = model.get_points3d(); |
||||
std::vector<cv::KeyPoint> keypoints_model = model.get_keypoints(); |
||||
cv::Mat descriptors_model = model.get_descriptors(); |
||||
|
||||
// -- Step 1: Robust matching between model descriptors and scene descriptors
|
||||
|
||||
std::vector<cv::DMatch> good_matches; // to obtain the 3D points of the model
|
||||
std::vector<cv::KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
|
||||
|
||||
//rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);
|
||||
rmatcher.robustMatch(img_vis, good_matches, keypoints_scene, descriptors_model); |
||||
|
||||
cv::Mat inliers_idx; |
||||
std::vector<cv::DMatch> matches_inliers; |
||||
std::vector<cv::Point2f> list_points2d_inliers; |
||||
std::vector<cv::Point3f> list_points3d_inliers; |
||||
|
||||
// -- Step 2: Find out the 2D/3D correspondences
|
||||
|
||||
std::vector<cv::Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
|
||||
std::vector<cv::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) |
||||
{ |
||||
cv::Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 3D point from model
|
||||
cv::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(img_vis, list_points2d_scene_match, red);
|
||||
|
||||
/*
|
||||
* COMPUTE PNP ERRORS: |
||||
* Calculation of the rotation and translation error |
||||
* |
||||
*/ |
||||
|
||||
pnp_verification_epnp.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::EPNP); |
||||
pnp_verification_iter.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::ITERATIVE); |
||||
//pnp_verification_p3p.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::P3P);
|
||||
pnp_verification_dls.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::DLS); |
||||
|
||||
// Draw mesh
|
||||
drawObjectMesh(img_vis, &mesh, &pnp_verification_dls, green); |
||||
drawObjectMesh(img_vis, &mesh, &pnp_verification_gt, yellow); |
||||
|
||||
cv::imshow("MODEL GROUND TRUTH", img_vis); |
||||
|
||||
cv::Mat t_true = pnp_verification_gt.get_t_matrix(); |
||||
cv::Mat t_epnp = pnp_verification_epnp.get_t_matrix(); |
||||
cv::Mat t_iter = pnp_verification_iter.get_t_matrix(); |
||||
cv::Mat t_p3p = pnp_verification_p3p.get_t_matrix(); |
||||
cv::Mat t_dls = pnp_verification_dls.get_t_matrix(); |
||||
|
||||
cv::Mat R_true = pnp_verification_gt.get_R_matrix(); |
||||
cv::Mat R_epnp = pnp_verification_epnp.get_R_matrix(); |
||||
cv::Mat R_iter = pnp_verification_iter.get_R_matrix(); |
||||
cv::Mat R_p3p = pnp_verification_p3p.get_R_matrix(); |
||||
cv::Mat R_dls = pnp_verification_dls.get_R_matrix(); |
||||
|
||||
double error_trans_epnp = get_translation_error(t_true, t_epnp); |
||||
double error_rot_epnp = get_rotation_error(R_true, R_epnp)*180/CV_PI; |
||||
|
||||
double error_trans_iter = get_translation_error(t_true, t_iter); |
||||
double error_rot_iter = get_rotation_error(R_true, R_iter)*180/CV_PI; |
||||
|
||||
double error_trans_p3p = get_translation_error(t_true, t_p3p); |
||||
double error_rot_p3p = get_rotation_error(R_true, R_p3p)*180/CV_PI; |
||||
|
||||
double error_trans_dls = get_translation_error(t_true, t_dls); |
||||
double error_rot_dls = get_rotation_error(R_true, R_dls)*180/CV_PI; |
||||
|
||||
|
||||
std::cout << std::endl << "**** EPNP ERRORS **** " << std::endl; |
||||
|
||||
std::cout << "Translation error: " << error_trans_epnp << " m." << std::endl; |
||||
std::cout << "Rotation error: " << error_rot_epnp << " deg." << std::endl; |
||||
|
||||
|
||||
std::cout << std::endl << "**** ITERATIVE ERRORS **** " << std::endl; |
||||
|
||||
std::cout << "Translation error: " << error_trans_iter << " m." << std::endl; |
||||
std::cout << "Rotation error: " << error_rot_iter << " deg." << std::endl; |
||||
|
||||
|
||||
std::cout << std::endl << "**** P3P ERRORS **** " << std::endl; |
||||
|
||||
std::cout << "Translation error: " << error_trans_p3p << " m." << std::endl; |
||||
std::cout << "Rotation error: " << error_rot_p3p << " deg." << std::endl; |
||||
|
||||
|
||||
std::cout << std::endl << "**** DLS ERRORS **** " << std::endl; |
||||
|
||||
std::cout << "Translation error: " << error_trans_dls << " m." << std::endl; |
||||
std::cout << "Rotation error: " << error_rot_dls << " deg." << std::endl; |
||||
|
||||
|
||||
// Show image until ESC pressed
|
||||
cv::waitKey(0); |
||||
|
||||
// Close and Destroy Window
|
||||
cv::destroyWindow("MODEL GROUND TRUTH"); |
||||
|
||||
std::cout << "GOODBYE" << std::endl; |
||||
|
||||
} |
||||
@ -0,0 +1,143 @@ |
||||
#include <opencv2/core/core.hpp> |
||||
#include <opencv2/calib3d/calib3d.hpp> |
||||
#include <opencv2/contrib/contrib.hpp> |
||||
|
||||
#include <iostream> |
||||
#include <fstream> |
||||
|
||||
using namespace std; |
||||
using namespace cv; |
||||
|
||||
void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1, |
||||
-1, 5), Point3f pmax = Point3f(1, 1, 10)) |
||||
{ |
||||
const Point3f delta = pmax - pmin; |
||||
for (size_t i = 0; i < points.size(); i++) |
||||
{ |
||||
Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX, |
||||
float(rand()) / RAND_MAX); |
||||
p.x *= delta.x; |
||||
p.y *= delta.y; |
||||
p.z *= delta.z; |
||||
p = p + pmin; |
||||
points[i] = p; |
||||
} |
||||
} |
||||
|
||||
void generateCameraMatrix(Mat& cameraMatrix, RNG& rng) |
||||
{ |
||||
const double fcMinVal = 1e-3; |
||||
const double fcMaxVal = 100; |
||||
cameraMatrix.create(3, 3, CV_64FC1); |
||||
cameraMatrix.setTo(Scalar(0)); |
||||
cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal); |
||||
cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal); |
||||
cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal); |
||||
cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal); |
||||
cameraMatrix.at<double>(2,2) = 1; |
||||
} |
||||
|
||||
void generateDistCoeffs(Mat& distCoeffs, RNG& rng) |
||||
{ |
||||
distCoeffs = Mat::zeros(4, 1, CV_64FC1); |
||||
for (int i = 0; i < 3; i++) |
||||
distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-6); |
||||
} |
||||
|
||||
void generatePose(Mat& rvec, Mat& tvec, RNG& rng) |
||||
{ |
||||
const double minVal = 1.0e-3; |
||||
const double maxVal = 1.0; |
||||
rvec.create(3, 1, CV_64FC1); |
||||
tvec.create(3, 1, CV_64FC1); |
||||
for (int i = 0; i < 3; i++) |
||||
{ |
||||
rvec.at<double>(i,0) = rng.uniform(minVal, maxVal); |
||||
tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10); |
||||
} |
||||
} |
||||
|
||||
void data2file(const string& path, const vector<vector<double> >& data) |
||||
{ |
||||
std::fstream fs; |
||||
fs.open(path.c_str(), std::fstream::in | std::fstream::out | std::fstream::app); |
||||
|
||||
for (int method = 0; method < data.size(); ++method) |
||||
{ |
||||
for (int i = 0; i < data[method].size(); ++i) |
||||
{ |
||||
fs << data[method][i] << " "; |
||||
} |
||||
fs << endl; |
||||
} |
||||
|
||||
fs.close(); |
||||
} |
||||
|
||||
|
||||
int main(int argc, char *argv[]) |
||||
{ |
||||
|
||||
RNG rng; |
||||
// TickMeter tm;
|
||||
vector<vector<double> > error_trans(4), error_rot(4), comp_time(4); |
||||
|
||||
int maxpoints = 2000; |
||||
for (int npoints = 10; npoints < maxpoints+10; ++npoints) |
||||
{ |
||||
// generate 3D point cloud
|
||||
vector<Point3f> points; |
||||
points.resize(npoints); |
||||
generate3DPointCloud(points); |
||||
|
||||
// generate cameramatrix
|
||||
Mat rvec, tvec; |
||||
Mat trueRvec, trueTvec; |
||||
Mat intrinsics, distCoeffs; |
||||
generateCameraMatrix(intrinsics, rng); |
||||
|
||||
// generate distorsion coefficients
|
||||
generateDistCoeffs(distCoeffs, rng); |
||||
|
||||
// generate groud truth pose
|
||||
generatePose(trueRvec, trueTvec, rng); |
||||
|
||||
for (int method = 0; method < 4; ++method) |
||||
{ |
||||
std::vector<Point3f> opoints; |
||||
if (method == 2) |
||||
{ |
||||
opoints = std::vector<Point3f>(points.begin(), points.begin()+4); |
||||
} |
||||
else |
||||
opoints = points; |
||||
|
||||
vector<Point2f> projectedPoints; |
||||
projectedPoints.resize(opoints.size()); |
||||
projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints); |
||||
|
||||
//tm.reset(); tm.start();
|
||||
|
||||
solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec, false, method); |
||||
|
||||
// tm.stop();
|
||||
|
||||
// double compTime = tm.getTimeMilli();
|
||||
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec); |
||||
|
||||
error_rot[method].push_back(rvecDiff); |
||||
error_trans[method].push_back(tvecDiff); |
||||
//comp_time[method].push_back(compTime);
|
||||
|
||||
} |
||||
|
||||
//system("clear");
|
||||
cout << "Completed " << npoints+1 << "/" << maxpoints << endl; |
||||
|
||||
} |
||||
|
||||
data2file("translation_error.txt", error_trans); |
||||
data2file("rotation_error.txt", error_rot); |
||||
data2file("computation_time.txt", comp_time); |
||||
|
||||
} |
||||
Loading…
Reference in new issue