LIO-SAM代码学习——imageProjection.cpp_nh.subscribe

imageProjection.cpp（激光雷达图像处理进程）

主函数

初始化ROS节点，创建一个图像进程类，以处理激光点云。
开辟一个三线程的spinner以供三个订阅者使用，防止线程占用导致数据丢失。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lio_sam");
    ImageProjection IP;
    ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
    return 0;
}
1
2
3
4
5
6
7
8
9

ImageProjection 激光雷达图像处理进程类

一、成员变量

private:
    std::mutex imuLock; // 限制访问IMU数据的互斥锁
    std::mutex odoLock; // 限制访问里程计数据的互斥锁

    ros::Subscriber subLaserCloud; // 雷达消息订阅器
    ros::Publisher pubLaserCloud;  // 没用

    ros::Publisher pubExtractedCloud; // 发布有效的雷达点云
    ros::Publisher pubLaserCloudInfo; // 发布雷达点云信息

    ros::Subscriber subImu;                // imu消息订阅器
    std::deque<sensor_msgs::Imu> imuQueue; // imu信息缓存器

    ros::Subscriber subOdom;                  // imu里程计增量订阅器
    std::deque<nav_msgs::Odometry> odomQueue; // imu里程计信息缓存器

    std::deque<sensor_msgs::PointCloud2> cloudQueue; // 原始雷达消息缓存器
    sensor_msgs::PointCloud2 currentCloudMsg;        // 当前雷达消息（ROS格式）

    // 进行点云偏斜矫正时所需的通过imu积分获得的imu姿态信息
    double *imuTime = new double[queueLength];
    double *imuRotX = new double[queueLength];
    double *imuRotY = new double[queueLength];
    double *imuRotZ = new double[queueLength];

    int imuPointerCur;
    bool firstPointFlag;
    Eigen::Affine3f transStartInverse;

    pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn; // 使用Velodyne的雷达时使用的，指向当前雷达帧点云消息的智能指针（Velodyne格式）
    pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn; // 使用Ouster的雷达时使用的，指向当前雷达帧点云消息的智能指针（Ouster格式）
    pcl::PointCloud<PointType>::Ptr fullCloud;      // 该帧完整点云
    pcl::PointCloud<PointType>::Ptr extractedCloud; // 偏斜矫正后点云

    int deskewFlag; // 点云时间标志位（点云数据中是否包含时间信息）
    cv::Mat rangeMat; // 点云投影获得的深度图

    // 进行点云偏斜矫正时所需的通过imu里程计获得的imu位置增量
    bool odomDeskewFlag;
    float odomIncreX; // 雷达数据帧起止时刻的IMU数据帧间变换的x量
    float odomIncreY; // 雷达数据帧起止时刻的IMU数据帧间变换的y量
    float odomIncreZ; // 雷达数据帧起止时刻的IMU数据帧间变换的z量

    lio_sam::cloud_info cloudInfo; // 用于储存标志位和中间变量的数据结构
    double timeScanCur; // 雷达帧扫描开始时间戳
    double timeScanEnd; // 雷达帧扫描结束时间戳
    std_msgs::Header cloudHeader; // 雷达数据帧时间戳存储变量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

二、构造函数 ImageProjection()

初始化的订阅者和发布者

    ImageProjection() : deskewFlag(0)
    {
        // 订阅imu传感器数据
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅增量式lidar里程计数据
        subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic + "_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅lidar传感器点云数据
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

        // 发送运动补偿后的点云
        pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/deskew/cloud_deskewed", 1);
        // 发送点云相关的信息
        pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1);

        allocateMemory(); // 定义指针并分配内存
        resetParameters(); // 参数复位

        pcl::console::setVerbosityLevel(pcl::console::L_ERROR); // 用于设置控制台输出的信息（控制那些信息会输出到控制台，如报错、日志、警告等）
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

三、void allocateMemory()

为定义的指针分配内存

    void allocateMemory() 
    {
        laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>()); // 给当前帧智能指针分配内存（Velodyne雷达）
        tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>()); // 给当前帧智能指针分配内存（Ouster雷达）
        fullCloud.reset(new pcl::PointCloud<PointType>()); // 给当前帧点云存储指针分配内存
        extractedCloud.reset(new pcl::PointCloud<PointType>()); // 给偏斜校正后点云指针分配内存空间

        fullCloud->points.resize(N_SCAN * Horizon_SCAN); // 初始化点云点集大小

        cloudInfo.startRingIndex.assign(N_SCAN, 0); // 我不明白这是什么实现，我猜是设置最大值是N_SCAN，然后如果超过该值对应的error编号是0
        cloudInfo.endRingIndex.assign(N_SCAN, 0); // 我猜是设置最大值是N_SCAN

        cloudInfo.pointColInd.assign(N_SCAN * Horizon_SCAN, 0); // 我猜是设置最大值是N_SCAN * Horizon_SCAN
        cloudInfo.pointRange.assign(N_SCAN * Horizon_SCAN, 0); // 我猜是设置最大值是N_SCAN * Horizon_SCAN

        resetParameters(); // 参数复位
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

