LOAM

LOAM (A-LOAM)

Note: The following introduces only LiDAR-related algorithms, and omits the IMU-related component in the original LOAM algorithm.

The algorithm consists of three parts: Scan Registration (pre-processing, feature extraction), Odometry (high speed front end), and Mapping (low speed back end). The input sensing data flows through three components one by one via ROS topics.

Scan Registration

• For each new scan received, transform from ROS msg into PCL point cloud, remove outlier points, compute and save metadata, rectify the point cloud if possible.

• For each point in the point cloud, compute its curvature as follows.

float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
            + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
            + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
            + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
            + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
            + laserCloud->points[i + 5].x;
float diffY = ...;  float diffZ = ...;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;

• For each scan line/beam, divide the point cloud into six sectors (each takes 1/6 of start-end index diff), and extract sharp (corner) and flat (plane/surface) features as follows.

  • In each sector, sort cloud index according to curvature. The top 2 largest curvature points (if points are not selected and curvature > 0.1) are marked as sharp, the top 20 largest curvature points are marked as less sharp (including the top 2 sharp points), the top 4 smallest curvature points (with additional condition curvature < 0.1) are marked as flat, and all the rest points plus the top 4 flat points will be down sampled and marked as less flat.

  • After each feature extraction, there will be a non-maximum suppression step to mark neighbor 10 points (+-5) to be already selected (marking will stop at points that are 0.05 squared distance away from currently picked point), so that they will not be picked in the next iteration for feature extraction.

  • For example for VLP-16 LiDAR, there could be 384 flat feature points and 192 sharp feature points extracted in a point cloud frame (if all meet the 0.1 curvature threshold).

  • A downsample operation (of leaf size 0.2m) is applied to each scan of less flat points, and then all downsampled scans are combined together (into one point cloud) to be published to odometry.

Odometry

• We use TransformToStart and TransformToEnd to undistort points, such that they can be treated as being captured at the beginning/end of a scan. (TransformToStart is useful in the second round of optimization. In the first round of optimization, TransformToStart is using the estimation from last frame, being applied in this frame assuming an uniform motion.)

• Synchronize the ROS topics received for sharp, less sharp, flat, less flat points. (Pick the one that shares the timestamp with others.) Run Ceres solver twice to quickly compute a rough estimate of the current pose (10Hz).

  • For each sharp point in the current frame, run TransformToStart and find two closest (3D distance) points in the previous less sharp frame as a line correspondence. One is found by Kd-tree search, and the other is found by line search in nearby scans, based on the distance to this kd-tree found one. Two points must be found in two different lines/scans. Then compute point-to-line distance and add this residual to Ceres solver. See LOAM paper Fig. 7(a) for details.

  • For each flat point in the current frame, run TransformToStart and find three closest (3D distance) points in the previous less flat frame as a plane correspondence. One is found by Kd-tree search, and the other two is found by line search. Two points should be on the same line/scan, and the third one should be on a different (but neighbor) line/scan. Then we compute point-to-plane distance and add this residual to Ceres solver. See LOAM paper Fig. 7(b) for details.

  • Solve the transformation by Ceres, update feature points stored in the last frame, and publish to ROS topics.

Mapping

• Point cloud map is maintained manually using array of pointers (of type pcl::PointCloud).

  • Since the array index must always be positive, we shift the center of the map by an offset in all x, y, z axes.

  • In each iteration, if the current map boundary is about to reach the maximum map boundary, we shift the whole map to make it away. This shifting is in fact done in a smart way that we only shift the pointers to the point cloud, not the actual points.

  • The entire map is divided into cubes of 50m, and the maximum map boundary is set to allow for 21 cubes in x, y axes and 11 cubes in z axis. (1050 meter horizontal square.)

• Synchronize incoming ROS msgs, adjust map and enable only nearby feature points on the map, downsample them and run Ceres solver twice.

  • High-level idea: Perform SVD for each feature point. For line features, one eigenvalue should be significantly greater than the other two. For plane features, there should be two large eigenvalues with the third one significantly smaller. Compute residual for line and plane features, stack into Ceres solver and solve them together in the end. Update map corner/surface clouds using estimated transformation.

  • Details: Receive less sharp points (of the last frame) from odometry, and downsample them with a leaf size of 0.2m. For each point p in this cloud, run PointAssociateToMap to obtain pm . Find 5 points closest to pm from map corner cloud. Compute the center of 5 points, and estimate the line/edge direction by picking the largest eigenvalue of the covariance matrix of the 5 points. Compute two artificial points by adding +- 0.1 offset to the center point and feed them together with the original point p to Ceres functor.

  • Details: Receive less flat points (of the last frame) from odometry, and downsample them with a leaf size of 0.4m. For each point p in this cloud, run PointAssociateToMap to obtain pm . Find 5 points closest to pm from map corner cloud and organize them into a 5-by-3 matrix M. Find the normal vector n by solving a system of linear equations M * n = -1. Finally, feed the original point p, normalized normal vector, and the reciprocal of the norm of the (non-unit) normal vector to Ceres functor. (See below plane fitting algorithms for details.)

  • Two outlier checks apply here. 1) Skip point p if the squared distance between point pm and its 5th neighbor is greater than one. 2) Skip point p if the line/plane correspondence is not healthy. For line/edge features, the largest eigenvalue should be at least 3 times greater than the second largest eigenvalue. For plane features, the dot product between any point fitted to the plane (5 neighbor points of pm) and the (non-unit) normal vector, should be greater than 0.2 (an experimental threshold) minus the reciprocal of the norm of the (non-unit) normal vector.

