Summary of various loams (laser SLAM)

To be continued

Why laser radar ？

Lidar phase ⽐ The image has the right lighting 、 The advantage of texture insensitivity , Lidar map phase ⽐ through ⽤ Visual feature points and descriptions ⼦ Maps have better stability , Laser radar ⽅ case ⽐ Vision ⽅ The scheme is more robust .

laser loam executive summary

• LOAM  Zhang Ji

LOAM The algorithm is a kind of laser matching slam Method , A new feature extraction method （ Edge points and plane points ）, Motion compensation （ Time stamp ）, The disadvantage is that there is no loopback detection , There is no factor graph optimization on the back end , Large environment mapping will produce drift , Can't handle large-scale rotation transformation .

Carnegie Mellon University Zhang Ji original ：https://github.com/laboshinl/loam_velodyne

Annotated version ：https://github.com/cuitaixiang/LOAM_NOTED

• A-LOAM  Qin Tong

A-LOAM It's Qin Tong of HKUST's comment on Zhang Ji LOAM The frame is reinforced by a laser SLAM frame . This framework makes ⽤Eigen as well as CeresSolver Original LOAM Into the ⾏ restructure , On the premise that the principle of the algorithm remains unchanged , The code framework ⾏ Optimize , Make the code ⼗ It is simple , Easier to read . Give up IMU Interface , meanwhile A-LOAM Used Eigen、Ceres The library is complete LM Optimization and positive and inverse solutions of Jacobian matrix , Replaced the original LOAM Manual implementation in code .

HKUST original ：https://github.com/HKUST-Aerial-Robotics/A-LOAM

Annotated version ：https://github.com/cgbcgb/A-LOAM-NOTED

• LeGO-LOAM TixiaoShan

Le Indicates lightweight （Lightweight）,GO Indicates ground based optimization （Ground-Optimized）;Lego-LOAM The front end is increased ⾯ Point extraction ; Loop detection and pose map optimization are added to the rear end ; Front and back ends LM Optimization optimizes the processing computation , Its computing speed increases ; Compare with LOAM Precision is not sacrificed ; Low requirements for equipment performance , Lightweight ; It is relatively stable when the ground points are abundant , It is easy to collapse when there is a lack of ground points ; The resulting map is sparse .

original edition ：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

Annotated version ：https://github.com/wykxwyc/LeGO-LOAM_NOTED

Improved version （ Better engineering ）：https://github.com/facontidavide/LeGO-LOAM-BOR

• FLOAM

FLOAM Is based on LOAM and ALOAM Modified version of , The calculation time is reduced 3 times , The accuracy is also improved . When the laser is blocked at close range, the effect is better than LeGo The effect is good .

Source code ：https://github.com/wh200720041/floam

• LIO-SAM TixiaoShan

LIO-SAM And IMU The close coupling is the front-end odometer and the GNSS Fit as global graph optimization ; It claims to run fast 10 times ; High requirements for sensors ,100hz+ Of 9 Axis IMU And with ring and time Channel lidar （velodyne and ouster）; Lidar inertial odometer based on factor graph , A large number of relative measurements can be 、 Absolute measured value 、 Loop and other different data are integrated into the laser radar inertial odometer system as factors ;IMU Preintegrated motion estimation is used to remove the motion distortion of lidar , It also provides the initial value for the optimization of lidar inertial odometer ; The results of the laser inertial odometer obtained are in turn used as estimates IMU The deviation of ; To ensure real-time and high performance , Some old lidar data are marginalized during pose optimization , Instead of matching the LIDAR point cloud to the entire map , Scanning and matching in local range instead of global range can effectively improve the real-time performance of the system ; Selective introduction of key frames and efficient sliding windows can also improve real-time performance ; First, the relative pose is calculated through the point cloud features , Then use the relative position and posture 、IMU Pre integral sum GPS Do fusion , Compared with direct one-step tight coupling , Greatly improved efficiency , And the measured performance is also excellent .

Source code ：https://github.com/TixiaoShan/LIO-SAM

Annotated version ：https://github.com/JokerJohn/opensource_slam_noted

• LVI-SAM TixiaoShan

LVI-SAM Add visual sensor .

• SC-LEGO-LOAM

stay Lego_LOAM Based on Scan_context Loop detection of , Other processes are exactly the same , The speed of loop detection is slightly improved .apt install libparmetis-dev

Source code ：https://github.com/irapkaist/SC-LeGO-LOAM.git

