SLAM中常见包含坐标变换的函数

一、TF

        1、tf_broadcaster----sendTransform()函数---T(父_子)

#include <ros/ros.h> 
#include <tf/transform_broadcaster.h> 
#include <Eigen/Geometry>

int main(int argc, char** argv) 
{ 
    ros::init(argc, argv, "tf_broadcaster"); 
    ros::NodeHandle n; 
    ros::Rate loop_rate(100); 
    tf::TransformBroadcaster broadcaster; 
    tf::Transform father2child; 
    father2child.setOrigin(tf::Vector3(0, 0, -1)); 
    // 任意的旋转都可以由两个互为相反数的四元数表示。
    father2child.setRotation(tf::Quaternion(-0.5, -0.5, 0.5, 0.5));  // [x, y, z, w]
    // father2child.setRotation(tf::Quaternion(0.5, 0.5, -0.5, -0.5)); // [x, y, z, w]
    
    Eigen::Quaterniond q;
    q.x() = -0.5;
    q.y() = -0.5;
    q.z() = 0.5;
    q.w() = 0.5;

    Eigen::Matrix3d R = q.toRotationMatrix();
    std::cout << "Rotation Matrix R:" << std::endl << R << std::endl;

    while(n.ok()) 
    { 
        // 发布 坐标变换信息 到话题 /tf
        // 注意坐标系变换 和 坐标变换
        // 函数发布一个从 父坐标系 到 子坐标系 的变换关系。以父坐标系为基准 即 T(父_子)
        // 从 father到 child的坐标系变换（frame transform）等同于把一个点从 child坐标系向 father坐标系的坐标变换，等于 child坐标系在 father坐标系的姿态描述
        broadcaster.sendTransform(tf::StampedTransform(father2child, ros::Time::now(), "father_frame_id_", "child_frame_id_")); //广播的是 T(父_子)
        // broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 0), tf::Vector3(0, 0, 0)), ros::Time::now(),"father_frame_id_","child_frame_id_")); 
        loop_rate.sleep(); 
    } 
    return 0; 
}

运行结果：

        2、tf_listener----waitForTransform()函数

#include <ros/ros.h> 
#include <tf/transform_listener.h> 
#include <geometry_msgs/PointStamped.h> 
#include <iostream> 

int main(int argc,char** argv) 
{ 
    ros::init(argc,argv,"tf_listener_test"); 
    ros::NodeHandle n; 
     
    ros::Rate loop_rate(100); 
    tf::TransformListener listener; 
    geometry_msgs::PointStamped father_pos; 
    father_pos.header.frame_id = "father_frame_id_";
    father_pos.header.stamp = ros::Time(); 
    father_pos.point.x =  1; 
    father_pos.point.y =  1; 
    father_pos.point.z =  1;

    geometry_msgs::PointStamped child_pos; 
    

    while(n.ok()) 
    {   
        try{
            // 测试结果：均可以正常运行，说明 tf 内部做好了转换
            // 方式①
            // if (listener.waitForTransform("child_frame_id_", "father_frame_id_", ros::Time(0), ros::Duration(3))) // ros::Duration(3) 表示一个持续时间为3秒的时间间隔
            // { 
            //     listener.transformPoint("child_frame_id_", father_pos, child_pos);  // 将激光点的位置转换到child坐标系
            //     ROS_INFO("pointpos in child:  (%.2f, %.2f. %.2f) ", child_pos.point.x, child_pos.point.y, child_pos.point.z); 
            //     ROS_INFO("pointpos in father: (%.2f, %.2f, %.2f) ", father_pos.point.x, father_pos.point.y, father_pos.point.z); 
            // } 
            // 方式②
            if (listener.waitForTransform("father_frame_id_", "child_frame_id_", ros::Time(0), ros::Duration(3)))
            { 
                listener.transformPoint("child_frame_id_", father_pos, child_pos);  
                ROS_INFO("pointpos in child:  (%.2f, %.2f. %.2f) ", child_pos.point.x, child_pos.point.y, child_pos.point.z); 
                ROS_INFO("pointpos in father: (%.2f, %.2f, %.2f) ", father_pos.point.x, father_pos.point.y, father_pos.point.z); 
            } 

            loop_rate.sleep(); 
        }
        catch(const std::exception &ex){
            ROS_WARN("catch tf: %s", ex.what());
        }
    }   
}

运行结果：方式①和方式②运行结果相同

[ INFO] [1723259218.944746957]: pointpos in child:  (1.00, -2.00. -1.00)
[ INFO] [1723259218.954646563]: pointpos in father: (1.00, 1.00, 1.00) 

        3、tf_listener----lookupTransform()函数

#include <ros/ros.h> 
#include <tf/transform_listener.h> 
#include <geometry_msgs/PointStamped.h> 
#include <iostream> 
#include <Eigen/Geometry>

