Aloam code reading and summary

Aloam

• A-LOAM yes LOAM Advanced implementation of , It USES Eigen and Ceres Solver To simplify the code structure . This code is created by LOAM and LOAM_NOTEA To modify . This code is simple and clear , There is no complex mathematical derivation and redundant operation . about SLAM It is a good learning material for beginners .

• Node diagram ：

  • 

• KITTI Example

  • download KITTI Odometry dataset Data set to Your dataset folder , Set up dataset_folder and sequence_number Parameters .
  • Please note that , You can also use the kitti_helper.launch Set appropriate parameters in to KITTI Data sets are converted to package files for ease of use .

Summary

advantage ：

• The code is indeed much simpler , If there is loam The foundation is looking at this , It's really clear at a glance .

Suggest ：

1. When calculating curvature , Between every line shift Not taken into account , But it doesn't affect , Because how many starting points of each line are skipped when extracting features

2. When filtering features based on curvature , Because the point cloud has been ordered based on curvature

   1. Corner point ：if The curvature should be greater than 0.1 establish , Otherwise directly break Is it not good? , Do not waste time after traversing
   2. Plane point ：if The curvature should be less than 0.1 establish , Otherwise directly break Is it not good? , Do not waste time after traversing

3. Make sure when you get close Straight line and plane I don't feel it lio-sam Medium and high precision

4. ceres Optimize , How about a manual derivation

rosbag read_write

• Reading and writing rosbag be used bag class , See the following example for specific use ：

//  Define read / write objects , And declare that 
rosbag::Bag i_bag, o_bag;
i_bag.open(src_bag, rosbag::bagmode::Read);
o_bag.open(new_bag, rosbag::bagmode::Write);

//  Define an array 
std::vector<std::string> topics;
topics.push_back(std::string(imu_topic));
topics.push_back(std::string(pcd_topic));
//  Affirming  rosbag::View object 
rosbag::View view(i_bag, rosbag::TopicQuery(topics));

///<  Read method 1 ：
for (auto m : view) {
      //  Traverse view
    sensor_msgs::Imu::ConstPtr imu = m.instantiate<sensor_msgs::Imu>();
    if (imu == nullptr) {
      //  Is there a imu
        std::cerr << "imu null " << std::endl;
    } else {
    
        std::cout << "imu stamp:" << imu->header.stamp << std::endl;
        o_bag.write(imu_topic, imu->header.stamp, imu);
    }
    sensor_msgs::PointCloud2::ConstPtr pcd =
        m.instantiate<sensor_msgs::PointCloud2>();
    if (pcd == nullptr) {
    
        std::cerr << "pcd null " << std::endl;
    } else {
    
        std::cout << "pcd stamp:" << pcd->header.stamp << std::endl;
        o_bag.write(pcd2_topic, pcd->header.stamp, pcd);
    }
}

///<  Read method 2 ：
rosbag::View view(bag);
for (const rosbag::ConnectionInfo* c : view.getConnections()) {
    

    const std::string& topic = c->topic;
    if (topic_to_publisher.count(topic) == 0) {
    
        ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum,
                                      c->datatype, c->msg_def);
        topic_to_publisher[topic] = node_handle.advertise(options);
    }
}

scanRegistration

The main function ：

• Ros node
  • ros Node initialization scanRegistration
  • Read parameters ：scan_line、minimum_range
• Only support 16,32,64 Linear velodyne, If not return
• Subscribe to the topic ：
  • /velodyne_pointslaserCloudHandler
• Post topics ：
  • sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
  • sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
  • sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
  • sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
  • sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
  • sensor_msgs::PointCloud2>("/laser_remove_points", 100);
• Based on parameters Determine whether to publish each line Normal does not execute

laserCloudHandler

• The last system initialization , Is simply how many frames to skip
• Point cloud rotation pcl Format ,pcl::removeNaNFromPointCloud
• Remove the nearest point ,removeClosedPointCloud
  • Traverse each point , seek Point to Laser distance , If the distance is less than the threshold , be continue
  • Otherwise, put the point into a new container
• Starting angle calculation ：
  • Starting angle ：startOri = -atan2(pointp[0].y,pointp[0].x)
  • End angle ：endOri = -atan2(pointp[size-1].y,pointp[size-1].x)+2π
  • if endOri-startOri > 3π endOri -= 2π
  • if endOri-startOri < 3π endOri += 2π
• Calculation scanID + relTime
  • scanID Which line , Good calculation
  • relTime Time difference from the initial point Calculate the horizontal angle with the initial point * cycle / Total angle
• Calculate the curvature of each point
  • Follow loam The calculation is the same
  • Between every line shift Not taken into account , But it doesn't affect , Because how many starting points of each line are skipped when extracting features
• Traverse Multi line N_SCANS, For each line , Divide the point cloud into 6 Share ：
  • Calculate the starting subscript of each copy ： sp,ep
  • Sort each point cloud according to the curvature from small to large std::sort function
  • Corner finding , Reverse traversal of ordered point clouds
    • If the curvature is greater than 0.1 And The surrounding points can be feature points when ：
      • Find the one with the largest curvature 2 Point in cornerPointsSharp Point cloud markers ：2
      • Find curvature maximal 20 Put in cornerPointsLessSharp Point cloud markers ：1
      • If the point is marked as a characteristic point , The surrounding 5 A point cannot be a feature point
  • Find a flat point , Traverse the ordered point cloud forward
    • If the curvature is less than 0.1 And The surrounding points can be feature points when ：
      • Find the one with the largest curvature 4 Point in surfPointsFlat Point cloud markers ：-1
      • If the point is marked as a characteristic point , The surrounding 5 A point cannot be a feature point
  • if Marked as -1 Put in surfPointsLessFlatScan

laserOdometry

The main function ：

• Ros node
  • ros Node initialization laserOdometry
  • Read parameters ：mapping_skip_frame Default 2
• Subscribe to the topic ：
  • /velodyne_cloud_2laserCloudFullResHandler
    • This point cloud goes to the nearest point and nan Of
    • This callback is also relatively simple , take Put the data in the queue
  • Four characteristics , Corner point , Corner point less, Plane point , Plane point less
    • These four callbacks are relatively simple , Just put the data in the queue
• Release ：
  • Corner point and Plane point
  • velodyne_cloud_3
  • laser_odom_to_init
  • laser_odom_path
• while The main thread ,ros::spin0nce,100hz
  • above 5 All kinds of data arrive And Take the first data when the time is the same
    • When 5 All kinds of data arrive , The time must be the same , atypism ros Report errors
  • 5 Kind of data pcl Data conversion
  • if systemInited== flase when ：
    • Direct assignment systemInited=true
  • otherwise , Optimize twice , Do the following for each time ：
    • The optimization variable is Postures para_q,para_t,4+3 A degree of freedom
    • Traverse corner features ：
      • Convert corners to odometer coordinate system ,TransformToStart
      • adopt kdTree find A straight line
        • adopt kdTree Find the nearest corner subscript in the previous frame pointSearchInd
        • This point subscript Up and down range The nearest point in is Another point
        • Two points form a straight line
      • adopt LidarEdgeFactor structure ceres Direct constraint
      • And add residuals
    • Traverse planar features ：
      • Convert the plane point to the odometer coordinate system ,TransformToStart
      • adopt kdTree find Plane
        • adopt kdTree Find the nearest corner subscript in the previous frame pointSearchInd
        • This point subscript Up and down range The nearest point in is Another point
        • same ring in Stretch forward Planar features as The third point
      • adopt LidarPlaneFactor structure ceres Direct constraint
      • And add residuals
  • Build solver ： iteration 4 Time ,QR decompose , Get the solution
  • to update q_w_curr and t_w_curr
  • to update On a frame Corner points and plane points kdTree

TransformToStart

• The relationship between the last odometer coordinate system is recorded , Directly convert according to the last relationship
• The idea that The initial value of the current frame is the same as that of the previous frame , Easy to calculate

LidarEdgeFactor

• The distance between a point and a line , Automatic derivation

LidarPlaneFactor

• Point to the distance of peace , Automatic derivation

laserMapping

The main function ：

• ros Node initialization
  • Define the node laserMapping
  • Read parameters ：
    • mapping_line_resolution 0.4
    • mapping_plane_resolution 0.8
• Subscribe to the topic ：
  • /laser_cloud_corner_lastlaserCloudCornerLastHandler
  • /laser_cloud_surf_lastlaserCloudSurfLastHandler
  • /laser_odom_to_initlaserOdometryHandler
  • /velodyne_cloud_3laserCloudFullResHandler
• Post topics ：
• The main thread mapping_process process

callback

• 3 Each laser callback puts the data into its own queue , There are mutually exclusive locks
• Laser odometer callback in addition to putting the data into the queue , It also sends data from the world coordinate system based on odometer drift

process

while 1 loop ,2s Do it once

• Data synchronization , Translates into pcl Format
  • if 3 Point cloud data and When odometer data is not empty ：
    • If a certain kind of comparison cornerLastBuf.front() Data early , be pop
  • Take the first data of each data , should The time of these four kinds of data is consistent
    • Sent by the same node , And the timestamp of the four kinds of data is the same
  • take 3 Kind of point cloud data through pcl Convert to respective objects , And from the queue pop_front
  • if cornerLastBuf When is not empty , pop Its data , Real time
• Get the pose of the current frame in the world coordinate system transformAssociateToMap()
  • ${}_L^{W}T{=}_O^{W}T{\ast }_L^{O}T$
• 1、 Optimize processing Find the current estimate lidar Which posture belongs to cube,I/J/K Corresponding cube The index of
  • cube Central location index ,50m The resolution of the , Initial value [10,10,5]
• 2、 If the current frame lidar The posture corresponds to cube In the whole big cube The edge moves the index one unit towards the center
  • width Direction ：centerCubeI and The central position index changes accordingly
    • centerCubeI stay width Small end of direction , Frame cube The pointer moves towards the center , namely i = i-1
    • centerCubeI stay width The big end of the direction , Frame cube The pointer moves towards the center , namely i = i+1
  • height Direction ：centerCubeJ and The central position index changes accordingly
    • centerCubeJ stay height Small end of direction , Frame cube The pointer moves towards the center , namely i = i-1
    • centerCubeJ stay height The big end of the direction , Frame cube The pointer moves towards the center , namely i = i+1
  • depth Direction ：centerCubeK and The central position index changes accordingly
    • centerCubeK stay depth Small end of direction , Frame cube The pointer moves towards the center , namely i = i-1
    • centerCubeK stay depth The big end of the direction , Frame cube The pointer moves towards the center , namely i = i+1
• 3、 take centerCube The surrounding point cloud is composed Local map
  • At present centerCube Of Expand in three directions 2 Each of them Valid array
  • take centerCube Surrounding valid arrays Corner point 、 The point cloud of the plane point atlas wants to add up
• 4、 The current frame Corner point and Plane point feature Down sampling
• 5、 Get the pose matching the current frame with the map
  • If meet The number of corner maps is greater than 10 And The number of points in the plane point map is greater than 50, To perform
  • take Map point cloud Put in kdtree Easy to find
  • Iterate twice solve ：
    • Traverse the current frame Corner features , structure ceres error model
      • Turn the current corner to the world coordinate system , And look for 5 A recent point
      • if 5 All points are less than 1m when
        • obtain 5 The covariance of points , And calculate its eigenvalue and eigenvector
        • If it is indeed a line feature , Build line feature model （ Note that the feature library sorts the feature values in ascending order ）
    • Traverse the current frame Pastry features , structure ceres error model
      • Go to the world coordinate system , And look for 5 A recent point
      • if 5 All points are less than 1m when , structure 5 Equation of points ： ax+by+c=1
      • If plane 5 The distance from each point to the plane is less than 0.2m, Prove that the plane holds , Build a face feature model
        • Build a face feature model
    • solve obtain Matching results
• 6、 After optimization , Update data
  • Update the pose
  • Update the corner in the current frame stay cube, And add the map
  • Update the current frame Plane point stay cubeI, And put it into the map
• 7、 Voxel filter the map Downsampling
• 8、 Publish corresponding data