Math

The overall cost function consists of two parts: sum of all point-to-line distances and sum of all point-to-plane distances. Mathematically,

$$f(\mathbf{x})=\sum_i d_\mathcal{E}(p_i(\mathbf{x})) + \sum_j d_\mathcal{H}(p_j(\mathbf{x}))$$

Point-to-line Distance

The area of a parallelogram can be computed by two ways: 1) the cross product of OA and OB (and then take the norm), and 2) AB multiplied by OD. Therefore, we can compute OD by |OA x OB|/|AB|. This is the approached adopted in the algorithm, where point O is available in the current frame, and two points A and B are the correspondence in the previous frame.

Point-to-plane Distance

If we have access to the normal vector of a plane, then the point-to-plane distance can be readily computed by the dot product with the normal vector. Specifically, O is the point out of the plane, and another point in the plane is known (for example A). Then the distance between O and the plane is the dot product between AO and the (normalized/unit) normal vector of the plane.

In the Odometry algorithm, where we have access to three points on the plane, the normal vector of the plane can be computed by a cross product (i.e. for any three points A, B, and C, just do ABxAC). In the Mapping algorithm, where we have access to more then three (e.g., five) points on the plane, the normal vector can be computed by SVD.

Plane Fitting Algorithms

Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.

The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]

Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]

We can set up a system of linear equations accordingly, and then solve it by an Eigen solver in C++ as follows.

// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();

// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);

// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
  if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
    planeValid = false; // 0.2 is an experimental threshold; can be tuned
    break;
  }
}

References: Wikipedia: Distance_from_a_point_to_a_line; Distances to lines and planes; Finding the normal to a plane (cross product for three points); Lesson Explainer: Equation of a Plane: Vector, Scalar, and General Forms; Stackoverflow: 3D Least Squares Plane; Stackoverflow: How to fit a plane to a 3D point cloud?;

Code Organization

• In LOAM, they use float array transform[6] and transformSum[6] for local and global transformation respectively. Optimization is solved by their code.

• In A-LOAM, they use Eigen::Quaterniond q_w_curr for rotation and Eigen::Vector3d t_w_curr for translation. Optimization is solved using Ceres.

• ROS Topics (Scan Registration --> Odometry)

  • /laser_cloud_sharp feature cloud, current frame, used in current optimization

  • /laser_cloud_less_sharp feature cloud, current frame, not used, saved as corner_last

  • /laser_cloud_flat feature cloud, current frame, used in current optimization

  • /laser_cloud_less_flat feature cloud, current frame, not used, saved as surf_last

  • /velodyne_cloud_2 full laser cloud, current frame

• ROS Topics (Odometry --> Mapping)

  • /laser_cloud_corner_last same as /laser_cloud_less_sharp in the last frame

  • /laser_cloud_surf_last same as /laser_cloud_less_flat in the last frame

  • /velodyne_cloud_3 full laser cloud, current frame, same as velodyne_cloud_2

  • /laser_odom_to_init latest pose estimation (odom world to robot q/t_odom_body)

Transformation Updates in A-LOAM

// laserOdometry.cpp
// two coordinates: q/t_w_curr and q/t_last_curr
// in each iteration, update global variable w_curr using local variable last_curr

// initialization
Eigen::Quaterniond q_w_curr(1, 0, 0, 0); // w, x, y, z for initialization
Eigen::Vector3d t_w_curr(0, 0, 0);
double para_q[4] = {0, 0, 0, 1}; // x, y, z, w for internal storage
double para_t[3] = {0, 0, 0};    // array format for Ceres solver
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

void TransformToStart(pi, po) {
  // assuming no distortion and s=1 by default
  q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
  t_point_last = s * t_last_curr;
  po = q_point_last * pi + t_point_last;
}

