TestRMatrix.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 */

#include <Geometry/RMatrix.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Math/Random.hh>
#include <Base/Math/Vector3.hh>
#include <iostream>
#include <stdio.h>
#include <string>
#include <vector>

using namespace BIAS;
using namespace std;

void computeRandomVec(BIAS::Random &random, BIAS::Vector3<double> &vec)
{
  double phi   = random.GetUniformDistributed(-1.0, 1.0) * M_PI;
  double theta = random.GetUniformDistributed(-0.5, 0.5) * M_PI;
  vec[0] = sin(phi) * cos(theta);
  vec[1] = -sin(theta);
  vec[2] = cos(phi) * cos(theta);
}

void printMatrix3x3(BIAS::Matrix3x3<double> &mat, const std::string &name)
{
  printf("%s = [ % 1.6f % 1.6f % 1.6f ; % 1.6f % 1.6f % 1.6f ; % 1.6f % 1.6f % 1.6f ]\n",
         name.c_str(), mat[0][0], mat[0][1], mat[0][2],
         mat[1][0], mat[1][1], mat[1][2], mat[2][0], mat[2][1], mat[2][2]);
}

/**
   @file TestRMatrix.cpp
   @brief Comparison of RMatrix and RMatrixNase classes for testing.

   Computes random rotation matrices from slightly non-orthogonal row vectors
   and applies different orthonormalization strategies implemented in RMatrix
   and RMatrixBase EnforceConstraints() methods. The resulting matrices are
   compared and their orthonormality constraints are checked for validity.

   Addtional, random vectors are rotated by the resulting matrices and their
   differences are evaluated. Note that the resulting matrices will differ by
   an order of magnitude comparable to the error on the matrix rows. This is
   due to the different orthonormalisation algorithms used (Gram-Schmidt vs.
   SVD based orthonormalization). However, the SVD based method from RMatrix
   provides better results as compared to the original rotation matrix.

   @relates RMatrix
   @ingroup g_tests
   @author MIP
*/
int main(int argc, char *argv[])
{
  // read parameters
  const int numMatrix = argc > 1 ? std::max(1, atoi(argv[1])) : 100;
  const int numVector = argc > 2 ? std::max(1, atoi(argv[2])) : 100;
  const double errorStd = argc > 3 ? std::min(1.0, fabs(atof(argv[3]))) : 0.01;
  const double matrixErrorThreshold = 1; //0.01;
  const double matrixCheckThreshold = 1e-10;
  Random random;
  cout << "\nUsage : TestRMatrix [<num matrices>] [<num vectors>] [<error sigma>]\n\n"
       << "Starting tests with " << numMatrix << " rotation matrices and "
       << numVector << " 3d vectors with error sigma " << errorStd << " ...\n";

  // create random vectors for matrix-vector-multiplication tests
  vector<Vector3<double> > vecs3d(numVector);
  for (int vnum = 0; vnum < numVector; vnum++)
    computeRandomVec(random, vecs3d[vnum]);
  double meanVectorError = 0, stdVectorError = 0;

  // create random rotation matrices from row vectors
  Vector3<double> rx, ry, rz;
  RMatrix R;
  RMatrixBase Rbase, Rgt;
  double meanMatrixError = 0, stdMatrixError = 0;
  int numMatrixFailed = 0;
  for (int mnum = 0; mnum < numMatrix; mnum++)
  {
    // compute orthonormal vectors
    computeRandomVec(random, rx);
    int min_id = (fabs(rx[1]) < fabs(rx[0])) ? 1 : 0;
    if (fabs(rx[2]) < fabs(rx[min_id])) min_id = 2;
    ry.SetZero();
    ry[min_id] = 1;
    rz = rx.CrossProduct(ry);
    rz.Normalize();
    ry = rx.CrossProduct(rz);
    ry.Normalize();

    // check ground truth rotation matrix
    for (int k = 0; k < 3; k++) {
      Rgt[0][k] = rx[k];
      Rgt[1][k] = ry[k];
      Rgt[2][k] = rz[k];
    }
    if (Rgt.GetDeterminant() < 0) Rgt *= -1.0;
    BIASASSERT(Rgt.Check(matrixCheckThreshold));

    // distort vectors
    for (int k = 0; k < 3; k++) {
      rx[k] += random.GetNormalDistributed(0, errorStd);
      ry[k] += random.GetNormalDistributed(0, errorStd);
      rz[k] += random.GetNormalDistributed(0, errorStd);
    }
    rx.Normalize();
    ry.Normalize();
    rz.Normalize();

    // set rotation matrices from vectors
    for (int k = 0; k < 3; k++) {
      R[0][k] = Rbase[0][k] = rx[k];
      R[1][k] = Rbase[1][k] = ry[k];
      R[2][k] = Rbase[2][k] = rz[k];
    }
    R.EnforceConstraints();
    Rbase.EnforceConstraints();
    const bool checkR = R.Check(matrixCheckThreshold);
    const bool checkRbase = Rbase.Check(matrixCheckThreshold);

    // evaluate matrix error
    double error = (Matrix3x3<double>(R) - Matrix3x3<double>(Rbase)).NormL2();
    meanMatrixError += error;
    stdMatrixError  += error * error;
    if (error > matrixErrorThreshold || !checkR || !checkRbase) {
      numMatrixFailed++;
      cout << "Test " << (mnum+1) << " / " << numMatrix << " failed : \n";
      if (!checkR)
        cout << "-> R is not orthonormal : "
             << R.GetCheckFailureReason(matrixCheckThreshold) << "\n";
      if (!checkRbase)
        cout << "-> Rbase is not orthonormal : "
             << Rbase.GetCheckFailureReason(matrixCheckThreshold) << "\n";
      if (error > matrixErrorThreshold)
        cout << "-> Difference " << error << " is too large!\n";
      printMatrix3x3(R, "->     R");
      printMatrix3x3(Rbase, "-> Rbase");
      cout <<  "--------------------------------------------------\n";
    }

    // evaluate matrix-vector-multiplication errors
    for (int vnum = 0; vnum < numVector; vnum++) {
      error = (R*vecs3d[vnum] - Rbase*vecs3d[vnum]).NormL2();
      meanVectorError += error;
      stdVectorError  += error * error;
    }
  }

  // print results
  meanMatrixError /= numMatrix;
  stdMatrixError = sqrt(stdMatrixError / numMatrix -
                        meanMatrixError * meanMatrixError);
  meanVectorError /= numMatrix * numVector;
  stdVectorError = sqrt(stdVectorError / (numMatrix * numVector) -
                        meanVectorError * meanVectorError);
  cout << "\nResults : " << numMatrixFailed << " / " << numMatrix
       << " tests failed (" << numMatrixFailed / (0.01 * numMatrix) << " %)\n";
  cout << "-> average difference between RMatrix and RMatrixBase : "
       << meanMatrixError << " +- " << stdMatrixError << "\n"
       << "-> average difference between rotated unit vectors : "
       << meanVectorError << " +- " << stdVectorError << "\n\n";

  return 0;
}