发表于 更新于

使用Rviz用PCD文件来验证外参

  • 发布外参TF
  • 发布第一个PCD文件
  • 发布第二个PCD文件
  • 打开Rviz查看

详细内容如下：

1.发布外参

使用 static_transform_publisher。 格式： x y z yaw pitch roll parent_frame child_frame （单位：米 和 弧度）

Bash

1
2
3
# 假设 cam1 是主坐标系，cam2 是从坐标系
# 这里发布了一个静态变换：cam2 在 cam1 的 x轴方向 0.5米处
rosrun tf2_ros static_transform_publisher 0.5 0 0 0 0 0 camera_link_1 camera_link_2

注意：如果你的外参是四元数，把命令参数换成 x y z qx qy qz qw 即可。

2.发布第一个 PCD

使用 pcl_ros 的 pcd_to_pointcloud 工具。 格式： _frame_id:=<坐标系名称> <pcd文件路径> <发布频率>

Bash

1
2
# 将 cam1.pcd 发布到 /cloud1 话题，绑定坐标系 camera_link_1
rosrun pcl_ros pcd_to_pointcloud cam1.pcd 1 _frame_id:=camera_link_1 cloud_pcd:=/cloud1
  1. 核心执行部分
  • rosrun pcl_ros pcd_to_pointcloud
    • 含义：运行 pcl_ros 功能包下的 pcd_to_pointcloud 可执行节点。
    • 底层逻辑：这个 C++ 节点内部执行了 pcl::io::loadPCDFile 读取文件，然后在一个 while(ros::ok()) 循环中不断 publish 消息。
  1. 位置参数 (Positional Arguments)

这是传给 main(argc, argv) 函数的前两个参数，顺序固定，不能写反：

  • cam1.pcd (参数1: 文件路径)
    • 含义：你要加载的 PCD 文件路径。
    • 注意：这里支持相对路径（相对于你当前终端的 pwd）或绝对路径（如 /home/user/data/cam1.pcd）。如果报 “No such file”，通常就是路径没对。
  • 1 (参数2: 发布间隔 Interval)
    • 含义：发布的时间间隔，单位是秒。
    • 底层逻辑：节点内部会调用 ros::Duration(1.0).sleep()。
    • 数值影响：
      • 设为 1 = 1秒发一次 (1Hz)。
      • 设为 0.1 = 0.1秒发一次 (10Hz)。
      • 设为 0 = 尽可能快地发（不推荐，会跑满单核 CPU）。
      • 在 RViz 调试时的建议：设为 1 或 0.5 足够了，静态数据不需要高频发，浪费资源。
  1. 私有参数 (Private Parameters)
  • _frame_id:=camera_link_1
    • 语法细节：注意前面的 下划线 _。在 ROS 命令行中，_param:=value 是给节点内部的私有参数（Private NodeHandle ~）赋值的标准写法。
    • 作用：它修改了输出消息 sensor_msgs::PointCloud2 中 header.frame_id 的值。
    • 为什么必须加这个？
      • 默认情况下，这个节点可能会把 frame_id 设为 /base_link 或者空的。
      • TF 树（TF Tree）是靠 frame_id 连接的。如果你不指定这个名字，RViz 收到点云后，发现它的 frame_id 是空的或者不存在于 TF 树中，就会报错 Fixed Frame [map] does not exist 或点云显示不出来（白色/透明）。
      • 这行代码赋予了点云“空间身份”，告诉 ROS：“这一坨点云数据，是属于 camera_link_1 这个坐标系的。”
  1. 话题重映射 (Topic Remapping)
  • cloud_pcd:=/cloud1
    • 语法细节：没有下划线，直接使用 旧名:=新名。
    • 作用：将节点原本默认发布的话题名 cloud_pcd 改名为 /cloud1。

3. 发布第二个 PCD

1
2
# 将 cam2.pcd 发布到 /cloud2 话题，绑定坐标系 camera_link_2
rosrun pcl_ros pcd_to_pointcloud cam2.pcd 1 _frame_id:=camera_link_2 cloud_pcd:=/cloud2

4. 打开 RViz 查看

1
rviz

RViz 设置步骤：

  1. Global Options -> Fixed Frame: 设置为 camera_link_1。
  2. Add -> PointCloud2: Topic 选择 /cloud1 (建议设为红色)。
  3. Add -> PointCloud2: Topic 选择 /cloud2 (建议设为绿色)。
  4. Add -> TF: 检查两个坐标系的关系。

具体c++代码

  1. 假设的 TXT 文件格式

假设你的 calib1.txt 和 calib2.txt 内容是标准的 4x4 变换矩阵（行优先，空格或换行分隔），例如：

Plaintext

1
2
3
4
0.99 0.01 0.00 0.15
-0.01 0.99 0.00 -0.05
0.00 0.00 1.00 0.80
0.00 0.00 0.00 1.00

(代码会自动解析这16个数，提取旋转和平移)

  1. 更新后的 C++ 代码 (multi_pcd_viz.cpp)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Vector3.h>
#include <fstream>
#include <vector>

// 定义点云类型 (如果有点颜色用 PointXYZRGB)
typedef pcl::PointXYZ PointT; 

