tf的lookTransform和齐次变换矩阵的关系推导

tf的lookTransform和齐次变换矩阵的关系推导 | 沉默杀手

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

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页的表示方法，写成 TABTAB
，也就是父子顺序。 因此 C—->A变换是 TAC=TAB∗TBCTAC=TAB∗TBC

CMake关键部分：

find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  roscpp
  tf
  pcl_ros
)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)

c++关键部分

#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之间的矩阵和向量互相转换。使用前

find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  tf_conversions
)

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

// 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)