rtcPose3D.h

Go to the documentation of this file.

/*
 * Copyright (C) 2009
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: Autonomous Technologies
 * file .......: rtcPose3D.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 02/29/2008
 * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
 * changed by .: $Author:benjaminpitzer $
 * revision ...: $Revision:141 $
 */
#ifndef RTC_POSE3D_H
#define RTC_POSE3D_H

//== INCLUDES ==================================================================
#include "rtc/rtcMath.h"
#include "rtc/rtcVec6.h"
#include "rtc/rtcRotation.h"

//== NAMESPACES ================================================================
namespace rtc {

template <class T>
class Pose3D {
public:
  // Constructor/Destructor
  Pose3D();
  Pose3D(T x, T y, T z, T roll, T pitch, T yaw);
  Pose3D(const Vec6<T>& pose);
  Pose3D(const Transform<T>& transform);;
  Pose3D(const Rotation<T>& rotation, const Vec3<T>& translation);

  // Accessors
  T x() const;
  T y() const;
  T z() const;
  T roll() const;
  T pitch() const;
  T yaw() const;
  Vec3<T> getTranslation() const;
  Rotation<T> getRotation() const;
  Transform<T> getTransform() const;

  // Mutators
  void set(const Pose3D& pose);
  void set(T x, T y, T z, T roll, T pitch, T yaw);
  void set(const Vec6<T>& pose);
  void set(const Transform<T>& transform);
  void set(const Rotation<T>& rotation, const Vec3<T>& translation);

  // Serialization
  bool write(OutputHandler& oh) const;
  bool read(InputHandler& ih);

  // data
  Vec6<T> p;
};

// Declare a few common typdefs
typedef Pose3D<float> Pose3Df;
typedef Pose3D<double> Pose3Dd;

//==============================================================================
// Pose3D<T>
//==============================================================================

template <class T>
Pose3D<T>::Pose3D()
: p(T(0))
{
}

template <class T>
Pose3D<T>::Pose3D(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
: p(_x,_y,_z,_roll,_pitch,_yaw)
{
}

template <class T>
Pose3D<T>::Pose3D(const Vec6<T>& v)
{
  set(v);
}

template <class T>
Pose3D<T>::Pose3D(const Transform<T>& t)
{
  set(t);
}

template <class T>
Pose3D<T>::Pose3D(const Rotation<T>& rotation, const Vec3<T>& translation)
{
  set(rotation,translation);
}

// Mutators

template <class T>
void Pose3D<T>::set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
{
  p.set(_x,_y,_z,_roll,_pitch,_yaw);
}

template <class T>
void Pose3D<T>::set(const Pose3D& p)
{
  *this = p;
}

template <class T>
void Pose3D<T>::set(const Vec6<T>& v)
{
  p = v;
}

template <class T>
void Pose3D<T>::set(const Transform<T>& t)
{
  Vec3<T> trans = t.getTranslation();
  EulerAnglesf e = t.getRotation();
  set(trans(0),trans(1),trans(2),e.roll(),e.pitch(),e.yaw());
}

template <class T>
void Pose3D<T>::set(const Rotation<T>& rotation, const Vec3<T>& translation)
{
  EulerAnglesf e(rotation);
  set(translation(0),translation(1),translation(2),e.roll(),e.pitch(),e.yaw());
}


template <class T>
T Pose3D<T>::x() const
{
  return p[0];
}

template <class T>
T Pose3D<T>::y() const
{
  return p[1];
}

template <class T>
T Pose3D<T>::z() const
{
  return p[2];
}

template <class T>
T Pose3D<T>::roll() const
{
  return p[3];
}

template <class T>
T Pose3D<T>::pitch() const
{
  return p[4];
}

template <class T>
T Pose3D<T>::yaw() const
{
  return p[5];
}

template <class T>
Vec3<T> Pose3D<T>::getTranslation() const
{
  return Vec3<T>(p[0],p[1],p[2]);
}

template <class T>
Rotation<T> Pose3D<T>::getRotation() const
{
  return Rotation<T>(EulerAngles<T>(p[3],p[4],p[5]));
}

template <class T>
Transform<T> Pose3D<T>::getTransform() const
{
  return Transform<T>(getRotation(),getTranslation());
}

template <class T>
inline bool Pose3D<T>::write(OutputHandler& oh) const {
  return p.write(oh);
}

template <class T>
inline bool Pose3D<T>::read(InputHandler& ih) {
  return p.read(ih);
}


template <class T>
std::ostream& operator<<(std::ostream& os, const Pose3D<T>& pose)
{
  os << pose.p;
  return os;
}

template <class T>
std::istream& operator>>(std::istream& is, Pose3D<T>& pose)
{
  is >> pose.p;
  return is;
}

template <class T>
bool rtc_write(OutputHandler& oh, const Pose3D<T>& data)
{
  return data.write(oh);
};

template <class T>
bool rtc_read(InputHandler& ih, Pose3D<T>& data)
{
  return data.read(ih);
};

//==============================================================================
} // NAMESPACE rtc
//==============================================================================
#endif // RTC_POSE3D_H defined
//==============================================================================