TestRotationConversion.cpp

/*
 This file is part of the BIAS library (Basic ImageAlgorithmS).

 Copyright (C) 2003-2009    (see file CONTACT for details)
 Multimediale Systeme der Informationsverarbeitung
 Institut fuer Informatik
 Christian-Albrechts-Universitaet Kiel


 BIAS is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 BIAS is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with BIAS; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/**
   @file TestRotationConversion.cpp
   @relates Quaternion
   @brief Test the conversion between rotation representations for
          consistency (quaternion <-> rotation matrix)
   @ingroup g_tests
   @author MIP
*/

#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Base/Math/Random.hh>

using namespace BIAS;
using namespace std;


Random randomizer;

double maxerror = -1.0;

const double EPS = 5e-10;

Quaternion<double> GenerateRandomQuaternion();

Quaternion<double> GenerateRandomQuaternion(double angle);

RMatrixBase GenerateRandomRMatrix();

RMatrixBase GenerateRandomRMatrix(double angle);

void TestConversionQuaternionMatrix();

bool CompareQuaternions(const Quaternion<double> &Q1, 
                        const Quaternion<double> &Q2);

bool CompareRMatrices(const RMatrixBase &R1, 
                      const RMatrixBase &R2);

int main(int argc, char *argv[])
{
  TestConversionQuaternionMatrix();

  return 0;
}

Quaternion<double> GenerateRandomQuaternion()
{
  double angle = randomizer.GetUniformDistributed(-2.0*M_PI, 2.0*M_PI);
  return GenerateRandomQuaternion(angle);
}

Quaternion<double> GenerateRandomQuaternion(double angle)
{
  Quaternion<double> Q;
  Q[3] = cos(0.5*angle);
  Q[0] = Q[1] = Q[2] = sin(0.5*angle);
  Vector3<double> axis;
  do {
    axis.Set(randomizer.GetUniformDistributed(-1.0, 1.0),
             randomizer.GetUniformDistributed(-1.0, 1.0),     
             randomizer.GetUniformDistributed(-1.0, 1.0));
  } while (Equal(axis.NormL2(), 0.0));
  axis.Normalize();
  for (int j = 0; j < 3; j++) Q[j] *= axis[j];
  Q.Normalize();
  return Q;
}

bool CompareQuaternions(const Quaternion<double> &Q1, 
                        const Quaternion<double> &Q2)
{
  // check unit norm constraint
  if (Equal(Q1.NormL2(), 0.0)) { BIASABORT; }
  if (Equal(Q2.NormL2(), 0.0)) { BIASABORT; }

  // multiplication with inverse must result in [0 0 0 +-1]
  const Quaternion<double> Qid(0, 0, 0, 1);
  Quaternion<double> Q(Q1);
  Q.Mult(Q2.Inverse());

  // compare entries with identity quaternion
  bool ok = true;
  for (int j = 0; j < 3; j++){
    if (fabs(fabs(Q[j])-Qid[j]) > maxerror)
      maxerror = fabs(fabs(Q[j])-Qid[j]);
    if (!Equal(fabs(Q[j]), Qid[j], EPS))
      ok = false;
  }
  if (!ok) {
    BIASERR("Inconsistent conversion quaternion -> rotation matrix -> "
            "quaternion:\n- srcQ*dstQ.Inverse() = " <<Q/Q.NormL2()
            <<"\n- srcQ = "<<Q1<<"\n- dstQ = "<<Q2<<"\n max. error = "
            <<maxerror<< endl);
  }
  return ok;
}

RMatrixBase GenerateRandomRMatrix()
{
  double angle = randomizer.GetUniformDistributed(-2.0*M_PI, 2.0*M_PI);
  return GenerateRandomRMatrix(angle);
}

RMatrixBase GenerateRandomRMatrix(double angle)
{
  RMatrixBase R;
  Vector3<double> axis;
  do {
    axis.Set(randomizer.GetUniformDistributed(-1.0, 1.0),
             randomizer.GetUniformDistributed(-1.0, 1.0),     
             randomizer.GetUniformDistributed(-1.0, 1.0));
  } while (Equal(axis.NormL2(), 0.0));
  axis.Normalize();
  R.Set(axis, angle);
  return R;
}

bool CompareRMatrices(const RMatrixBase &R1, 
                      const RMatrixBase &R2)
{
  // multiplication with inverse must result in I
  const RMatrixBase I(MatrixIdentity);
  Matrix3x3<double> R(R1);
  R *= R2.Transpose();

  // compare entries with I
  bool ok = true;
  for (int j = 0; j < 3; j++){
    for (int k = 0; k < 3; k++){
      if (fabs(R[j][k]-I[j][k]) > maxerror)
        maxerror = fabs(R[j][k]-I[j][k]);
      if (!Equal(R[j][k], I[j][k], EPS))
        ok = false;
    }
  }
  
  if (!ok) {
    BIASERR("Inconsistent conversion rotation matrix -> quaternion -> "
            "rotation matrix:\n- srcR*dstR.Inverse() = " <<R
            <<"\n- srcR = "<<R1<<"\n- dstR = "<<R2<<"\n max. error = "
            <<maxerror<< endl);
  }
  return ok;
}

