vpMbKltTracker.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.
 *
 * Description:
 * Model based tracker using only KLT
 */

 
#ifndef _vpMbKltTracker_h_
#define _vpMbKltTracker_h_
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
 
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpSubColVector.h>
#include <visp3/core/vpSubMatrix.h>
#include <visp3/klt/vpKltOpencv.h>
#include <visp3/mbt/vpMbTracker.h>
#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
#include <visp3/mbt/vpMbtDistanceKltPoints.h>
#include <visp3/vision/vpHomography.h>
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker
{
protected:
  cv::Mat cur;
  vpHomogeneousMatrix c0Mo;
  bool firstInitialisation;
  unsigned int maskBorder;
  double threshold_outlier;
  double percentGood;
  vpHomogeneousMatrix ctTc0;
  vpKltOpencv tracker;
  std::list<vpMbtDistanceKltPoints *> kltPolygons;
  std::list<vpMbtDistanceKltCylinder *> kltCylinders;
  std::list<vpMbtDistanceCircle *> circles_disp;
  unsigned int m_nbInfos;
  unsigned int m_nbFaceUsed;
  vpMatrix m_L_klt;
  vpColVector m_error_klt;
  vpColVector m_w_klt;
  vpColVector m_weightedError_klt;
  vpRobust m_robust_klt;
  std::vector<std::vector<double> > m_featuresToBeDisplayedKlt;
 
public:
  vpMbKltTracker();
  virtual ~vpMbKltTracker();

 
  void addCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, const std::string &name = "");
  virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                       const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) VP_OVERRIDE;
  virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                       const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) VP_OVERRIDE;

  virtual std::list<vpMbtDistanceCircle *> &getFeaturesCircle() { return circles_disp; }
  virtual std::list<vpMbtDistanceKltCylinder *> &getFeaturesKltCylinder() { return kltCylinders; }
  virtual std::list<vpMbtDistanceKltPoints *> &getFeaturesKlt() { return kltPolygons; }

  inline std::vector<cv::Point2f> getKltPoints() const { return tracker.getFeatures(); }
 
  std::vector<vpImagePoint> getKltImagePoints() const;
 
  std::map<int, vpImagePoint> getKltImagePointsWithId() const;

  inline vpKltOpencv getKltOpencv() const { return tracker; }

  inline unsigned int getKltMaskBorder() const { return maskBorder; }

  inline int getKltNbPoints() const { return tracker.getNbFeatures(); }

  inline double getKltThresholdAcceptation() const { return threshold_outlier; }
 
  virtual inline vpColVector getError() const VP_OVERRIDE { return m_error_klt; }
 
  virtual inline vpColVector getRobustWeights() const VP_OVERRIDE { return m_w_klt; }
 
  virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
                                                               const vpHomogeneousMatrix &cMo,
                                                               const vpCameraParameters &cam,
                                                               bool displayFullModel = false) VP_OVERRIDE;
 
  virtual void loadConfigFile(const std::string &configFile, bool verbose = true) VP_OVERRIDE;
 
  virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo,
                           bool verbose = false, const vpHomogeneousMatrix &T = vpHomogeneousMatrix());
  void resetTracker() VP_OVERRIDE;
 
  void setCameraParameters(const vpCameraParameters &cam) VP_OVERRIDE;

  inline void setKltMaskBorder(const unsigned int &e)
  {
    maskBorder = e;
    // if(useScanLine)
    faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
  }
 
  virtual void setKltOpencv(const vpKltOpencv &t);

  inline void setKltThresholdAcceptation(double th) { threshold_outlier = th; }

  virtual void setOgreVisibilityTest(const bool &v) VP_OVERRIDE
  {
    vpMbTracker::setOgreVisibilityTest(v);
#ifdef VISP_HAVE_OGRE
    faces.getOgreContext()->setWindowName("MBT Klt");
#endif
  }

  virtual void setScanLineVisibilityTest(const bool &v) VP_OVERRIDE
  {
    vpMbTracker::setScanLineVisibilityTest(v);
 
    for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it)
      (*it)->useScanLine = v;
  }
 
  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE;
  virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE;

  virtual void setProjectionErrorComputation(const bool &flag) VP_OVERRIDE
  {
    if (flag)
      std::cerr << "This option is not yet implemented in vpMbKltTracker, "
      "projection error computation set to false."
      << std::endl;
  }
 
  void setUseKltTracking(const std::string &name, const bool &useKltTracking);
 
  virtual void testTracking() VP_OVERRIDE;
  virtual void track(const vpImage<unsigned char> &I) VP_OVERRIDE;
  virtual void track(const vpImage<vpRGBa> &I_color) VP_OVERRIDE;


  /* VP_DEPRECATED */ inline unsigned int getMaskBorder() const { return maskBorder; }

  /* VP_DEPRECATED */ inline int getNbKltPoints() const { return tracker.getNbFeatures(); }

  /* VP_DEPRECATED */ inline double getThresholdAcceptation() const { return threshold_outlier; }

  /* VP_DEPRECATED */ inline void setMaskBorder(const unsigned int &e)
  {
    maskBorder = e;
    // if(useScanLine)
    faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
  }

  /* VP_DEPRECATED */ inline void setThresholdAcceptation(double th) { threshold_outlier = th; }

 
protected:
  void computeVVS();
  virtual void computeVVSInit() VP_OVERRIDE;
  virtual void computeVVSInteractionMatrixAndResidu() VP_OVERRIDE;
 
  virtual std::vector<std::vector<double> > getFeaturesForDisplayKlt();
 
  virtual void init(const vpImage<unsigned char> &I) VP_OVERRIDE;
  virtual void initFaceFromCorners(vpMbtPolygon &polygon) VP_OVERRIDE;
  virtual void initFaceFromLines(vpMbtPolygon &polygon) VP_OVERRIDE;
  virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name = "") VP_OVERRIDE;
  virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name = "") VP_OVERRIDE;
 
  void preTracking(const vpImage<unsigned char> &I);
  bool postTracking(const vpImage<unsigned char> &I, vpColVector &w);
  virtual void reinit(const vpImage<unsigned char> &I);
  virtual void setPose(const vpImage<unsigned char> *I, const vpImage<vpRGBa> *I_color,
                       const vpHomogeneousMatrix &cdMo);
};
END_VISP_NAMESPACE
#endif
#endif // VISP_HAVE_OPENCV