Add vpMatrix version of MBT tracking, modify mbt wrapper to use it

modules/python/bindings/include/mbt.hpp

@@ -13,7 +13,7 @@ void bindings_vpMbGenericTracker(py::class_<vpMbGenericTracker, vpMbTracker> &py
   pyMbGenericTracker.def("track", [](vpMbGenericTracker &self, std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      std::map<std::string, py::array_t<double, py::array::c_style>> &mapOfPointClouds) {
     std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
-    std::map<std::string, std::vector<vpColVector>> mapOfVectors;
+    std::map<std::string, vpMatrix> mapOfVectors;
     double t = vpTime::measureTimeMs();
     for (const auto &point_cloud_pair: mapOfPointClouds) {
 
@@ -27,20 +27,12 @@ void bindings_vpMbGenericTracker(py::class_<vpMbGenericTracker, vpMbTracker> &py
       const auto shape = buffer.shape;
       mapOfHeights[point_cloud_pair.first] = shape[0];
       mapOfWidths[point_cloud_pair.first] = shape[1];
-
-      std::vector<vpColVector> pc(shape[0] * shape[1], vpColVector(3));
+      vpMatrix pc(shape[0] * shape[1], 3);
       const double *data = point_cloud_pair.second.unchecked<3>().data(0, 0, 0);
-      for (ssize_t i = 0; i < shape[0]; ++i) {
-        for (ssize_t j = 0; j < shape[1]; ++j) {
-          size_t vec_idx = i * shape[1] + j;
-          size_t idx = i * shape[1] * 3 + j * 3;
-          memcpy(pc[vec_idx].data, data + idx, sizeof(double) * 3);
-        }
-      }
-
+      memcpy(pc.data, data, shape[0] * shape[1] * 3 * sizeof(double));
       mapOfVectors[point_cloud_pair.first] = std::move(pc);
     }
-    std::map<std::string, const std::vector<vpColVector> * > mapOfVectorPtrs;
+    std::map<std::string, const vpMatrix * > mapOfVectorPtrs;
     for (const auto &p: mapOfVectors) {
       mapOfVectorPtrs[p.first] = &(p.second);
     }

modules/python/config/imgproc.json

@@ -3,5 +3,24 @@
   "ignored_classes": [],
   "user_defined_headers": [],
   "classes": {},
+  "functions": [
+    {
+      "static": false,
+      "signature": "void findContours(const vpImage<unsigned char>&, vp::vpContour&, std::vector<std::vector<vpImagePoint>>&, const vp::vpContourRetrievalType&)",
+      "use_default_param_policy": false,
+      "param_is_input": [
+        true,
+        true,
+        false,
+        true
+      ],
+      "param_is_output": [
+        false,
+        false,
+        true,
+        false
+      ]
+    }
+  ],
   "enums": {}
 }

modules/python/config/klt.json

@@ -9,6 +9,23 @@
           "static": false,
           "signature": "void display(const vpImage<unsigned char>&, const vpColor&, unsigned int)",
           "custom_name": "displaySelf"
+        },
+        {
+          "static": false,
+          "signature": "void getFeature(const int&, long&, float&, float&)",
+          "use_default_param_policy": false,
+          "param_is_input": [
+            true,
+            false,
+            false,
+            false
+          ],
+          "param_is_output": [
+            false,
+            true,
+            true,
+            true
+          ]
         }
       ]
     }

modules/python/examples/synthetic_data_mbt.py

