vpMbEdgeTracker.cpp

/****************************************************************************
 *
 * $Id: vpMbEdgeTracker.cpp 5285 2015-02-09 14:32:54Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 *
 *****************************************************************************/

#include <visp/vpDebug.h>
#include <visp/vpPose.h>
#include <visp/vpExponentialMap.h>
#include <visp/vpPixelMeterConversion.h>
#include <visp/vpImageIo.h>
#include <visp/vpRobust.h>
#include <visp/vpDisplayOpenCV.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDisplayGDI.h>
#include <visp/vpMatrixException.h>
#include <visp/vpMath.h>
#include <visp/vpException.h>
#include <visp/vpTrackingException.h>
#include <visp/vpMbEdgeTracker.h>
#include <visp/vpMbtDistanceLine.h>
#include <visp/vpMbtXmlParser.h>
#include <visp/vpMbtPolygon.h>
#include <visp/vpVelocityTwistMatrix.h>

#include <limits>
#include <string>
#include <sstream>
#include <float.h>
#include <map>

bool samePoint(const vpPoint &P1, const vpPoint &P2, double threshold);

vpMbEdgeTracker::vpMbEdgeTracker()
  : compute_interaction(1), lambda(1), me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0),
    nbvisiblepolygone(0), percentageGdPt(0.4), scales(1),
    Ipyramid(0), scaleLevel(0)
{
  angleAppears = vpMath::rad(89);
  angleDisappears = vpMath::rad(89);

  scales[0] = true;
  
#ifdef VISP_HAVE_OGRE
  faces.getOgreContext()->setWindowName("MBT Edge");
#endif
}

vpMbEdgeTracker::~vpMbEdgeTracker()
{
  vpMbtDistanceLine *l ;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

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

      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
        cy = *it;
        if (cy!=NULL){
          delete cy ;
        }
        cy = NULL ;
      }

      lines[i].clear();
      cylinders[i].clear();
    }
  }
  for (unsigned int i = 0; i < circles.size(); i += 1){
    for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
      ci = *it;
      if (ci!=NULL){
        delete ci ;
      }
      ci = NULL ;
    }
    circles[i].clear();
  }
  cleanPyramid(Ipyramid);
}

void
vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
{
  this->me = p_me;

  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)->setMovingEdge(&(this->me)) ;
      }

      vpMbtDistanceCylinder *cy;
      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
        cy = *it;
        cy->setMovingEdge(&(this->me)) ;
      }

      vpMbtDistanceCircle *ci;
      for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
        ci = *it;
        ci->setMovingEdge(&(this->me)) ;
      }
    }
  }
}