void TestConversionQuaternionMatrix()
{
  // Test with random rotations
  {
    const int num = 10000;
    
    cout << "\ntest rotation conversion for " << num << " random rotations"
         << "\n------------------------------------------------------\n"
         << "- test quaternion -> rotation matrix -> quaternion : ";
    Quaternion<double> srcQ, dstQ;
    RMatrixBase R;
    maxerror = -1.0;
    for (int n = 0; n < num; n++)
    {
      srcQ = GenerateRandomQuaternion();
      if (R.SetFromQuaternion(srcQ) != 0) { BIASABORT; }
      if (R.GetQuaternion(dstQ) != 0) { BIASABORT; }
      if (!CompareQuaternions(srcQ, dstQ)){
        cout << "max. error = " << setprecision(30) << maxerror << endl;
        cerr << "[!] Error for sample id " << n
             << " : source quaternion " << srcQ/srcQ.NormL2()
             << ", resulting quaternion " << dstQ/dstQ.NormL2() << endl;
        BIASABORT;
      }
    }
    cout << "max. error = " << setprecision(30) << maxerror << endl;

    cout << "- test rotation matrix -> quaternion -> rotation matrix : ";
    Quaternion<double> Q;
    RMatrixBase srcR, dstR;
    maxerror = -1.0;
    for (int n = 0; n < num; n++)
    {
      srcR = GenerateRandomRMatrix();
      if (srcR.GetQuaternion(Q) != 0) { BIASABORT; }
      if (dstR.SetFromQuaternion(Q) != 0) { BIASABORT; }
      if (!CompareRMatrices(srcR, dstR)) {
        cout << "max. error = " << setprecision(30) << maxerror << endl;
        cerr << "[!] Error for sample id " << n
             << ", source rotation matrix " << srcR
             << ", resulting rotation matrix " << dstR << endl;
        BIASABORT;
      }
    }
    cout << "max. error = " << setprecision(30) << maxerror << endl;
  }

  // Test with rotations about -360, ..., 0, 90, 180, 270, 360 degree
  {
    const int num = 1000;
    const double angleStep = 0.5*M_PI;
    for (int k = -4; k <= 4; k++)
    {
      const double angle = k*angleStep;
      cout << "\ntest rotation conversion for " << num << " rotations "
           << "with rotation angle " << angle*180.0/M_PI << " degree"
           << "\n------------------------------------------------------\n"
           << "- test quaternion -> rotation matrix -> quaternion : ";
      Quaternion<double> srcQ, dstQ;
      RMatrixBase R;
      maxerror = -1.0;
      for (int n = 0; n < num; n++)
      {
        srcQ = GenerateRandomQuaternion(angle);
        if (R.SetFromQuaternion(srcQ) != 0) { BIASABORT; }
        if (R.GetQuaternion(dstQ) != 0) { BIASABORT; }
        if (!CompareQuaternions(srcQ, dstQ)) {
          cout << "max. error = " << setprecision(30) << maxerror << endl;
          cerr << "[!] Error for sample id " << n << " with "
               << angle*180.0/M_PI << " degree, "
               << "source quaternion " << srcQ/srcQ.NormL2()
               << ", resulting quaternion " << dstQ/dstQ.NormL2() << endl;
          BIASABORT;
        }
      }
      cout << "max. error = " << setprecision(30) << maxerror << endl;
      
      cout << "- test rotation matrix -> quaternion -> rotation matrix : ";
      Quaternion<double> Q;
      RMatrixBase srcR, dstR;
      maxerror = -1.0;
      for (int n = 0; n < num; n++)
      {
        srcR = GenerateRandomRMatrix(angle);
        if (srcR.GetQuaternion(Q) != 0) { BIASABORT; }
        if (dstR.SetFromQuaternion(Q)!= 0) { BIASABORT; }
        if (!CompareRMatrices(srcR, dstR)) {
          cout << "max. error = " << setprecision(30) << maxerror << endl;
          cerr << "[!] Error for sample id " << n << " with "
               << angle*180.0/M_PI << " degree, "
               << "source rotation matrix " << srcR
               << ", resulting rotation matrix " << dstR << endl;
          BIASABORT;
        }
      }
      cout << "max. error = " << setprecision(30) << maxerror << endl;
    }
  }

  // Special test case (esquivel 03/2011)
  {
    cout << "\nevaluate special test case which is known for problems"
         << "\n------------------------------------------------------\n";
    RMatrixBase Rsrc, Rdst;
    Quaternion<double> q;
    Rsrc[0][0] = Rsrc[2][2] = 0.0;
    Rsrc[1][1] = Rsrc[0][2] = Rsrc[2][0] = -1.0;
    Rsrc.GetQuaternion(q);
    Rdst.SetFromQuaternion(q);
    double diff = (Rsrc-Rdst).NormL2();
    cout << "- rotation by " << Rsrc.GetRotationAngle()*180.0/M_PI
         << " degree around axis " << Rsrc.GetRotationAxis()
         << "\n- rotation matrix " << Rsrc
         << "\n-> to quaternion " << q
         << "\n-> to rotation matrix " << Rdst
         << "\ndifference = " << setprecision(30) << diff << endl;
    const double eps = 5e-10;

    if (!Equal(diff, 0.0, eps)) {
      BIASERR("[!] Inconsistent conversion rotation matrix -> quaternion -> "
              "rotation matrix for special test case, difference is "
              << setprecision(30) << diff);
      BIASABORT;
    }

    cout << endl;
  }
}