vpLine.cpp

/****************************************************************************
 *
 * $Id: vpLine.cpp 4056 2013-01-05 13:04:42Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <visp/vpLine.h>

#include <visp/vpDebug.h>
#include <visp/vpMath.h>

#include <visp/vpFeatureDisplay.h>

void
vpLine::init()
{
  oP.resize(8) ;
  cP.resize(8) ;
  p.resize(2) ;
}

vpLine::vpLine()
{
  init() ;
}



void
vpLine::setWorldCoordinates(const double &A1, const double &B1,
                const double &C1, const double &D1,
                const double &A2, const double &B2,
                const double &C2, const double &D2)
{
  oP[0] = A1 ;
  oP[1] = B1 ;
  oP[2] = C1 ;
  oP[3] = D1 ;

  oP[4] = A2 ;
  oP[5] = B2 ;
  oP[6] = C2 ;
  oP[7] = D2 ;
}


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)
{
 //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);

  if (p.getRows() != 2)
    p.resize(2);

  p[0] = rho ;
  p[1] = theta ;
}


void
vpLine::changeFrame(const vpHomogeneousMatrix &cMo)
{
  changeFrame(cMo,cP) ;
}


void
vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
{

  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;

  if (cP.getRows() != 8)
    cP.resize(8);

  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);

  // in case of verification
  // ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1;
  // ap2 = A2; bp2 = B2; cp2 = C2; dp2 = 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) ;

  // 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);
  cP[0] = A1 = a1*d1 ;
  cP[1] = B1 = b1*d1 ;
  cP[2] = C1 = c1*d1 ;
  cP[3] = D1 = 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;

  // in case of verification
  /* 
  x = -A2*D2;
  y = -B2*D2;
  z = -C2*D2;
  d1 = ap1*x+bp1*y+cp1*z+dp1;
  d2 = ap2*x+bp2*y+cp2*z+dp2;
  if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
    {
      printf("PB in VPline: P1 : 0 = %lf, P2: 0 = %lf\n",d1,d2);
      exit(-1);
    }
  d1 = A1*x+B1*y+C1*z+D1;
  d2 = A2*x+B2*y+C2*z+D2; 
  if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
    {
      printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2);
      exit(-1);
    }
  */

}



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


// non destructive wrt. cP and p
void vpLine::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::displayLine(_p[0],_p[1],
                                cam, I, color, thickness) ;

}


vpLine *vpLine::duplicate() const
{
  vpLine *feature = new vpLine(*this) ;
  return feature ;
}

/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */