订阅TF消息，并转化为齐次变换矩阵

功能介绍

tf是很常用的消息类型，但有时候我们更希望直到他的四乘四的齐次变换矩阵。

源代码

这个代码是辅助velo2cam_calibration用的，实现两个tf的接收，得到相机到雷达的坐标系转换关系。
大家完全可以根据自己需求修改接收的tf，并进行相应处理。
用到的主要函数就是
listener.lookupTransform()

#include <ros/ros.h>
#include <Eigen/Dense>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node;
  

  tf::TransformListener listener;  

 
  ros::Rate rate(10.0);
  while (node.ok()){
    //创建一个StampedTransform对象存储变换结果数据
    tf::StampedTransform transform_lidar;
    tf::StampedTransform transform_cam;
    
    //监听包装在一个try-catch块中以捕获可能的异常
    try{

      listener.lookupTransform("/rslidar", "/rotated_camera",
                               ros::Time(0), transform_lidar);
      listener.lookupTransform("/rotated_camera", "/camera",
                               ros::Time(0), transform_cam);
      Eigen::Matrix4f lidar2cam;
      Eigen::Matrix4f cam2cam;
      pcl_ros::transformAsMatrix(transform_lidar, lidar2cam);   //直接得到矩阵
      pcl_ros::transformAsMatrix(transform_cam, cam2cam);   //直接得到矩阵
      std::cout << "lidar2cam:"  <<std::endl;
      std::cout << lidar2cam  <<std::endl;
      std::cout << "cam2cam:"  <<std::endl;
      std::cout << cam2cam  <<std::endl;
      std::cout << "final:"  <<std::endl;
      std::cout << lidar2cam*cam2cam  <<std::endl;
      std::cout << "final2:"  <<std::endl;
      // std::cout << (lidar2cam*cam2cam).inverse()  <<std::endl;
      std::cout << (cam2cam*lidar2cam).inverse()  <<std::endl;

    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    rate.sleep();
  }
  return 0;
};

CMakeLists.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(Rosbag_w)

set(PACKAGE_DEPENDENCIES
  roscpp
  sensor_msgs
  std_srvs
  std_msgs
  eigen_conversions
)


find_package(PCL 1.3 REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(${catkin_INCLUDE_DIRS}  )
add_executable(pcl_tf pcl_tf.cpp)
target_link_libraries(pcl_tf ${catkin_LIBRARIES})

按照一般的CMake编译过程编译即可。