ROS的tf转为RT矩阵

cpp 复制代码
void // Get the TF as an Eigen matrix
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
{
  double mv[12];
  bt.getBasis ().getOpenGLSubMatrix (mv);

  tf::Vector3 origin = bt.getOrigin ();

  out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
  out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
  out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];

  out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
  out_mat (0, 3) = origin.x ();
  out_mat (1, 3) = origin.y ();
  out_mat (2, 3) = origin.z ();
}

这段代码定义了一个函数 transformAsMatrix,它将一个 tf::Transform 对象转换为一个 4x4 的 Eigen 矩阵 (Eigen::Matrix4f)。tf::Transform 是一个表示 3D 空间中的变换的对象,通常包括旋转和平移。下面是这段代码的逐步解释:

函数签名

cpp 复制代码
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
  • const tf::Transform& btbt 是一个 tf::Transform 类型的常量引用,表示输入的变换。
  • Eigen::Matrix4f &out_matout_mat 是一个 Eigen::Matrix4f 类型的引用,表示输出的 4x4 矩阵。

函数内部

  1. 获取旋转矩阵(基础部分)

    cpp 复制代码
    double mv[12];
    bt.getBasis ().getOpenGLSubMatrix (mv);
    • bt.getBasis().getOpenGLSubMatrix(mv):从 tf::Transform 对象 bt 中提取旋转矩阵部分,并以 OpenGL 的行优先顺序存储在 mv 数组中。mv 是一个长度为 12 的数组,包含了旋转矩阵的前 3 行的内容(不包括平移部分)。
  2. 获取平移向量

    cpp 复制代码
    tf::Vector3 origin = bt.getOrigin ();
    • bt.getOrigin():从 tf::Transform 对象中获取平移向量(原点)。这个向量存储了变换的平移部分。
  3. 填充输出矩阵的旋转部分

    cpp 复制代码
    out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
    out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
    out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
    • 这几行代码将 mv 中的元素填入 out_mat 的旋转部分。注意 mv 中的元素是按照行优先顺序存储的。
  4. 填充输出矩阵的平移部分

    cpp 复制代码
    out_mat (0, 3) = origin.x ();
    out_mat (1, 3) = origin.y ();
    out_mat (2, 3) = origin.z ();
    • 这几行代码将平移向量 origin 的 x、y 和 z 分量填入 out_mat 的最后一列。
  5. 填充输出矩阵的齐次坐标部分

    cpp 复制代码
    out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
    • 最后一行 [0, 0, 0, 1] 是为了齐次坐标表示法,确保矩阵是一个有效的 4x4 变换矩阵。前三个元素设置为 0,最后一个元素设置为 1。

总结

  • 该函数将 tf::Transform 对象 bt 中的旋转和平移信息提取出来,并将其填充到 Eigen::Matrix4f 类型的 4x4 矩阵 out_mat 中。这种矩阵可以用来进行3D空间中的坐标变换。
相关推荐
古希腊掌握嵌入式的神1 天前
[ROS]ROS系统是如何协调工作机器人
机器人·ros
慕羽★8 天前
多无人车协同探索开源包启动文件介绍(上)
机器人·ros·gazebo·运动规划·rrt·多无人车·协同探索
Mr.Winter`14 天前
轨迹优化 | 基于梯度下降的路径规划算法(附ROS C++/Python仿真)
c++·人工智能·算法·机器人·自动驾驶·ros·ros2
白云千载尽19 天前
开源的自动驾驶视觉语言模型标注数据集
算法·机器学习·自动驾驶·ros
PNP机器人21 天前
Franka机器人ROS 2 发布:赋能机器人研究和行业应用
人工智能·深度学习·机器人·ros·franka fr3
小仇学长22 天前
ROS实践一构建Gazebo机器人模型文件urdf
机器人·ros
Mr.Winter`1 个月前
深度强化学习 | 详解过估计现象与Double DQN算法(附Pytorch实现)
人工智能·pytorch·深度学习·神经网络·自动驾驶·ros·强化学习
白云千载尽1 个月前
端到端自动驾驶——cnn网络搭建
人工智能·神经网络·算法·机器学习·cnn·自动驾驶·ros
是阿牛啊1 个月前
【ubuntu20安装usv_sim_lsa (无人水面航行器模拟环境) - v0.3】
ubuntu·ros·osg·usv_sim
泽慕1 个月前
ROS的action通信——实现阶乘运算(二)
ros