4

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

 2 years ago
source link: https://charon-cheung.github.io/2022/12/13/ROS/ROS%E7%A8%8B%E5%BA%8F%E5%92%8C%E6%BA%90%E7%A0%81%E5%88%86%E6%9E%90/tf%E7%9A%84lookTransform%E5%92%8C%E9%BD%90%E6%AC%A1%E5%8F%98%E6%8D%A2%E7%9F%A9%E9%98%B5%E7%9A%84%E5%85%B3%E7%B3%BB%E6%8E%A8%E5%AF%BC/#tf-eigen
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
neoserver,ios ssh client

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

tf的lookTransform和齐次变换矩阵的关系推导
2022-12-13|
Word count: 775|Reading time: 4 min

建立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)

report

Recommend

About Joyk


Aggregate valuable and interesting links.
Joyk means Joy of geeK