vpMbEdgeTracker.h

/****************************************************************************
 *
 * $Id: vpMbEdgeTracker.h 4122 2013-02-08 10:45:54Z ayol $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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 <visp/vpPoint.h>
#include <visp/vpMbTracker.h>
#include <visp/vpMe.h>
#include <visp/vpMbtMeLine.h>
#include <visp/vpMbtDistanceLine.h>
#include <visp/vpMbtDistanceCylinder.h>
#include <visp/vpXmlParser.h>

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

#if defined(VISP_HAVE_COIN)
//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

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
#  include <visp/vpList.h>
#endif

class VISP_EXPORT vpMbEdgeTracker: virtual public vpMbTracker
{
  protected :
    
    int compute_interaction;
    double lambda;
    
    vpMe  me;
    std::vector< std::list< vpMbtDistanceLine*> > lines;
    std::vector< std::list< vpMbtDistanceCylinder*> > cylinders;

    unsigned int nline;

    unsigned int ncylinder;
    
    int index_polygon;
    
    vpMbHiddenFaces<vpMbtPolygon> faces;
    
    unsigned int nbvisiblepolygone;
    
    double percentageGdPt;
    
    std::vector<bool> scales;
    
    std::vector< const vpImage<unsigned char>* > Ipyramid;
    
    unsigned int scaleLevel;
    
    bool useOgre;
  
    double angleAppears;
  
    double angleDisappears;
  
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 getAngleAppear() { return angleAppears; }   
  
  virtual inline double getAngleDisappear() { return angleDisappears; } 
  
  vpMbHiddenFaces<vpMbtPolygon> & getFaces() { return faces;}
  double getFirstThreshold() { return percentageGdPt;}
  
  void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level = 0);
  void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level = 0);
  
  inline void getMovingEdge(vpMe &me ) { me = this->me;}
  
  unsigned int getNbPoints(const unsigned int level=0);
  unsigned int getNbPolygon();
  vpMbtPolygon* getPolygon(const unsigned int index);
  
  std::vector<bool> getScales() const {return scales;}
  
  void loadConfigFile(const std::string &configFile);
  void loadConfigFile(const char* configFile);
  void loadModel(const std::string &cad_name);
  void loadModel(const char* cad_name);  
  
  void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo);
  void resetTracker();
  
  virtual inline  void setAngleAppear(const double &a) { angleAppears = a; }   
  
  virtual inline void setAngleDisappear(const double &a) { angleDisappears = a; }
  
  virtual void setCameraParameters(const vpCameraParameters& cam) {
    this->cam = cam;

    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);
        }
      }
    }
  }
  
  void setDisplayMovingEdges(const bool displayMe) {displayFeatures = displayMe;}
  
  void setFirstThreshold(const double  threshold1) {percentageGdPt = threshold1;}
  
  inline void setLambda(const double lambda) {this->lambda = lambda;}
  
  void setMovingEdge(const vpMe &me);
  
  virtual void setOgreVisibilityTest(const bool &v);
  
  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
  
  void setScales(const std::vector<bool>& _scales);

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

protected:
  void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, const std::string& name = "");
  void addLine(vpPoint &p1, vpPoint &p2, int polygone = -1, std::string name = "");
  void addPolygon(vpMbtPolygon &p) ;
  void cleanPyramid(std::vector<const vpImage<unsigned char>* >& _pyramid);
  void computeVVS(const vpImage<unsigned char>& _I);
  void downScale(const unsigned int _scale);
  void init(const vpImage<unsigned char>& I);
  virtual void initCylinder(const vpPoint& _p1, const vpPoint _p2, const double _radius, const unsigned int _indexCylinder=0);
  virtual void initFaceFromCorners(const std::vector<vpPoint>& _corners, const unsigned int _indexFace = -1);
  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 removeCylinder(const std::string& name);
  void removeLine(const std::string& name);
  void testTracking();
  void trackMovingEdge(const vpImage<unsigned char> &I) ;
  void updateMovingEdge(const vpImage<unsigned char> &I) ;
  void upScale(const unsigned int _scale); 
  void visibleFace(const vpHomogeneousMatrix &_cMo, bool &newvisibleline) ;
  void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline) ; 
};

#endif