Logo-beam source code analysis - imageproject CPP


A lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. The system takes in point cloud from a Velodyne VLP-16 Lidar (placed horizontal) and optional IMU data as inputs. It outputs 6D pose estimation in real-time.

A brief introduction to logo-LOAM (laser SLAM, IMU+LiDAR) is given. Based on LOAM, it achieves the same accuracy and greatly reduces the amount of calculation. Using point cloud segmentation to distinguish noise, extract plane and edge features, use two-step LM optimization method to solve pose, and add loop detection to reduce drift.

Next, go straight to the code section:

github: https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

Logo-loam code structure

First, let's take a look at the code structure. It is very clear. cloud_msgs, launch, include, src (core program). Let's take a look at the following:

  • CMakeList.txt

    find_package(GTSAM REQUIRED QUIET)
    find_package(PCL REQUIRED QUIET)
    find_package(OpenCV REQUIRED QUIET)

    Dependent libraries: GTSAM, PCL, OPenCV

    Here is a brief introduction to GTSAM. GTSAM is a C++ library for smoothing and mapping in the field of robotics and computer vision. It uses factor graphs and Bayesian networks to maximize the posterior probability. If you want to learn more, you can see resources [2].

  • launch/run.launch

    Nodes: rviz, camera_init_to_map, base_link_to_camera, imageProjection, featureAssociation, mapOptmization, transformFusion

    The main nodes are the last four, which are also the core programs under the corresponding src directory.

  • include/utility.h

    Define data structure: point cloud, topic, lidar parameters, image segmentation parameters, feature extraction parameters, optimization parameters, mapping parameters

        * A point cloud type that has "ring" channel
    struct PointXYZIR
        uint16_t ring;
    } EIGEN_ALIGN16;
                                       (float, x, x) (float, y, y)
                                       (float, z, z) (float, intensity, intensity)
                                       (uint16_t, ring, ring)

    Here, we define a_ ID point cloud data type. velodyne lidar has this data, but other manufacturers may not have it; Different from laser_id, ring_id refers to the line number (0-15 for VLP-16) that is incremented from the lowest laser line.

    // Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
    extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
    // VLP-16
    extern const int N_SCAN = 16;//number of lasers
    extern const int Horizon_SCAN = 1800;//number of points from one laser of one scan
    extern const float ang_res_x = 0.2;//angle resolution of horizon
    extern const float ang_res_y = 2.0;//angle resolution of vertical
    extern const float ang_bottom = 15.0+0.1;
    extern const int groundScanInd = 7;//Indicates the number of coils required on the ground;

    The parameter settings of different laser radars should be adjusted according to their own actual conditions. If you use the data of a data set, such as KITTI, you need to adjust for HDL-64E. Moreover, because the vertical angle resolution of the radar is not uniform, you also need to write your own subsequent projection program.

    Next, let's cut into the core program. Since there are four CPPs, in order not to be too long, let's analyze imageproject cpp.

  • src/imageProjection.cpp

    Overall process: subscribe to point cloud data callback processing - > point cloud conversion to pcl preprocessing - > intercept a frame of laser data - > projection mapping to image - > ground removal - > point cloud segmentation - > publish point cloud data - > Reset parameters;

    Callback processing function subscribed to lidar data:

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
     // 1. Convert ros message to pcl point cloud
    copyPointCloud(laserCloudMsg);//Use pcl library function;
     // 2. Start and end angle of a scan
     findStartEndAngle();//1. the angle difference of one circle of data is calculated by atan2; 2. pay attention to the rationality of the range of calculation results
     // 3. Range image projection
     projectPointCloud();//Project the lidar data into a 16x1800 (depending on the radar angular resolution) point cloud array
     // 4. Mark ground points
     groundRemoval();//Calculate the pitch angle between the two lines according to the position of the points between the upper and lower lines. If it is less than 10 degrees, it is the ground point
     // 5. Point cloud segmentation
     cloudSegmentation();//Firstly, the point cloud is labeled by clustering, and the corresponding point cloud block is stored according to the label;
     // 6. Publish all clouds
     publishCloud();//Publish various point cloud data
     // 7. Reset parameters for next iteration

    projectPointCloud(): Map 3D point cloud projection to 2D range image

    // find the row and column index in the image for this point
    if (useCloudRing == true){
        rowIdn = laserCloudInRing->points[i].ring;
        verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
        rowIdn = (verticalAngle + ang_bottom) / ang_res_y;//Determine row index
    horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
    columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;//Determine column index
    range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);//Determine depth
    Lidar coordinate system


    The angle of the arctangent is equal to the included angle between the X axis and the line passing through the origin and the given coordinate point (x, y). A positive result indicates the angle of counterclockwise rotation from the X axis, and a negative result indicates the angle of clockwise rotation from the X axis;

    atan2 function

    1. determine row index

    Calculate according to ring id or coordinates:


    row=(θv+θbottom)/resolutionvrow = (\theta_v+\theta_{bottom})/resolution_vrow=(θv​+θbottom​)/resolutionv​

    2. determine the column index ([− π, π][-\pi,\pi][− π, π] is converted to [01800][01800][01800]): (the front of the y axis is the middle dividing line)



    3. determine the depth value: (pay attention to the effective range of the data)


    groundRemoval(): ground removal

    // groundMat
    // -1, no valid info to check if ground of not
    //  0, initial value, after validation, means not ground
    //  1, ground
    for (size_t j = 0; j < Horizon_SCAN; ++j){
        for (size_t i = 0; i < groundScanInd; ++i){
            //groundScanInd defines the number of laser lines hitting the ground
            lowerInd = j + ( i )*Horizon_SCAN;
            upperInd = j + (i+1)*Horizon_SCAN;
            if (fullCloud->points[lowerInd].intensity == -1 ||
                fullCloud->points[upperInd].intensity == -1){
                // no info to check, invalid points
                groundMat.at<int8_t>(i,j) = -1;
            diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
            diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
            diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
            angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
            //If the pitch angle of two adjacent points in the vertical direction is less than 10 degrees, it is determined as the ground point; Adjacent scanning circle
            if (abs(angle - sensorMountAngle) <= 10){
                groundMat.at<int8_t>(i,j) = 1;
                groundMat.at<int8_t>(i+1,j) = 1;
    Ground judgment principle

    If the pitch angle of two vertical adjacent points is less than 10 degrees, it is determined as a ground point.


    void cloudSegmentation(){
        // segmentation process
        for (size_t i = 0; i < N_SCAN; ++i)
            for (size_t j = 0; j < Horizon_SCAN; ++j)
                if (labelMat.at<int>(i,j) == 0)
                    labelComponents(i, j);//->Labelcomponents() clustering operation
    //Local feature detection clustering
    //Plane point and edge point
    void labelComponents(int row, int col){
        while(queueSize > 0){
            // Pop point
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            // Mark popped point
            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
            // Loop through all the neighboring grids of popped grid
            //Upper, lower, left and right neighborhood points
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
                // new index
                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;
                // index should be within the boundary
                if (thisIndX < 0 || thisIndX >= N_SCAN)
                // at range image margin (left or right side)
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;
                // prevent infinite loop (caused by put already examined point back)
                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                //Compare larger depth with smaller depth
                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                                rangeMat.at<float>(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                                rangeMat.at<float>(thisIndX, thisIndY));
                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                    alpha = segmentAlphaY;
                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));//Plane clustering formula: the larger the angle, the more the two points tend to be plane
                //segmentTheta=60 degrees
                if (angle > segmentTheta){
                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                    lineCountFlag[thisIndX] = true;
                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
        // check if this segment is valid
        bool feasibleSegment = false;
        if (allPushedIndSize >= 30)
            feasibleSegment = true;//Pastry
        else if (allPushedIndSize >= segmentValidPointNum){
            int lineCount = 0;
            for (size_t i = 0; i < N_SCAN; ++i)
                if (lineCountFlag[i] == true)
            if (lineCount >= segmentValidLineNum)
                feasibleSegment = true;//Edge point            
        // segment is valid, mark these points
        if (feasibleSegment == true){
        }else{ // segment is invalid, mark these points
            for (size_t i = 0; i < allPushedIndSize; ++i){
                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;

    Take the labelMat with the ground removed, calculate 4 points in the neighborhood point by point (pay attention to the effective range of the neighborhood index), compare the maximum value and the minimum value of the distance with the center point, and combine the threshold π /3\pi/3 π /3 through the following formula. If the value is greater than this value, the plane feature will be formed as a plane point. The array stores the index, labelMat stores the label value, and lineCountFlag is set to true until the threshold is not met, jump out and proceed to the next step.



    Plane judgment principle

    Clustering, more than 30 points are clustered into a class, labelCount++; If it is less than 30 and more than 5, the number of cluster points in the vertical direction will be counted, and if it is more than 3, it will also be marked as a class; If none is satisfied, the value 999999 indicates the cluster points to be discarded.

    publishCloud(): point cloud Publishing

    resetParameters(): parameter reset


[1]. Shan T, Englot B. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4758-4765.

[2]. gtsam_tutorial video

[3].pcl::fromROSMsg & pcl::toROSMsg


[5].velodyne_pointcloud::PointXYZIR Struct Reference


Tags: slam

Posted by deko on Tue, 31 May 2022 05:46:42 +0530