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__

---