Merge branch 'module_rbt' into module_rbt_fs

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

@@ -154,7 +154,7 @@ class VISP_EXPORT vpRBFeatureTracker
    *
    * The updateCovariance method should have been called before
    */
-  const vpMatrix &getCovariance() const { return m_cov; }
+  virtual const vpMatrix getCovariance() const { return m_cov; }
   /**
    * \brief Update the covariance matrix
    *
@@ -179,14 +179,14 @@ class VISP_EXPORT vpRBFeatureTracker
   void setTrackerWeight(double weight) { m_userVvsWeight = weight; }
 
   /**
-   * \brief Get the left side term of the Gauss-Newton optimization term
+   * \brief Get the left-side term of the Gauss-Newton optimization term
    */
-  const vpMatrix &getLTL() const { return m_LTL; }
+  virtual vpMatrix getLTL() const { return m_LTL; }
 
   /**
-   * \brief Get the right side term of the Gauss-Newton optimization term
+   * \brief Get the right-side term of the Gauss-Newton optimization term
    */
-  const vpColVector &getLTR() const { return m_LTR; }
+  virtual vpColVector getLTR() const { return m_LTR; }
 
   /**
    * \brief Get a weighted version of the error vector.
@@ -204,7 +204,7 @@ class VISP_EXPORT vpRBFeatureTracker
 protected:
 
   static void computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR);
-  static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector & /*x*/, const vpColVector &b, const vpMatrix &W);
+  static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &b, const vpMatrix &W);
 
   vpMatrix m_L; //! Error jacobian (In VS terms, the interaction matrix)
   vpMatrix m_LTL;  //! Left side of the Gauss newton minimization

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

@@ -226,7 +226,10 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker
 
   void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE;
   void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE;
-  void updateCovariance(const double /*lambda*/) VP_OVERRIDE { }
+  void updateCovariance(const double /*lambda*/) VP_OVERRIDE
+  {
+    m_cov = Sigma_Phi;
+  }
 
   void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpImage<vpRGBa> &IRGB, const vpImage<unsigned char> &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE;
 

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

@@ -82,6 +82,8 @@ class VISP_EXPORT vpRBTracker
   const vpRBFeatureTrackerInput &getMostRecentFrame() const { return m_currentFrame; }
   const vpRBTrackerLogger &getLogger() const { return m_logger; }
 
+  vpMatrix getCovariance() const;
+
   /**
    * @}
    */
@@ -190,11 +192,6 @@ class VISP_EXPORT vpRBTracker
     const vpImage<vpRGBf> &Ior, const vpImage<unsigned char> &Ivalid,
     const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp);
 
-  vpMatrix getCovariance()
-  {
-    throw vpException(vpException::notImplementedError);
-  }
-
   template<typename T>
   void checkDimensionsOrThrow(const vpImage<T> &I, const std::string &imgType) const
   {

modules/tracker/rbt/src/core/vpRBTracker.cpp

@@ -83,6 +83,29 @@ void vpRBTracker::setPose(const vpHomogeneousMatrix &cMo)
   m_renderer.setCameraPose(cMo.inverse());
 }
 
+vpMatrix vpRBTracker::getCovariance() const
+{
+  vpMatrix sumInvs(6, 6, 0.0);
+  double sumWeights = 0.0;
+  for (const std::shared_ptr<vpRBFeatureTracker> &tracker: m_trackers) {
+    if (tracker->getNumFeatures() == 0) {
+      continue;
+    }
+    tracker->updateCovariance(m_lambda);
+    vpMatrix trackerCov = tracker->getCovariance();
+    double trackerWeight = tracker->getVVSTrackerWeight();
+    if (trackerCov.getRows() != 6 || trackerCov.getCols() != 6) {
+      throw vpException(vpException::dimensionError,
+      "Expected tracker pose covariance to have dimensions 6x6, but got %dx%d",
+      trackerCov.getRows(), trackerCov.getCols());
+    }
+
+    sumInvs += (trackerWeight * trackerCov.pseudoInverse());
+    sumWeights += trackerWeight;
+  }
+  return sumWeights * sumInvs.pseudoInverse();
+}
+
 vpCameraParameters vpRBTracker::getCameraParameters() const { return m_cam; }
 
 void vpRBTracker::setCameraParameters(const vpCameraParameters &cam, unsigned h, unsigned w)

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

@@ -106,12 +106,12 @@ void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame
     m_weighted_error.resize(m_depthPoints.size(), false);
     m_L.resize(m_depthPoints.size(), 6, false, false);
     m_numFeatures = m_L.getRows();
+    m_cov.resize(6, 6, false, false);
+    m_covWeightDiag.resize(m_depthPoints.size(), false);
   }
   else {
     m_numFeatures = 0;
   }
-  std::cout << "Depth extraction took: " << vpTime::measureTimeMs() - t1 << std::endl;
-
 }
 
 void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/)
@@ -122,6 +122,8 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*fram
     m_error = 0;
     m_weights = 1.0;
     m_weighted_error = 0.0;
+    m_cov = 0.0;
+    m_covWeightDiag = 0.0;
   }
   vpRotationMatrix cRo = cMo.getRotationMatrix();
 #ifdef VISP_HAVE_OPENMP
@@ -139,6 +141,7 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*fram
   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];
+    m_covWeightDiag[i] = m_weights[i] * m_weights[i];
     for (unsigned int dof = 0; dof < 6; ++dof) {
       m_L[i][dof] *= m_weights[i];
     }

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

@@ -47,8 +47,7 @@ void vpRBFeatureTracker::updateCovariance(const double lambda)
 {
   vpMatrix  D;
   D.diag(m_covWeightDiag);
-  vpColVector v;
-  m_cov = computeCovarianceMatrix(m_L, -v, lambda * m_error, D);
+  m_cov = computeCovarianceMatrix(m_L, lambda * m_error, D);
 }
 
 void vpRBFeatureTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR)
@@ -73,10 +72,11 @@ void vpRBFeatureTracker::computeJTR(const vpMatrix &interaction, const vpColVect
 #endif
 }
 
-vpMatrix vpRBFeatureTracker::computeCovarianceMatrix(const vpMatrix &A, const vpColVector & /*x*/, const vpColVector &b, const vpMatrix &W)
+vpMatrix vpRBFeatureTracker::computeCovarianceMatrix(const vpMatrix &DJ, const vpColVector &e, const vpMatrix &covDiag)
 {
-  double sigma2 = (((W * b).t()) * W * b)/((double)b.getRows());
-  return (A.t() * W * A).pseudoInverse(b.getRows() * std::numeric_limits<double>::epsilon()) * sigma2;
+  const vpColVector covDiagE = covDiag * e;
+  double sigma2 = (covDiagE.t() * covDiag * e) / ((double)e.getRows());
+  return (DJ.t() * covDiag * DJ).pseudoInverse() * sigma2;
 }
 
 END_VISP_NAMESPACE

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

@@ -263,6 +263,8 @@ void vpRBKltTracker::initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpR
   m_weights.resize(m_numFeatures, false);
   m_LTL.resize(6, 6, false, false);
   m_LTR.resize(6, false);
+  m_cov.resize(6, 6, false, false);
+  m_covWeightDiag.resize(m_numFeatures, false);
   m_error = 0;
 }
 
@@ -287,14 +289,14 @@ void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, co
   m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights);
   for (unsigned int i = 0; i < m_error.getRows(); ++i) {
     m_weighted_error[i] = m_error[i] * m_weights[i];
+    m_covWeightDiag[i] = m_weights[i] * m_weights[i];
     for (unsigned int dof = 0; dof < 6; ++dof) {
       m_L[i][dof] *= m_weights[i];
     }
   }
 
   m_LTL = m_L.AtA();
   computeJTR(m_L, m_weighted_error, m_LTR);
-  //std::cout << "m_LTL klt = " << m_LTL << std::endl;
 }
 
 

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

@@ -653,9 +653,8 @@ void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
 
   vpMatrix hessian_E_inv = hessian_E.inverseByCholesky();
   //Sigma_Phi = /*Sigma_Phi +*/ 2*hessian_E_inv;
-  Sigma_Phi = m_ccdParameters.covarianceIterDecreaseFactor * Sigma_Phi + 2 * (1 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv;
+  Sigma_Phi = m_ccdParameters.covarianceIterDecreaseFactor * Sigma_Phi + 2.0 * (1.0 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv;
 
-  m_cov = Sigma_Phi;
 }
 
 END_VISP_NAMESPACE

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

@@ -180,11 +180,8 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram
     const double eri = m_error[i];
     m_covWeightDiag[i] = wi * wi;
     m_weighted_error[i] = wi * eri;
-  }
-
-  for (unsigned int i = 0; i < m_error.size(); i++) {
     for (unsigned int j = 0; j < 6; j++) {
-      m_L[i][j] = m_weights[i] * m_L[i][j];
+      m_L[i][j] = wi * m_L[i][j];
     }
   }
 

