LVI: feature extraction and sorting of lidar subsystem

One 、 Node relationship

Review the node diagram ：

The last article analyzed in detail imageProjection node , This node subscribes to the original point cloud topic 、imu Original measurement topic 、 as well as VIS Odometer topic , Released preprocessing （ Filter invalid points 、 Orderly 、 To distort ） The following point cloud topics cloud_deskewed and cloud_info（ among cloud_deskewed The topic is ordinary PointCloud2 The message type of ,cloud_info The topic is custom formatted message types ）.

Looking at the node diagram, you can see cloud_deskewed Topics are visualized by nodes lvi_sam_rviz and VIS Visual feature node visual_feature subscribe , Therefore, in this paper, we follow cloud_info The trend of the topic , Analyze feature extraction nodes featureExtraction.

deskewed/cloud_info Topics are only subscribed by feature extraction nodes , And the feature extraction node only has feature/cloud_info A topic is subscribed to by other nodes .

Two 、 Custom format message types cloud_info

# Cloud Info
Header header   ##  header 


int32[] startRingIndex  ##  Point cloud's first i Scanning lines （ring） The first point on which curvature can be calculated 
int32[] endRingIndex    ##  Point cloud's first i Scanning lines （ring） The last point on which curvature can be calculated 


int32[]  pointColInd # point column index in range image ##  The column number of each point in the point cloud in the projected picture 
float32[] pointRange # point range ##  Each point in the point cloud is associated with LiDAR Distance of , That is, the pixel value of the projected picture 


int64 imuAvailable  ## imu Is the rotation measurement of aligned to LiDAR, If alignment instructions imu Rotation measurements are available 
int64 odomAvailable ##  Whether there is a pose transformation available between the two adjacent frames closest to the current frame 


# Attitude for lidar odometry initialization
##  Usable imu Rotation measurement , As LIS The predicted value of inter frame pose transformation 
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit


# Odometry
##  Available adjacent frame pose transformations , Also as LIS The predicted value of inter frame pose transformation 
float32 odomX
float32 odomY
float32 odomZ
float32 odomRoll
float32 odomPitch
float32 odomYaw


# Odometry reset ID
##  Rounding of covariance of position and attitude transformation obtained from odometer （ rounding ）, It can be used to calculate the reliability of the pose transformation value 
int64 odomResetId


# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed  Original de distorted point cloud 
sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature  A point cloud of corners 
sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature  A point cloud composed of pasta points 

3、 ... and 、 The main function

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar");


      And imageProjection be similar , The core work consists of FeatureExtraction Class constructor complete 
    FeatureExtraction FE;


    ROS_INFO("\033[1;32m----> Lidar Feature Extraction Started.\033[0m");
   
    ros::spin();


    return 0;
}

Four 、FeatureExtraction class

4.1 Parent class ParamServer class

As explained in the previous article ParamServer The class , Its main function is to set various parameters , And keep imu Coordinate transformation of measurement data to LiDAR The interface of .

4.2 FeatureExtraction Structure used by class

struct smoothness_t{ 
    float value;          Curvature value 
    size_t ind;           Point number 
};


struct by_value{ 
      An imitative function is used as a comparator , Used to do 
    bool operator()(smoothness_t const &left, smoothness_t const &right) { 
        return left.value < right.value;
    }
};

4.3 FeatureExtraction Class member Overview

4.4 FeatureExtraction Class

Let's focus on some good practices , This may follow from LeGO-LOAM and LIO-SAM, We don't have to worry about the source .

4.4.1 calculateSmoothness() Calculate curvature

LOAM How to calculate curvature ：

LVI-SAM How to calculate curvature ：

void calculateSmoothness()
    {
        int cloudSize = extractedCloud->points.size();
        for (int i = 5; i < cloudSize - 5; i++)
        {
              Use distance to calculate curvature , This is right before imageProjection The distance information calculated by the node is used again , Improve information utilization and computing efficiency .
            float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
                            + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
                            + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
                            + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
                            + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
                            + cloudInfo.pointRange[i+5];            


            cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;


            cloudNeighborPicked[i] = 0;
            cloudLabel[i] = 0;
            // cloudSmoothness for sorting
            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }

4.4.2 markOccludedPoints() Occlusion point marker

chart (a) Is a parallel beam , chart (b) Is the occlusion point . Both of these cases will lead to the plane , But it is mistaken for a corner with higher curvature , So before we calculate the curvature , These points need to be marked first , Prevent being extracted by mistake .

void markOccludedPoints()
{
    int cloudSize = extractedCloud->points.size();
    // mark occluded points and parallel beam points
    for (int i = 5; i < cloudSize - 6; ++i)
    {
        // occluded points
        float depth1 = cloudInfo.pointRange[i];
        float depth2 = cloudInfo.pointRange[i+1];
        int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));


          Proximity point ： Next to each other in an ordered point cloud , And the difference between the column numbers on the distance image is less than 10
        if (columnDiff < 10){
            // 10 pixel diff in range image
              Two similar points and LiDAR The distance difference between is greater than 0.3m, They are considered to be points that block each other 
              Scenic spots after marking , Avoid using occluded points as corners 
            if (depth1 - depth2 > 0.3){
                cloudNeighborPicked[i - 5] = 1;
                cloudNeighborPicked[i - 4] = 1;
                cloudNeighborPicked[i - 3] = 1;
                cloudNeighborPicked[i - 2] = 1;
                cloudNeighborPicked[i - 1] = 1;
                cloudNeighborPicked[i] = 1;
            }else if (depth2 - depth1 > 0.3){
                cloudNeighborPicked[i + 1] = 1;
                cloudNeighborPicked[i + 2] = 1;
                cloudNeighborPicked[i + 3] = 1;
                cloudNeighborPicked[i + 4] = 1;
                cloudNeighborPicked[i + 5] = 1;
                cloudNeighborPicked[i + 6] = 1;
            }
        }
        // parallel beam
          Theoretically, the farther away the object is , The point cloud should be more sparse , The adjacent points are far away 
          But for a surface parallel to the laser beam , Even close to lidar , It will still cause two adjacent points to be far away 
          So when the distance difference between adjacent points is greater than 0.02 Times the distance , It is considered to be a plane parallel to the laser beam , Exclude this point 
        float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
        float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));


        if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
            cloudNeighborPicked[i] = 1;
    }
}

4.4.3 freeCloudInfoMemory() Free custom point cloud memory

cloud_info yes LVI-SAM Custom format point cloud , Contains a large amount of data content , It takes up a lot of memory space .

imageProjection Nodes put the original unordered point cloud , The distortion is removed and ordered , And set up a large number of indexes to facilitate traversal to find feature points , The distance information is also reserved to screen out some special cloud points .

These data are for featureExtraction Nodes are necessary , But it doesn't make sense for the subsequent process , And create a new one cloud_info Examples are inconvenient , So before you post the topic ,lvi-sam Yes cloudInfo There was one “ Slimming ”, To improve the later operation efficiency .

void freeCloudInfoMemory()
{
    cloudInfo.startRingIndex.clear();
    cloudInfo.startRingIndex.shrink_to_fit();  Release the array's capacity
    cloudInfo.endRingIndex.clear();
    cloudInfo.endRingIndex.shrink_to_fit();
    cloudInfo.pointColInd.clear();
    cloudInfo.pointColInd.shrink_to_fit();
    cloudInfo.pointRange.clear();
    cloudInfo.pointRange.shrink_to_fit();
}

It's used here shrink_to_fit() Function to release the capacity, Because only clear() Function will only change the size, We don't know when to release memory , While using shrink_to_fit() Functions can explicitly 、 Actively free array memory .

4.5 FeatureExtraction Class constructor

Finally, let's go over it again FeatureExtraction Class constructor , Complete the summary of this article .

FeatureExtraction()
{
      Subscribe to de distortion 、 Ordered custom format point cloud cloudInfo, The callback function is responsible for removing interference points , Extract corner points and plane points according to curvature 
    subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/deskew/cloud_info", 5, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());


      Released and registered corner point cloud and facet point cloud , also “ Slimming ” After that cloudInfo
    pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5);
      Release corner point cloud and facet point cloud 
    pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_corner", 5);
    pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_surface", 5);
    
      Reset all parameters , Set voxel filter resolution 
    initializationValue();
}

This article is only for academic sharing , If there is any infringement , Please contact to delete .

3D Recommended visual quality courses ：

1. Multi sensor data fusion technology for automatic driving field

2. For the field of automatic driving 3D Whole stack learning route of point cloud target detection ！( Single mode + Multimodal / data + Code )
3. Thoroughly understand the visual three-dimensional reconstruction ： Principle analysis 、 Code explanation 、 Optimization and improvement
4. China's first point cloud processing course for industrial practice
5. laser - Vision -IMU-GPS The fusion SLAM Algorithm sorting and code explanation
6. Thoroughly understand the vision - inertia SLAM： be based on VINS-Fusion The class officially started
7. Thoroughly understand based on LOAM Framework of the 3D laser SLAM: Source code analysis to algorithm optimization
8. Thorough analysis of indoor 、 Outdoor laser SLAM Key algorithm principle 、 Code and actual combat (cartographer+LOAM +LIO-SAM)

9. Build a set of structured light from zero 3D Rebuild the system [ theory + Source code + practice ]

10. Monocular depth estimation method ： Algorithm sorting and code implementation

11. Deployment of deep learning model in autopilot

12. Camera model and calibration ( Monocular + Binocular + fisheye ）

13. blockbuster ！ Four rotor aircraft ： Algorithm and practice

14.ROS2 From entry to mastery ： Theory and practice

blockbuster ！3DCVer- Academic paper writing contribution   Communication group Established

Scan the code to add a little assistant wechat , can Apply to join 3D Visual workshop - Academic paper writing and contribution   WeChat ac group , The purpose is to communicate with each other 、 Top issue 、SCI、EI And so on .

meanwhile You can also apply to join our subdivided direction communication group , At present, there are mainly 3D Vision 、CV& Deep learning 、SLAM、 Three dimensional reconstruction 、 Point cloud post processing 、 Autopilot 、 Multi-sensor fusion 、CV introduction 、 Three dimensional measurement 、VR/AR、3D Face recognition 、 Medical imaging 、 defect detection 、 Pedestrian recognition 、 Target tracking 、 Visual products landing 、 The visual contest 、 License plate recognition 、 Hardware selection 、 Academic exchange 、 Job exchange 、ORB-SLAM Series source code exchange 、 Depth estimation Wait for wechat group .

Be sure to note ： Research direction + School / company + nickname , for example ：”3D Vision  + Shanghai Jiaotong University + quietly “. Please note... According to the format , Can be quickly passed and invited into the group . Original contribution Please also contact .

▲ Long press and add wechat group or contribute

▲ The official account of long click attention

3D Vision goes from entry to mastery of knowledge ： in the light of 3D In the field of vision Video Course cheng （ 3D reconstruction series 、 3D point cloud series 、 Structured light series 、 Hand eye calibration 、 Camera calibration 、 laser / Vision SLAM、 Automatically Driving, etc ）、 Summary of knowledge points 、 Introduction advanced learning route 、 newest paper Share 、 Question answer Carry out deep cultivation in five aspects , There are also algorithm engineers from various large factories to provide technical guidance . meanwhile , The planet will be jointly released by well-known enterprises 3D Vision related algorithm development positions and project docking information , Create a set of technology and employment as one of the iron fans gathering area , near 4000 Planet members create better AI The world is making progress together , Knowledge planet portal ：

Study 3D Visual core technology , Scan to see the introduction ,3 Unconditional refund within days

  There are high quality tutorial materials in the circle 、 Answer questions and solve doubts 、 Help you solve problems efficiently

Feel useful , Please give me a compliment ~