Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef TRANSFORM_UTIL_TRANSFORM_H_
00031 #define TRANSFORM_UTIL_TRANSFORM_H_
00032
00033 #include <boost/shared_ptr.hpp>
00034
00035 #include <ros/ros.h>
00036
00037 #include <tf/transform_datatypes.h>
00038
00039 namespace swri_transform_util
00040 {
00049 class TransformImpl
00050 {
00051 public:
00052 TransformImpl() {}
00053 virtual ~TransformImpl() {}
00054
00061 virtual void Transform(
00062 const tf::Vector3& v_in, tf::Vector3& v_out) const = 0;
00063
00071 virtual tf::Quaternion GetOrientation() const
00072 {
00073 tf::Vector3 offset;
00074 Transform(tf::Vector3(1, 0, 0), offset);
00075
00076 tf::Vector3 origin;
00077 Transform(tf::Vector3(0, 0, 0), origin);
00078
00079 tf::Vector3 vector = offset - origin;
00080
00081
00082
00083
00084
00085 tf::Vector3 cross = tf::Vector3(1, 0, 0).cross(vector);
00086 double w = vector.length() + tf::Vector3(1, 0, 0).dot(vector);
00087 return tf::Quaternion(cross.x(), cross.y(), cross.z(), w).normalized();
00088 }
00089
00090 virtual boost::shared_ptr<TransformImpl> Inverse() const = 0;
00091
00093 ros::Time stamp_;
00094 };
00095 typedef boost::shared_ptr<TransformImpl> TransformImplPtr;
00096
00105 class Transform
00106 {
00107 public:
00111 Transform();
00112
00118 explicit Transform(const tf::Transform& transform);
00119
00125 explicit Transform(const tf::StampedTransform& transform);
00126
00132 explicit Transform(boost::shared_ptr<TransformImpl> transform);
00133
00141 Transform& operator=(const tf::Transform transform);
00142
00150 Transform& operator=(boost::shared_ptr<TransformImpl> transform);
00151
00159 tf::Vector3 operator()(const tf::Vector3& v) const;
00160
00168 tf::Vector3 operator*(const tf::Vector3& v) const;
00169
00177 tf::Quaternion operator*(const tf::Quaternion& q) const;
00178
00184 tf::Transform GetTF() const;
00185
00191 Transform Inverse() const;
00192
00201 tf::Vector3 GetOrigin() const;
00202
00208 tf::Quaternion GetOrientation() const;
00209
00214 ros::Time GetStamp() { return transform_->stamp_; }
00215
00216 private:
00218 boost::shared_ptr<TransformImpl> transform_;
00219 };
00220
00225 class IdentityTransform : public TransformImpl
00226 {
00227 public:
00231 IdentityTransform() { stamp_ = ros::Time(0); }
00232
00239 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00240 virtual TransformImplPtr Inverse() const;
00241 };
00242
00247 class TfTransform : public TransformImpl
00248 {
00249 public:
00254 explicit TfTransform(const tf::Transform& transform);
00255
00260 explicit TfTransform(const tf::StampedTransform& transform);
00261
00268 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00269
00274 virtual tf::Quaternion GetOrientation() const;
00275 virtual TransformImplPtr Inverse() const;
00276
00277 protected:
00278 tf::Transform transform_;
00279 };
00280 }
00281
00282 #endif // TRANSFORM_UTIL_TRANSFORM_H_