Add mid-optimization normal filtering to MBT

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

@@ -1,6 +1,6 @@
 /*
  * ViSP, open source Visual Servoing Platform software.
- * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
+ * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
  *
  * This software is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -31,8 +31,8 @@
  * Model-based tracker using depth normal features.
  */
 
-#ifndef _vpMbDepthNormalTracker_h_
-#define _vpMbDepthNormalTracker_h_
+#ifndef VP_MB_DEPTH_NORMAL_TRACKER_H
+#define VP_MB_DEPTH_NORMAL_TRACKER_H
 
 #include <visp3/core/vpConfig.h>
 #include <visp3/core/vpPlane.h>

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

@@ -133,6 +133,8 @@ class VISP_EXPORT vpMbtFaceDepthNormal
   void computeVisibility();
   void computeVisibilityDisplay();
 
+  bool planeIsInvalid(const vpHomogeneousMatrix &cMo, double maxAngle);
+
   void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
                                vpColVector &face_normal);
 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)

modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp

@@ -267,12 +267,21 @@ void vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu()
        it != m_depthNormalListOfActiveFaces.end(); ++it) {
     vpMatrix L_face;
     vpColVector features_face;
+
     (*it)->computeInteractionMatrix(m_cMo, L_face, features_face);
 
     vpColVector face_error = features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt];
 
-    m_error_depthNormal.insert(cpt * 3, face_error);
-    m_L_depthNormal.insert(L_face, cpt * 3, 0);
+    if (!(*it)->planeIsInvalid(m_cMo, angleDisappears)) {
+      m_error_depthNormal.insert(cpt * 3, face_error);
+      m_L_depthNormal.insert(L_face, cpt * 3, 0);
+    }
+    else {
+      face_error = 0;
+      L_face = 0;
+      m_error_depthNormal.insert(cpt * 3, face_error);
+      m_L_depthNormal.insert(L_face, cpt * 3, 0);
+    }
 
     cpt++;
   }

modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp

@@ -1038,6 +1038,32 @@ void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double
   }
 }
 
+/**
+ * Returns true when the plane is nearly parallalel to the optical axis and close to the optical center.
+ * In this case, the interaction matrix related to this face may "explode" leading to a tracking failure.
+ */
+bool vpMbtFaceDepthNormal::planeIsInvalid(const vpHomogeneousMatrix &cMo, double maxAngle)
+{
+  m_planeCamera = m_planeObject;
+  m_planeCamera.changeFrame(cMo);
+  const vpTranslationVector t = cMo.getTranslationVector();
+  // const double D = -(t[0] * m_planeCamera.getA() + t[1] * m_planeCamera.getB() + t[2] * m_planeCamera.getC());
+  const double D = m_planeCamera.getD();
+  vpPoint centroid;
+  std::vector<vpPoint> polyPts;
+  m_polygon->getPolygonClipped(polyPts);
+  computePolygonCentroid(polyPts, centroid);
+  centroid.changeFrame(cMo);
+  centroid.project();
+  vpColVector c(3);
+  c[0] = centroid.get_X();
+  c[1] = centroid.get_Y();
+  c[2] = centroid.get_Z();
+  const double L = c.frobeniusNorm();
+  const double minD = L * cos(maxAngle);
+  return fabs(D) <= minD;
+}
+
 void vpMbtFaceDepthNormal::computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features)
 {
   L.resize(3, 6, false, false);

tutorial/tracking/model-based/generic-rgbd-blender/model/teabox/teabox_depth.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <conf>
   <depth_normal>
-    <feature_estimation_method>2</feature_estimation_method>
+    <feature_estimation_method>0</feature_estimation_method>
     <PCL_plane_estimation>
       <method>2</method>
       <ransac_max_iter>200</ransac_max_iter>

tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp

@@ -410,7 +410,7 @@ int main(int argc, const char **argv)
         {
           std::stringstream ss;
           ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt()
-            << ", depth " << tracker.getNbFeaturesDepthDense();
+            << ", dense depth " << tracker.getNbFeaturesDepthDense() << ", depth normals " << tracker.getNbFeaturesDepthNormal();
           vpDisplay::displayText(I, I.getHeight() - 30, 20, ss.str(), vpColor::red);
         }
       }