当前位置: 首页 > 工具软件 > Care2X > 使用案例 >

tf2常用数据类型与常用函数汇总

羊舌高爽
2023-12-01

本篇文章将对tf2的一些常用函数及类进行记录,以方便以后使用.

tf2的wiki教程为 http://wiki.ros.org/tf2/Tutorials

tf2的源码地址为 https://github.com/ros/geometry2

geometry2包里有几个文件夹,分别为 tf2 | tf2_bullet | tf2_eigen | tf2_geometry_msgs | tf2_kdl | tf2_msgs | tf2_py | tf2_ros | tf2_sensor_msgs | tf2_tools

本篇文章将其常用包中的常用函数的源码列举出来,做以后备用.

1 tf2

1.1 tf2::Transform

需要引入头文件 tf2/LinearMath/Transform.h

1.1.1 变量与构造函数

其类的构造函数与变量为

/**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
 *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */
class Transform {
	
  ///Storage for the rotation
	Matrix3x3 m_basis;
  ///Storage for the translation
	Vector3   m_origin;

public:
	
  /**@brief No initialization constructor */
	Transform() {}
  /**@brief Constructor from Quaternion (optional Vector3 )
   * @param q Rotation from quaternion 
   * @param c Translation from Vector (default 0,0,0) */
	explicit TF2SIMD_FORCE_INLINE Transform(const Quaternion& q, 
		const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) 
		: m_basis(q),
		m_origin(c)
	{}

  /**@brief Constructor from Matrix3x3 (optional Vector3)
   * @param b Rotation from Matrix 
   * @param c Translation from Vector default (0,0,0)*/
	explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b, 
		const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
		: m_basis(b),
		m_origin(c)
	{}
  /**@brief Copy constructor */
	TF2SIMD_FORCE_INLINE Transform (const Transform& other)
		: m_basis(other.m_basis),
		m_origin(other.m_origin)
	{	}

1.1.2 常用函数

  /**@brief Return the basis matrix for the rotation */
	TF2SIMD_FORCE_INLINE Matrix3x3&       getBasis()          { return m_basis; }
  /**@brief Return the basis matrix for the rotation */
	TF2SIMD_FORCE_INLINE const Matrix3x3& getBasis()    const { return m_basis; }

  /**@brief Return the origin vector translation */
	TF2SIMD_FORCE_INLINE Vector3&         getOrigin()         { return m_origin; }
  /**@brief Return the origin vector translation */
	TF2SIMD_FORCE_INLINE const Vector3&   getOrigin()   const { return m_origin; }

  /**@brief Return a quaternion representing the rotation */
	Quaternion getRotation() const { 
		Quaternion q;
		m_basis.getRotation(q);
		return q;
	}

  /**@brief Set the translational element
   * @param origin The vector to set the translation to */
	TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) 
	{ 
		m_origin = origin;
	}
	  /**@brief Set the rotational element by Quaternion */
	TF2SIMD_FORCE_INLINE void setRotation(const Quaternion& q)
	{
		m_basis.setRotation(q);
	}

  /**@brief Set this transformation to the identity */
	void setIdentity()
	{
		m_basis.setIdentity();
		m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
	}
	
  /**@brief Return the inverse of this transform */
	Transform inverse() const
	{ 
		Matrix3x3 inv = m_basis.transpose();
		return Transform(inv, inv * -m_origin);
	}
	
	// 重载*号
	TF2SIMD_FORCE_INLINE Transform 
	Transform::operator*(const Transform& t) const
	{
		return Transform(m_basis * t.m_basis, 
			(*this)(t.m_origin));
	}

1.1.3 用法举例

#include <tf2/LinearMath/Transform.h>

	// 1 直接构造
	tf2::Transform transform(
	    tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI / 2),
	    tf2::Vector3(1, 0, 0.0)
	);

    // 2 分步构造
    tf2::Transform transform2;
    transform2.setOrigin(tf2::Vector3(1.0, 2.0, 3.0));
    tf2::Quaternion q;
    q.setRPY(0.0, 0.0, M_PI / 2);
    transform2.setRotation(q);
    
	// 3 单位矩阵
    tf2::Transform transform3;
    transform3.setIdentity();

	// 4 获取平移的xy与角度(弧度)
    std::cout << "origin: " << transform.getOrigin().x() << std::endl;
    // 使用tf2::getYaw() 方法需要引入一个头文件
    #include <tf2/utils.h>
    std::cout << "theta: " << tf2::getYaw(transform.getRotation()) << std::endl;

1.2 tf2::Vector3

需要引入头文件 tf2/LinearMath/Vector3.h


/**@brief tf2::Vector3 can be used to represent 3D points and vectors.
 * It has an un-used w component to suit 16-byte alignment when tf2::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
 * Ideally, this class should be replaced by a platform optimized TF2SIMD version that keeps the data in registers
 */
