vpMbEdgeTracker.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:
 * Make the complete tracking of an object by using its CAD model
 *
 * Authors:
 * Nicolas Melchior
 * Romain Tallonneau
 * Eric Marchand
 *
 *****************************************************************************/

#ifndef vpMbEdgeTracker_HH
#define vpMbEdgeTracker_HH

#include <visp3/core/vpPoint.h>
#include <visp3/mbt/vpMbTracker.h>
#include <visp3/me/vpMe.h>
#include <visp3/mbt/vpMbtMeLine.h>
#include <visp3/mbt/vpMbtDistanceLine.h>
#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/mbt/vpMbtDistanceCylinder.h>
#include <visp3/core/vpXmlParser.h>
#include <visp3/core/vpRobust.h>

#include <iostream>
#include <fstream>
#include <vector>
#include <list>

#if defined(VISP_HAVE_COIN3D)
//Inventor includes
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/misc/SoChildList.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoGetPrimitiveCountAction.h>
#include <Inventor/actions/SoToVRML2Action.h>
#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/VRMLnodes/SoVRMLShape.h>
#endif

#ifdef VISP_HAVE_OPENCV
#  if VISP_HAVE_OPENCV_VERSION >= 0x020101
#    include <opencv2/core/core.hpp>
#    include <opencv2/imgproc/imgproc.hpp>
#    include <opencv2/imgproc/imgproc_c.h>
#  else
#    include <cv.h>
#  endif
#endif

class VISP_EXPORT vpMbEdgeTracker: virtual public vpMbTracker
{
  friend class vpMbEdgeMultiTracker;
  friend class vpMbEdgeKltMultiTracker;

  protected :
    
    int compute_interaction;
    double lambda;
    
    vpMe me;
    std::vector< std::list< vpMbtDistanceLine*> > lines;

    std::vector< std::list< vpMbtDistanceCircle*> > circles;

    std::vector< std::list< vpMbtDistanceCylinder*> > cylinders;

    unsigned int nline;

    unsigned int ncircle;

    unsigned int ncylinder;
    
    unsigned int nbvisiblepolygone;
    
    double percentageGdPt;
    
    std::vector<bool> scales;
    
    std::vector< const vpImage<unsigned char>* > Ipyramid;
    
    unsigned int scaleLevel;

    unsigned int nbFeaturesForProjErrorComputation;

public:
  
  vpMbEdgeTracker(); 
  virtual ~vpMbEdgeTracker();
  

  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);
  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 inline double getLambda() const {return lambda;}
  
  void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level = 0) const;
  void getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level = 0) const;
  void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level = 0) const;

  virtual inline void getMovingEdge(vpMe &p_me ) const { p_me = this->me;}
  virtual inline vpMe getMovingEdge() const { return this->me;}

  virtual unsigned int getNbPoints(const unsigned int level=0) const;
  
  std::vector<bool> getScales() const {return scales;}
  inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt;}

  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();
  
  virtual void setCameraParameters(const vpCameraParameters& camera) {
    this->cam = camera;

    for (unsigned int i = 0; i < scales.size(); i += 1){
      if(scales[i]){
        for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
          (*it)->setCameraParameters(cam);
        }

        for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
          (*it)->setCameraParameters(cam);
        }

        for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
          (*it)->setCameraParameters(cam);
        }
      }
    }
  }

  virtual void setClipping(const unsigned int &flags);

  virtual void setFarClippingDistance(const double &dist);

  virtual void setNearClippingDistance(const double &dist);

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

  virtual void setScanLineVisibilityTest(const bool &v){
    vpMbTracker::setScanLineVisibilityTest(v);

    for (unsigned int i = 0; i < scales.size(); i += 1){
      if(scales[i]){
        for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
          (*it)->useScanLine = v;
        }
      }
    }
  }

  void setGoodMovingEdgesRatioThreshold(const double  threshold) {percentageGdPt = threshold;}

  virtual inline void setLambda(const double gain) {this->lambda = gain;}
  
  void setMovingEdge(const vpMe &me);

  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
  
  void setScales(const std::vector<bool>& _scales);

  void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking);

  void track(const vpImage<unsigned char> &I);

protected:
  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
  void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace = -1, const std::string& name = "");
  void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace = -1, const std::string& name = "");
  void addLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");
  void addPolygon(vpMbtPolygon &p) ;

  void cleanPyramid(std::vector<const vpImage<unsigned char>* >& _pyramid);
  void computeProjectionError(const vpImage<unsigned char>& _I);

  void computeVVS(const vpImage<unsigned char>& _I, const unsigned int lvl);
  void computeVVSFirstPhase(const vpImage<unsigned char>& I, const unsigned int iter,
      vpMatrix &L, vpColVector &factor, double &count, vpColVector &error, vpColVector &w_mbt, const unsigned int lvl = 0);
  void computeVVSFirstPhaseFactor(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl = 0);
  void computeVVSFirstPhasePoseEstimation(const unsigned int nerror, const unsigned int iter, const vpColVector &factor,
      vpColVector &weighted_error, vpMatrix &L, bool &isoJoIdentity_);
  void computeVVSSecondPhase(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &error_lines,
      vpColVector &error_cylinders, vpColVector &error_circles, vpColVector &error, const unsigned int lvl);
  void computeVVSSecondPhaseCheckLevenbergMarquardt(const unsigned int iter, const unsigned int nbrow,
      const vpColVector &m_error_prev, const vpColVector &m_w_prev, const vpHomogeneousMatrix &cMoPrev,
      double &mu, bool &reStartFromLastIncrement);
  void computeVVSSecondPhasePoseEstimation(const unsigned int nerror, vpMatrix &L, vpMatrix &L_true, vpMatrix &LVJ_true,
      vpColVector &W_true, const vpColVector &factor, const unsigned int iter, const bool isoJoIdentity_,
      vpColVector &weighted_error, double &mu, vpColVector &m_error_prev, vpColVector &m_w_prev,
      vpHomogeneousMatrix &cMoPrev, double &residu_1, double &r);
  void computeVVSSecondPhaseWeights(const unsigned int iter, const unsigned int nerror,
      const unsigned int nbrow, vpColVector &weighted_error,
      vpRobust &robust_lines, vpRobust &robust_cylinders, vpRobust &robust_circles,
      vpColVector &w_lines, vpColVector &w_cylinders, vpColVector &w_circles,
      vpColVector &error_lines, vpColVector &error_cylinders, vpColVector &error_circles,
      const unsigned int nberrors_lines, const unsigned int nberrors_cylinders, const unsigned int nberrors_circles);

  void displayFeaturesOnImage(const vpImage<unsigned char>& I, const unsigned int lvl);
  void downScale(const unsigned int _scale);
  void init(const vpImage<unsigned char>& I);
  virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
                          const int idFace=0, const std::string &name="");
  virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
                            const std::string &name="");
  virtual void initFaceFromCorners(vpMbtPolygon &polygon);
  virtual void initFaceFromLines(vpMbtPolygon &polygon);
  unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles);
  void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo) ;
  void initPyramid(const vpImage<unsigned char>& _I, std::vector<const vpImage<unsigned char>* >& _pyramid);
  void reInitLevel(const unsigned int _lvl);
  void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
  void removeCircle(const std::string& name);
  void removeCylinder(const std::string& name);
  void removeLine(const std::string& name);
  void resetMovingEdge();
  void testTracking();
  void trackMovingEdge(const vpImage<unsigned char> &I) ;
  void updateMovingEdge(const vpImage<unsigned char> &I) ;
  void updateMovingEdgeWeights();
  void upScale(const unsigned int _scale); 
  void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline) ; 
};

#endif