trial with cylinder of 8 vertexes each

flann_based/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/CsvReader.cpp

@@ -45,10 +45,14 @@ void CsvReader::readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list
              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);
+            //  tmp_p.x = (float)StringToInt(x);
+            //  tmp_p.y = (float)StringToInt(y);
+            //  tmp_p.z = (float)StringToInt(z);
+             tmp_p.x = std::stod(x);
+             tmp_p.y = std::stod(y);
+             tmp_p.z = std::stod(z);
              list_vertex.push_back(tmp_p);
+             std::cout << "x: " << tmp_p.x << " y: " << tmp_p.y << " z: " << tmp_p.z << std::endl;
 
              count++;
              if(count == num_vertex)

flann_based/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Mesh.cpp

@@ -75,4 +75,5 @@ void Mesh::load(const std::string& path)
     // Update mesh attributes
     num_vertices_ = (int)list_vertex_.size();
     num_triangles_ = (int)list_triangles_.size();
+    std::cout << "num_vertices_: " << num_vertices_ << std::endl;
 }

flann_based/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp

@@ -31,9 +31,9 @@ const int radius = 4;
 // Draw a text with the question point
 void drawQuestion(cv::Mat image, cv::Point3f point, cv::Scalar color)
 {
-    std::string x = IntToString((int)point.x);
-    std::string y = IntToString((int)point.y);
-    std::string z = IntToString((int)point.z);
+    std::string x = std::to_string(point.x);
+    std::string y = std::to_string(point.y);
+    std::string z = std::to_string(point.z);
 
     std::string text = " Where is point (" + x + ","  + y + "," + z + ") ?";
     cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8);
@@ -71,7 +71,8 @@ void drawCounter(cv::Mat image, int n, int n_max, cv::Scalar color)
 {
     std::string n_str = IntToString(n);
     std::string n_max_str = IntToString(n_max);
-    std::string text = n_str + " of " + n_max_str + " points";
+    // std::string text = n_str + " of " + n_max_str + " points";
+    std::string text = "";
     cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8);
 }
 

flann_based/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp

@@ -62,14 +62,23 @@ int main(int argc, char *argv[])
     // samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply");         // mesh
 
     // Intrinsic camera parameters: UVC WEBCAM
-    double f = 55;                           // focal length in mm
-    double sx = 22.3, sy = 14.9;             // sensor size
-    double width = 640, height = 480;        // image size
+    // double f = 55;                           // focal length in mm
+    // double sx = 22.3, sy = 14.9;             // sensor size
+    // double width = 640, height = 480;        // image size
 
-    double params_WEBCAM[] = { width*f/sx,   // fx
-                               height*f/sy,  // fy
-                               width/2,      // cx
-                               height/2};    // cy
+    // double params_WEBCAM[] = { width*f/sx,   // fx
+    //                            height*f/sy,  // fy
+    //                            width/2,      // cx
+    //                            height/2};    // cy
+
+    // Intrinsic camera parameters: Greenhouse Cameras
+    const double f = 45; // focal length in mm
+    const double sx = 22.3, sy = 14.9;
+    const double width = 1280, height = 720;
+    const double params_WEBCAM[] = { 643.008 ,   // fx
+                                642.201,  // fy
+                                648.812 ,      // cx
+                                360.262};    // cy
 
     // Some basic colors
     Scalar red(0, 0, 255);
@@ -293,6 +302,7 @@ int main(int argc, char *argv[])
 
             // -- Step 6: Set estimated projection matrix
             pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
+            std::cout << "Current Pose!! x: " << translation_estimated.at<double>(0,0) << " y: " << translation_estimated.at<double>(1,0) << " z: " << translation_estimated.at<double>(2,0) << std::endl;
         }
 
         // -- Step X: Draw pose and coordinate frame

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

@@ -20,8 +20,16 @@ using namespace std;
 
 // Boolean the know if the registration it's done
 bool end_registration = false;
-
 // Intrinsic camera parameters: UVC WEBCAM
