vpMbtDistanceCircle.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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 <algorithm>
#include <stdlib.h>

#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPlane.h>
#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/vision/vpPose.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.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, 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, bool doNotTrack,
                                         const vpImage<bool> *mask)
{
  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->setMask(*mask);
    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 n20_p, n11_p, n02_p;
      vpMeterPixelConversion::convertEllipse(cam, *circle, ic, n20_p, n11_p, n02_p);
      meEllipse->initTracking(I, ic, n20_p, n11_p, n02_p, doNotTrack);
    } 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 n20_p, n11_p, n02_p;
      vpMeterPixelConversion::convertEllipse(cam, *circle, ic, n20_p, n11_p, n02_p);
      meEllipse->updateParameters(I, ic, n20_p, n11_p, n02_p);
    } catch (...) {
      Reinit = true;
    }
    nbFeature = (unsigned int)meEllipse->getMeList().size();
  }
}

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

  meEllipse = NULL;

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

  Reinit = false;
}

void vpMbtDistanceCircle::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
                                  bool displayFullModel)
{
  std::vector<double> params = getModelForDisplay(cMo, camera, displayFullModel);

  vpImagePoint center(params[0], params[1]);
  double n20_p = params[2];
  double n11_p = params[3];
  double n02_p = params[4];
  vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, col, thickness);
}

void vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                                  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
                                  bool displayFullModel)
{
  std::vector<double> params = getModelForDisplay(cMo, camera, displayFullModel);

  vpImagePoint center(params[1], params[2]);
  double n20_p = params[3];
  double n11_p = params[4];
  double n02_p = params[5];
  vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, col, thickness);
}

std::vector<std::vector<double> > vpMbtDistanceCircle::getFeaturesForDisplay()
{
  std::vector<std::vector<double> > features;

  if (meEllipse != NULL) {
    for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end(); ++it) {
      vpMeSite p_me = *it;
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
      std::vector<double> params = {0, //ME
                                    p_me.get_ifloat(),
                                    p_me.get_jfloat(),
                                    static_cast<double>(p_me.getState())};
#else
      std::vector<double> params;
      params.push_back(0); //ME
      params.push_back(p_me.get_ifloat());
      params.push_back(p_me.get_jfloat());
      params.push_back(static_cast<double>(p_me.getState()));
#endif
      features.push_back(params);
    }
  }

  return features;
}

std::vector<double> vpMbtDistanceCircle::getModelForDisplay(const vpHomogeneousMatrix &cMo,
                                                            const vpCameraParameters &camera,
                                                            bool displayFullModel)
{
  std::vector<double> params;

  if ((isvisible && isTrackedCircle) || displayFullModel) {
    // Perspective projection
    circle->changeFrame(cMo);

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

    vpImagePoint center;
    double n20_p, n11_p, n02_p;
    vpMeterPixelConversion::convertEllipse(camera, *circle, center, n20_p, n11_p, n02_p);
    params.push_back(1); //1 for ellipse parameters
    params.push_back(center.get_i());
    params.push_back(center.get_j());
    params.push_back(n20_p);
    params.push_back(n11_p);
    params.push_back(n02_p);
  }

  return params;
}

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::displayMovingEdges(const vpImage<vpRGBa> &I)
{
  if (meEllipse != NULL) {
    meEllipse->display(I); // display the me
    if (vpDEBUG_ENABLE(3))
      vpDisplay::flush(I);
  }
}

void vpMbtDistanceCircle::initInteractionMatrixError()
{
  if (isvisible) {
    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 n20 = circle->p[2];
    double n11 = circle->p[3];
    double n02 = 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 * (n11 * (y - yg) + n02 * (xg - x));
      H[1] = 2 * (n20 * (yg - y) + n11 * (x - xg));
      H[2] = vpMath::sqr(y - yg) - n02;
      H[3] = 2 * (yg * (x - xg) + y * xg + n11 - x * y);
      H[4] = vpMath::sqr(x - xg) - n20;

      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] = n02 * vpMath::sqr(x) + n20 * vpMath::sqr(y) - 2 * n11 * x * y + 2 * (n11 * yg - n02 * xg) * x +
                 2 * (n11 * xg - n20 * yg) * y + n02 * vpMath::sqr(xg) + n20 * vpMath::sqr(yg) -
                 2 * n11 * xg * yg + vpMath::sqr(n11) - n20 * n02;

      j++;
    }
  }
}