四、void resetParameters()

参数复位

    void resetParameters()
    {
        laserCloudIn->clear();   // 雷达点云信息清空
        extractedCloud->clear(); // 偏斜矫正后点云清空
        // reset range matrix for range image projection
        rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); // 投影深度图

        imuPointerCur = 0; // IMU数据标志位（为0表示没有IMU数据）
        firstPointFlag = true; // 首点标志位（为true表示是当前点云中时间最靠前的点）
        odomDeskewFlag = false; // 终了点标志位

        // imu通过积分获得的姿态信息（将IMU姿态信息数组初始化为0）
        for (int i = 0; i < queueLength; ++i)
        {
            imuTime[i] = 0;
            imuRotX[i] = 0;
            imuRotY[i] = 0;
            imuRotZ[i] = 0;
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

五、void imuHandler(const sensor_msgs::Imu::ConstPtr &imuMsg)

IMU信息处理函数，处理IMU传感器数据。
将新到来的IMU数据转换到雷达坐标系定义（前左上坐标系）下，通过进程锁保护公共内存，并将IMU数据添加到IMU队列中。

    void imuHandler(const sensor_msgs::Imu::ConstPtr &imuMsg)
    {
        // 将imu信息在雷达坐标系下表达（转到前左上的坐标系下）
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

        std::lock_guard<std::mutex> lock1(imuLock);
        imuQueue.push_back(thisImu); // 缓存imu数据
    }
1
2
3
4
5
6
7
8

六、void odometryHandler(const nav_msgs::Odometry::ConstPtr &odometryMsg)

增量式雷达里程计数据处理函数。
通过进程锁保护公共内存，将接收到的雷达里程计数据添加到队列当中。

    void odometryHandler(const nav_msgs::Odometry::ConstPtr &odometryMsg)
    {
        // 缓存imu里程计增量信息
        std::lock_guard<std::mutex> lock2(odoLock);
        odomQueue.push_back(*odometryMsg);
    }
1
2
3
4
5
6

七、void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)

雷达点云数据处理函数。
当一个新的雷达数据（应该是一帧串口数据，可能是一帧雷达消息）到来时，进程进行的整体处理流程。

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
    {
        // 转换雷达点云信息为可处理点云数据并缓存
        if (!cachePointCloud(laserCloudMsg))
            return;
        // 点云偏斜矫正所需的imu数据的预处理(计算雷达帧扫描开始和结束时间戳之间的imu相对位姿变换)
        if (!deskewInfo())
            return;
        // 对点云进行偏斜矫正（运动补偿），并投影到深度图中
        projectPointCloud();
        // 确定每根线的起始和结束点索引，并提取出偏斜矫正后点云及对应的点云信息
        cloudExtraction(); 
        // 发布有效点云和激光点云信息(包括每根线的起始和结束点索引、点深度、列索引)
        publishClouds();
        // 重置中间变量
        resetParameters();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

八、bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)

转换雷达点云信息为可处理点云数据，判断接收到的雷达消息所包含的信息并缓存。

存储点云数据

将新到来的雷达帧消息添加到雷达消息队列（原始雷达消息缓存器）的末尾，判断雷达消息队列长度是否大于2，若小于2则退出雷达信息处理回调函数，不进行后边的操作。

进行数据类型转换

将原始雷达消息缓存器队列头（时间最早）的ROS类型的雷达数据，根据使用的雷达数据格式转换成相应的PCL数据类型，并将该数据从队列中移除。

获取雷达数据的起始时间

从接收的雷达消息头获取消息的起始时间，根据消息中最后一个点相对于起始点的时间间隔，计算雷达消息的结束时间。

判断点云数据是否有限

通过消息中的标志位is_dense判断点云数据当中是否存在inf或NaN值（无限值）的点，有则需先滤除这些点（给从传感器获取的数据加一个滤波程序）。
若存在无限值则直接关闭ROS，停止程序。

检查点云消息中是否包含线束信息

线束信息标志位ringFlag初始化为0，通过检测雷达数据类型currentCloudMsg中字段中变量集合fields是否存在名字为ring的变量来判断是否存在线束信息。
若存在则将标志位置1，若不存在则将标志位置-1并退出ROS停止程序。

检查点云消息中是否包含时间字段

时间字段标志位deskewFlag初始化值为0，同检查是否包含线束信息相同检查是否包含名称为time或t的时间字段。
若存在则将标志位置1，若不存在则将标志位置-1但不退出ROS不结束程序。

返回值

