基于rosbag制作三维点云数据集——以KITTI数据集为例（一）

1.利用Tools_RosBag2KITTI软件包从ubuntu当中录制的rosbag文件提取出点云文件和图像文件

1.1 克隆Tools_RosBag2KITTI软件包，官方地址为：https://github.com/leofansq/Tools_RosBag2KITTI.git，

​git clone https://github.com/leofansq/Tools_RosBag2KITTI.git

1.2进入catkin_ws文件夹中

cd catkin_ws

1.3编译

catkin_make

1.4查看rosbag中对应的雷达点云话题以及图像话题，修改Tools_RosBag2KITTI/catkin_ws/src/obstacle_detection/src/map_generation_node.cpp文件中对应的订阅话题（map_generation_node.cpp文件中的27-28对应雷达点云话题，30-31对应图像话题），修改输出的点云和图像保存路径（对应/Tools_RosBag2KITTI/catkin_ws/output/pcd以及/Tools_RosBag2KITTI/catkin_ws/output/png，修改位置位于map_generation_node.cpp文件中的52和112行）修改后的map_generation_node.cpp如下

#include "map_generation_node.h"
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Geometry>
 
using namespace cv;
 
double trans_roll_  = 0.0;
double trans_pitch_ = 0.0;
double trans_yaw_   = 0.0;
double trans_tx_    = 0.0;
double trans_ty_    = 0.0;
double trans_tz_    = 0.0;
Eigen::Affine3f transform_matrix_ = Eigen::Affine3f::Identity();
 
MapGenerationNode::MapGenerationNode()
    : lidar_index(0), camera_captured(false), it(nh), init_camera_time(false), init_lidar_time(false)
{
    ROS_INFO("Initializing MapGenerationNode...");
 
    sub_lidar = nh.subscribe("/livox/lidar", 1000, &MapGenerationNode::lidarCallback, this);
    ROS_INFO("Subscribed to /livox/lidar");
 
    sub_camera = it.subscribe("/bow_cam/image_raw", 1000, &MapGenerationNode::cameraCallback, this);
    ROS_INFO("Subscribed to /bow_cam/image_raw");
 
    // Ensure both subscriptions are active
    ROS_INFO("MapGenerationNode initialized.");
}
 
void MapGenerationNode::lidarCallback(const sensor_msgs::PointCloud2::ConstPtr& lidar)
{
    ROS_INFO("lidarCallback triggered");
 
    if (!init_lidar_time)
    {
        lidar_base_time = lidar->header.stamp.sec * 1e3 + lidar->header.stamp.nsec / 1e6;
        init_lidar_time = true;
        ROS_INFO("Initialized lidar base time: %lld ms", lidar_base_time);
    }
 
    long long lidar_delta_time = lidar->header.stamp.sec * 1e3 + lidar->header.stamp.nsec / 1e6 - lidar_base_time;
    ROS_INFO("Received lidar data: %lld ms", lidar_delta_time);
 
    char s[200];
    snprintf(s, sizeof(s), "/home/jetson/Tools_RosBag2KITTI/catkin_ws/output/pcd/%06lld.pcd", lidar_index); 
    ++lidar_index;
    camera_captured = false;
 
    pcl::PointCloud<pcl::PointXYZI> lidar_cloud;
    pcl::fromROSMsg(*lidar, lidar_cloud);
 
    // 打印原始点云数据的大小
    ROS_INFO("Original PointCloud size: %zu", lidar_cloud.size());
 
    // 重置变换矩阵
    transform_matrix_ = Eigen::Affine3f::Identity();
    transform_matrix_.translation() << trans_tx_, trans_ty_, trans_tz_;
    transform_matrix_.rotate(Eigen::AngleAxisf(trans_yaw_ * M_PI / 180, Eigen::Vector3f::UnitZ()));
    transform_matrix_.rotate(Eigen::AngleAxisf(trans_pitch_ * M_PI / 180, Eigen::Vector3f::UnitY()));
    transform_matrix_.rotate(Eigen::AngleAxisf(trans_roll_ * M_PI / 180, Eigen::Vector3f::UnitX()));
 
    // 检查转换矩阵
    ROS_INFO_STREAM("Transform matrix: \n" << transform_matrix_.matrix());
 
    pcl::PointCloud<pcl::PointXYZI>::Ptr trans_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::transformPointCloud(lidar_cloud, *trans_cloud_ptr, transform_matrix_);
 
    // 打印变换后点云数据的大小
    ROS_INFO("Transformed PointCloud size: %zu", trans_cloud_ptr->size());
 
    // 验证点云数据的完整性（可选）
    for (const auto& point : trans_cloud_ptr->points)
    {
        if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z))
        {
            ROS_WARN("NaN point detected after transformation.");
        }
    }
 
    pcl::PCDWriter writer;
    writer.writeBinary(s, *trans_cloud_ptr);
    ROS_INFO("Saved PCD file: %s", s);
}
 
