vpRotationMatrix.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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:
 * Rotation matrix.
 */

 
#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>
 
BEGIN_VISP_NAMESPACE
const unsigned int vpRotationMatrix::constr_val_3 = 3;
void vpRotationMatrix::eye()
{
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_3; ++j) {
      if (i == j) {
        (*this)[i][j] = 1.0;
      }
      else {
        (*this)[i][j] = 0.0;
      }
    }
  }
}

vpRotationMatrix &vpRotationMatrix::operator=(const vpRotationMatrix &R)
{
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_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)
{
  const unsigned int val_3 = 3;
  if ((M.getCols() != val_3) && (M.getRows() != val_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 < val_3; ++i) {
    for (unsigned int j = 0; j < val_3; ++j) {
      (*this)[i][j] = M[i][j];
    }
  }
  if (!isARotationMatrix()) {
    if (isARotationMatrix(1e-3)) {
      orthogonalize();
    }
    else {
      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;
 
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_3; ++j) {
      double s = 0;
      for (unsigned int k = 0; k < val_3; ++k) {
        s += rowPtrs[i][k] * R.rowPtrs[k][j];
      }
      p[i][j] = s;
    }
  }
  return p;
}

vpMatrix vpRotationMatrix::operator*(const vpMatrix &M) const
{
  const unsigned int val_3 = 3;
  if ((M.getRows() != val_3) || (M.getCols() != val_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 < val_3; ++i) {
    for (unsigned int j = 0; j < val_3; ++j) {
      double s = 0;
      for (unsigned int k = 0; k < val_3; ++k) {
        s += (*this)[i][k] * M[k][j];
      }
      p[i][j] = s;
    }
  }
  return p;
}

vpHomogeneousMatrix vpRotationMatrix::operator*(const vpHomogeneousMatrix &M) const
{
  return (vpHomogeneousMatrix(*this * M.getTranslationVector(), *this * M.getRotationMatrix()));
}

vpColVector vpRotationMatrix::operator*(const vpColVector &v) const
{
  const unsigned int rows_size = 3;
  if (v.getRows() != rows_size) {
    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;
  const unsigned int val_3 = 3;
 
  for (unsigned int j = 0; j < val_3; ++j) {
    p[j] = 0;
  }
 
  for (unsigned int j = 0; j < val_3; ++j) {
    for (unsigned int i = 0; i < val_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;
  const unsigned int val_3 = 3;
 
  if ((getCols() != val_3) || (getRows() != val_3)) {
    return false;
  }
 
  // --comment: test R^TR = Id
  vpRotationMatrix RtR = (*this).t() * (*this);
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_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
  const unsigned int index_2 = 2;
  for (unsigned int i = 0; i < val_3; ++i) {
    if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[index_2][i])) - 1) > threshold) {
      isRotation = false;
    }
  }
 
  // test || Ri || = 1
  for (unsigned int i = 0; i < val_3; ++i) {
    if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][index_2])) - 1) > threshold) {
      isRotation = false;
    }
  }
 
  //  test if the basis is orthogonal
  return isRotation;
}

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

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

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

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

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

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

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

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

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

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

vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(constr_val_3, constr_val_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;
 
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_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);
 
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    std::cout << tu[i] << "  ";
  }
 
  std::cout << std::endl;
}

vpRotationMatrix &vpRotationMatrix::buildFrom(const vpThetaUVector &v)
{
  double theta, si, co, sinc, mcosc;
  vpRotationMatrix R;
 
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  theta = sqrt((v[index_0] * v[index_0]) + (v[index_1] * v[index_1]) + (v[index_2] * v[index_2]));
  si = sin(theta);
  co = cos(theta);
  sinc = vpMath::sinc(si, theta);
  mcosc = vpMath::mcosc(co, theta);
 
  R[index_0][index_0] = co + (mcosc * v[index_0] * v[index_0]);
  R[index_0][index_1] = (-sinc * v[index_2]) + (mcosc * v[index_0] * v[index_1]);
  R[index_0][index_2] = (sinc * v[index_1])  + (mcosc * v[index_0] * v[index_2]);
  R[index_1][index_0] = (sinc * v[index_2])  + (mcosc * v[index_1] * v[index_0]);
  R[index_1][index_1] = co + (mcosc * v[index_1] * v[index_1]);
  R[index_1][index_2] = (-sinc * v[index_0]) + (mcosc * v[index_1] * v[index_2]);
  R[index_2][index_0] = (-sinc * v[index_1]) + (mcosc * v[index_2] * v[index_0]);
  R[index_2][index_1] = (sinc * v[index_0])  + (mcosc * v[index_2] * v[index_1]);
  R[index_2][index_2] = co + (mcosc * v[index_2] * v[index_2]);
 
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_3; ++j) {
      (*this)[i][j] = R[i][j];
    }
  }
 
  return *this;
}