void
vpMbEdgeTracker::computeVVS(const vpImage<unsigned char>& _I)
{
  double residu_1 =1e3;
  double r =1e3-1;
  vpMatrix LTL;
  vpColVector LTR;

  // compute the interaction matrix and its pseudo inverse
  vpMbtDistanceLine *l ;
  vpMbtDistanceCylinder *cy ;
  vpMbtDistanceCircle *ci ;

  //vpColVector w;
  vpColVector factor;
  //vpColVector error; // s-s*
  vpColVector weighted_error; // Weighted error vector wi(s-s)*
  vpVelocityTwistMatrix cVo;

  unsigned int iter = 0;

  //Nombre de moving edges
  unsigned int nbrow  = 0;
  unsigned int nberrors_lines = 0;
  unsigned int nberrors_cylinders = 0;
  unsigned int nberrors_circles = 0;

  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    nbrow += l->nbFeature;
    nberrors_lines+=l->nbFeature;
    l->initInteractionMatrixError();
  }

  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    nbrow += cy->nbFeature ;
    nberrors_cylinders += cy->nbFeature ;
    cy->initInteractionMatrixError() ;
  }

  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    nbrow += ci->nbFeature ;
    nberrors_circles += ci->nbFeature ;
    ci->initInteractionMatrixError() ;
  }

  if (nbrow==0){
    throw vpTrackingException(vpTrackingException::notEnoughPointError, "No data found to compute the interaction matrix...");
  }
  
  vpMatrix L(nbrow,6), Lp;

  // compute the error vector
  m_error.resize(nbrow);
  unsigned int nerror = m_error.getRows();
  vpColVector v ;

  double limite = 3; //Une limite de 3 pixels
  limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
  
  //Parametre pour la premiere phase d'asservissement
  double e_prev = 0, e_cur, e_next;
  bool reloop = true;
  double count = 0;
  
  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank

  /*** First phase ***/

  while ( reloop == true && iter<10)
  {
    if(iter==0)
    {
      weighted_error.resize(nerror) ;
      m_w.resize(nerror);
      m_w = 0;
      factor.resize(nerror);
      factor = 1;
    }
    
    count = 0;

    unsigned int n = 0;
    reloop = false;
    for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
      l = *it;
      l->computeInteractionMatrixError(cMo);
      
      double fac = 1;
      if (iter == 0)
      {
        for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
          int index = *itindex;
          if (l->hiddenface->isAppearing((unsigned int)index))
          {
            fac = 0.2;
            break;
          }
          if(l->closeToImageBorder(_I, 10))
          {
            fac = 0.1;
            break;
          }
        }
      }
      
      std::list<vpMeSite>::const_iterator itListLine;
      if (iter == 0 && l->meline != NULL)
        itListLine = l->meline->getMeList().begin();
      
      for (unsigned int i=0 ; i < l->nbFeature ; i++)
      {
        for (unsigned int j=0; j < 6 ; j++)
        {
          L[n+i][j] = l->L[i][j]; //On remplit la matrice d'interaction globale
        }
        m_error[n+i] = l->error[i]; //On remplit la matrice d'erreur

        if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur

        m_w[n+i] = 0;

        if (iter == 0)
        {
          factor[n+i] = fac;
          vpMeSite site = *itListLine;
          if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
          ++itListLine;
        }

        //If pour la premiere extremite des moving edges
        if (i == 0)
        {
          e_cur = l->error[0];
          if (l->nbFeature > 1)
          {
            e_next = l->error[1];
            if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
            {
              m_w[n+i] = 1/*0.5*/;
            }
            e_prev = e_cur;
          }
          else m_w[n+i] = 1;
        }

        //If pour la derniere extremite des moving edges
        else if(i == l->nbFeature-1)
        {
          e_cur = l->error[i];
          if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
          {
            m_w[n+i] += 1/*0.5*/;
          }
        }

        else
        {
          e_cur = l->error[i];
          e_next = l->error[i+1];
          if ( fabs(e_cur - e_prev) < limite )
          {
            m_w[n+i] += 0.5;
          }
          if ( fabs(e_cur - e_next) < limite )
          {
            m_w[n+i] += 0.5;
          }
          e_prev = e_cur;
        }
      }
      
      n+= l->nbFeature ;
    }
    for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
      cy = *it;
      cy->computeInteractionMatrixError(cMo, _I);
      double fac = 1.0;

      std::list<vpMeSite>::const_iterator itCyl1;
      std::list<vpMeSite>::const_iterator itCyl2;
      if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
        itCyl1 = cy->meline1->getMeList().begin();
        itCyl2 = cy->meline2->getMeList().begin();
      }

      for(unsigned int i=0 ; i < cy->nbFeature ; i++){
        for(unsigned int j=0; j < 6 ; j++){
          L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
        }
        m_error[n+i] = cy->error[i]; //On remplit la matrice d'erreur

        if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur

        m_w[n+i] = 0;

        if (iter == 0)
        {
          factor[n+i] = fac;
          vpMeSite site;
          if(i<cy->nbFeaturel1) {
            site= *itCyl1;
            ++itCyl1;
          }
          else{
            site= *itCyl2;
            ++itCyl2;
          }
          if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
        }

        //If pour la premiere extremite des moving edges
        if (i == 0)
        {
          e_cur = cy->error[0];
          if (cy->nbFeature > 1)
          {
            e_next = cy->error[1];
            if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
            {
              m_w[n+i] = 1/*0.5*/;
            }
            e_prev = e_cur;
          }
          else m_w[n+i] = 1;
        }
        if (i == cy->nbFeaturel1)
        {
          e_cur = cy->error[i];
          if (cy->nbFeaturel2 > 1)
          {
            e_next = cy->error[i+1];
            if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
            {
              m_w[n+i] = 1/*0.5*/;
            }
            e_prev = e_cur;
          }
          else m_w[n+i] = 1;
        }

        //If pour la derniere extremite des moving edges
        else if(i == cy->nbFeaturel1-1)
        {
          e_cur = cy->error[i];
          if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
          {
            m_w[n+i] += 1/*0.5*/;
          }
        }
        //If pour la derniere extremite des moving edges
        else if(i == cy->nbFeature-1)
        {
          e_cur = cy->error[i];
          if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
          {
            m_w[n+i] += 1/*0.5*/;
          }
        }

        else
        {
          e_cur = cy->error[i];
          e_next = cy->error[i+1];
          if ( fabs(e_cur - e_prev) < limite ){
            m_w[n+i] += 0.5;
          }
          if ( fabs(e_cur - e_next) < limite ){
            m_w[n+i] += 0.5;
          }
          e_prev = e_cur;
        }
      }

      n+= cy->nbFeature ;
    }

    for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
      ci = *it;
      ci->computeInteractionMatrixError(cMo);
      double fac = 1.0;

      std::list<vpMeSite>::const_iterator itCir;
      if (iter == 0 && (ci->meEllipse != NULL)) {
        itCir = ci->meEllipse->getMeList().begin();
      }

      for(unsigned int i=0 ; i < ci->nbFeature ; i++){
        for(unsigned int j=0; j < 6 ; j++){
          L[n+i][j] = ci->L[i][j]; //On remplit la matrice d'interaction globale
        }
        m_error[n+i] = ci->error[i]; //On remplit la matrice d'erreur

        if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur

        m_w[n+i] = 0;

        if (iter == 0)
        {
          factor[n+i] = fac;
          vpMeSite site = *itCir;
          if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
          ++itCir;
        }

        //If pour la premiere extremite des moving edges
        if (i == 0)
        {
          e_cur = ci->error[0];
          if (ci->nbFeature > 1)
          {
            e_next = ci->error[1];
            if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
            {
              m_w[n+i] = 1/*0.5*/;
            }
            e_prev = e_cur;
          }
          else m_w[n+i] = 1;
        }

        //If pour la derniere extremite des moving edges
        else if(i == ci->nbFeature-1)
        {
          e_cur = ci->error[i];
          if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
          {
            m_w[n+i] += 1/*0.5*/;
          }
        }

        else
        {
          e_cur = ci->error[i];
          e_next = ci->error[i+1];
          if ( fabs(e_cur - e_prev) < limite ){
            m_w[n+i] += 0.5;
          }
          if ( fabs(e_cur - e_next) < limite ){
            m_w[n+i] += 0.5;
          }
          e_prev = e_cur;
        }
      }

      n+= ci->nbFeature ;
    }

    count = count / (double)nbrow;
    if (count < 0.85){
      reloop = true;
    }

    double num=0;
    double den=0;

    double wi ; double eri ;
    for(unsigned int i = 0; i < nerror; i++){
      wi = m_w[i]*factor[i];
      eri = m_error[i];
      num += wi*vpMath::sqr(eri);
      den += wi ;

      weighted_error[i] =  wi*eri ;
    }

    if((iter==0) || compute_interaction){
      for (unsigned int i=0 ; i < nerror ; i++){
        for (unsigned int j=0 ; j < 6 ; j++){
          L[i][j] = m_w[i]*factor[i]*L[i][j] ;
        }
      }
    }

    // If all the 6 dof should be estimated, we check if the interaction matrix is full rank.
    // If not we remove automatically the dof that cannot be estimated
    // This is particularly useful when consering circles (rank 5) and cylinders (rank 4)
    if (isoJoIdentity_) {
      cVo.buildFrom(cMo);

      vpMatrix K; // kernel
      unsigned int rank = (L*cVo).kernel(K);
      if(rank == 0) {
        throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
      }
      if (rank != 6) {
        vpMatrix I; // Identity
        I.eye(6);
        oJo = I-K.AtA();

        isoJoIdentity_ = false;
      }
    }

    if(isoJoIdentity_){
        LTL = L.AtA();
        computeJTR(L, weighted_error, LTR);
        v = -0.7*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
    }
    else{
        cVo.buildFrom(cMo);
        vpMatrix LVJ = (L*cVo*oJo);
        vpMatrix LVJTLVJ = (LVJ).AtA();
        vpMatrix LVJTR;
        computeJTR(LVJ, weighted_error, LVJTR);
        v = -0.7*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
        v = cVo * v;
    }

    cMo =  vpExponentialMap::direct(v).inverse() * cMo;

    iter++;
  }

