前言

KITTI数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，是目前国际上最大的自动驾驶场景下的计算机视觉算法评测数据集。该数据集用于评测立体图像(stereo)，光流(optical flow)，视觉测距(visual odometry)，3D物体检测(object detection)和3D跟踪(tracking)等计算机视觉技术在车载环境下的性能。KITTI包含市区、乡村和高速公路等场景采集的真实图像数据，每张图像中最多达15辆车和30个行人，还有各种程度的遮挡与截断。整个数据集由389对立体图像和光流图，39.2 km视觉测距序列以及超过200k 3D标注物体的图像组成，以10Hz的频率采样及同步。总体上看，原始数据集被分类为’Road’, ’City’, ’Residential’, ’Campus’ 和 ’Person’。对于3D物体检测，label细分为car, van, truck, pedestrian, pedestrian(sitting), cyclist, tram以及misc组成。

通过该数据集可以验证自己的算法

但是获取数据后,需要将数据进行一个转换,转成ROS 的 Topic 或者 rosbag,否则没法使用.
例如图片是png格式的,lidar的数据是bin文件形式的.

本篇详解 如何通过 代码 来实现 kitti数据集的处理

KITTI数据集文件结构

kitti 数据集的文件结构,注意这个和程序里面的路径设置有关,不一致则读不到数据

—kitti_data
------sequence
---------00
------------image_0
------------------000000.png
------------------000001.png
------------------000002.png
------------image_1
------------------000000.png
------------------000001.png
------------------000002.png
------------calib.txt
------------times.txt
------velodye
---------sequence
------------00
---------------velodye
------------------000000.bin
------------------000001.bin
------------------000002.bin

Code

下面的代码实现了读取kitti数据集的 图像\雷达\时间戳 并发布topic和保存bag

//

#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>   //处理图像用的
#include <image_transport/image_transport.h> //opencv图像和ros图像的转换
#include <opencv2/highgui/highgui.hpp>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <geometry_msgs/PoseStamped.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <eigen3/Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
 

要包含的头文件 主要是 字节流的读入 消息 的处理等

//

int main(int argc, char** argv)
{   
    //初始化节点
    ros::init(argc, argv, "kitti_helper");
    // ~ 发布的topic  会 以  节点名字+话题名字  发布  
    ros::NodeHandle n("~");

  

 

  
1
  
2
  
3
  
4
  
5
  
6
 

初始化节点

//

    // 声明字符串  相关的参数设置 在launch中设置的  数据的路径  数据的sequence 输出的bag路径
    std::string dataset_folder, sequence_number, output_bag_file;
    //从参数服务器获取 数据路径
    n.getParam("dataset_folder", dataset_folder);
    //从参数服务器获取 数据的sequence
    n.getParam("sequence_number", sequence_number);
    //打印 kitti数据 的信息
    std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
    
    // 声明 变量 是否需要 生成bag
    bool to_bag;
    //从参数服务器获取 是否需要 生成bag
    n.getParam("to_bag", to_bag);
    //如果生成bag
    if (to_bag)
        n.getParam("output_bag_file", output_bag_file);//从参数服务器获取 
    //声明 变量 发布延时
    int publish_delay;
    //从参数服务器获取 发布延时
    n.getParam("publish_delay", publish_delay);
    //如果小于0 则 设置为1
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;
 

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
 

从参数服务器中 读取 对应的 信息 注意 参数 服务器 的设置 在 launch 文件中

<launch>
    <node name="kittiHelper" pkg="aloam_velodyne" type="kittiHelper" output="screen"> <!-- 要启动的节点 -->
        <param name="dataset_folder" type="string" value="/data/KITTI/odometry/" /> <!-- 数据的路径 -->
        <param name="sequence_number" type="string" value="00" /> <!-- 数据的sequence -->
        <param name="to_bag" type="bool" value="false" /> <!-- 是否保存bag文件 -->
        <param name="output_bag_file" type="string" value="/tmp/kitti.bag" /> <!-- bag的输出路径 -->
        <param name="publish_delay" type="int" value="1" /><!-- topic发布的延时 -->
    </node>
</launch>

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
 

程序从参数服务器中读取的参数都是从 launch文件中设置的 需要设置的已注释

//

    //lidar 的点云 发布句柄
    ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 2);

    //图像 的 发布句柄
    image_transport::ImageTransport it(n);
    image_transport::Publisher pub_image_left = it.advertise("/image_left", 2);
    image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);

    //里程计的发布句柄
    ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);

    //声明一个 里程计
    nav_msgs::Odometry odomGT;
    odomGT.header.frame_id = "/camera_init";//赋值里程计的坐标系 父
    odomGT.child_frame_id = "/ground_truth";//赋值里程计的坐标系 子

    //声明路径的 消息 发布 句柄
    ros::Publisher pubPathGT = n.advertise<nav_msgs::Path> ("/path_gt", 5);
    nav_msgs::Path pathGT;//声明一个路径变量
    pathGT.header.frame_id = "/camera_init";//赋值坐标系

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
 

声明设置 要通过 ros 发布的topic 句柄
有:点云的\图像的\里程计的\路径的

//

    //读取 时间戳的文件
    std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
    std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);

  

 

  
1
  
2
  
3
 

读取时间戳的文件
一个时间戳对应一组数据 ,后面的 点云\图像的消息的时间戳则赋值现在设置的 时间戳文件路径

//

    //读取 路径的 文件
    std::string ground_truth_path = "results/" + sequence_number + ".txt";
    std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);

  

 

  
1
  
2
  
3
 

设置 path的文件 的 路径

//

   //记录 rosbag
    rosbag::Bag bag_out;
    if (to_bag)
        bag_out.open(output_bag_file, rosbag::bagmode::Write);

  

 

  
1
  
2
  
3
  
4
 

根据参数设置 决定 是否 生成 rosbag 文件

//

    Eigen::Matrix3d R_transform;//方向设置矩阵  本质上就是一个旋转矩阵
    R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;//附初值
    Eigen::Quaterniond q_transform(R_transform);//旋转矩阵 转为 四元数

  

 

  
1
  
2
  
3
 

设置 方向设置矩阵 本质上就是一个旋转矩阵
位姿传感器的坐标系 方向 和 lidar 图像 的坐标系 方向 不一致

//

    std::string line;//声明 读取行数后保存的字符串
    std::size_t line_num = 0;//声明读取的行数 控制读取的 图像  lidar 的 数据序号

    ros::Rate r(10.0 / publish_delay);//控制发布频率

  

 

  
1
  
2
  
3
  
4
 

声明 读取行数后保存的字符串
声明读取的行数 控制读取的 图像 lidar 的 数据序号
控制发布频率 publish_delay 这个值 从参数服务器中来 也就是 launch中
//

    //循环读 时间戳 文件 的 每一行 
    while (std::getline(timestamp_file, line) && ros::ok())
    {
        float timestamp = stof(line);//将时间戳从 字符串 转 成 float


  

 

  
1
  
2
  
3
  
4
  
5
 

循环读 时间戳 文件 的 每一行
将时间戳从 字符串 转 成 float
//

        std::stringstream left_image_path, right_image_path;//声明 左右图像的路径

        //设置 左图片的路径
        left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        //通过opencv的方法 读取图片
        cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
        //设置 右图片的路径
        right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        //通过opencv的方法 读取图片
        cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
 

通过opencv的方法 读取图片

//

        //读取 ground_truth的一行
        std::getline(ground_truth_file, line);
        std::stringstream pose_stream(line);
        std::string s;
        Eigen::Matrix<double, 3, 4> gt_pose;//声明位姿 矩阵 3行4列
        for (std::size_t i = 0; i < 3; ++i)
        {
            for (std::size_t j = 0; j < 4; ++j)
            {
                std::getline(pose_stream, s, ' ');
                gt_pose(i, j) = stof(s);//给位姿矩阵赋值
            }
        }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
 

读取 ground_truth的一行
给位姿矩阵赋值
//

        Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());//提取位姿矩阵的角度部分 并转成四元数
        Eigen::Quaterniond q = q_transform * q_w_i;//进行 方向的 旋转
        q.normalize();//归一化
        Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();//提取位姿矩阵的位置部分 


  

 

  
1
  
2
  
3
  
4
  
5
 

