Some more cleanup for python

lagadic · Sep 27, 2024 · 60d9a40 · 60d9a40
1 parent 027ca12
commit 60d9a40
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 9 deletions.
diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h
@@ -206,18 +206,18 @@ class VISP_EXPORT vpRBFeatureTracker
   }
 #endif
 
-protected:
-
   static void computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR);
   static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &b, const vpMatrix &W);
 
+protected:
+
+
   vpMatrix m_L; //! Error jacobian (In VS terms, the interaction matrix)
   vpMatrix m_LTL;  //! Left side of the Gauss newton minimization
   vpColVector m_LTR; //! Right side of the Gauss Newton minimization
   vpMatrix m_cov; //! Covariance matrix
   vpColVector m_covWeightDiag;
 
-
   vpColVector m_error; //! Raw VS Error vector
   vpColVector m_weighted_error; //! Weighted VS error
   vpColVector m_weights; //! Error weights

diff --git a/modules/tracker/rbt/src/core/vpRBInitializationHelper.cpp b/modules/tracker/rbt/src/core/vpRBInitializationHelper.cpp
@@ -113,7 +113,6 @@ void vpRBInitializationHelper::initClick(const vpImage<T> &I, const std::string
     vpDisplay::display(I);
     vpDisplay::flush(I);
 
-
     vpPose pose;
 
     pose.clearPoint();
@@ -255,8 +254,16 @@ void vpRBInitializationHelper::initClick(const vpImage<T> &I, const std::string
       vpDisplay::flush(I);
       vpDisplay::display(I);
 
-
-      pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, m_cMo);
+      std::cout << "Before optim: " << m_cam << std::endl;
+      if (!pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, m_cMo)) {
+        std::cout << "Pose computation from points failed!" << std::endl;
+        for (unsigned int i = 0; i < n3d; ++i) {
+          std::cout << "Point " << i << ": " << std::endl;
+          std::cout << " 3D: " << pose.getPoints()[i].get_oP().t() << std::endl;
+          std::cout << "2D: " << pose.getPoints()[i].get_x() << ", " << pose.getPoints()[i].get_y() << std::endl;
+        }
+      }
+      std::cout << "POSE after optim: " << m_cMo << std::endl;
 
       vpDisplay::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
 
@@ -290,8 +297,6 @@ void vpRBInitializationHelper::initClick(const vpImage<T> &I, const std::string
     }
   }
 
-  std::cout << "cMo : " << std::endl << m_cMo << std::endl;
-
 }
 template void vpRBInitializationHelper::initClick<unsigned char>(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp);
 template void vpRBInitializationHelper::initClick<vpRGBa>(const vpImage<vpRGBa> &I, const std::string &initFile, bool displayHelp);

diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp
@@ -56,7 +56,6 @@ void fastProjection(const vpHomogeneousMatrix &oTc, double X, double Y, double Z
 
 void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &cMo)
 {
-  double t1 = vpTime::measureTimeMs();
   const vpImage<float> &depthMap = frame.depth;
   const vpImage<float> &renderDepth = frame.renders.depth;
   vpRect bb = frame.renders.boundingBox;