notes ：https://github.com/Black-Chocolate/sc_lego_loam_noted

Improve the situation ：​​​​​​ Outside 3D Mapping and positioning （ One ）Lego-Loam Several version tests _yuanguobin01 The blog of -CSDN Blog

SC-LEGO-LOAM Extension and deep parsing （ One ）
SC-LEGO-LOAM Extension and deep parsing （ Two ）
SC-LEGO-LOAM Extension and deep parsing （ 3、 ... and ）

Small FOV Solid state radar loam executive summary

• LIVOX-LOAM

in the light of livox lidar Developed loam Algorithm ,livox Solid state lidar , Different from the tradition velodyne Wait for the radar , Its field of vision is very small , It is easy to drift the feature points between frames , So you need to customize loam Algorithm . Feature extraction and selection in a very limited field of view , Robust outlier suppression , Moving object filtering and motion distortion compensation . Other functions are also integrated , Such as parallel pipeline 、 Point cloud management using cells and maps 、 Loop closed 、 Map saving and reloading utilities, etc .

loam_livox Introduction to code structure _ Dongfeng Xiaohuo's blog -CSDN Blog Analyze from the perspective of data flow loam_livox_ Dongfeng Xiaohuo's blog -CSDN Blog

Source code ：https://github.com/hku-mars/loam_livox

• livox_mapping

livox The company is based on LOAM_NOTED Developed algorithms , No loopback function .

Source code ：https://github.com/Livox-SDK/livox_mapping

Overview of optimization methods

• wave filtering

efk

ukf

• Graph optimization

g2o： Such as orb Back end

gtsam： Such as lio-sam Back end 、LegoLOAM Back end

• Nonlinear optimization （ least square ）

ceres：Aloam front end 、Aloam Back end

cv-solve：LegoLOAM front end 、LegoLOAM Back end scan2mapLM

ALoam

For a good article, see ：：https://www.cnblogs.com/wellp/p/8877990.html

feature extraction

• subscribe ： Initial lidar data
• Release ：
  • /velodyne_cloud_2          Ordered point cloud （ catalog index , Remove NaN value , Delete Lidar The origin of the coordinate system is too close ）
  • /laser_cloud_sharp          Set of maximal boundary points
  • /laser_cloud_less_sharp  Submaximal boundary point set （ Containing maximal boundary points ）
  • /laser_cloud_flat                Set of maximal plane points
  • /laser_cloud_less_flat        Submaximal plane point set （ Containing maximal plane points ）

• Calculate the horizontal angle of the point cloud , Make the point cloud orderly ： Find the corresponding scan line for each point （SCAN）; Assign time stamps to points on each scan line . The rear laser frame is basically unnecessary , The driving of the rear radar will change the wire number of each point 、 angle 、 The time stamps are all sent out .
• Feature point extraction method , Feature points are extracted according to the curvature of the points . That is, the particularly sharp edge points and the particularly flat plane points are taken as feature points .
• Curvature is obtained by taking the left and right sides of the target point on the same scanning line 5 A little bit , Make difference with the coordinates of the target point , The result is the curvature of the target point . When the target point is at the edge position , The difference from the surrounding points is large , Large curvature , It belongs to line feature ; On the contrary, when the target point is on the plane , The coordinates of the surrounding point and the target point are similar , This is a face feature .
• After the curvature is calculated, the feature is classified and extracted ： Each scan line is divided into 6 Sectors , Within each sector , Find the one with the largest curvature 20 A little bit , As a submaximal boundary point , The biggest of them 2 A little bit , At the same time, it serves as the maximum boundary point ; Find the one with the least curvature 4 A little bit , As a maximal plane point , The remaining unmarked points , All as submaximal plane points
• In order to prevent excessive accumulation of feature points , Every mention of （ Maximal edge point / Submaximal boundary point / Maximal plane point ）, Mark this point and all points near it as “ Selected ”, The next time feature points are extracted , These points will be skipped ; For submaximal plane points , Adopt the method of downsampling to avoid excessive stacking .
• The submaximal boundary point contains two maximal boundary points , The secondary large boundary point is the set of boundary points , The maximal boundary points are the best two in the set of boundary points .

front end odometry

• subscribe ： Ordered point cloud velodyne_cloud_2、 Maximal edge point 、 Submaximal boundary point 、 Maximal plane point 、 Submaximal plane point .
• Release ：
  • /laser_cloud_corner_last    Edge point of the previous frame
  • /laser_cloud_surf_last        Plane point of the previous frame
  • velodyne_cloud_3              Ordered point cloud （ from velodyne_cloud_2 The subscribed point cloud has not been processed ）
  • /laser_odom_to_init            Rough estimation of pose between frames
  • /laser_odom_path                Translational motion between frames for rviz
• Remove distortion caused by motion , No, IMU, Use the uniform velocity model to assume , send ⽤ On ⼀ Between frames ⾥ The result of the log is used as the motion between the current two frames , Suppose the current frame is also moving at a uniform speed , The position and posture of each point relative to the starting time can be estimated .
• Edge point matching method ： Based on the nearest neighbor principle, the association between edge points is established , Each maximal edge point goes to the sub maximal edge point of the previous frame to find a match , If it's in the first k+1 Edge points found in the frame i, adopt KD-tree The query is on page k The nearest neighbor in the frame j, Inquire about j The nearest neighbor on the scan line near l,j And l Connect to form a straight line l-j, Give way i The shortest distance from this line , Construct a nonlinear optimization problem , With a little i And straight lines lj Is the cost function , Change in position and posture 7 Quantity ( Quaternions represent rotation + Displacement ) To optimize variables .
• Plane point matching method ： If it's in the first k+1 Plane points are found in the frame i, adopt KD-tree The query is on page k frame （ On a frame ） The nearest neighbor in j, Inquire about j The nearest neighbor on the scan line near l And the nearest neighbor of the same scan line m, These three points define a plane , Give way i The shortest distance from this plane , Construct a nonlinear optimization problem , With a little i And plane lmj Is the cost function , Change in position and posture 7 Quantity ( Quaternions represent rotation + Displacement ) To optimize variables .
• After matching, you need to update the secondary large line points and face points KD-tree, For the next frame .
• ceres solve , Two least squares means two iterations ICP, The first initial value is R and T by （0,0,0,1） and （0,0,0）, The second time is the result of the first time .
• The least square is the sum of the squares of the residuals , Because there is square , So it's nonlinear .
• ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); Define it ceres Kernel function of , Use Huber Kernel function to reduce the influence of outer points , I.e. removal outliers.

Back end mapping

• subscribe ： All point clouds in the current frame ( After a downsampling )、 Set of edge points of the previous frame , A collection of plane points from the previous frame , Rough pose estimation of the current frame .
• Release ：
  • /laser_cloud_surround            near 5 Frame composition of the downcast image map for rviz
  • /laser_cloud_map                  A point cloud map of all frames
  • /aft_mapped_to_init                after Map to Map Fine estimation after optimization, the current frame pose is fine estimated
  • /velodyne_cloud_registered              The original point cloud of the current frame （ from velodyne_cloud_3 The subscribed point cloud has not been processed ）
  • /aft_mapped_to_init_high_frec          The position and attitude of odometer coordinate system is transformed into that of world coordinate system （ Map coordinate system ）,mapping Output 1Hz Postures ,odometry Output 10Hz Postures , Integrated into 10hz As a result
  • /aft_mapped_path after Map to Map  Fine estimate the current frame translation after optimization
• 
• “ Around 5 Sub map composed of frames submap Match all maps made up of all other frames “ This is wrong ！ It should be the feature point of the current frame and submap Match .
• Rough estimation according to the front-end pose , Convert odometer pose to map pose , Get an initial estimate of the back-end optimization , The point cloud of the current frame that has been removed from distortion is first converted to the global coordinate system and becomes Qk+1, Then with the local map or called submap Do feature matching , Turn the registered point cloud into Qk+1 Add a partial map Qk in .
• If the point cloud of all areas is used for matching , This kind of efficiency will be very low , And memory space may explode .LOAM The grid is used （cube） Map method , Divide the whole map into 21×21×11 A Sanger , Each Sanger is ⼀ One side ⻓50m Positive of ⽅ body , As the map builds up , The rest of Sanger was discarded , This ensures that the memory space will not run with the program ⾏⽽ Explode , While ensuring efficiency .
• Yes submap The management of is not a sliding window method based on time sequence , It's the way the space is divided , Present frame Corresponding submap Can include very early frames, such submap Method contains more historical frame information than sliding window method based on time sequence , It is more friendly for point cloud registration , This, of course, leads to an increase in memory usage .
• Then there is matching , That is, on the local map Qk Find the association of feature points in , stay odometry In the algorithm, , The correlation is determined for the fastest calculation speed , It uses the idea of nearest neighbor to find corresponding lines and corresponding surfaces ; and mapping The algorithm is based on the point cloud clusters around the feature points （ The nearest neighbor 5 Points as point cloud clusters ） Conduct PCA Principal component analysis （ Find the eigenvalue and eigenvector of the covariance matrix of the point cloud cluster ）, To find the direction vector of the edge and the normal vector of the face .
  • Find all map feature points , Of the current feature point 5 Nearest neighbors .
  • If it is a boundary point , Then take the center of mass of these five points , With 5 The principal direction vector of points （ Be similar to PCA Method ） For the direction , Make a straight line , Make the distance between the edge point and the straight line the shortest , Construct a nonlinear optimization problem
  • If it is a plane point , Then find the normal direction of five points （ In reverse PCA Method ）, Let the distance between this plane point and the five nearest neighbors in the normal direction be the smallest , Construct a nonlinear optimization problem .
  • ceres Solve twice ICP,kdtree Search is the same as front-end optimization .

Back end mapping： Integrate front-end output

• mapping Output 1Hz Postures ,odometry Output 10Hz Postures , Need to be integrated into 10hz As a result
• The process ： When mapping Calculate a pose Pmap after , Just the one in sync with it odometry Postures Podom Calculation odom The incremental △P = Pmap * Podom-1, And then use △P To correct the subsequent high frequency odometry Postures , Revised 10Hz High frequency pose is LOAM The final output of the real-time pose , Until then mapping The algorithm then calculates a pose and repeats the above process .
• △P = Pmap * Podom-1 ：-1 Is inverse , namely  Podom*△P=Pmap , therefore △P  Represents that the current frame does not occur Podom when mapping value

LeGO-LOAM

For a good article, see ：https://zhuanlan.zhihu.com/p/382460472;blog.csdn.net/m0_50610065/article/details/123834057

Point cloud preprocessing ： Projection + Division Projection+Segmentation

• subscribe ： Initial lidar data
• Release ：
  • /full_cloud_projected                      Ordered point cloud （intensity Is row number column number , That is, the integer part is the harness value , The decimal part is the horizontal angle 0~1799）
  • /full_cloud_info                                Ordered point cloud （intensity It's distance range）
  • /ground_cloud                                Point clouds on the ground
  • /segmented_cloud                          Split point cloud （ Including ground ）
  • /segmented_cloud_pure                Split point cloud （ Excluding the ground ）
  • /segmented_cloud_info                  Split point cloud labels （ Custom message ）
  • /outlier_cloud                                  The point cloud that failed to split , That is, non clustered points
• copyPointCloud;copy Point cloud to pcl Mark Ring index, If not velodyne Of lidar Later, we will calculate which harness the point cloud belongs to , Prepare for projection
• findStartEndAngle(); Find the direction angle of the start time and the end time , It is useful to remove distortion later ;
• projectPointCloud(); Calculated distance （Range）, Project a point cloud onto RangeImage On , That is to say, the original point cloud is a sphere , We project the sphere onto a cylinder and expand it . With 16 Line laser as an example , The conversion range chart , High for 16, The length is the number of samples rotated for one cycle , Here for 1800, So the end result is 16*1800 Image . This image can be represented as an array , Where the coordinates are x,y, and z Will be converted to depth information , Finally, the converted rangeImage The effect is as follows , Similar to a depth map .  The processing of laser point cloud here is LEGO-LOAM and LOAM A difference of . The coordinate information of the point cloud is stored in intensity in , The method used is ： The integer part stores the laser harness value , The horizontal heading value stored in the decimal part （0~1799）.​
• groundRemoval(); Perform ground extraction , Yes rangeImage Bank of China Ring Less than 7 Ground detection at the point of , If it is 32 Line radar , It is Ring Less than 20 Of ; Judge rangeImage in A And adjacent points B The included angle with the horizontal direction ɑ Less than 10° Is the ground .
  ​
• cloudSegmentation(); After removing the ground, segment the remaining point cloud , Yes rangeImage from [0,0] PM , adopt BFS（ Depth first ） Search recursively , Before traversing it 、 after 、 Left 、 On the right 4 A little bit , Compare them separately ;A and B yes Lidar Continuous scanning 2 A point cloud , If A and B The angle between β Less than 60°, Red line 2 Points do not belong to the same object , On the contrary, the green line can be regarded as the same object , This clustering method is used for segmentation ; To improve efficiency and reliability , The number of segmented point clouds is greater than 30 Clusters of are considered effective ;5 More than points and the number of horizontal lines is greater than 3 The cluster is also considered effective .
  
• publishCloud(); Finally, the point cloud is ordered ros Release ： Column number + Line number * 1800 namely points[columnIdn + rowIdn * Horizon_SCAN]
• Split point cloud labels （ Custom message ）：
  • int32[N_SCAN] startRingIndex
  • int32[N_SCAN] endRingIndex
  • float32 startOrientation
  • float32 endOrientation
  • float32 orientationDiff
  • bool[N_SCAN*Horizon_SCAN]   segmentedCloudGroundFlag    Whether it is a ground point
  • uint32[N_SCAN*Horizon_SCAN]  segmentedCloudColInd          column index（0~1799）
  • float32[N_SCAN*Horizon_SCAN] segmentedCloudRange          Distance information

front end Odometry：Feature Extraction+Feature Association

• subscribe ： Non clustered points 、 Split point cloud 、 Split point cloud labels （ Custom message ）、imu Original information
• Release ：
  • /laser_cloud_sharp          Maximum edge point set for rviz
  • /laser_cloud_less_sharp  Submaximal boundary point set is used for rviz
  • /laser_cloud_flat                The maximal set of planar points is used for rviz
  • /laser_cloud_less_flat        Submaximal plane point sets are used for rviz
  • /laser_cloud_corner_last    Set of edge points
  • /laser_cloud_surf_last        Set of plane points
  • /outlier_cloud_last              Set of far points and discrete points
  • /laser_odom_to_init            Radar odometer rough estimation
• Take the segmented point cloud for feature extraction , The purpose of the extracted features is to perform point cloud registration , So as to get the current pose .
• imu To deal with ：IMU Angle conversion ; Right position 、 Integrating velocity and angle , Calculate the current position 、 Speed and angle .
• Feature Extraction： and ALoam The feature extraction part is similar
  • utilize imu Integral to remove motion distortion adjustDistortion();
  • Calculate curvature smoothness to determine plane points and edge points calculateSmoothness();
  • Mark the blocking point markOccludedPoints();ALoam There is no such part in
    
    The first type of block point ： Such as (a) Medium B As you can see from this point , Although it is on a plane and can be identified as a plane point , But that plane is basically parallel to the laser beam , It is very likely that it will not be visible in the next scan, so it needs to be excluded . Judge by the distance difference between two adjacent points , If the distance is greater than 2cm The plane points of are excluded , The more parallel the laser beam is, the farther the two points are .
    The second type of block point ： Such as (b) Medium A As you can see from this point , Such points will be recognized as edge points , But such points may be plane points , Because the orange part caused by occlusion is not scanned, it is misjudged as a boundary point . Judge by the distance difference between two adjacent points , Distance greater than 30cm The edge points of are excluded .
  • Extract line features and face features extractFeatures();
  • Publish feature point clouds for rviz publishCloud();
• Feature Association：
  • utilize imu Update initial guess location updateInitialGuess(), The following least square method must have an initial value to solve the nonlinear problem ;
  • Calculate the relative pose transformation between two frames updateTransformation();
    Pose reasoning of ground points and boundary points , Estimate the variables of three degrees of freedom respectively ,cv::solve Solving nonlinear problems , The partial derivative is 0 Find the extreme value at , The nature of the problem is also ICP：
    • Use ground points to optimize pitch,roll,z： The ground points are more consistent with the characteristics of the surface , Therefore, the optimization problem of ground points is constructed by using point-to-face constraints , But the constraint pairs between ground points x,y and yaw These three degrees of freedom have little effect , When the values of these three degrees of freedom change , Point to face residuals do not change significantly , therefore , The optimization between ground points will only pitch,roll as well as z To constrain and optimize , Detailed calculation process LEGO-LOAM The source code parsing --- FeatureAssociation node (4) - You know
    • Optimize with corners x,y,yaw： Because the corners extracted by multi line lidar are usually vertical edge features , Like a telegraph pole , These characteristics are right for x、y as well as yaw It has good observability , Detailed calculation process LEGO-LOAM The source code parsing --- FeatureAssociation node (3) - You know
  • Integral relative pose , Update pose integrateTransformation();
  • Release radar odometer message publishOdometry();
  • Released for mapOptimization Point cloud of   publishCloudsLast();

