Find absolute pose in map that aligns segment matches
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
absPoseMap = findPose(
SelectedSubmap property of
Optional Name-Value Arguments
[___] = findPose(___,
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
Lidar Localization Using Segment Matching
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.
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))
sMap — Map of segments and features
Map of segments and features, specified as a
refPose — Reference pose of last added view
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.
currentFeatures — Current features
M-element vector of
Current features, specified as an M-element vector of
currentSegments — Current segments
M-element vector of
Current segments, specified as an M-element vector of
Specify optional pairs of arguments as
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.
Before R2021a, use commas to separate each name and value, and enclose
Name in quotes.
'MatchThreshold',1.5 sets the matching threshold to 1.5
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
3. Decreasing this value can result in false
positives. If the number of detected inliers is less than
'MinNumInliers', the function returns an empty
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
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
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
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
absPoseMap — Absolute pose in the map
Absolute pose in the map, returned as a
rigid3d object. This object specifies the absolute pose that
aligns the segment matches.
matchViewId — View identifier containing most inlier 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.
inlierFeatures — Inlier features
N-by-2 matrix of
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
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
Removing the segments from the map using
deleteSegments, before using the
findPosefunction, can improve performance.
findPose finds the absolute pose of a segmented point cloud
using the SegMatch  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
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
SelectedSubmapproperty of the map.
The last added view corresponds to a loop closure when the
findPosefunction can estimate a valid geometric transformation. If the function cannot estimate this transformation, then the function returns an empty value for
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
pcviewsetobject as a connection using the
addConnectionobject function. Use the
optimizePosesfunction 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
currentFeaturesand the submap specified by the
sMap. If it cannot estimate a valid geometric transformation cannot be estimated, the function returns an empty value for the
Visualization — Use the
inlierSegmentsoutput arguments with the
pcshowMatchedFeaturesfunction to visualize the segment matches between the features and segments included in the map.
 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.