3D激光SLAM：ALOAM中KITTI数据集处理-转换成ROS可用数据代码解析

3D激光SLAM：ALOAM中KITTI数据集处理--转换成ROS可⽤数据代码解析3D激光SLAM:ALOAM中KITTI数据集处理--转换成ROS可⽤数据前⾔KITTI数据集由德国卡尔斯鲁厄理⼯学院和丰⽥美国技术研究院联合创办，是⽬前国际上最⼤的⾃动驾驶场景下的计算机视觉算法评测数据集。该数据集⽤于评测⽴体图像(stereo)，光流(opticalflow)，视觉测距(visualodometry)，3D物体检测(objectdetection)和3D跟踪(tracking)等计算机视觉技术在车载环境下的性能。KITTI包含市区、乡村和⾼速公路等场景采集的真实图像数据，每张图像中最多达15辆车和30个⾏⼈，还有各种程度的遮挡与截断。整个数据集由389对⽴体图像和光流图，39.2km视觉测距序列以及超过200k3D标注物体的图像组成，以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.binCode下⾯的代码实现了读取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>要包含的头⽂件主要是字节流的读⼊消息的处理等//intmain(intargc,char**argv){//初始化节点ros::init(argc,argv,"kitti_helper");//~发布的topic会以节点名字+话题名字发布ros::NodeHandlen("~");初始化节点////声明字符串相关的参数设置在launch中设置的数据的路径数据的sequence输出的bagstd::stringdataset_folder,sequence_number,output_bag_file;//从参数服务器获取数据路径路径n.getParam("dataset_folder",dataset_folder);//从参数服务器获取数据的sequencen.getParam("sequence_number",sequence_number);//打印kitti数据的信息std::cout<<"Readingsequence"<<sequence_number<<"from"<<dataset_folder<<'\n';//声明变量是否需要⽣成bagboolto_bag;//从参数服务器获取是否需要⽣成bagn.getParam("to_bag",to_bag);//如果⽣成bagif(to_bag)n.getParam("output_bag_file",output_bag_file);//从参数服务器获取//声明变量发布延时intpublish_delay;//从参数服务器获取发布延时n.getParam("publish_delay",publish_delay);//如果⼩于0则设置为1publish_delay=publish_delay<=0?1:publish_delay;从参数服务器中读取对应的信息注意参数服务器的设置在launch⽂件中<launch><nodename="kittiHelper"pkg="aloam_velodyne"type="kittiHelper"output="screen"><!--要启动的节点--><paramname="dataset_folder"type="string"value="/data/KITTI/odometry/"/><!--数据的路径--><paramname="sequence_number"type="string"value="00"/><!--数据的sequence--><paramname="to_bag"type="bool"value="false"/><!--是否保存bag⽂件--><paramname="output_bag_file"type="string"value="/tmp/kitti.bag"/><!--bag的输出路径--><paramname="publish_delay"type="int"value="1"/><!--topic发布的延时--></node></launch>程序从参数服务器中读取的参数都是从launch⽂件中设置的需要设置的已注释////lidar的点云发布句柄ros::Publisherpub_laser_cloud=n.advertise<sensor_msgs::PointCloud2>("/velodyne_points",2);//图像的发布句柄image_transport::ImageTransportit(n);image_transport::Publisherpub_image_left=it.advertise("/image_left",2);image_transport::Publisherpub_image_right=it.advertise("/image_right",2);//⾥程计的发布句柄ros::PublisherpubOdomGT=n.advertise<nav_msgs::Odometry>("/odometry_gt",5);//声明⼀个⾥程计nav_msgs::OdometryodomGT;odomGT.header.frame_id="/camera_init";//赋值⾥程计的坐标系⽗odomGT.child_frame_id="/ground_truth";//赋值⾥程计的坐标系⼦//声明路径的消息发布句柄ros::PublisherpubPathGT=n.advertise<nav_msgs::Path>("/path_gt",5);nav_msgs::PathpathGT;//声明⼀个路径变量pathGT.header.frame_id="/camera_init";//赋值坐标系声明设置要通过ros发布的topic句柄有:点云的\图像的\⾥程计的\路径的////读取时间戳的⽂件std::stringtimestamp_path="sequences/"+sequence_number+"/times.txt";std::ifstreamtimestamp_file(dataset_folder+timestamp_path,std::ifstream::in);读取时间戳的⽂件⼀个时间戳对应⼀组数据,后⾯的点云\图像的消息的时间戳则赋值现在设置的时间戳⽂件路径////读取路径的⽂件std::stringground_truth_path="results/"+sequence_number+".txt";std::ifstreamground_truth_file(dataset_folder+ground_truth_path,std::ifstream::in);设置path的⽂件的路径////记录rosbagrosbag::Bagbag_out;if(to_bag)bag_out.open(output_bag_file,rosbag::bagmode::Write);根据参数设置决定是否⽣成rosbag⽂件//Eigen::Matrix3dR_transform;//⽅向设置矩阵本质上就是⼀个旋转矩阵R_transform<<0,0,1,-1,0,0,0,-1,0;//附初值Eigen::Quaterniondq_transform(R_transform);//旋转矩阵转为四元数设置⽅向设置矩阵本质上就是⼀个旋转矩阵位姿传感器的坐标系⽅向和lidar图像的坐标系⽅向不⼀致//std::stringline;//声明读取⾏数后保存的字符串std::size_tline_num=0;//声明读取的⾏数控制读取的图像lidar的数据序号ros::Rater(10.0/publish_delay);//控制发布频率声明读取⾏数后保存的字符串声明读取的⾏数控制读取的图像lidar的数据序号控制发布频率publish_delay这个值从参数服务器中来也就是launch中////循环读时间戳⽂件的每⼀⾏while(std::getline(timestamp_file,line)&&ros::ok()){floattimestamp=stof(line);//将时间戳从字符串转成float循环读时间戳⽂件的每⼀⾏将时间戳从字符串转成float//std::stringstreamleft_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::Matleft_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::Matright_image=cv::imread(left_image_path.str(),CV_LOAD_IMAGE_GRAYSCALE);通过opencv的⽅法读取图⽚////读取ground_truth的⼀⾏std::getline(ground_truth_file,line);std::stringstreampose_stream(line);std::strings;Eigen::Matrix<double,3,4>gt_pose;//声明位姿矩阵3⾏4列for(std::size_ti=0;i<3;++i){for(std::size_tj=0;j<4;++j){std::getline(pose_stream,s,'');gt_pose(i,j)=stof(s);//给位姿矩阵赋值}}读取ground_truth的⼀⾏给位姿矩阵赋值//Eigen::Quaterniondq_w_i(gt_pose.topLeftCorner<3,3>());//提取位姿矩阵的⾓度部分并转成四元数Eigen::Quaterniondq=q_transform*q_w_i;//进⾏⽅向的旋转q.normalize();//归⼀化Eigen::Vector3dt=q_transform*gt_pose.topRightCorner<3,1>();//提取位姿矩阵的位置部分提取位姿矩阵的⾓度部分并转成四元数提取位姿矩阵的位置部分////赋值⾥程计的内容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::PoseStampedposeGT;poseGT.header=odomGT.header;poseGT.pose=odomGT.pose.pose;pathGT.header.stamp=odomGT.header.stamp;pathGT.poses.push_back(poseGT);//发布路径pubPathGT.publish(pathGT)赋值⾥程计的内容发布⾥程计赋值路径的内容发布路径////读取lidar的点云std::stringstreamlidar_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的数据以[xyzixyzi....]存储咋向量中std::cout<<"totally"<<lidar_data.size()/4.0<<"pointsinthislidarframe\n";//4个元素是⼀个点打印⼀共多少个点读取lidar的点云以[xyzixyzi…]存储咋向量中//std::vector<Eigen::Vector3d>lidar_points;//声明雷达点的位置向量std::vector<float>lidar_intensities;//声明雷达点的强度的向量pcl::PointCloud<pcl::PointXYZI>laser_cloud;//声明⼀个PCL的点云for(std::size_ti=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::PointXYZIpoint;//声明⼀个PCL的点//赋值点的xyzipoint.x=lidar_data[i];point.y=lidar_data[i+1];point.z=lidar_data[i+2];ensity=lidar_data[i+3];laser_cloud.push_back(point);//将点存⼊点云}将lidar的信息存⼊PCL形式的点云中//sensor_msgs::PointCloud2laser_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的点云发布ros的点云////opecv的图像转成ros