|
Autoware.Auto
|
|
This package contains structures and functionality to allow localization with 3D lidar scans using Normal distribution transform matching. See this article for further details of the implementation.
NDTMap, similar to voxel_grid, is based on an std::unordered_map and allows fast lookup given a point.
NDT voxel map has 2 main types: DynamicNDTMap and StaticNDTMap.
DynamicNDTMap transforms a raw point cloud message into an NDT map representation by passing the points to their corresponding voxels. Each voxel's centroid and covariance gets computed with respect to the points that fall inside it. Covariance and centroid computation is done online with respect to Welford's algorithm .
Once DynamicNDTMap transforms a dense point cloud into voxels, each voxel's centroid and covariance can be serialized into a PointCloud2 message where the resulting point cloud is sparse and only an intermediate representation of a transformed map. This point cloud can then be converted back into an NDT map representation via StaticNDTMap where no centroid/covariance is computed but only the points are inserted into their corresponding voxels for lookup. Input point cloud is validated via validate_pcl_map() function before converting it into the map representation.
Inputs:
Outputs:
An NDT scan is a data structure to represent a lidar scan. The implementations depend on the optimization problem. All implementations must provide basic iterating abilities which is necessary for all ndt optimization problems.
P2DNDTScan is a wrapper around a vector of points(Eigen::Vector3d). The class allows iterating through the internal container by exposing the iterators of its vector.
The algorithm is defined in detail in the paper "The Three-Dimensional Normal-Distributions Transform" [Magnusson 2009]. The actual implementation is based on the PCL and autoware.ai implementations.
A CachedExpression is used to represent the optimization problem. As a result, score, jacobian and hessian are given the option to be computed all computed together to make use of the synergy stemming from the shared terms within the computation.
Inputs:
NDTLocalizerBase is a type of RelativeLocalizerBase. It is templated on scan, map and optimization problem types, allowing to register measurements using varying ndt methods. An implementation of NDTLocalizerBase can choose to override the message and guess validations as well as covariance computation steps.
P2DNDTLocalizer is the NDTLocalizerBase implementation for P2D NDT objective.
Inputs: