Table of contents

1. Project introduction

2. Interpretation of the paper LeGO-LOAM: Lightweight and ground-optimized variable terrain laser odometry and mapping

2.1 Summary

2.2 Introduction

2.3 Text Part 1 --- System Review

2.4 Text part 2 --- Segmatation (separation of ground points, corners and surface points)

2.5 Feature Extraction Text Part 3 --- Feature Extraction

2.6 Lidar Odom Main body part 4 --- front-end odometer

2.7 Liadr Mapping Text Part 5 --- Backend Optimization

3 Project configuration and compilation problem solving

1. Project introduction

        LeGO-LOAM is an improved version based on LOAM proposed by Tixiao Shan. It is mainly to realize the
positioning and mapping of the car under variable terrain. It has made a series of improvements for both the front end and the back end. Specifically, :
        Front-end
1. Classify and extract ground points, avoiding the extraction of some edge points
2. Apply a simple point cloud clustering algorithm to eliminate some possible outliers
3. Two-step iterative solution for front-end inter-frame mileage , reduce the calculation load without affecting the accuracy, and ensure the real-time performance of the embedded platform

        Backend
1. Restructured the backend part using the concept of key frames in slam
2. Introduced the concepts of loop closure detection and pose graph optimization to make the map more consistent globally
Finally, the 6D transformation between two consecutive scans is found         by fusing $[t_z, \theta_{roll},\theta_{pitch}]$ and summing. $[t_x,t_y,\theta_{yaw}]$ By using the proposed two-step optimization method, we observe that similar accuracies can be achieved with ~35% reduction in computation time (Table III).

2. Interpretation of the paper LeGO-LOAM: Lightweight and ground-optimized variable terrain laser odometry and mapping

2.1 Summary

Abstract—We propose a lightweight and ground-optimized laser odometry and mapping method, called LeGO-LOAM, for real-time six-degree-of-freedom attitude estimation of ground vehicles (requiring the radar to be mounted horizontally) . LeGO-LOAM is lightweight because it enables real-time pose estimation on low-power embedded systems. LeGO-LOAM is ground-optimized because it exploits the presence of a ground plane in the segmentation and optimization steps.

We first apply point cloud segmentation to filter out noise, and perform feature extraction to obtain salient planar and edge features. Then, a two-step Levenberg-Marquardt optimization method uses these planar and edge features to solve inter-frame odometry for six-degree-of-freedom transformations between consecutive scans. We compare the performance of LeGO-LOAM with the existing method LOAM using a dataset collected from ground vehicles in variable terrain environments, and show that LeGO-LOAM achieves similar or better accuracy while reducing computational cost. We also integrate LeGO-LOAM into the SLAM framework to remove pose estimation errors due to drift, which is tested using the KITTI dataset.

2.2 Introduction

        In this work, we pursue to achieve reliable, real-time 6DOF pose estimation for ground vehicles equipped with 3D lidar, in a manner suitable for efficient implementation in small-scale embedded systems. Such a task is not easy for a reason. Many unmanned ground vehicles (UGVs) do not have suspension systems or powerful computing units due to their limited size. When small UGVs drive on different terrains, they often encounter non-smooth motions (there are bumps during operation) , so the acquired data is often distorted. Reliable feature correspondence between two consecutive scans is also difficult to find because of the presence of large motions (larger jolts can err the motion association between two frames) due to the limited overlapping area . Furthermore, the massive amount of point cloud data received from 3D lidar poses challenges for real-time processing with limited onboard computing resources.

        When we use LOAM for this kind of task, we can obtain low-drift motion estimation when the unmanned ground vehicle (UGV) operates in a stable feature with smooth motion and has enough computing resources to support it . However, the performance of LOAM degrades when resources are limited. Due to the need to calculate the curvature of each point in a dense 3D point cloud, the update frequency of feature extraction on lightweight embedded systems may not always keep up with the update frequency of sensors. Operating UGVs in noisy environments also poses challenges for LOAM. Since lidars are often mounted close to the ground on small UGVs, sensor noise from the ground can be an ongoing problem. For example, grass reflections can result in high roughness values. Therefore, unreliable edge features may be extracted from these points. Likewise, it is also possible to extract edge or planar features from the points returned by the leaves. These features are generally unreliable for scan matching because the same blade of grass or leaf may not appear in two consecutive scans. Using these features may lead to inaccurate registration and large drift.

        Therefore, we propose a lightweight and ground-optimized LOAM (LeGO-LOAM) for estimating the pose of UGVs in complex variable terrain environments. LeGO-LOAM is lightweight because real-time pose estimation and mapping can be achieved on embedded systems. We perform point cloud segmentation to discard points that may represent unreliable features after ground separation. LeGO-LOAM is also ground-optimized because we introduce a two-step optimized pose estimation method. The planar features extracted from the ground are used for acquisition in the first step $[t_z,\theta_{roll},\theta_{pitch}]$ . In the second step, the remaining transformations are obtained by matching with the edge features extracted from the segmented point cloud $[t_x,t_y,\theta_{raw}]$ . We also incorporated the ability to perform loop closure to correct motion estimation drift. The remainder of this paper is organized as follows. The second section describes the hardware used for the experiments. Section III describes the proposed method in detail. Section IV conducts a series of experiments in various outdoor environments.

