功能介绍
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编译过程编译即可。