//   std::cout << "\t First minimization in " << iter << " iteration give as initial cMo: \n" << cMo << std::endl ;
  

/*** Second phase ***/

  vpRobust robust_lines(nberrors_lines);
  vpRobust robust_cylinders(nberrors_cylinders);
  vpRobust robust_circles(nberrors_circles);
  robust_lines.setIteration(0) ;
  robust_cylinders.setIteration(0) ;
  robust_circles.setIteration(0) ;
  iter = 0;
  vpColVector w_lines(nberrors_lines);
  vpColVector w_cylinders(nberrors_cylinders);
  vpColVector w_circles(nberrors_circles);
  vpColVector error_lines(nberrors_lines);
  vpColVector error_cylinders(nberrors_cylinders);
  vpColVector error_circles(nberrors_circles);

  vpHomogeneousMatrix cMoPrev;
  vpColVector W_true;
  vpMatrix L_true;
  vpMatrix LVJ_true;
  
  while ( ((int)((residu_1 - r)*1e8) !=0 )  && (iter<30))
  {
    unsigned int n = 0 ;
    unsigned int nlines = 0;
    unsigned int ncylinders = 0;
    unsigned int ncircles = 0;
    for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
      l = *it;
      l->computeInteractionMatrixError(cMo) ;
      for (unsigned int i=0 ; i < l->nbFeature ; i++){
        for (unsigned int j=0; j < 6 ; j++){
          L[n+i][j] = l->L[i][j];
          m_error[n+i] = l->error[i];
          error_lines[nlines+i] = m_error[n+i];
        }
      }
      n+= l->nbFeature;
      nlines+= l->nbFeature;
    }

    for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
      cy = *it;
      cy->computeInteractionMatrixError(cMo, _I) ;
      for(unsigned int i=0 ; i < cy->nbFeature ; i++){
        for(unsigned int j=0; j < 6 ; j++){
          L[n+i][j] = cy->L[i][j];
          m_error[n+i] = cy->error[i];
          error_cylinders[ncylinders+i] = m_error[n+i];
        }
      }

      n+= cy->nbFeature ;
      ncylinders+= cy->nbFeature ;
    }

    for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
      ci = *it;
      ci->computeInteractionMatrixError(cMo) ;
      for(unsigned int i=0 ; i < ci->nbFeature ; i++){
        for(unsigned int j=0; j < 6 ; j++){
          L[n+i][j] = ci->L[i][j];
          m_error[n+i] = ci->error[i];
          error_circles[ncircles+i] = m_error[n+i];
        }
      }

      n+= ci->nbFeature ;
      ncircles+= ci->nbFeature ;
    }

    if(iter==0)
    {
      weighted_error.resize(nerror);
      m_w.resize(nerror);
      m_w = 1;
      w_lines.resize(nberrors_lines);
      w_lines = 1;
      w_cylinders.resize(nberrors_cylinders);
      w_cylinders = 1;
      w_circles.resize(nberrors_circles);
      w_circles = 1;

      robust_lines.setThreshold(2/cam.get_px());
      robust_cylinders.setThreshold(2/cam.get_px());
      robust_circles.setThreshold(vpMath::sqr(2/cam.get_px()));
      if(nberrors_lines > 0)
                robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
      if(nberrors_cylinders > 0)
       robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
      if(nberrors_circles > 0)
        robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
    }
    else
    {
      robust_lines.setIteration(iter);
      robust_cylinders.setIteration(iter);
      robust_circles.setIteration(iter);
      if(nberrors_lines > 0)
                robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
      if(nberrors_cylinders > 0)
        robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
      if(nberrors_circles > 0)
        robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
    }

    unsigned int cpt = 0;
    while(cpt<nbrow){
      if(cpt<nberrors_lines){
        m_w[cpt] = w_lines[cpt];
      }
      else if (cpt<nberrors_lines+nberrors_cylinders){
        m_w[cpt] = w_cylinders[cpt-nberrors_lines];
      }
      else {
        m_w[cpt] = w_circles[cpt-nberrors_lines-nberrors_cylinders];
      }
      cpt++;
    }

    residu_1 = r;

    double num=0;
    double den=0;
    double wi;
    double eri;
    
    L_true = L;
    W_true = vpColVector(nerror);

    if(computeCovariance){
        L_true = L;
       if(!isoJoIdentity_){
         cVo.buildFrom(cMo);
         LVJ_true = (L*cVo*oJo);
       }
    }
    
    for(unsigned int i=0; i<nerror; i++){
      wi = m_w[i]*factor[i];
      W_true[i] = wi;
      eri = m_error[i];
      num += wi*vpMath::sqr(eri);
      den += wi;

      weighted_error[i] =  wi*eri ;
    }
    
    r = sqrt(num/den); //Le critere d'arret prend en compte le poids

    if((iter==0)|| compute_interaction){
      for (unsigned int i=0 ; i < nerror ; i++){
        for (unsigned int j=0 ; j < 6 ; j++){
          L[i][j] = m_w[i]*factor[i]*L[i][j];
        }
      }
    }

    if(isoJoIdentity_){
        LTL = L.AtA();
        computeJTR(L, weighted_error, LTR);
        v = -lambda*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
    }
    else{
        cVo.buildFrom(cMo);
        vpMatrix LVJ = (L*cVo*oJo);
        vpMatrix LVJTLVJ = (LVJ).AtA();
        vpMatrix LVJTR;
        computeJTR(LVJ, weighted_error, LVJTR);
        v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
        v = cVo * v;
    }

    cMoPrev = cMo;
    cMo =  vpExponentialMap::direct(v).inverse() * cMo;

    iter++;
  }

  // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
  if(computeCovariance){
    vpMatrix D;
    D.diag(W_true);

    // Note that here the covariance is computed on cMoPrev for time computation efficiency
    if(isoJoIdentity_){
        computeCovarianceMatrix(cMoPrev,m_error,L_true,D);
    }
    else{
        computeCovarianceMatrix(cMoPrev,m_error,LVJ_true,D);
    }
  }
  
  unsigned int n =0 ;
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    {
      double wmean = 0 ;
      std::list<vpMeSite>::iterator itListLine;
      if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
      
      for (unsigned int i=0 ; i < l->nbFeature ; i++){
        wmean += m_w[n+i] ;
        vpMeSite p = *itListLine;
        if (m_w[n+i] < 0.5){
          p.setState(vpMeSite::M_ESTIMATOR);
          
          *itListLine = p;
        }

        ++itListLine;
      }
      n+= l->nbFeature ;
      
      if (l->nbFeature!=0)
        wmean /= l->nbFeature ;
      else
        wmean = 1;
            
      l->setMeanWeight(wmean);

      if (wmean < 0.8)
        l->Reinit = true;
    }
  }

  // Same thing with cylinders as with lines
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    double wmean = 0 ;
    std::list<vpMeSite>::iterator itListCyl1;
    std::list<vpMeSite>::iterator itListCyl2;

    if (cy->nbFeature > 0){
      itListCyl1 = cy->meline1->getMeList().begin();
      itListCyl2 = cy->meline2->getMeList().begin();
    }

    wmean = 0;
    for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
      wmean += m_w[n+i] ;
      vpMeSite p = *itListCyl1;
      if (m_w[n+i] < 0.5){
        p.setState(vpMeSite::M_ESTIMATOR);
          
        *itListCyl1 = p;
      }

      ++itListCyl1;
    }

    if (cy->nbFeaturel1!=0)
      wmean /= cy->nbFeaturel1 ;
    else
      wmean = 1;

    cy->setMeanWeight1(wmean);

    if (wmean < 0.8){
      cy->Reinit = true;
    }

    wmean = 0;
    for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
      wmean += m_w[n+i] ;
      vpMeSite p = *itListCyl2;
      if (m_w[n+i] < 0.5){
        p.setState(vpMeSite::M_ESTIMATOR);
        
        *itListCyl2 = p;
      }

      ++itListCyl2;
    }

    if (cy->nbFeaturel2!=0)
      wmean /= cy->nbFeaturel2 ;
    else
      wmean = 1;

    cy->setMeanWeight2(wmean);

    if (wmean < 0.8){
      cy->Reinit = true;
    }

    n+= cy->nbFeature ;
  }

  // Same thing with circles as with lines
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    double wmean = 0 ;
    std::list<vpMeSite>::iterator itListCir;

    if (ci->nbFeature > 0){
      itListCir = ci->meEllipse->getMeList().begin();
    }

    wmean = 0;
    for(unsigned int i=0 ; i < ci->nbFeature ; i++){
      wmean += m_w[n+i] ;
      vpMeSite p = *itListCir;
      if (m_w[n+i] < 0.5){
        p.setState(vpMeSite::M_ESTIMATOR);

        *itListCir = p;
      }

      ++itListCir;
    }

    if (ci->nbFeature!=0)
      wmean /= ci->nbFeature ;
    else
      wmean = 1;

    ci->setMeanWeight(wmean);

    if (wmean < 0.8){
      ci->Reinit = true;
    }

    n+= ci->nbFeature ;
  }
}

