From d0617111452639d7fffdac92a5524ba07d558929 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 24 Jul 2024 22:55:42 +0200 Subject: [PATCH 1/6] Add degenerate base condition, needs further refinement --- .../visp3/mbt/vpMbDepthNormalTracker.h | 1 + .../include/visp3/mbt/vpMbtFaceDepthNormal.h | 2 ++ .../mbt/src/depth/vpMbDepthNormalTracker.cpp | 20 +++++++++++++++++-- .../mbt/src/depth/vpMbtFaceDepthNormal.cpp | 15 ++++++++++++++ .../model/teabox/teabox_depth.xml | 2 +- 5 files changed, 37 insertions(+), 3 deletions(-) diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h index c3eb11ae3e..079c269616 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h @@ -120,6 +120,7 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker #endif virtual void track(const std::vector &point_cloud, unsigned int width, unsigned int height); + protected: //! Method to estimate the desired features vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h index f641100151..78e8cf72e5 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h @@ -133,6 +133,8 @@ class VISP_EXPORT vpMbtFaceDepthNormal void computeVisibility(); void computeVisibilityDisplay(); + bool planeIsDegenerate(const vpHomogeneousMatrix &cMo); + void computeNormalVisibility(double nx, double ny, double nz, const vpColVector ¢roid_point, vpColVector &face_normal); #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) diff --git a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp index 9f6612e75b..824dfa4846 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp @@ -267,12 +267,28 @@ 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); + 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)) { + 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); + m_L_depthNormal.insert(L_face, cpt * 3, 0); + } cpt++; } diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp index 4c9e75caec..4dfa6bc9b9 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp @@ -729,6 +729,9 @@ 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); @@ -1038,6 +1041,17 @@ void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double } } +bool vpMbtFaceDepthNormal::planeIsDegenerate(const vpHomogeneousMatrix &cMo) +{ + 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; +} + void vpMbtFaceDepthNormal::computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features) { L.resize(3, 6, false, false); @@ -1057,6 +1071,7 @@ 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; diff --git a/tutorial/tracking/model-based/generic-rgbd-blender/model/teabox/teabox_depth.xml b/tutorial/tracking/model-based/generic-rgbd-blender/model/teabox/teabox_depth.xml index 19bd860683..49808f5f4c 100644 --- a/tutorial/tracking/model-based/generic-rgbd-blender/model/teabox/teabox_depth.xml +++ b/tutorial/tracking/model-based/generic-rgbd-blender/model/teabox/teabox_depth.xml @@ -1,7 +1,7 @@ - 2 + 0 2 200 From 91ff15c603c98965270c6b30d6585d11208ac9e0 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 13 Sep 2024 14:00:28 +0200 Subject: [PATCH 2/6] Determine D value threshold automatically from the angleDisappears, remove debug prints --- .../include/visp3/mbt/vpMbtFaceDepthNormal.h | 2 +- .../mbt/src/depth/vpMbDepthNormalTracker.cpp | 9 +------- .../mbt/src/depth/vpMbtFaceDepthNormal.cpp | 22 +++++++++++++------ 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h index 78e8cf72e5..dbe83e219b 100644 --- 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 ¢roid_point, vpColVector &face_normal); diff --git a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp index 824dfa4846..0f50af016a 100644 --- 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 index 4dfa6bc9b9..f7ea62722b 100644 --- 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 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; From 5544d270ace4e5df8e186bc34ad2b6a2fdd8d3f2 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 13 Sep 2024 14:00:44 +0200 Subject: [PATCH 3/6] Show number of normal features in blender tutorial --- .../tutorial-mb-generic-tracker-rgbd-blender.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp b/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp index 30b67c045c..683cd22e7c 100644 --- a/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp +++ b/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); } } From 495c363c90db62238f9eeae2355dfb40669f9ed9 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Sun, 15 Sep 2024 21:29:04 +0200 Subject: [PATCH 4/6] Fix issue with c++98 compat for vector instanciation --- modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp index f7ea62722b..9e46056465 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp @@ -1055,7 +1055,7 @@ bool vpMbtFaceDepthNormal::planeIsInvalid(const vpHomogeneousMatrix &cMo, double computePolygonCentroid(polyPts, centroid); centroid.changeFrame(cMo); centroid.project(); - const vpColVector c { centroid.get_X(), centroid.get_Y(), centroid.get_Z() }; + 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; From 9db743194088836e9e8a7ab9b2c55122b79e91d0 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Sun, 15 Sep 2024 22:59:05 +0200 Subject: [PATCH 5/6] Second fix for col vector c++98 compatibility --- modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp index 9e46056465..9be38b48b4 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp @@ -1055,7 +1055,10 @@ bool vpMbtFaceDepthNormal::planeIsInvalid(const vpHomogeneousMatrix &cMo, double computePolygonCentroid(polyPts, centroid); centroid.changeFrame(cMo); centroid.project(); - const vpColVector c({ centroid.get_X(), centroid.get_Y(), centroid.get_Z() }); + 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; From 0fa864a6526ab108222d155118e9850b8662990a Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 16 Sep 2024 11:35:25 +0200 Subject: [PATCH 6/6] Update header --- .../tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h index 079c269616..f66d0f7bf7 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h +++ b/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 #include @@ -120,7 +120,6 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker #endif virtual void track(const std::vector &point_cloud, unsigned int width, unsigned int height); - protected: //! Method to estimate the desired features vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod;