diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h index 4409ae7998..3ff030027e 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h +++ b/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 diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h index 195ebaadf4..602b566049 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h +++ b/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 &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h index 0b546426cd..ed66edc3db 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h +++ b/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 &Ior, const vpImage &Ivalid, const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp); - vpMatrix getCovariance() - { - throw vpException(vpException::notImplementedError); - } - template void checkDimensionsOrThrow(const vpImage &I, const std::string &imgType) const { diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index 0a67ab1c9c..7c9b525166 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/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 &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) diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp index 06ad78b0d7..bfb71cfd5d 100644 --- a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp +++ b/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]; } diff --git a/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp b/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp index 70f55d9db1..4d485413f9 100644 --- a/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp +++ b/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::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 diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp index c1eefc7508..736ecd3f5f 100644 --- a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp +++ b/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,6 +289,7 @@ 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]; } @@ -294,7 +297,6 @@ void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, co m_LTL = m_L.AtA(); computeJTR(m_L, m_weighted_error, m_LTR); - //std::cout << "m_LTL klt = " << m_LTL << std::endl; } diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp index 0e986abad1..a884fb07c5 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp +++ b/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 diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp index 4efd06f1ea..7833e7228c 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp +++ b/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]; } } diff --git a/tutorial/tracking/render-based/render-based-tutorial-utils.h b/tutorial/tracking/render-based/render-based-tutorial-utils.h index adcc697653..5bc4db0da2 100644 --- a/tutorial/tracking/render-based/render-based-tutorial-utils.h +++ b/tutorial/tracking/render-based/render-based-tutorial-utils.h @@ -10,7 +10,6 @@ #include #include - #include #include #include @@ -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(display->getWidth())); } - numPlots = static_cast(plotPose) + static_cast(plotDivergenceMetrics) + static_cast(plotPose3d); + numPlots = static_cast(plotPose) + static_cast(plotDivergenceMetrics) + static_cast(plotPose3d) + static_cast(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; };