From 94968c81e000ec80f2e4318f4e4504a1a1c57645 Mon Sep 17 00:00:00 2001 From: edgarriba Date: Wed, 30 Jul 2014 12:57:11 +0200 Subject: [PATCH] Code tutorial --- .../src/main_registration.cpp | 280 +++++++++++++++++++++ 1 file changed, 280 insertions(+) create mode 100644 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp new file mode 100644 index 0000000..cdb00de --- /dev/null +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp @@ -0,0 +1,280 @@ +#include +#include + +#include "Mesh.h" +#include "Model.h" +#include "PnPProblem.h" +#include "RobustMatcher.h" +#include "ModelRegistration.h" +#include "Utils.h" + +#include +#include +#include +#include +#include + + /* + * 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"; + + // 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[]) +{ + + std::cout << "!!!Hello Registration!!!" << std::endl; + + // load a mesh given the *.ply file path + mesh.load(ply_read_path); + + // set parameters + int numKeyPoints = 10000; + + //Instantiate robust matcher: detector, extractor, matcher + RobustMatcher rmatcher; + 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 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, "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 list_points2d = registration.get_points2d(); + std::vector 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 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 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 list_points_in = model.get_points2d_in(); + std::vector list_points_out = model.get_points2d_out(); + + // Draw some debug text + std::string num = boost::lexical_cast< std::string >(list_points_in.size()); + std::string text = "There are " + num + " inliers"; + drawText(img_vis, text, green); + + // Draw some debug text + num = boost::lexical_cast< std::string >(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; + +} -- 2.7.4