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