Fixed indexing issue in python bindings, Silhouette ccd tracker inver…

…sion issue protection
lagadic · Oct 1, 2024 · 8d9c56a · 8d9c56a
1 parent 26469c7
commit 8d9c56a
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 7 deletions.
diff --git a/modules/python/bindings/include/core/images.hpp b/modules/python/bindings/include/core/images.hpp
@@ -57,7 +57,7 @@ void define_get_item_2d_image(py::class_<vpImage<T>, std::shared_ptr<vpImage<T>>
 {
   pyClass.def("__getitem__", [](const vpImage<T> &self, std::pair<int, int> pair) -> T {
     int i = pair.first, j = pair.second;
-    const int rows = (int)self.getHeight(), cols = (int)self.getRows();
+    const int rows = (int)self.getRows(), cols = (int)self.getCols();
     if (i >= rows || j >= cols || i < -rows || j < -cols) {
       std::stringstream ss;
       ss << "Invalid indexing into a 2D image: got indices " << shape_to_string({ i, j })
@@ -92,6 +92,14 @@ void define_get_item_2d_image(py::class_<vpImage<T>, std::shared_ptr<vpImage<T>>
     return (py::cast(self).template cast<np_array_cf<NpRep> >())[tuple].template cast<py::array_t<NpRep>>();
   }, py::keep_alive<0, 1>());
 }
+/*
+ * Image 2D indexing
+ */
+template<typename T, typename NpRep>
+void define_set_item_2d_image(py::class_<vpImage<T>, std::shared_ptr<vpImage<T>>> &pyClass)
+{
+
+}
 
 /*
  * vpImage
@@ -121,6 +129,7 @@ Construct an image by **copying** a 2D numpy array.
 )doc", py::arg("np_array"));
 
   define_get_item_2d_image<T, T>(pyImage);
+  define_set_item_2d_image<T, T>(pyImage);
 
   pyImage.def("__repr__", [](const vpImage<T> &self) -> std::string {
     std::stringstream ss;

diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp
@@ -94,8 +94,12 @@ void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &fr
     // std::cout << sp.j << ", " << sp.i << std::endl;
     int ii = sp.i, jj = sp.j;
 
+    if (ii < 4 || jj < 4 || static_cast<unsigned int>(ii) > frame.I.getHeight() - 4 || static_cast<unsigned int>(jj) > frame.I.getWidth() - 4) {
+      continue;
+    }
     vpRBSilhouetteControlPoint pccd;
     pccd.buildSilhouettePoint(ii, jj, sp.Z, sp.orientation, sp.normal, cMo, oMc, frame.cam);
+
     pccd.detectSilhouette(frame.renders.depth);
     if (pccd.isSilhouette() && !std::isnan(sp.orientation) && pccd.isValid()) {
       m_controlPoints.push_back(std::move(pccd));
@@ -373,7 +377,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage<vpRGBa> &I,
       normalized_param[kk][1] += vic_ptr[10 * negative_normal + 7];
     }
 
-    }
+  }
 #ifdef VISP_HAVE_OPENMP
 #pragma omp parallel for
 #endif
@@ -489,7 +493,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage<vpRGBa> &I,
     }
 
   }
-  }
+}
 
 void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
 {
@@ -647,10 +651,14 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
 
   m_LTL = m_hessian;
   m_LTR = -m_gradient;
-
-  vpMatrix hessian_E_inv = m_hessian.inverseByCholesky();
-  //m_sigma = /*m_sigma +*/ 2*hessian_E_inv;
-  m_sigma = m_ccdParameters.covarianceIterDecreaseFactor * m_sigma + 2.0 * (1.0 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv;
+  try {
+    vpMatrix hessian_E_inv = m_hessian.inverseByCholesky();
+    //m_sigma = /*m_sigma +*/ 2*hessian_E_inv;
+    m_sigma = m_ccdParameters.covarianceIterDecreaseFactor * m_sigma + 2.0 * (1.0 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv;
+  }
+  catch (vpException &e) {
+    std::cerr << "Inversion issues in CCD tracker" << std::endl;
+  }
 
 }