vpMbKltTracker.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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 http://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
 *
 * Authors:
 * Romain Tallonneau
 * Aurelien Yol
 *
 *****************************************************************************/
#ifndef vpMbKltTracker_h
#define vpMbKltTracker_h

#include <visp3/core/vpConfig.h>

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

#include <visp3/mbt/vpMbTracker.h>
#include <visp3/klt/vpKltOpencv.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/mbt/vpMbtKltXmlParser.h>
#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpRobust.h>
#include <visp3/core/vpSubColVector.h>
#include <visp3/core/vpSubMatrix.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/mbt/vpMbtDistanceKltPoints.h>
#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/mbt/vpMbtDistanceKltCylinder.h>

class VISP_EXPORT vpMbKltTracker: virtual public vpMbTracker
{
  friend class vpMbKltMultiTracker;
  friend class vpMbEdgeKltMultiTracker;

protected:
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
  cv::Mat cur;
#else
  IplImage *cur;
#endif
  vpHomogeneousMatrix c0Mo;
  bool compute_interaction;
  bool firstInitialisation;
  unsigned int maskBorder;
  double lambda;
  unsigned int  maxIter;
  double threshold_outlier;
  double percentGood;
  vpHomogeneousMatrix ctTc0;
  vpKltOpencv tracker;
  std::list<vpMbtDistanceKltPoints*> kltPolygons;
  std::list<vpMbtDistanceKltCylinder*> kltCylinders;
  std::list<vpMbtDistanceCircle*> circles_disp;

public:
  vpMbKltTracker();
  virtual ~vpMbKltTracker();
  

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

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

#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
  inline  std::vector<cv::Point2f> getKltPoints() const {return tracker.getFeatures();}
#else
  inline  CvPoint2D32f*   getKltPoints() {return tracker.getFeatures();}
#endif
  
  std::vector<vpImagePoint> getKltImagePoints() const;

  std::map<int, vpImagePoint> getKltImagePointsWithId() const;

  inline  vpKltOpencv getKltOpencv() const { return tracker; }

  virtual inline  double getLambda() const {return lambda;}
  
  inline  unsigned int getMaskBorder() const { return maskBorder; }
  
  virtual inline unsigned int getMaxIter() const {return maxIter;}
  
  inline  int  getNbKltPoints() const {return tracker.getNbFeatures();}

  inline  double getThresholdAcceptation() const { return threshold_outlier;}
  
  virtual void loadConfigFile(const std::string& configFile);
  void loadConfigFile(const char* configFile);

  virtual void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
                           const bool verbose=false);
  void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
                   const bool verbose=false);
  void resetTracker();

  void setCameraParameters(const vpCameraParameters& cam);

  virtual void setKltOpencv(const vpKltOpencv& t);

  virtual inline void setLambda(const double gain) {this->lambda = gain;}
  
  inline  void setMaskBorder(const unsigned int &e)
  {
    maskBorder = e;
    //if(useScanLine)
    faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
  }
  
  virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}

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

  virtual void setScanLineVisibilityTest(const bool &v){
    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);

  virtual void setProjectionErrorComputation(const bool &flag) {
    if(flag)
      vpCTRACE << "This option is not yet implemented in vpMbKltTracker, projection error computation set to false." << std::endl ; }
  
  inline  void setThresholdAcceptation(const double th) {threshold_outlier = th;}

  void setUseKltTracking(const std::string &name, const bool &useKltTracking);
  
  virtual void testTracking();
  virtual void track(const vpImage<unsigned char>& I);
  
protected:
  void computeVVS(const unsigned int &nbInfos, vpColVector &w);
  void computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
                                            const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
                                            double &mu, bool &reStartFromLastIncrement);
  void computeVVSCovariance(const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev,
                            const vpMatrix &L_true, const vpMatrix &LVJ_true);
  void computeVVSInteractionMatrixAndResidu(unsigned int shift, vpColVector &R, vpMatrix &L, vpHomography &H,
                                            std::list<vpMbtDistanceKltPoints*> &kltPolygons_, std::list<vpMbtDistanceKltCylinder*> &kltCylinders_,
                                            const vpHomogeneousMatrix &ctTc0_);
  void computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L,
                                const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
                                vpColVector &R, vpMatrix &LTL, vpColVector &LTR, vpColVector &error_prev, vpColVector &v, double &mu,
                                vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev);
  void computeVVSWeights(const unsigned int iter, const unsigned int nbInfos, const vpColVector &R,
                         vpColVector &w_true, vpColVector &w, vpRobust &robust);

  virtual void init(const vpImage<unsigned char>& I);
  virtual void initFaceFromCorners(vpMbtPolygon &polygon);
  virtual void initFaceFromLines(vpMbtPolygon &polygon);
  virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double, const int,
                          const std::string &name="");
  virtual void initCylinder(const vpPoint&, const vpPoint &, const double, const int,
                            const std::string &name="");

  void preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed);
  bool postTracking(const vpImage<unsigned char>& I, vpColVector &w);
  virtual void reinit(const vpImage<unsigned char>& I);
};

#endif
#endif // VISP_HAVE_OPENCV