vpHomogeneousMatrix.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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://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:
 * Homogeneous matrix.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpQuaternionVector.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpException.h>

vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
                                         const vpQuaternionVector &q)
  : vpArray2D<double>(4, 4)
{
  buildFrom(t,q);
  (*this)[3][3] = 1.;
}

vpHomogeneousMatrix::vpHomogeneousMatrix()
  : vpArray2D<double>(4, 4)
{
  eye() ;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M)
  : vpArray2D<double>(4, 4)
{
  *this = M;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
                                         const vpThetaUVector &tu)
  : vpArray2D<double>(4, 4)
{
  buildFrom(t, tu);
  (*this)[3][3] = 1.;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
                                         const vpRotationMatrix &R)
  : vpArray2D<double>(4, 4)
{
  insert(R);
  insert(t);
  (*this)[3][3] = 1.;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p)
  : vpArray2D<double>(4, 4)
{
  buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]) ;
  (*this)[3][3] = 1.;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v)
  : vpArray2D<double>(4, 4)
{
  buildFrom(v) ;
  (*this)[3][3] = 1.;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v)
  : vpArray2D<double>(4, 4)
{
  buildFrom(v) ;
  (*this)[3][3] = 1.;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
                                         const double ty,
                                         const double tz,
                                         const double tux,
                                         const double tuy,
                                         const double tuz)
  : vpArray2D<double>(4, 4)
{
  buildFrom(tx, ty, tz, tux, tuy, tuz);
  (*this)[3][3] = 1.;
}

void
vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
                               const vpThetaUVector &tu)
{
  insert(tu) ;
  insert(t) ;
}

void
vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
                               const vpRotationMatrix &R)
{
  insert(R) ;
  insert(t) ;
}

void
vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
{
  vpTranslationVector tv(p[0], p[1], p[2]);
  vpThetaUVector tu(p[3], p[4], p[5]);

  insert(tu) ;
  insert(tv) ;
}

void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
                                    const vpQuaternionVector &q)
{
  insert(t);
  insert(q);
}

void
vpHomogeneousMatrix::buildFrom(const double tx,
                               const double ty,
                               const double tz,
                               const double tux,
                               const double tuy,
                               const double tuz)
{
  vpRotationMatrix R(tux, tuy, tuz) ;
  vpTranslationVector t(tx, ty, tz) ;

  insert(R) ;
  insert(t) ;
}

void
vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
{
  if (v.size() != 12 && v.size() != 16) {
    throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
  }

  for (unsigned int i=0; i < 12; i++)
    this->data[i] = (double)v[i];
}

void
vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
{
  if (v.size() != 12 && v.size() != 16) {
    throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
  }

  for (unsigned int i=0; i < 12; i++)
    this->data[i] = v[i];
}

vpHomogeneousMatrix &
vpHomogeneousMatrix::operator=(const vpHomogeneousMatrix &M)
{
  for (int i=0; i<4; i++) {
    for (int j=0; j<4; j++) {
      rowPtrs[i][j] = M.rowPtrs[i][j];
    }
  }
  return *this;
}

vpHomogeneousMatrix
vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
{
  vpHomogeneousMatrix p;

  vpRotationMatrix R1, R2, R ;
  vpTranslationVector T1, T2 , T;

  extract(T1) ;
  M.extract(T2) ;

  extract (R1) ;
  M.extract (R2) ;

  R = R1*R2 ;

  T = R1*T2 + T1 ;

  p.insert(T) ;
  p.insert(R) ;

  return p;
}

vpHomogeneousMatrix &
vpHomogeneousMatrix::operator*=(const vpHomogeneousMatrix &M)
{
  (*this) = (*this) * M;
  return (*this);
}

vpColVector
vpHomogeneousMatrix::operator*(const vpColVector &v) const
{
  if (v.getRows() != 4) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (4x4) homogeneous matrix by a (%dx1) column vector",
                      v.getRows()));
  }
  vpColVector p(rowNum);

  p = 0.0;

  for (unsigned int j=0;j<4;j++) {
    for (unsigned int i=0;i<4;i++) {
      p[i]+=rowPtrs[i][j] * v[j];
    }
  }

  return p;
}

vpPoint vpHomogeneousMatrix::operator*(const vpPoint& bP) const
{
  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] = (*this)[0][0]*v[0] + (*this)[0][1]*v[1]+ (*this)[0][2]*v[2]+ (*this)[0][3]*v[3] ;
  v1[1] = (*this)[1][0]*v[0] + (*this)[1][1]*v[1]+ (*this)[1][2]*v[2]+ (*this)[1][3]*v[3] ;
  v1[2] = (*this)[2][0]*v[0] + (*this)[2][1]*v[1]+ (*this)[2][2]*v[2]+ (*this)[2][3]*v[3] ;
  v1[3] = (*this)[3][0]*v[0] + (*this)[3][1]*v[1]+ (*this)[3][2]*v[2]+ (*this)[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 ;
}