void
vpMbEdgeTracker::testTracking()
{
  int nbExpectedPoint = 0;
  int nbGoodPoint = 0;
  int nbBadPoint = 0;
  
  vpMbtDistanceLine *l ;
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    if (l->isVisible() && l->meline != NULL)
    {
      nbExpectedPoint += (int)l->meline->expecteddensity;
      for(std::list<vpMeSite>::const_iterator itme=l->meline->getMeList().begin(); itme!=l->meline->getMeList().end(); ++itme){
        vpMeSite pix = *itme;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
        else nbBadPoint++;
      }
    }
  }    

  vpMbtDistanceCylinder *cy ;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    if ((cy->meline1 !=NULL && cy->meline2 != NULL) && cy->isVisible())
    {
      nbExpectedPoint += (int)cy->meline1->expecteddensity;
      for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
        vpMeSite pix = *itme1;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
        else nbBadPoint++;
      }
      nbExpectedPoint += (int)cy->meline2->expecteddensity;
      for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
        vpMeSite pix = *itme2;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
        else nbBadPoint++;
      }
    }
  }

  vpMbtDistanceCircle *ci ;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    if (ci->isVisible() && ci->meEllipse !=NULL)
    {
      nbExpectedPoint += ci->meEllipse->getExpectedDensity();
      for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
        vpMeSite pix = *itme;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
        else nbBadPoint++;
      }
    }
  }

  // Compare the number of good points with the min between the number of expected points and
  // number of points that are tracked
  int nb_min = (int)vpMath::minimum(percentageGdPt *nbExpectedPoint, percentageGdPt *(nbGoodPoint + nbBadPoint) );
  //int nb_min = std::min(val1, val2);
  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
    std::ostringstream oss;
    oss << "Not enough moving edges ("
        << nbGoodPoint
        << ") to track the object: expected "
        << nb_min
        << ". Try to reduce the threshold="
        << percentageGdPt
        << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
    throw vpTrackingException(vpTrackingException::fatalError, oss.str());
  }      
}

void
vpMbEdgeTracker::track(const vpImage<unsigned char> &I)
{ 
  initPyramid(I, Ipyramid);
  
//  for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
  unsigned int lvl = (unsigned int)scales.size();
  do{
    lvl--;
    if(scales[lvl]){
      vpHomogeneousMatrix cMo_1 = cMo;
      try
      {
        downScale(lvl);

        try
        {  
          trackMovingEdge(*Ipyramid[lvl]);
        }
        catch(...)
        {
          vpTRACE("Error in moving edge tracking") ;
          throw ;
        }

        // initialize the vector that contains the error and the matrix that contains
        // the interaction matrix
        vpMbtDistanceLine *l ;
        for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
          l = *it;
          if (l->isVisible()){
            l->initInteractionMatrixError() ;
          }
        }  

        vpMbtDistanceCylinder *cy ;
        for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
          cy = *it;
          if(cy->isVisible()) {
            cy->initInteractionMatrixError();
          }
        }

        vpMbtDistanceCircle *ci ;
        for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
          ci = *it;
          if (ci->isVisible()){
            ci->initInteractionMatrixError();
          }
        }

        try
        {
          computeVVS(*Ipyramid[lvl]);
        }
        catch(vpException &e)
        {
          covarianceMatrix = -1;
          throw e;
        }

        try
        {
          testTracking();
        }
        catch(vpException &e)
        {
          throw e;
        }

        if (displayFeatures)
        {
          if(lvl == 0){
            for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
              l = *it;
              if (l->isVisible()){
                l->displayMovingEdges(I);
              }
            }

            for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
              cy = *it;
              if(cy->isVisible()) {
                cy->displayMovingEdges(I);
              }
            }

            for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
              ci = *it;
              if (ci->isVisible()){
                ci->displayMovingEdges(I);
              }
            }
          }
        }

        try
        {
          updateMovingEdge(I);
        }
        catch(vpException &e)
        {
          throw e;
        }

        // Looking for new visible face
        bool newvisibleface = false ;
        visibleFace(I, cMo, newvisibleface) ;
        initMovingEdge(I,cMo) ;

        // Reinit the moving edge for the lines which need it.
        reinitMovingEdge(I,cMo);
        upScale(lvl);
      }
      catch(vpException &e)
      {
        if(lvl != 0){
          cMo = cMo_1;
          reInitLevel(lvl);
          upScale(lvl);
        }
        else{
          upScale(lvl);
          throw(e) ;
        }
      }
    }
  } while(lvl != 0);
  
  cleanPyramid(Ipyramid);
}

