mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
8b8511e154
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
276 lines
8.9 KiB
C++
276 lines
8.9 KiB
C++
/**********************************************************************
|
|
*
|
|
* This source code is part of the Tree-based Network Optimizer (TORO)
|
|
*
|
|
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
|
|
* Slawomir Grzonka, and Wolfram Burgard
|
|
*
|
|
* TORO is licences under the Common Creative License,
|
|
* Attribution-NonCommercial-ShareAlike 3.0
|
|
*
|
|
* You are free:
|
|
* - to Share - to copy, distribute and transmit the work
|
|
* - to Remix - to adapt the work
|
|
*
|
|
* Under the following conditions:
|
|
*
|
|
* - Attribution. You must attribute the work in the manner specified
|
|
* by the author or licensor (but not in any way that suggests that
|
|
* they endorse you or your use of the work).
|
|
*
|
|
* - Noncommercial. You may not use this work for commercial purposes.
|
|
*
|
|
* - Share Alike. If you alter, transform, or build upon this work,
|
|
* you may distribute the resulting work only under the same or
|
|
* similar license to this one.
|
|
*
|
|
* Any of the above conditions can be waived if you get permission
|
|
* from the copyright holder. Nothing in this license impairs or
|
|
* restricts the author's moral rights.
|
|
*
|
|
* TORO is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
|
|
* PURPOSE.
|
|
**********************************************************************/
|
|
|
|
#ifndef _TRANSFORMATION3_HXX_
|
|
#define _TRANSFORMATION3_HXX_
|
|
|
|
#include <assert.h>
|
|
#include <cmath>
|
|
#include "dmatrix.hh"
|
|
|
|
namespace AISNavigation {
|
|
|
|
template <class T>
|
|
struct Vector3 {
|
|
T elems[3] ;
|
|
|
|
Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
|
|
Vector3() {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
|
|
Vector3(const DVector<T>& t){}
|
|
|
|
// translational view
|
|
inline const T& x() const {return elems[0];}
|
|
inline const T& y() const {return elems[1];}
|
|
inline const T& z() const {return elems[2];}
|
|
inline T& x() {return elems[0];}
|
|
inline T& y() {return elems[1];}
|
|
inline T& z() {return elems[2];}
|
|
|
|
// rotational view
|
|
inline const T& roll() const {return elems[0];}
|
|
inline const T& pitch() const {return elems[1];}
|
|
inline const T& yaw() const {return elems[2];}
|
|
inline T& roll() {return elems[0];}
|
|
inline T& pitch() {return elems[1];}
|
|
inline T& yaw() {return elems[2];}
|
|
|
|
};
|
|
|
|
template <class T>
|
|
struct Pose3 : public DVector<T>{
|
|
Pose3();
|
|
Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
|
|
Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
|
|
Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);}
|
|
|
|
inline operator const DVector<T>& () {return (const DVector<T>)*this;}
|
|
inline operator DVector<T>& () {return *this;}
|
|
|
|
inline const T& roll() const {return DVector<T>::elems[0];}
|
|
inline const T& pitch() const {return DVector<T>::elems[1];}
|
|
inline const T& yaw() const {return DVector<T>::elems[2];}
|
|
inline const T& x() const {return DVector<T>::elems[3];}
|
|
inline const T& y() const {return DVector<T>::elems[4];}
|
|
inline const T& z() const {return DVector<T>::elems[5];}
|
|
|
|
inline T& roll() {return DVector<T>::elems[0];}
|
|
inline T& pitch() {return DVector<T>::elems[1];}
|
|
inline T& yaw() {return DVector<T>::elems[2];}
|
|
inline T& x() {return DVector<T>::elems[3];}
|
|
inline T& y() {return DVector<T>::elems[4];}
|
|
inline T& z() {return DVector<T>::elems[5];}
|
|
|
|
};
|
|
|
|
|
|
/*!
|
|
* A Quaternion can be used to either represent a rotational axis
|
|
* and a Rotation, or, the point which will be rotated
|
|
*/
|
|
|
|
template <class T>
|
|
struct Quaternion{
|
|
|
|
/*!
|
|
* Default Constructor: w=x=y=z=0;
|
|
*/
|
|
Quaternion();
|
|
|
|
/*!
|
|
* The Quaternion representation of the point "pose"
|
|
*/
|
|
Quaternion(const Vector3<T>& pose);
|
|
|
|
/*!
|
|
* create a Quaternion by scalar w and the imaginery parts x,y, and z.
|
|
*/
|
|
Quaternion(const T _w, const T _x, const T _y, const T _z);
|
|
|
|
/*!
|
|
* create a rotational Quaternion, roll along x-axis, pitch along y-axis and yaw along z-axis
|
|
*/
|
|
Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
|
|
|
|
/*!
|
|
* @return the conjugated version of this quaternion
|
|
*/
|
|
inline Quaternion<T> conjugated() const;
|
|
|
|
/*!
|
|
* @return this quaternion, but normalized
|
|
*/
|
|
inline Quaternion<T> normalized() const;
|
|
|
|
/*!
|
|
* @return the inverse of this Quaternion
|
|
*/
|
|
inline Quaternion<T> inverse() const;
|
|
|
|
/*construct a quaternion on the axis/angle representation*/
|
|
inline Quaternion(const Vector3<T>& axis, const T& angle);
|
|
|
|
/*!
|
|
* if this Quaternion represents a point, use this function
|
|
* to rotate the point along <axis> with angle <alpha>
|
|
* @param axis the rotational axis
|
|
* @param alpha rotational angle
|
|
*/
|
|
inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
|
|
|
|
|
|
/*!
|
|
* if this Quaternion represents a rotational axis + rotation,
|
|
* use this function to rotate another point represented as a Quaternion p
|
|
* @param p the point to be rotated by <this>. Point is represented as a Quaternion
|
|
* @return rotated Point (represented as a Quaternion)
|
|
*/
|
|
inline Quaternion<T> rotatePoint(const Quaternion& p) const;
|
|
|
|
/*!
|
|
* if this Quaternion represents a rotational axis + rotation,
|
|
* use this function to rotate another point
|
|
* @param p the point to be rotated by <this>.
|
|
* @return rotated Point
|
|
*/
|
|
inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
|
|
|
|
/*!
|
|
* if this Quaternion represents a rotational axis, add a rotation of angle <alpha>
|
|
* along <this> axis to the Quaternion
|
|
* @param alpha rotational value
|
|
* @return this Quaternion with included information about the rotation along <this> axis
|
|
*/
|
|
inline Quaternion withRotation (const T alpha) const;
|
|
|
|
/*!
|
|
* Given rotational axis x,y,z, get the rotation along these axis encoded in this Quaternion
|
|
* @return rotation along x,y,z axis encoded in <this> Quaternion
|
|
*/
|
|
inline Vector3<T> toAngles() const;
|
|
|
|
inline Vector3<T> axis() const;
|
|
inline T angle() const;
|
|
|
|
|
|
/*!
|
|
* @return the norm of this Quaternion
|
|
*/
|
|
inline T norm() const;
|
|
|
|
/*!
|
|
* @return the real part (==w) of this Quaternion
|
|
*/
|
|
inline T re() const;
|
|
|
|
/*!
|
|
* @return the imaginery part (== (x,y,z)) of this Quaternion
|
|
*/
|
|
inline Vector3<T> im() const;
|
|
|
|
T w,x,y,z;
|
|
};
|
|
|
|
|
|
|
|
template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
|
|
template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
|
|
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
|
|
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
|
|
template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
|
|
template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
|
|
|
|
template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
|
|
template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
|
|
|
|
|
|
|
|
template <class T>
|
|
struct Transformation3{
|
|
Quaternion<T> rotationQuaternion;
|
|
Vector3<T> translationVector;
|
|
|
|
Transformation3(){}
|
|
|
|
inline static Transformation3<T> identity();
|
|
|
|
Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
|
|
Transformation3 (const Pose3<T>& v);
|
|
Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
|
|
|
|
inline Vector3<T> translation() const;
|
|
inline Quaternion <T> rotation() const;
|
|
|
|
inline Pose3<T> toPoseType() const;
|
|
|
|
inline void setTranslation(const Vector3<T>& t);
|
|
inline void setTranslation(const T& x, const T& y, const T& z);
|
|
|
|
inline void setRotation(const Vector3<T>& r);
|
|
inline void setRotation(const T& roll, const T& pitch, const T& yaw);
|
|
inline void setRotation(const Quaternion<T>& q);
|
|
|
|
|
|
inline Transformation3<T> inv() const;
|
|
inline bool validRotation(const T& epsilon=0.001) const;
|
|
|
|
};
|
|
|
|
template <class T>
|
|
inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
|
|
|
|
template <class T>
|
|
inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
|
|
|
|
template <class T>
|
|
struct Operations3D{
|
|
typedef T BaseType;
|
|
typedef Pose3<T> PoseType;
|
|
typedef Quaternion<T> RotationType;
|
|
typedef Vector3<T> TranslationType;
|
|
typedef Transformation3<T> TransformationType;
|
|
typedef DMatrix<T> CovarianceType;
|
|
typedef DMatrix<T> InformationType;
|
|
typedef Transformation3<T> ParametersType;
|
|
};
|
|
|
|
} // namespace AISNavigation
|
|
/**************************** IMPLEMENTATION ****************************/
|
|
|
|
#include "transformation3.hxx"
|
|
|
|
#endif
|
|
|