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