void vpMbEdgeTracker::init(const vpImage<unsigned char>& I)
{
  if(!modelInitialised){
    throw vpException(vpException::fatalError, "model not initialized");
  }

    bool a = false;

#ifdef VISP_HAVE_OGRE 
  if(useOgre){
    if(!faces.isOgreInitialised()){
      faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
      faces.initOgre(cam);
    }
  }
#endif
  
  
  initPyramid(I, Ipyramid);
  visibleFace(I, cMo, a);
  unsigned int i = (unsigned int)scales.size();
  
  do {
    i--;
    if(scales[i]){
      downScale(i);
      initMovingEdge(*Ipyramid[i], cMo);
      upScale(i);
    }
  } while(i != 0);
  
  cleanPyramid(Ipyramid);
}

void           
vpMbEdgeTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
{
  cMo = cdMo;
  
  if (! lines[scaleLevel].empty()) {
    lines[scaleLevel].front() ;
    for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
      if((*it)->meline != NULL){
        delete (*it)->meline;
        (*it)->meline = NULL;
      }
    }
  }

  if (! cylinders[scaleLevel].empty()) {
    cylinders[scaleLevel].front() ;
    for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
      if((*it)->meline1 != NULL){
        delete (*it)->meline1;
        (*it)->meline1 = NULL;
      }
      if((*it)->meline2 != NULL){
        delete (*it)->meline2;
        (*it)->meline2 = NULL;
      }
    }
  }

  if (! circles[scaleLevel].empty()) {
    circles[scaleLevel].front() ;
    for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
      if((*it)->meEllipse != NULL){
        delete (*it)->meEllipse;
        (*it)->meEllipse = NULL;
      }
    }
  }

  init(I);
}

void 
vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
{
  vpMbEdgeTracker::loadConfigFile(configFile.c_str());
}

void
vpMbEdgeTracker::loadConfigFile(const char* configFile)
{
#ifdef VISP_HAVE_XML2
  vpMbtXmlParser xmlp;
  
  xmlp.setCameraParameters(cam);
  xmlp.setAngleAppear(vpMath::deg(angleAppears));
  xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
  xmlp.setMovingEdge(me);

  try{
    std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
    xmlp.parse(configFile);
  }
  catch(...){
    throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile);
  }
  
  vpCameraParameters camera;
  vpMe meParser;
  xmlp.getCameraParameters(camera);
  xmlp.getMe(meParser);
  
  setCameraParameters(camera);
  setMovingEdge(meParser);
  angleAppears = vpMath::rad(xmlp.getAngleAppear());
  angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
  
  if(xmlp.hasNearClippingDistance())
    setNearClippingDistance(xmlp.getNearClippingDistance());
  
  if(xmlp.hasFarClippingDistance())
    setFarClippingDistance(xmlp.getFarClippingDistance());
  
  if(xmlp.getFovClipping())
    setClipping(clippingFlag | vpMbtPolygon::FOV_CLIPPING);

  useLodGeneral = xmlp.getLodState();
  minLineLengthThresholdGeneral = xmlp.getMinLineLengthThreshold();
  minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();

  applyLodSettingInConfig = false;
  if(this->getNbPolygon() > 0) {
    applyLodSettingInConfig = true;
    setLod(useLodGeneral);
    setMinLineLengthThresh(minLineLengthThresholdGeneral);
    setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
  }

#else
  vpTRACE("You need the libXML2 to read the config file %s", configFile);
#endif
}


void
vpMbEdgeTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
                         const vpCameraParameters &camera, const vpColor& col,
                         const unsigned int thickness, const bool displayFullModel)
{  
  for (unsigned int i = 0; i < scales.size(); i += 1){
    if(scales[i]){
      for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
        (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
      }

      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
        (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
      }

      for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
        (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
      }

      break ; //displaying model on one scale only
    }
  }
  
#ifdef VISP_HAVE_OGRE
  if(useOgre)
    faces.displayOgre(cMo_);
#endif
}

void
vpMbEdgeTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_,
                         const vpCameraParameters &camera, const vpColor& col,
                         const unsigned int thickness, const bool displayFullModel)
{
  for (unsigned int i = 0; i < scales.size(); i += 1){
    if(scales[i]){
      for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
        (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
      }

      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
        (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
      }

      for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
        (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
      }
      break ; //displaying model on one scale only
    }
  }
  
#ifdef VISP_HAVE_OGRE
  if(useOgre)
    faces.displayOgre(cMo_);
#endif
}


