vpMbtDistanceCircle.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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 https://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.
 */
 
#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>
 
BEGIN_VISP_NAMESPACE
namespace
{
const unsigned int defaultRange = 1U;
}

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

vpMbtDistanceCircle::~vpMbtDistanceCircle()
{
  if (meEllipse != nullptr)
    delete meEllipse;
  if (circle != nullptr)
    delete circle;
  if (p1 != nullptr)
    delete p1;
  if (p2 != nullptr)
    delete p2;
  if (p3 != nullptr)
    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 != nullptr) {
    meEllipse->setMe(me);
  }
}

bool vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                         bool doNotTrack, const vpImage<bool> *mask, const int &initRange)
{
  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
    unsigned int initRange_;
    if (initRange < 0) {
      initRange_ = defaultRange;
    }
    else {
      initRange_ = static_cast<unsigned int>(initRange);
    }
 
    meEllipse = new vpMbtMeEllipse;
    meEllipse->setMask(*mask);
    meEllipse->setMe(me);
    int oldInitRange = me->getInitRange();
    me->setInitRange(initRange_);
 
    // meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
 
    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);
      me->setInitRange(oldInitRange);
    }
    catch (...) {
   // vpTRACE("the circle can't be initialized");
      me->setInitRange(oldInitRange);
      return false;
    }
  }
  return true;
}

void vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  (void)cMo;
  int oldInitRange = me->getInitRange();
  me->setInitRange(defaultRange);
  if (isvisible) {
    try {
      meEllipse->track(I);
    }
    catch (...) {
   // std::cout << "Track meEllipse failed" << std::endl;
      meEllipse->reset();
      Reinit = true;
    }
 
    // Update the number of features
    nbFeature = static_cast<unsigned int>(meEllipse->getMeList().size());
  }
  me->setInitRange(oldInitRange);
}

void vpMbtDistanceCircle::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  int oldInitRange = me->getInitRange();
  me->setInitRange(defaultRange);
  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 = static_cast<unsigned int>(meEllipse->getMeList().size());
  }
  me->setInitRange(oldInitRange);
}

void vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                           const vpImage<bool> *mask)
{
  if (meEllipse != nullptr)
    delete meEllipse;
 
  meEllipse = nullptr;
 
  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 != nullptr) {
    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_98)
      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 != nullptr) {
    meEllipse->display(I); // display the me
    if (vpDEBUG_ENABLE(3))
      vpDisplay::flush(I);
  }
}
 
void vpMbtDistanceCircle::displayMovingEdges(const vpImage<vpRGBa> &I)
{
  if (meEllipse != nullptr) {
    meEllipse->display(I); // display the me
    if (vpDEBUG_ENABLE(3))
      vpDisplay::flush(I);
  }
}

void vpMbtDistanceCircle::initInteractionMatrixError()
{
  if (isvisible) {
    nbFeature = static_cast<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->m_j, it->m_i, x, y);
      // TRO Chaumette 2004 eq 25
      H[0] = 2 * (n11 * (y - yg) + n02 * (xg - x));
      H[1] = 2 * (n20 * (yg - y) + n11 * (x - xg));
      H[2] = vpMath::sqr(y - yg) - 4.0 * n02;
      H[3] = 2 * (yg * (x - xg) + y * xg + 4.0 * n11 - x * y);
      H[4] = vpMath::sqr(x - xg) - 4.0 * 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 +
        4.0 * vpMath::sqr(n11) - 4.0 * n20 * n02;
      j++;
    }
  }
}
END_VISP_NAMESPACE