RotationAveraging.cpp

#include "RotationAveraging.hh"

#include <Base/Math/Matrix.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Vector.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Geometry/RMatrix.hh>
#include <MathAlgo/SVD3x3.hh>

using namespace std;
using namespace BIAS;

RotationAveraging::RotationAveraging() : tolerance_(1e-10), maxIters_(10)
{
  NewDebugLevel("ROTATIONAVERGAGE_ITERS");
  //AddDebugLevel("ROTATIONAVERGAGE_ITERS");
}

int RotationAveraging::
GeodesicL1Mean(const std::vector<BIAS::RMatrix> &R, BIAS::RMatrix &meanR)
{
  BIASASSERT(TestRotations(R));
  const unsigned int n = R.size();
  BIAS::RMatrix resR, meanResR;
  BIAS::Vector3<double> resW, meanResW;
  double scale;
  int res = GeodesicL2Mean(R, meanR);
  if (res != 0) {
    BIASERR("Initialization of mean rotation via geodesic L2-mean failed!");
    return res;
  }
  double updateAngle; //, lastUpdateAngle = 0.0;
  for (unsigned int iter = 0; iter < maxIters_; iter++)
  {
    // Compute mean normalized residual rotation
    meanResW.SetZero();
    scale = 0.0;
    for (unsigned int i = 0; i < n; i++)
    {
      resR = R[i] * meanR.Transpose();
      resR.GetRotationAxisAngle(resW);
      if (BIAS::Equal(resW.NormL2(), 0.0))
        continue;
      scale += 1.0 / resW.NormL2();
      resW.Normalize();
      meanResW += resW;
    }
    if (!BIAS::Equal(scale, 0.0))
      meanResW.MultiplyIP(1.0 / scale);
    // Update mean rotation
    updateAngle = meanResW.NormL2();
    BCDOUT(ROTATIONAVERGAGE_ITERS, "Geodesic L1-mean : Iteration " << iter
           << " : update angle = " << (updateAngle * 180.0 / M_PI) << " degree" << endl);
    //if (fabs(updateAngle - lastUpdateAngle) < tolerance_)
    if (updateAngle < tolerance_)
      break;
    else {
      //lastUpdateAngle = updateAngle;
      meanResW.Normalize();
      meanResR.Set(meanResW, updateAngle);
      meanR = meanResR * meanR;
    }
  }
  return 0;
}

int RotationAveraging::
GeodesicL2Mean(const std::vector<BIAS::RMatrix> &R, BIAS::RMatrix &meanR)
{
  BIASASSERT(TestRotations(R));
  const unsigned int n = R.size();
  BIAS::RMatrix resR, meanResR;
  BIAS::Vector3<double> resW, meanResW;
  meanR = R[0];
  double updateAngle; //, lastUpdateAngle = 0.0;
  for (unsigned int iter = 0; iter < maxIters_; iter++)
  {
    // Compute mean residual rotation
    meanResW.SetZero();
    for (unsigned int i = 0; i < n; i++)
    {
      resR = meanR.Transpose() * R[i];
      resR.GetRotationAxisAngle(resW);
      meanResW += resW;
    }
    meanResW /= (double)n;
    // Update mean rotation
    updateAngle = meanResW.NormL2();
    BCDOUT(ROTATIONAVERGAGE_ITERS, "Geodesic L2-mean : Iteration " << iter
           << " : update angle = " << (updateAngle * 180.0 / M_PI) << " degree" << endl);
    //if (fabs(updateAngle - lastUpdateAngle) < tolerance_)
    if (updateAngle < tolerance_)
      break;
    else {
      //lastUpdateAngle = updateAngle;
      meanResW.Normalize();
      meanResR.Set(meanResW, updateAngle);
      meanR *= meanResR;
    }
  }
  return 0;
}