void
vpMbEdgeTracker::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
{  
  vpMbtDistanceLine *l ;

  lines[scaleLevel].front() ;
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    bool isvisible = false ;

    for(std::list<int>::const_iterator itindex=l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
      int index = *itindex;
      if (index ==-1) isvisible =true ;
      else
      {
        if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
      }
    }

    //Si la ligne n'appartient a aucune face elle est tout le temps visible
    if (l->Lindex_polygon.empty()) isvisible = true; // Not sure that this can occur

    if (isvisible)
    {
      l->setVisible(true) ;
      if (l->meline==NULL)
      {
//         std::cout << "init me line "<< l->getIndex() << std::endl ;
        l->initMovingEdge(I, _cMo) ;
      }
    }
    else
    {
      l->setVisible(false) ;
      if (l->meline!=NULL) delete l->meline;
      l->meline=NULL;
    }
  }   

  vpMbtDistanceCylinder *cy ;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;

    bool isvisible = false ;

    int index = cy->index_polygon;
    if (index ==-1) isvisible =true ;
    else
    {
      if (cy->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
    }
//    vpTRACE("cyl with index %d is visible: %d", index, isvisible);

    if (isvisible)
    {
      cy->setVisible(true) ;
      if (cy->meline1==NULL || cy->meline2==NULL){
        cy->initMovingEdge(I, _cMo) ;
      }
    }
    else
    {
      cy->setVisible(false) ;
      if (cy->meline1!=NULL) delete cy->meline1;
      if (cy->meline2!=NULL) delete cy->meline2;
      cy->meline1=NULL;
      cy->meline2=NULL;
    }
  }

  vpMbtDistanceCircle *ci ;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    bool isvisible = false ;

    int index = ci->index_polygon;
    if (index ==-1) isvisible =true ;
    else
    {
      if (ci->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
    }

    if (isvisible)
    {
      ci->setVisible(true) ;
      if (ci->meEllipse==NULL)
      {
        ci->initMovingEdge(I, _cMo) ;
      }
    }
    else
    {
      ci->setVisible(false) ;
      if (ci->meEllipse!=NULL) delete ci->meEllipse;
      ci->meEllipse=NULL;
    }
  }
}


void
vpMbEdgeTracker::trackMovingEdge(const vpImage<unsigned char> &I)
{
  vpMbtDistanceLine *l ;
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    if(l->isVisible() == true){
      if(l->meline == NULL){
        l->initMovingEdge(I, cMo);
      }
      l->trackMovingEdge(I, cMo) ;
    }
  }

  vpMbtDistanceCylinder *cy;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    if(cy->isVisible()) {
        if(cy->meline1 == NULL || cy->meline2 == NULL){
          cy->initMovingEdge(I, cMo);
        }
        cy->trackMovingEdge(I, cMo) ;
    }
  }

  vpMbtDistanceCircle *ci;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    if(ci->isVisible() == true){
      if(ci->meEllipse == NULL){
        ci->initMovingEdge(I, cMo);
      }
      ci->trackMovingEdge(I, cMo) ;
    }
  }
}


void
vpMbEdgeTracker::updateMovingEdge(const vpImage<unsigned char> &I)
{
  vpMbtDistanceLine *l ;
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    l->updateMovingEdge(I, cMo) ;
    if (l->nbFeature == 0 && l->isVisible()){
      l->Reinit = true;
    }
  }

  vpMbtDistanceCylinder *cy ;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    cy->updateMovingEdge(I, cMo) ;
    if((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()){
      cy->Reinit = true;
    }
  }

  vpMbtDistanceCircle *ci ;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    ci->updateMovingEdge(I, cMo) ;
    if(ci->nbFeature == 0  && ci->isVisible()){
      ci->Reinit = true;
    }
  }
}


void
vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
{
  vpMbtDistanceLine *l ;  
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    if (l->Reinit && l->isVisible())
      l->reinitMovingEdge(I, _cMo);
  }

  vpMbtDistanceCylinder*cy;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    if (cy->Reinit && cy->isVisible())
      cy->reinitMovingEdge(I, _cMo);
  }

  vpMbtDistanceCircle*ci;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    if (ci->Reinit && ci->isVisible())
      ci->reinitMovingEdge(I, _cMo);
  }
}


bool
vpMbEdgeTracker::samePoint(const vpPoint &P1, const vpPoint &P2)
{
  double dx = fabs(P1.get_oX() - P2.get_oX());
  double dy = fabs(P1.get_oY() - P2.get_oY());
  double dz = fabs(P1.get_oZ() - P2.get_oZ());

  if (dx  <= std::numeric_limits<double>::epsilon() && dy  <= std::numeric_limits<double>::epsilon() && dz <= std::numeric_limits<double>::epsilon())
    return true ;
  else
    return false ;
}


void
vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
{
  //suppress line already in the model
  bool already_here = false ;
  vpMbtDistanceLine *l ;
  
  for (unsigned int i = 0; i < scales.size(); i += 1){
    if(scales[i]){
      downScale(i);
      for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
        l = *it;
        if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
           (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
          already_here = true ;
          l->Lindex_polygon.push_back(polygon);
          l->hiddenface = &faces ;
        }
      }

      if (!already_here){
        l = new vpMbtDistanceLine ;

        l->setCameraParameters(cam) ;
        l->buildFrom(P1,P2) ;
        l->Lindex_polygon.push_back(polygon);
        l->setMovingEdge(&me) ;
        l->hiddenface = &faces ;

        l->setIndex(nline) ;
        l->setName(name);
        
        if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
          l->getPolygon().setClipping(clippingFlag);
        
        if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
          l->getPolygon().setNearClippingDistance(distNearClip);
        
        if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
          l->getPolygon().setFarClippingDistance(distFarClip);
        
        nline +=1 ;
        lines[i].push_back(l);
      }
      upScale(i);
    }
  }
}

void
vpMbEdgeTracker::removeLine(const std::string& name)
{
  vpMbtDistanceLine *l;
  
  for(unsigned int i=0; i<scales.size(); i++){
    if(scales[i]){
      for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
        l = *it;
        if (name.compare(l->getName()) == 0){
          lines[i].erase(it);
          break;
        }
      }
    }
  }
}

void
vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace, const std::string& name)
{
  bool already_here = false ;
  vpMbtDistanceCircle *ci ;

  for (unsigned int i = 0; i < scales.size(); i += 1){
    if(scales[i]){
      downScale(i);
      for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
        ci = *it;
        if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
           (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
          already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
        }
      }

      if (!already_here){
        ci = new vpMbtDistanceCircle ;

        ci->setCameraParameters(cam);
        ci->buildFrom(P1, P2, P3, r);
        ci->setMovingEdge(&me);
        ci->setIndex(ncircle);
        ci->setName(name);
        ci->index_polygon = idFace;
        ci->hiddenface = &faces ;

//        if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
//          ci->getPolygon().setClipping(clippingFlag);

//        if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
//          ci->getPolygon().setNearClippingDistance(distNearClip);

//        if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
//          ci->getPolygon().setFarClippingDistance(distFarClip);

        ncircle +=1;
        circles[i].push_back(ci);
      }
      upScale(i);
    }
  }
}