vpTranslationVector vpHomogeneousMatrix::operator*(const vpTranslationVector &t) const
{
  vpTranslationVector t_out;
  t_out[0] = (*this)[0][0]*t[0] + (*this)[0][1]*t[1]+ (*this)[0][2]*t[2]+ (*this)[0][3];
  t_out[1] = (*this)[1][0]*t[0] + (*this)[1][1]*t[1]+ (*this)[1][2]*t[2]+ (*this)[1][3];
  t_out[2] = (*this)[2][0]*t[0] + (*this)[2][1]*t[1]+ (*this)[2][2]*t[2]+ (*this)[2][3];

  return t_out ;
}

/*********************************************************************/

bool
vpHomogeneousMatrix::isAnHomogeneousMatrix() const
{
  vpRotationMatrix R ;
  extract(R) ;

  return  R.isARotationMatrix() ;
}

void
vpHomogeneousMatrix::extract(vpRotationMatrix &R) const
{
  for (unsigned int i=0 ; i < 3 ; i++)
    for (unsigned int j=0 ; j < 3; j++)
      R[i][j] = (*this)[i][j] ;
}

void
vpHomogeneousMatrix::extract(vpTranslationVector &t) const
{
  t[0] = (*this)[0][3] ;
  t[1] = (*this)[1][3] ;
  t[2] = (*this)[2][3] ;
}
void
vpHomogeneousMatrix::extract(vpThetaUVector &tu) const
{
  vpRotationMatrix R;
  (*this).extract(R);
  tu.buildFrom(R);
}

void
vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
{
  vpRotationMatrix R;
  (*this).extract(R);
  q.buildFrom(R);
}

void
vpHomogeneousMatrix::insert(const vpRotationMatrix &R)
{
  for (unsigned int i=0 ; i < 3 ; i++)
    for (unsigned int j=0 ; j < 3; j++)
      (*this)[i][j] = R[i][j] ;
}

void
vpHomogeneousMatrix::insert(const vpThetaUVector &tu)
{
  vpRotationMatrix R(tu) ;
  insert(R) ;
}

void
vpHomogeneousMatrix::insert(const vpTranslationVector &t)
{
  (*this)[0][3] = t[0] ;
  (*this)[1][3] = t[1] ;
  (*this)[2][3] = t[2] ;
}

void
vpHomogeneousMatrix::insert(const vpQuaternionVector &q){
  insert(vpRotationMatrix(q));
}

vpHomogeneousMatrix
vpHomogeneousMatrix::inverse() const
{
  vpHomogeneousMatrix Mi ;

  vpRotationMatrix R ;      extract(R) ;
  vpTranslationVector T ;   extract(T) ;

  vpTranslationVector RtT ; RtT = -(R.t()*T) ;


  Mi.insert(R.t()) ;
  Mi.insert(RtT) ;

  return Mi ;
}

void vpHomogeneousMatrix::eye()
{
  (*this)[0][0] = 1 ;
  (*this)[1][1] = 1 ;
  (*this)[2][2] = 1 ;
  (*this)[3][3] = 1 ;

  (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0 ;
  (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0 ;
  (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0 ;
  (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0 ;
}

void
vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const
{
  M = inverse() ;
}


void
vpHomogeneousMatrix::save(std::ofstream &f) const
{
  if (! f.fail()) {
    f << *this ;
  }
  else {
    throw(vpException(vpException::ioError,
                      "Cannot save homogeneous matrix: ostream not open")) ;
  }
}


void
vpHomogeneousMatrix::load(std::ifstream &f)
{
  if (! f.fail()) {
    for (unsigned int i=0 ; i < 4 ; i++) {
      for (unsigned int j=0 ; j < 4 ; j++) {
        f >> (*this)[i][j] ;
      }
    }
  }
  else {
    throw(vpException(vpException::ioError,
                      "Cannot laad homogeneous matrix: ifstream not open")) ;
  }
}

void
vpHomogeneousMatrix::print() const
{
  vpPoseVector r(*this) ;
  std::cout << r.t() ;
}

void vpHomogeneousMatrix::convert(std::vector<float> &M)
{
  M.resize(12);
  for(unsigned int i=0; i < 12; i++)
    M[i] = (float)(this->data[i]);
}

void vpHomogeneousMatrix::convert(std::vector<double> &M)
{
  M.resize(12);
  for(unsigned int i=0; i < 12; i++)
    M[i] = this->data[i];
}

vpTranslationVector vpHomogeneousMatrix::getTranslationVector() const
{
  vpTranslationVector tr;
  this->extract(tr);
  return tr;
}

vpRotationMatrix vpHomogeneousMatrix::getRotationMatrix() const
{
  vpRotationMatrix R;
  this->extract(R);
  return R;
}

vpThetaUVector vpHomogeneousMatrix::getThetaUVector() const
{
  vpThetaUVector tu;
  this->extract(tu);
  return tu;
}

vpColVector
vpHomogeneousMatrix::getCol(const unsigned int j) const
{
  if (j >= getCols())
    throw(vpException(vpException::dimensionError,
                      "Unable to extract a column vector from the homogeneous matrix"));
  unsigned int nb_rows = getRows();
  vpColVector c(nb_rows);
  for (unsigned int i=0 ; i < nb_rows ; i++)
    c[i] = (*this)[i][j];
  return c;
}

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

void
vpHomogeneousMatrix::setIdentity()
{
  eye() ;
}

#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)