samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp

   1 #include <iostream>
   2
   3 #include "Mesh.h"
   4 #include "Model.h"
   5 #include "PnPProblem.h"
   6 #include "RobustMatcher.h"
   7 #include "ModelRegistration.h"
   8 #include "Utils.h"
   9
  10 #include <opencv2/core/core.hpp>
  11 #include <opencv2/imgproc/imgproc.hpp>
  12 #include <opencv2/calib3d/calib3d.hpp>
  13 #include <opencv2/features2d/features2d.hpp>
  14 #include <opencv2/nonfree/features2d.hpp>
  15
  16   /*
  17    * Set up the images paths
  18    */
  19
  20   // COOKIES BOX [718x480]
  21   std::string img_path = "../Data/resized_IMG_3875.JPG"; // f 55
  22
  23   // COOKIES BOX MESH
  24   std::string ply_read_path = "../Data/box.ply";
  25
  26   // YAML writting path
  27   std::string write_path = "../Data/cookies_ORB.yml";
  28
  29   void help()
  30   {
  31   std::cout
  32   << "--------------------------------------------------------------------------"   << std::endl
  33   << "This program shows how to create your 3D textured model. "                    << std::endl
  34   << "Usage:"                                                                       << std::endl
  35   << "./pnp_registration "                                                          << std::endl
  36   << "--------------------------------------------------------------------------"   << std::endl
  37   << std::endl;
  38   }
  39
  40   // Boolean the know if the registration it's done
  41   bool end_registration = false;
  42
  43   /*
  44    * Set up the intrinsic camera parameters: CANON
  45    */
  46   double f = 45; // focal length in mm
  47   double sx = 22.3, sy = 14.9;
  48   double width = 2592, height = 1944;
  49   double params_CANON[] = { width*f/sx,   // fx
  50                             height*f/sy,  // fy
  51                             width/2,      // cx
  52                             height/2};    // cy
  53
  54
  55   // Setup the points to register in the image
  56   // In the order of the *.ply file and starting at 1
  57   int n = 8;
  58   int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
  59
  60   /*
  61    * Set up some basic colors
  62    */
  63   cv::Scalar red(0, 0, 255);
  64   cv::Scalar green(0,255,0);
  65   cv::Scalar blue(255,0,0);
  66   cv::Scalar yellow(0,255,255);
  67
  68   /*
  69    * CREATE MODEL REGISTRATION OBJECT
  70    * CREATE OBJECT MESH
  71    * CREATE OBJECT MODEL
  72    * CREATE PNP OBJECT
  73    */
  74   ModelRegistration registration;
  75   Model model;
  76   Mesh mesh;
  77   PnPProblem pnp_registration(params_CANON);
  78
  79
  80 // Mouse events for model registration
  81 static void onMouseModelRegistration( int event, int x, int y, int, void* )
  82 {
  83   if  ( event == cv::EVENT_LBUTTONUP )
  84   {
  85       int n_regist = registration.getNumRegist();
  86       int n_vertex = pts[n_regist];
  87
  88       cv::Point2f point_2d = cv::Point2f(x,y);
  89       cv::Point3f point_3d = mesh.getVertex(n_vertex-1);
  90
  91       bool is_registrable = registration.is_registrable();
  92       if (is_registrable)
  93       {
  94         registration.registerPoint(point_2d, point_3d);
  95         if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
  96       }
  97   }
  98 }
  99
 100 /*
 101  *   MAIN PROGRAM
 102  *
 103  */
 104
 105 int main(int argc, char *argv[])
 106 {
 107
 108   help();
 109
 110   // load a mesh given the *.ply file path
 111   mesh.load(ply_read_path);
 112
 113   // set parameters
 114   int numKeyPoints = 10000;
 115
 116   //Instantiate robust matcher: detector, extractor, matcher
 117   RobustMatcher rmatcher;
 118   cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints);
 119   rmatcher.setFeatureDetector(detector);
 120
 121   /*
 122    * GROUND TRUTH OF THE FIRST IMAGE
 123    *
 124    * by the moment it is the reference image
 125    */
 126
 127   // Create & Open Window
 128   cv::namedWindow("MODEL REGISTRATION", cv::WINDOW_KEEPRATIO);
 129
 130   // Set up the mouse events
 131   cv::setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 );
 132
 133   // Open the image to register
 134   cv::Mat img_in = cv::imread(img_path, cv::IMREAD_COLOR);
 135   cv::Mat img_vis = img_in.clone();
 136
 137   if (!img_in.data) {
 138     std::cout << "Could not open or find the image" << std::endl;
 139     return -1;
 140   }
 141
 142   // Set the number of points to register
 143   int num_registrations = n;
 144   registration.setNumMax(num_registrations);
 145
 146   std::cout << "Click the box corners ..." << std::endl;
 147   std::cout << "Waiting ..." << std::endl;
 148
 149   // Loop until all the points are registered
 150   while ( cv::waitKey(30) < 0 )
 151   {
 152     // Refresh debug image
 153     img_vis = img_in.clone();
 154
 155     // Current registered points
 156     std::vector<cv::Point2f> list_points2d = registration.get_points2d();
 157     std::vector<cv::Point3f> list_points3d = registration.get_points3d();
 158
 159     // Draw current registered points
 160     drawPoints(img_vis, list_points2d, list_points3d, red);
 161
 162     // If the registration is not finished, draw which 3D point we have to register.
 163     // If the registration is finished, breaks the loop.
 164     if (!end_registration)
 165     {
 166       // Draw debug text
 167       int n_regist = registration.getNumRegist();
 168       int n_vertex = pts[n_regist];
 169       cv::Point3f current_poin3d = mesh.getVertex(n_vertex-1);
 170
 171       drawQuestion(img_vis, current_poin3d, green);
 172       drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);
 173     }
 174     else
 175     {
 176       // Draw debug text
 177       drawText(img_vis, "END REGISTRATION", green);
 178       drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
 179       break;
 180     }
 181
 182     // Show the image
 183     cv::imshow("MODEL REGISTRATION", img_vis);
 184   }
 185
 186
 187   /*
 188    *
 189    * COMPUTE CAMERA POSE
 190    *
 191    */
 192
 193   std::cout << "COMPUTING POSE ..." << std::endl;
 194
 195   // The list of registered points
 196   std::vector<cv::Point2f> list_points2d = registration.get_points2d();
 197   std::vector<cv::Point3f> list_points3d = registration.get_points3d();
 198
 199   // Estimate pose given the registered points
 200   bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, cv::ITERATIVE);
 201   if ( is_correspondence )
 202   {
 203     std::cout << "Correspondence found" << std::endl;
 204
 205     // Compute all the 2D points of the mesh to verify the algorithm and draw it
 206     std::vector<cv::Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh);
 207     draw2DPoints(img_vis, list_points2d_mesh, green);
 208
 209   } else {
 210     std::cout << "Correspondence not found" << std::endl << std::endl;
 211   }
 212
 213   // Show the image
 214   cv::imshow("MODEL REGISTRATION", img_vis);
 215
 216   // Show image until ESC pressed
 217   cv::waitKey(0);
 218
 219
 220    /*
 221     *
 222     * COMPUTE 3D of the image Keypoints
 223     *
 224     */
 225
 226
 227   // Containers for keypoints and descriptors of the model
 228   std::vector<cv::KeyPoint> keypoints_model;
 229   cv::Mat descriptors;
 230
 231   // Compute keypoints and descriptors
 232   rmatcher.computeKeyPoints(img_in, keypoints_model);
 233   rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);
 234
 235   // Check if keypoints are on the surface of the registration image and add to the model
 236   for (unsigned int i = 0; i < keypoints_model.size(); ++i) {
 237     cv::Point2f point2d(keypoints_model[i].pt);
 238     cv::Point3f point3d;
 239     bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
 240     if (on_surface)
 241     {
 242         model.add_correspondence(point2d, point3d);
 243         model.add_descriptor(descriptors.row(i));
 244         model.add_keypoint(keypoints_model[i]);
 245     }
 246     else
 247     {
 248         model.add_outlier(point2d);
 249     }
 250   }
 251
 252   // save the model into a *.yaml file
 253   model.save(write_path);
 254
 255   // Out image
 256   img_vis = img_in.clone();
 257
 258   // The list of the points2d of the model
 259   std::vector<cv::Point2f> list_points_in = model.get_points2d_in();
 260   std::vector<cv::Point2f> list_points_out = model.get_points2d_out();
 261
 262   // Draw some debug text
 263   std::string num = IntToString(list_points_in.size());
 264   std::string text = "There are " + num + " inliers";
 265   drawText(img_vis, text, green);
 266
 267   // Draw some debug text
 268   num = IntToString(list_points_out.size());
 269   text = "There are " + num + " outliers";
 270   drawText2(img_vis, text, red);
 271
 272   // Draw the object mesh
 273   drawObjectMesh(img_vis, &mesh, &pnp_registration, blue);
 274
 275   // Draw found keypoints depending on if are or not on the surface
 276   draw2DPoints(img_vis, list_points_in, green);
 277   draw2DPoints(img_vis, list_points_out, red);
 278
 279   // Show the image
 280   cv::imshow("MODEL REGISTRATION", img_vis);
 281
 282   // Wait until ESC pressed
 283   cv::waitKey(0);
 284
 285   // Close and Destroy Window
 286   cv::destroyWindow("MODEL REGISTRATION");
 287
 288   std::cout << "GOODBYE" << std::endl;
 289
 290 }