+// const double f = 45; // focal length in mm
+// const double sx = 22.3, sy = 14.9;
+// const double width = 2592, height = 1944;
+// const double params_CANON[] = { width*f/sx,   // fx
+//                                 height*f/sy,  // fy
+//                                 width/2,      // cx
+//                                 height/2};    // cy
+
+// Intrinsic camera parameters: Greenhouse Cameras
 const double f = 45; // focal length in mm
 const double sx = 22.3, sy = 14.9;
 const double width = 1280, height = 720;
@@ -30,10 +38,16 @@ const double params_CANON[] = { 643.008 ,   // fx
                                 648.812 ,      // cx
                                 360.262};    // cy
 
+
 // Setup the points to register in the image
 // In the order of the *.ply file and starting at 1
-const int n = 8;
-const int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
+// const int n = 8;
+// const int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4
+
+// Setup the points to register in the image
+// In the order of the *.ply file and starting at 1
+const int n = 16;
+const int pts[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; // 3 -> 4
 
 /*
  * CREATE MODEL REGISTRATION OBJECT

flann_based/sq_2.yml

@@ -0,0 +1,75 @@
+%YAML:1.0
+---
+points_3d: !!opencv-matrix
+   rows: 9
+   cols: 1
+   dt: "3f"
+   data: [ 5.56015968e-03, 1.42522305e-02, -2.98023224e-08,
+       3.35794687e-03, 1.43547952e-02, -2.98023224e-08, 4.15065885e-03,
+       1.42303258e-02, 0., 0., 4.48415509e-44, 0., 1.32452548e-02,
+       1.17547214e-02, 9.76203918e-01, -1.85193717e-02, 5.06480634e-01,
+       3.08125377e-01, 1.08953416e-02, 1.41046643e-02, 9.83510494e-01,
+       -2.08262205e-02, 5.04173756e-01, 3.06661725e-01, -2.39754319e-02,
+       5.01024604e-01, 1.64968729e-01 ]
+points_2d: !!opencv-matrix
+   rows: 9
+   cols: 1
+   dt: "2f"
+   data: [ 288., 6.45119995e+02, 2.88576019e+02, 6.48000061e+02,
+       2.88230438e+02, 6.46963318e+02, 8.65935547e+02, 4.77757568e+02,
+       5.25035645e+02, 3.18505035e+02, 8.68921570e+02, 4.77757568e+02,
+       5.26727722e+02, 3.18903198e+02, 8.67130005e+02, 4.80146362e+02,
+       9.17294556e+02, 5.51809998e+02 ]
+keypoints:
+   - [ 288., 6.4511999511718750e+02, 4.4640003204345703e+01,
+       2.0814511108398438e+02, 2.2853157133795321e-04, 2, -1 ]
+   - [ 2.8857601928710938e+02, 6.4800006103515625e+02,
+       5.3568004608154297e+01, 2.0699720764160156e+02,
+       2.5694066425785422e-04, 3, -1 ]
+   - [ 2.8823043823242188e+02, 6.4696331787109375e+02,
+       6.4281608581542969e+01, 2.0898962402343750e+02,
+       2.3757420422043651e-04, 4, -1 ]
+   - [ 8.6593554687500000e+02, 4.7775756835937500e+02,
+       7.7137939453125000e+01, 3.2710388183593750e+02,
+       2.0695610146503896e-04, 5, -1 ]
+   - [ 5.2503564453125000e+02, 3.1850503540039062e+02,
+       7.7137939453125000e+01, 1.6490226745605469e+01,
+       1.4561964781023562e-04, 5, -1 ]
+   - [ 8.6892156982421875e+02, 4.7775756835937500e+02,
+       9.2565528869628906e+01, 3.3250491333007812e+02,
+       1.7157701950054616e-04, 6, -1 ]
+   - [ 5.2672772216796875e+02, 3.1890319824218750e+02,
+       1.1107863616943359e+02, 4.8507041931152344e+01,
+       1.9299793348181993e-04, 7, -1 ]
+   - [ 8.6713000488281250e+02, 4.8014636230468750e+02,
+       1.1107863616943359e+02, 3.3898977661132812e+02,
+       1.3654734357260168e-04, 7, -1 ]
+   - [ 9.1729455566406250e+02, 5.5180999755859375e+02,
+       1.1107863616943359e+02, 3.0569372558593750e+02,
+       1.0311038204235956e-04, 7, -1 ]
+descriptors: !!opencv-matrix
+   rows: 9
+   cols: 32
+   dt: u
+   data: [ 59, 123, 112, 225, 229, 191, 248, 87, 15, 161, 190, 15, 143,
+       11, 86, 136, 238, 189, 223, 29, 156, 186, 81, 200, 215, 221, 154,
+       179, 143, 220, 219, 159, 186, 255, 112, 227, 245, 157, 254, 247,
+       13, 45, 254, 15, 175, 15, 22, 132, 238, 187, 159, 221, 222, 184,
+       80, 230, 255, 217, 142, 243, 39, 156, 216, 207, 42, 253, 114, 243,
+       53, 157, 252, 182, 173, 12, 190, 15, 174, 27, 118, 133, 238, 163,
+       223, 21, 238, 188, 80, 239, 255, 217, 158, 211, 6, 12, 217, 207,
+       9, 151, 175, 129, 244, 118, 189, 87, 60, 77, 222, 231, 156, 54,
+       162, 236, 109, 158, 191, 156, 149, 153, 49, 192, 255, 103, 190,
+       192, 138, 76, 165, 249, 90, 35, 19, 168, 59, 105, 25, 105, 173,
+       172, 139, 169, 206, 214, 104, 84, 246, 21, 237, 32, 16, 3, 163,
+       212, 144, 244, 43, 255, 34, 6, 37, 119, 19, 155, 167, 241, 246,
+       190, 60, 87, 53, 89, 222, 102, 185, 63, 180, 109, 230, 31, 63,
+       156, 147, 217, 33, 196, 127, 251, 246, 194, 166, 204, 181, 255,
+       67, 26, 205, 189, 114, 136, 105, 19, 62, 73, 82, 245, 189, 7, 146,
+       88, 6, 45, 63, 157, 135, 156, 197, 69, 196, 58, 186, 192, 44, 132,
+       1, 89, 3, 147, 63, 249, 182, 140, 104, 55, 63, 25, 82, 114, 176,
+       31, 180, 97, 230, 27, 63, 145, 211, 216, 67, 213, 12, 58, 254,
+       194, 148, 128, 241, 173, 92, 205, 106, 206, 157, 210, 254, 232,
+       196, 128, 207, 2, 107, 233, 61, 96, 120, 254, 139, 234, 72, 163,
+       28, 253, 251, 237, 143, 73, 51, 59, 124, 42 ]
+training_image_path: "/media/psf/freelancing/greenhouse/flann_based/stereo_image782.jpeg"

flann_based/sq_2_try.ply

@@ -0,0 +1,38 @@
+ply
+format ascii 1.0
+comment Created by Blender 3.4.1 - www.blender.org
+element vertex 16
+property float x
+property float y
+property float z
+element face 12
+property list uchar uint vertex_indices
+end_header
+0.000000 0.025000 0.000000
+0.000000 0.025000 1.000000
+0.025000 0.000000 1.000000
+0.025000 0.000000 0.000000
+0.000000 -0.025000 1.000000
+0.000000 -0.025000 0.000000
+-0.025000 0.000000 1.000000
+-0.025000 0.000000 0.000000
+0.000000 0.525000 0.000000
+0.000000 0.525000 1.000000
+0.025000 0.500000 1.000000
+0.025000 0.500000 0.000000
+0.000000 0.475000 1.000000
+0.000000 0.475000 0.000000
+-0.025000 0.500000 1.000000
+-0.025000 0.500000 0.000000
+4 0 1 2 3
+4 3 2 4 5
+4 2 1 6 4
+4 5 4 6 7
+4 7 6 1 0
+4 0 3 5 7
+4 8 9 10 11
+4 11 10 12 13
+4 10 9 14 12
+4 13 12 14 15
+4 15 14 9 8
+4 8 11 13 15