vpRotationMatrix.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:
 * Rotation matrix.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>

// Rotation classes
#include <visp3/core/vpRotationMatrix.h>

// Exception
#include <visp3/core/vpException.h>

// Debug trace
#include <math.h>
#include <visp3/core/vpDebug.h>

void vpRotationMatrix::eye()
{
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      if (i == j)
        (*this)[i][j] = 1.0;
      else
        (*this)[i][j] = 0.0;
    }
  }
}

vpRotationMatrix &vpRotationMatrix::operator=(const vpRotationMatrix &R)
{
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      rowPtrs[i][j] = R.rowPtrs[i][j];
    }
  }

  return *this;
}

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

vpRotationMatrix& vpRotationMatrix::operator=(const std::initializer_list<double> &list)
{
  if (dsize != static_cast<unsigned int>(list.size())) {
    throw(vpException(vpException::dimensionError, "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
  }

  std::copy(list.begin(), list.end(), data);

  if (! isARotationMatrix() ) {
    throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
  }

  return *this;
}
#endif

vpRotationMatrix &vpRotationMatrix::operator=(const vpMatrix &M)
{
  if ((M.getCols() != 3) && (M.getRows() != 3)) {
    throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }

  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      (*this)[i][j] = M[i][j];
    }
  }

  if (isARotationMatrix() == false) {
    throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
                                               "from a matrix that is not a "
                                               "rotation matrix"));
  }

  return *this;
}

vpRotationMatrix& vpRotationMatrix::operator<<(double val)
{
  m_index = 0;
  data[m_index] = val;
  return *this;
}

vpRotationMatrix& vpRotationMatrix::operator,(double val)
{
  m_index ++;
  if (m_index >= size()) {
    throw(vpException(vpException::dimensionError, "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize with %d elements", size(), m_index+1));
  }
  data[m_index] = val;
  return *this;
}

vpRotationMatrix vpRotationMatrix::operator*(const vpRotationMatrix &R) const
{
  vpRotationMatrix p;

  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      double s = 0;
      for (unsigned int k = 0; k < 3; k++)
        s += rowPtrs[i][k] * R.rowPtrs[k][j];
      p[i][j] = s;
    }
  }
  return p;
}
vpMatrix vpRotationMatrix::operator*(const vpMatrix &M) const
{
  if (M.getRows() != 3 || M.getCols() != 3) {
    throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }
  vpMatrix p(3, 3);

  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      double s = 0;
      for (unsigned int k = 0; k < 3; k++)
        s += (*this)[i][k] * M[k][j];
      p[i][j] = s;
    }
  }
  return p;
}

vpColVector vpRotationMatrix::operator*(const vpColVector &v) const
{
  if (v.getRows() != 3) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (3x3) rotation matrix by a %d "
                      "dimension column vector",
                      v.getRows()));
  }
  vpColVector v_out(3);

  for (unsigned int j = 0; j < colNum; j++) {
    double vj = v[j]; // optimization em 5/12/2006
    for (unsigned int i = 0; i < rowNum; i++) {
      v_out[i] += rowPtrs[i][j] * vj;
    }
  }

  return v_out;
}

vpTranslationVector vpRotationMatrix::operator*(const vpTranslationVector &tv) const
{
  vpTranslationVector p;

  for (unsigned int j = 0; j < 3; j++)
    p[j] = 0;

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

  return p;
}

vpRotationMatrix vpRotationMatrix::operator*(double x) const
{
  vpRotationMatrix R;

  for (unsigned int i = 0; i < rowNum; i++)
    for (unsigned int j = 0; j < colNum; j++)
      R[i][j] = rowPtrs[i][j] * x;

  return R;
}

vpRotationMatrix &vpRotationMatrix::operator*=(double x)
{
  for (unsigned int i = 0; i < rowNum; i++)
    for (unsigned int j = 0; j < colNum; j++)
      rowPtrs[i][j] *= x;

  return *this;
}

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

bool vpRotationMatrix::isARotationMatrix(double threshold) const
{
  bool isRotation = true;

  if (getCols() != 3 || getRows() != 3) {
    return false;
  }

  // test R^TR = Id ;
  vpRotationMatrix RtR = (*this).t() * (*this);
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      if (i == j) {
        if (fabs(RtR[i][j] - 1) > threshold) {
          isRotation = false;
        }
      } else {
        if (fabs(RtR[i][j]) > threshold) {
          isRotation = false;
        }
      }
    }
  }
  // test if it is a basis
  // test || Ci || = 1
  for (unsigned int i = 0; i < 3; i++) {
    if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) {
      isRotation = false;
    }
  }

  // test || Ri || = 1
  for (unsigned int i = 0; i < 3; i++) {
    if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) {
      isRotation = false;
    }
  }

  //  test if the basis is orthogonal
  return isRotation;
}

vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3), m_index(0) { eye(); }

vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }

vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(M); }

vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(tu); }

vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(p); }

vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(euler); }

vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }

vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }

vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }

vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(3, 3), m_index(0)
{
  buildFrom(tux, tuy, tuz);
}

vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(q); }

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(3, 3, list), m_index(0)
{
  if (! isARotationMatrix() ) {
    throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
  }
}
#endif

vpRotationMatrix vpRotationMatrix::t() const
{
  vpRotationMatrix Rt;

  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++)
      Rt[j][i] = (*this)[i][j];

  return Rt;
}

vpRotationMatrix vpRotationMatrix::inverse() const
{
  vpRotationMatrix Ri = (*this).t();

  return Ri;
}

void vpRotationMatrix::inverse(vpRotationMatrix &R) const { R = inverse(); }

