selectSubmap

Select submap within map

Since R2021a

Syntax

ndtMapOut = selectSubmap(ndtMapIn,roi)

ndtMapOut = selectSubmap(ndtMapIn,center,sz)

Description

ndtMapOut = selectSubmap(ndtMapIn,roi) selects a submap within the NDT map ndtMapIn using the specified region of interest roi.

Use this function to confine the search space for localization when using coarse position estimates.

ndtMapOut = selectSubmap(ndtMapIn,center,sz) selects a submap specified by the center center and size sz of the submap.

Examples

Lidar Localization Using NDT

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

Input Arguments

ndtMapIn — NDT map

pcmapndt object

NDT map, specified as a pcmapndt object.

roi — Region of interest

6-element vector

Region of interest, specified as a 6-element vector of the form [xmin xmax ymin ymax zmin zmax].

center — Center of submap

3-element vector

Center of the submap, specified as 3-element vector of the form [xc yc zc].

sz — Size of submap along each axis

3-element vector

Size of the submap along each axis, specified as 3-element vector of the form [xsz ysz zsz].

Output Arguments

ndtMapOut — NDT map

pcmapndt object

NDT map, returned as a pcmapndt object with an updated SelectedSubmap property.

If a region of the selected submap is outside of the limits of the map, the selected submap is constrained to the map limits as described by the XLimits, YLimits, and ZLimits properties of the pcmapndt object.

Tips

• Use a submap size large enough to include the uncertainty of the position estimates and the range of the sensor used with findPose. A larger submap can increase computation time during each call to the findPose function, but it can also reduce the frequency of submap updates.

Extended Capabilities

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

Version History

Introduced in R2021a