ATTRIBUTE_ALIGNED16(class) Vector3
{
tf2Scalar	m_floats[4];
public:
  /**@brief Constructor from scalars 
   * @param x X value
   * @param y Y value 
   * @param z Z value 
   */
	TF2SIMD_FORCE_INLINE Vector3(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z)
	{
		m_floats[0] = x;
		m_floats[1] = y;
		m_floats[2] = z;
		m_floats[3] = tf2Scalar(0.);
	}

1.2.2 常用函数

  // 重载了加减乘除

  // 对应相乘,点乘
  /**@brief Return the dot product
   * @param v The other vector in the dot product */
	TF2SIMD_FORCE_INLINE tf2Scalar dot(const Vector3& v) const
	{
		return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
	}
	
	// 距离的平方
	TF2SIMD_FORCE_INLINE tf2Scalar length2() const
	{
		return dot(*this);
	}

	// 距离
	TF2SIMD_FORCE_INLINE tf2Scalar length() const
	{
		return tf2Sqrt(length2());
	}
	
  /**@brief Normalize this vector 
   * x^2 + y^2 + z^2 = 1 */
	TF2SIMD_FORCE_INLINE Vector3& normalize() 
	{
		return *this /= length();
	}

  /**@brief Rotate this vector 
   * @param wAxis The axis to rotate about 
   * @param angle The angle to rotate by */
	TF2SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf2Scalar angle ) const;

  /**@brief Return the angle between this and another vector
   * @param v The other vector */
	TF2SIMD_FORCE_INLINE tf2Scalar angle(const Vector3& v) const 
	{
		tf2Scalar s = tf2Sqrt(length2() * v.length2());
		tf2FullAssert(s != tf2Scalar(0.0));
		return tf2Acos(dot(v) / s);
	}

  /**@brief Return the cross product between this and another vector 
   * @param v The other vector */
	TF2SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const
	{
		return Vector3(
			m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
	}

	 /**@brief Return the x value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; }
  /**@brief Return the y value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; }
  /**@brief Return the z value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; }
  /**@brief Set the x value */
		TF2SIMD_FORCE_INLINE void	setX(tf2Scalar x) { m_floats[0] = x;};
  /**@brief Set the y value */
		TF2SIMD_FORCE_INLINE void	setY(tf2Scalar y) { m_floats[1] = y;};
  /**@brief Set the z value */
		TF2SIMD_FORCE_INLINE void	setZ(tf2Scalar z) {m_floats[2] = z;};
  /**@brief Set the w value */
		TF2SIMD_FORCE_INLINE void	setW(tf2Scalar w) { m_floats[3] = w;};
  /**@brief Return the x value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; }
  /**@brief Return the y value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; }
  /**@brief Return the z value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; }
  /**@brief Return the w value */
		TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; }

1.3 tf2::Matrix3x3

需要引入头文件 tf2/LinearMath/Matrix3x3.h

1.3.1 变量与构造函数

/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3.
* Make sure to only include a pure orthogonal matrix without scaling. */
class Matrix3x3 {

	///Data storage for the matrix, each vector is a row of the matrix
	Vector3 m_el[3];

public:
	/** @brief No initializaion constructor */
	Matrix3x3 () {}
	
	/**@brief Constructor from Quaternion */
	explicit Matrix3x3(const Quaternion& q) { setRotation(q); }

旋转矩阵的私有变量就是由3个vector3组成的矩阵.构造函数常用的就是通过四元数来进行构造.

1.3.2 常用函数

	/** @brief Set the matrix from a quaternion
	*  @param q The Quaternion to match */  
	void setRotation(const Quaternion& q) 
	{
		tf2Scalar d = q.length2();
		tf2FullAssert(d != tf2Scalar(0.0));
		tf2Scalar s = tf2Scalar(2.0) / d;
		tf2Scalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
		tf2Scalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
		tf2Scalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
		tf2Scalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
		setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
			xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx,
			xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy));
	}
	/** @brief Set the matrix from euler angles YPR around ZYX axes
	* @param eulerZ Yaw aboud Z axis
	* @param eulerY Pitch around Y axis
	* @param eulerX Roll about X axis
	* 
	* These angles are used to produce a rotation matrix. The euler
	* angles are applied in ZYX order. I.e a vector is first rotated 
	* about X then Y and then Z
	**/
	void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) 

	/** @brief Set the matrix using RPY about XYZ fixed axes
	 * @param roll Roll about X axis
         * @param pitch Pitch around Y axis
         * @param yaw Yaw aboud Z axis
         * 
	 **/
	void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) { 
               setEulerYPR(yaw, pitch, roll);
	}
	/**@brief Set the matrix to the identity */
	void setIdentity()
	
	static const Matrix3x3&	getIdentity()
	
	/**@brief Get the matrix represented as a quaternion 
	* @param q The quaternion which will be set */
	void getRotation(Quaternion& q) const
	
	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
	* @param yaw Yaw around Z axis
	* @param pitch Pitch around Y axis
	* @param roll around X axis */	
	void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const


	/**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ
	* @param roll around X axis 
	* @param pitch Pitch around Y axis
	* @param yaw Yaw around Z axis
	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
	void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const
	{
		getEulerYPR(yaw, pitch, roll, solution_number);
	}

	TF2SIMD_FORCE_INLINE Matrix3x3 
	Matrix3x3::inverse() const

1.4 tf2::Quaternion

需要引入头文件 tf2/LinearMath/Quaternion.h

1.4.1 变量与构造函数

/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */
class Quaternion : public QuadWord {
public:
  /**@brief No initialization constructor */
	Quaternion() {}

	//		template <typename tf2Scalar>
	//		explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(v) {}
  /**@brief Constructor from scalars */
	Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w) 
		: QuadWord(x, y, z, w) 
	{}
  /**@brief Axis angle Constructor
   * @param axis The axis which the rotation is around
   * @param angle The magnitude of the rotation around the angle (Radians) */
	Quaternion(const Vector3& axis, const tf2Scalar& angle) 
	{ 
		setRotation(axis, angle); 
	}

1.4.2 常用函数

  /**@brief Set the rotation using axis angle notation 
   * @param axis The axis around which to rotate
   * @param angle The magnitude of the rotation in Radians */
	void setRotation(const Vector3& axis, const tf2Scalar& angle)

  /**@brief Set the quaternion using Euler angles
   * @param yaw Angle around Y
   * @param pitch Angle around X
   * @param roll Angle around Z */
	void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)

  /**@brief Set the quaternion using fixed axis RPY
   * @param roll Angle around X 
   * @param pitch Angle around Y
   * @param yaw Angle around Z*/
  void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)

  /**@brief Return the dot product between this quaternion and another
   * @param q The other quaternion */
	tf2Scalar dot(const Quaternion& q) const
	{
		return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
	}

  /**@brief Return the length squared of the quaternion */
	tf2Scalar length2() const
	{
		return dot(*this);
	}

  /**@brief Return the length of the quaternion */
	tf2Scalar length() const
	{
		return tf2Sqrt(length2());
	}

  /**@brief Normalize the quaternion 
   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
	Quaternion& normalize() 
	{
		return *this /= length();
	}

  /**@brief Return the ***half*** angle between this quaternion and the other 
   * @param q The other quaternion */
	tf2Scalar angle(const Quaternion& q) const 
	{
		tf2Scalar s = tf2Sqrt(length2() * q.length2());
		tf2Assert(s != tf2Scalar(0.0));
		return tf2Acos(dot(q) / s);
	}
	/**@brief Return the angle between this quaternion and the other along the shortest path
	* @param q The other quaternion */
	tf2Scalar angleShortestPath(const Quaternion& q) const 
	{
		tf2Scalar s = tf2Sqrt(length2() * q.length2());
		tf2Assert(s != tf2Scalar(0.0));
		if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
			return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
		else 
			return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
	}
        /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
	tf2Scalar getAngle() const 
	{
		tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
		return s;
	}

	/**@brief Return the inverse of this quaternion */
	Quaternion inverse() const
	{
		return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
	}

1.4.3 用法举例

#include <tf2/LinearMath/Transform.h>
    tf2::Quaternion q;
    q.setX(0);
    q.setW(1);
    q.setRotation(tf2::Vector3(0, 0, 1), M_PI / 2);
    q.setRPY(0, 0, M_PI / 2);
    tf2::Quaternion norm_q = q.normalize();
    std::cout << "x: " << norm_q.x() << " y: " << norm_q.y()
        << " z: " << norm_q.z() << " w: " << norm_q.w() << std::endl;

1.5 utils.h

需要引入头文件 tf2/utils.h,这个文件的依赖项为 tf2,tf2_geometry_msgs

这个文件实现了一些常用的数据类型转换的函数

/** Function needed for the generalization of toQuaternion
 * \param q a geometry_msgs::Quaternion
 * \return a copy of the same quaternion as a tf2::Quaternion
 */
inline
tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q)

/** Function needed for the generalization of toQuaternion
 * \param q a geometry_msgs::QuaternionStamped
 * \return a copy of the same quaternion as a tf2::Quaternion
 */
inline
tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q)

/** Function needed for the generalization of toQuaternion
 * \param t some tf2::Stamped object
 * \return a copy of the same quaternion as a tf2::Quaternion
 */
template<typename T>
  tf2::Quaternion toQuaternion(const tf2::Stamped<T>& t)