void
vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace, const std::string& name)
{
  bool already_here = false ;
  vpMbtDistanceCylinder *cy ;

  for (unsigned int i = 0; i < scales.size(); i += 1){
    if(scales[i]){
      downScale(i);
      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
        cy = *it;
        if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
           (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
          already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
        }
      }

      if (!already_here){
        cy = new vpMbtDistanceCylinder ;

        cy->setCameraParameters(cam);
        cy->buildFrom(P1, P2, r);
        cy->setMovingEdge(&me);
        cy->setIndex(ncylinder);
        cy->setName(name);
        cy->index_polygon = idFace;
        cy->hiddenface = &faces ;
        ncylinder +=1;
        cylinders[i].push_back(cy);
      }
      upScale(i);
    }
  }
}

void
vpMbEdgeTracker::removeCylinder(const std::string& name)
{
  vpMbtDistanceCylinder *cy;

  for(unsigned int i=0; i<scales.size(); i++){
    if(scales[i]){
      for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
        cy = *it;
        if (name.compare(cy->getName()) == 0){
          cylinders[i].erase(it);
          break;
        }
      }
    }
  }
}

void
vpMbEdgeTracker::removeCircle(const std::string& name)
{
  vpMbtDistanceCircle *ci;

  for(unsigned int i=0; i<scales.size(); i++){
    if(scales[i]){
      for(std::list<vpMbtDistanceCircle*>::iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
        ci = *it;
        if (name.compare(ci->getName()) == 0){
          circles[i].erase(it);
          break;
        }
      }
    }
  }
}

void
vpMbEdgeTracker::addPolygon(vpMbtPolygon &p)
{
  unsigned int nbpt = p.getNbPoint() ;
  if(nbpt > 0){
    for (unsigned int i=0 ; i < nbpt-1 ; i++)
      addLine(p.p[i], p.p[i+1], p.getIndex()) ;
    addLine(p.p[nbpt-1], p.p[0], p.getIndex()) ;
  }
}


#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

void
vpMbEdgeTracker::visibleFace(const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
{
  unsigned int n ;

  if(!useOgre)
    n = faces.setVisible(_cMo) ;
  else{
#ifdef VISP_HAVE_OGRE   
    bool changed = false;
    n = faces.setVisibleOgre(_cMo, vpMath::rad(70), vpMath::rad(70), changed);
#else
    n = faces.setVisible(_cMo) ;
#endif
  } 
  
//  cout << "visible face " << n << endl ;
  if (n > nbvisiblepolygone)
  {
    //cout << "une nouvelle face est visible " << endl ;
    newvisibleline = true ;
  }
  else
    newvisibleline = false ;

  nbvisiblepolygone= n ;
}
#endif //VISP_BUILD_DEPRECATED_FUNCTIONS

void
vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> & _I,
                             const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
{
  unsigned int n ;
  bool changed = false;

  if(!useOgre) {
    //n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
    n = faces.setVisible(_I, cam, _cMo,  angleAppears, angleDisappears, changed) ;
  }
  else{
#ifdef VISP_HAVE_OGRE
    n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
#else
    n = faces.setVisible(_I, cam, _cMo,  angleAppears, angleDisappears, changed) ;
#endif
  } 

  if (n > nbvisiblepolygone)
  {
    //cout << "une nouvelle face est visible " << endl ;
    newvisibleline = true ;
  }
  else
    newvisibleline = false ;

  nbvisiblepolygone= n ;
}

void
vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon)
{
  unsigned int nbpt = polygon.getNbPoint() ;
  if(nbpt > 0){
    for (unsigned int i=0 ; i < nbpt-1 ; i++)
      addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
    addLine(polygon.p[nbpt-1], polygon.p[0], polygon.getIndex(), polygon.getName());
  }
}
void
vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon)
{
  unsigned int nbpt = polygon.getNbPoint() ;
  if(nbpt > 0){
    for (unsigned int i=0 ; i < nbpt-1 ; i++)
      addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
  }
}

void
vpMbEdgeTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
                            const int idFace, const std::string &name)
{
  addCircle(p1, p2, p3, radius, (int)idFace, name);
}

void
vpMbEdgeTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
    const std::string &name)
{
  addCylinder(p1, p2, radius, (int)idFace, name);
}

void
vpMbEdgeTracker::resetTracker()
{
  this->cMo.setIdentity();
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  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){
        l = *it;
        if (l!=NULL) delete l ;
        l = NULL ;
      }

      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
        cy = *it;
        if (cy!=NULL) delete cy;
        cy = NULL;
      }

      for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
        ci = *it;
        if (ci!=NULL) delete ci;
        ci = NULL;
      }
      lines[i].clear();
      cylinders[i].clear();
      circles[i].clear();
    }
  }

  faces.reset();

#ifdef VISP_HAVE_OGRE
  useOgre = false;
#endif
  
  compute_interaction=1;
  nline = 0;
  ncylinder = 0;
  lambda = 1;
  nbvisiblepolygone = 0;
  percentageGdPt = 0.4;
  
  angleAppears = vpMath::rad(89);
  angleDisappears = vpMath::rad(89);
  clippingFlag = vpMbtPolygon::NO_CLIPPING;

  // reinitialization of the scales.
  this->setScales(scales);
}

void
vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
                             const vpHomogeneousMatrix& cMo_, const bool verbose)
{
  reInitModel(I, cad_name.c_str(), cMo_, verbose);
}

void
vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
                             const vpHomogeneousMatrix& cMo_, const bool verbose)
{
  this->cMo.setIdentity();
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  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){
        l = *it;
        if (l!=NULL) delete l ;
        l = NULL ;
      }

      for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
        cy = *it;
        if (cy!=NULL) delete cy;
        cy = NULL;
      }

      for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
        ci = *it;
        if (ci!=NULL) delete ci;
        ci = NULL;
      }

      lines[i].clear();
      cylinders[i].clear();
      circles[i].clear();
    }
  }

  faces.reset();

  //compute_interaction=1;
  nline = 0;
  ncylinder = 0;
  ncircle = 0;
  //lambda = 1;
  nbvisiblepolygone = 0;

  loadModel(cad_name, verbose);
  initFromPose(I, cMo_);
}

