Main Content

findPose

Localize a point cloud within a map using the normal distributions transform (NDT) algorithm

Description

example

currPose = findPose(ndtMap,ptCloud,initPose) localizes the pose of the point cloud ptCloud within the NDT map ndtMap using the NDT algorithm. The function confines the search space to the submap, specified by the SelectedSubmap property of the ndtMap object.

currPose = findPose(___,Name,Value) specifies options using one or more name-value arguments in addition to the input arguments in previous syntax. For example, 'MaxIterations',30 sets the maximum number of iterations before the function stops the NDT algorithm.

Examples

collapse all

Load a normal distributions transform (NDT) map from a MAT file.

data = load('ndtMapParkingLot.mat');
ndtMap = data.ndtMapParkingLot;

Load point cloud scans and pose estimates from a second MAT file.

data = load('parkingLotData.mat');
ptCloudScans = data.parkingLotData.ptCloudScans;
initPoseEsts = data.parkingLotData.initPoseEsts;

Display the NDT map.

show(ndtMap)

Change the viewing angle to top-view.

view(2)

Select the submap centered around the first estimate.

center = initPoseEsts(1).Translation;
sz = [70 50 20];
ndtMap = selectSubmap(ndtMap,center,sz);

Set the radius for visualization of the current location and the distance threshold to update the submap.

radius = 0.5;
distThresh = 15;

Loop over the point clouds, localize them in the map, and update the selected submap as needed.

numScans   = numel(ptCloudScans);
  
for n = 1:numScans
    ptCloud = ptCloudScans(n);
    initPose = initPoseEsts(n);

    poseTranslation = initPose.Translation;
    [isInside,distToEdge] = isInsideSubmap(ndtMap,poseTranslation);
    submapNeedsUpdate = ~isInside ...       % Current pose is outside submap
        || any(distToEdge(1:2) < distThresh);   % Current pose is close to submap edge

if submapNeedsUpdate
    ndtMap = selectSubmap(ndtMap,poseTranslation,sz);
end

% Localize the point cloud scan in the map.
currPose = findPose(ndtMap,ptCloud,initPose);

% Display the position of the estimate as a circle.
pos = [currPose.Translation(1:2) radius]; 
showShape('circle',pos,'Color','r');

% Pause to view the change.
pause(0.05)
end

Figure contains an axes object. The axes object contains an object of type scatter.

Input Arguments

collapse all

NDT map, specified as a pcmapndt object.

Point cloud in the sensor coordinate system, specified as a pointCloud object.

Initial estimate for the pose of the sensor in the map, specified as a rigid3d object.

Name-Value Arguments

Specify optional comma-separated pairs of Name,Value arguments. Name is the argument name and Value is the corresponding value. Name must appear inside quotes. You can specify several name and value pair arguments in any order as Name1,Value1,...,NameN,ValueN.

Example: 'MaxIterations',30 stops the NDT algorithm after 30 iterations.

Expected percentage of outliers with respect to a normal distribution, specified as a scalar in the range [0, 1). The NDT algorithm assumes a point is generated by the mixture of a normal distribution for inliers and a uniform distribution for outliers. A larger value of 'OutlierRatio' reduces the influence of outliers.

Data Types: single | double

Maximum number of iterations before the NDT algorithm stops, specified as a nonnegative integer.

Data Types: single | double

Tolerance between consecutive NDT iterations, specified as a 2-element vector with nonnegative values. The vector, [Tdiff Rdiff], represents the tolerance of absolute difference in translation and rotation, respectively, estimated in consecutive NDT iterations. Tdiff measures the Euclidean distance between two translation vectors. Rdiff measures the angular difference in degrees. The algorithm stops when the difference between estimated rigid transformations in the most recent consecutive iterations falls below the specified tolerance values.

Data Types: single | double

Display progress information, specified as a logical 0 (false) or 1 (true). Set 'Verbose' to true to display progress information.

Data Types: logical

Output Arguments

collapse all

Pose of the sensor in the map, returned as a rigid3d object. The function confines the search space to the submap, specified by the SelectedSubmap property of ndtMap.

Tips

  • To improve the accuracy and efficiency of localization, consider downsampling the point cloud using pcdownsample before using this function.

References

Biber, P., and W. Strasser. “The Normal Distributions Transform: A New Approach to Laser Scan Matching.” In Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453) Vol. 3, 2743–48. Las Vegas, Nevada, USA: IEEE, 2003. https://doi.org/10.1109/IROS.2003.1249285.

[1] Magnusson, Martin. "The Three-Dimensional Normal-Distributions Transform: An Efficient Representation forRegistration, Surface Analysis, and Loop Detection." PhD thesis, Örebro universitet, 2009. http://urn.kb.se/resolve?urn=urn:nbn:se:oru:diva-8458 urn:nbn:se:oru:diva-8458.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Introduced in R2021a