A-LOAM代码注释学习（二）kittiHelper.cpp

---

一、代码注释

kittiHelper.cpp主要用来把文件格式的KITTI数据集打包成ros格式并发布topic，需要与kitti_helper.launch配合使用。

1.1 kittiHelper.cpp

注意：
此处有一个矩阵运算的bug，已进行标注，可以分别对修改前后生成的bag的信息进行查看。主要查看 /path_gt 和 /odometry_gt 的输出。

#include <iostream>                         //标准的输入输出流头文件
#include <fstream>                          //fstream也就是文件流file stream
#include <iterator>                         //STL标准中的一些迭代器模板类
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>      //高层GUI图像交互模块
#include <nav_msgs/Odometry.h>              //ROS导航功能包里的里程计
#include <nav_msgs/Path.h>                  //用于显示轨迹
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <geometry_msgs/PoseStamped.h>      //时间、位置和姿态
#include <cv_bridge/cv_bridge.h>            //在ROS图像消息和OpenCV图像之间进行转换的一个功能包
#include <sensor_msgs/image_encodings.h>    //关于图像编码模式的源文件，其中规定了RGB的图像以及深度图的编码模式
#include <eigen3/Eigen/Dense>               //Eigen库
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>   //一些数据类型转换的函数
#include <sensor_msgs/PointCloud2.h>

//定义一个float类型的容器read_lidar_data读取雷达数据
std::vector<float> read_lidar_data(const std::string lidar_data_path)    
{
    //c++中文件读取和写入以流的形式进行
    // 函数原型 ifstream(const char *filename, std::ifstream::openmode mode);
    //ifstream 打开文件，mode模式std::ifstream::in | std::ifstream::binary：以只读方式打开 | 以二进制而非文本格式进行操作
    std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);

    //seekg()用于设置在输入流中的位置，seek + get
    //新位置将从由 place 给出的起始位置开始，偏移 offset 个字节。offset 形参是一个 long 类型的整数，而 place 可以是以下 ios 类中的之一
    //起始位置可能是文件的开头、文件的当前位置或文件的末尾，这些地方分别由常量 ios::beg、ios::cur 和 ios::end 表示。
    lidar_data_file.seekg(0, std::ios::end);

    //size_t 一个与机器相关的unsigned类型。统计一下文件有多少float数据
    //tellg() 用于在输入流中获取位置
    const size_t num_elements = lidar_data_file.tellg() / sizeof(float);

    // 文件指针指向文件开始
    lidar_data_file.seekg(0, std::ios::beg);

    std::vector<float> lidar_data_buffer(num_elements);  //定义float类型的容器，雷达数据缓冲（数据量）
    // 读取所有的数据
    //.read（）文件读取，有两个参数，从  lidar_data_file读取num_elements*sizeof(float)数量的数据到&lidar_data_buffer[0]里面
    lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements*sizeof(float));
    return lidar_data_buffer;  //返回 lidar_data_buffer
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "kitti_helper");  //node name为kitti_helper
    ros::NodeHandle n("~");                 //命名空间为/node_namespace/node_name ，即 kitti_helper/kitti_helper
    std::string dataset_folder, sequence_number, output_bag_file;
    n.getParam("dataset_folder", dataset_folder);
    n.getParam("sequence_number", sequence_number);
    std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
    
    // 是否将数据集转换为rosbag文件格式，由launch文件设置
    //从参数服务器获取 是否需要生成bag
    bool to_bag;
    n.getParam("to_bag", to_bag);
    if (to_bag)                                                 //如果生成bag
        n.getParam("output_bag_file", output_bag_file);         //获取bag输出文件夹
    int publish_delay;
    n.getParam("publish_delay", publish_delay);                 //定义 一个整型 发布延迟，由launch文件设置
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;     //三目运算符，延迟时间<=0 则设为1

    //创建一个publisher，发布名为/velodyne_points的topic，消息类型为sensor_msgs::PointCloud2，队列长度为2
    ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 2);

    //image_transport发布图像
    image_transport::ImageTransport it(n);         //Image_transport 提供用于为低带宽压缩格式(compressed formats)image传输提供透明支持
    image_transport::Publisher pub_image_left = it.advertise("/image_left", 2);
    image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);
    //常规发布节点 ros::Publisher pub = n.advertise<sensor_msgs::Image>("/image_left", 2);

    //里程计发布，odomGT，包括旋转的四元数和位置坐标向量
    ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
    nav_msgs::Odometry odomGT;                      //声明一个里程计信息类型
    // 赋值里程计的坐标系
    odomGT.header.frame_id = "camera_init";         //真值pose的坐标系实际是左相机的坐标系 
    odomGT.child_frame_id = "/ground_truth";        

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

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

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

    //函数原型，void rosbag::Bag::open(const std::__cxx11::string &filename, uint32_t mode = 2U)
    rosbag::Bag bag_out;
    if (to_bag)
        bag_out.open(output_bag_file, rosbag::bagmode::Write);
    
    //camera 相对于lidar的旋转矩阵
    Eigen::Matrix3d R_transform;
    R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;                //其实更准确的应该使用KITTI提供的左相机到Lidar的标定参数进行变换
    Eigen::Quaterniond q_transform(R_transform);

    std::string line;                   //声明字符串line
    std::size_t line_num = 0;           //声明读取的行数

    ros::Rate r(10.0 / publish_delay);  //发布频率
    while (std::getline(timestamp_file, line) && ros::ok())     ///读 时间戳 文件 成为一个字符串
    {
        float timestamp = stof(line);                           //stof()将字符串转换为float型

        std::stringstream left_image_path, right_image_path;
        //自动填充'0'使得'0'+line_num凑成6个字符
        left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        
        //通过opencv读取图片
        //cv::Mat是OpenCV2和OpenCV3中基本的数据类型，函数cv2.imread(filepath,flags)读入一副图片，CV_LOAD_IMAGE_GRAYSCALE，其为将原图转为灰度图片
        cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);
        right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);

        // 得到ground truth的文件，这里没用
        std::getline(ground_truth_file, line);  //读取 ground_truth的一行,一行12个数，塞到矩阵 gt_pose 里
        std::stringstream pose_stream(line);
        std::string s;
        Eigen::Matrix<double, 3, 4> gt_pose;
        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);
            }
        }

        Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());            //提取位姿矩阵的旋转矩阵部分 并转成四元数
        //Eigen::Quaterniond q = q_transform * q_w_i;                       //此处有bug，修改成下一行
        Eigen::Quaterniond q = q_transform * q_w_i * q_transform.inverse(); //坐标系进行变换的四元数 q
        q.normalize();                                                      //对四元数 q 归一化

        //q 来自于 q_w_i, q_w_i来自于 gt_pose， gt_pose来自于ground_truth_file，也就是真值文件.txt
        //t 来自于 gt_pose，
        Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>(); //q_transform是四元数，和3*1向量相乘，有运算法则
       

        //odomGT 又来自于 q，t
        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);                                  //发布里程计

        //又把odomGT 放到了 poseGT
        geometry_msgs::PoseStamped poseGT;                          //定义几何信息
        poseGT.header = odomGT.header;
        poseGT.pose = odomGT.pose.pose;

        //nav_msgs::Path pathGT; 用于ROS中画轨迹
        //又把odomGT 放到了 pathGT 
        // nav_msgs::Path = std_msgs/Header + geometry_msgs::PoseStamped
        pathGT.header.stamp = odomGT.header.stamp;
        pathGT.poses.push_back(poseGT);                             //函数将一个新的元素加到最后面

        pubPathGT.publish(pathGT);                                  //发布pathGT

        // 读取雷达点云
        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]存储在向量中
        std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";  //以四个元素（xyzi）为一个点，总共打印lidar_data.size() / 4.0个点


        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)
        {
            //emplace_back是就地构造，不用构造后再次复制到容器中。因此效率更高
            lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);  //位置XYZ塞到3维向量lidar_points
            lidar_intensities.push_back(lidar_data[i+3]);                                //点云强度塞到浮点一维向量lidar_intensities

            pcl::PointXYZI point;
            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);           //将点存入laser_cloud
        }

        sensor_msgs::PointCloud2 laser_cloud_msg;
        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的点云

        //将opencv的图像转换成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);

        if (to_bag)
        {
            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);
        }

        line_num ++;        //更新行数，相当于更新读取的文件序号
        r.sleep();          //按照循环频率延时  
    }
    bag_out.close();        //关闭输出的rosbag
    std::cout << "Done \n"; //打印 done


    return 0;
}

1.2 kitti_helper.launch

根据KITTI数据集存放位置和kittiHelper.cpp中的文件路径对kitti_helper.launch文件进行修改。
注意：
kittiHelper.cpp中的dataset_folder和sequence_number 是数据集路径设置，取决于kitti_helper.launch文件的设置。
to_bag和output_bag_file设置是否将数据转换为bag文件及文件保存路径，取决于kitti_helper.launch文件的设置。
“10 / publish_delay” 设置ROS的发布频率，但是，取1，按照10Hz发布。

• kittiHelper.cpp中图片和激光雷达点云文件路径：

//图片的路径 
left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
//
//激光雷达点云数据路径
lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" << std::setfill('0') << std::setw(6) << line_num << ".bin";

主要是文件路径修改对，建议按照KITTI数据集的结构保存数据。

<launch>
    	<node name="kittiHelper" pkg="aloam_velodyne" type="kittiHelper" output="screen"> 
        <param name="dataset_folder" type="string" value="/mnt/hgfs/H/KITTI/" />
        <param name="sequence_number" type="string" value="00" />
        <param name="to_bag" type="bool" value="true" />
        <param name="output_bag_file" type="string" value="/mnt/hgfs/H/KITTI/kitti01.bag" /> <!-- replace with your output folder -->
        <param name="publish_delay" type="int" value="1" />
    </node>
</launch>

• 数据集结构如下：

//文件夹结构如下
KITTI
     --sequences
    		--01
    				--image_0
    				--image_1
    				--cablib.txt
    				--times.txt
     --velodyne
    		--sequences
    				--01
    					--velodyne
    						--*.bin文件
     --results
     		--01.txt

二、使用方法

2.1 用法一：KITTI数据集转rosbag

把launch文件中设置为<param name=“to_bag” type=“bool” value=“true” >，用于将KITTI数据集分散的图片、点云和路径信息打包成一个rosbag文件，用于算法测试。转换成的bag信息如下图所示。

2.2 用法二：在线转换并运行A-LOAM算法

先执行算法，然后新开一个终端在线转换并播放数据。

source devel/setup.bash
roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
roslaunch aloam_velodyne kitti_helper.launch