@@ -72,6 +72,7 @@ def read_data(exp_config: MBTConfig, cam_depth: CameraParameters | None, I: Imag
   use_depth = cam_depth is not None
   iteration = 1
   while True:
+    start_parse_time = time.time()
     color_filepath = exp_config.color_images_dir / color_format.format(iteration)
     if not color_filepath.exists():
       print(f'Could not find image {color_filepath}, is the sequence finished?')
@@ -82,12 +83,14 @@ def read_data(exp_config: MBTConfig, cam_depth: CameraParameters | None, I: Imag
     I_depth_raw = None
     point_cloud = None
     if use_depth:
+      t = time.time()
       depth_filepath = exp_config.depth_images_dir / depth_format.format(iteration)
       if not depth_filepath.exists():
         print(f'Could not find image {depth_filepath}')
         return
       I_depth_np = cv2.imread(str(depth_filepath), cv2.IMREAD_ANYCOLOR | cv2.IMREAD_ANYDEPTH)
       I_depth_np = I_depth_np[..., 0]
+      print(f'\tDepth load took {(time.time() - t) * 1000}ms')
       I_depth_raw = ImageUInt16(I_depth_np * 32767.5)
       if I_depth_np.size == 0:
         print('Could not successfully read the depth image')
@@ -102,13 +105,15 @@ def read_data(exp_config: MBTConfig, cam_depth: CameraParameters | None, I: Imag
       point_cloud[..., 0] = xs * Z
       point_cloud[..., 1] = ys * Z
       point_cloud[..., 2] = Z
-      print(f'Point_cloud took {time.time() - t}')
+      print(f'\tPoint_cloud took {(time.time() - t) * 1000}ms')
 
 
     cMo_ground_truth = HomogeneousMatrix()
     ground_truth_file = exp_config.ground_truth_dir / (exp_config.color_camera_name + '_{:04d}.txt'.format(iteration))
     cMo_ground_truth.load(str(ground_truth_file))
     iteration += 1
+    end_parse_time = time.time()
+    print(f'Data parsing took: {(end_parse_time - start_parse_time) * 1000}ms')
     yield FrameData(I, I_depth_raw, point_cloud, cMo_ground_truth)
 
 
@@ -190,7 +195,7 @@ def parse_camera_file(exp_config: MBTConfig, is_color: bool) -> CameraParameters
     tracker.initFromPose(I, frame_data.cMo_ground_truth)
   else:
     tracker.initClick(I, str(mbt_model.init_file))
-
+  start_time =  time.time()
   for frame_data in data_generator:
     if frame_data.I_depth is not None:
       ImageConvert.createDepthHistogram(frame_data.I_depth, I_depth)
@@ -208,7 +213,7 @@ def parse_camera_file(exp_config: MBTConfig, is_color: bool) -> CameraParameters
       }
       t = time.time()
       tracker.track(image_dict, {'Camera2': pc})
-      print(f'Tracking took {time.time() - t}s')
+      print(f'Tracking took {(time.time() - t) * 1000}ms')
     cMo = HomogeneousMatrix()
     tracker.getPose(cMo)
 
@@ -219,3 +224,5 @@ def parse_camera_file(exp_config: MBTConfig, is_color: bool) -> CameraParameters
       Display.flush(I_depth)
     if args.step_by_step:
       Display.getKeyboardEvent(I, blocking=True)
+  end_time = time.time()
+  print(f'total time = {end_time - start_time}')

modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h

@@ -168,5 +168,6 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker
   void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
 #endif
   void segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
+  void segmentPointCloud(const vpMatrix &point_cloud, unsigned int width, unsigned int height);
 };
 #endif

modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h

@@ -183,5 +183,6 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker
   void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
 #endif
   void segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
+  void segmentPointCloud(const vpMatrix &point_cloud, unsigned int width, unsigned int height);
 };
 #endif

modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h

@@ -606,6 +606,15 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker
     std::map<std::string, unsigned int> &mapOfPointCloudWidths,
     std::map<std::string, unsigned int> &mapOfPointCloudHeights);
 
+  virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+    std::map<std::string, const vpMatrix *> &mapOfPointClouds,
+    std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+    std::map<std::string, unsigned int> &mapOfPointCloudHeights);
+  virtual void track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
+    std::map<std::string, const vpMatrix *> &mapOfPointClouds,
+    std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+    std::map<std::string, unsigned int> &mapOfPointCloudHeights);
+
 protected:
   virtual void computeProjectionError();
 
@@ -642,6 +651,10 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker
     std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
     std::map<std::string, unsigned int> &mapOfPointCloudWidths,
     std::map<std::string, unsigned int> &mapOfPointCloudHeights);
+  virtual void preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+    std::map<std::string, const vpMatrix *> &mapOfPointClouds,
+    std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+    std::map<std::string, unsigned int> &mapOfPointCloudHeights);
 
 private:
   class TrackerWrapper : public vpMbEdgeTracker,
@@ -770,6 +783,9 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker
     virtual void preTracking(const vpImage<unsigned char> *const ptr_I = nullptr,
       const std::vector<vpColVector> *const point_cloud = nullptr,
       const unsigned int pointcloud_width = 0, const unsigned int pointcloud_height = 0);
