pcmapsegmatch

Map of segments and features for localization and loop closure detection

Since R2021a

Description

The pcmapsegmatch object creates a map of segments and features, and uses the segment matching (SegMatch [1]) algorithm for place recognition. This segment matching approach is robust to dynamic obstacles and reliable on large scale environments. The object stores the features, and segments, and their corresponding view IDs. Use the view IDs to link the features to a view in the point cloud view set object, pcviewset, for map building. pcmapsegmatch implements feature matching using eigenvalue-based features. It uses the Euclidean distance between segment features to find segment matches.

Creation

Syntax

sMap = pcmapsegmatch

sMap = pcmapsegmatch('CentroidDistance',dist)

Description

sMap = pcmapsegmatch returns a default pcmapsegmatch object. Use the addView object function to add views and their corresponding segments and features to the map.

sMap = pcmapsegmatch('CentroidDistance',dist) additionally specifies the minimum distance between segment centroids when adding segments and their corresponding features to the map. Segments with centroids closer than the specified distance dist, are not added to the map. dist is specified as a positive scalar with a default value of 0.1.

Properties

ViewIds — View identifier

M-element vector

This property is read-only.

View identifier, specified as an M-element vector of integers, where M is the number of views added to pcmapsegmatch.

Features — Feature vector

N-element vector of eigenFeature objects

This property is read-only.

Feature vector, specified as an N-element vector of eigenFeature objects, where N is the number of features.

Use the addView object function to add features for unique segments to the map. When you update the map using the updateMap object function, features that correspond to duplicate segments are removed from the map if they are within the CentroidDistance.

Segments — Point cloud segments

N-element vector of pointCloud objects

This property is read-only.

Point cloud segments, specified as an N-element vector of pointCloud objects, where N is the number of point cloud segments.

A segment is a group of 3-D points that are close together and represent a partial or full object.

SelectedSubmap — Currently selected submap

entire map (default) | 6-element vector

This property is read-only.

Currently selected submap, specified as a 6-element vector of the form [xmin,xmax ymin ymax zmin zmax] that describes the range of the submap along each axis. The elements of the vector describe the region of interest represented by the submap.

XLimits — Range of map along x-axis

2-element vector

This property is read-only.

Range of the map along the x-axis, specified as a 2-element vector of the form [xmin xmax].

YLimits — Range of map along the Y-axis

2-element vector

This property is read-only.

Range of the map along the Y-axis, specified as a 2-element vector of the form [ymin ymax].

ZLimits — Range of map along the z-axis

2-element vector

This property is read-only.

Range of the map along the z-axis, specified as a 2-element vector of the form [zmin zmax].

CentroidDistance — Minimum distance between segment centroids

positive scalar

This property is read-only.

Minimum distance between segment centroids, specified as a positive scalar. The object uses the minimum distance when adding segments and corresponding features to the map as unique segments and features.

Object Functions

addView	Add view to map
deleteView	Delete view from map
findView	Retrieve feature and segment indices corresponding to map view
hasView	Check if view is in the map
deleteSegments	Delete all segments in map
findPose	Find absolute pose in map that aligns segment matches
updateMap	Update centroid and point cloud segment locations in map
selectSubmap	Select submap within map
isInsideSubmap	Check if query position is inside selected submap
show	Visualize the point cloud segments in the map

Examples

Lidar Localization Using Segment Matching

Load a map of segments and features from a MAT file into the workspace. The point cloud data in the map has been collected using the Simulation 3D Lidar (UAV Toolbox) block.

data = load('segmatchMapFullParkingLot.mat');
sMap = data.segmatchMapFullParkingLot;

Load point cloud scans from a MAT file.

data = load('fullParkingLotData.mat');
ptCloudScans = data.fullParkingLotData;

Display the map of segments.

ax = show(sMap);

Change the viewing angle to top-view.

view(2)
pause(0.2)

Set the radius for selecting a cylindrical neighborhood.

outerCylinderRadius = 20;
innerCylinderRadius = 3;

Set the threshold parameters for segmentation.

distThreshold = 0.5;
angleThreshold = 180;

Set the size and submap threshold parameters for the selected submap

sz = [65 30 20];
submapThreshold = 10;

Set the radius parameter for visualization.

radius = 0.5;

Segment each point cloud and localize by finding segment matches.

for n = 1:numel(ptCloudScans)
    ptCloud = ptCloudScans(n);

    % Segment and remove the ground plane.
    groundPtsIdx = segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',11);
    ptCloud = select(ptCloud,~groundPtsIdx,'OutputSize','full');

    % Select the cylindrical neighborhood.
    dists = sqrt(ptCloud.Location(:,:,1).^2 + ptCloud.Location(:,:,2).^2);
    cylinderIdx = dists <= outerCylinderRadius & dists > innerCylinderRadius;
    ptCloud = select(ptCloud,cylinderIdx,'OutputSize','full');

    % Segment the point cloud.
    labels = segmentLidarData(ptCloud,distThreshold,angleThreshold,'NumClusterPoints',[50 5000]);

    % Extract features from the point cloud.
    [features,segments] = extractEigenFeatures(ptCloud,labels);

    % Localize by finding the absolute pose in the map that aligns the segment matches.
    [absPoseMap,~,inlierFeatures,inlierSegments] = findPose(sMap,features,segments);
    
    if isempty(absPoseMap)
        continue;
    end
 
    % Display the position estimate in the map.
    poseTranslation = absPoseMap.Translation;
    pos = [poseTranslation(1:2) radius];
    showShape('circle',pos,'Color','r','Parent',ax);
    pause(0.2)

    % Determine if the selected submap needs to be updated.
    [isInside,distToEdge] = isInsideSubmap(sMap,poseTranslation);
    needSelectSubmap = ~isInside ...                  % Current pose is outside submap
        || any(distToEdge(1:2) < submapThreshold) ... % Current pose is close to submap edge
        || n == 1;                                    % 1st time localizing using whole map

    % Select a new submap.
    if needSelectSubmap
        sMap = selectSubmap(sMap,poseTranslation,sz);
    end
 end

% Visualize the last segment matches.
figure; 
pcshowMatchedFeatures(inlierSegments(:,1),inlierSegments(:,2),inlierFeatures(:,1),inlierFeatures(:,2))

References

[1] Dube, Renaud, Daniel Dugas, Elena Stumm, Juan Nieto, Roland Siegwart, and Cesar Cadena. “SegMatch: Segment Based Place Recognition in 3D Point Clouds.” In 2017 IEEE International Conference on Robotics and Automation (ICRA), 5266–72. Singapore, Singapore: IEEE, 2017. https://doi.org/10.1109/ICRA.2017.7989618.

Version History

Introduced in R2021a