DualQuaternion.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 __DualQuaternion_hh__
#define __DualQuaternion_hh__

#include <bias_config.h>

#include <Base/Geometry/Quaternion.hh>
#include <Base/Geometry/DualQuaternionOperators.hh>
#include <Base/Geometry/PoseParametrization.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Vector.hh>
#include <Base/Math/Vector3.hh>

namespace BIAS {

  /** @class DualQuaternion

      @ingroup g_geometry
      @brief class representing rigid motion by dual quaternions

      Dual quaternions can be used to represent a rigid motion in 3d space
      consisting of a rotation and a translation.
     
      A dual quaternion can be described as consisting of a real part q and
      a dual part p which are both real quaternions. The real part q simply
      describes the rotational part of the motion (see #Quaternion).
      The dual part p describes the translational part decomposed into
      a translation orthogonal to the rotation axis (related to the "moment")
      and the amount of translation along the rotation axis ("pitch").

      Dual quaternions describing rigid motion are constrained to have
      a unit real part and orthogonal real and dual parts.

      See for example K. Daniilidis: Hand-Eye Calibration Using Dual
      Quaternions (1999) for further information on dual quaternions
      and their relation to screw theory.

      @author esquivel 02/2011
    */

  template<class QUAT_TYPE> 
  class BIASGeometryBase_EXPORT DualQuaternion
  {

  private:

    Quaternion<QUAT_TYPE> real, dual;

  public:

    DualQuaternion();

    DualQuaternion(const DualQuaternion<QUAT_TYPE>& d);

    DualQuaternion(const Quaternion<QUAT_TYPE>& q,
                   const Quaternion<QUAT_TYPE>& p);

    ~DualQuaternion() {}

    inline void Invert();

    inline DualQuaternion<QUAT_TYPE> Inverse() const;

    /** Make dual quaternion representation unique with respect to the
        rigid motion it represents. */
    inline void MakeUnique();

    /** Scales dual quaternion to unit length, i.e. norm of real part equals 1
        and real part is orthogonal to dual part. */
    inline void Normalize();

    /** Computes dual quaternion norm which is a dual number consisting of
        real part r and dual part d. */
    inline void GetDualNorm(QUAT_TYPE &r, QUAT_TYPE &d) const;

    inline void Add(const DualQuaternion<QUAT_TYPE> &r);

    inline void Add(const DualQuaternion<QUAT_TYPE> &r,
                    DualQuaternion<QUAT_TYPE> &res) const;

    inline void Sub(const DualQuaternion<QUAT_TYPE> &r);

    inline void Sub(const DualQuaternion<QUAT_TYPE> &r,
                    DualQuaternion<QUAT_TYPE> &res) const;

    inline void Mult(const QUAT_TYPE &scalar);

    inline void Mult(const QUAT_TYPE &scalar,
                     DualQuaternion<QUAT_TYPE> &res) const;

    inline void Div(const QUAT_TYPE &scalar);

    inline void Div(const QUAT_TYPE &scalar,
                    DualQuaternion<QUAT_TYPE> &res) const;

    inline void Mult(const DualQuaternion<QUAT_TYPE> &r);

    inline void Mult(const DualQuaternion<QUAT_TYPE> &r,
                     DualQuaternion<QUAT_TYPE> &res) const;

    inline void MultLeft(const DualQuaternion<QUAT_TYPE> &l);
  
    inline void MultLeft(const DualQuaternion<QUAT_TYPE> &l,
                         DualQuaternion<QUAT_TYPE> &res) const;

    inline void MultVec(const Vector3<QUAT_TYPE> &vec,
                        Vector3<QUAT_TYPE> &res) const;

    inline Vector3<QUAT_TYPE> MultVec(const Vector3<QUAT_TYPE> &vec) const;

    inline void SetIdentity();

    inline void Set(const Quaternion<QUAT_TYPE> &q,
                    const Quaternion<QUAT_TYPE> &p);

    inline void SetRealPart(const Quaternion<QUAT_TYPE> &q);

    inline void SetDualPart(const Quaternion<QUAT_TYPE> &p);

    inline Quaternion<QUAT_TYPE> GetRealPart() const;

    inline Quaternion<QUAT_TYPE> GetDualPart() const;

    inline Vector<QUAT_TYPE> GetVector() const;

    /* Get pose parametrization represented by dual quaternion.
       @note Dual quaternion is assumed to have unit norm! */
    PoseParametrization GetPoseParametrization() const;

    /* Get rigid motion (pose transformation) represented by dual quaternion.
       @note Dual quaternion is assumed to have unit norm! */
    void GetRigidMotion(Quaternion<QUAT_TYPE> &rotation,
                        Vector3<QUAT_TYPE> &translation) const;

    /* Get rigid motion (pose transformation) represented by dual quaternion.
       @note Dual quaternion is assumed to have unit norm! */
    inline void GetRigidMotion(RMatrixBase &rotation,
                               Vector3<QUAT_TYPE> &translation) const;

    /* Set dual quaternion from pose parametrization. */
    void SetFromPoseParametrization(const PoseParametrization &params);

    /* Set dual quaternion from rigid motion parametrized by unit quaternion
       and translation vector. */
    void SetFromRigidMotion(const Quaternion<QUAT_TYPE> &rotation,
                            const Vector3<QUAT_TYPE> &translation);