若“存储点云数据”步骤中点云数据队列小于等于2，则返回false；
若“存储点云数据”步骤中点云数据队列大于2，且后续判断均为使程序结束，则最终返回true。

    bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) // 判断雷达msg所包含信息并进行缓存
    {
        // cache point cloud
        // 缓存雷达点云消息
        cloudQueue.push_back(*laserCloudMsg);
        if (cloudQueue.size() <= 2)
            return false;

        // convert cloud
        // 将雷达点云消息转换成pcl点云数据结构
        currentCloudMsg = std::move(cloudQueue.front()); // 获取队首点云信息
        cloudQueue.pop_front();
        if (sensor == SensorType::VELODYNE)
        {
            pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
        }
        else if (sensor == SensorType::OUSTER)
        {
            // Convert to Velodyne format
            pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
            laserCloudIn->points.resize(tmpOusterCloudIn->size());
            laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
            for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
            {
                auto &src = tmpOusterCloudIn->points[i];
                auto &dst = laserCloudIn->points[i];
                dst.x = src.x;
                dst.y = src.y;
                dst.z = src.z;
                dst.intensity = src.intensity;
                dst.ring = src.ring;
                dst.time = src.t * 1e-9f;
            }
        }
        else
        {
            ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
            ros::shutdown();
        }

        // get timestamp
        // 获取当前帧的时间戳
        cloudHeader = currentCloudMsg.header;
        timeScanCur = cloudHeader.stamp.toSec();
        timeScanEnd = timeScanCur + laserCloudIn->points.back().time; // 计算当前帧扫描结束时时间戳

        // check dense flag
        // 判断点云是否是稠密的(判断点云是否包含nan或者inf值)
        // 判断点云是否是有限，is_dense为1表示是有限的
        if (laserCloudIn->is_dense == false)
        {
            ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
            ros::shutdown();
        }

        // check ring channel
        // 检查点云信息当中是否有所属线束信息
        static int ringFlag = 0; // 线束信息标志位
        if (ringFlag == 0)
        {
            ringFlag = -1;
            for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
            {
                if (currentCloudMsg.fields[i].name == "ring")
                {
                    ringFlag = 1;
                    break;
                }
            }
            if (ringFlag == -1)
            {
                //没有可以像loam或legoloam那样手动计算给他加上
                ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
                ros::shutdown();
            }
        }

        // check point time
        // 当雷达点云信息内包含时间戳字段，则设置去畸变标志位为true
        if (deskewFlag == 0)
        {
            deskewFlag = -1;
            for (auto &field : currentCloudMsg.fields) // 每次循环的变量名（如i）为field，field的值从currentCloudMsg.fields中依次取值
            {
                if (field.name == "time" || field.name == "t")
                {
                    deskewFlag = 1;
                    break;
                }
            }
            if (deskewFlag == -1)
                ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
        }

        return true;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

九、bool deskewInfo()

点云偏斜矫正所需的imu数据的预处理(计算雷达帧扫描开始和结束时间戳之间的imu相对位姿变换)

使用进程锁保护公共内存

对访问IMU权限和访问雷达数据权限的进程锁均上锁。

判断是否满足预处理条件

仅当IMU数据非空，且最早的IMU数据时间戳小于当前雷达帧起始时间戳，最晚IMU数据时间戳大于当前雷达帧结束时间戳时，才可以对雷达数据全时段计算IMU相对位姿。

对IMU和里程计数据进行预处理

通过imuDeskewInfo()和odomDeskewInfo()对IMU和里程计数据进行预处理。

返回值

如果满足预处理要求则返回true，否则返回false。

    bool deskewInfo() // 运动补偿所需的IMU数据处理
    {
        std::lock_guard<std::mutex> lock1(imuLock);
        std::lock_guard<std::mutex> lock2(odoLock);

        // make sure IMU data available for the scan
        // 当没有imu信息时，无法进行偏斜矫正
        if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
        {
            ROS_DEBUG("Waiting for IMU data ...");
            return false;
        }

        // 通过积分获取当前雷达帧扫描开始和结束时间戳内的imu姿态信息(主要为获取imu帧相对姿态信息)
        imuDeskewInfo();

        // 获得imu里程计在当前雷达帧扫描开始和结束时间戳内的起始和结束帧，并计算两者之间的位姿变换(主要为获取imu帧相对位置信息)
        odomDeskewInfo();

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

十、void imuDeskewInfo()

根据IMU数据消息队列，求出用于雷达点云运动补偿所需的IMU姿态变化信息。
即求解以雷达帧初始位姿（近似）为0位姿，雷达帧起始时间段内，以IMU数据周期为间隔的相对位姿变换。

置位标志位

在函数开始时将补偿所需IMU姿态标志位cloudInfo.imuAvailable置false，若处理函数顺利运行，没有在中间返回退出，则会在最后将该标志位置为true。

剔除过期数据

对于位姿补偿，时间早于当前雷达帧起始时间的IMU数据是无用（过期）的，通过从队列首开始循环遍历非空的IMU消息队列，将时间小于timeScanCur - 0.01的IMU数据从队列中剔除。

判断队列是否非空

剔除完过期数据后，判断队列是否非空，若为空则直接返回退出函数。
将IMU数据序号imuPointerCur初始化为0。

循环处理IMU队列消息

从队列首开始循环遍历IMU队列中剩余的元素，将第i个元素赋值给IMU数据中间变量thisImuMsg，并读取变量消息头当中的时间信息。

首个IMU数据（imuPointerCur == 0）

判断该IMU数据的时间戳是否小于当前雷达帧的时间戳，若小于，则使用utility.h文件中定义的imuRPY2rosRPY()函数将用四元素表示的imu姿态信息转换成欧拉角表示，并将欧拉角存储在进程中间变量cloudInfo当中，作为该雷达帧的姿态估计值。
同时若imuPointerCur == 0等于0，则将序号为0的IMU相对位姿初始化为0（xyz旋转均为0），并将imuPointerCur加一，直接开始下一次循环。

非首个IMU数据（imuPointerCur != 0）

判断IMU数据的时间是否大于雷达帧结束时间（近似），若大于则直接跳出循环，否则进行一下操作：
从thisImuMsg中通过utility.h文件中的imuAngular2rosAngular()函数取出IMU的角速度信息，根据第i和i-1个IMU数据的时间信息，计算出积分所需要的时间间隔timeDiff。
根据上一个IMU相对位姿，和当前IMU角速度信息，通过“积分”算出当前IMU相对位姿，并标记时间信息供下一次计算时间差使用。
将imuPointerCur加一。

判断IMU姿态信息是否可用

将imuPointerCur减一，减去最后一次处理时为下一个无用数据进行的序号自加。
判断imuPointerCur是否小于等于0，若成立则直接退出函数，不将cloudInfo.imuAvailable置true。

    void imuDeskewInfo() //运动补偿所需的IMU姿态变化信息
    {
        cloudInfo.imuAvailable = false;

        while (!imuQueue.empty())
        {
            // 丢弃早于当前雷达帧开始扫描时间戳的缓存的imu帧
            // 此处0.01的选定应该是根据其IMU传感器数据传输频率为100Hz
            if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
                imuQueue.pop_front();
            else
                break;
        }

        // 如果没有imu数据，直接返回
        if (imuQueue.empty())
            return;

        imuPointerCur = 0; // IMU消息序号，以最接近雷达其实时间的IMU数据帧为第0个

        for (int i = 0; i < (int)imuQueue.size(); ++i)
        {
            // 获取imu帧时间戳
            sensor_msgs::Imu thisImuMsg = imuQueue[i];
            double currentImuTime = thisImuMsg.header.stamp.toSec();

            // get roll, pitch, and yaw estimation for this scan
            // 将用四元数表示的imu姿态信息转换成欧拉角表示
            // 在上边对IMU数据队列进行过处理，将时间在timeScanCur-0.01之前的IMU数据从队列中剔除了，此处IMU队列第一个数据应该是时间在（timeScanCur-0.01，timeScanCur）之间距离雷达帧时间戳较近的IMU数据，可将其大致视为与当前雷达帧时间戳重合
            if (currentImuTime <= timeScanCur)
                imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

            // 如果imu帧时间戳大于雷达帧扫描结束的时间戳，则退出
            if (currentImuTime > timeScanEnd + 0.01)
                break;

            // 初始化第一帧imu姿态信息为0
            if (imuPointerCur == 0)
            {
                imuRotX[0] = 0;
                imuRotY[0] = 0;
                imuRotZ[0] = 0;
                imuTime[0] = currentImuTime;
                ++imuPointerCur;
                continue;
            }

            // get angular velocity
            // 获取imu角速度信息
            double angular_x, angular_y, angular_z;
            imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

            // integrate rotation
            // 积分imu的姿态信息
            double timeDiff = currentImuTime - imuTime[imuPointerCur - 1];
            imuRotX[imuPointerCur] = imuRotX[imuPointerCur - 1] + angular_x * timeDiff;
            imuRotY[imuPointerCur] = imuRotY[imuPointerCur - 1] + angular_y * timeDiff;
            imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur - 1] + angular_z * timeDiff;
            imuTime[imuPointerCur] = currentImuTime;
            ++imuPointerCur;
        }

        --imuPointerCur;

        if (imuPointerCur <= 0)
            return;

        cloudInfo.imuAvailable = true; // 可以使用IMU对lidar点云进行运动补偿
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

十一、void odomDeskewInfo()

根据IMU里程计信息，求出用于运动补偿所需的IMU位置变化信息，
以及雷达帧从开始到结束时刻之间的IMU坐标变换，
并记录起始时刻的IMU位姿。

置位标志位

将里程计补偿可用标志位cloudInfo.odomAvailable置为false。

剔除过期数据

将里程计队列当中时间小于（近似）雷达帧起始时间（timeScanCur - 0.01）的里程计数据从队列当中剔除。

判断是否继续执行程序

判断剔除过期数据后队列是否为空，若剔除数据后队列为空，则直接返回退出函数。
判断剔除过期数据后首个IMU里程计数据时间戳是否大于当前雷达帧起始时间，若大于则直接返回退出函数。

取出起始时刻（近似）IMU里程计数据

取出离雷达帧最近且时间大于等于雷达帧起始时间的IMU里程计信息，将imu里程计帧的姿态信息转换为欧拉角表示，记录imu里程计的位姿，存储在cloudInfo当中，将被作为地图优化的初始估计值（点云起始时刻的位姿）。

置位标志位

将里程计补偿可用标志位cloudInfo.odomAvailable置为true。
将结束里程计标志位odomDeskewFlag置为false。

判断是否继续执行程序

判断IMU里程计数据队列尾的数据时间戳是否小于雷达帧结束时间，若小于则直接返回退出函数。

取出结束时刻（近似）IMU里程计数据

获得imu里程计结束帧(也就是在雷达帧扫描开始和结束时间戳之间的最后一个imu里程计帧)数据；
判断odom是否退化，若odom退化了，说明置信度不高，则没有必要通过odom进行运动补偿（我不明白这里怎么看出是否有退化）；

计算从起始时刻IMU里程计位姿到结束时刻IMU里程计位姿的相对位姿变换

通过pcl::getTransformation()将起始IMU里程计位姿转换为eigen格式的坐标变换形式；
将结束帧IMU位姿如起始帧IMU位姿一样操作，获得其欧拉角，并转换为eigen格式的坐标变换形式；
通过矩阵运算得到起止IMU数据帧之间的相对坐标变换；
通过pcl::getTranslationAndEulerAngles()将相对坐标变换转换为xyz rpy格式。

置位标志位

将结束里程计标志位odomDeskewFlag置为true。

    void odomDeskewInfo() //运动补偿所需的IMU位置变化信息，以及开始IMU帧到结束IMU帧的坐标变换，并记录起始时刻的IMU位姿（供后端使用）
    {
        cloudInfo.odomAvailable = false;

        // 丢弃早于当前雷达帧开始扫描时间的imu里程计帧
        while (!odomQueue.empty())
        {
            if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
                odomQueue.pop_front();
            else
                break;
        }

        // 如果没有imu里程计帧直接返回
        if (odomQueue.empty())
            return;
        // 保证IMU最早一帧的时间戳小于点云起始时间戳
        if (odomQueue.front().header.stamp.toSec() > timeScanCur) // [cn]???
            return;

        // get start odometry at the beinning of the
        // 获得imu里程计起始帧(也就是在雷达帧扫描开始和结束时间戳之间的第一个imu里程计帧)
        nav_msgs::Odometry startOdomMsg;
        // 取出离雷达帧最近且时间大于等于雷达帧起始时间的IMU里程计信息
        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            startOdomMsg = odomQueue[i];

            if (ROS_TIME(&startOdomMsg) < timeScanCur)
                continue;
            else
                break;
        }

        // 将imu里程计帧的姿态信息转换为欧拉角表示
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
        double roll, pitch, yaw;
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

        // Initial guess used in mapOptimization
        // 将imu里程计的位姿记录，并将被发布出去用于地图优化的初始值（点云起始时刻的位姿）
        cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
        cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
        cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
        cloudInfo.initialGuessRoll = roll;
        cloudInfo.initialGuessPitch = pitch;
        cloudInfo.initialGuessYaw = yaw;

        cloudInfo.odomAvailable = true;

        // get end odometry at the end of the scan
        odomDeskewFlag = false;

        if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
            return;

        // 获得imu里程计结束帧(也就是在雷达帧扫描开始和结束时间戳之间的最后一个imu里程计帧)
        nav_msgs::Odometry endOdomMsg;
        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            endOdomMsg = odomQueue[i];

            if (ROS_TIME(&endOdomMsg) < timeScanEnd)
                continue;
            else
                break;
        }

        //判断odom是否退化，若odom退化了，说明置信度不高，则没有必要通过odom进行运动补偿
        if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0]))) // round为四舍五入函数
            return;

        // 获得imu里程计起始帧位姿
        Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);

        // 获得imu里程计结束帧位姿
        tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
        // 获得起始和结束帧之间的相对转换
        Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

        // 获得起始和结束帧之间的位姿增量，其中姿态用欧拉角形式表示
        float rollIncre, pitchIncre, yawIncre; // 雷达数据帧起止时刻的IMU数据帧间变换的rpy量
        pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

        odomDeskewFlag = true;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89