2.3 Text Part 1 --- System Review

        An overview of the proposed framework is shown in Figure 1. The system receives input from a 3D lidar and outputs a 6-DOF pose estimate . The overall system is divided into five modules.

        The first module is the segmentation module, which projects the point cloud of a single scan into a range image for segmentation.

        Then, the segmented point cloud is sent to the feature extraction module. Next, the laser odometry uses the features extracted by the previous module to find the transformation that correlates successive scans. These features are further processed in laser mapping to register them into a global point cloud map. Finally, the transform fusion module combines the pose estimation results of laser odometry and laser mapping, and outputs the final pose estimate. The proposed system seeks to improve the efficiency and accuracy of ground vehicles relative to the original generalized LOAM framework. Below is a detailed description of these modules.

        "6 DOF pose" is the ability to describe the position and orientation of an object in three-dimensional space. It includes translation along three axes and rotation around three axes, and is used for precise control of object motion and positioning in robotics, engineering, and computer graphics.

2.4 Text part 2 --- Segmatation (separation of ground points, corners and surface points)

$P_t =\{p_1,p_2,...,p_n\}$ Set as        $t$ the point cloud collected at time, where $p_i$ is $P_t$ a point in . First, $P_t$ is projected onto a range image. Since the horizontal and vertical angular resolutions of VLP-16 are 0.2° and 2° respectively, the resolution of the projected range image is 1800x16 (1800 points per line, 16 lines in total). $P_t$ Each valid point in is now represented by a unique pixel $p_i$ in the distance image . A distance value associated with a point represents the Euclidean distance from the corresponding point to the sensor. Due to the presence of sloping terrain in many environments, we do not assume that the ground is flat. Before segmentation, the range image is evaluated column-first, which can be viewed as ground plane estimation, to extract ground points. After this process, points that may represent the ground are marked as ground points and not used for segmentation. $p_i$ $r_i$ $p_i$

        Then, an image-based segmentation method is applied to the range image to divide the point cloud into many clusters. Points in the same cluster are given unique labels. It is important to note that ground points are a special type of clustering . Applying segmentation to point clouds can improve processing efficiency and accuracy of feature extraction. Assuming the robot operates in a noisy environment, small objects such as leaves may form trivial and unreliable features since the same leaf is unlikely to appear in two consecutive scans. For fast and reliable feature extraction using segmented point clouds, we ignore clusters with less than 30 points. Figure 2 shows the point cloud visualization before and after segmentation. The raw point cloud includes many points from surrounding vegetation that can produce unreliable features.

Feature extraction process for scans in noisy environments. The original point cloud is shown in Fig. (a). In Figure (b), red points are marked as ground points. The rest of the points are the ones kept after splitting. Only the trunk is kept. The leaf points are all removed. In Figure (c), the blue and yellow points represent the edge and plane features in F e and F p . In panel (d), green and pink dots represent edge and planar features in F e and F p , respectively.

        After this process, only points likely to represent large objects (such as tree trunks) and ground points are retained for further processing (as shown in Figure 2(b)) . At the same time, only these points are saved in the distance image. We also obtain three attributes for each point: (1) its label as a ground point or segmentation point, (2) its column and row index in the distance image, and (3) its distance value. These properties will be exploited in the following modules.

