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
[___] = 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
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
Current features, specified as an M-element vector of
currentSegments— Current segments
Current segments, specified as an M-element vector of
comma-separated pairs of
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
'MatchThreshold',1.5sets 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
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
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
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
function, can improve performance.
findPose finds the absolute pose of a segmented point cloud
using the SegMatch  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
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
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
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
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
absPoseMap output argument.
Visualization — Use the
inlierSegments output arguments with the
pcshowMatchedFeatures function 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.