Tools/Math/Pose3D.h

Go to the documentation of this file.

/**
 * @file Pose3D.h
 * Contains class Pose3D
 *
 * @author <a href="mailto:martin.kallnik@gmx.de">Martin Kallnik</a>
 * @author Max Risler
 */

#ifndef __Pose3D_h__
#define __Pose3D_h__

#include "Matrix.h"

/** representation for 3D Transformation (Location + Orientation)*/
class Pose3D {
  public:

  /** rotation as a RotationMatrix*/
  RotationMatrix rotation;

  /** translation as a Vector3*/
  Vector3<double> translation;

  /** constructor*/
  Pose3D() {}

  /** constructor from rotation and translation
   * \param rot Rotation
   * \param trans Translation
   */
  Pose3D(const RotationMatrix& rot, const Vector3<double>& trans): rotation(rot), translation(trans){}

  /** constructor from rotation
   * \param rot Rotation
   */
  Pose3D(const RotationMatrix& rot): rotation(rot) {}

  /** constructor from translation
   * \param trans Translation
   */
  Pose3D(const Vector3<double>& trans): translation(trans) {}

  /** constructor from three translation values
   * \param x translation x component
   * \param y translation y component
   * \param z translation z component
   */
  Pose3D(const double x, const double y, const double z) : translation(x,y,z) {}

  /** Assignment operator
  *\param other The other Pose3D that is assigned to this one
  *\return A reference to this object after the assignment.
  */
  Pose3D& operator=(const Pose3D& other)
  {
    rotation=other.rotation;
    translation=other.translation;
    
    return *this;
  }

  /** Copy constructor
  *\param other The other vector that is copied to this one
  */
  Pose3D(const Pose3D& other) {*this = other;}

  /** Multiplication with Point
  *\param point (Vector3&lt;double&gt;)
  */
  Vector3<double> operator*(const Vector3<double>& point) const
  {
    return rotation * point + translation;
  }

  /** Comparison of another vector with this one.
  *\param other The other vector that will be compared to this one
  *\return Whether the two vectors are equal.
  */
  bool operator==(const Pose3D& other) const
  {
    return ((translation==other.translation)&&(rotation==other.rotation));
  }

  /** Comparison of another vector with this one.
  *\param other The other vector that will be compared to this one
  *\return Whether the two vectors are unequal.
  */
  bool operator!=(const Pose3D& other) const
    {return !(*this == other);}

  /**Concatenation of this pose with another pose
  *\param other The other pose that will be concatenated to this one.
  *\return A reference to this pose after concatenation
  */
  Pose3D& conc(const Pose3D& other)
  {
    translation = *this * other.translation;
    rotation *= other.rotation;
    return *this;
  }
  
  /** Calculates the inverse transformation from the current pose
  * @return The inverse transformation pose.
  */
  Pose3D invert() const
  {
    Pose3D result;
    result.rotation = this->rotation.transpose();
    result.translation = Vector3<double>(0, 0, 0) - this->translation;
    return result;
  }

  /**Translate this pose by a translation vector
  *\param trans Vector to translate with
  *\return A reference to this pose after translation
  */
  Pose3D& translate(const Vector3<double>& trans)
  {
    translation = *this * trans;
    return *this;
  }

  /**Translate this pose by a translation vector
  *\param x x component of vector to translate with
  *\param y y component of vector to translate with
  *\param z z component of vector to translate with
  *\return A reference to this pose after translation
  */
  Pose3D& translate(const double x, const double y, const double z)
  {
    translation = *this * Vector3<double>(x,y,z);
    return *this;
  }

  /**Rotate this pose by a rotation
  *\param rot Rotationmatrix to rotate with
  *\return A reference to this pose after rotation
  */
  Pose3D& rotate(const RotationMatrix& rot)
  {
    rotation *= rot;
    return *this;
  }

  /**Rotate this pose around its x-axis
  *\param angle angle to rotate with
  *\return A reference to this pose after rotation
  */
  Pose3D& rotateX(const double angle)
  {
    rotation.rotateX(angle);
    return *this;
  }
  
  /**Rotate this pose around its y-axis
  *\param angle angle to rotate with
  *\return A reference to this pose after rotation
  */
  Pose3D& rotateY(const double angle)
  {
    rotation.rotateY(angle);
    return *this;
  }
  
  /**Rotate this pose around its z-axis
  *\param angle angle to rotate with
  *\return A reference to this pose after rotation
  */
  Pose3D& rotateZ(const double angle)
  {
    rotation.rotateZ(angle);
    return *this;
  }

  /**Concatenation of this pose with another pose
  *\param other The other pose that will be concatenated to this one.
  *\return The Concatenation of this pose with the other pose
  */
//    Pose3D& operator*(const Pose3D& other)
//   {
//      return Pose3D(rotation * other.rotation, *this * other.translation);
//    }
};

/**
* Streaming operator that reads a Pose3D from a stream.
* @param stream The stream from which is read.
* @param pose3D The Pose3D object.
* @return The stream.
*/ 
In& operator>>(In& stream, Pose3D& pose3D);

/**
* Streaming operator that writes a Pose3D to a stream.
* @param stream The stream to write on.
* @param pose3D The Pose3D object.
* @return The stream.
*/ 
Out& operator<<(Out& stream, const Pose3D& pose3D);

/* *Concatenation of two 3D poses
 * \param p1 pose1
 * \param p2 pose2
 * /
static Pose3D conc(const Pose3D& p1, const Pose3D& p2){
  return Pose3D(p1.rotation*p2.rotation, p1*p2.translation);  
}
*/

#endif // __Pose3D_h__

/*
* Change log :
* 
* $Log: Pose3D.h,v $
* Revision 1.2  2004/06/17 15:52:06  nistico
* Added visualization of camera motion vector on image
* Fixed cameraMatrix.frameNumber=0 problem when playing logfiles
* (the image frame number is copied)
*
* Revision 1.1.1.1  2004/05/22 17:37:14  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2003/12/02 13:44:56  cesarz
* added streaming operators
*
* Revision 1.1  2003/10/07 10:13:24  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.5  2003/03/06 13:57:34  dueffert
* commented unused methods out to reduce warnings
*
* Revision 1.4  2002/11/19 17:38:32  dueffert
* doxygen bugs corrected
*
* Revision 1.3  2002/11/19 15:43:04  dueffert
* doxygen comments corrected
*
* Revision 1.2  2002/11/12 23:00:47  dueffert
* started restore greenhills compatibility
*
* Revision 1.1  2002/09/22 13:10:50  risler
* new Math headers added
*
*
*/

---