tutorial/tracking/render-based/render-based-tutorial-utils.h

@@ -10,7 +10,6 @@
 #include <math.h>
 #include <string.h>
 
-
 #include <visp3/core/vpImage.h>
 #include <visp3/core/vpImageFilter.h>
 #include <visp3/core/vpImageConvert.h>
@@ -197,19 +196,21 @@ class vpRBExperimentPlotter
 {
 public:
 
-  vpRBExperimentPlotter() : enabled(false), plotPose(false), plotPose3d(false), plotDivergenceMetrics(false) { }
+  vpRBExperimentPlotter() : enabled(false), plotPose(false), plotPose3d(false), plotDivergenceMetrics(false), plotCovariance(false) { }
 
   void registerArguments(vpJsonArgumentParser &parser)
   {
     parser
       .addFlag("--plot-pose", plotPose, "Plot the pose of the object in the camera frame")
       .addFlag("--plot-position", plotPose3d, "Plot the position of the object in a 3d figure")
-      .addFlag("--plot-divergence", plotDivergenceMetrics, "Plot the metrics associated to the divergence threshold computation");
+      .addFlag("--plot-divergence", plotDivergenceMetrics, "Plot the metrics associated to the divergence threshold computation")
+      .addFlag("--plot-cov", plotCovariance, "Plot the pose covariance trace for each feature");
+
   }
 
   void postProcessArguments(bool displayEnabled)
   {
-    enabled = plotPose || plotDivergenceMetrics || plotPose3d;
+    enabled = plotPose || plotDivergenceMetrics || plotPose3d || plotCovariance;
     if (enabled && !displayEnabled) {
       throw vpException(vpException::badValue, "Tried to plot data, but display is disabled");
     }
@@ -226,7 +227,7 @@ class vpRBExperimentPlotter
       xpos = std::max(xpos, display->getWindowXPosition() + static_cast<int>(display->getWidth()));
     }
 
-    numPlots = static_cast<int>(plotPose) + static_cast<int>(plotDivergenceMetrics) + static_cast<int>(plotPose3d);
+    numPlots = static_cast<int>(plotPose) + static_cast<int>(plotDivergenceMetrics) + static_cast<int>(plotPose3d) + static_cast<int>(plotCovariance);
     plotter.init(numPlots, 600, 800, xpos, ypos, "Plot");
     unsigned int plotIndex = 0;
     if (plotPose) {
@@ -254,6 +255,14 @@ class vpRBExperimentPlotter
       plotter.setTitle(plotIndex, "Divergence");
       ++plotIndex;
     }
+    if (plotCovariance) {
+      plotter.initGraph(plotIndex, 2);
+      plotter.setLegend(plotIndex, 0, "Translation trace standard deviation (cm)");
+      plotter.setLegend(plotIndex, 1, "Rotation trace standard deviation (deg)");
+
+      plotter.setTitle(plotIndex, "Covariance");
+      ++plotIndex;
+    }
   }
 
   void plot(const vpRBTracker &tracker, double time)
@@ -281,13 +290,28 @@ class vpRBExperimentPlotter
       plotter.plot(plotIndex, 0, time, metric);
       ++plotIndex;
     }
+    if (plotCovariance) {
+      vpMatrix cov = tracker.getCovariance();
+      double traceTranslation = 0.0, traceRotation = 0.0;
+      for (unsigned int i = 0; i < 3; ++i) {
+        traceTranslation += cov[i][i];
+        traceRotation += cov[i + 3][i + 3];
+      }
+      traceTranslation = sqrt(traceTranslation) * 100;
+      traceRotation = vpMath::deg(sqrt(traceRotation));
+
+      plotter.plot(plotIndex, 0, time, traceTranslation);
+      plotter.plot(plotIndex, 1, time, traceRotation);
+
+      ++plotIndex;
+    }
   }
 private:
   bool enabled;
   bool plotPose;
   bool plotPose3d;
-
   bool plotDivergenceMetrics;
+  bool plotCovariance;
   int numPlots;
   vpPlot plotter;
 };