vpCircle.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:
 * Visual feature circle.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/core/vpCircle.h>

#include <visp3/core/vpFeatureDisplay.h>

void vpCircle::init()
{

  oP.resize(7);
  cP.resize(7);

  p.resize(5);
}

void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }

void vpCircle::setWorldCoordinates(const double A, const double B, const double C, const double X0, const double Y0,
                                   const double Z0, const double R)
{
  oP[0] = A;
  oP[1] = B;
  oP[2] = C;
  oP[3] = X0;
  oP[4] = Y0;
  oP[5] = Z0;
  oP[6] = R;
}

vpCircle::vpCircle() { init(); }

vpCircle::vpCircle(const vpColVector &oP_)
{
  init();
  setWorldCoordinates(oP_);
}

vpCircle::vpCircle(const double A, const double B, const double C, const double X0, const double Y0, const double Z0,
                   const double R)
{
  init();
  setWorldCoordinates(A, B, C, X0, Y0, Z0, R);
}

vpCircle::~vpCircle() {}

void vpCircle::projection() { projection(cP, p); }

void vpCircle::projection(const vpColVector &cP_, vpColVector &p_)
{
  vpColVector K(6);
  {
    double A = cP_[0];
    double B = cP_[1];
    double C = cP_[2];

    double X0 = cP_[3];
    double Y0 = cP_[4];
    double Z0 = cP_[5];

    double r = cP_[6];

    // projection
    double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
    double det = A * X0 + B * Y0 + C * Z0;
    A = A / det;
    B = B / det;
    C = C / det;

    K[0] = 1 - 2 * A * X0 + A * A * s;
    K[1] = 1 - 2 * B * Y0 + B * B * s;
    K[2] = -A * Y0 - B * X0 + A * B * s;
    K[3] = -C * X0 - A * Z0 + A * C * s;
    K[4] = -C * Y0 - B * Z0 + B * C * s;
    K[5] = 1 - 2 * C * Z0 + C * C * s;
  }

  //  {
  //    std::cout << "K dans vpCircle::projection(): " << std::endl;
  //    for (unsigned int i=1; i<6; i++)
  //      std::cout << K[i]/K[0] << std::endl;
  //  }
  double det = K[2] * K[2] - K[0] * K[1];
  if (fabs(det) < 1e-8) {
    vpERROR_TRACE("division par 0");
    throw(vpException(vpException::divideByZeroError, "division par 0"));
  }

  double xc = (K[1] * K[3] - K[2] * K[4]) / det;
  double yc = (K[0] * K[4] - K[2] * K[3]) / det;

  double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]);
  double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]);

  double A, B, E;

  // if (fabs(K[2])<1e-6)
  if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
    E = 0.0;
    if (K[0] > K[1]) {
      A = sqrt(s / (K[0] + K[1] + c));
      B = sqrt(s / (K[0] + K[1] - c));
    } else {
      A = sqrt(s / (K[0] + K[1] - c));
      B = sqrt(s / (K[0] + K[1] + c));
    }
  } else {
    E = (K[1] - K[0] + c) / (2 * K[2]);
    if (fabs(E) > 1.0) {
      A = sqrt(s / (K[0] + K[1] + c));
      B = sqrt(s / (K[0] + K[1] - c));
    } else {
      A = sqrt(s / (K[0] + K[1] - c));
      B = sqrt(s / (K[0] + K[1] + c));
      E = -1.0 / E;
    }
  }

  det = (1.0 + vpMath::sqr(E));
  double mu20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
  double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
  double mu02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;

  p_[0] = xc;
  p_[1] = yc;
  p_[2] = mu20;
  p_[3] = mu11;
  p_[4] = mu02;
}

void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
{

  double A, B, C;
  A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
  B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
  C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];

  double X0, Y0, Z0;
  X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
  Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
  Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
  double R = oP[6];

  cP_[0] = A;
  cP_[1] = B;
  cP_[2] = C;

  cP_[3] = X0;
  cP_[4] = Y0;
  cP_[5] = Z0;

  cP_[6] = R;

  // vpTRACE("_cP :") ; std::cout << _cP.t() ;
}

void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo)
{

  double A, B, C;
  A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
  B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
  C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];

  double X0, Y0, Z0;
  X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
  Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
  Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
  double R = oP[6];

  cP[0] = A;
  cP[1] = B;
  cP[2] = C;

  cP[3] = X0;
  cP[4] = Y0;
  cP[5] = Z0;

  cP[6] = R;

  // vpTRACE("_cP :") ; std::cout << _cP.t() ;
}

void vpCircle::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
                       const unsigned int thickness)
{
  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
}

// non destructive wrt. cP and p
void vpCircle::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                       const vpColor &color, const unsigned int thickness)
{
  vpColVector _cP, _p;
  changeFrame(cMo, _cP);
  projection(_cP, _p);
  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
}
vpCircle *vpCircle::duplicate() const
{
  vpCircle *feature = new vpCircle(*this);
  return feature;
}

void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
                                        const double &theta, double &i, double &j)
{
  // This was taken from the code of art-v1. (from the artCylinder class)
  double px = cam.get_px();
  double py = cam.get_py();
  double u0 = cam.get_u0();
  double v0 = cam.get_v0();

  double mu11 = circle.p[3];
  double mu02 = circle.p[4];
  double mu20 = circle.p[2];
  double Xg = u0 + circle.p[0] * px;
  double Yg = v0 + circle.p[1] * py;

  // Find Intersection between line and ellipse in the image.

  // Optimised calculation for X
  double stheta = sin(theta);
  double ctheta = cos(theta);
  double sctheta = stheta * ctheta;
  double m11yg = mu11 * Yg;
  double ctheta2 = vpMath::sqr(ctheta);
  double m02xg = mu02 * Xg;
  double m11stheta = mu11 * stheta;
  j = ((mu11 * Xg * sctheta - mu20 * Yg * sctheta + mu20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
        m02xg * ctheta2 + m11stheta * rho) /
       (mu20 * ctheta2 + 2.0 * m11stheta * ctheta + mu02 - mu02 * ctheta2));
  // Optimised calculation for Y
  double rhom02 = rho * mu02;
  double sctheta2 = stheta * ctheta2;
  double ctheta3 = ctheta2 * ctheta;
  i = (-(-rho * mu11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + mu11 * Xg * sctheta2 - mu20 * Yg * sctheta2 -
         ctheta * mu11 * Yg + ctheta3 * mu11 * Yg + ctheta * mu02 * Xg - ctheta3 * mu02 * Xg) /
       (mu20 * ctheta2 + 2.0 * mu11 * stheta * ctheta + mu02 - mu02 * ctheta2) / stheta);
}