mirror of
https://github.com/opencv/opencv.git
synced 2026-07-25 21:33:04 +04:00
Merge pull request #27874 from asmorkalov:as/3dobject_pose
3D object pose estimation tutorial update #27874 1. Use cv::loadMesh instead of custom csv-based reader. 2. Use code snippets ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [ ] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Alexander Smorkalov
committed by
GitHub
GitHub
parent
2470c07f1b
commit
9c33ddea31
@@ -2,8 +2,6 @@ set(sample_dir ${CMAKE_CURRENT_SOURCE_DIR}/tutorial_code/calib3d/real_time_pose_
|
||||
set(target example_tutorial_)
|
||||
|
||||
set(sample_pnplib
|
||||
${sample_dir}CsvReader.cpp
|
||||
${sample_dir}CsvWriter.cpp
|
||||
${sample_dir}ModelRegistration.cpp
|
||||
${sample_dir}Mesh.cpp
|
||||
${sample_dir}Model.cpp
|
||||
|
||||
@@ -1,79 +0,0 @@
|
||||
#include "CsvReader.h"
|
||||
|
||||
/** The default constructor of the CSV reader Class */
|
||||
CsvReader::CsvReader(const string &path, char separator){
|
||||
_file.open(path.c_str(), ifstream::in);
|
||||
_separator = separator;
|
||||
}
|
||||
|
||||
/* Read a plane text file with .ply format */
|
||||
void CsvReader::readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles)
|
||||
{
|
||||
std::string line, tmp_str, n;
|
||||
int num_vertex = 0, num_triangles = 0;
|
||||
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 = (float)StringToInt(x);
|
||||
tmp_p.y = (float)StringToInt(y);
|
||||
tmp_p.z = (float)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)
|
||||
{
|
||||
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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
#ifndef CSVREADER_H
|
||||
#define CSVREADER_H
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include "Utils.h"
|
||||
|
||||
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 string &path, char separator = ' ');
|
||||
|
||||
/**
|
||||
* Read a plane text file with .ply format
|
||||
*
|
||||
* @param list_vertex - The container of the vertices list of the mesh
|
||||
* @param list_triangle - The container of the triangles list of the mesh
|
||||
* @return
|
||||
*/
|
||||
void readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles);
|
||||
|
||||
private:
|
||||
/** The current stream file for the reader */
|
||||
ifstream _file;
|
||||
/** The separator character between words for each line */
|
||||
char _separator;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -1,45 +0,0 @@
|
||||
#include "CsvWriter.h"
|
||||
|
||||
CsvWriter::CsvWriter(const string &path, const string &separator){
|
||||
_file.open(path.c_str(), ofstream::out);
|
||||
_isFirstTerm = true;
|
||||
_separator = separator;
|
||||
}
|
||||
|
||||
CsvWriter::~CsvWriter() {
|
||||
_file.flush();
|
||||
_file.close();
|
||||
}
|
||||
|
||||
void CsvWriter::writeXYZ(const vector<Point3f> &list_points3d)
|
||||
{
|
||||
for(size_t i = 0; i < list_points3d.size(); ++i)
|
||||
{
|
||||
string x = FloatToString(list_points3d[i].x);
|
||||
string y = FloatToString(list_points3d[i].y);
|
||||
string z = FloatToString(list_points3d[i].z);
|
||||
|
||||
_file << x << _separator << y << _separator << z << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void CsvWriter::writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors)
|
||||
{
|
||||
for(size_t i = 0; i < list_points3d.size(); ++i)
|
||||
{
|
||||
string u = FloatToString(list_points2d[i].x);
|
||||
string v = FloatToString(list_points2d[i].y);
|
||||
string x = FloatToString(list_points3d[i].x);
|
||||
string y = FloatToString(list_points3d[i].y);
|
||||
string z = FloatToString(list_points3d[i].z);
|
||||
|
||||
_file << u << _separator << v << _separator << x << _separator << y << _separator << z;
|
||||
|
||||
for(int j = 0; j < 32; ++j)
|
||||
{
|
||||
string descriptor_str = FloatToString(descriptors.at<float>((int)i,j));
|
||||
_file << _separator << descriptor_str;
|
||||
}
|
||||
_file << std::endl;
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
#ifndef CSVWRITER_H
|
||||
#define CSVWRITER_H
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <opencv2/core.hpp>
|
||||
#include "Utils.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
|
||||
class CsvWriter {
|
||||
public:
|
||||
CsvWriter(const string &path, const string &separator = " ");
|
||||
~CsvWriter();
|
||||
void writeXYZ(const vector<Point3f> &list_points3d);
|
||||
void writeUVXYZ(const vector<Point3f> &list_points3d, const vector<Point2f> &list_points2d, const Mat &descriptors);
|
||||
|
||||
private:
|
||||
ofstream _file;
|
||||
string _separator;
|
||||
bool _isFirstTerm;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -6,8 +6,7 @@
|
||||
*/
|
||||
|
||||
#include "Mesh.h"
|
||||
#include "CsvReader.h"
|
||||
|
||||
#include <opencv2/3d.hpp>
|
||||
|
||||
// --------------------------------------------------- //
|
||||
// TRIANGLE CLASS //
|
||||
@@ -60,19 +59,12 @@ Mesh::~Mesh()
|
||||
}
|
||||
|
||||
/** Load a CSV with *.ply format **/
|
||||
//! [mesh_load]
|
||||
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_);
|
||||
|
||||
cv::loadMesh(path, list_vertex_, list_triangles_);
|
||||
// Update mesh attributes
|
||||
num_vertices_ = (int)list_vertex_.size();
|
||||
num_triangles_ = (int)list_triangles_.size();
|
||||
}
|
||||
//! [mesh_load]
|
||||
|
||||
@@ -6,7 +6,6 @@
|
||||
*/
|
||||
|
||||
#include "Model.h"
|
||||
#include "CsvWriter.h"
|
||||
|
||||
Model::Model() : n_correspondences_(0), list_points2d_in_(0), list_points2d_out_(0), list_points3d_in_(0), training_img_path_()
|
||||
{
|
||||
@@ -60,7 +59,8 @@ void Model::save(const std::string &path)
|
||||
storage.release();
|
||||
}
|
||||
|
||||
/** Load a YAML file using OpenCv functions **/
|
||||
/** Load a YAML file using OpenCV functions **/
|
||||
//! [model_load]
|
||||
void Model::load(const std::string &path)
|
||||
{
|
||||
cv::Mat points3d_mat;
|
||||
@@ -81,3 +81,4 @@ void Model::load(const std::string &path)
|
||||
|
||||
storage.release();
|
||||
}
|
||||
//! [model_load]
|
||||
|
||||
@@ -59,7 +59,7 @@ static cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, c
|
||||
}
|
||||
|
||||
// Custom constructor given the intrinsic camera parameters
|
||||
|
||||
//! [pnp_ctor]
|
||||
PnPProblem::PnPProblem(const double params[])
|
||||
{
|
||||
A_matrix_ = cv::Mat::zeros(3, 3, CV_64FC1); // intrinsic camera parameters
|
||||
@@ -73,6 +73,7 @@ PnPProblem::PnPProblem(const double params[])
|
||||
P_matrix_ = cv::Mat::zeros(3, 4, CV_64FC1); // rotation-translation matrix
|
||||
|
||||
}
|
||||
//! [pnp_ctor]
|
||||
|
||||
PnPProblem::~PnPProblem()
|
||||
{
|
||||
@@ -122,7 +123,7 @@ bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
|
||||
}
|
||||
|
||||
// Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use
|
||||
|
||||
//! [pnp_ransac]
|
||||
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
|
||||
@@ -145,6 +146,7 @@ void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points
|
||||
this->set_P_matrix(R_matrix_, t_matrix_); // set rotation-translation matrix
|
||||
|
||||
}
|
||||
//! [pnp_ransac]
|
||||
|
||||
// Given the mesh, backproject the 3D points to 2D to verify the pose estimation
|
||||
std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
|
||||
@@ -161,6 +163,7 @@ std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
|
||||
}
|
||||
|
||||
// Backproject a 3D point to 2D using the estimated pose parameters
|
||||
//! [pnp_backproj]
|
||||
cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
|
||||
{
|
||||
// 3D point vector [x y z 1]'
|
||||
@@ -181,6 +184,7 @@ cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
|
||||
|
||||
return point2d;
|
||||
}
|
||||
//! [pnp_backproj]
|
||||
|
||||
// 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)
|
||||
|
||||
@@ -84,6 +84,7 @@ void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& m
|
||||
}
|
||||
}
|
||||
|
||||
//! [robust_match]
|
||||
void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
|
||||
std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model,
|
||||
const std::vector<cv::KeyPoint>& keypoints_model)
|
||||
@@ -118,6 +119,7 @@ void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>&
|
||||
cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_);
|
||||
}
|
||||
}
|
||||
//! [robust_match]
|
||||
|
||||
void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
|
||||
std::vector<cv::KeyPoint>& keypoints_frame,
|
||||
|
||||
@@ -148,12 +148,14 @@ int main(int argc, char *argv[])
|
||||
Mesh mesh; // instantiate Mesh object
|
||||
mesh.load(ply_read_path); // load an object mesh
|
||||
|
||||
//! [features]
|
||||
RobustMatcher rmatcher; // instantiate RobustMatcher
|
||||
|
||||
Ptr<FeatureDetector> detector, descriptor;
|
||||
createFeatures(featureName, numKeyPoints, detector, descriptor);
|
||||
rmatcher.setFeatureDetector(detector); // set feature detector
|
||||
rmatcher.setDescriptorExtractor(descriptor); // set descriptor extractor
|
||||
//! [features]
|
||||
rmatcher.setDescriptorMatcher(createMatcher(featureName, useFLANN)); // set matcher
|
||||
rmatcher.setRatio(ratioTest); // set ratio test parameter
|
||||
if (!model.get_trainingImagePath().empty())
|
||||
@@ -162,6 +164,7 @@ int main(int argc, char *argv[])
|
||||
rmatcher.setTrainingImage(trainingImg);
|
||||
}
|
||||
|
||||
//! [Kalman_init_call]
|
||||
KalmanFilter KF; // instantiate Kalman Filter
|
||||
int nStates = 18; // the number of states
|
||||
int nMeasurements = 6; // the number of measured states
|
||||
@@ -169,6 +172,8 @@ int main(int argc, char *argv[])
|
||||
double dt = 0.125; // time between measurements (1/FPS)
|
||||
|
||||
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function
|
||||
//! [Kalman_init_call]
|
||||
|
||||
Mat measurements(nMeasurements, 1, CV_64FC1); measurements.setTo(Scalar(0));
|
||||
bool good_measurement = false;
|
||||
|
||||
@@ -209,6 +214,7 @@ int main(int argc, char *argv[])
|
||||
frame_vis = frame.clone(); // refresh visualisation frame
|
||||
|
||||
// -- Step 1: Robust matching between model descriptors and scene descriptors
|
||||
//! [robust_match_call]
|
||||
vector<DMatch> good_matches; // to obtain the 3D points of the model
|
||||
vector<KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
|
||||
|
||||
@@ -220,6 +226,7 @@ int main(int argc, char *argv[])
|
||||
{
|
||||
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model, keypoints_model);
|
||||
}
|
||||
//! [robust_match_call]
|
||||
|
||||
frame_matching = rmatcher.getImageMatching();
|
||||
if (!frame_matching.empty())
|
||||
@@ -228,6 +235,7 @@ int main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
// -- Step 2: Find out the 2D/3D correspondences
|
||||
//! [correspondences]
|
||||
vector<Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
|
||||
vector<Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
|
||||
|
||||
@@ -238,6 +246,7 @@ int main(int argc, char *argv[])
|
||||
list_points3d_model_match.push_back(point3d_model); // add 3D point
|
||||
list_points2d_scene_match.push_back(point2d_scene); // add 2D point
|
||||
}
|
||||
//! [correspondences]
|
||||
|
||||
// Draw outliers
|
||||
draw2DPoints(frame_vis, list_points2d_scene_match, red);
|
||||
@@ -268,6 +277,7 @@ int main(int argc, char *argv[])
|
||||
|
||||
// -- Step 5: Kalman Filter
|
||||
|
||||
//! [step_5]
|
||||
// GOOD MEASUREMENT
|
||||
if( inliers_idx.rows >= minInliersKalman )
|
||||
{
|
||||
@@ -287,12 +297,14 @@ int main(int argc, char *argv[])
|
||||
Mat rotation_estimated(3, 3, CV_64FC1);
|
||||
updateKalmanFilter( KF, measurements,
|
||||
translation_estimated, rotation_estimated);
|
||||
//! [step_5]
|
||||
|
||||
// -- Step 6: Set estimated projection matrix
|
||||
pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
|
||||
}
|
||||
|
||||
// -- Step X: Draw pose and coordinate frame
|
||||
//! [step_x]
|
||||
float l = 5;
|
||||
vector<Point2f> pose_points2d;
|
||||
if (!good_measurement || displayFilteredPose)
|
||||
@@ -315,6 +327,7 @@ int main(int argc, char *argv[])
|
||||
pose_points2d.push_back(pnp_detection.backproject3DPoint(Point3f(0,0,l))); // axis z
|
||||
draw3DCoordinateAxes(frame_vis, pose_points2d); // draw axes
|
||||
}
|
||||
//! [step_x]
|
||||
|
||||
// FRAME RATE
|
||||
// see how much time has elapsed
|
||||
@@ -389,6 +402,7 @@ void help()
|
||||
}
|
||||
|
||||
/**********************************************************************************************************/
|
||||
//! [Kalman_init]
|
||||
void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt)
|
||||
{
|
||||
KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter
|
||||
@@ -457,8 +471,10 @@ void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInp
|
||||
KF.measurementMatrix.at<double>(4,10) = 1; // pitch
|
||||
KF.measurementMatrix.at<double>(5,11) = 1; // yaw
|
||||
}
|
||||
//! [Kalman_init]
|
||||
|
||||
/**********************************************************************************************************/
|
||||
//! [Kalman_update]
|
||||
void updateKalmanFilter( KalmanFilter &KF, Mat &measurement,
|
||||
Mat &translation_estimated, Mat &rotation_estimated )
|
||||
{
|
||||
@@ -482,8 +498,10 @@ void updateKalmanFilter( KalmanFilter &KF, Mat &measurement,
|
||||
// Convert estimated quaternion to rotation matrix
|
||||
rotation_estimated = euler2rot(eulers_estimated);
|
||||
}
|
||||
//! [Kalman_update]
|
||||
|
||||
/**********************************************************************************************************/
|
||||
//! [fill_measure]
|
||||
void fillMeasurements( Mat &measurements,
|
||||
const Mat &translation_measured, const Mat &rotation_measured)
|
||||
{
|
||||
@@ -499,3 +517,4 @@ void fillMeasurements( Mat &measurements,
|
||||
measurements.at<double>(4) = measured_eulers.at<double>(1); // pitch
|
||||
measurements.at<double>(5) = measured_eulers.at<double>(2); // yaw
|
||||
}
|
||||
//! [fill_measure]
|
||||
|
||||
Reference in New Issue
Block a user