十二、void projectPointCloud()

对点云进行偏斜矫正（运动补偿），并投影到深度图中。
遍历全部点云集合laserCloudIn中的所有点，取出三维位置坐标和强度值；
通过三维位置坐标计算深度值（即坐标点到原点的欧氏距离）；
对深度值在[lidarMinRange,lidarMaxRange]区间之内的点（有效点）进行处理，忽略深度在区间范围之外的点；
取出有效点的线数信息，作为点在深度图的行坐标，如果线束过多可以进行降采样，通过对downsampleRate取余等于0，只对线数为downsampleRate整数倍的线上的点进行操作；
根据有效点的xy坐标，计算点的水平角度，根据水平角度进行一定的补偿，得到点在深度图的列坐标；
通过检查当前点对应的行列位置在深度图中的值是否为FLT_MAX（在一开始进行过定义和初始化，将深度图中的深度信息均初始化为浮点数最大值FLT_MAX）来判断深度图中该点是否进行过填充，若没有进行过填充，则通过deskewPoint()函数对当前雷达点进行运动补偿，得到校正后的雷达点，再将校正前得到的深度值填充到深度图中；
通过计算列数+行数*列长度计算当前雷达点在行优先一维数组中的序号，将校正后的雷达点加入到整体点云数据fullCloud当中。

    void projectPointCloud()
    {
        int cloudSize = laserCloudIn->points.size();
        // range image projection
        // 遍历所有点云
        for (int i = 0; i < cloudSize; ++i)
        {
            PointType thisPoint;
            // 取出单个点云信息
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            thisPoint.intensity = laserCloudIn->points[i].intensity;

            float range = pointDistance(thisPoint);             // 计算当前点深度（欧氏距离）
            if (range < lidarMinRange || range > lidarMaxRange) // 如果深度在给定范围之外，则认为是异常点并直接丢弃
                continue;

            int rowIdn = laserCloudIn->points[i].ring; // 获得当前点所在的线索引，也即在深度图中的行索引
            if (rowIdn < 0 || rowIdn >= N_SCAN)        // 如果在给定的线数范围之外，则直接丢弃
                continue;

            if (rowIdn % downsampleRate != 0) // 如果线数太多可以对其进行降采样
                continue;

            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; // 计算当前点的水平倾角，以便求得深度图的列索引

            static float ang_res_x = 360.0 / float(Horizon_SCAN);                         // 计算水平角分辨率
            int columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2; // 计算当前点在在深度图中的列索引（通过补偿，转换为x轴负方向为起始，并以顺时针为正方向）
            // 做一些冗余判断，防止越界
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;

            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
                continue;

            // 对当前点进行偏斜矫正
            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

            // 填充深度图，对补偿后的点进行存储
            rangeMat.at<float>(rowIdn, columnIdn) = range;

            // 记录完整点云
            int index = columnIdn + rowIdn * Horizon_SCAN; //计算点云索引，用以为数组储存图像信息
            fullCloud->points[index] = thisPoint;
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

十三、PointType deskewPoint(PointType *point, double relTime)

对雷达点进行运动补偿（偏斜校正）。
第一个参数是要校正点云的XYZI，第二个参数是该雷达点相对于雷达帧起始时刻的相对时间。
通过点云时间标志位deskewFlag和IMU数据补偿标志位cloudInfo.imuAvailable判断运动补偿是否可以进行，若不能进行则直接返回原来的点；
通过雷达帧起始时间+雷达点相对时间计算雷达点的绝对时间；
通过findRotation()获得当前点时间戳对应的相对于雷达帧起始时刻的imu姿态相对变化（相对旋转补偿），通过findPosition()获得当前点时间戳对应的相对于雷达帧起始时刻的imu位置变换（由于移动较慢，此处直接置为0）；
根据标志位firstPointFlag判断是否是雷达帧的第一个点，若是则获取第一个点相对于起始位置的相对位姿变换（的逆）；若不是则计算当前点到第一个点之间的相对位姿变换，根据该相对位姿变换将各点补偿到第一个点的位姿上，返回补偿后点的XYZI信息（I不变）。

    PointType deskewPoint(PointType *point, double relTime)
    {
        // 等待imu数据就绪才能进行处理
        if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
            return *point;

        double pointTime = timeScanCur + relTime; // 当前点采集时的时间戳(该帧点云数据的起始时间+该点云的相对时间)

        float rotXCur, rotYCur, rotZCur;
        findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur); // 获得当前点时间戳对应的imu姿态变化（相对旋转补偿）

        float posXCur, posYCur, posZCur;
        findPosition(relTime, &posXCur, &posYCur, &posZCur); // 获得当前点时间戳对应imu位置变换(由于移动较慢，此处直接置为0)

        // 获取第一个点时间戳对应的位姿变化
        if (firstPointFlag == true)
        {
            transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
            firstPointFlag = false;
        }

        // transform points to start
        // 获得当前点对应的imu位姿和第一个点对应的imu位姿之间的相对变换
        Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
        Eigen::Affine3f transBt = transStartInverse * transFinal;

        // 变换当前点到第一个点所在的坐标系也即雷达坐标系(至此完成偏斜矫正)
        PointType newPoint;
        newPoint.x = transBt(0, 0) * point->x + transBt(0, 1) * point->y + transBt(0, 2) * point->z + transBt(0, 3);
        newPoint.y = transBt(1, 0) * point->x + transBt(1, 1) * point->y + transBt(1, 2) * point->z + transBt(1, 3);
        newPoint.z = transBt(2, 0) * point->x + transBt(2, 1) * point->y + transBt(2, 2) * point->z + transBt(2, 3);
        newPoint.intensity = point->intensity;

        return newPoint;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

十四、void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)

第一个参数为当前点的绝对时间，后三个参数为从IMU数据中得到的三轴旋转数据。
查找第一个时间戳大于等于当前点的imu数据指针，如果不存在时间戳上大于当前点的imu数据帧或者该数据帧为首帧，则直接返回时间最晚的imu数据帧姿态信息，如果存在时间戳大于当前点的imu数据帧且不为首帧，则利用该帧前一帧和该帧进行插值（线性插值），获取当前点时间戳对应的相对姿态信息。

    void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
    {
        *rotXCur = 0;
        *rotYCur = 0;
        *rotZCur = 0;

        // 查找第一个时间戳大于等于当前点的imu数据指针
        int imuPointerFront = 0;
        while (imuPointerFront < imuPointerCur)
        {
            if (pointTime < imuTime[imuPointerFront])
                break;
            ++imuPointerFront;
        }
        // 如果不存在时间戳上大于当前点的imu数据帧或者该数据帧为首帧，则直接返回最近的imu数据帧姿态信息
        if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
        {
            *rotXCur = imuRotX[imuPointerFront];
            *rotYCur = imuRotY[imuPointerFront];
            *rotZCur = imuRotZ[imuPointerFront];
        }
        else // 如果存在时间戳大于当前点的imu数据帧且不为首帧，则利用该帧前一帧和该帧进行插值（线性插值），获取当前点时间戳对应的姿态信息
        {
            int imuPointerBack = imuPointerFront - 1;
            double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
            *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
            *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

十五、void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)

如果传感器相对移动比较缓慢，则该移动对偏斜矫正影响较小，所以此处作者直接将移动量置为0，影响不大

    void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
    {
        *posXCur = 0;
        *posYCur = 0;
        *posZCur = 0;

        // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.

        // if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
        //     return;

        // float ratio = relTime / (timeScanEnd - timeScanCur);

        // *posXCur = ratio * odomIncreX;
        // *posYCur = ratio * odomIncreY;
        // *posZCur = ratio * odomIncreZ;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

十六、void cloudExtraction()

由于点云偏斜校正后被存储在一个行优先一维数组当中，该数组的信息可以构成一个深度图，但由于深度图中并非所有的点都被填充，因此一维数组当中有些点是没有有效信息的，通过判断深度值是否为FLT_MAX（深度图初始化时的深度值）来判断元素是否被填充，用新的数组记录被填充点的列索引（新数组元素为深度图一维数组中被填充过的列索引，可通过行数和列索引得到对应深度图一维数组中的点），
用cloudInfo.pointColInd[]数组记录被填充的点的列索引，
用cloudInfo.pointRange[]数组记录cloudInfo.pointColInd[]中索引相同的点对应的深度信息，
用cloudInfo.startRingIndex[]数组记录cloudInfo.pointColInd[]中每一行可用于计算的起始点的索引，
用cloudInfo.endRingIndex[]数组记录cloudInfo.pointColInd[]中每一行可用于计算的结束点的索引，
用extractedCloud保存所有有效点，其索引与cloudInfo.pointColInd[]的索引对应。

    void cloudExtraction()
    {
        int count = 0;
        // extract segmented cloud for lidar odometry
        // 确定每根线的起始和结束点索引（为存储所有点云数据的一维数组的索引），并提取出偏斜矫正后点云
        for (int i = 0; i < N_SCAN; ++i)
        {
            // 每根线上点云信息的起始索引未从起始点开始，原因是该部分提取出来的点云是用于特征提取的，起始点附近的点都无法有效计算曲率
            // 将每根线上起始索引设置成起始处第五个点
            cloudInfo.startRingIndex[i] = count - 1 + 5;

            for (int j = 0; j < Horizon_SCAN; ++j)
            {
                if (rangeMat.at<float>(i, j) != FLT_MAX)
                {
                    // mark the points' column index for marking occlusion later
                    cloudInfo.pointColInd[count] = j; // 第count个点，对应哪一根垂线（列）
                    // save range info
                    cloudInfo.pointRange[count] = rangeMat.at<float>(i, j); // 该点距离信息
                    // save extracted cloud
                    extractedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); // 3D坐标信息
                    // size of extracted cloud
                    ++count;
                }
            }
            // 将每根线上结束索引设置成结束处倒数第五个点
            cloudInfo.endRingIndex[i] = count - 1 - 5;
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

十七、void publishClouds()

输出处理好的点云信息。
发布偏斜校正后的点云信息；
并将偏斜校正后的点云消息添加到整体消息cloudInfo当中将整体消息发布出去。

    void publishClouds()
    {
        cloudInfo.header = cloudHeader;
        cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame); // 发布提取出的有效点云
        pubLaserCloudInfo.publish(cloudInfo);                                                                       // 发布激光点云信息(包括每根线的起始和结束点索引、点深度、列索引)
    }
1
2
3
4
5
6

数据结构

一、nav_msgs/Odometry

http://docs.ros.org/en/melodic/api/geometry_msgs/html/msg/Pose.html

std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose // 带协方差矩阵的位姿信息 
geometry_msgs/TwistWithCovariance twist // 带协方差矩阵的速度信息
1
2
3
4

二、sensor_msgs/PointCloud2

http://docs.ros.org/en/jade/api/sensor_msgs/html/msg/PointCloud2.html

std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense
1
2
3
4
5
6
7
8
9

三、lio_sam::cloud_info

# Cloud Info
Header header 

int32[] startRingIndex # 有效点云数组第i行起始有效计算数据索引
int32[] endRingIndex # 有效点云数组第i行最后有效计算数据索引

int32[]  pointColInd # point column index in range image 有效点云列索引
float32[] pointRange # point range 有效点云深度信息

int64 imuAvailable # IMU姿态估计信息可用标志位
int64 odomAvailable # IMU里程计位姿估计信息可用标志位

# Attitude for LOAM initialization IMU得到的雷达帧初始姿态估计值
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit

# Initial guess from imu pre-integration 雷达帧初始位姿估计值
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw

# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed 偏斜校正后的点云
sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature 角点云
sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature 面点云
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

自定义的PCL点数据

struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D // 表示给点结构添加基本xyz属性
    PCL_ADD_INTENSITY; // 表示给点结构添加基本强度属性
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16; // (EIGEN_ALIGN16)强制SSE填充以获得正确的内存对齐  
POINT_CLOUD_REGISTER_POINT_STRUCT(VelodynePointXYZIRT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring)(float, time, time)) // 注册成员变量信息




struct OusterPointXYZIRT
{
    PCL_ADD_POINT4D;
    float intensity;
    uint32_t t;
    uint16_t reflectivity;
    uint8_t ring;
    uint16_t noise;
    uint32_t range;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint32_t, t, t)(uint16_t, reflectivity, reflectivity)(uint8_t, ring, ring)(uint16_t, noise, noise)(uint32_t, range, range))

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

其他相关知识

互斥锁mutex

当仅存在一个互斥锁时，可以简单理解为将互斥锁上锁，相当于把进程的公共内存锁住，这样在一个线程操作内存是，其他线程就不会对内存进行操作，可以避免写内存的进程还没有执行完，读内存的进程就开始读取，导致读取的数据为改了一半的异常数据。
当存在两个互斥锁时，就不能简单理解为将进程公共区域上锁了，如果依旧这样理解，两个锁看上去就会很奇怪。实际上互斥锁的本质是互斥量，可以理解为互斥量有两个状态：上锁和未上锁。对互斥锁进行锁操作时，首先检查互斥量状态，如果为未上锁状态，则将其改为上锁状态，并继续执行线程，如果此时互斥量为上锁状态，则将阻塞线程，直到该互斥量的状态变为未上锁状态，线程才开始运行，并由该线程将互斥锁设置为上锁状态。
在该程序中，需要进行读写的内存有里程计数据和IMU读取到的数据，因此当进程想要进行IMU数据读写时，就需要上锁imuLock，在想要进行里程计数据读取时，就需要上锁odoLock，因此需要两个互斥锁。
注意对互斥锁进行上锁，不是锁住了哪一片内存，而是相当于关闭了可访问权限。就像疫情时逛超市，限制人员进入，则超市门口的保安就是互斥锁，商品就是内存，当A进来买生活用品时，保安要做的不是把生活用品区给封起来，只给A使用，其他人就算能进来要购买也只能看着直到A买完；保安要做的是让A进入，并让其他要购买生活用品的人挡在外面，直到A购买完；如果此时有其他人想买电器，则该保安就不知道要不要放人进入了，这时就需要另一个管电器区保安，也就是令一个互斥锁，这就相当于使用两个互斥锁的情况。

    std::mutex imuLock;
    std::mutex odoLock;
1
2