Fix interaction matrix for depth, Interaction matrix for 3D point error

Author: SamFlt

modules/tracker/rbt/include/visp3/rbt/vpPointMap.h

@@ -80,6 +80,43 @@ class VISP_EXPORT vpPointMap
     }
   }
 
+  void compute3DErrorAndJacobian(const vpArray2D<int> &indices, const vpHomogeneousMatrix &cTw, const vpMatrix &observations, vpMatrix &J, vpColVector &e) const
+  {
+    J.resize(indices.getRows() * 3, 6, true, false);
+    e.resize(indices.getRows() * 3, 1, false);
+
+    vpColVector cX(3);
+    vpColVector wX(3);
+    const vpRotationMatrix cRw = cTw.getRotationMatrix();
+    const vpTranslationVector t = cTw.getTranslationVector();
+    for (unsigned int i = 0; i < indices.getRows(); ++i) {
+      const unsigned int pointIndex = indices[i][0];
+      const double *p = m_X[pointIndex];
+      wX[0] = p[0]; wX[1] = p[1]; wX[2] = p[2];
+      cX = cRw * wX;
+      cX += t;
+
+      const double X = cX[0], Y = cX[1], Z = cX[2];
+
+      e[i * 3] = X - observations[i][0];
+      e[i * 3 + 1] = Y - observations[i][1];
+      e[i * 3 + 2] = Z - observations[i][2];
+
+      J[i * 3][0] = -1;
+      J[i * 3 + 1][1] = -1;
+      J[i * 3 + 2][2] = -1;
+
+      J[i * 3][4] = -Z;
+      J[i * 3][5] = Y;
+
+      J[i * 3 + 1][3] = Z;
+      J[i * 3 + 1][5] = -X;
+
+      J[i * 3 + 2][3] = -Y;
+      J[i * 3 + 2][4] = X;
+    }
+  }
+
 private:
   vpMatrix m_X; // N x 3, points expressed in world frame
   unsigned m_maxPoints;

modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h

@@ -154,7 +154,7 @@ class VISP_EXPORT vpRBDenseDepthTracker : public vpRBFeatureTracker
 
     inline void interaction(vpMatrix &L, unsigned i)
     {
-      const double X = currentPoint[0], Y = currentPoint[1], Z = currentPoint[2];
+      const double X = oP.get_X(), Y = oP.get_Y(), Z = oP.get_Z();
       const double nx = cameraNormal[0], ny = cameraNormal[1], nz = cameraNormal[2];
       L[i][0] = nx;
       L[i][1] = ny;

modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp

@@ -60,13 +60,13 @@ void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame
   const vpHomogeneousMatrix &cMo = frame.renders.cMo;
   vpHomogeneousMatrix oMc = cMo.inverse();
   vpRotationMatrix cRo = cMo.getRotationMatrix();
+  vpTranslationVector co = oMc.getTranslationVector();
   bool useMask = m_useMask && frame.hasMask();
   m_depthPoints.clear();
   m_depthPoints.reserve(static_cast<size_t>(bb.getArea() / (m_step * m_step * 2)));
   vpDepthPoint point;
   for (unsigned int i = static_cast<unsigned int>(bb.getTop()); i < static_cast<unsigned int>(bb.getBottom()); i += m_step) {
     for (unsigned int j = static_cast<unsigned int>(bb.getLeft()); j < static_cast<unsigned int>(bb.getRight()); j += m_step) {
-      // if (renderDepth[i][j] > frame.renders.zNear && renderDepth[i][j] < frame.renders.zFar && depthMap[i][j] > frame.renders.zNear * 0.33 && depthMap[i][j] < frame.renders.zFar * 3.0) {
       double Z = renderDepth[i][j];
       double currZ = depthMap[i][j];
       if (Z > 0.f && currZ > 0.f) {
@@ -80,14 +80,16 @@ void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame
         point.objectNormal[1] = frame.renders.normals[i][j].G;
         point.objectNormal[2] = frame.renders.normals[i][j].B;
 
-        //fastRotationMatmul(cRo, frame.renders.normals[i][j], point.objectNormal);
-        //vpColVector cameraNormal = cRo * objectNormal;
-        // if (acos(cameraNormal * vpColVector({ 0.0, 0.0, -1.0 })) > vpMath::rad(70.0)) {
-        //   continue;
-        // }
+        fastProjection(oMc, x * Z, y * Z, Z, point.oP);
+
+        vpColVector cameraRay({ co[0] - point.oP.get_oX(), co[1] - point.oP.get_oY(), co[2] - point.oP.get_oZ() });
+        cameraRay.normalize();
+
+        if (acos(cameraRay * point.objectNormal) > vpMath::rad(85.0)) {
+          continue;
+        }
         // vpColVector cp({ x * Z, y * Z, Z, 1 });
         // vpColVector oP = oMc * cp;
-        fastProjection(oMc, x * Z, y * Z, Z, point.oP);
         // point.oP = vpPoint(oP);
         point.pixelPos.set_ij(i, j);
         point.currentPoint[0] = x * currZ;
@@ -136,7 +138,7 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*fram
   }
 
   //m_weights = 0.0;
-  //m_robust.setMinMedianAbsoluteDeviation(1e-3);
+  m_robust.setMinMedianAbsoluteDeviation(1e-3);
   m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights);
   for (unsigned int i = 0; i < m_depthPoints.size(); ++i) {
     m_weighted_error[i] = m_error[i] * m_weights[i];