SimilarityTransform.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/SimilarityTransform.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;


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

bool SimilarityTransform::
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 SimilarityTransform::Dump()
{
  cout<<"Translation:"<<C_<<endl;

  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;
  cout<<"Matrix representation:"<<GetTransformMatrix()<<endl;
}

Matrix4x4<double> SimilarityTransform::GetTransformMatrix() const
{
  Matrix4x4<double> result;
  RMatrixBase R; 
  result[3][3] = 1.0;
  R.SetFromQuaternion(Q_);
  R *= scale_;
  for (unsigned int r = 0; r < 3; r++)
    for (unsigned int c = 0; c < 3; c++)
      result[r][c]= R[r][c];
  result[0][3] = C_[0];
  result[1][3] = C_[1];
  result[2][3] = C_[2];
  return result;
}