Determine D value threshold automatically from the angleDisappears, r…

…emove debug prints
lagadic · Sep 13, 2024 · 91ff15c · 91ff15c
1 parent d061711
commit 91ff15c
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 16 deletions.
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h
@@ -133,7 +133,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal
   void computeVisibility();
   void computeVisibilityDisplay();
 
-  bool planeIsDegenerate(const vpHomogeneousMatrix &cMo);
+  bool planeIsInvalid(const vpHomogeneousMatrix &cMo, double maxAngle);
 
   void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
                                vpColVector &face_normal);

diff --git a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp
@@ -272,18 +272,11 @@ void vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu()
 
     vpColVector face_error = features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt];
 
-    std::cout << "cpt = " << cpt << std::endl;
-    std::cout << "Current features = " << features_face.t() << std::endl;
-    std::cout << "Desired features = " << m_depthNormalListOfDesiredFeatures[(size_t)cpt].t() << std::endl;
-    std::cout << "error = " << face_error.t() << std::endl;
-
-    if (!(*it)->planeIsDegenerate(m_cMo)) {
+    if (!(*it)->planeIsInvalid(m_cMo, angleDisappears)) {
       m_error_depthNormal.insert(cpt * 3, face_error);
       m_L_depthNormal.insert(L_face, cpt * 3, 0);
     }
     else {
-      std::cout << "DEGENERATE PLANE" << std::endl;
-
       face_error = 0;
       L_face = 0;
       m_error_depthNormal.insert(cpt * 3, face_error);

diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp
@@ -729,9 +729,6 @@ void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures(const std::vecto
   centroid_point[0] /= den;
   centroid_point[1] /= den;
   centroid_point[2] /= den;
-  std::cout << "Centroid = " << centroid_point.t() << std::endl;
-  std::cout << "Desired features = " << desired_features.t() << std::endl;
-
 
   computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point,
                           desired_normal);
@@ -1041,15 +1038,27 @@ void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double
   }
 }
 
-bool vpMbtFaceDepthNormal::planeIsDegenerate(const vpHomogeneousMatrix &cMo)
+/**
+ * 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();
-  std::cout << "D = " << D << std::endl;
-  return fabs(D) < 5e-2;
+  vpPoint centroid;
+  std::vector<vpPoint> polyPts;
+  m_polygon->getPolygonClipped(polyPts);
+  computePolygonCentroid(polyPts, centroid);
+  centroid.changeFrame(cMo);
+  centroid.project();
+  const vpColVector c { centroid.get_X(), centroid.get_Y(), 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)
@@ -1071,7 +1080,6 @@ void vpMbtFaceDepthNormal::computeInteractionMatrix(const vpHomogeneousMatrix &c
   features[0] = -ux / D;
   features[1] = -uy / D;
   features[2] = -uz / D;
-  std::cout << "Current features = " << features.t() << std::endl;
 
   // L_A
   L[0][0] = ux * ux / D2;