unsigned int 
vpMbEdgeTracker::getNbPoints(const unsigned int level) const
{
  if((level > scales.size()) || !scales[level]){
    throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used", level);
  }
  
  unsigned int nbGoodPoints = 0;
  vpMbtDistanceLine *l ;
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
    l = *it;
    if (l->isVisible() && l->meline != NULL)
    {
      for(std::list<vpMeSite>::const_iterator itme=l->meline->getMeList().begin(); itme!=l->meline->getMeList().end(); ++itme){
        if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
      }
    }
  }

  vpMbtDistanceCylinder *cy ;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
    cy = *it;
    if (cy->isVisible() && (cy->meline1 != NULL || cy->meline2 != NULL))
    {
      for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
        if (itme1->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
      }
      for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
        if (itme2->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
      }
    }
  }

  vpMbtDistanceCircle *ci ;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[level].begin(); it!=circles[level].end(); ++it){
    ci = *it;
    if (ci->isVisible() && ci->meEllipse != NULL)
    {
      for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
        if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
      }
    }
  }

  return nbGoodPoints;
}

void 
vpMbEdgeTracker::setScales(const std::vector<bool>& scale)
{
  unsigned int nbActivatedLevels = 0;
  for (unsigned int i = 0; i < scale.size(); i += 1){
    if(scale[i]){
      nbActivatedLevels++;
    }
  }
  if((scale.size() < 1) || (nbActivatedLevels == 0)){
    vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
    this->scales.resize(0);
    this->scales.push_back(true);
    lines.resize(1);
    lines[0].clear();
    cylinders.resize(1);
    cylinders[0].clear();
  }
  else{
    this->scales = scale;
    lines.resize(scale.size());
    cylinders.resize(scale.size());
    for (unsigned int i = 0; i < lines.size(); i += 1){
      lines[i].clear();
      cylinders[i].clear();
    }
  }
}

void            
vpMbEdgeTracker::setFarClippingDistance(const double &dist) 
{ 
  if( (clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING && dist <= distNearClip)
    vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
  else if ( dist < 0 ) 
    vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
  else{  
    vpMbTracker::setFarClippingDistance(dist);
    vpMbtDistanceLine *l;

    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){
          l = *it;
          l->getPolygon().setFarClippingDistance(distFarClip);
        }
      }
    }  
  }
}

void           
vpMbEdgeTracker::setNearClippingDistance(const double &dist) 
{ 
  if( (clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING && dist >= distFarClip)
    vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
  else if ( dist < 0 ) 
    vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
  else{
    vpMbTracker::setNearClippingDistance(dist);
    vpMbtDistanceLine *l;

    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){
          l = *it;
          l->getPolygon().setNearClippingDistance(distNearClip);
        }
      }
    }
  }
}

void            
vpMbEdgeTracker::setClipping(const unsigned int &flags) 
{ 
  vpMbTracker::setClipping(flags);
  
  vpMbtDistanceLine *l;

  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){
        l = *it;
        l->getPolygon().setClipping(clippingFlag);
      }
    }
  }
}

void 
vpMbEdgeTracker::initPyramid(const vpImage<unsigned char>& _I, std::vector< const vpImage<unsigned char>* >& _pyramid)
{
  _pyramid.resize(scales.size());
  
  if(scales[0]){
    _pyramid[0] = &_I;
  }
  else{
    _pyramid[0] = NULL;
  }
  
  for(unsigned int i=1; i<_pyramid.size(); i += 1){
    if(scales[i]){
      unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
      vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
      IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
      vpI0->imageData = (char*)(_I.bitmap);
      IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
      cvResize(vpI0, vpI, CV_INTER_NN);
      vpImageConvert::convert(vpI, *I);
      cvReleaseImage(&vpI);  
      vpI0->imageData = NULL;
      cvReleaseImageHeader(&vpI0);    
#else
      for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale){
        for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale){
          (*I)[k][l] = _I[ii][jj];
        }
      }
#endif   
      _pyramid[i] = I;
    }
    else{
      _pyramid[i] = NULL;
    }
  }
}

void 
vpMbEdgeTracker::cleanPyramid(std::vector< const vpImage<unsigned char>* >& _pyramid)
{
  if(_pyramid.size() > 0){
    _pyramid[0] = NULL;
    for (unsigned int i = 1; i < _pyramid.size(); i += 1){
      if(_pyramid[i] != NULL){
        delete _pyramid[i];
        _pyramid[i] = NULL;
      }
    }
    _pyramid.resize(0);
  }
}

void
vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level)
{
  if(level > scales.size() || !scales[level]){
    std::ostringstream oss;
    oss << level;
    std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
    throw vpException(vpException::dimensionError, errorMsg);
  }

  linesList = lines[level];
}


void
vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level)
{
  if(level > scales.size() || !scales[level]){
    std::ostringstream oss;
    oss << level;
    std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
    throw vpException(vpException::dimensionError, errorMsg);
  }

  cylindersList = cylinders[level];
}


void
vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level)
{
  if(level > scales.size() || !scales[level]){
    std::ostringstream oss;
    oss << level;
    std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
    throw vpException(vpException::dimensionError, errorMsg);
  }

  circlesList = circles[level];
}

void 
vpMbEdgeTracker::downScale(const unsigned int _scale)
{
  const double ratio = pow(2., (int)_scale);
  scaleLevel = _scale;
  
  vpMatrix K = cam.get_K();
  
  K[0][0] /= ratio;
  K[1][1] /= ratio;
  K[0][2] /= ratio;
  K[1][2] /= ratio;

  cam.initFromCalibrationMatrix(K);
}

void 
vpMbEdgeTracker::upScale(const unsigned int _scale)
{
  const double ratio = pow(2., (int)_scale);
  scaleLevel = 0;
  
  vpMatrix K = cam.get_K();
  
  K[0][0] *= ratio;
  K[1][1] *= ratio;
  K[0][2] *= ratio;
  K[1][2] *= ratio;


  cam.initFromCalibrationMatrix(K);
}

void 
vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
{
  unsigned int scaleLevel_1 = scaleLevel;
  scaleLevel = _lvl;

  vpMbtDistanceLine *l;  
  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
    l = *it;
    l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
  }

  vpMbtDistanceCylinder *cy;
  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
    cy = *it;
    cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
  }
  
  vpMbtDistanceCircle *ci;
  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
    ci = *it;
    ci->reinitMovingEdge(*Ipyramid[_lvl], cMo);
  }

  trackMovingEdge(*Ipyramid[_lvl]);
  updateMovingEdge(*Ipyramid[_lvl]);
  scaleLevel = scaleLevel_1;
}