void MapGenerationNode::cameraCallback(const sensor_msgs::ImageConstPtr& camera)
{
    ROS_INFO("cameraCallback triggered");
 
    if (!init_camera_time)
    {
        camera_base_time = camera->header.stamp.sec * 1e3 + camera->header.stamp.nsec / 1e6;
        init_camera_time = true;
        ROS_INFO("Initialized camera base time: %lld ms", camera_base_time);
    }
 
    long long camera_delta_time = camera->header.stamp.sec * 1e3 + camera->header.stamp.nsec / 1e6 - camera_base_time;
    if (camera_captured)
    {
        ROS_INFO("Camera already captured for this lidar frame.");
        return;
    }
 
    ROS_INFO("Received camera data: %lld ms", camera_delta_time);
    char s[200];
    snprintf(s, sizeof(s), "/home/jetson/Tools_RosBag2KITTI/catkin_ws/output/png/%06lld.png", lidar_index - 1); 
 
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(camera, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
 
    if (!cv_ptr || cv_ptr->image.empty())
    {
        ROS_ERROR("Failed to convert image message to cv::Mat.");
        return;
    }
 
    try
    {
        bool result = imwrite(s, cv_ptr->image);
        if (result)
        {
            ROS_INFO("Image saved successfully: %s", s);
        }
        else
        {
            ROS_ERROR("Failed to save image: %s", s);
        }
    }
    catch (const cv::Exception& e)
    {
        ROS_ERROR("OpenCV exception: %s", e.what());
    }
 
    camera_captured = true;
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_generation");
    MapGenerationNode mapgeneration;
    ROS_INFO("MapGenerationNode started");
    ros::spin();
    return 0;
}

1.5在catkin_ws文件夹下重新编译

catkin_make

1.6运行roscore，没有的话可以自行安装，推荐使用鱼香一键ROS安装，跟着提示即可安装，非常简单，否则在下一步运行过程中会报错，如下图

wget http://fishros.com/install -O fishros && . Fishros

1.7在catkin_ws文件夹的终端下运行以下命令从bag文件中读取pcd和png文件，正常运行文件如下，可以看到订阅的话题对应我们自己在1.4中修改后的话题

./devel/lib/obstacle_detection/map_generate

1.8运行我们需要读取的rosbag包（rosbag文件位置随意），这里我们以0.1倍速去运行rosbag，如果速度过大，节点可能跳过部分数据，或者因为频率过高而丢失重要信息。

rosbag play -r 0.1 input.bag

™

可以看到我们1.7步的终端中会跟着保存我们对应的点云以及图像数据，如下图

保存的点云以及图像数据地址如下：

/Tools_RosBag2KITTI/catkin_ws/output

2.pcd点云文件转KITTI数据集要求的bin文件

2.1 pcd文件转bin文件

路径转至/Tools_RosBag2KITTI/pcd2bin下

将1.8步/Tools_RosBag2KITTI/catkin_ws/output/pcd中保存的点云文件复制到当前文件夹的pcd文件夹中

在/Tools_RosBag2KITTI/pcd2bin路径下创建一个项目进行编译

mkdir CMakeFile  #创建文件夹

cd CMakeFile  #进入文件夹

cmake ..  #编译

make     #编译

2.2修改/Tools_RosBag2KITTI/pcd2bin/ pcd2bin.cpp文件中的路径（修改85和86行为输出的bin文件夹和输入的pcd文件夹地址），pcd2bin.cpp文件如下：

#include <iostream>           
#include <pcl/io/pcd_io.h>      
#include <pcl/point_types.h>     
#include <dirent.h>            
#include <vector>              
#include <algorithm>           
#include <fstream>             
 
using namespace std;
 
// 将 PCD 文件转换为 BIN 文件
void pcd2bin(const string &in_file, const string &out_file)
{ 
    // 创建一个 PointCloud 对象
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
  
    // 尝试加载 PCD 文件
    if (pcl::io::loadPCDFile<pcl::PointXYZI>(in_file, *cloud) == -1) 
    {
        PCL_ERROR("Couldn't read file: %s\n", in_file.c_str());
        return;  // 如果加载失败，退出函数
    }
    
    // 尝试创建并打开 .bin 文件
    ofstream bin_file(out_file.c_str(), ios::out | ios::binary);
    if (!bin_file.good()) 
    {
        cout << "Couldn't open output file: " << out_file << endl;
        return;  // 如果打开失败，退出函数
    }  
 
    // 将 PCD 转换为 BIN
    cout << "Converting " << in_file << " to " << out_file << endl;
    for (const auto& point : cloud->points)
    {
        // 旋转点云坐标系
        float new_x = point.y;
        float new_y = -point.x;
        float z = point.z;
        float intensity = point.intensity;
        
        // 写入旋转后的坐标和强度值
        bin_file.write(reinterpret_cast<char*>(&new_x), sizeof(float));
        bin_file.write(reinterpret_cast<char*>(&new_y), sizeof(float));
        bin_file.write(reinterpret_cast<char*>(&z), sizeof(float));
        bin_file.write(reinterpret_cast<char*>(&intensity), sizeof(float));
    }
    
    bin_file.close();
}
 
// 读取指定类型的文件列表
void read_filelists(const string &dir_path, vector<string> &out_filelists, const string &type)
{
    struct dirent *ptr;
    DIR *dir = opendir(dir_path.c_str());
    if (!dir) {
        cerr << "Error opening directory: " << dir_path << endl;
        return;
    }
 
    out_filelists.clear();
    while ((ptr = readdir(dir)) != NULL)
    {
        string tmp_file = ptr->d_name;
        if (tmp_file[0] == '.') continue;
 
        if (tmp_file.size() >= type.size() && tmp_file.substr(tmp_file.size() - type.size()) == type)
        {
            out_filelists.push_back(ptr->d_name);
        }
    }
    closedir(dir);
}
 
// 排序文件列表
void sort_filelists(vector<string> &filelists)
{
    sort(filelists.begin(), filelists.end());
}
 
int main(int argc, char **argv)
{
    // 设置文件路径
    string bin_path = "/home/jetson/Tools_RosBag2KITTI/pcd2bin/bin/";
    string pcd_path = "/home/jetson/Tools_RosBag2KITTI/pcd2bin/pcd/";
 
    // 声明并读取文件列表
    vector<string> file_lists;
    read_filelists(pcd_path, file_lists, "pcd");
    sort_filelists(file_lists);
    
    // 一个一个地转换 PCD 到 BIN
    for (const auto& file : file_lists)
    {
        string pcd_file = pcd_path + file;
        string bin_file = bin_path + file.substr(0, file.length() - 4) + ".bin";
        pcd2bin(pcd_file, bin_file);
    }
 
    return 0;
}

之后在CMakeFile文件夹路径下运行以下命令进行相应的bin文件的生成，下图为生成的bin文件夹

./pcd2bin