SimilarityTransform.hh

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


#ifndef __BIAS_SimilarityTransformation3D_hh__
#define __BIAS_SimilarityTransformation3D_hh__

// never include this unless it is absolutely needed:
// #include "bias_config.h" 

#include <Base/Common/BIASpragmaStart.hh>
#include <Base/Debug/Debug.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Base/Math/Matrix4x4.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Math/Vector3.hh>

namespace BIAS {

  /** @class SimilarityTransform
      @brief Implements a 3D similarity transformation (rotation, translation,
             and isometric scaling).

      Similarity transformations can be interpreted either as a coordinate
      transformation or motion and scaling of points. Euclidean transformations
      are special similiarty transformations with scale = 1.
    
      This transformation is identical to the following matrix:
    
                    [                C_[0] ]
                    [   scale_*R     C_[1] ]
             T  =   [                C_[2] ]
                    [  0    0    0     1   ]
    
      The internal representation of the rotation is a unit quaternion Q_.
      This quaternion, a translation vector and a scale are used to parametrize
      the similarity with 8 parameters (and one constraint Q_.NormL2() == 1).
      Additional, a covariance matrix for all parameters can be stored.

      Can be used to represent poses, rigid motions or coordinate transforms.

      @author grest 05/2006
   */
class BIASGeometry_EXPORT SimilarityTransform : public BIAS::Debug
{
public:

  /** @brief Creates the identity transformation with zero covariance. */
  SimilarityTransform()
    : Q_(0.0, 0.0, 0.0, 1.0), 
      C_(0.0,0.0,0.0), scale_(1.0) {}
  
  /** @brief Copy constructor, calls operator= */
  SimilarityTransform(const SimilarityTransform& c) {
    *this = c;
  }
  
  /** @brief Copy all elements of another SimilarityTransform instance */
  const SimilarityTransform& 
  operator=(const SimilarityTransform& c) {
    Q_= c.Q_;
    C_ = c.C_;
    scale_ = c.scale_;
    return (*this);
  }
  
  /** @brief Set rotational part from unit quaternion Q */
  inline void SetQ(const Quaternion<double>& Q) { Q_ = Q; }
  
  /** @brief Set rotational part from rotation matrix R */
  inline void SetR(const BIAS::RMatrixBase &R) 
  { R.GetQuaternion(Q_); }
  
  /** @brief Set translational part from vector  C */
  inline void SetC(const Vector3<double>& C) { C_ = C; }
  
  /** @brief Set rotation, translation, and optional scaling */
  inline void Set(const Quaternion<double>& Q, const Vector3<double>& C, 
                  const double& scale = 1.0)
  { Q_ = Q; C_ = C; scale_ = scale; }
  
  /** @brief Return unit quaternion representing rotation */
  inline const Quaternion<double>& GetQ() const { return Q_; }
  
  /** @brief Return vector representing translation */
  inline const Vector3<double>& GetC() const { return C_; }
  
  /** @brief Returns 4x4 transformation matrix corresponding to this */
  Matrix4x4<double> GetTransformMatrix() const;
  
  /** @brief Compute rotation matrix from the unit quaternion representing
             the rotational part of this */
  inline BIAS::RMatrixBase GetR() const {
    RMatrixBase R; 
    R.SetFromQuaternion(GetQ());
    return R;
  }

  /** @brief Return isometric scaling of this transformation */
  inline double GetScale(){ return scale_; }
  
  friend std::ostream& operator<<(std::ostream &os, const SimilarityTransform& p);
  friend std::istream& operator>>(std::istream &is, SimilarityTransform& p);
  
  /** @brief Load similarity transform from text file inputFile
      @return Return true when loading was successful, false otherwise
  */
  bool Load(const std::string& inputFile);

  /** @brief Save similarity transform to text file outputFile
      @return Return true when saving was successful, false otherwise
  */
  bool Save(const std::string& outputFile);
  
  /** @brief Invert the similarity transform (in place) */
  inline void InvertIP() { 
    Q_.Invert(); scale_ = 1.0 / scale_; 
    C_ = -scale_ * Q_.MultVec(C_);
  }
  
  /** @brief Concatenate this similarity transform and the given one so that
             result = this * arg holds for the corresponding 4x4 matrices.
  */
  inline void Mult(const SimilarityTransform &arg, SimilarityTransform &result)
  {
    Q_.Mult(arg.Q_,result.Q_);
    Q_.MultVec(arg.C_, result.C_);
    result.C_ *= scale_;
    result.C_+= C_;
    result.scale_= scale_ * arg.scale_;
  }
  
  /** @brief Transform the given vector with this similarity transform so that
             result = this * vec holds for the corresponding 4x4 matrix.
  */
  inline void MultVec(const Vector3<double> &vec, Vector3<double> &result) const
  { Q_.MultVec(vec, result); result *= scale_; result += C_; } 
  
  
  /** @brief Transform the given homogeneous 3D point with this similarity
             transform so that result = this * vec holds for the corresponding
             4x4 matrix. Computes the 4x4 transformation matrix internally.
  */
  inline void Mult(const HomgPoint3D &vec, HomgPoint3D &result) const
  { GetTransformMatrix().Mult(vec, result); } 

  /** @brief Print transformation together with more intuitive rotation 
             representations for debugging purposed.
  */
  void Dump();
   
protected:
  
  /// this is the rotation 
  Quaternion<double> Q_;
  
  /// the translation 
  Vector3<double> C_;
  
  double scale_;
};

inline std::ostream& operator<<(std::ostream &os, 
                                const SimilarityTransform& p) {
  os<<"Q_= "<<p.Q_<<" C_= "<<p.C_<<" scale_= "<<p.scale_;
  return os;  
}

inline std::istream& operator>>(std::istream &is, 
                                SimilarityTransform& p){
  std::string temp;
  is>>temp>>p.Q_>>temp>>p.C_>>temp>>p.scale_;
  return is;
}

inline SimilarityTransform operator*(SimilarityTransform &arg1,
                                     SimilarityTransform &arg2)
{
  SimilarityTransform result;
  arg1.Mult(arg2,result);
  return result;
}

inline BIAS::Vector3<double> operator*(const SimilarityTransform &arg1,
                                       const BIAS::Vector3<double> &vec)
{
  BIAS::Vector3<double> result;
  arg1.MultVec(vec,result);
  return result;
}



} // end namespace 


#include <Base/Common/BIASpragmaEnd.hh>

#endif