findPose

Find absolute pose in map that aligns segment matches

Since R2021a

Syntax

absPoseMap = findPose(sMap,refPose)

[absPoseMap,matchViewId] = findPose(sMap,refPose)

absPoseMap = findPose(sMap,currentFeatures)

absPoseMap = findPose(sMap,currentFeatures,currentSegments)

[___,inlierFeatures,inlierSegments] = findPose(___)

[___] = findPose(___,Name,Value)

Description

Map Building

absPoseMap = findPose(sMap,refPose) finds the absolute pose of the last added view that aligns the segment matches of the detected loop closure. The function looks for segment matches between the last added view and the segment features inside the submap specified by the SelectedSubmap property of sMap.

[absPoseMap,matchViewId] = findPose(sMap,refPose) returns the view identifier for the view that contains the most inliers. Use matchViewId to add the loop closure as a connection in a pcviewset, using the addConnection object function. Correct for accumulated drift using optimizePoses.

Localization

absPoseMap = findPose(sMap,currentFeatures) finds the absolute pose that aligns the segments that correspond to the current features currentFeatures to the segments in the submap specified by the SelectedSubmap property of sMap.

absPoseMap = findPose(sMap,currentFeatures,currentSegments) specifies the segments currentSegments that correspond to the current features currentFeatures.

Visualization

[___,inlierFeatures,inlierSegments] = findPose(___) returns the inlier features inlierFeatures and inlier segments inlierSegments in addition to any combination of arguments from previous syntaxes.

Optional Name-Value Arguments

[___] = findPose(___,Name,Value) specifies options using one or more name-value arguments in addition to the input arguments in previous syntaxes. For example, 'MaxThreshold',1.5 sets the matching threshold to 1.5 percent.

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

Input Arguments

sMap — Map of segments and features

pcmapsegmatch object

Map of segments and features, specified as a pcmapsegmatch object.

refPose — Reference pose of last added view

rigidtform3d object

Reference pose of the last added view, specified as a rigidtform3d object. The reference pose is the estimated absolute pose used to transform the point cloud from the sensor frame to the world frame for feature extraction.

currentFeatures — Current features

M-element vector of eigenFeature objects

Current features, specified as an M-element vector of eigenFeature objects.

currentSegments — Current segments

M-element vector of pointCloud objects

Current segments, specified as an M-element vector of pointCloud objects.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Example: MatchThreshold=1.5 sets the matching threshold to 1.5 percent.

Before R2021a, use commas to separate each name and value, and enclose Name in quotes.

Example: 'MatchThreshold',1.5 sets the matching threshold to 1.5 percent.

MatchThreshold — Matching threshold

1.5 (default) | scalar in range (0, 100]

Matching threshold, specified as a scalar in the range (0, 100]. The threshold is the maximum percentage of the distance from a perfect match. The function classifies segments are classified as possible matches if the distance between their feature vectors is lower than the threshold.

MinNumInliers — Minimum number of inliers

4 (default) | scalar

Minimum number of inliers, specified as a scalar greater than or equal to 3. Decreasing this value can result in false positives. If the number of detected inliers is less than 'MinNumInliers', the function returns an empty output for absPoseMap.

NumExcludedViews — Number of most recently added views to exclude

auto (default) | integer

Number of most recently added views to exclude, specified as an integer. For loop closure detection, exclude the most recently added views to avoid matches against the most recent features. Specify a larger value for this argument if many consecutive views correspond to the same area, such as scans from a slow-moving vehicle.

The function uses a default value of 10 for map building and 0 for localization.

MaxDistance — Maximum distance for inlier centroid match

1 (default) | positive numeric scalar

Maximum distance for inlier centroid match, specified as a positive numeric scalar. This value is the maximum distance that a centroid can differ from the projected location of its centroid match to be considered an inlier in the geometric verification step.

NumNearestNeighbor — Number of closest features selected as feature match candidates

100 (default) | positive integer

Number of closest features selected as feature match candidates, specified as a positive integer. For each feature in the last added view, or in the current features currentFeatures, the function selects the closest 'NumNearestNeighbor' features as candidate feature matches. Specify a larger value for this argument for maps with numerous similar features.

NumSelectedClusters — Number of feature clusters to check for matches

Inf (default) | positive integer

Number of feature clusters to check for matches, specified as a positive integer. The function clusters candidate features based on their centroid locations. If you specify refPose, then the findPose function selects the clusters closest to the centroids of the last added view currentFeatures. Decrease this value to improve performance at the expense of increasing the likelihood of false negatives.

Output Arguments

absPoseMap — Absolute pose in the map

rigidtform3d object

Absolute pose in the map, returned as a rigidtform3d object. This object specifies the absolute pose that aligns the segment matches.

matchViewId — View identifier containing most inlier matches

integer

View identifier containing the most inlier matches, returned as an integer. The inliers used to compute the absolute pose map can come from several views.

inlierFeatures — Inlier features

N-by-2 matrix of eigenFeature objects

Inlier features, returned as an N-by-2 matrix of eigenFeature objects. The first column corresponds to the inliers in the map, and the second column corresponds to the inliers in the last added view or the current features input.

inlierSegments — Inlier segments

N-by-2 matrix of pointCloud objects

Inlier segments, returned as an N-by-2 matrix of pointCloud objects. The first column corresponds to the inliers in the map, and the second column corresponds to the inliers in the last added view or the current segments input.

Tips

• Removing the segments from the map using deleteSegments, before using the findPose function, can improve performance.

Algorithms

findPose finds the absolute pose of a segmented point cloud using the SegMatch [1] algorithm for place recognition. It uses the Euclidean distance between segment features to find segment matches. The function finds the matches between the segments of interest and the segments in the map, and returns the absolute pose that aligns the segment matches in the map.

• Map Building: Loop Closure Detection — Loop closure starts with finding the absolute pose by finding the segment matches between the last added view and the segment features in the selected submap, which is specified by the SelectedSubmap property of the map.

  The last added view corresponds to a loop closure when the findPose function can estimate a valid geometric transformation. If the function cannot estimate this transformation, then the function returns an empty value for absPoseMap.

• Map Building: Correct Drift — To correct for drift, add the view that contains the most inliers for loop closure as a connection to the point cloud view set pcviewset object as a connection using the addConnection object function. Use the optimizePoses function to correct for accumulated drift.

• Localization — To find the absolute pose of the point cloud in the map, the function looks for segment matches between the current features currentFeatures and the submap specified by the SelectedSubmap property of sMap. If it cannot estimate a valid geometric transformation cannot be estimated, the function returns an empty value for the absPoseMap output argument.

• Visualization — Use the inlierFeatures and inlierSegments output arguments with the pcshowMatchedFeatures function to visualize the segment matches between the features and segments included in the map.

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

R2022b: Supports rigidtform3d objects

You can now specify refPose as a rigidtform3d object, which uses the premultiply convention. Although you can still specify refPose as a rigid3d object, this object is not recommended because it uses the postmultiply convention. For more information, see Migrate Geometric Transformations to Premultiply Convention.

When you use a syntax that includes the refPose argument, the findPose function returns absPoseMap as an object of the same type. When you use a syntax that includes the currentFeatures argument,the findPose function now returns absPoseMap as a rigidtform3d object. Before, the function returned absPoseMap as a rigid3d object.