selectSubmap

Select submap within map

Since R2021a

Syntax

``ndtMapOut = selectSubmap(ndtMapIn,roi)``
``ndtMapOut = selectSubmap(ndtMapIn,center,sz)``

Description

example

````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

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```

Input Arguments

collapse all

NDT map, specified as a `pcmapndt` object.

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

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

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

Output Arguments

collapse all

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.

Version History

Introduced in R2021a