Main Content

pcmapsegmatch

Map of segments and features for localization and loop closure detection

Since R2021a

expand all in page

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.

example

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

expand all

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.

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.

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.

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.

This property is read-only.

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

This property is read-only.

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

This property is read-only.

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

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

addViewAdd view to map
deleteViewDelete view from map
findViewRetrieve feature and segment indices corresponding to map view
hasViewCheck if view is in the map
deleteSegmentsDelete all segments in map
findPoseFind absolute pose in map that aligns segment matches
updateMapUpdate centroid and point cloud segment locations in map
selectSubmapSelect submap within map
isInsideSubmapCheck if query position is inside selected submap
showVisualize the point cloud segments in the map

Examples

collapse all

Open Live Script

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

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

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

Figure contains an axes object. The axes object contains 5 objects of type scatter, line. One or more of the lines displays its values using only markers

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

See Also

Functions

Objects

Topics