void my_StampedTransform_2_Matrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {

    Eigen::Vector3f tl(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
    
    Eigen::Quaternionf quaternion;
    quaternion.x() = transform.getRotation().getX();
    quaternion.y() = transform.getRotation().getY();
    quaternion.z() = transform.getRotation().getZ();
    quaternion.w() = transform.getRotation().getW();

    Eigen::Matrix3f rotation_matrix = quaternion.toRotationMatrix();
    transform_matrix = Eigen::Matrix4f::Identity();
    transform_matrix.block<3,3>(0,0) = rotation_matrix;
    transform_matrix.block<3,1>(0,3) = tl;

    std::cout << "my-旋转矩阵R: " << std::endl;
    std::cout << transform_matrix<< std::endl;
}



int main(int argc,char** argv) 
{ 
    ros::init(argc,argv,"tf_listener_test"); 
    ros::NodeHandle n; 
     
    ros::Rate loop_rate(100); 
    tf::TransformListener listener; 

    Eigen::Vector4f father_point(1, 1, 1, 1);

    while(n.ok()) 
    {   
        if (listener.waitForTransform("father_frame_id_","child_frame_id_",ros::Time(0),ros::Duration(3))) // ros::Duration(3) 表示一个持续时间为3秒的时间间隔
        { 

            // tf::StampedTransform 是 ROS 中用于表示带有时间戳的坐标变换的数据类型。它包含了坐标变换的旋转和平移信息，以及与之相关的时间戳信息。
            tf::StampedTransform myTransform; 

            // // 方式①
            // // 获取 父坐标系 到 子坐标系 的变换
            // // 或者说 子坐标系 在 父坐标系下的坐标值
            // listener.lookupTransform("father_frame_id_", "child_frame_id_", ros::Time(0), myTransform); // myTransform 即 T(父_子)

            // // 输出变换的平移部分
            // ROS_INFO("Translation: (%f, %f, %f)", myTransform.getOrigin().getX(), myTransform.getOrigin().getY(), myTransform.getOrigin().getZ());
            // // 输出变换的旋转部分 [x, y, z, w]   Quaternion: 0.5 0.5 -0.5 -0.5
            // ROS_INFO("Rotation [x, y, z, w]: (%f, %f, %f, %f)", myTransform.getRotation().getX(), myTransform.getRotation().getY(), myTransform.getRotation().getZ(), myTransform.getRotation().getW());

            // Eigen::Matrix4f T1;
            // my_StampedTransform_2_Matrix(myTransform, T1);

            // Eigen::Vector4f child_point = T1.inverse() * father_point;
            // ROS_INFO("child_point: (%f, %f, %f, %f) ", child_point.x(), child_point.y(), child_point.z(), child_point.w()); 

            // 方式②
            listener.lookupTransform("child_frame_id_", "father_frame_id_", ros::Time(0), myTransform); // myTransform 即 T(子_父)

            // 输出变换的平移部分
            ROS_INFO("Translation: (%f, %f, %f)", myTransform.getOrigin().getX(), myTransform.getOrigin().getY(), myTransform.getOrigin().getZ());
            // 输出变换的旋转部分 [x, y, z, w]   Quaternion: 0.5 0.5 -0.5 0.5
            ROS_INFO("Rotation [x, y, z, w]: (%f, %f, %f, %f)", myTransform.getRotation().getX(), myTransform.getRotation().getY(), myTransform.getRotation().getZ(), myTransform.getRotation().getW());

            Eigen::Matrix4f T1;
            my_StampedTransform_2_Matrix(myTransform, T1);

            Eigen::Vector4f child_point = T1 * father_point;
            ROS_INFO("child_point: (%f, %f, %f, %f) ", child_point.x(), child_point.y(), child_point.z(), child_point.w()); 

        } 
        loop_rate.sleep(); 
    } 
}

运行结果：方式①和方式②运行结果不同

方式①运行结果: T_father_child

[ INFO] [1723262555.188721557]: Translation: (0.000000, 0.000000, -1.000000)
[ INFO] [1723262555.188734684]: Rotation [x, y, z, w]: (0.500000, 0.500000, -0.500000, -0.500000)
my-旋转矩阵R: 
 0  0 -1  0
 1  0  0  0
 0 -1  0 -1
 0  0  0  1
[ INFO] [1723262555.188756822]: child_point: (1.000000, -2.000000, -1.000000, 1.000000) 

方式②运行结果: T_child_father

[ INFO] [1723266471.083481570]: Translation: (0.000000, -1.000000, 0.000000)
[ INFO] [1723266471.083510776]: Rotation [x, y, z, w]: (0.500000, 0.500000, -0.500000, 0.500000)
my-旋转矩阵R: 
 0  1  0  0
 0  0 -1 -1
-1  0  0  0
 0  0  0  1
[ INFO] [1723266471.083533838]: child_point: (1.000000, -2.000000, -1.000000, 1.000000) 

lookupTransform()函数介绍：该函数获取的 transform 是 T_target_source

  /** \brief Get the transform between two frames by frame ID.
   * \param target_frame The frame to which data should be transformed
   * \param source_frame The frame where the data originated
   * \param time The time at which the value of the transform is desired. (0 will get the latest)
   * \param transform The transform reference to fill.  
   *
   * Possible exceptions tf::LookupException, tf::ConnectivityException,
   * tf::MaxDepthException, tf::ExtrapolationException
   */
  void lookupTransform(const std::string& target_frame, const std::string& source_frame,
                       const ros::Time& time, StampedTransform& transform) const;

二、PCL

1.pcl::transformPointCloud()函数

        其中 transform 为 T_out_in，表示将 in坐标系 下的点云转移到 out坐标系 下。

  template <typename PointT> void 
  transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                       pcl::PointCloud<PointT> &cloud_out, 
                       const Eigen::Matrix4f &transform,
                       bool copy_all_fields = true)

2.

3.