vpRotationMatrix &vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  const unsigned int val_3 = 3;
  for (unsigned int i = 0; i < val_3; ++i) {
    for (unsigned int j = 0; j < val_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;
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
 
  c0 = cos(v[index_0]);
  c1 = cos(v[index_1]);
  c2 = cos(v[index_2]);
  s0 = sin(v[index_0]);
  s1 = sin(v[index_1]);
  s2 = sin(v[index_2]);
 
  (*this)[index_0][index_0] = (c0 * c1 * c2) - (s0 * s2);
  (*this)[index_0][index_1] = (-c0 * c1 * s2) - (s0 * c2);
  (*this)[index_0][index_2] = c0 * s1;
  (*this)[index_1][index_0] = (s0 * c1 * c2) + (c0 * s2);
  (*this)[index_1][index_1] = (-s0 * c1 * s2) + (c0 * c2);
  (*this)[index_1][index_2] = s0 * s1;
  (*this)[index_2][index_0] = -s1 * c2;
  (*this)[index_2][index_1] = s1 * s2;
  (*this)[index_2][index_2] = c1;
 
  return *this;
}

vpRotationMatrix &vpRotationMatrix::buildFrom(const vpRxyzVector &v)
{
  double c0, c1, c2, s0, s1, s2;
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
 
  c0 = cos(v[index_0]);
  c1 = cos(v[index_1]);
  c2 = cos(v[index_2]);
  s0 = sin(v[index_0]);
  s1 = sin(v[index_1]);
  s2 = sin(v[index_2]);
 
  (*this)[index_0][index_0] = c1 * c2;
  (*this)[index_0][index_1] = -c1 * s2;
  (*this)[index_0][index_2] = s1;
  (*this)[index_1][index_0] = (c0 * s2) + (s0 * s1 * c2);
  (*this)[index_1][index_1] = (c0 * c2) - (s0 * s1 * s2);
  (*this)[index_1][index_2] = -s0 * c1;
  (*this)[index_2][index_0] = (-c0 * s1 * c2) + (s0 * s2);
  (*this)[index_2][index_1] = (c0 * s1 * s2) + (c2 * s0);
  (*this)[index_2][index_2] = c0 * c1;
 
  return *this;
}

vpRotationMatrix &vpRotationMatrix::buildFrom(const vpRzyxVector &v)
{
  double c0, c1, c2, s0, s1, s2;
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
 
  c0 = cos(v[index_0]);
  c1 = cos(v[index_1]);
  c2 = cos(v[index_2]);
  s0 = sin(v[index_0]);
  s1 = sin(v[index_1]);
  s2 = sin(v[index_2]);
 
  (*this)[index_0][index_0] = c0 * c1;
  (*this)[index_0][index_1] = (c0 * s1 * s2) - (s0 * c2);
  (*this)[index_0][index_2] = (c0 * s1 * c2) + (s0 * s2);
 
  (*this)[index_1][index_0] = s0 * c1;
  (*this)[index_1][index_1] = (s0 * s1 * s2) + (c0 * c2);
  (*this)[index_1][index_2] = (s0 * s1 * c2) - (c0 * s2);
 
  (*this)[index_2][index_0] = -s1;
  (*this)[index_2][index_1] = c1 * s2;
  (*this)[index_2][index_2] = c1 * c2;
 
  return *this;
}

vpRotationMatrix &vpRotationMatrix::buildFrom(const double &tux, const double &tuy, const 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();
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  (*this)[index_0][index_0] = (((a * a) + (b * b)) - (c * c)) - (d * d);
  (*this)[index_0][index_1] = (2.0 * b * c) - (2.0 * a * d);
  (*this)[index_0][index_2] = (2.0 * a * c) + (2.0 * b * d);
 
  (*this)[index_1][index_0] = (2.0 * a * d) + (2.0 * b * c);
  (*this)[index_1][index_1] = (((a * a) - (b * b)) + (c * c)) - (d * d);
  (*this)[index_1][index_2] = (2.0 * c * d) - (2.0 * a * b);
 
  (*this)[index_2][index_0] = (2.0 * b * d) - (2.0 * a * c);
  (*this)[index_2][index_1] = (2.0 * a * b) + (2.0 * c * d);
  (*this)[index_2][index_2] = ((a * a) - (b * b) - (c * c)) + (d * d);
  return *this;
}

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;
  size_t vec_m_size = vec_M.size();
  for (size_t i = 0; i < vec_m_size; ++i) {
    R = vec_M[i].getRotationMatrix();
    meanR += static_cast<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);
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  double det = sv[index_0] * sv[index_1] * sv[index_2];
  if (det > 0) {
    meanR = U * V.t();
  }
  else {
    vpMatrix D(3, 3);
    D = 0.0;
    D[index_0][index_0] = 1.0;
    D[index_1][index_1] = 1.0;
    D[index_2][index_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;
  size_t vec_r_size = vec_R.size();
  for (size_t i = 0; i < vec_r_size; ++i) {
    meanR += static_cast<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);
  const unsigned int index_0 = 0;
  const unsigned int index_1 = 1;
  const unsigned int index_2 = 2;
  double det = sv[index_0] * sv[index_1] * sv[index_2];
  if (det > 0) {
    meanR = U * V.t();
  }
  else {
    vpMatrix D(3, 3);
    D = 0.0;
    D[index_0][index_0] = 1.0;
    D[index_1][index_1] = 1.0;
    D[index_2][index_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;
  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 index_4 = 4;
  const unsigned int index_5 = 5;
  const unsigned int index_6 = 6;
  const unsigned int index_7 = 7;
  const unsigned int index_8 = 8;
 
  double det = R.det();
  if (det < 0) {
    Vt[index_2][index_0] *= -1;
    Vt[index_2][index_1] *= -1;
    Vt[index_2][index_2] *= -1;
 
    R = U * Vt;
  }
 
  data[index_0] = R[index_0][index_0];
  data[index_1] = R[index_0][index_1];
  data[index_2] = R[index_0][index_2];
  data[index_3] = R[index_1][index_0];
  data[index_4] = R[index_1][index_1];
  data[index_5] = R[index_1][index_2];
  data[index_6] = R[index_2][index_0];
  data[index_7] = R[index_2][index_1];
  data[index_8] = R[index_2][index_2];
}
 
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

void vpRotationMatrix::setIdentity() { eye(); }
 
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

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;
}
END_VISP_NAMESPACE