TestQuaternion.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 TestQuaternion.cpp
   @relates Quaternion
   @brief Test quaternion usage and consistency with rotation matrix
   @ingroup g_tests
   @author MIP  
*/

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

using namespace std;
using namespace BIAS;


Quaternion<double> GenerateRandomQuaternion(Random &randomizer, 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;
}

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

Vector3<double> GenerateRandomVector(Random &randomizer)
{
  Vector3<double> vec;
  do {
    vec.Set(randomizer.GetUniformDistributed(-100.0, 100.0),
            randomizer.GetUniformDistributed(-100.0, 100.0),     
            randomizer.GetUniformDistributed(-100.0, 100.0));
  } while (Equal(vec.NormL2(), 0.0));
  return vec;
}

double TestQuatQuatMult(Quaternion<double> &q, Quaternion<double> &p,
                        const double epsilon = 1e-12,
                        const bool verbose = false)
{ 
  Quaternion<double> qp = q*p;
  Matrix4x4<double> L = q.GetQuaternionMultMatrixLeft();
  Matrix4x4<double> R = p.GetQuaternionMultMatrixRight();
  Quaternion<double> Lp = L*p;
  Quaternion<double> Rq = R*q;

  // compare quaternion multiplication results
  double diffL = (Lp-qp).NormL2();
  double diffR = (Rq-qp).NormL2();
  double maxDiffLR = (diffL > diffR ? diffL : diffR);
  if (verbose) {
    cout << "q = " << q << ", p = " << p << ", q*p = " << qp << endl
         << "L(q)*p = " << Lp << ", |L(q)*p - q*p| = " << diffL << endl
         << "R(p)*q = " << Rq << ", |R(p)*q - q*p| = " << diffR << endl;
  }
  BIASASSERT(Equal(diffL, 0.0, epsilon));
  BIASASSERT(Equal(diffR, 0.0, epsilon));

  // compare result with rotation matrix
  RMatrixBase Mq, Mp;
  Mq.SetFromQuaternion(q);
  Mp.SetFromQuaternion(p);
  RMatrixBase MqMp = Mq*Mp;
  Quaternion<double> r;
#ifdef BIAS_DEBUG
  BIASASSERT(MqMp.GetQuaternion(r) == 0);
#else
  MqMp.GetQuaternion(r);
#endif

  double diffQuatR = fabs(r.ScalarProduct(qp))-1.0;
  if (verbose) {
    cout << "|<quat(M(q)*M(p)), q*p>| = " << diffQuatR+1.0 << endl << flush;
  }
  BIASASSERT(Equal(diffQuatR, 0.0, epsilon));

  return (fabs(diffQuatR) > maxDiffLR ? fabs(diffQuatR) : maxDiffLR);
}

double TestQuatVecMult(Quaternion<double> &q, Vector3<double> &v,
                       const double epsilon = 1e-12,
                       const bool verbose = false)
{
  // compare quaternion - vector multiplication with
  // rotation matrix - vector multiplication
  Vector3<double> qvq;
  q.MultVec(v, qvq);
  RMatrixBase R;
  R.SetFromQuaternion(q);
  Vector3<double> Rv = R*v;
  double diffV = (Rv-qvq).NormL2();
  if (verbose) {
    cout << "v = " << v << ", q*v*q' = " << qvq
         << ", |R*v - q*v*q'| = " << diffV << endl << flush;
  }
  BIASASSERT(Equal(diffV, 0.0, epsilon));
  return diffV;
}

int main() 
{
  const int num = 10000;
  const double epsilon = 1e-12;
  const bool verbose = false;
  Random randomizer;

  cout << "\ntest quaternions for " << num << " random rotations"
       << "\n---------------------------------------------------\n";

  double error, maxError = 0.0;
  Quaternion<double> q, p;
  Vector3<double> v;
  for (int n = 0; n < num; n++)
  {
    q = GenerateRandomQuaternion(randomizer);
    p = GenerateRandomQuaternion(randomizer);
    v = GenerateRandomVector(randomizer);
    error = TestQuatQuatMult(q, p, epsilon, verbose);
    if (error > maxError) maxError = error;
    error = TestQuatVecMult(q, v, epsilon, verbose);
    if (error > maxError) maxError = error;
    error = TestQuatVecMult(p, v, epsilon, verbose);
    if (error > maxError) maxError = error;
  }
  cout << "max. error = " << setprecision(30) << maxError << endl << endl;

  return 0;
}