vpPoint.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:
 * Point feature.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpFeatureDisplay.h>
#include <visp3/core/vpPoint.h>

void vpPoint::init()
{
  p.resize(3);
  p = 0;
  p[2] = 1;
  oP.resize(4);
  oP = 0;
  oP[3] = 1;
  cP.resize(4);
  cP = 0;
  cP[3] = 1;

  // default value Z (1 meters)
  set_Z(1);
}

vpPoint::vpPoint() { init(); }

vpPoint::vpPoint(double oX, double oY, double oZ)
{
  init();
  setWorldCoordinates(oX, oY, oZ);
}

vpPoint::vpPoint(const vpColVector &P)
{
  init();
  setWorldCoordinates(P);
}

vpPoint::vpPoint(const std::vector<double> &P)
{
  init();
  setWorldCoordinates(P);
}

void vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
{
  oP[0] = oX;
  oP[1] = oY;
  oP[2] = oZ;
  oP[3] = 1;
}

void vpPoint::setWorldCoordinates(const vpColVector &P)
{
  if (P.size() == 3) {
    oP[0] = P[0];
    oP[1] = P[1];
    oP[2] = P[2];
    oP[3] = 1.;
  } else if (P.size() == 4) {
    oP[0] = P[0];
    oP[1] = P[1];
    oP[2] = P[2];
    oP[3] = P[3];
    oP /= oP[3];
  } else {
    throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size()));
  }
}

void vpPoint::setWorldCoordinates(const std::vector<double> &P)
{
  if (P.size() == 3) {
    oP[0] = P[0];
    oP[1] = P[1];
    oP[2] = P[2];
    oP[3] = 1.;
  } else if (P.size() == 4) {
    oP[0] = P[0];
    oP[1] = P[1];
    oP[2] = P[2];
    oP[3] = P[3];
    oP /= oP[3];
  } else {
    throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size()));
  }
}

void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
{
  oX = oP[0];
  oY = oP[1];
  oZ = oP[2];
}

void vpPoint::getWorldCoordinates(vpColVector &P) { P = oP; }
void vpPoint::getWorldCoordinates(std::vector<double> &P)
{
  P.resize(oP.size());
  for (unsigned int i = 0; i < oP.size(); i++)
    P[i] = oP[i];
}

vpColVector vpPoint::getWorldCoordinates(void) { return this->oP; }

void vpPoint::projection(const vpColVector &_cP, vpColVector &_p)
{
  _p.resize(3);

  _p[0] = _cP[0] / _cP[2];
  _p[1] = _cP[1] / _cP[2];
  _p[2] = 1;
}

void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
{

  _cP.resize(4);

  _cP[0] = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
  _cP[1] = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
  _cP[2] = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
  _cP[3] = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];

  double d = 1 / _cP[3];
  _cP[0] *= d;
  _cP[1] *= d;
  _cP[2] *= d;
  _cP[3] *= d;
  ;
}

void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo)
{
  double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
  double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
  double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
  double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];

  double d = 1 / W;
  cP[0] = X * d;
  cP[1] = Y * d;
  cP[2] = Z * d;
  cP[3] = 1;
}

#if 0

const vpPoint
operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
{
  vpPoint aP ;

  vpColVector v(4),v1(4) ;

  v[0] = bP.get_X() ;
  v[1] = bP.get_Y() ;
  v[2] = bP.get_Z() ;
  v[3] = bP.get_W() ;

  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;

  v1 /= v1[3] ;

  //  v1 = M*v ;
  aP.set_X(v1[0]) ;
  aP.set_Y(v1[1]) ;
  aP.set_Z(v1[2]) ;
  aP.set_W(v1[3]) ;
 
  aP.set_oX(v1[0]) ;
  aP.set_oY(v1[1]) ;
  aP.set_oZ(v1[2]) ;
  aP.set_oW(v1[3]) ;
 
  return aP ;
}

const vpPoint
operator*(const vpHomography &aHb, const vpPoint& bP)
{
  vpPoint aP ;
  vpColVector v(3),v1(3) ;

  v[0] = bP.get_x() ;
  v[1] = bP.get_y() ;
  v[2] = bP.get_w() ;

  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;

  //  v1 = M*v ;
  aP.set_x(v1[0]) ;
  aP.set_y(v1[1]) ;
  aP.set_w(v1[2]) ;

  return aP ;
}
#endif
vpPoint *vpPoint::duplicate() const
{
  vpPoint *feature = new vpPoint(*this);
  return feature;
}

void vpPoint::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);

  if (_cP[2] < 0) // no display if point is behind the camera
    return;

  vpPoint::projection(_cP, _p);
  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
}

void vpPoint::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                      const vpColor &color, const unsigned int thickness)
{
  vpColVector _cP, _p;
  changeFrame(cMo, _cP);

  if (_cP[2] < 0) // no display if point is behind the camera
    return;

  vpPoint::projection(_cP, _p);
  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
}

VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }

vpPoint &vpPoint::operator=(const vpPoint &vpp)
{
  p = vpp.p;
  cP = vpp.cP;
  oP = vpp.oP;
  cPAvailable = vpp.cPAvailable;

  return *this;
}

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

// Get coordinates
double vpPoint::get_X() const { return cP[0]; }
double vpPoint::get_Y() const { return cP[1]; }
double vpPoint::get_Z() const { return cP[2]; }
double vpPoint::get_W() const { return cP[3]; }

double vpPoint::get_oX() const { return oP[0]; }
double vpPoint::get_oY() const { return oP[1]; }
double vpPoint::get_oZ() const { return oP[2]; }
double vpPoint::get_oW() const { return oP[3]; }

double vpPoint::get_x() const { return p[0]; }
double vpPoint::get_y() const { return p[1]; }
double vpPoint::get_w() const { return p[2]; }

void vpPoint::projection()
{
  double d = 1 / cP[2];
  p[0] = cP[0] * d;
  p[1] = cP[1] * d;
  p[2] = 1;
}

void vpPoint::set_X(const double X) { cP[0] = X; }
void vpPoint::set_Y(const double Y) { cP[1] = Y; }
void vpPoint::set_Z(const double Z) { cP[2] = Z; }
void vpPoint::set_W(const double W) { cP[3] = W; }

void vpPoint::set_oX(const double oX) { oP[0] = oX; }
void vpPoint::set_oY(const double oY) { oP[1] = oY; }
void vpPoint::set_oZ(const double oZ) { oP[2] = oZ; }
void vpPoint::set_oW(const double oW) { oP[3] = oW; }

void vpPoint::set_x(const double x) { p[0] = x; }
void vpPoint::set_y(const double y) { p[1] = y; }
void vpPoint::set_w(const double w) { p[2] = w; }