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& bt
:bt
是一个tf::Transform
类型的常量引用,表示输入的变换。Eigen::Matrix4f &out_mat
:out_mat
是一个Eigen::Matrix4f
类型的引用,表示输出的 4x4 矩阵。
函数内部
-
获取旋转矩阵(基础部分):
cppdouble mv[12]; bt.getBasis ().getOpenGLSubMatrix (mv);
bt.getBasis().getOpenGLSubMatrix(mv)
:从tf::Transform
对象bt
中提取旋转矩阵部分,并以 OpenGL 的行优先顺序存储在mv
数组中。mv
是一个长度为 12 的数组,包含了旋转矩阵的前 3 行的内容(不包括平移部分)。
-
获取平移向量:
cpptf::Vector3 origin = bt.getOrigin ();
bt.getOrigin()
:从tf::Transform
对象中获取平移向量(原点)。这个向量存储了变换的平移部分。
-
填充输出矩阵的旋转部分:
cppout_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
中的元素是按照行优先顺序存储的。
- 这几行代码将
-
填充输出矩阵的平移部分:
cppout_mat (0, 3) = origin.x (); out_mat (1, 3) = origin.y (); out_mat (2, 3) = origin.z ();
- 这几行代码将平移向量
origin
的 x、y 和 z 分量填入out_mat
的最后一列。
- 这几行代码将平移向量
-
填充输出矩阵的齐次坐标部分:
cppout_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空间中的坐标变换。