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()) {
    if (isARotationMatrix(1e-3)) {
      orthogonalize();
    } else {
      throw(vpException(
          vpException::fatalError,
          "Rotation matrix initialization fails since its elements do not represent a valid 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()) {
    if (isARotationMatrix(1e-3)) {
      orthogonalize();
    } else {
      throw(vpException(
          vpException::fatalError,
          "Rotation matrix initialization fails since its elements do not represent a valid 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;
}
 
void vpRotationMatrix::orthogonalize()
{
  vpMatrix U = *this;
  vpColVector w;
  vpMatrix V;
  U.svd(w, V);
  vpMatrix Vt = V.t();
  vpMatrix R = U * Vt;
 
  double det = R.det();
  if (det < 0) {
    Vt[2][0] *= -1;
    Vt[2][1] *= -1;
    Vt[2][2] *= -1;
 
    R = U * Vt;
  }
 
  data[0] = R[0][0];
  data[1] = R[0][1];
  data[2] = R[0][2];
  data[3] = R[1][0];
  data[4] = R[1][1];
  data[5] = R[1][2];
  data[6] = R[2][0];
  data[7] = R[2][1];
  data[8] = R[2][2];
}
 
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
 
void vpRotationMatrix::setIdentity() { eye(); }
 
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)