code for tracking lines

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)
@@ -70,6 +74,7 @@ void CsvReader::readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list
              tmp_triangle[0] = StringToInt(id0);
              tmp_triangle[1] = StringToInt(id1);
              tmp_triangle[2] = StringToInt(id2);
+             std::cout << "id0: " << tmp_triangle[0] << " id1: " << tmp_triangle[1] << " id2: " << tmp_triangle[2] << std::endl;
              list_triangles.push_back(tmp_triangle);
 
              count++;

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

@@ -19,6 +19,9 @@
 #include <opencv2/xfeatures2d.hpp>
 #endif
 
+#include <opencv2/line_descriptor.hpp>
+using namespace cv::line_descriptor;
+
 // For text
 const int fontFace = cv::FONT_ITALIC;
 const double fontScale = 0.75;
@@ -31,9 +34,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 +74,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

@@ -47,7 +47,7 @@ int main(int argc, char *argv[])
             "{inliers in        |30    | minimum inliers for Kalman update                                  }"
             "{method  pnp       |0     | PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS - (5) AP3P}"
             "{fast f            |true  | use of robust fast match                                           }"
-            "{feature           |ORB   | feature name (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)  }"
+            "{feature           |ORB   | feature name (Line, ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)  }"
             "{FLANN             |false | use FLANN library for descriptors matching                         }"
             "{save              |      | path to the directory where to save the image results              }"
             "{displayFiltered   |false | display filtered pose (from Kalman filter)                         }"
@@ -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);
@@ -92,7 +101,7 @@ int main(int argc, char *argv[])
 
     // PnP parameters
     int pnpMethod = SOLVEPNP_ITERATIVE;
-    string featureName = "ORB";
+    string featureName = "Line";
     bool useFLANN = false;
 
     // Save results
@@ -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

@@ -13,15 +13,25 @@
 #include "ModelRegistration.h"
 #include "Utils.h"
 
+#include <opencv2/line_descriptor.hpp>
+
 using namespace cv;
 using namespace std;
-
+ using namespace cv::line_descriptor;
 /**  GLOBAL VARIABLES  **/
 
 // 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 +40,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
@@ -89,7 +105,7 @@ int main(int argc, char *argv[])
             "{model         |      | path to output yml model                                           }"
             "{mesh          |      | path to ply mesh                                                   }"
             "{keypoints k   |2000  | number of keypoints to detect (only for ORB)                       }"
-            "{feature       |ORB   | feature name (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)  }"
+            "{feature       |ORB   | feature name (Line, ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)  }"
             ;
     CommandLineParser parser(argc, argv, keys);
 
@@ -129,7 +145,15 @@ int main(int argc, char *argv[])
     //Instantiate robust matcher: detector, extractor, matcher
     RobustMatcher rmatcher;
     Ptr<Feature2D> detector, descriptor;
-    createFeatures(featureName, numKeyPoints, detector, descriptor);
+    Ptr<BinaryDescriptor> bd = BinaryDescriptor::createBinaryDescriptor();
+    if(featureName == "Line")
+    {
+        bd = BinaryDescriptor::createBinaryDescriptor();
+    }
+    else
+    {
+        createFeatures(featureName, numKeyPoints, detector, descriptor);
+    }
     rmatcher.setFeatureDetector(detector);
     rmatcher.setDescriptorExtractor(descriptor);
 
@@ -235,26 +259,110 @@ int main(int argc, char *argv[])
     vector<KeyPoint> keypoints_model;
     Mat descriptors;
 
-    // Compute keypoints and descriptors
-    rmatcher.computeKeyPoints(img_in, keypoints_model);
-    rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);
+    
+    /* compute lines */
+    std::vector<KeyLine> keylines;
 
-    // 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) {
-        Point2f point2d(keypoints_model[i].pt);
-        Point3f point3d;
-        bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
-        if (on_surface)
+    cv::Rect bbox(78, 152, 430, 550); // define the bounding box
+
+    cv::Mat roi = img_in(bbox); // extract the region of interest (ROI) inside the bounding box
+    imshow( "roi", roi );
+    waitKey();
+
+    if(featureName == "Line")
+    {
+        /* create a binary mask */
+        cv::Mat mask = Mat::ones( roi.size(), CV_8UC1 );
+        
+        bd->detect( roi, keylines, mask );
+        
+        bd->compute( roi, keylines, descriptors);
+    
+        cv::Mat output = img_in.clone();
+        /* draw lines extracted from octave 0 */
+        if( output.channels() == 1 )
+            cvtColor( output, output, COLOR_GRAY2BGR );
+        for ( size_t i = 0; i < keylines.size(); i++ )
         {
-            model.add_correspondence(point2d, point3d);
-            model.add_descriptor(descriptors.row(i));
-            model.add_keypoint(keypoints_model[i]);
+            KeyLine kl = keylines[i];
+            std::cout << "keyline length: " << kl.lineLength << std::endl;
+            if( kl.octave == 0 && kl.lineLength > 100)
+            {
+                /* get a random color */
+                int R = ( rand() % (int) ( 255 + 1 ) );
+                int G = ( rand() % (int) ( 255 + 1 ) );
+                int B = ( rand() % (int) ( 255 + 1 ) );
+            
+                /* get extremes of line */
+                Point pt1 = Point2f( kl.startPointX + bbox.x, kl.startPointY + bbox.y);
+                Point pt2 = Point2f( kl.endPointX + bbox.x, kl.endPointY + bbox.y);
+            
+                /* draw line */
+                line( output, pt1, pt2, Scalar( B, G, R ), 3 );
+            }
+        
         }
-        else
-        {
-            model.add_outlier(point2d);
+        
+        /* show lines on image */
+        imshow( "LSD lines", output );
+        imwrite( "lsd_lines.jpeg", output);
+        waitKey();
+        // Check if keypoints are on the surface of the registration image and add to the model
+        for (unsigned int i = 0; i < keylines.size(); ++i) {
+            KeyLine kl = keylines[i];
+            if( kl.octave == 0 && kl.lineLength > 100)
+            {
+                /* get extremes of line */
+                Point pt1 = Point2f( kl.startPointX + bbox.x, kl.startPointY + bbox.y);
+                Point pt2 = Point2f( kl.endPointX + bbox.x, kl.endPointY + bbox.y);
+                std::vector<Point2f> point2d;
+                // point2d.push_back(keylines[i].pt); // mid point of the line
+                point2d.push_back(pt1);
+                point2d.push_back(pt2);
+
+                for (unsigned int j = 0; j < point2d.size(); j++)
+                {
+                    Point3f point3d;
+                    bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d[j], point3d);
+                    if (on_surface)
+                    {
+                        model.add_correspondence(point2d[j], point3d);
+                        model.add_descriptor(descriptors.row(i));
+                        KeyPoint mid_point;
+                        mid_point.pt = point2d[j];
+                        model.add_keypoint(mid_point);
+                    }
+                    else
+                    {
+                        model.add_outlier(point2d[j]);
+                    }
+                }
+            }
         }
     }
+    else
+    {
+        // Compute keypoints and descriptors
+        rmatcher.computeKeyPoints(roi, keypoints_model);
+        rmatcher.computeDescriptors(roi, 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) {
+            Point2f point2d(keypoints_model[i].pt);
+            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);
+            }
+        }
+
+    }
 
     model.set_trainingImagePath(img_path);
     // save the model into a *.yaml file

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