// === 辅助函数：从TXT读取4x4矩阵并转换为Transform ===
bool loadTransformFromTxt(const std::string& filepath, 
                          geometry_msgs::Transform& transform_msg) 
{
    std::ifstream file(filepath);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open file: %s", filepath.c_str());
        return false;
    }

    // 读取16个浮点数 (Row-Major: r11 r12 r13 tx ... )
    double m[16];
    int count = 0;
    while (count < 16 && file >> m[count]) {
        count++;
    }

    if (count != 16) {
        ROS_ERROR("Invalid matrix format in %s. Expected 16 numbers.", filepath.c_str());
        return false;
    }

    // 1. 提取平移 (第4列: m[3], m[7], m[11])
    transform_msg.translation.x = m[3];
    transform_msg.translation.y = m[7];
    transform_msg.translation.z = m[11];

    // 2. 提取旋转 (左上角3x3) 并转为四元数
    tf2::Matrix3x3 rot_matrix(
        m[0], m[1], m[2],
        m[4], m[5], m[6],
        m[8], m[9], m[10]
    );
    
    tf2::Quaternion q;
    rot_matrix.getRotation(q); // 从旋转矩阵提取四元数

    transform_msg.rotation.x = q.x();
    transform_msg.rotation.y = q.y();
    transform_msg.rotation.z = q.z();
    transform_msg.rotation.w = q.w();

    return true;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "multi_camera_viewer");
    ros::NodeHandle nh("~"); // 使用私有句柄允许传参

    // === 1. 参数配置 (建议改成你的实际路径) ===
    std::string pcd1_path = "/home/user/data/cam1.pcd";
    std::string pcd2_path = "/home/user/data/cam2.pcd";
    std::string txt1_path = "/home/user/data/T_base_cam1.txt"; // T_base_to_cam1
    std::string txt2_path = "/home/user/data/T_base_cam2.txt"; // T_base_to_cam2
    
    // 定义坐标系名称
    std::string base_frame = "base_link";     // 机器人基座
    std::string cam1_frame = "camera1_link";
    std::string cam2_frame = "camera2_link";

    // === 2. 读取外参 TXT 并构建 TF 消息 ===
    std::vector<geometry_msgs::TransformStamped> tf_list;
    geometry_msgs::Transform t1, t2;

    if (!loadTransformFromTxt(txt1_path, t1)) return -1;
    if (!loadTransformFromTxt(txt2_path, t2)) return -1;

    // 构建 TF 1: Base -> Cam1
    geometry_msgs::TransformStamped ts1;
    ts1.header.frame_id = base_frame;
    ts1.child_frame_id = cam1_frame;
    ts1.transform = t1;
    tf_list.push_back(ts1);

    // 构建 TF 2: Base -> Cam2
    geometry_msgs::TransformStamped ts2;
    ts2.header.frame_id = base_frame;
    ts2.child_frame_id = cam2_frame;
    ts2.transform = t2;
    tf_list.push_back(ts2);

    // 广播静态 TF
    static tf2_ros::StaticTransformBroadcaster static_broadcaster;
    // 注意：sendTransform 可以接受 vector 一次性发多个
    // 加上时间戳
    ros::Time now = ros::Time::now();
    for(auto& tf : tf_list) tf.header.stamp = now;
    static_broadcaster.sendTransform(tf_list);

    ROS_INFO("TF Tree Published: %s -> {%s, %s}", base_frame.c_str(), cam1_frame.c_str(), cam2_frame.c_str());

    // === 3. 读取并发布 PCD ===
    ros::Publisher pub1 = nh.advertise<sensor_msgs::PointCloud2>("/cloud_cam1", 1, true);
    ros::Publisher pub2 = nh.advertise<sensor_msgs::PointCloud2>("/cloud_cam2", 1, true);

    pcl::PointCloud<PointT>::Ptr cloud1(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud2(new pcl::PointCloud<PointT>);

    if (pcl::io::loadPCDFile(pcd1_path, *cloud1) == -1 || 
        pcl::io::loadPCDFile(pcd2_path, *cloud2) == -1) {
        ROS_ERROR("Failed to load PCD files.");
        return -1;
    }

    sensor_msgs::PointCloud2 msg1, msg2;
    pcl::toROSMsg(*cloud1, msg1);
    pcl::toROSMsg(*cloud2, msg2);

    // 绑定坐标系
    msg1.header.frame_id = cam1_frame;
    msg2.header.frame_id = cam2_frame;

    ros::Rate loop(1);
    while (ros::ok()) {
        now = ros::Time::now();
        
        // 更新时间戳
        msg1.header.stamp = now;
        msg2.header.stamp = now;
        
        // 为了确保 RViz 中 TF 不会过期（Static其实只要发一次，但为了保险可以在循环里更新stamp）
        for(auto& tf : tf_list) tf.header.stamp = now;
        static_broadcaster.sendTransform(tf_list);

        pub1.publish(msg1);
        pub2.publish(msg2);

        ros::spinOnce();
        loop.sleep();
    }

    return 0;
}
  1. 使用说明

步骤一：配置 CMakeLists.txt

确保包含了 roscpp, pcl_ros, tf2, tf2_geometry_msgs 等。

CMake

1
2
add_executable(multi_pcd_viz src/multi_pcd_viz.cpp)
target_link_libraries(multi_pcd_viz ${catkin_LIBRARIES} ${PCL_LIBRARIES})

步骤二：准备文件

确保你有 4 个文件，并且路径在代码里改对了（或者稍后我教你用 roslaunch 传参）：

  1. cam1.pcd, cam2.pcd
  2. T1.txt, T2.txt (格式必须是 16 个数字)

步骤三：RViz 设置 (关键)

编译运行后，打开 RViz：

  1. Fixed Frame: 设置为 base_link (因为这是根节点)。
  2. Add TF: 你会看到一个三叉结构（Base 指向 Cam1 和 Cam2）。
  3. Add PointCloud2 (x2):
    • Topic: /cloud_cam1 (颜色设红)
    • Topic: /cloud_cam2 (颜色设绿)