vpRBFeatureTracker.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#ifndef VP_RB_FEATURE_TRACKER_H
#define VP_RB_FEATURE_TRACKER_H
 
#include <array>
 
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpRect.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
 
#include <visp3/rbt/vpRBFeatureTrackerInput.h>
#include <visp3/rbt/vpRBSilhouettePoint.h>
#include <visp3/rbt/vpTemporalWeighting.h>
 
#if defined(VISP_HAVE_NLOHMANN_JSON)
#include VISP_NLOHMANN_JSON(json.hpp)
#endif
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpRBFeatureTracker
{
public:
 
  vpRBFeatureTracker();
 
  virtual ~vpRBFeatureTracker() = default;


  unsigned getNumFeatures() const { return m_numFeatures; }


  virtual bool requiresRGB() const = 0;

  virtual bool requiresDepth() const = 0;

  virtual bool requiresSilhouetteCandidates() const = 0;


  virtual void onTrackingIterStart(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo) = 0;

  virtual void onTrackingIterEnd(const vpHomogeneousMatrix &cMo) = 0;

  virtual void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0;

  virtual void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0;
 
  virtual void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0;
  virtual void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) = 0;

  virtual void reset() { }


 
  bool featuresShouldBeDisplayed() const { return m_enableDisplay; }
  void setFeaturesShouldBeDisplayed(bool enableDisplay) { m_enableDisplay = enableDisplay; }
 
  virtual void display(
    const vpCameraParameters &cam,
    const vpImage<unsigned char> &I,
    const vpImage<vpRGBa> &IRGB,
    const vpImage<unsigned char> &depth) const = 0;

 

  virtual const vpMatrix getCovariance() const { return m_cov; }
  virtual void updateCovariance(const double lambda);

  bool vvsHasConverged() const { return m_vvsConverged; }

  virtual double getVVSTrackerWeight(double optimizationProgress) const { return m_weighting->weight(optimizationProgress) / m_numFeatures; }
  const std::shared_ptr<vpTemporalWeighting> getTemporalTrackerWeight() const { return m_weighting; }
  void setTrackerWeight(double weight) { m_weighting = std::make_shared<vpFixedTemporalWeighting>(weight); }
  void setTrackerWeight(const std::shared_ptr<vpTemporalWeighting> &weight) { m_weighting = weight; }

  virtual vpMatrix getLTL() const { return m_LTL; }

  virtual vpColVector getLTR() const { return m_LTR; }

  const vpColVector &getWeightedError() const { return m_weighted_error; }
 
#if defined(VISP_HAVE_NLOHMANN_JSON)
  virtual void loadJsonConfiguration(const nlohmann::json &j)
  {
    m_weighting = vpTemporalWeighting::parseTemporalWeighting(j.at("weight"));
    m_enableDisplay = j.value("display", m_enableDisplay);
    const std::array<bool, 6> allDofs { true, true, true, true, true, true };
    setEstimatedDofs(j.value("dofs", allDofs));
  }
#endif
 
  void setComputeJacobianObjectSpace(bool inObjectSpace)
  {
    m_jacobianInObjectSpace = inObjectSpace;
  }
 
  static void computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR);
  static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &b, const vpMatrix &W);
 
  bool hasIgnoredDofs() const
  {
    for (unsigned int i = 0; i < m_estimatedDofs.size(); ++i) {
      if (!m_estimatedDofs[i]) {
        return true;
      }
    }
    return false;
  }
  void setEstimatedDofs(const std::array<bool, 6> &dofs)
  {
    m_estimatedDofs = dofs;
    m_oJo = computeoJo();
  }
 
 
  void updateOptimizerTerms(const vpHomogeneousMatrix &cMo)
  {
 
    for (unsigned int i = 0; i < m_L.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];
      }
    }
    if (m_jacobianInObjectSpace) {
      vpVelocityTwistMatrix cVo(cMo);
      vpMatrix cVooJo = cVo * m_oJo;
      m_L = m_L * cVooJo;
    }
 
    m_LTL = m_L.AtA();
    computeJTR(m_L, m_weighted_error, m_LTR);
  }
 
protected:
 
  vpMatrix computeoJo() const
  {
    vpMatrix oJo(6, 6, 0);
    for (unsigned int i = 0; i < 6; ++i) {
      if (m_estimatedDofs[i]) {
        oJo[i][i] = 1;
      }
    }
    return oJo;
  }
 
 
 
 
  vpMatrix m_L; 
  vpMatrix m_LTL;  
  vpColVector m_LTR; 
  vpMatrix m_cov; 
  vpColVector m_covWeightDiag;
 
  vpColVector m_error; 
  vpColVector m_weighted_error; 
  vpColVector m_weights; 
 
 
  unsigned m_numFeatures; 
  std::shared_ptr<vpTemporalWeighting> m_weighting; 
 
  bool m_vvsConverged; 
 
  bool m_enableDisplay; 
  std::array<bool, 6> m_estimatedDofs;
  vpMatrix m_oJo; 
  bool m_jacobianInObjectSpace;
};
 
END_VISP_NAMESPACE
 
#endif