+    virtual void preTracking(const vpImage<unsigned char> *const ptr_I = nullptr,
+      const vpMatrix *const point_cloud = nullptr,
+      const unsigned int pointcloud_width = 0, const unsigned int pointcloud_height = 0);
 
     virtual void reInitModel(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
       const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose = false,

modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h

@@ -53,7 +53,8 @@
 class VISP_EXPORT vpMbtFaceDepthDense
 {
 public:
-  enum vpDepthDenseFilteringType {
+  enum vpDepthDenseFilteringType
+  {
     NO_FILTERING = 0,                         ///< Face is used if visible
     DEPTH_OCCUPANCY_RATIO_FILTERING = 1 << 1, ///< Face is used if there is
                                               ///< enough depth information in
@@ -103,6 +104,14 @@ class VISP_EXPORT vpMbtFaceDepthDense
 #if DEBUG_DISPLAY_DEPTH_DENSE
                               ,
                               vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+                              ,
+                              const vpImage<bool> *mask = nullptr);
+  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
+                              const vpMatrix &point_cloud, unsigned int stepX, unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+                              ,
+                              vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
 #endif
                               ,
                               const vpImage<bool> *mask = nullptr);
@@ -146,7 +155,8 @@ class VISP_EXPORT vpMbtFaceDepthDense
   {
     if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
       std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
-    } else {
+    }
+    else {
       m_depthDenseFilteringOccupancyRatio = occupancyRatio;
     }
   }
@@ -168,7 +178,7 @@ class VISP_EXPORT vpMbtFaceDepthDense
     //! The second extremity clipped in the image frame
     vpImagePoint m_imPt2;
 
-    PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() {}
+    PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() { }
 
     PolygonLine(const PolygonLine &polyLine)
       : m_p1(nullptr), m_p2(nullptr), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)

modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h

@@ -53,12 +53,14 @@
 class VISP_EXPORT vpMbtFaceDepthNormal
 {
 public:
-  enum vpFaceCentroidType {
+  enum vpFaceCentroidType
+  {
     GEOMETRIC_CENTROID, ///< Compute the geometric centroid
     MEAN_CENTROID       ///< Compute the mean centroid
   };
 
-  enum vpFeatureEstimationType {
+  enum vpFeatureEstimationType
+  {
     ROBUST_FEATURE_ESTIMATION = 0,
     ROBUST_SVD_PLANE_ESTIMATION = 1,
 #ifdef VISP_HAVE_PCL
@@ -106,6 +108,15 @@ class VISP_EXPORT vpMbtFaceDepthNormal
 #if DEBUG_DISPLAY_DEPTH_NORMAL
                               ,
                               vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+                              ,
+                              const vpImage<bool> *mask = nullptr);
+  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
+                              const vpMatrix &point_cloud, vpColVector &desired_features,
+                              unsigned int stepX, unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+                              ,
+                              vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
 #endif
                               ,
                               const vpImage<bool> *mask = nullptr);
@@ -179,7 +190,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal
     //! The second extremity clipped in the image frame
     vpImagePoint m_imPt2;
 
-    PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() {}
+    PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() { }
 
     PolygonLine(const PolygonLine &polyLine)
       : m_p1(nullptr), m_p2(nullptr), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
@@ -211,7 +222,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal
   public:
     std::vector<T> data;
 
-    Mat33() : data(9) {}
+    Mat33() : data(9) { }
 
     inline T operator[](const size_t i) const { return data[i]; }
 
@@ -221,7 +232,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal
     {
       // determinant
       T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
-              data[2] * (data[3] * data[7] - data[4] * data[6]);
+        data[2] * (data[3] * data[7] - data[4] * data[6]);
       T invdet = 1 / det;
 
       Mat33<T> minv;
@@ -305,7 +316,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal
 #ifdef VISP_HAVE_NLOHMANN_JSON
 #include<nlohmann/json.hpp>
 #ifdef VISP_HAVE_PCL
-NLOHMANN_JSON_SERIALIZE_ENUM( vpMbtFaceDepthNormal::vpFeatureEstimationType, {
+NLOHMANN_JSON_SERIALIZE_ENUM(vpMbtFaceDepthNormal::vpFeatureEstimationType, {
     {vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION, "robust"},
     {vpMbtFaceDepthNormal::ROBUST_SVD_PLANE_ESTIMATION, "robustSVD"},
     {vpMbtFaceDepthNormal::PCL_PLANE_ESTIMATION, "pcl"}