vpMbtDistanceCircle.cpp

/****************************************************************************
 *
 * 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. Circle
 * tracking.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpConfig.h>

#include <stdlib.h>
#include <algorithm>

#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/core/vpPlane.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/vision/vpPose.h>

vpMbtDistanceCircle::vpMbtDistanceCircle()
  : name(), index(0), cam(), me(NULL), wmean(1),
    featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
    circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
    L(), error(), nbFeature(0), Reinit(false),
    hiddenface(NULL), index_polygon(-1), isvisible(false)
{
}

vpMbtDistanceCircle::~vpMbtDistanceCircle()
{
  if (meEllipse != NULL) delete meEllipse;
  if (circle != NULL) delete circle;
  if (p1 != NULL) delete p1;
  if (p2 != NULL) delete p2;
  if (p3 != NULL) delete p3;
}

void
vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo)
{
  circle->project(cMo) ;
}


void
vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
{
  circle = new vpCircle ;
  p1 = new vpPoint ;
  p2 = new vpPoint ;
  p3 = new vpPoint ;

  // Get the points
  *p1 = _p1;
  *p2 = _p2;
  *p3 = _p3;

  // Get the radius
  radius = r;

  vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);

  // Build our circle
  circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
}


void
vpMbtDistanceCircle::setMovingEdge(vpMe *_me)
{
  me = _me ;
  if (meEllipse != NULL)
  {
    meEllipse->setMe(me) ;
  }
}

bool
vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  if(isvisible){
    // Perspective projection
    circle->changeFrame(cMo);

    try{
      circle->projection();
    }
    catch(...){
      std::cout<<"Problem when projecting circle\n";
      return false;
    }

    // Create the moving edges containers
    meEllipse = new vpMbtMeEllipse;
    meEllipse->setMe(me) ;

    //meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
    meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines

    try
    {
      vpImagePoint ic;
      double mu20_p, mu11_p, mu02_p;
      vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
      meEllipse->initTracking(I, ic, mu20_p, mu11_p, mu02_p);
    }
    catch(...)
    {
      //vpTRACE("the circle can't be initialized");
      return false;
    }
  }
  return true;
}

void
vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
{
  if(isvisible){
    try
    {
      meEllipse->track(I) ;
    }
    catch(...)
    {
      //std::cout << "Track meEllipse failed" << std::endl;
      meEllipse->reset();
      Reinit = true;
    }

    // Update the number of features
    nbFeature = (unsigned int)meEllipse->getMeList().size();
  }
}


void
vpMbtDistanceCircle::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  if(isvisible){
    // Perspective projection
    circle->changeFrame(cMo);

    try{
      circle->projection();
    }
    catch(...){std::cout<<"Problem when projecting circle\n";}

    try
    {

      vpImagePoint ic;
      double mu20_p, mu11_p, mu02_p;
      vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
      meEllipse->updateParameters(I, ic, mu20_p, mu11_p, mu02_p);
    }
    catch(...)
    {
      Reinit = true;
    }
    nbFeature = (unsigned int)meEllipse->getMeList().size();
  }
}


void
vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  if(meEllipse!= NULL)
    delete meEllipse;
  
  meEllipse = NULL;

  if(initMovingEdge(I,cMo) == false)
    Reinit = true;

  Reinit = false;
}


void
vpMbtDistanceCircle::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
                             const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
                             const bool displayFullModel )
{
  if( (isvisible && isTrackedCircle) || displayFullModel){
    // Perspective projection
    circle->changeFrame(cMo);

    try{
      circle->projection();
    }
    catch(...){std::cout<<"Cannot project the circle";}

    vpImagePoint center;
    double mu20_p, mu11_p, mu02_p;
    vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
    vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
  }
}

void
vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                             const vpCameraParameters &camera, const vpColor col,
                             const unsigned int thickness, const bool displayFullModel)
{
  if( (isvisible && isTrackedCircle) || displayFullModel){
    // Perspective projection
    circle->changeFrame(cMo);

    try{
      circle->projection();
    }
    catch(...){std::cout<<"Cannot project the circle";}

    vpImagePoint center;
    double mu20_p, mu11_p, mu02_p;
    vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
    vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
  }
}


void
vpMbtDistanceCircle::displayMovingEdges(const vpImage<unsigned char> &I)
{
  if (meEllipse != NULL)
  {
    meEllipse->display(I); // display the me
    if (vpDEBUG_ENABLE(3))
      vpDisplay::flush(I);
  }
}

void
vpMbtDistanceCircle::initInteractionMatrixError()
{
  if (isvisible == true)
  {
    nbFeature = (unsigned int)meEllipse->getMeList().size();
    L.resize(nbFeature, 6);
    error.resize(nbFeature);
  }
  else
    nbFeature = 0 ;
}

void
vpMbtDistanceCircle::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
{
  if (isvisible)
  {
    // Perspective projection
    circle->changeFrame(cMo) ;
    try{
      circle->projection();
    }
    catch(...){std::cout<<"Problem projection circle\n";}

    vpFeatureBuilder::create(featureEllipse, *circle);

    vpMatrix H1 = featureEllipse.interaction();

    vpRowVector H(5);
    double x=0, y=0;

    // Get the parameters of the ellipse in the image plane
    double xg = circle->p[0];
    double yg = circle->p[1];
    double mu20 = circle->p[2];
    double mu11 = circle->p[3];
    double mu02 = circle->p[4];

    unsigned int j = 0;

    for(std::list<vpMeSite>::const_iterator it=meEllipse->getMeList().begin(); it!=meEllipse->getMeList().end(); ++it){
      vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
      H[0] = 2*(mu11*(y-yg)+mu02*(xg-x));
      H[1] = 2*(mu20*(yg-y)+mu11*(x-xg));
      H[2] = vpMath::sqr(y-yg)-mu02;
      H[3] = 2*(yg*(x-xg)+y*xg+mu11-x*y);
      H[4] = vpMath::sqr(x-xg)-mu20;

      for (unsigned int k=0; k<6; k++)
        L[j][k] = H[0]*H1[0][k] + H[1]*H1[1][k] + H[2]*H1[2][k] + H[3]*H1[3][k]  + H[4]*H1[4][k];

      error[j] = mu02*vpMath::sqr(x) + mu20*vpMath::sqr(y) - 2*mu11*x*y
          + 2*(mu11*yg-mu02*xg)*x + 2*(mu11*xg-mu20*yg)*y
          + mu02*vpMath::sqr(xg) + mu20*vpMath::sqr(yg) - 2*mu11*xg*yg
          + vpMath::sqr(mu11) - mu20*mu02;

      j++;
    }
  }
}