A-LOAM源码解读

代码

1、kittiHelper.cpp

/*
 * @Author: your name
 * @Date: 2020-09-18 09:28:16
 * @LastEditTime: 2020-10-05 21:29:26
 * @LastEditors: Please set LastEditors
 * @Description: 读取kitti文件，发布odomGT,pathGT
 * @FilePath: \A-LOAM-NOTED-devel\src\kittiHelper.cpp
 */

// Author:   Tong Qin               qintonguav@gmail.com
// 	         Shaozu Cao 		    saozu.cao@connect.ust.hk

#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#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>

std::vector<float> read_lidar_data(const std::string lidar_data_path)
{
   
   
    std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
    lidar_data_file.seekg(0, std::ios::end);
    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);
    lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements*sizeof(float));
    return lidar_data_buffer;
}

int main(int argc, char** argv)
{
   
   
    ros::init(argc, argv, "kitti_helper");
    ros::NodeHandle n("~");
    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';
    bool to_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);
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;

    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);

    // 发布odomGT
    ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
    nav_msgs::Odometry odomGT;         
    odomGT.header.frame_id = "/camera_init";            // 发布从camera_init到 ground_trurh的坐标系变换 已经转换为lidar的odom了
    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);

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

    // 是否导出未bag文件
    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;
    Eigen::Quaterniond q_transform(R_transform);

    std::string 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);       // 时间戳

        // 读取图片路径
        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";
        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";
        cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);

        // 读取ground_truth_path
        std::getline(ground_truth_file, line);
        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>());            // q_w_i代表camera的odom
//        Eigen::Quaterniond q = q_transform * q_w_i;
//        此处应该添加 * q_transform.inverse()，如下所示
        Eigen::Quaterniond q = q_transform * q_w_i *q_transform.inverse();          // camera-odom变为lidar的odom
        q.normalize();
        Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();           // lidar-->camera的旋转矩阵*camera坐标系的平移


        // odomGT代表lidar的pose
        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


        // 发布lidar的pathGT
        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);              // 发布lidar的pathGT
        
        // 读取点云数据
        // read lidar point cloud
        std::stringstream lidar_data_path;
        lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" 
                        << std::setfill('0') << std::setw(6) << line_num << ".bin";         // 设置前面5个0
        std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
        std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";

        std::vector<Eigen::Vector3d> lidar_points;
        std::vector<float> lidar_intensities;
        pcl::PointCloud<pcl::PointXYZI> laser_cloud;

        // 读取当前帧的点云
        for (std::size_t i = 0; i < lidar_data.size(); i += 4)
        {
   
   
            lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
            lidar_intensities.push_back(lidar_data[i+3]);

            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);
        }

        sensor_msgs::PointCloud2 laser_cloud_msg;
        pcl::toROSMsg(laser_cloud, laser_cloud_msg);
        laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        laser_cloud_msg.header.frame_id = "/camera_init";
        pub_laser_cloud.publish(laser_cloud_msg);           //  发布点云信息

        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);           // 发布图像信息

        // 保存为bag文件
        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();
    std::cout << "Done \n";


    return 0;
}

2、scanRegistration.cpp

scanRegistration的功能就是提取特征点。

• comp函数，比较两个点的曲率

bool comp (int i,int j) {
   
    return (cloudCurvature[i]<cloudCurvature[j]); }

• removeClosedPointCloud函数，丢弃一定距离以内的点

// 丢弃一定距离以内的点
template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
   
   
    if (&cloud_in != &cloud_out)
    {
   
   
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0;

    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {
   
   
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;
        cloud_out.points[j] = cloud_in.points[i];
        j++;
    }
    if (j != cloud_in.points.size())
    {
   
   
        cloud_out.points.resize(j);
    }

    cloud_out.height = 1;
    cloud_out.width = static_cast<uint32_t>(j);
    cloud_out.is_dense = true;
}

• laserCloudHandler函数

这个函数主要就是对接收到的点云数据进行处理，最终发布关键点（laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat），以方便后续使用。

1、首先计算当前帧的第一个点和最后一个点的角度

    // 计算起始点和终止点角度
    // 解释一下：atan2的范围是[-180,180]，atan的范围是[-90,90]
    int cloudSize = laserCloudIn.points.size();
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);        // 第一个扫描点的角度
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,                         // 最后一个扫描点的角度
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;

    // 最后一个点的角度 - 第一个点的角度>PI
    if (endOri - startOri > 3 * M_PI)
    {
   
   
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
   
   
        endOri += 2 * M_PI;
    }
    printf("[scanRegistration] start Ori %f\n",startOri);
    printf("[scanRegistration] end Ori %f\n", endOri);

2、对每一个点计算其scanID和该点对应的扫描到的时间，将结果放在intensity中。

    // 对每一个点计算scanID和扫描的点的时间
    bool halfPassed = false;
    int count = cloudSize;          // 所有的点数
    PointType point;
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);       // 每一行扫描视为一个点云
    for (int i = 0; i < cloudSize; i++)
    {
   
   
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        // 根据激光扫描线的俯仰角获取scanid[0,15]
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;         // 俯仰角[-90,90]
        int scanID = 0;

        if (N_SCANS == 16)
        {
   
   
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
   
   
                count--;
                continue;
            }
        }
        else if (N_SCANS == 32)
        {
   
   
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
   
   
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {
   
      
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
   
   
                count--;
                continue;
            }
        }
        else
        {
   
   
            printf("[scanRegistration] wrong scan number\n");
            ROS_BREAK();
        }
        //printf("angle %f scanID %d \n", angle, scanID);

        // 获取每一个点对应的角度
        float ori = -atan2(point.y, point.x);           // 每一个点的水平角度
        if (!halfPassed)                                // 判断当前点过没过一半（开始的时候是没过一半的）
        {
   
    
            if (ori < startOri - M_PI / 2)
            {
   
   
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
   
   
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
   
   
                halfPassed = true;
            }
        }
        else            // 后半段时间
        {
   
   
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
   
   
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
   
   
                ori -= 2 * M_PI;
            }
        }

        float relTime = (ori - startOri) / (endOri - startOri);     // 时间占比
        point.intensity = scanID + scanPeriod * relTime;            // intensity=scanID+点对应的在线上的时间
        laserCloudScans[scanID].push_back(point);                   // 把当前点放在对应的scanID对应的线上
    }

3、计算16条scan的起始点的索引和终止点的索引

    // 记录16个scan的每一个的startInd，endInd
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)
    {
   
    
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6; 
    }

4、对每一个点计算曲率

    // 有序地计算曲率
    for (int i = 5; i < cloudSize - 5; i++)     // 忽略前后的5个点
    {
   
    
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i