vpLine.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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:
 * Line feature.
 *
*****************************************************************************/
 
#include <visp3/core/vpLine.h>
 
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
 
#include <visp3/core/vpFeatureDisplay.h>
 
void vpLine::init()
{
  oP.resize(8);
  cP.resize(8);
  p.resize(2);
}
 
vpLine::vpLine() { init(); }
 
void vpLine::setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1,
                                 const double &oA2, const double &oB2, const double &oC2, const double &oD2)
{
  oP[0] = oA1;
  oP[1] = oB1;
  oP[2] = oC1;
  oP[3] = oD1;
 
  oP[4] = oA2;
  oP[5] = oB2;
  oP[6] = oC2;
  oP[7] = oD2;
}
 
void vpLine::setWorldCoordinates(const vpColVector &oP_)
{
  if (oP_.getRows() != 8) {
    throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
  }
  this->oP = oP_;
}
 
void vpLine::setWorldCoordinates(const vpColVector &oP1, const vpColVector &oP2)
{
  if (oP1.getRows() != 4) {
    throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
  }
  if (oP2.getRows() != 4) {
    throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
  }
  for (unsigned int i = 0; i < 4; ++i) {
    oP[i] = oP1[i];
    oP[i + 4] = oP2[i];
  }
}
 
void vpLine::projection() { projection(cP, p); }
 
void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const
{
  p_.resize(2, false);
  // projection
 
  if (cP.getRows() != 8) {
    throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
  }
  double A1, A2, B1, B2, C1, C2, D1, D2;
 
  A1 = cP_[0];
  B1 = cP_[1];
  C1 = cP_[2];
  D1 = cP_[3];
 
  A2 = cP_[4];
  B2 = cP_[5];
  C2 = cP_[6];
  D2 = cP_[7];
 
  double a, b, c, s;
  a = (A2 * D1) - (A1 * D2);
  b = (B2 * D1) - (B1 * D2);
  c = (C2 * D1) - (C1 * D2);
  s = (a * a) + (b * b);
  if (s <= 1e-8) // seuil pas terrible
  {
    printf("Degenerate case: the image of the straight line is a point!\n");
    throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
  }
  s = 1.0 / sqrt(s);
 
  double rho = -c * s;
  double theta = atan2(b, a);
 
  p_[0] = rho;
  p_[1] = theta;
}
 
void vpLine::changeFrame(const vpHomogeneousMatrix &cMo) { changeFrame(cMo, cP); }
 
void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const
{
  cP_.resize(8, false);
 
  double a1, a2, b1, b2, c1, c2, d1, d2;
  double A1, A2, B1, B2, C1, C2, D1, D2;
 
  // in case of verification
  // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
 
  a1 = oP[0];
  b1 = oP[1];
  c1 = oP[2];
  d1 = oP[3];
 
  a2 = oP[4];
  b2 = oP[5];
  c2 = oP[6];
  d2 = oP[7];
 
  A1 = (cMo[0][0] * a1) + (cMo[0][1] * b1) + (cMo[0][2] * c1);
  B1 = (cMo[1][0] * a1) + (cMo[1][1] * b1) + (cMo[1][2] * c1);
  C1 = (cMo[2][0] * a1) + (cMo[2][1] * b1) + (cMo[2][2] * c1);
  D1 = d1 - ((cMo[0][3] * A1) + (cMo[1][3] * B1) + (cMo[2][3] * C1));
 
  A2 = (cMo[0][0] * a2) + (cMo[0][1] * b2) + (cMo[0][2] * c2);
  B2 = (cMo[1][0] * a2) + (cMo[1][1] * b2) + (cMo[1][2] * c2);
  C2 = (cMo[2][0] * a2) + (cMo[2][1] * b2) + (cMo[2][2] * c2);
  D2 = d2 - ((cMo[0][3] * A2) + (cMo[1][3] * B2) + (cMo[2][3] * C2));
 
  // Adding constraints on the straight line to have a unique representation
 
  // direction of the straight line = N1 x N2
  a2 = (B1 * C2) - (C1 * B2);
  b2 = (C1 * A2) - (A1 * C2);
  c2 = (A1 * B2) - (B1 * A2);
 
  // Constraint D1 = 0 (the origin belongs to P1)
  a1 = (A2 * D1) - (A1 * D2);
  b1 = (B2 * D1) - (B1 * D2);
  c1 = (C2 * D1) - (C1 * D2);
 
  if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
  {
    A2 = A1;
    B2 = B1;
    C2 = C1;
    D2 = D1;
  }
 
  // Constraint A1^2 + B1^2 + C1^2 = 1
  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
  A1 = a1 * d1;
  B1 = b1 * d1;
  C1 = c1 * d1;
  cP_[0] = A1;
  cP_[1] = B1;
  cP_[2] = C1;
 
  cP_[3] = 0;
 
  // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
  // N2_new = (N1 x N2) x N1_new
  a1 = (b2 * C1) - (c2 * B1);
  b1 = (c2 * A1) - (a2 * C1);
  c1 = (a2 * B1) - (b2 * A1);
 
  // Constraint A2^2 + B2^2 + C2^2 = 1
  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
  a1 *= d1;
  b1 *= d1;
  c1 *= d1;
 
  // D2_new = D2 / (N2^T . N2_new)
  D2 /= ((A2 * a1) + (B2 * b1) + (C2 * c1));
  A2 = a1;
  B2 = b1;
  C2 = c1;
 
  // Constraint D2 < 0
  if (D2 > 0) {
    A2 = -A2;
    B2 = -B2;
    C2 = -C2;
    D2 = -D2;
  }
  //  vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f  ", A1, B1, C1, D1) ;
  //  vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f  ", A2, B2, C2, D2) ;
 
  cP_[4] = A2;
  cP_[5] = B2;
  cP_[6] = C2;
  cP_[7] = D2;
}
 
void vpLine::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
                     unsigned int thickness)
{
  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
}
 
void vpLine::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
                     unsigned int thickness)
{
  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
}
 
void vpLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                     const vpColor &color, unsigned int thickness)
{
  vpColVector v_cP, v_p;
  changeFrame(cMo, v_cP);
  try {
    projection(v_cP, v_p);
    vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
  }
  catch (...) {
 // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
  }
}
 
void vpLine::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                     const vpColor &color, unsigned int thickness)
{
  vpColVector v_cP, v_p;
  changeFrame(cMo, v_cP);
  try {
    projection(v_cP, v_p);
    vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
  }
  catch (...) {
 // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
  }
}
 
vpLine *vpLine::duplicate() const
{
  vpLine *feature = new vpLine(*this);
  return feature;
}