rtcPose3D.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcPose3D.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 02/29/2008
00015  * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
00016  * changed by .: $Author:benjaminpitzer $
00017  * revision ...: $Revision:141 $
00018  */
00019 #ifndef RTC_POSE3D_H
00020 #define RTC_POSE3D_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVec6.h"
00025 #include "rtc/rtcRotation.h"
00026 
00027 //== NAMESPACES ================================================================
00028 namespace rtc {
00029 
00033 template <class T>
00034 class Pose3D {
00035 public:
00036   // Constructor/Destructor
00037   Pose3D();
00038   Pose3D(T x, T y, T z, T roll, T pitch, T yaw);
00039   Pose3D(const Vec6<T>& pose);
00040   Pose3D(const Transform<T>& transform);;
00041   Pose3D(const Rotation<T>& rotation, const Vec3<T>& translation);
00042 
00043   // Accessors
00044   T x() const;
00045   T y() const;
00046   T z() const;
00047   T roll() const;
00048   T pitch() const;
00049   T yaw() const;
00050   Vec3<T> getTranslation() const;
00051   Rotation<T> getRotation() const;
00052   Transform<T> getTransform() const;
00053 
00054   // Mutators
00055   void set(const Pose3D& pose);
00056   void set(T x, T y, T z, T roll, T pitch, T yaw);
00057   void set(const Vec6<T>& pose);
00058   void set(const Transform<T>& transform);
00059   void set(const Rotation<T>& rotation, const Vec3<T>& translation);
00060 
00061   // Serialization
00062   bool write(OutputHandler& oh) const;
00063   bool read(InputHandler& ih);
00064 
00065   // data
00066   Vec6<T> p;
00067 };
00068 
00069 // Declare a few common typdefs
00070 typedef Pose3D<float> Pose3Df;
00071 typedef Pose3D<double> Pose3Dd;
00072 
00073 //==============================================================================
00074 // Pose3D<T>
00075 //==============================================================================
00076 
00080 template <class T>
00081 Pose3D<T>::Pose3D()
00082 : p(T(0))
00083 {
00084 }
00085 
00089 template <class T>
00090 Pose3D<T>::Pose3D(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
00091 : p(_x,_y,_z,_roll,_pitch,_yaw)
00092 {
00093 }
00094 
00098 template <class T>
00099 Pose3D<T>::Pose3D(const Vec6<T>& v)
00100 {
00101   set(v);
00102 }
00103 
00107 template <class T>
00108 Pose3D<T>::Pose3D(const Transform<T>& t)
00109 {
00110   set(t);
00111 }
00112 
00117 template <class T>
00118 Pose3D<T>::Pose3D(const Rotation<T>& rotation, const Vec3<T>& translation)
00119 {
00120   set(rotation,translation);
00121 }
00122 
00123 // Mutators
00124 
00127 template <class T>
00128 void Pose3D<T>::set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
00129 {
00130   p.set(_x,_y,_z,_roll,_pitch,_yaw);
00131 }
00132 
00136 template <class T>
00137 void Pose3D<T>::set(const Pose3D& p)
00138 {
00139   *this = p;
00140 }
00141 
00145 template <class T>
00146 void Pose3D<T>::set(const Vec6<T>& v)
00147 {
00148   p = v;
00149 }
00150 
00154 template <class T>
00155 void Pose3D<T>::set(const Transform<T>& t)
00156 {
00157   Vec3<T> trans = t.getTranslation();
00158   EulerAnglesf e = t.getRotation();
00159   set(trans(0),trans(1),trans(2),e.roll(),e.pitch(),e.yaw());
00160 }
00161 
00166 template <class T>
00167 void Pose3D<T>::set(const Rotation<T>& rotation, const Vec3<T>& translation)
00168 {
00169   EulerAnglesf e(rotation);
00170   set(translation(0),translation(1),translation(2),e.roll(),e.pitch(),e.yaw());
00171 }
00172 
00173 
00176 template <class T>
00177 T Pose3D<T>::x() const
00178 {
00179   return p[0];
00180 }
00181 
00184 template <class T>
00185 T Pose3D<T>::y() const
00186 {
00187   return p[1];
00188 }
00189 
00192 template <class T>
00193 T Pose3D<T>::z() const
00194 {
00195   return p[2];
00196 }
00197 
00200 template <class T>
00201 T Pose3D<T>::roll() const
00202 {
00203   return p[3];
00204 }
00205 
00208 template <class T>
00209 T Pose3D<T>::pitch() const
00210 {
00211   return p[4];
00212 }
00213 
00216 template <class T>
00217 T Pose3D<T>::yaw() const
00218 {
00219   return p[5];
00220 }
00221 
00224 template <class T>
00225 Vec3<T> Pose3D<T>::getTranslation() const
00226 {
00227   return Vec3<T>(p[0],p[1],p[2]);
00228 }
00229 
00232 template <class T>
00233 Rotation<T> Pose3D<T>::getRotation() const
00234 {
00235   return Rotation<T>(EulerAngles<T>(p[3],p[4],p[5]));
00236 }
00237 
00240 template <class T>
00241 Transform<T> Pose3D<T>::getTransform() const
00242 {
00243   return Transform<T>(getRotation(),getTranslation());
00244 }
00245 
00248 template <class T>
00249 inline bool Pose3D<T>::write(OutputHandler& oh) const {
00250   return p.write(oh);
00251 }
00252 
00255 template <class T>
00256 inline bool Pose3D<T>::read(InputHandler& ih) {
00257   return p.read(ih);
00258 }
00259 
00260 
00263 template <class T>
00264 std::ostream& operator<<(std::ostream& os, const Pose3D<T>& pose)
00265 {
00266   os << pose.p;
00267   return os;
00268 }
00269 
00272 template <class T>
00273 std::istream& operator>>(std::istream& is, Pose3D<T>& pose)
00274 {
00275   is >> pose.p;
00276   return is;
00277 }
00278 
00282 template <class T>
00283 bool rtc_write(OutputHandler& oh, const Pose3D<T>& data)
00284 {
00285   return data.write(oh);
00286 };
00287 
00291 template <class T>
00292 bool rtc_read(InputHandler& ih, Pose3D<T>& data)
00293 {
00294   return data.read(ih);
00295 };
00296 
00297 //==============================================================================
00298 } // NAMESPACE rtc
00299 //==============================================================================
00300 #endif // RTC_POSE3D_H defined
00301 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53