RMatrix.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 "RMatrix.hh"
#include <MathAlgo/SVD.hh>
#include <MathAlgo/Minpack.hh> 

using namespace BIAS;
using namespace std;


RMatrix::~RMatrix()
{}

RMatrix::RMatrix(const TNT::Matrix<ROTATION_MATRIX_TYPE>& mat)
  : RMatrixBase(mat)
{
  if ((mat.num_rows() != 3) || (mat.num_cols() != 3)) {
    BIASERR("wrong matrix size "<<mat.num_rows()<<"x"
           << mat.num_cols());
  }
  pvecR_ = NULL;
  bestErrorSoFar_ = DBL_MAX;
  bestQuaternionSoFar_.Set(0,0,0,1);
}

void RMatrix::EnforceConstraints()
{
  // enforce real rotation matrix using SVD
  // by setting all singular values to unity 
  SVD RotSVD(Matrix<ROTATION_MATRIX_TYPE>(*this));
  Matrix3x3<ROTATION_MATRIX_TYPE> UVT = RotSVD.GetU() * RotSVD.GetVT();
  memcpy(this->Data_, UVT.GetData(), 9*sizeof(ROTATION_MATRIX_TYPE));

  // enforce right-handed system (det = +1)
  if (this->GetDeterminant() < 0) *this *= -1.0;
}

int ErrorFunctionAverageRMatrix(void *p, int numerrors, int numvars,
                                const double *x, double *fvec, int iflag)
{
  BIASASSERT(numvars==4);
  RMatrix *pInstance = (RMatrix *)p;
  // create rotation matrix vector from parametrization
  Quaternion<double> q(x[0],x[1],x[2],x[3]);
  q.Normalize();
  RMatrixBase R;
  R.SetFromQuaternion(q);
 

  // compute residuum
  double error = 0, err;
  for (unsigned int j=0; j<pInstance->pvecR_->size(); j++) {
    for (unsigned int row=0; row<3; row++) {
      for (unsigned int col=0; col<3; col++) {
        err = fvec[9*j+3*col+row]   
          = R[row][col] - (*pInstance->pvecR_)[j][row][col];
        error += err*err;
      }
    }
  }

  // in case LM runs out of iterations, remember best in a global variable
  if (error<pInstance->bestErrorSoFar_) {
    pInstance->bestErrorSoFar_ = error;
    pInstance->bestQuaternionSoFar_ = q;
  }
  cout<<error<<" ";
  return 0;
}

RMatrix RMatrix::GetAverageRMatrix(std::vector<RMatrix>& vecR,
                                   bool RefineRotation)
{
  if (vecR.empty()) {
    BIASERR("Empty vector given for average rotation matrix estimation!");
    return RMatrix(MatrixIdentity);
  }
  BIASWARN("Untested code for average rotation matrix computation so far!");
  RMatrix result = vecR[0];
  Vector3<double> axis(0,0,0);
  double angle(0.0);
  double weight(1.0);
  // For the first two matrices we have RDiff = vecR[0] * vecR[1].Transpose();
  // Therefore result = sqrt(Rdiff) * vecR[1] = sqrt(Rdiff^T) * vecR[1]
  // is an average orientation, where square root is realized by axis-angle
  // decomposition (sqrt = half angle)
  for (unsigned int i = 1; i < vecR.size(); i++) {
    // After the first averaging, our result becomes more reliable, so
    // use a higher weight each time. 
    // This is equivalent to not using sqrt = ^0.5 but ^weight
    weight = 1.0 / double(i+1);
    RMatrix RDiff = result * vecR[i].Transpose();
    RDiff.GetRotationAxisAngle(axis, angle);
    // Compute transposed of difference multiplied by -1 and go only by
    // weight into this new direction
    angle *= -1.0 * weight;
    RDiff.Set(axis, angle);
    result = RDiff * result;
    result.EnforceConstraints();
  }

  // Take this as start value for nonlinear optimization (opt.)
  if (RefineRotation) {
    pvecR_ = &vecR;
    bestErrorSoFar_ = DBL_MAX;
    long int num_params = 4;
    long int num_errors = 9 * vecR.size();
    long int max_error_calls = 1000;
    double epsilon = 0.02; // used for Jcobian approximation
    
    // Setup parametrization
    Quaternion<double> q(0,0,0,1), qres(0,0,0,1);
    result.GetQuaternion(q);
    Vector<double> qvec(4), qresvec(4);
    for (unsigned int i = 0; i < 4; i++) qvec[i] = q[i];
    
    // Compute solution with Levenberg-Marquardt algorithm
    long int LMResult = 
      LevenbergMarquardtExtended(&ErrorFunctionAverageRMatrix,
                                 (void*)(this), num_errors, num_params,
                                 qvec, qresvec, epsilon,
                                 1e-10, max_error_calls);
    if ((LMResult != LM_OK) && (LMResult != LM_TOO_MANY_IT)) {
      BIASERR("Error in Levenberg-Marquardt optimization (error code is "
              << LMResult << ")");
    } else {
      cout << "Levenberg-Marquardt optimization finished with code "
           << LMResult << endl;
      result.SetFromQuaternion(bestQuaternionSoFar_);
      cout << "-> best error so far is " << bestErrorSoFar_ << endl;
    }
  }

  return result;
}