2.5 Feature Extraction Text Part 3 --- Feature Extraction

Instead of extracting features from raw point clouds, we extract features from ground points and segmentation points. Let S be the set of consecutive points from points pi at the same row distance from the image. Half of the points in S lie on either side of pi. In this article, we set |S| to 10. Using the distance values computed during segmentation, we can evaluate the roughness of the point pi in S,

To uniformly extract features from all directions, we divide the range image into several equal sub-images in the horizontal direction. Then, the points of each row in the subimage are sorted according to their roughness value c. Similar to LOAM, we use a threshold c th to distinguish different types of features. We call points with c greater than c th as edge features, and points with c less than c th as planar features. Then, select n F e edge feature points that do not belong to the ground with the largest c from each row of the sub-image. In the same way, select n F p planar feature points with the smallest c, which may be labeled as ground points or segmentation points. Let F e and F p be the set of edge and planar features in all sub-images, respectively. These features are visualized in Fig. 2(d). Then, n F e edge features that do not belong to the ground are extracted from each row of the sub-image with the largest c. Similarly, n Fp planar features with the smallest c are extracted from each row of the subimage, which must be ground points. Let F e and F p be the set of all edge and planar features in this process, respectively. Here, we split the 360° range image into 6 sub-images. Each subimage has a resolution of 300x16. Choose n F e , n F p , n F e , and n F p as 2, 4, 40, and 80, respectively.

2.6 Lidar Odom Main body part 4 --- front-end odometer

        The laser odometry module estimates the sensor motion between two consecutive scans. The transformation between the two scans was found by performing point-to-edge and point-to-plane scan matching. In other words, we need to find the features corresponding to this scan from the feature set of the previous scan.

        However, we note that some improvements can be made to improve the accuracy and efficiency of feature matching:

        1) Label matching: Since each feature in $F_{e}^t$ and $F_{p}^t$ is encoded as its label after segmentation, we only look for correspondences with the same label in $F_{e}^{t-1}$ and . $F_{p}^{t-1}$ For $F_{p}^t$ planar features in , $F_{p}^{t-1}$ only points labeled as ground points are used in to find a planar patch as a correspondence. For $F_{e}^t$ an edge feature in , its corresponding edge line $F_{e}^{t-1}$ is found in the segmentation cluster of . Finding correspondences in this way can improve the accuracy of matching. In other words, matching correspondences for the same object are more likely to be found between scans. This process also narrows down the set of potential candidates for correspondences.

        2) Two-step LM optimization: In [20], a series of non-linear expressions for the distances between edge and plane feature points in the current scan and their correspondences in the previous scan are compiled to form a comprehensive distance vector . Apply the Levenberg-Marquardt (LM) method to find the minimum distance transformation between two consecutive scans.

        We introduce here a two-step LM optimization method. Find the optimal transformation T in two steps:

        (1) Estimated by matching $F_{p}^t$ the planar features in and their $F_{e}^{t-1}$ correspondences in $[t_z, \theta_{roll},\theta_{pitch}]$

        (2) Then use $F_{e}^t$ the edge features in and their $F_{p}^{t-1}$ correspondences in to estimate the rest $[t_x,t_y,\theta_{yaw}]$ , while taking as $[t_z, \theta_{roll},\theta_{pitch}]$ a constraint. It should be noted that although $[t_x,t_y,\theta_{yaw}]$ they can also be obtained from the first step optimization, they are less accurate and are not used in the second step.

Finally, the 6D transformation between two consecutive scans is found         by fusing $[t_z, \theta_{roll},\theta_{pitch}]$ and summing. $[t_x,t_y,\theta_{yaw}]$ By using the proposed two-step optimization method, we observe that similar accuracies can be achieved with ~35% reduction in computation time (Table III).