/** Function needed for the generalization of toQuaternion
 * \param t some tf2::Stamped object
 * \return a copy of the same quaternion as a tf2::Quaternion
 */
template<typename T>
  tf2::Quaternion toQuaternion(const T& t)

/** The code below is blantantly copied from urdfdom_headers
 * only the normalization has been added.
 * It computes the Euler roll, pitch yaw from a tf2::Quaternion
 * It is equivalent to tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
 * \param q a tf2::Quaternion
 * \param yaw the computed yaw
 * \param pitch the computed pitch
 * \param roll the computed roll
 */
inline
void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll)

/** The code below is a simplified version of getEulerRPY that only
 * returns the yaw. It is mostly useful in navigation where only yaw
 * matters
 * \param q a tf2::Quaternion
 * \return the computed yaw
 */
inline
double getYaw(const tf2::Quaternion& q)

2 tf2_ros

在ros中进行tf的订阅与发布,相关操作

以后再更新吧...

3 tf2_geometry_msgs

geometry_msgs 与 tf2 数据类型间的转换

4 tf2_sensor_msgs

只有一个有用函数 doTransform()

将点云按照输入的坐标变换进行每个点的坐标变换,输出变换后的点云

/** \brief Extract a timestamp from the header of a PointCloud2 message.
 * This function is a specialization of the getTimestamp template defined in tf2/convert.h.
 * \param t PointCloud2 message to extract the timestamp from.
 * \return The timestamp of the message. The lifetime of the returned reference
 * is bound to the lifetime of the argument.
 */
template <>
inline
const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;}

/** \brief Extract a frame ID from the header of a PointCloud2 message.
 * This function is a specialization of the getFrameId template defined in tf2/convert.h.
 * \param t PointCloud2 message to extract the frame ID from.
 * \return A string containing the frame ID of the message. The lifetime of the
 * returned reference is bound to the lifetime of the argument.
 */
template <>
inline
const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}

// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)

5 tf2_eigen

geometry_msgs 与 Eigen 数据类型间的转换

6 tf2数据的相乘与打印

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"

#include <tf/transform_datatypes.h>

#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

#include <tf2/LinearMath/Transform.h>
#include <tf2/utils.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_tf"); //Init ROS

    tf2::Quaternion q;
    q.setX(0);
    q.setW(1);
    q.setRotation(tf2::Vector3(0, 0, 1), M_PI / 2);
    q.setRPY(0, 0, M_PI / 2);
    tf2::Quaternion norm_q = q.normalize();
    // std::cout << "x: " << norm_q.x() << " y: " << norm_q.y()
    //     << " z: " << norm_q.z() << " w: " << norm_q.w() << std::endl;

    tf2::Transform baselink_to_laserlink(
        tf2::Quaternion(tf2::Vector3(1, 0, 0), M_PI),
        tf2::Vector3(0.5, 0, 0));

    tf2::Transform coord1_in_laserlink(
        tf2::Quaternion(tf2::Vector3(0, 0, 1), 0),
        tf2::Vector3(1, 1, 0));

    tf2::Transform coord1_in_baselink = baselink_to_laserlink * coord1_in_laserlink * baselink_to_laserlink.inverse();

    std::cout << "base in odom x: " << coord1_in_baselink.getOrigin().x() 
              << " , y: " << coord1_in_baselink.getOrigin().y()
              << " , z: " << coord1_in_baselink.getOrigin().z() << std::endl;
    std::cout << "theta: " << tf2::getYaw(coord1_in_baselink.getRotation()) * 180 / M_PI << std::endl;


    tf::Transform base_to_laser_(
        tf::Quaternion(tf::Vector3(1, 0, 0), M_PI ),
        tf::Vector3(0, 0, 1));

    tf::Transform coord2_in_laserlink(
        tf::Quaternion(tf::Vector3(0, 0, 1), 0 ),
        tf::Vector3(1, 2, 3));

    // tf::Transform coord2_in_baselink = base_to_laser_ * coord2_in_laserlink * base_to_laser_.inverse();
    tf::Transform coord2_in_baselink = base_to_laser_ * coord2_in_laserlink;

    tf::Vector3 pointPosBaseFrame(base_to_laser_ * tf::Vector3(
        coord2_in_laserlink.getOrigin().x(), coord2_in_laserlink.getOrigin().y(), coord2_in_laserlink.getOrigin().y()));


    std::cout << "coord2_in_baselink x: " << coord2_in_baselink.getOrigin().x() 
              << " , y: " << coord2_in_baselink.getOrigin().y()
              << " , z: " << coord2_in_baselink.getOrigin().z() << std::endl;
    std::cout << "theta: " << tf::getYaw(coord2_in_baselink.getRotation()) * 180 / M_PI << std::endl;

    return 0;
};
 类似资料: