vpPoint.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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/vpFeatureDisplay.h>
#include <visp3/core/vpPoint.h>
 
BEGIN_VISP_NAMESPACE
void vpPoint::init()
{
  const unsigned int val_3 = 3;
  const unsigned int val_4 = 4;
  const unsigned int index_2 = 2;
  const unsigned int index_3 = 3;
  p.resize(val_3);
  p = 0;
  p[index_2] = 1.;
  oP.resize(val_4);
  oP = 0.;
  oP[index_3] = 1.;
  cP.resize(val_4);
  cP = 0.;
  cP[index_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)
{
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  const unsigned int index_3 = 3;
  oP[index_0] = oX;
  oP[index_1] = oY;
  oP[index_2] = oZ;
  oP[index_3] = 1;
}

void vpPoint::setWorldCoordinates(const vpColVector &oP_)
{
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  const unsigned int index_3 = 3;
  const unsigned int val_3 = 3;
  const unsigned int val_4 = 4;
 
  if (oP_.size() == val_3) {
    oP[index_0] = oP_[index_0];
    oP[index_1] = oP_[index_1];
    oP[index_2] = oP_[index_2];
    oP[index_3] = 1.;
  }
  else if (oP_.size() == val_4) {
    oP[index_0] = oP_[index_0];
    oP[index_1] = oP_[index_1];
    oP[index_2] = oP_[index_2];
    oP[index_3] = oP_[index_3];
    oP /= oP[index_3];
  }
  else {
    throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
  }
}

void vpPoint::setWorldCoordinates(const std::vector<double> &oP_)
{
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  const unsigned int index_3 = 3;
  const unsigned int val_3 = 3;
  const unsigned int val_4 = 4;
 
  if (oP_.size() == val_3) {
    oP[index_0] = oP_[index_0];
    oP[index_1] = oP_[index_1];
    oP[index_2] = oP_[index_2];
    oP[index_3] = 1.;
  }
  else if (oP_.size() == val_4) {
    oP[index_0] = oP_[index_0];
    oP[index_1] = oP_[index_1];
    oP[index_2] = oP_[index_2];
    oP[index_3] = oP_[index_3];
    oP /= oP[index_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)
{
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  oX = oP[index_0];
  oY = oP[index_1];
  oZ = oP[index_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
{
  const unsigned int val_3 = 3;
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  v_p.resize(val_3, false);
 
  v_p[index_0] = v_cP[index_0] / v_cP[index_2];
  v_p[index_1] = v_cP[index_1] / v_cP[index_2];
  v_p[index_2] = 1;
}

void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &v_cP) const
{
  const unsigned int val_4 = 4;
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  const unsigned int index_3 = 3;
  v_cP.resize(val_4, false);
 
  v_cP[index_0] = (cMo[index_0][index_0] * oP[index_0]) + (cMo[index_0][index_1] * oP[index_1]) + (cMo[index_0][index_2] * oP[index_2]) + (cMo[index_0][index_3] * oP[index_3]);
  v_cP[index_1] = (cMo[index_1][index_0] * oP[index_0]) + (cMo[index_1][index_1] * oP[index_1]) + (cMo[index_1][index_2] * oP[index_2]) + (cMo[index_1][index_3] * oP[index_3]);
  v_cP[index_2] = (cMo[index_2][index_0] * oP[index_0]) + (cMo[index_2][index_1] * oP[index_1]) + (cMo[index_2][index_2] * oP[index_2]) + (cMo[index_2][index_3] * oP[index_3]);
  v_cP[index_3] = (cMo[index_3][index_0] * oP[index_0]) + (cMo[index_3][index_1] * oP[index_1]) + (cMo[index_3][index_2] * oP[index_2]) + (cMo[index_3][index_3] * oP[index_3]);
 
  double d = 1 / v_cP[index_3];
  v_cP[index_0] *= d;
  v_cP[index_1] *= d;
  v_cP[index_2] *= d;
  v_cP[index_3] *= d;
}

void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo)
{
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  const unsigned int index_3 = 3;
  double X = (cMo[index_0][index_0] * oP[index_0]) + (cMo[index_0][index_1] * oP[index_1]) + (cMo[index_0][index_2] * oP[index_2]) + (cMo[index_0][index_3] * oP[index_3]);
  double Y = (cMo[index_1][index_0] * oP[index_0]) + (cMo[index_1][index_1] * oP[index_1]) + (cMo[index_1][index_2] * oP[index_2]) + (cMo[index_1][index_3] * oP[index_3]);
  double Z = (cMo[index_2][index_0] * oP[index_0]) + (cMo[index_2][index_1] * oP[index_1]) + (cMo[index_2][index_2] * oP[index_2]) + (cMo[index_2][index_3] * oP[index_3]);
  double W = (cMo[index_3][index_0] * oP[index_0]) + (cMo[index_3][index_1] * oP[index_1]) + (cMo[index_3][index_2] * oP[index_2]) + (cMo[index_3][index_3] * oP[index_3]);
 
  double d = 1. / W;
  cP[index_0] = X * d;
  cP[index_1] = Y * d;
  cP[index_2] = Z * d;
  cP[index_3] = 1.;
}

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);
  const unsigned int index_2 = 2;
 
  if (v_cP[index_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);
  const unsigned int index_2 = 2;
 
  if (v_cP[index_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<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 { const unsigned int index_0 = 0; return cP[index_0]; }
double vpPoint::get_Y() const { const unsigned int index_1 = 1; return cP[index_1]; }
double vpPoint::get_Z() const { const unsigned int index_2 = 2; return cP[index_2]; }
double vpPoint::get_W() const { const unsigned int index_3 = 3; return cP[index_3]; }

double vpPoint::get_oX() const { const unsigned int index_0 = 0; return oP[index_0]; }
double vpPoint::get_oY() const { const unsigned int index_1 = 1; return oP[index_1]; }
double vpPoint::get_oZ() const { const unsigned int index_2 = 2; return oP[index_2]; }
double vpPoint::get_oW() const { const unsigned int index_3 = 3; return oP[index_3]; }

double vpPoint::get_x() const { const unsigned int index_0 = 0; return p[index_0]; }
double vpPoint::get_y() const { const unsigned int index_1 = 1; return p[index_1]; }
double vpPoint::get_w() const { const unsigned int index_2 = 2; return p[index_2]; }

void vpPoint::projection()
{
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  double d = 1 / cP[index_2];
  p[index_0] = cP[index_0] * d;
  p[index_1] = cP[index_1] * d;
  p[index_2] = 1;
}

void vpPoint::set_X(double cX) { const unsigned int index_0 = 0; cP[index_0] = cX; }
void vpPoint::set_Y(double cY) { const unsigned int index_1 = 1; cP[index_1] = cY; }
void vpPoint::set_Z(double cZ) { const unsigned int index_2 = 2; cP[index_2] = cZ; }
void vpPoint::set_W(double cW) { const unsigned int index_3 = 3; cP[index_3] = cW; }

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

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

VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint &vpp)
{
  os << "oP=(" << vpp.get_oX() << ", " << vpp.get_oY() << ", " << vpp.get_oZ() << ", " << vpp.get_oW() << ")\n";
  os << "cP=(" << vpp.get_X() << ", " << vpp.get_Y() << ", " << vpp.get_Z() << ", " << vpp.get_W() << ")\n";
  os << "ip=(" << vpp.get_x() << ", " << vpp.get_y() << ", " << vpp.get_w()<< ")";
  return os;
}
END_VISP_NAMESPACE