提取位姿矩阵的角度部分 并转成四元数
提取位姿矩阵的位置部分
//

        //赋值里程计的 内容
        odomGT.header.stamp = ros::Time().fromSec(timestamp);
        odomGT.pose.pose.orientation.x = q.x();
        odomGT.pose.pose.orientation.y = q.y();
        odomGT.pose.pose.orientation.z = q.z();
        odomGT.pose.pose.orientation.w = q.w();
        odomGT.pose.pose.position.x = t(0);
        odomGT.pose.pose.position.y = t(1);
        odomGT.pose.pose.position.z = t(2);
        //发布里程计
        pubOdomGT.publish(odomGT);

        //赋值路径的内容
        geometry_msgs::PoseStamped poseGT;
        poseGT.header = odomGT.header;
        poseGT.pose = odomGT.pose.pose;
        pathGT.header.stamp = odomGT.header.stamp;
        pathGT.poses.push_back(poseGT);
        //发布路径
        pubPathGT.publish(pathGT)

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
 

赋值里程计的 内容 发布里程计
赋值路径的内容 发布路径
//

        // 读取 lidar 的点云 
        std::stringstream lidar_data_path; //声明点云路径字符串
        //赋值点云的路径
        lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" 
                        << std::setfill('0') << std::setw(6) << line_num << ".bin";
        
        std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());//读取lidar 的数据  以 [x y z i x y z i ....] 存储咋向量中
        std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";//4个元素是一个点 打印一共多少个点


  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
 

读取 lidar 的点云
以 [x y z i x y z i …] 存储咋向量中
//

        std::vector<Eigen::Vector3d> lidar_points; //声明雷达点 的位置 向量
        std::vector<float> lidar_intensities;//声明雷达点 的 强度 的 向量
        pcl::PointCloud<pcl::PointXYZI> laser_cloud; //声明 一个 PCL 的 点云
        for (std::size_t i = 0; i < lidar_data.size(); i += 4)//遍历上面读取的那个 lidar数据的向量
        {
            lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);//存入lidar位置
            lidar_intensities.push_back(lidar_data[i+3]);//存入lidar 强度

            pcl::PointXYZI point; //声明一个PCL的点
            //赋值点的 x y z i
            point.x = lidar_data[i];
            point.y = lidar_data[i + 1];
            point.z = lidar_data[i + 2];
            point.intensity = lidar_data[i + 3];
            laser_cloud.push_back(point);//将点存入点云
        }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
 

将lidar 的信息 存入 PCL形式的点云中

//

        sensor_msgs::PointCloud2 laser_cloud_msg;//声明一个ros的点云
        pcl::toROSMsg(laser_cloud, laser_cloud_msg);//将pcl的点云转成ros的
        laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);//赋值时间戳
        laser_cloud_msg.header.frame_id = "/camera_init";//声明坐标系
        pub_laser_cloud.publish(laser_cloud_msg);//发布ros的点云

  

 

  
1
  
2
  
3
  
4
  
5
 

发布ros的点云

//

        //将opecv的图像 转成ros的图像
        sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
        sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg();
        //发布左右图像
        pub_image_left.publish(image_left_msg);
        pub_image_right.publish(image_right_msg);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
 

发布ROS的左右图像

//

        if (to_bag)//如果发布rosbag 的话
        {   
            //写入 rosbag
            bag_out.write("/image_left", ros::Time::now(), image_left_msg);
            bag_out.write("/image_right", ros::Time::now(), image_right_msg);
            bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
            bag_out.write("/path_gt", ros::Time::now(), pathGT);
            bag_out.write("/odometry_gt", ros::Time::now(), odomGT);
        }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
 

写入 rosbag
//

        line_num ++;//更新行数  也相当于 更新 读取的 文件 序号
        r.sleep();//进行延时 控制发布频率

  

 

  
1
  
2
 

更新行数 也相当于 更新 读取的 文件 序号
进行延时 控制发布频率
//

    bag_out.close(); // 关闭rosbag
    std::cout << "Done \n";
    return 0;
}

  

 

  
1
  
2
  
3
  
4
 

关闭rosbag
//

Result

成功的话可以在 rviz里面 播放出 图片和 lidar的点云 信息

文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/qq_32761549/article/details/120322726