Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/Math/Pose3D.h

Go to the documentation of this file.

    00001 /**
    00002  * @file Pose3D.h
    00003  * Contains class Pose3D
    00004  *
    00005  * @author <a href="mailto:martin.kallnik@gmx.de">Martin Kallnik</a>
    00006  * @author Max Risler
    00007  */
    00008
    00009 #ifndef __Pose3D_h__
    00010 #define __Pose3D_h__
    00011
    00012 #include "Matrix.h"
    00013
    00014 /** representation for 3D Transformation (Location + Orientation)*/
    00015 class Pose3D {
    00016   public:
    00017
    00018   /** rotation as a RotationMatrix*/
    00019   RotationMatrix rotation;
    00020
    00021   /** translation as a Vector3*/
    00022   Vector3<double> translation;
    00023
    00024   /** constructor*/
    00025   Pose3D() {}
    00026
    00027   /** constructor from rotation and translation
    00028    * \param rot Rotation
    00029    * \param trans Translation
    00030    */
    00031   Pose3D(const RotationMatrix& rot, const Vector3<double>& trans): rotation(rot), translation(trans){}
    00032
    00033   /** constructor from rotation
    00034    * \param rot Rotation
    00035    */
    00036   Pose3D(const RotationMatrix& rot): rotation(rot) {}
    00037
    00038   /** constructor from translation
    00039    * \param trans Translation
    00040    */
    00041   Pose3D(const Vector3<double>& trans): translation(trans) {}
    00042
    00043   /** constructor from three translation values
    00044    * \param x translation x component
    00045    * \param y translation y component
    00046    * \param z translation z component
    00047    */
    00048   Pose3D(const double x, const double y, const double z) : translation(x,y,z) {}
    00049
    00050   /** Assignment operator
    00051   *\param other The other Pose3D that is assigned to this one
    00052   *\return A reference to this object after the assignment.
    00053   */
    00054   Pose3D& operator=(const Pose3D& other)
    00055   {
    00056     rotation=other.rotation;
    00057     translation=other.translation;
    00058
    00059     return *this;
    00060   }
    00061
    00062   /** Copy constructor
    00063   *\param other The other vector that is copied to this one
    00064   */
    00065   Pose3D(const Pose3D& other) {*this = other;}
    00066
    00067   /** Multiplication with Point
    00068   *\param point (Vector3&lt;double&gt;)
    00069   */
    00070   Vector3<double> operator*(const Vector3<double>& point) const
    00071   {
    00072     return rotation * point + translation;
    00073   }
    00074
    00075   /** Comparison of another vector with this one.
    00076   *\param other The other vector that will be compared to this one
    00077   *\return Whether the two vectors are equal.
    00078   */
    00079   bool operator==(const Pose3D& other) const
    00080   {
    00081     return ((translation==other.translation)&&(rotation==other.rotation));
    00082   }
    00083
    00084   /** Comparison of another vector with this one.
    00085   *\param other The other vector that will be compared to this one
    00086   *\return Whether the two vectors are unequal.
    00087   */
    00088   bool operator!=(const Pose3D& other) const
    00089     {return !(*this == other);}
    00090
    00091   /**Concatenation of this pose with another pose
    00092   *\param other The other pose that will be concatenated to this one.
    00093   *\return A reference to this pose after concatenation
    00094   */
    00095   Pose3D& conc(const Pose3D& other)
    00096   {
    00097     translation = *this * other.translation;
    00098     rotation *= other.rotation;
    00099     return *this;
    00100   }
    00101
    00102   /** Calculates the inverse transformation from the current pose
    00103   * @return The inverse transformation pose.
    00104   */
    00105   Pose3D invert() const
    00106   {
    00107     Pose3D result;
    00108     result.rotation = this->rotation.transpose();
    00109     result.translation = Vector3<double>(0, 0, 0) - this->translation;
    00110     return result;
    00111   }
    00112
    00113   /**Translate this pose by a translation vector
    00114   *\param trans Vector to translate with
    00115   *\return A reference to this pose after translation
    00116   */
    00117   Pose3D& translate(const Vector3<double>& trans)
    00118   {
    00119     translation = *this * trans;
    00120     return *this;
    00121   }
    00122
    00123   /**Translate this pose by a translation vector
    00124   *\param x x component of vector to translate with
    00125   *\param y y component of vector to translate with
    00126   *\param z z component of vector to translate with
    00127   *\return A reference to this pose after translation
    00128   */
    00129   Pose3D& translate(const double x, const double y, const double z)
    00130   {
    00131     translation = *this * Vector3<double>(x,y,z);
    00132     return *this;
    00133   }
    00134
    00135   /**Rotate this pose by a rotation
    00136   *\param rot Rotationmatrix to rotate with
    00137   *\return A reference to this pose after rotation
    00138   */
    00139   Pose3D& rotate(const RotationMatrix& rot)
    00140   {
    00141     rotation *= rot;
    00142     return *this;
    00143   }
    00144
    00145   /**Rotate this pose around its x-axis
    00146   *\param angle angle to rotate with
    00147   *\return A reference to this pose after rotation
    00148   */
    00149   Pose3D& rotateX(const double angle)
    00150   {
    00151     rotation.rotateX(angle);
    00152     return *this;
    00153   }
    00154
    00155   /**Rotate this pose around its y-axis
    00156   *\param angle angle to rotate with
    00157   *\return A reference to this pose after rotation
    00158   */
    00159   Pose3D& rotateY(const double angle)
    00160   {
    00161     rotation.rotateY(angle);
    00162     return *this;
    00163   }
    00164
    00165   /**Rotate this pose around its z-axis
    00166   *\param angle angle to rotate with
    00167   *\return A reference to this pose after rotation
    00168   */
    00169   Pose3D& rotateZ(const double angle)
    00170   {
    00171     rotation.rotateZ(angle);
    00172     return *this;
    00173   }
    00174
    00175   /**Concatenation of this pose with another pose
    00176   *\param other The other pose that will be concatenated to this one.
    00177   *\return The Concatenation of this pose with the other pose
    00178   */
    00179 //    Pose3D& operator*(const Pose3D& other)
    00180 //   {
    00181 //      return Pose3D(rotation * other.rotation, *this * other.translation);
    00182 //    }
    00183 };
    00184
    00185 /**
    00186 * Streaming operator that reads a Pose3D from a stream.
    00187 * @param stream The stream from which is read.
    00188 * @param pose3D The Pose3D object.
    00189 * @return The stream.
    00190 */
    00191 In& operator>>(In& stream, Pose3D& pose3D);
    00192
    00193 /**
    00194 * Streaming operator that writes a Pose3D to a stream.
    00195 * @param stream The stream to write on.
    00196 * @param pose3D The Pose3D object.
    00197 * @return The stream.
    00198 */
    00199 Out& operator<<(Out& stream, const Pose3D& pose3D);
    00200
    00201 /* *Concatenation of two 3D poses
    00202  * \param p1 pose1
    00203  * \param p2 pose2
    00204  * /
    00205 static Pose3D conc(const Pose3D& p1, const Pose3D& p2){
    00206   return Pose3D(p1.rotation*p2.rotation, p1*p2.translation);
    00207 }
    00208 */
    00209
    00210 #endif // __Pose3D_h__