本篇文章将对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;
};
3929
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