Back end Mapping ： Graph optimization +LM

• Yes 2 Two back-end methods ：
  • The first way ： Obtain the feature set within the field of view of the sensor , By looking away from the current position 100 Point cloud map within meters . Match the scanned point cloud of the current frame with the point cloud map .
  • The second way ： By creating the attitude map (Pose-Graph), Of feature sets per frame pose Can be used as a node in the attitude graph , The feature can be regarded as the sensor measurement of the node .
    Because the attitude drift of laser mapping is very low , Suppose there is no drift in a short time , Select the nearest set of features , The space constraint between the new node and the nearest node can be used isam Optimize , Get the pose transformation .
    Vertices represent optimization variables , The edge represents the error term ; The vertex represents the pose , Edges represent constraints .
• LOAM Use the first method ,LeGO-LOAM Combine in two ways .
• LOAM Save the point cloud map ,LeGO-LOAM The feature point cloud set of each frame is saved .
• LeGO-LOAM Match the line and surface features of the point cloud scanned in the current frame with the point cloud map of the surrounding key frames , Further optimize pose changes , Lower frequency operation . The process ：
  • 1. The switch to map Coordinate system  transformAssociateToMap().
  • 2. Extract the surrounding key frames and downsample the key frame features extractSurroundingKeyFrames().
    If loop back detection is used for graph optimization , Then extract the last moment 50 Key frames ; If loop back detection is not used , Add the nearest to the current European style 50 Key frames are spliced into a point cloud .
    Yes Key frame downsampling , The main function is to facilitate the subsequent graph optimization process , Improve the speed of graph optimization .
  • 3. Downsampling the current frame feature downsampleCurrentScan().
  • 4. Current frame to map The optimization of the  scan2MapOptimization()
    With The first way is to optimize , similar ICP, At most 10 Suboptimization , Repeat the following process ：
    a. Create from the surrounding key cloud kdtree（ Corner feature point cloud 、 Surface feature point cloud ）;
    b. from kdtree Mid search Of the current feature point 5 Nearest neighbors ;
    c. If it is a boundary point , Then take the center of mass of these five points , With 5 The principal direction vector of points （ Be similar to PCA Method ） For the direction , Make a straight line , Make the distance between the edge point and the straight line the shortest , Construct a nonlinear optimization problem ;
        If it is a plane point , Then find the normal direction of five points （ In reverse PCA Method ）, Let the distance between this plane point and the five nearest neighbors in the normal direction be the smallest , Construct a nonlinear optimization problem ;
    d. use  cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR) Conduct LM Optimize ,QR decompose （cv::DECOMP_QR） Or singular value decomposition （cv::DECOMP_SVD） Can solve overdetermined （ Singular matrix ） Linear systems .
  • 5. Keyframes Graph optimization  saveKeyFramesAndFactor()
    With The second way is to optimize , If a new key frame is added , You need to calculate the constraints between the current frame and the previous frame , This constraint can be considered as a small loop , Add to the back end to optimize , The optimized result is used as the pose and point cloud of the key frame , And then synchronize to scan2Map In the process ：
    a. Set the current key frame pose Join in gtsam graph;
    b. to update iSAM
    c. Graph optimization isam->calculateEstimate() And save the key poses;
    d. Save updated transform
    Key frame or not ： The distance between the current frame and the previous frame is greater than 0.3 rice , namely 0.3m A small loop graph optimization .
  • 6. Correct the pose ： Independent thread loopback detection if successful , To do this , Replace the optimized pose with the previous one .
  • 7. Publish coordinate conversion 、 Keyframes 、 Posture .

Back end Mapping ： Loop back detection

• The cumulative error is further eliminated by loop back detection , Loop back detection is a thread on the back end loopClosureThread() Independent execution ：
  • detectLoopClosure Determine whether to loop back ： The distance between the head and the tail is less than 7 rice , And the time difference 30s above
  • If looping is carried out ：
    a. through too ICP The algorithm compares whether the current frame matches the previous frame ;
    b. If it matches, add a new constraint to isam;
    c. And then through iSAM2 To optimize the attitude map .
• The author also reminds us of the loopback detection ICP The algorithm often fails when the odometer drift is too large , For more advanced closed-loop methods , The proposal USES SC-LeGO-LOAM , It features point cloud descriptors .