Two-step optimization of the laser odometry module. [tz , θ roll , θ pitch ] are first obtained by matching planar features extracted from ground points. Then, [tx , ty , θ yaw ] is estimated using the edge features extracted from the segmentation points while [tz , θ roll , θ pitch ] are constrained.

2.7 Liadr Mapping Text Part 5 --- Backend Optimization

        The laser mapping module matches features $\{ F_e^{t}, F_e^{p} \}$ to the surrounding point cloud map $\bar{\mathbb{Q}}^{t-1}$ to further optimize the pose transformation, but runs less frequently. Then, use the LM method here again to get the final transformation.

        The main difference of LeGO-LOAM is how the final point cloud map is stored. Instead of saving a single point cloud map , we save each independent feature set $\{ F_e^{t}, F_e^{p} \}$ . Let be $M^{t-1} = \{ \{ F_e^1,F_p^1 \} ,...,\{ F_e^{t-1},F_p^{t-1} \} \}$ the set holding all previous feature sets. $M^{t-1}$ Each feature set in is also associated with the pose of the sensor at the time of scanning. Then, $\bar{\mathbb{Q}}^{t-1}$ it can be obtained $M^{t-1}$ from it in two ways.

        In the first method, $\bar{\mathbb{Q}}^{t-1}$ it is obtained by selecting the feature set within the field of view of the sensor. For simplicity, we can choose the feature set where the sensor pose is within 100 meters of the current sensor location. Then, the selected feature set is transformed and fused into a single surrounding map $\bar{\mathbb{Q}}^{t-1}$ .

        We can also integrate pose-graph SLAM into LeGO-LOAM. The sensor pose for each feature set can be modeled as a node in a pose graph. The feature set $\{ F_e^{t}, F_e^{p} \}$ can be viewed as the sensor measurements of this node. Since the pose estimation drift of the laser mapping module is very small, we can assume that there is no drift in a short period of time. In this way, can be formed by selecting a set of recent feature sets $\bar{\mathbb{Q}}^{t-1}$ , ie $Q^{t-1} = \{ \{ F_e^{t-k},F_p^{t-k} \} ,...,\{ F_e^{t-1},F_p^{t-1} \} \}$ , where k defines $\bar{\mathbb{Q}}^{t-1}$ the size of . Then, the transformation obtained after LM optimization can be used to add spatial constraints between the new node t−1 and the selected node in Q. We can also further remove drift in this module by performing loop closure detection. In this case, if there is a match between the current feature set and the previous feature set, a new constraint can be added using ICP (Iterative Closest Point). Then, the estimated pose of the sensor is updated by sending the pose graph to the optimization system.

3 Project configuration and compilation problem solving

        My computer is a Y9000K 2022 3080Ti computer, the Ubuntu version is 18.04.06, and the ROS version is ROS melodic.

        Lego-LOAM depends on the gtsam library. The author uses gtsam4.0.0, but the gtsam used by lio-sam and lvi-sam is 4.0.2. Here I install gtsam4.0.2 and it can still compile and run.

        When compiling, you will encounter the following problems:
/usr/include/flann/ext/lz4.h:249:72: error: conflicting declaration ‘typedef struct LZ4_streamDecode_t LZ4_streamDecode_t’
 typedef struct { unsigned long long table[LZ4_STREAMDECODESIZE_U64]; } LZ4_streamDecode_t;
The solution is to create a soft link:
sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak
sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4hc.h.bak

sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h
sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h
just run

LeGO-Loam code analysis (1) project introduction, paper interpretation, configuration and installation

1. Project introduction

2. Interpretation of the paper LeGO-LOAM: Lightweight and ground-optimized variable terrain laser odometry and mapping

2.1 Summary

2.2 Introduction

2.3 Text Part 1 --- System Review

2.4 Text part 2 --- Segmatation (separation of ground points, corners and surface points)

2.5 Feature Extraction Text Part 3 --- Feature Extraction

2.6 Lidar Odom Main body part 4 --- front-end odometer

2.7 Liadr Mapping Text Part 5 --- Backend Optimization

3 Project configuration and compilation problem solving

Guess you like