vpPoint.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:
 * Point feature.
 *
*****************************************************************************/
 
#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 &oP_)
{
  init();
  setWorldCoordinates(oP_);
}
 
vpPoint::vpPoint(const std::vector<double> &oP_)
{
  init();
  setWorldCoordinates(oP_);
}
 
void vpPoint::setWorldCoordinates(double oX, double oY, double oZ)
{
  oP[0] = oX;
  oP[1] = oY;
  oP[2] = oZ;
  oP[3] = 1;
}
 
void vpPoint::setWorldCoordinates(const vpColVector &oP_)
{
  if (oP_.size() == 3) {
    oP[0] = oP_[0];
    oP[1] = oP_[1];
    oP[2] = oP_[2];
    oP[3] = 1.;
  }
  else if (oP_.size() == 4) {
    oP[0] = oP_[0];
    oP[1] = oP_[1];
    oP[2] = oP_[2];
    oP[3] = oP_[3];
    oP /= oP[3];
  }
  else {
    throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
  }
}
 
void vpPoint::setWorldCoordinates(const std::vector<double> &oP_)
{
  if (oP_.size() == 3) {
    oP[0] = oP_[0];
    oP[1] = oP_[1];
    oP[2] = oP_[2];
    oP[3] = 1.;
  }
  else if (oP_.size() == 4) {
    oP[0] = oP_[0];
    oP[1] = oP_[1];
    oP[2] = oP_[2];
    oP[3] = oP_[3];
    oP /= oP[3];
  }
  else {
    throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
  }
}
 
void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
{
  oX = oP[0];
  oY = oP[1];
  oZ = oP[2];
}
 
void vpPoint::getWorldCoordinates(vpColVector &oP_) { oP_ = oP; }
 
void vpPoint::getWorldCoordinates(std::vector<double> &oP_)
{
  oP_.resize(oP.size());
  unsigned int oP_size = oP.size();
  for (unsigned int i = 0; i < oP_size; ++i) {
    oP_[i] = oP[i];
  }
}
 
vpColVector vpPoint::getWorldCoordinates(void) { return this->oP; }
 
void vpPoint::projection(const vpColVector &v_cP, vpColVector &v_p) const
{
  v_p.resize(3, false);
 
  v_p[0] = v_cP[0] / v_cP[2];
  v_p[1] = v_cP[1] / v_cP[2];
  v_p[2] = 1;
}
 
void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &v_cP) const
{
  v_cP.resize(4, false);
 
  v_cP[0] = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + (cMo[0][3] * oP[3]);
  v_cP[1] = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + (cMo[1][3] * oP[3]);
  v_cP[2] = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + (cMo[2][3] * oP[3]);
  v_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 / v_cP[3];
  v_cP[0] *= d;
  v_cP[1] *= d;
  v_cP[2] *= d;
  v_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, unsigned int thickness)
{
 
  vpColVector v_cP, v_p;
  changeFrame(cMo, v_cP);
 
  if (v_cP[2] < 0) { // no display if point is behind the camera
    return;
  }
 
  vpPoint::projection(v_cP, v_p);
  vpFeatureDisplay::displayPoint(v_p[0], v_p[1], cam, I, color, thickness);
}
 
void vpPoint::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);
 
  if (v_cP[2] < 0) { // no display if point is behind the camera
    return;
  }
 
  vpPoint::projection(v_cP, v_p);
  vpFeatureDisplay::displayPoint(v_p[0], v_p[1], cam, I, color, thickness);
}
 
VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
 
void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
                      unsigned int thickness)
{
  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
}
 
void vpPoint::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
                      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(double cX) { cP[0] = cX; }
void vpPoint::set_Y(double cY) { cP[1] = cY; }
void vpPoint::set_Z(double cZ) { cP[2] = cZ; }
void vpPoint::set_W(double cW) { cP[3] = cW; }
 
void vpPoint::set_oX(double oX) { oP[0] = oX; }
void vpPoint::set_oY(double oY) { oP[1] = oY; }
void vpPoint::set_oZ(double oZ) { oP[2] = oZ; }
void vpPoint::set_oW(double oW) { oP[3] = oW; }
 
void vpPoint::set_x(double x) { p[0] = x; }
void vpPoint::set_y(double y) { p[1] = y; }
void vpPoint::set_w(double w) { p[2] = w; }