ExampleRotationAveraging.cpp

#include <Geometry/RotationAveraging.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Random.hh>
#include <Geometry/RMatrix.hh>
#include <fstream>
#include <string>
#include <vector>

using namespace std;
using namespace BIAS;

/**
  @example ExampleRotationAveraging
  @brief   Example for usage of rotation averaging. Compares different
           algorithms to compute the mean of rotations with respect to
           some distance measure d: SO(3) x SO(3) -> R.
  @see     RotationAveraging
  @ingroup g_examples
  @author  esquivel
  @date    09/2011
 */

double AngleError(const BIAS::RMatrix &R, const BIAS::RMatrix &S)
{
  BIAS::RMatrix dR = R * S.Transpose();
  double dAngle = dR.GetRotationAngle();
  return std::min(fabs(dAngle), std::min(fabs(dAngle + 2*M_PI), fabs(dAngle - 2*M_PI)));
}

double AngleError(const BIAS::Quaternion<double> &Q, const BIAS::Quaternion<double> &P)
{
  BIAS::Quaternion<double> dQ = Q * P.Inverse();
  double dAngle = dQ.GetRotationAngle();
  return std::min(fabs(dAngle), std::min(fabs(dAngle + 2*M_PI), fabs(dAngle - 2*M_PI)));
}

void AccumulateError(double error, double &sum, double &sqsum, unsigned int &count)
{
  sum += error;
  sqsum += error*error;
  count++;
}

