Main Content

findPose

Find absolute pose in map that aligns segment matches

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

collapse all

Load a map of segments and features from a MAT file. 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.

Input Arguments

collapse all

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

Reference pose of the last added view, specified as a rigid3d 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.

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

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

Name-Value Arguments

Specify optional comma-separated pairs of Name,Value arguments. Name is the argument name and Value is the corresponding value. Name must appear inside quotes. You can specify several name and value pair arguments in any order as Name1,Value1,...,NameN,ValueN.

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

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.

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 .

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.

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.

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.

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

collapse all

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

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.

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.

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

Introduced in R2021a