vpMbEdgeTracker.h

/****************************************************************************
 *
 * $Id: vpMbEdgeTracker.h 2807 2010-09-14 10:14:54Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 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: 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;
    vpMbtHiddenFaces faces;
    unsigned int nbvisiblepolygone;
    
    bool displayMe;
    
    double percentageGdPt;
    
    std::vector<bool> scales;
    
    std::vector< const vpImage<unsigned char>* > Ipyramid;
    
    unsigned int scaleLevel;
  
 public:
  
  vpMbEdgeTracker(); 
  virtual ~vpMbEdgeTracker();
  
  inline void setLambda(const double lambda) {this->lambda = lambda;}
  
  void setMovingEdge(const vpMe &_me);
  void loadConfigFile(const std::string &filename);
  void loadConfigFile(const char* filename);
  void loadModel(const std::string &cad_name);
  void loadModel(const char* cad_name);

private:
    void init(const vpImage<unsigned char>& I);
    
public:
  void track(const vpImage<unsigned char> &I);
  void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor& col , const unsigned int l=1, const bool displayFullModel = false);
  void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor& col , const unsigned int l=1, const bool displayFullModel = false);
  void resetTracker();
  void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& _cMo);
   
  void setDisplayMovingEdges(const bool displayMe) {this->displayMe = displayMe;}
  
  void setFirstThreshold(const double  threshold1) {percentageGdPt = threshold1;}
  double getFirstThreshold() { return percentageGdPt;}


  inline void getMovingEdge(vpMe &_me ) { _me = this->me;}
  
  unsigned int getNbPoints(const unsigned int _level=0);
  vpMbtPolygon* getPolygon(const unsigned int _index); 
  unsigned int getNbPolygon();
  void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int _level = 0);
  void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int _level = 0);
  
  void setScales(const std::vector<bool>& _scales);
  
  std::vector<bool> getScales() const {return scales;}



  virtual void setCameraParameters(const vpCameraParameters& _cam) {
    this->cam = _cam; 
    cameraInitialised = true;

    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);
        }
      }
    }
  }

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

  public:
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

  vp_deprecated vpList<vpMbtDistanceLine *>* getLline(const unsigned int _level = 0);
  vp_deprecated vpList<vpMbtDistanceCylinder *>* getLcylinder(const unsigned int _level = 0);
  
  vp_deprecated void init(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo) ;
#endif

  
};

#endif