diff --git a/doc/tutorials/calib3d/real_time_pose/real_time_pose.markdown b/doc/tutorials/calib3d/real_time_pose/real_time_pose.markdown index 48c38982ec..4c1699ebe9 100644 --- a/doc/tutorials/calib3d/real_time_pose/real_time_pose.markdown +++ b/doc/tutorials/calib3d/real_time_pose/real_time_pose.markdown @@ -9,15 +9,16 @@ Real Time pose estimation of a textured object {#tutorial_real_time_pose} | | | | -: | :- | | Original author | Edgar Riba | -| Compatibility | OpenCV >= 3.0 | +| Compatibility | OpenCV >= 5.0 | Nowadays, augmented reality is one of the top research topic in computer vision and robotics fields. The most elemental problem in augmented reality is the estimation of the camera pose respect of an -object in the case of computer vision area to perform subsequent 3D rendering or, in robotics, to obtain an object pose for grasping and manipulation. However, this is not a trivial -problem to solve due to the fact that the most common issue in image processing is the computational -cost of applying a lot of algorithms or mathematical operations for solving a problem which is basic -and immediately for humans. +object in the case of computer vision area to perform subsequent 3D rendering or, in robotics, to +obtain an object pose for grasping and manipulation. However, this is not a trivial problem to solve +due to the fact that the most common issue in image processing is the computational cost of applying +a lot of algorithms or mathematical operations for solving a problem which is basic and immediately +for humans. Goal ---- @@ -158,22 +159,9 @@ Here is explained in detail the code for the real time application: *load()* that opens a YAML file and take the stored 3D points with its corresponding descriptors. You can find an example of a 3D textured model in `samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml`. - @code{.cpp} - /* Load a YAML file using OpenCV */ - 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_; + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp model_load - points3d_mat.copyTo(list_points3d_in_); - - storage.release(); - - } - @endcode In the main program the model is loaded as follows: @code{.cpp} Model model; // instantiate Model object @@ -183,27 +171,9 @@ Here is explained in detail the code for the real time application: that opens a \f$*\f$.ply file and store the 3D points of the object and also the composed triangles. You can find an example of a model mesh in `samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply`. - @code{.cpp} - /* Load a CSV with *.ply format */ - void Mesh::load(const std::string path) - { - // Create the reader - CsvReader csvReader(path); + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp mesh_load - // 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(); - - } - @endcode In the main program the mesh is loaded as follows: @code{.cpp} Mesh mesh; // instantiate Mesh object @@ -260,15 +230,9 @@ Here is explained in detail the code for the real time application: The following code is how to instantiate and set the features detector and the descriptors extractor: - @code{.cpp} - RobustMatcher rmatcher; // instantiate RobustMatcher - cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints); // instantiate ORB feature detector - cv::DescriptorExtractor * extractor = new cv::OrbDescriptorExtractor(); // instantiate ORB descriptor extractor + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp features - rmatcher.setFeatureDetector(detector); // set feature detector - rmatcher.setDescriptorExtractor(extractor); // set descriptor extractor - @endcode The features and descriptors will be computed by the *RobustMatcher* inside the matching function. -# **Match scene descriptors with model descriptors using Flann matcher** @@ -305,21 +269,9 @@ Here is explained in detail the code for the real time application: std::vector 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 @endcode - @code{.cpp} - // -- Step 1: Robust matching between model descriptors and scene descriptors - std::vector good_matches; // to obtain the model 3D points in the scene - std::vector keypoints_scene; // to obtain the 2D points of the scene + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp robust_match_call - if(fast_match) - { - rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model); - } - else - { - rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model); - } - @endcode The following code corresponds to the *robustMatch()* function which belongs to the *RobustMatcher* class. This function uses the given image to detect the keypoints and extract the descriptors, match using *two Nearest Neighbour* the extracted descriptors with the given model @@ -327,56 +279,15 @@ Here is explained in detail the code for the real time application: remove these matches which its distance ratio between the first and second best match is larger than a given threshold. Finally, a symmetry test is applied in order to remove non symmetrical matches. - @code{.cpp} - void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector& good_matches, - std::vector& keypoints_frame, - const std::vector& keypoints_model, const cv::Mat& descriptors_model ) - { - // 1a. Detection of the ORB features - this->computeKeyPoints(frame, keypoints_frame); + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp robust_match - // 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 > 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); - - } - @endcode After the matches filtering we have to subtract the 2D and 3D correspondences from the found scene keypoints and our 3D model using the obtained *DMatches* vector. For more information about @ref cv::DMatch check the documentation. - @code{.cpp} - // -- Step 2: Find out the 2D/3D correspondences - std::vector list_points3d_model_match; // container for the model 3D coordinates found in the scene - std::vector list_points2d_scene_match; // container for the model 2D coordinates found in the scene + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp correspondences - 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 - } - @endcode You can also change the ratio test threshold, the number of keypoints to detect as well as use or not the robust matcher: @code{.cpp} @@ -402,6 +313,7 @@ Here is explained in detail the code for the real time application: @ref tutorial_camera_calibration_square_chess and @ref tutorial_camera_calibration tutorials. The following code is how to declare the *PnPProblem class* in the main program: + @code{.cpp} // Intrinsic camera parameters: UVC WEBCAM @@ -417,23 +329,9 @@ Here is explained in detail the code for the real time application: PnPProblem pnp_detection(params_WEBCAM); // instantiate PnPProblem class @endcode The following code is how the *PnPProblem class* initialises its attributes: - @code{.cpp} - // 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(0, 0) = params[0]; // [ fx 0 cx ] - _A_matrix.at(1, 1) = params[1]; // [ 0 fy cy ] - _A_matrix.at(0, 2) = params[2]; // [ 0 0 1 ] - _A_matrix.at(1, 2) = params[3]; - _A_matrix.at(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 + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp pnp_ctor - } - @endcode OpenCV provides four PnP methods: ITERATIVE, EPNP, P3P and DLS. Depending on the application type, the estimation method will be different. In the case that we want to make a real time application, the more suitable methods are EPNP and P3P since they are faster than ITERATIVE and DLS at @@ -462,38 +360,16 @@ Here is explained in detail the code for the real time application: *PnPProblem class*. This function estimates the rotation and translation matrix given a set of 2D/3D correspondences, the desired PnP method to use, the output inliers container and the Ransac parameters: - @code{.cpp} - // Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use - void PnPProblem::estimatePoseRANSAC( const std::vector &list_points3d, // list with model 3D coordinates - const std::vector &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 + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp pnp_ransac - 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 - - } - @endcode In the following code are the 3rd and 4th steps of the main algorithm. The first, calling the above function and the second taking the output inliers vector from RANSAC to get the 2D scene points for drawing purpose. As seen in the code we must be sure to apply RANSAC if we have - matches, in the other case, the function @ref cv::solvePnPRansac crashes due to any OpenCV *bug*. + matches, in the other case, the function @ref cv::solvePnPRansac throws assert on invalid input + (not enough points). @code{.cpp} - if(good_matches.size() > 0) // None matches, then RANSAC crashes + if(good_matches.size() > 4) // OpenCV requires solvePnPRANSAC to minimally have 4 set of points { // -- Step 3: Estimate the pose using RANSAC approach @@ -516,30 +392,9 @@ Here is explained in detail the code for the real time application: The following code corresponds to the *backproject3DPoint()* function which belongs to the *PnPProblem class*. The function backproject a given 3D point expressed in a world reference frame onto a 2D image: - @code{.cpp} - // 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(0) = point3d.x; - point3d_vec.at(1) = point3d.y; - point3d_vec.at(2) = point3d.z; - point3d_vec.at(3) = 1; + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp pnp_backproj - // 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(0) / point2d_vec.at(2); - point2d.y = point2d_vec.at(1) / point2d_vec.at(2); - - return point2d; - } - @endcode The above function is used to compute all the 3D points of the object *Mesh* to show the pose of the object. @@ -573,17 +428,9 @@ Here is explained in detail the code for the real time application: actions to apply to the system which in this case will be *zero*. Finally, we have to define the differential time between measurements which in this case is \f$1/T\f$, where *T* is the frame rate of the video. - @code{.cpp} - 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 + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp Kalman_init_call - double dt = 0.125; // time between measurements (1/FPS) - - initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function - @endcode The following code corresponds to the *Kalman Filter* initialisation. Firstly, is set the process noise, the measurement noise and the error covariance matrix. Secondly, are set the transition matrix which is the dynamic model and finally the measurement matrix, which is the measurement @@ -592,162 +439,29 @@ Here is explained in detail the code for the real time application: You can tune the process and measurement noise to improve the *Kalman Filter* performance. As the measurement noise is reduced the faster will converge doing the algorithm sensitive in front of bad measurements. - @code{.cpp} - void initKalmanFilter(cv::KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt) - { - KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp Kalman_init - cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5)); // set process noise - cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-4)); // 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(0,3) = dt; - KF.transitionMatrix.at(1,4) = dt; - KF.transitionMatrix.at(2,5) = dt; - KF.transitionMatrix.at(3,6) = dt; - KF.transitionMatrix.at(4,7) = dt; - KF.transitionMatrix.at(5,8) = dt; - KF.transitionMatrix.at(0,6) = 0.5*pow(dt,2); - KF.transitionMatrix.at(1,7) = 0.5*pow(dt,2); - KF.transitionMatrix.at(2,8) = 0.5*pow(dt,2); - - // orientation - KF.transitionMatrix.at(9,12) = dt; - KF.transitionMatrix.at(10,13) = dt; - KF.transitionMatrix.at(11,14) = dt; - KF.transitionMatrix.at(12,15) = dt; - KF.transitionMatrix.at(13,16) = dt; - KF.transitionMatrix.at(14,17) = dt; - KF.transitionMatrix.at(9,15) = 0.5*pow(dt,2); - KF.transitionMatrix.at(10,16) = 0.5*pow(dt,2); - KF.transitionMatrix.at(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(0,0) = 1; // x - KF.measurementMatrix.at(1,1) = 1; // y - KF.measurementMatrix.at(2,2) = 1; // z - KF.measurementMatrix.at(3,9) = 1; // roll - KF.measurementMatrix.at(4,10) = 1; // pitch - KF.measurementMatrix.at(5,11) = 1; // yaw - - } - @endcode In the following code is the 5th step of the main algorithm. When the obtained number of inliers after *Ransac* is over the threshold, the measurements matrix is filled and then the *Kalman Filter* is updated: - @code{.cpp} - // -- Step 5: Kalman Filter - // GOOD MEASUREMENT - if( inliers_idx.rows >= minInliersKalman ) - { + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp step_5 - // 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); - - } - - // 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); - @endcode The following code corresponds to the *fillMeasurements()* function which converts the measured [Rotation Matrix to Eulers angles](http://euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm) and fill the measurements matrix along with the measured translation vector: - @code{.cpp} - 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(0) = translation_measured.at(0); // x - measurements.at(1) = translation_measured.at(1); // y - measurements.at(2) = translation_measured.at(2); // z - measurements.at(3) = measured_eulers.at(0); // roll - measurements.at(4) = measured_eulers.at(1); // pitch - measurements.at(5) = measured_eulers.at(2); // yaw - } - @endcode + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp fill_measure + The following code corresponds to the *updateKalmanFilter()* function which update the Kalman Filter and set the estimated Rotation Matrix and translation vector. The estimated Rotation Matrix comes from the estimated [Euler angles to Rotation Matrix](http://euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm). - @code{.cpp} - 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(); + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp Kalman_update - // 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(0) = estimated.at(0); - translation_estimated.at(1) = estimated.at(1); - translation_estimated.at(2) = estimated.at(2); - - // Estimated euler angles - cv::Mat eulers_estimated(3, 1, CV_64F); - eulers_estimated.at(0) = estimated.at(9); - eulers_estimated.at(1) = estimated.at(10); - eulers_estimated.at(2) = estimated.at(11); - - // Convert estimated quaternion to rotation matrix - rotation_estimated = euler2rot(eulers_estimated); - - } - @endcode The 6th step is set the estimated rotation-translation matrix: @code{.cpp} // -- Step 6: Set estimated projection matrix @@ -755,20 +469,9 @@ Here is explained in detail the code for the real time application: @endcode The last and optional step is draw the found pose. To do it I implemented a function to draw all the mesh 3D points and an extra reference axis: - @code{.cpp} - // -- Step X: Draw pose - drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose - drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose + @snippet samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp step_x - double l = 5; - std::vector 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 - @endcode You can also modify the minimum inliers to update Kalman Filter: @code{.cpp} ./cpp-tutorial-pnp_detection --inliers=20 diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt index 5b5520f287..5082332e00 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt @@ -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 diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp deleted file mode 100644 index 1461877ef6..0000000000 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp +++ /dev/null @@ -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 &list_vertex, vector > &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 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++; - } - } - } -} diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h deleted file mode 100644 index cf54cb740c..0000000000 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef CSVREADER_H -#define CSVREADER_H - -#include -#include -#include -#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 &list_vertex, vector > &list_triangles); - -private: - /** The current stream file for the reader */ - ifstream _file; - /** The separator character between words for each line */ - char _separator; -}; - -#endif diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp deleted file mode 100644 index 10d0df479e..0000000000 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.cpp +++ /dev/null @@ -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 &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 &list_points3d, const vector &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((int)i,j)); - _file << _separator << descriptor_str; - } - _file << std::endl; - } -} diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h deleted file mode 100644 index 499fb115ec..0000000000 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef CSVWRITER_H -#define CSVWRITER_H - -#include -#include -#include -#include "Utils.h" - -using namespace std; -using namespace cv; - -class CsvWriter { -public: - CsvWriter(const string &path, const string &separator = " "); - ~CsvWriter(); - void writeXYZ(const vector &list_points3d); - void writeUVXYZ(const vector &list_points3d, const vector &list_points2d, const Mat &descriptors); - -private: - ofstream _file; - string _separator; - bool _isFirstTerm; -}; - -#endif diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp index 2228b6e9d9..c3eb7fb254 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp @@ -6,8 +6,7 @@ */ #include "Mesh.h" -#include "CsvReader.h" - +#include // --------------------------------------------------- // // 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] diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp index e2dc2a8a37..fb9068ba9c 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.cpp @@ -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] diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp index b714a64977..c0f0fca4da 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp @@ -59,7 +59,7 @@ static cv::Point3f get_nearest_3D_point(std::vector &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 &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 &list_points3d, // list with model 3D coordinates const std::vector &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 &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 PnPProblem::verify_points(Mesh *mesh) @@ -161,6 +163,7 @@ std::vector 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) diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp index 740e35931f..07e4530eb9 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp @@ -84,6 +84,7 @@ void RobustMatcher::symmetryTest( const std::vector >& m } } +//! [robust_match] void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector& good_matches, std::vector& keypoints_frame, const cv::Mat& descriptors_model, const std::vector& keypoints_model) @@ -118,6 +119,7 @@ void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector& cv::drawMatches(frame, keypoints_frame, training_img_, keypoints_model, good_matches, img_matching_); } } +//! [robust_match] void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector& good_matches, std::vector& keypoints_frame, diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp index 209eb7abfe..214bacf853 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp @@ -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 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 good_matches; // to obtain the 3D points of the model vector 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 list_points3d_model_match; // container for the model 3D coordinates found in the scene vector 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 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(4,10) = 1; // pitch KF.measurementMatrix.at(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(4) = measured_eulers.at(1); // pitch measurements.at(5) = measured_eulers.at(2); // yaw } +//! [fill_measure]