vpRBKltTracker.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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_KLT_TRACKER_H
#define VP_RB_KLT_TRACKER_H
 
#include <visp3/core/vpConfig.h>
 
#if (defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO))
#define VP_HAVE_RB_KLT_TRACKER
#endif
 
#if defined(VP_HAVE_RB_KLT_TRACKER)
 
#include <map>
 
#include <visp3/rbt/vpRBFeatureTracker.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpRobust.h>
#include <visp3/klt/vpKltOpencv.h>
 
#include <opencv2/core/mat.hpp>
 
 
BEGIN_VISP_NAMESPACE

class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker
{
public:
  vpRBKltTracker();
 
  virtual ~vpRBKltTracker() = default;
 
  bool requiresRGB() const VP_OVERRIDE { return false; }
 
  bool requiresDepth() const VP_OVERRIDE { return false; }
 
  bool requiresSilhouetteCandidates() const VP_OVERRIDE { return false; }
 
  void onTrackingIterStart(const vpRBFeatureTrackerInput &/*frame*/, const vpHomogeneousMatrix & /*cMo*/) VP_OVERRIDE { }
 
  void onTrackingIterEnd(const vpHomogeneousMatrix & /*cMo*/) VP_OVERRIDE { }
 
  void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE;
 
  void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE;
 
  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 display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpImage<vpRGBa> &IRGB, const vpImage<unsigned char> &depth) const VP_OVERRIDE;


  unsigned int getMinimumNumberOfPoints() const { return m_numPointsReinit; }
  void setMinimumNumberOfPoints(unsigned int points) { m_numPointsReinit = points; }

  double getMinimumDistanceNewPoints() const { return m_newPointsDistanceThreshold; }
  void setMinimumDistanceNewPoints(double distance) { m_newPointsDistanceThreshold = distance; }

  unsigned int getFilteringBorderSize() const { return m_border; }
  void setFilteringBorderSize(unsigned int border) { m_border = border; }

  double getFilteringMaxReprojectionError() const { return m_maxErrorOutliersPixels; }
  void setFilteringMaxReprojectionError(double maxError) { m_maxErrorOutliersPixels = maxError; }

  bool shouldUseMask() const { return m_useMask; }
  void setShouldUseMask(bool useMask) { m_useMask = useMask; }

  float getMinimumMaskConfidence() const { return m_minMaskConfidence; }
  void setMinimumMaskConfidence(float confidence)
  {
    if (confidence > 1.f || confidence < 0.f) {
      throw vpException(vpException::badValue, "Mask confidence should be between 0 and 1");
    }
    m_minMaskConfidence = confidence;
  }

  const vpKltOpencv &getKltTracker() const { return m_klt; }
  vpKltOpencv &getKltTracker() { return m_klt; }
 
#if defined(VISP_HAVE_NLOHMANN_JSON)
  virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE
  {
    vpRBFeatureTracker::loadJsonConfiguration(j);
 
    m_klt = j;
 
    setMinimumNumberOfPoints(j.value("minimumNumPoints", m_numPointsReinit));
    setMinimumDistanceNewPoints(j.value("newPointsMinPixelDistance", m_newPointsDistanceThreshold));
    setFilteringMaxReprojectionError(j.value("maxReprojectionErrorPixels", m_maxErrorOutliersPixels));
    setShouldUseMask(j.value("useMask", m_useMask));
    setMinimumMaskConfidence(j.value("minMaskConfidence", m_minMaskConfidence));
  }
#endif

  struct vpTrackedKltPoint
  {
  public:
    vpHomogeneousMatrix cTo0; 
    vpPoint oX; 
    vpColVector normal; 
    vpImagePoint currentPos; 
 
    inline double rotationDifferenceToInitial(const vpHomogeneousMatrix &oMc)
    {
      const vpHomogeneousMatrix cinitTc = cTo0 * oMc;
      return cinitTc.getThetaUVector().getTheta();
    }
 
    inline double normalDotProd(const vpHomogeneousMatrix &cMo)
    {
      vpColVector cameraNormal = cMo.getRotationMatrix() * normal;
      oX.changeFrame(cMo);
      vpColVector dir({ -oX.get_X(), -oX.get_Y(), -oX.get_Z() });
      dir.normalize();
      return dir * cameraNormal;
    }
 
    inline void update(const vpHomogeneousMatrix &cMo)
    {
      oX.changeFrame(cMo);
      oX.projection();
    }
 
    inline void error(vpColVector &e, unsigned i) const
    {
      e[i * 2] = oX.get_x() - currentPos.get_u();
      e[i * 2 + 1] = oX.get_y() - currentPos.get_v();
    }
 
    inline double distance(const vpTrackedKltPoint &other) const
    {
      const double d = sqrt(std::pow(oX.get_oX() - other.oX.get_oX(), 2) +
        std::pow(oX.get_oY() - other.oX.get_oY(), 2) +
        std::pow(oX.get_oZ() - other.oX.get_oZ(), 2));
      return d;
    }
 
    inline void interaction(vpMatrix &L, unsigned i) const
    {
      double x = oX.get_x(), y = oX.get_y();
      double xy = x * y;
      double Zinv = 1.0 / oX.get_Z();
      L[i * 2][0] = -Zinv;
      L[i * 2][1] = 0.0;
      L[i * 2][2] = x * Zinv;
      L[i * 2][3] = xy;
      L[i * 2][4] = -(1.0 + x * x);
      L[i * 2][5] = y;
 
      L[i * 2 + 1][0] = 0.0;
      L[i * 2 + 1][1] = -Zinv;
      L[i * 2 + 1][2] = y * Zinv;
      L[i * 2 + 1][3] = 1.0 + y * y;
      L[i * 2 + 1][4] = -xy;
      L[i * 2 + 1][5] = -x;
    }
  };
 
private:
 
  void tryAddNewPoint(const vpRBFeatureTrackerInput &frame, std::map<long, vpTrackedKltPoint> &points,
                      long id, const float u, const float v, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc);
 
  vpRobust m_robust;
 
  cv::Mat m_I, m_Iprev;
  vpKltOpencv m_klt;
 
  unsigned int m_numPointsReinit; 
  double m_newPointsDistanceThreshold; 
  unsigned int m_border; 
 
  double m_maxErrorOutliersPixels; 
 
  std::map<long, vpTrackedKltPoint> m_points;
 
  bool m_useMask;
  float m_minMaskConfidence;
};
 
END_VISP_NAMESPACE
 
#endif
#endif