while (ros::ok()) {
  ros::spinOnce();
  // check message buffers and synchronize timestamps
  // run optimization twice

    // TransformToStart (cornerPointsSharp and surfPointsFlat)
    // solve for para_q and para_t using Ceres solver

  // transform update
  t_w_curr = t_w_curr + q_w_curr * t_last_curr;
  q_w_curr = q_w_curr * q_last_curr;

  // publish ros topics
}

// laserMapping.cpp
// three coordinates: w_curr, wmap_wodom, wodom_curr

// initialization
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);
// wmap_T_odom * odom_T_curr = wmap_T_curr;
// transformation between odom's world and map's world frame
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0); // w, x, y, z for initialization
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);

void pointAssociateToMap(pi, po) {
  Eigen::Vector3d po = q_w_curr * pi + t_w_curr;
}

while (ros::ok()) {
  // check message buffers and synchronize timestamps
  // update q_wodom_curr and t_wodom_curr from laserOdometry ROS topic
  
  // transformAssociateToMap() as follows --> set initial guess for Ceres
  q_w_curr = q_wmap_wodom * q_wodom_curr;
  t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
  
  // map management: shift map boundary
  // run optimization twice

    // pointAssociateToMap (laserCloudCornerStack and laserCloudSurfStack)
    // solve for q_w_curr and t_w_curr using Ceres solver

  // transformUpdate() as follows --> save map_odom transformation
  q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
  t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;

  // save maps using pointAssociateToMap
  // publish ros topics
}

LeGO-LOAM

This project is optimized specifically for a mobile robot operating on the ground, and for multi-beam LiDAR such as Velodyne. (The original LOAM algorithm uses a 2D LiDAR operating in 3D space with a 6 DoF pose.)

Image Projection

• Act as if we project Velodyne points back to an image. Use 2D indices and matrices to mark, label and segment Velodyne points (row, col = 16x1800), but the actual points are saved in 1D pcl::PointCloud structure in an organized way.

• Remove outliers, find start and end orientation of current point cloud, find the row and column index and compute the depth for each point.

• Find and label ground points as follows. For points in lower scans (scan ID 0-7 in VLP-16), compute the inclination angle between current point and the point in the same column but with a level higher scan ID. If this angle is less then 10 degree, then mark both points to be ground points.

• Segment cloud as follows. Starts with label #1, for each point, compute the tangent angle between current point and the horizontal or vertical neighbor point, and assign the same label to neighbor points if this angle is less than 30 degree. In other words, this will grow the current segment if points tend to be in the same tangent plane (wrt LiDAR center). Then increment the label number and proceed to the next unlabeled point.

Odometry and Mapping

• Most parts remain the same as in the original LOAM algorithm. One addition is that GTSAM is applied to refine the estimation output from the mapping module.

Loop Closure

• If there exists any keypose in the history that is closer than 7m in distance and greater than 30s in time with respect to the current keypose, we will match this keypose with the current keypose using the ICP algorithm provided in PCL. If succeeded, we add this relation/observation of two keyposes to GTSAM.

• This is a naive method and can be improved by other algorithms such as ScanContext.

Observations

Geometric features and degeneration

• Lines/edges are 1 dof (1 trans) features in 3D space; they can be used to estimate any motion that is perpendicular to the line. We have tolerance on the estimation error of this landmark along its direction.

• Planes are 3 dof (2 trans + 1 rot) features in 3D space; they can be used to estimate any motion that is (at least partially) perpendicular to the plane. We can tolerate any estimation error of this landmark inside its 3 dof plane.

• Objects are 6 dof (3 trans + 3 rot) features in 3D space; they can be used to estimate motion towards any direction. However, the tradeoff is that any estimation error of the object itself will be carried on to the estimation of the robot pose.

• As a special case of the plane, points on the ground are not useful for the translation estimation on the ground (because motion is in parallel to the plane), but they are helpful to bound any possible disturbance that happens in the z axis. This is true if the ground is rigid and flat, otherwise they are noises. (e.g., floor vs. grassland)

• Case 1: apply 0-2m filter on z axis. If no features are available from ground or ceiling, then estimation in the z axis can have large errors. The degeneration happens in the z axis since all features (lines and planes) are in parallel to the z axis.

• Case 2: infinite long corridor. Our forward motion is in parallel to all features (planes) around us. Degeneration happens along the moving direction.

• Case 3: infinite large ground surface. No landmarks around are available except for a ground plane. Any moving direction on the x-y plane is degenerative since it is in parallel to the plane.

Example LiDAR Odometry Module (Taken from Lego-LOAM)

• Note that in the diagram below, planar features are used to estimate tz, roll and pitch only, whereas edge features are used to estimate tx, ty, and yaw. (This odometry algorithm targets at scenarios where mobile robots operating on the ground.)