沉默杀手
tf(三) lookTransform和齐次变换矩阵的关系推导
|
Word count: 775|Reading time: 4 min

建立tf树 A—->B—->C，求 C—->A变换

1
2
rosrun tf static_transform_publisher 0.169, 0.035, -0.125,   -1.57, 0, -1.57  A  B 100
rosrun tf static_transform_publisher   0,   0.015,     0,    -1.57, 0, -1.57  B  C 100

平时使用的tf是tf::Transformer::lookupTransform(target_frame, source_frame)，表示的是 source_frame ---> target_frame的变换，获得的位姿关系是子在父坐标系中，所以是lookupTransform("A", "B")，根据《SLAM十四讲》63页的表示方法，写成
，也就是父子顺序。 因此 C—->A变换是

代码

CMake关键部分：

1
2
3
4
5
6
7
find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  roscpp
  tf
  pcl_ros
)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)

c++关键部分
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
#include <ros/ros.h>
#include <Eigen/Dense>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>

tf::TransformListener listener;

ros::Rate rate(10.0);
while (node.ok())
{
	//创建一个StampedTransform对象存储变换结果数据
	tf::StampedTransform ab;
	tf::StampedTransform bc;
	tf::StampedTransform tfAC;
	try{
	    listener.lookupTransform("A", "B",
	                             ros::Time(0), ab);
	    listener.lookupTransform("B", "C",
	                             ros::Time(0), bc);
	    listener.lookupTransform("A", "C",
	                             ros::Time(0), tfAC);
	    // 输出 lookupTransform 输出的 C--->A 变换
	    cout << "X: "<< tfAC.getOrigin().getX() << " Y: "<< tfAC.getOrigin().getY()
	            <<"  Z: "<< tfAC.getOrigin().getZ()<< endl;
	    cout << "quaternion: "<< tfAC.getRotation().x() <<" " << tfAC.getRotation().y()<<
	            " " << tfAC.getRotation().z() << " " << tfAC.getRotation().w() << endl;

    Eigen::Matrix4f AB, BC, AC;
    // 使用 pcl_ros 直接得到 4x4的齐次变换矩阵
    pcl_ros::transformAsMatrix(ab, AB);   
    pcl_ros::transformAsMatrix(bc, BC);
    std::cout << "AB:"  <<std::endl;
    std::cout << AB  <<std::endl;
    std::cout << "BC:"  <<std::endl;
    std::cout << BC  <<std::endl;
    std::cout << "AC:"  <<std::endl;
    AC = AB * BC;
    std::cout << AC  <<std::endl;
    cout << "-------------------------------------" << endl;
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
  }
}

可以看到AC的右侧平移部分和tfAC.getOrigin()部分相同

tf::lookupTransform的源码一直追溯到tf2_ros::Buffer::lookupTransform，源码在f2/src/buffer_core.cpp，注意其中的accum函数和BufferCore::walkToTopParent函数

tf_eigen

tf_eigen.h提供了不少很有用的函数，用于tf和Eigen之间的矩阵和向量互相转换。使用前

1
2
3
4
find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  tf_conversions
)

头文件#include <tf_conversions/tf_eigen.h>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// Converts an Eigen Quaternion into a tf Matrix3x3.
void  tf::matrixEigenToTF (const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
  // Converts a tf Matrix3x3 into an Eigen Quaternion.
void  tf::matrixTFToEigen (const tf::Matrix3x3 &t, Eigen::Matrix3d &e)

  // Converts an Eigen Affine3d into a tf Transform.
void  tf::poseEigenToTF (const Eigen::Affine3d &e, tf::Pose &t)
  // Converts a tf Pose into an Eigen Affine3d.
void  tf::poseTFToEigen (const tf::Pose &t, Eigen::Affine3d &e)
  
// Converts an Eigen Quaternion into a tf Quaternion.
void  tf::quaternionEigenToTF (const Eigen::Quaterniond &e, tf::Quaternion &t)
  // Converts a tf Quaternion into an Eigen Quaternion.
void  tf::quaternionTFToEigen (const tf::Quaternion &t, Eigen::Quaterniond &e)
  
  // Converts an Eigen Affine3d into a tf Transform.
void  tf::transformEigenToTF (const Eigen::Affine3d &e, tf::Transform &t)
  // Converts a tf Transform into an Eigen Affine3d.
void  tf::transformTFToEigen (const tf::Transform &t, Eigen::Affine3d &e)

  // Converts an Eigen Vector3d into a tf Vector3.
void  tf::vectorEigenToTF (const Eigen::Vector3d &e, tf::Vector3 &t)
   // Converts a tf Vector3 into an Eigen Vector3d.
void  tf::vectorTFToEigen (const tf::Vector3 &t, Eigen::Vector3d &e)