int main(int argc, char** argv)
{
  const unsigned int trials = 100;
  const double sigma = 2.0 * M_PI / 180.0;
  const unsigned int minNum = 10, maxNum = 80, numStep = 10;
  const unsigned int range = (maxNum - minNum) / numStep + 1;
  const unsigned int methods = 7;
  std::vector<std::vector<double> > avgErrors(methods, std::vector<double>(range, 0.0));
  std::vector<std::vector<double> > stdErrors(methods, std::vector<double>(range, 0.0));
  std::vector<std::string> methodNames(methods);
  methodNames[0] = "angle/axis L2-mean";
  methodNames[1] = "quaternion L2-mean";
  methodNames[2] = "geodesic L2-mean";
  methodNames[3] = "geodesic L1-mean";
  methodNames[4] = "chordal L2-mean";
  methodNames[5] = "quaternion L2 (quat.)";
  methodNames[6] = "chordal L2 (quat.)";

  BIAS::Random randomizer;
  BIAS::RotationAveraging algorithm;
  algorithm.SetTolerance(1e-12);
  algorithm.SetMaxIterations(25);

  cout << "Computing mean rotation for " << minNum << " to " << maxNum
       << " rotation measurements with angular error of sigma = "
       << sigma * 180.0 / M_PI << " degree for " << trials << " trials" << endl;
  for (unsigned int k = 0; k < range; k++)
  {
    const unsigned int num = minNum + k * numStep;
    std::vector<unsigned int> numSuccess(methods, 0);
    for (unsigned int t = 0; t < trials; t++)
    {
      // Compute ground truth rotation R_gt
      BIAS::RMatrix gtR;
      BIAS::Vector3<double> gtAxis;
      for (unsigned int j = 0; j < 3; j++)
        gtAxis[j] = randomizer.GetUniformDistributed(-1.0, 1.0);
      gtAxis.Normalize();
      double gtAngle = randomizer.GetUniformDistributed(0.0, M_PI/2);
      gtR.SetFromAxisAngle(gtAngle * gtAxis);
      BIAS::Quaternion<double> gtQ;
      gtR.GetQuaternion(gtQ);

      // Compute n random measurements R_i for R_gt with noise
      std::vector<BIAS::RMatrix> measR;
      measR.reserve(num);
      std::vector<BIAS::Quaternion<double> > measQ;
      measQ.reserve(num);
      for (unsigned int i = 0; i < num; i++)
      {
        BIAS::Vector3<double> errAxis;
        for (unsigned int j = 0; j < 3; j++)
          errAxis[j] = randomizer.GetUniformDistributed(-1.0, 1.0);
        errAxis.Normalize();
        double errAngle = randomizer.GetNormalDistributed(0.0, sigma);
        BIAS::RMatrix errR;
        errR.SetFromAxisAngle(errAngle * errAxis);
        measR.push_back(gtR * errR);
        BIAS::Quaternion<double> errQ;
        errR.GetQuaternion(errQ);
        measQ.push_back(gtQ * errQ);
      }
      // Compute mean rotation with different methods
      BIAS::RMatrix meanR;
      BIAS::Quaternion<double> meanQ;
      if (algorithm.LogarithmicL2Mean(measR, meanR) == 0)
        AccumulateError(AngleError(gtR, meanR),
                        avgErrors[0][k], stdErrors[0][k], numSuccess[0]);
      if (algorithm.QuaternionL2Mean(measR, meanR) == 0)
        AccumulateError(AngleError(gtR, meanR),
                        avgErrors[1][k], stdErrors[1][k], numSuccess[1]);
      if (algorithm.GeodesicL2Mean(measR, meanR) == 0)
        AccumulateError(AngleError(gtR, meanR),
                        avgErrors[2][k], stdErrors[2][k], numSuccess[2]);
      if (algorithm.GeodesicL1Mean(measR, meanR) == 0)
        AccumulateError(AngleError(gtR, meanR),
                        avgErrors[3][k], stdErrors[3][k], numSuccess[3]);
      if (algorithm.ChordalL2Mean(measR, meanR) == 0)
        AccumulateError(AngleError(gtR, meanR),
                        avgErrors[4][k], stdErrors[4][k], numSuccess[4]);
      if (algorithm.QuaternionL2Mean(measQ, meanQ) == 0)
        AccumulateError(AngleError(gtQ, meanQ),
                        avgErrors[5][k], stdErrors[5][k], numSuccess[5]);
      if (algorithm.ChordalL2Mean(measQ, meanQ) == 0)
        AccumulateError(AngleError(gtQ, meanQ),
                        avgErrors[6][k], stdErrors[6][k], numSuccess[6]);
      cout << "\r" << (int)(100.0 * (k*trials+t+1) / (double)(range*trials))
           << "% done..." << flush;
    }
    // Compute mean estimation error
    for (unsigned int m = 0; m < methods; m++) {
      avgErrors[m][k] /= (double)numSuccess[m];
      stdErrors[m][k] = sqrt(stdErrors[m][k] / (double)numSuccess[m] -
                             avgErrors[m][k]*avgErrors[m][k]);
    }
  }
  cout << endl;

  // Print results
  printf(" %21s ", "Average errors");
  for (unsigned int k = 0; k < range; k++)
    printf("| %8d ", minNum + k * numStep);
  cout << "rotations" << endl << flush;
  for (unsigned int i = 0; i < 23; i++)
    cout << "-";
  for (unsigned int k = 0; k < range; k++)
    cout << "+----------";
  cout << "---------" << endl << flush;
  for (unsigned int m = 0; m < methods; m++) {
    printf(" %21s ", methodNames[m].c_str());
    for (unsigned int k = 0; k < range; k++)
      printf("| %6f ", avgErrors[m][k]*180.0/M_PI);
    cout << "degree" << endl << flush;
    printf(" %18s +- ", "");
    for (unsigned int k = 0; k < range; k++)
      printf("| %6f ", stdErrors[m][k]*180.0/M_PI);
    cout << "degree" << endl << flush;
    for (unsigned int i = 0; i < 23; i++)
      cout << "-";
    for (unsigned int k = 0; k < range; k++)
      cout << "+----------";
    cout << "---------" << endl << flush;
  }

  // Write results to log file
  const std::string filename = "ExampleRotationAveraging.log";
  std::ofstream fout(filename.c_str());
  for (unsigned int k = 0; k < range; k++)
    fout << minNum + k * numStep << " ";
  fout << endl << flush;
  for (unsigned int m = 0; m < methods; m++) {
    for (unsigned int k = 0; k < range; k++)
      fout << avgErrors[m][k] << " ";
    fout << endl << flush;
  }
  fout.close();
  cout << "Results written to file \"" << filename << "\"." << endl;

  // Write results to Matlab file
  const std::string mfilename = "ExampleRotationAveraging.m";
  std::ofstream mfout(mfilename.c_str());
  mfout << "x = [ ";
  for (unsigned int k = 0; k < range; k++)
    mfout << minNum + k * numStep << " ";
  mfout << "];" << endl << flush;
  mfout << "y = [ ";
  for (unsigned int m = 0; m < methods; m++) {
    for (unsigned int k = 0; k < range; k++)
      mfout << avgErrors[m][k] << " ";
    if (m+1 == methods)
      mfout << "];" << endl << flush;
    else
      mfout << endl << "      " << flush;
  }
  mfout << "plot(x, y*180/pi)" << endl;
  mfout << "legend(";
  for (unsigned int m = 0; m < methods; m++) {
    mfout << "'" << methodNames[m] << "'";
    if (m+1 < methods) mfout << ", ";
  }
  mfout << ")" << endl << flush;
  mfout.close();
  cout << "Created Matlab plot file \"" << mfilename << "\"." << endl;

  return 0;
}