CoordinateTransform3D.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_CoordSystsemTransform3D_hh__
#define __BIAS_CoordSystsemTransform3D_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/Geometry/HomgPoint3DCov.hh>
#include <Base/Math/Vector3.hh>

namespace BIAS {


  /** @class CoordinateTransform3D

      @brief Transforms 3d points between two different coodinate systems, e.g.
             from camera to world coordinates using the camera pose.
    
      We define a local coordinate system LCS (i.e. camera coordinate system)
      and a global coordinate system GCS (i.e. world coordinate system). 
      The transformation from local to global coordinates is defined by matrix
    
                   [                C_[0] ]                  <br>
                   [   scale_*R     C_[1] ]                  <br>
             T  =  [                C_[2] ] = invTransform_  <br>
                   [  0    0    0     1   ]                  <br>
    
      This matrix converts a point X_L in LCS to its representation in GCS like
    
                      X_G = T * X_L
    
      R is the matrix equivalent to unit quaternion Q.
     
      Obviously,      X_L = T.Invert() * X_G
    
             T.invert() =  [   R^T/scale_      -R^T*C_/scale_ ] = (*this) <br>
                           [  0      0      0        1        ]           <br>
    
      C is the origin of the local coordinate system in global coordinates,
      Q (or R) tranforms a direction in local coordinates into a direction
      in global coordinates.
      The terms global and local are only used here to distinguish between
      two arbitrary coordinate systems, neither of them has to be local/global.
    
      @attention do not confuse T with the matrix stored in (*this) (see above)!
    
      Apply this for e.g. hand-eye transformation, global-local coordinates,
      camera to world coordinates.

      @ingroup g_geometry
    
      @author koeser/bartczak
      @date 03/2006
   */
  class BIASGeometry_EXPORT CoordinateTransform3D :
    protected Matrix4x4<double>
  {
  public:
    /** @brief Set identity transformation. */
    CoordinateTransform3D()
      : Matrix4x4<double>(MatrixIdentity), Q_(0.0, 0.0, 0.0, 1.0), 
        C_(0.0, 0.0, 0.0), scale_(1.0), invTransform_(MatrixIdentity)
    {}

    /** @brief Copy constructor */
    CoordinateTransform3D(const CoordinateTransform3D& c)
    {
      *this = c;
    }

    /** @brief Copy all elements of another instance. */
    const CoordinateTransform3D& 
    operator=(const CoordinateTransform3D& c) {
      Q_= c.Q_;
      C_ = c.C_;
      scale_ = c.scale_;
      invTransform_ = c.invTransform_;
      Matrix4x4<double>::operator=(c);
      return (*this);
    }

    /** @brief Set 3x4 matrix which transforms points in global coordinate system
               to Euclidean points in local coordinate system.
        @attention Use only for matrices that can not be decomposed properly,
                   otherwise use Set method!
    */
    void SetFromMatrix3x4(const Matrix3x4<double> &M);

    ///** @brief Set rotation and translation from pose object. */
    //void SetFromPose(const Pose &pose);

    /** @brief Get 3x4 matrix which transforms points in global coordinate system
               to Euclidean points in local coordinate system. */
    Matrix3x4<double> GetMatrix3x4() const;

    /** @brief Get 4x4 matrix which transforms points in global coordinate system
               to homogeneous points in local coordinate system. */
    Matrix4x4<double> GetMatrix4x4() const;

    /** @brief Set orientation of local coordinate system from unit quaternion.
        @see   CoordinateTransform3D::Set() */
    void SetQ(const Quaternion<double>& Q) { Q_ = Q; UpdateMatrix_(); }

    /** @brief Set orientation of local coordinate system from rotation matrix.
        @see   CoordinateTransform3D::Set() */
    void SetR(const BIAS::RMatrixBase &R);

    /** @brief Set origin of local coordinate system in global coordinates.
        @see   CoordinateTransform3D::Set() */
    void SetC(const Vector3<double>& C) { C_ = C; UpdateMatrix_(); }

    /** @brief Define local coordinate frame from within the world frame.
        @param[in] Q Specifies the orientation of the local frame in 
                   the world frame base:
                   Q*(global base vecor)*Q_conj = local base vector
        @param[in] C Specifies the position of the origin of the local frame
                   in the world coordinate frame:
                   global base origin + C_ = local base origin
        @param[in] scale Specifies the isotropic scale of base vector length
                   to get local base vector length:
                   ||global base vector||*scale = ||local base vector||
     */
    virtual void Set(const Quaternion<double>& Q, const Vector3<double>& C, 
                     const double& scale = 1.0) {
      Q_ = Q; C_ = C; scale_ = scale; UpdateMatrix_();
    }

    /** @brief Set the length of the local coordinate axes in the world 
               coordinate frame. */
    virtual inline void SetScale(const double& scale) { 
      scale_ = scale; 
      UpdateMatrix_();}

    /** @brief Transform point from world frame to local coordinate frame.
        @param[in] X_G Homogeneous 3d point within world coordinate system
        @return Returns homogeneous 3d point within local coordinate system
     */
    HomgPoint3D GlobalToLocal(const HomgPoint3D& X_G) const {
      Vector4<double> X_L;
      this->Mult(X_G, X_L);
      return X_L;
    }

    /** @brief Transform point from local frame to world coordinate frame.
        @param[in] X_L Homogeneous 3d point within local coordinate system
        @return Returns homogeneous 3d point within world coordinate system
     */    
    HomgPoint3D LocalToGlobal(const HomgPoint3D& X_L) const {
      Vector4<double> X_G;
      invTransform_.Mult(X_L, X_G);
      return X_G;
    }

    /** @brief Transform point and its covariance matrix from world frame to
               local coordinate frame.
        @param[inout] X_G Gets homogeneous 3d point within world coordinate system,
                          returns 3d point within local coordinate system,
        @param[inout] Cov_G Gets homogeneous 3d covariance within world frame,
                            returns covariance matrix in local coordinate system.
     */
    void GlobalToLocal(HomgPoint3D& X_G, 
                       HomgPoint3DCov& Cov_G) const {
      Vector4<double> X(X_G);
      this->Mult(X, X_G);
      Cov_G = (*this) * Cov_G * (this->Transpose());
    }

    /** @brief Transform point and its covariance matrix from local frame to
               world coordinate frame.
        @param[inout] X_L Gets homogeneous 3d point within local coordinate system,
                          returns 3d point within wordl coordinate system,
        @param[inout] Cov_L Gets homogeneous 3d covariance within local frame,
                            returns covariance matrix in world coordinate system.
    */
    void LocalToGlobal(HomgPoint3D& X_L, 
                       HomgPoint3DCov& Cov_L) const {
      HomgPoint3D X(X_L);
      invTransform_.Mult(X, X_L);
      Cov_L = invTransform_ * Cov_L * invTransform_.Transpose();
    }

    /** @brief Get the length of the local coordinate axes in the world 
               coordinate frame.
        @see   CoordinateTransform3D::Set()
    */
    const double& GetScale() const { return scale_; }

    /** @brief Get orientation of local coordinate system as unit quaternion
               mapping from local coordinates to global coordinates.
        @see   CoordinateTransform3D::Set()
    */
    const Quaternion<double>& GetQ() const { return Q_; }

    /** @brief Set origin of local coordinate system in global coordinates. 
        @see   CoordinateTransform3D::Set()
    */
    const Vector3<double>& GetC() const { return C_; }

    /** @brief Get orientation and origin of local coordinate system as a
               single vector containing first a 3d vector [c_x, c_y, c_z],
               then a unit quaternion [q_x, q_y, q_z, q_w].
        @see   CoordinateTransform3D::Set()
    */
    inline const Vector<double> GetCQ() const {
      Vector<double> res(7);
      for (int i=0; i<3; i++){
        res[i] = C_[i]; res[i+3] = Q_[i];
      }
      res[6] = Q_[3];
      return res;
    }

    /** @brief Get orientation of local coordinate system as rotation matrix
               mapping from local coordinates to global coordinates.
        @see   CoordinateTransform3D::Set()
    */
    inline BIAS::RMatrixBase GetR() const {
      RMatrixBase R; 
      R.SetFromQuaternion(GetQ());
      return R;
    }

    /** @brief Get 4x4 matrix which transforms points in global coordinate system
               to homogeneous points in local coordinate system.
    */
    inline  Matrix4x4<double> GetGlobalToLocalTransform() const
    { return *this; }

    /** @brief Get 4x4 matrix which transforms points in local coordinate system
               to homogeneous points in global coordinate system.
    */
    inline  Matrix4x4<double> GetLocalToGlobalTransform() const
    { return invTransform_; }

    friend std::ostream& operator<<(std::ostream &os, 
                                    const CoordinateTransform3D& p);

    friend std::istream& operator>>(std::istream &is, 
                                    CoordinateTransform3D& p);

    /** @brief Load 3d coordinate transformation from a text file.
               The file format is specified by:

               Q_= <q_x> <q_y> <q_z> <q_w> C_= <c_x> <c_y> <c_z> scale_= <s>

               where the 3d vector [c_x, c_y, c_z] specifies the origin and
               the unit quaternion [q_x, q_y, q_z, q_w] the orientation of
               the local coordinate frame within the global coordinate frame.
        @param[in] inputFile Name of file to read from
        @return Returns false if reading from file failed, true otherwise.
    */
    bool Load(const std::string& inputFile);

    /** @brief Write 3d coordinate transformation to a text file.
        @see CoordinateTransform3D::Load()
        @param[in] outputFile Name of file to write to
        @return Returns false if writing to file failed, true otherwise.
    */
    bool Save(const std::string& outputFile);

    /** @brief Calculates transformation from global system of this instance
               to the local coordinate system of the given instance.
               If this instance defines [global <-> local] and parameter
               localT defines [local <-> local'] then the result defines
               [global <-> local'].
        @param[in] localT Specifies transformation [local <-> local']
        @param[out] res Returns concatenated transformation [global <-> local']

    */
    void ConcatenateLocalTransform(const CoordinateTransform3D& localT,
                                   CoordinateTransform3D& res) const;

    /** @brief Changes this coordinate transformation so that parameter
               newLocal becomes the local coordinate frame and parameter
               newGlobal becomes the global coordinate system. 
       
       It is assumed that newLocal and newGlobal are related through the same
       global frame i.e.: A world point p_world is described by the
       coordinates p_newGlobal and p_newLocal so that:

       p_world = T_newLocal * p_newLocal = T_newGlobal * p_newGLobal

       It follows that p_newGlobal (which is will become the new global 
       coordinate frame point) is computed by:

       p_newGlobal = T_newGlobal^-1 * T_newLocal * p_newLocal

       If newLocal defines [global <-> local] and newGlobal defines
       [global <-> local'] then this instance becomes [local' -> local].
      
       @param[in] newLocal Specifies transformation [global <-> local]
       @param[in] newGlobal Specifies transformation [global <-> local']
    */
    void BecomeRelativeTransform(const CoordinateTransform3D& newLocal,
                                 const CoordinateTransform3D& newGlobal);

    /** @brief Swaps the global and local coordinate frame. */
    void InvertIP() { 
      Q_.Invert(); scale_ = 1.0/scale_; 
      C_ = -scale_ * Q_.MultVec(C_); 
      UpdateMatrix_(); }

    /** @brief Applies the passed rotation to the orientation of the local frame.
               Convenience wrapper, which simulates
        \verbatim
        LocalCoordTrsf.Set(Q); 
        LocalCoordTrsf.Set(Vector3<double>(0,0,0));
        this->ConcatenateLocalTransform(Q, *this);
        \endverbatim
    */
    void RotateLocalFrame(const Quaternion<double>& Q);

    /** @brief Prints this transformation with more intuitive rotation representation. */
    void Talk() const;

  protected:

    /** @brief Computes the 4x4 transformation matrix from the currently set
                unit quaternion Q_ and translation vector C_ */
    virtual void UpdateMatrix_();

    /** @brief Specifies the rotation from local to global coordinate frame */
    Quaternion<double> Q_;

    /** @brief Specifies the translation from local to global coordinate frame */
    Vector3<double> C_;

    /** @brief Specifies isometric scaling from local to global coordinate frame */
    double scale_;

    /** @brief Specifies the homogeneous matrix representation of the transformation
               from local to global coordinate frame
        @attention This transformation is considered as "inverse" since the
                   transformation from global to local is considered as the
                   reference transformation direction!
    */
    Matrix4x4<double> invTransform_;
  };

  /** @brief Write 3d coordinate transformation to stream.
      @see CoordinateTransform3D::Save()
  */
  inline std::ostream& operator<<(std::ostream &os, 
                                  const CoordinateTransform3D& p) {
    os<<"Q_= "<<p.Q_<<" C_= "<<p.C_<<" scale_= "<<p.scale_;
    return os;  
  }

  /** @brief Read 3d coordinate transformation from stream.
      @see CoordinateTransform3D::Load()
  */
  inline std::istream& operator>>(std::istream &is, 
                                  CoordinateTransform3D& p){
    std::string temp;
    is>>temp>>p.Q_>>temp>>p.C_>>temp>>p.scale_;
    return is;
  }  
  
}

#include <Base/Common/BIASpragmaEnd.hh>

#endif