NDT localization workflow

Higher level workflow

1. Set input point cloud
   1. Copy the pointer to the pointcloud message
   1. Build the voxel grid

1. Calculate the initial guess:
   • No sensors:
     • Vehicle kinematic state is derived at the end of each ndt update using the transform and the time deltas. Only yaw is derived.
     • Initial guess is estimated using the velocities and acceleration depending on the motion model. (Either linear with velocity or quadratic with the acceleration added.)
   • IMU available:
     • Use the IMU data to have a translation(quadratic motion model) and orientation update.
     • Uses the derived velocity from the ndt update.
   • Vehicle odometry available:
     • Use odometry data to do a position update(constant velocity model + orientation update.)
   • IMU and Vehicle odometry available:
     • Identical to imu-only update, only use the velocity info from vehicle odometry instead.

1. Execute NDT optimization loop starting with the initial guess
   • See low level workflow

1. Set the vehicle pose state to the resulting base_link pose
   • Calculate the translation difference between the initial guess and the ndt estimate:
     • If it's bigger than a threshold, use the initial guess.
     • Else, use the ndt estimate.

1. Calculate the velocity and acceleration using this estimate
   • Mostly first level derivation by dividing the transform delta to the time delta.
   • Acceleration is estimated using the difference between the current and the previous velocity estimates.

1. Publish the estimates
   • Publish all of the initial guesses(despite only one of them should be getting calculated).
   • Publish the ndt estimate.
     • Published on the base_link and *Publish a reliability score

1. Add the transform to the frame graph.

Notes:

• A fixed transform is applied to the initial guess before ndt matching. The authors refer to the estimates with this transformation applied, the localizer estimates and when the estimate is transformed back to its original frame, it's called the ndt estimate.
  • This static transform is a global variable and is not read from the transform graph.

Lower Level Workflow

Follows the equations from this paper, section 6.2.

1. Compute the gauss parameters
   • The gaussian probability function is modified to account for outliers. See Eq. 6.7 and 6.8 on details of this.

1. Transform the scanned cloud using the initial guess.

1. Compute the gradients and Hessian analytically. See page 63 on the paper for exact mathematical details.
   1. Compute coefficients for computing the Jacobian and Hessian matrices. (Eq. 6.19)
   1. For each point in the translated point cloud:
      1. Search for a list of neighbors for a fixed radius in the voxel grid:
      1. For each neighboring voxel
         1. Compute the first and second order derivatives using the pre-calculated coefficients
            • Using the point derivatives and pre-computed gauss paramters, update the elements of the Hessian matrix.
            • Update the gradient of the NDT score function. (Eq. 6.12)

1. Using the Hessian and the probability score gradient, the pose update is calculated according to Newton's method.
   • $H\Delta p = -g$ : Here the $H$ is the hessian matrix, $\Delta p$ is the pose delta and $g$ is the gradient of the NDT score function that was calculated with the rest of derivatives in the previous steps.
   • This linear equation is solved by applying JacobiSVD to the Hessian matrix and then solving for the negative score gradient $-g$.

1. Normalize the pose delta

1. Calculate the step length
   • Moré-Thuente line search is used for estimating this parameter.
   • Derivatives are re-calculated during the step-size search at each iteration.
   • Hessian is updated after a step-length is selected.

1. Udate the pose

1. If the max number of iterations are reached or the step size is smaller than an epsilon, stop the loop. Else go to step 4.
   • The paper suggests an epsilon of 1e-6 but the implementation uses 0.1

Positive Impressions

• Concerns are somewhat separated on the algorithm level.
• Having a base class of registration that the NDT entity is derived from seems like the start of a good inheritance structure for future extensions.
• Parameter naming and calculations are taken from an actual paper making it easier to read, review and reproduce.

Negative Impressions

High level workflow:

• Minimal documentation/comments.
• Relies heavily on global variables that get modified by different static callbacks.
• Almost entire high level workflow of ndt localization occurs in the pointcloud callback without utilizing any helper functions.
  • Low readability.
  • Hard to distinguish separate functionality.
  • Interleaved steps.
• A lot of repeating code for different configurations/implementations.
• Somewhat cryptic parameter names.
• Vehicle kinematic state relies on differentiating two consecutive transforms over a time delta.

Low level workflow:

• Redundant operations occur in the run-time loop:
  • Parameter calculations could occur only when they are updated rather than on every computation.
• Optimization loop is interleaved with NDT specific steps.