int RotationAveraging::
QuaternionL2Mean(const std::vector<BIAS::RMatrix> &R, BIAS::RMatrix &meanR)
{
  BIAS::Quaternion<double> q;
  std::vector<BIAS::Quaternion<double> > Q;
  Q.reserve(R.size());
  for (unsigned int i = 0; i < R.size(); i++) {
    R[i].GetQuaternion(q);
    Q.push_back(q);
  }
  BIAS::Quaternion<double> meanQ;
  int res = QuaternionL2Mean(Q, meanQ);
  if (res == 0)
    meanR.SetFromQuaternion(meanQ);
  return res;
}

int RotationAveraging::
QuaternionL2Mean(const std::vector<BIAS::Quaternion<double> > &Q,
                 BIAS::Quaternion<double> &meanQ)
{
  BIASASSERT(TestRotations(Q));
  meanQ.SetZero();
  for (unsigned int i = 0; i < Q.size(); i++) {
    if (Q[i][3] < 0)
      meanQ -= Q[i];
    else
      meanQ += Q[i];
  }
  meanQ.MultiplyIP(1.0 / (double)Q.size());
  meanQ.Normalize();
  return 0;
}

int RotationAveraging::
LogarithmicL2Mean(const std::vector<BIAS::RMatrix> &R, BIAS::RMatrix &meanR)
{
  BIASASSERT(TestRotations(R));
  BIAS::Vector3<double> w, meanW(0.0, 0.0, 0.0);
  for (unsigned int i = 0; i < R.size(); i++) {
    R[i].GetRotationAxisAngle(w);
    meanW += w;
  }
  meanW.MultiplyIP(1.0 / (double)R.size());
  meanR.SetFromAxisAngle(meanW);
  return 0;
}

int RotationAveraging::
ChordalL2Mean(const std::vector<BIAS::RMatrix> &R, BIAS::RMatrix &meanR)
{
  // Implementation of the first algorithm in sec. 3.1 [2]
  BIASASSERT(TestRotations(R));
  BIAS::Matrix3x3<double> S(BIAS::MatrixZero);
  for (unsigned int i = 0; i < R.size(); i++)
    S += R[i];
  S /= (double)R.size();
  meanR = S;
  // Set R = U * V^T with S = U * D * V^T calculated from SVD,
  // multiply R by -1 if determinant det(R) is negative
  meanR.EnforceConstraints();
  return 0;
}

int RotationAveraging::
ChordalL2Mean(const std::vector<BIAS::Quaternion<double> > &Q,
              BIAS::Quaternion<double> &meanQ)
{
  // Implementation of the second algorithm in sec. 3.1 [2]
  BIASASSERT(TestRotations(Q));
  meanQ.SetZero();
  BIAS::Matrix<double> A(4, 4, BIAS::MatrixZero);
  for (unsigned int i = 0; i < Q.size(); i++)
    for (unsigned int j = 0; j < 4; j++)
      for (unsigned int k = 0; k < 4; k++)
        A[j][k] += Q[i][j]*Q[i][k];
  BIAS::SVD svd(A);
  meanQ = BIAS::Quaternion<double>(svd.GetV().GetCol(0));
  meanQ.Normalize();
  return 0;
}

bool RotationAveraging::
TestRotations(const std::vector<BIAS::RMatrix> &R)
{
  if (R.empty()) {
    BIASERR("No input rotations given!");
    return false;
  }
  for (unsigned int i = 0; i < R.size(); i++) {
    if (BIAS::GreaterEqual(fabs(R[i].GetRotationAngle()), 180.0)) {
      BIASERR("Input rotation angle larger than 180 degree!");
      return false;
    }
  }
  return true;
}

bool RotationAveraging::
TestRotations(const std::vector<BIAS::Quaternion<double> > &Q)
{
  if (Q.empty()) {
    BIASERR("No input rotations given!");
    return false;
  }
  for (unsigned int i = 0; i < Q.size(); i++) {
    if (BIAS::GreaterEqual(fabs(Q[i].GetRotationAngle()), 180.0)) {
      BIASERR("Input rotation angle is larger than 180 degree!");
      return false;
    }
  }
  return true;
}