Set the radius for visualization of the current location and the distance threshold to update the submap.
radius = 0.5;
distThresh = 15;
Loop over the point clouds, localize them in the map, and update the selected submap as needed.
numScans = numel(ptCloudScans);
for n = 1:numScans
ptCloud = ptCloudScans(n);
initPose = initPoseEsts(n);
poseTranslation = initPose.Translation;
[isInside,distToEdge] = isInsideSubmap(ndtMap,poseTranslation);
submapNeedsUpdate = ~isInside ... % Current pose is outside submap
|| any(distToEdge(1:2) < distThresh); % Current pose is close to submap edgeif submapNeedsUpdate
ndtMap = selectSubmap(ndtMap,poseTranslation,sz);
end% Localize the point cloud scan in the map.
currPose = findPose(ndtMap,ptCloud,initPose);
% Display the position of the estimate as a circle.
pos = [currPose.Translation(1:2) radius];
showShape('circle',pos,'Color','r');
% Pause to view the change.
pause(0.05)
end
NDT map, returned as a pcmapndt object with an updated
SelectedSubmap property.
If a region of the selected submap is outside of the limits of the map,
the selected submap is constrained to the map limits as described by the
XLimits, YLimits, and
ZLimits properties of the pcmapndt object.
Tips
Use a submap size large enough to include the uncertainty of the position
estimates and the range of the sensor used with findPose. A larger submap can increase computation time during
each call to the findPose function, but it can also reduce the frequency of
submap updates.
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window.
Web browsers do not support MATLAB commands.
Website auswählen
Wählen Sie eine Website aus, um übersetzte Inhalte (sofern verfügbar) sowie lokale Veranstaltungen und Angebote anzuzeigen. Auf der Grundlage Ihres Standorts empfehlen wir Ihnen die folgende Auswahl: .
Sie können auch eine Website aus der folgenden Liste auswählen:
So erhalten Sie die bestmögliche Leistung auf der Website
Wählen Sie für die bestmögliche Website-Leistung die Website für China (auf Chinesisch oder Englisch). Andere landesspezifische Websites von MathWorks sind für Besuche von Ihrem Standort aus nicht optimiert.