CoordinateTransform3D.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/CoordinateTransform3D.hh>
#include <MathAlgo/SVD.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <fstream>
#include <iostream>
#include <Base/Math/Matrix3x4.hh>

using namespace BIAS;
using namespace std;


void CoordinateTransform3D::UpdateMatrix_() {
  // delete previous data in matrix
  this->SetIdentity();
  invTransform_.SetIdentity();

  // compute rotational part
  RMatrixBase R;
  R.SetFromQuaternion(Q_);

  Vector3<double> RTC(R.Transpose()*C_);

  // copy rotation and translation into matrix
  for (unsigned int row=0; row<3; row++) {
    for (unsigned int column=0; column<3; column++) {
      // *this Global -> Local
      (*this)[row][column] = R[column][row] / scale_;

      //invTransform Local -> Global
      invTransform_[column][row] = R[column][row] * scale_;
    }
    (*this)[row][3] = RTC[row] / scale_ * (-1.0);// *this Global -> Local
    invTransform_[row][3] = C_[row]*scale_;//invTransform Local -> Global
  }
}

void CoordinateTransform3D::
SetFromMatrix3x4(const Matrix3x4<double> &M){
  // delete previous data in matrix
  this->SetIdentity();
  invTransform_.SetIdentity();

  for (unsigned int row=0; row<3; row++) {
    for (unsigned int column=0; column<4; column++) {
      (*this)[row][column] = M[row][column];
    }
  }
  // set this to defined values
  Q_.Set(0,0,0,0);
  C_.Set(0,0,0);
  scale_ = 0.0;

  SVD svd((*this));

  invTransform_ = svd.Invert();
}

BIAS::Matrix3x4<double>
CoordinateTransform3D::GetMatrix3x4() const {
  Matrix3x4<double> P;
  for (unsigned int row=0; row<3; row++) {
    for (unsigned int column=0; column<4; column++) {
      P[row][column] = (*this)[row][column];
    }
  }
  return P;
}

BIAS::Matrix4x4<double>
CoordinateTransform3D::GetMatrix4x4() const {
  return *this;
}



bool CoordinateTransform3D::
Load(const std::string& inputFile)
{
  ifstream ifs(inputFile.c_str());
  if(!ifs) {
    return false;
  }
  ifs >> *this;
  if(!ifs) {
    ifs.close();
    return false;
  }
  ifs.close();
  UpdateMatrix_();
  return true;
}

bool CoordinateTransform3D::
Save(const std::string& outputFile)
{
  ofstream ofs(outputFile.c_str());
  if(!ofs) {
    return false;
  }
  ofs << *this;
  if(!ofs) {
    ofs.close();
    return false;
  }
  ofs.close();
  return true;
}

void CoordinateTransform3D::
BecomeRelativeTransform(const CoordinateTransform3D& newLocal,
                        const CoordinateTransform3D& newGlobal)
{
  CoordinateTransform3D newGlobalInv = newGlobal;
  newGlobalInv.InvertIP();
  newGlobalInv.ConcatenateLocalTransform(newLocal, (*this));
}

void CoordinateTransform3D::
ConcatenateLocalTransform(const CoordinateTransform3D& localT,
                          CoordinateTransform3D& res) const
{
  /* if we were using matrices:
   * B1 is linear transform local to global (global = B1*local)
   * B(1,i) is linear transform local(2) to local (local = B(1,i)*local(2))
   * then BC = B1*B(1,i) should be global = BC * local(2)
   * this stores B1 in Q_, C_, and scale_.
   * localT store B(1,i) in its Q_, C_, and scale_.
   */
  res.Set(Q_ * localT.Q_,
          C_ + Q_.MultVec(localT.C_) * scale_,
          scale_ * localT.scale_);
}


void CoordinateTransform3D::
RotateLocalFrame(const Quaternion<double>& Q)
{
  //moves the local frame:
  // e.g. camera is rotated by Q
  Q_ = Q_*Q;
  UpdateMatrix_();
}

void CoordinateTransform3D::
Talk() const
{
  cout<<"Transformation RAW: "<<endl;
//  cout<<*this<<endl;

  for(unsigned int i=0; i < 4; i++){
    for(unsigned int j=0; j < 4; j++){
      cout << (*this)[i][j] << " ";
    }
    cout << endl;
  }

  if(Q_ != Quaternion<double>(0, 0, 0, 0)){
    RMatrixBase R;
    R.SetFromQuaternion(Q_);
    cout<<"Rotation Matrix: "<<R<<endl;

    Vector3<double> axis;
    double angle;

    R.GetRotationAxisAngle(axis, angle);
    cout<<" axis = "<<axis<<endl;
    cout<<"angle = "<<angle<<endl<<endl;
  }
}

void
CoordinateTransform3D::SetR(const BIAS::RMatrixBase &R)
{
  R.GetQuaternion(Q_);
  UpdateMatrix_();
}