void vpRotationMatrix::printVector()
{
  vpThetaUVector tu(*this);

  for (unsigned int i = 0; i < 3; i++)
    std::cout << tu[i] << "  ";

  std::cout << std::endl;
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpThetaUVector &v)
{
  double theta, si, co, sinc, mcosc;
  vpRotationMatrix R;

  theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  si = sin(theta);
  co = cos(theta);
  sinc = vpMath::sinc(si, theta);
  mcosc = vpMath::mcosc(co, theta);

  R[0][0] = co + mcosc * v[0] * v[0];
  R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
  R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
  R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
  R[1][1] = co + mcosc * v[1] * v[1];
  R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
  R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
  R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
  R[2][2] = co + mcosc * v[2] * v[2];

  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++)
      (*this)[i][j] = R[i][j];

  return *this;
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++)
      (*this)[i][j] = M[i][j];

  return *this;
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpPoseVector &p)
{
  vpThetaUVector tu(p);
  return buildFrom(tu);
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyzVector &v)
{
  double c0, c1, c2, s0, s1, s2;

  c0 = cos(v[0]);
  c1 = cos(v[1]);
  c2 = cos(v[2]);
  s0 = sin(v[0]);
  s1 = sin(v[1]);
  s2 = sin(v[2]);

  (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
  (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
  (*this)[0][2] = c0 * s1;
  (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
  (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
  (*this)[1][2] = s0 * s1;
  (*this)[2][0] = -s1 * c2;
  (*this)[2][1] = s1 * s2;
  (*this)[2][2] = c1;

  return (*this);
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpRxyzVector &v)
{
  double c0, c1, c2, s0, s1, s2;

  c0 = cos(v[0]);
  c1 = cos(v[1]);
  c2 = cos(v[2]);
  s0 = sin(v[0]);
  s1 = sin(v[1]);
  s2 = sin(v[2]);

  (*this)[0][0] = c1 * c2;
  (*this)[0][1] = -c1 * s2;
  (*this)[0][2] = s1;
  (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
  (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
  (*this)[1][2] = -s0 * c1;
  (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
  (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
  (*this)[2][2] = c0 * c1;

  return (*this);
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyxVector &v)
{
  double c0, c1, c2, s0, s1, s2;

  c0 = cos(v[0]);
  c1 = cos(v[1]);
  c2 = cos(v[2]);
  s0 = sin(v[0]);
  s1 = sin(v[1]);
  s2 = sin(v[2]);

  (*this)[0][0] = c0 * c1;
  (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
  (*this)[0][2] = c0 * s1 * c2 + s0 * s2;

  (*this)[1][0] = s0 * c1;
  (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
  (*this)[1][2] = s0 * s1 * c2 - c0 * s2;

  (*this)[2][0] = -s1;
  (*this)[2][1] = c1 * s2;
  (*this)[2][2] = c1 * c2;

  return (*this);
}

vpRotationMatrix vpRotationMatrix::buildFrom(double tux, double tuy, double tuz)
{
  vpThetaUVector tu(tux, tuy, tuz);
  buildFrom(tu);
  return *this;
}

vpRotationMatrix vpRotationMatrix::buildFrom(const vpQuaternionVector &q)
{
  double a = q.w();
  double b = q.x();
  double c = q.y();
  double d = q.z();
  (*this)[0][0] = a * a + b * b - c * c - d * d;
  (*this)[0][1] = 2 * b * c - 2 * a * d;
  (*this)[0][2] = 2 * a * c + 2 * b * d;

  (*this)[1][0] = 2 * a * d + 2 * b * c;
  (*this)[1][1] = a * a - b * b + c * c - d * d;
  (*this)[1][2] = 2 * c * d - 2 * a * b;

  (*this)[2][0] = 2 * b * d - 2 * a * c;
  (*this)[2][1] = 2 * a * b + 2 * c * d;
  (*this)[2][2] = a * a - b * b - c * c + d * d;
  return *this;
}

vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
{
  vpRotationMatrix C;

  unsigned int Rrow = R.getRows();
  unsigned int Rcol = R.getCols();

  for (unsigned int i = 0; i < Rrow; i++)
    for (unsigned int j = 0; j < Rcol; j++)
      C[i][j] = R[i][j] * x;

  return C;
}

vpThetaUVector vpRotationMatrix::getThetaUVector()
{
  vpThetaUVector tu;
  tu.buildFrom(*this);
  return tu;
}

vpColVector vpRotationMatrix::getCol(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;
}

vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
{
  vpMatrix meanR(3, 3);
  vpRotationMatrix R;
  for (size_t i = 0; i < vec_M.size(); i++) {
    R = vec_M[i].getRotationMatrix();
    meanR += (vpMatrix) R;
  }
  meanR /= static_cast<double>(vec_M.size());

  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
  vpMatrix M, U, V;
  vpColVector sv;
  meanR.pseudoInverse(M, sv, 1e-6, U, V);
  double det = sv[0]*sv[1]*sv[2];
  if (det > 0) {
    meanR = U * V.t();
  }
  else {
    vpMatrix D(3,3);
    D = 0.0;
    D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
    meanR = U * D * V.t();
  }

  R = meanR;
  return R;
}

vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
{
  vpMatrix meanR(3, 3);
  vpRotationMatrix R;
  for (size_t i = 0; i < vec_R.size(); i++) {
    meanR += (vpMatrix) vec_R[i];
  }
  meanR /= static_cast<double>(vec_R.size());

  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
  vpMatrix M, U, V;
  vpColVector sv;
  meanR.pseudoInverse(M, sv, 1e-6, U, V);
  double det = sv[0]*sv[1]*sv[2];
  if (det > 0) {
    meanR = U * V.t();
  }
  else {
    vpMatrix D(3,3);
    D = 0.0;
    D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
    meanR = U * D * V.t();
  }

  R = meanR;
  return R;
}

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

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

#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)