    /* Set dual quaternion from rigid motion parametrized by rotation matrix
       and translation vector. */
    inline void SetFromRigidMotion(const RMatrixBase &rotation,
                                   const Vector3<QUAT_TYPE> &translation);

    inline DualQuaternion<QUAT_TYPE>&
    operator = (const DualQuaternion<QUAT_TYPE> &r)
    { real = r.real; dual = r.dual; return *this; }

    inline DualQuaternion<QUAT_TYPE>&
    operator += (const DualQuaternion<QUAT_TYPE> &r)
    { Add(r); return *this; }

    inline DualQuaternion<QUAT_TYPE>&
    operator -= (const DualQuaternion<QUAT_TYPE> &r)
    { Sub(r); return *this; }

    inline DualQuaternion<QUAT_TYPE>&
    operator *= (const QUAT_TYPE &scalar)
    { Mult(scalar); return *this; }

    inline DualQuaternion<QUAT_TYPE>&
    operator /= (const QUAT_TYPE &scalar)
    { Div(scalar); return *this; }

    inline DualQuaternion<QUAT_TYPE>&
    operator *= (const DualQuaternion<QUAT_TYPE> &r)
    { Mult(r); return *this; }

    inline DualQuaternion<QUAT_TYPE>&
    operator /= (const DualQuaternion<QUAT_TYPE> &r)
    { Mult(r.Inverse()); return *this; }

    inline bool operator == (const DualQuaternion<QUAT_TYPE> &r) const
    { // @todo verify that this is correct!
      return (real == r.real) && (dual == r.dual);
    }

    inline bool operator != (const DualQuaternion<QUAT_TYPE> &r) const
    { return !(*this == r); }

    /* Spherical interpolation between this and given dual quaternion.
       @note Dual quaternions are assumed to have unit norm! */
    DualQuaternion<QUAT_TYPE> Interpolate(const DualQuaternion<QUAT_TYPE> &to,
                                          const QUAT_TYPE &t) const;

    /* Linear interpolation between this and given dual quaternion.
       Unit norm constraint is enforced afterwards by normalization.
       @note Dual quaternions are assumed to have unit norm! */
    DualQuaternion<QUAT_TYPE> InterpolateLinear(const DualQuaternion<QUAT_TYPE> &to,
                                                const QUAT_TYPE &t) const;

    /* Returns 8x8 matrix which expresses left dual quaternion multiplication.
     * A dual quaternion (q,p) consisting of quaternions q = [q0,q1,q2,q3] and
     * p = [p0,p1,p2,p3] is interpreted as a 8-vector [q0,q1,q2,q3,p0,p1,p2,p3].
     * Left multiplying a dual quaternion (q',p') with a dual quaternion (q,p)
     * can be expressed in terms of matrix multiplication as
     * res = (q,p)*(q',p') = LM((q,p))*[q',p'].
     * This method returns the matrix LM(*this).
     */
    Matrix<QUAT_TYPE> GetLeftMultMatrix() const;

    /* Returns 8x8 matrix which expresses right dual quaternion multiplication.
     * A dual quaternion (q,p) consisting of quaternions q = [q0,q1,q2,q3] and
     * p = [p0,p1,p2,p3] is interpreted as a 8-vector [q0,q1,q2,q3,p0,p1,p2,p3].
     * Right multiplying a dual quaternion (q',p') with a dual quaternion (q,p)
     * can be expressed in terms of matrix multiplication as
     * res = (q',p')*(q,p) = RM((q,p))*[q',p'].
     * This method returns the matrix RM(*this).
     */
    Matrix<QUAT_TYPE> GetRightMultMatrix() const;

    /** @brief Enforce rigid coupling constraint for this and the other given
               dual quaternion (equal rotation angle and equal motion pitch/equal
               real and dual scalar part for both).
        Computes direct simple interpolation of both dual quaternion where
        the scalar parts of the real and dual quaternion parts of the resulting
        dual quaternion are identical. This solution can be used as initial
        guess for numerical refinement.
        @param[in/out] other Dual quaternion to enforce rigid coupling constraint with
        @param[in] useOtherAngle Use rotation angle (= real scalar part) of other
                                 dual quaternion instead of interpolating both
        @param[in] useOtherPitch Use motion pitch (= dual scalar part) of other
                                 dual quaternion instead of interpolating both
        @author esquivel 02/2012 */
    void EnforceRigidCouplingConstraint(DualQuaternion<QUAT_TYPE> &other,
                                        bool useOtherAngle = false,
                                        bool useOtherPitch = false);

    /** @brief Enforce rigid coupling constraint for all dual quaternions in given vector.
        @param[in/out] quats Dual quaternions to enforce rigid coupling constraint for
        @param[in] useFirstAngle Use rotation angle (= real scalar part) of first
                                 dual quaternion instead of interpolating all
        @param[in] useFirstPitch Use motion pitch (= dual scalar part) of first
                                 dual quaternion instead of interpolating all
        @author esquivel 02/2012 */
    static void EnforceRigidCouplingConstraint(std::vector< DualQuaternion<QUAT_TYPE> > &quats,
                                               bool useFirstAngle = false,
                                               bool useFirstPitch = false);

  };

  #include <Base/Geometry/DualQuaternionInl.hh>

}

#endif // __DualQuaternion_hh__