From 55819ae464023fadfe47e50ed0c4ba73587c45c8 Mon Sep 17 00:00:00 2001 From: edgarriba Date: Tue, 5 Aug 2014 15:48:54 +0200 Subject: [PATCH] Tutorial code restructure --- .../real_time_pose_estimation/CMakeLists.txt | 2 - .../{include => src}/CsvReader.h | 0 .../{include => src}/CsvWriter.h | 0 .../{include => src}/Mesh.h | 0 .../{include => src}/Model.h | 0 .../{include => src}/ModelRegistration.h | 0 .../{include => src}/PnPProblem.h | 0 .../{include => src}/RobustMatcher.h | 0 .../{include => src}/Utils.h | 0 .../src/main_detection.cpp | 4 +- .../src/main_verification.cpp | 328 +++++++++++++++++++++ .../real_time_pose_estimation/src/test_pnp.cpp | 144 +++++++++ 12 files changed, 474 insertions(+), 4 deletions(-) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/CsvReader.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/CsvWriter.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/Mesh.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/Model.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/ModelRegistration.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/PnPProblem.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/RobustMatcher.h (100%) rename samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/{include => src}/Utils.h (100%) create mode 100644 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_verification.cpp create mode 100644 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/test_pnp.cpp 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 a87f9e5..4382fd1 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 @@ -3,14 +3,12 @@ project( PNP_DEMO ) ADD_DEFINITIONS( -std=c++11 - -std=c++0x # Other flags ) find_package( OpenCV REQUIRED ) include_directories( - $(PNP_DEMO_SOURCE_DIR)/include ${OpenCV_INCLUDE_DIRS} ) diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/CsvReader.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/CsvReader.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/CsvWriter.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/CsvWriter.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvWriter.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/Mesh.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/Mesh.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/Model.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/Model.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Model.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/ModelRegistration.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/ModelRegistration.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/ModelRegistration.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/PnPProblem.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/PnPProblem.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/RobustMatcher.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/RobustMatcher.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.h diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/Utils.h b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h similarity index 100% rename from samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/include/Utils.h rename to samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h 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 faa26f8..750f147 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 @@ -160,7 +160,7 @@ int main(int argc, char *argv[]) cv::VideoCapture cap; // instantiate VideoCapture - (argc < 2) ? cap.open(0) : cap.open(argv[1]); // open the default camera device + (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 @@ -238,7 +238,7 @@ int main(int argc, char *argv[]) // -- Step 3: Estimate the pose using RANSAC approach pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match, - cv::ITERATIVE, inliers_idx, + cv::DLS, inliers_idx, iterationsCount, reprojectionError, confidence ); // -- Step 4: Catch the inliers keypoints to draw diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_verification.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_verification.cpp new file mode 100644 index 0000000..1c6b15b --- /dev/null +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_verification.cpp @@ -0,0 +1,328 @@ +#include +#include + +#include "cv.h" +#include "highgui.h" + +#include +#include +#include +#include + +#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 list_points2d = registration.get_points2d(); + std::vector 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 list_points2d = registration.get_points2d(); + std::vector 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 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 list_points2d_model = model.get_points2d_in(); + std::vector list_points3d_model = model.get_points3d(); + std::vector keypoints_model = model.get_keypoints(); + cv::Mat descriptors_model = model.get_descriptors(); + + // -- Step 1: Robust matching between model descriptors and scene descriptors + + std::vector good_matches; // to obtain the 3D points of the model + std::vector 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 matches_inliers; + std::vector list_points2d_inliers; + std::vector list_points3d_inliers; + + // -- 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 + + 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; + +} diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/test_pnp.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/test_pnp.cpp new file mode 100644 index 0000000..5717a91 --- /dev/null +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/test_pnp.cpp @@ -0,0 +1,144 @@ +#include +#include +#include + +#include +#include + +using namespace std; +using namespace cv; + +void generate3DPointCloud(vector& 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(0,0) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(1,1) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(0,2) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(1,2) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(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(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(i,0) = rng.uniform(minVal, maxVal); + tvec.at(i,0) = rng.uniform(minVal, maxVal/10); + } +} + +void data2file(const string& path, const vector >& 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 > 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 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 opoints; + if (method == 2) + { + opoints = std::vector(points.begin(), points.begin()+4); + } + else + opoints = points; + + vector 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); + +} + -- 2.7.4