Main Content

findPose

Localize a point cloud within a map using the normal distributions transform (NDT) algorithm

所有的页面崩溃

Syntax

铜rrPose = findPose(ndtMap,ptCloud,initPose)
铜rrPose = findPose(___,Name,Value)

Description

example

铜rrPose= findPose(ndtMap,ptCloud,initPose)localizes the pose of the point cloudptCloudwithin the NDT mapndtMapusing the NDT algorithm. The function confines the search space to the submap, specified by theSelectedSubmapproperty of thendtMapobject.

铜rrPose= findPose(___,Name,Value)specifies options using one or more name-value arguments in addition to the input arguments in previous syntax. For example, 'MaxIterations',30sets the maximum number of iterations before the function stops the NDT algorithm.

Examples

collapse all

Open Live Script

Load a normal distributions transform (NDT) map from a MAT file.

data = load('ndtMapParkingLot.mat'); ndtMap = data.ndtMapParkingLot;

Load point cloud scans and pose estimates from a second MAT file.

data = load('parkingLotData.mat'); ptCloudScans = data.parkingLotData.ptCloudScans; initPoseEsts = data.parkingLotData.initPoseEsts;

Display the NDT map.

show(ndtMap)

Change the viewing angle to top-view.

view(2)

Select the submap centered around the first estimate.

center = initPoseEsts(1).Translation; sz = [70 50 20]; ndtMap = selectSubmap(ndtMap,center,sz);

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);forn = 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 edgeifsubmapNeedsUpdate ndtMap = selectSubmap(ndtMap,poseTranslation,sz);end% Localize the point cloud scan in the map.铜rrPose = 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

Figure contains an axes object. The axes object contains an object of type scatter.

Input Arguments

collapse all

NDT map, specified as apcmapndtobject.

Point cloud in the sensor coordinate system, specified as apointCloudobject.

Initial estimate for the pose of the sensor in the map, specified as arigid3dobject.

Name-Value Arguments

Specify optional pairs of arguments asName1=Value1,...,NameN=ValueN, whereNameis the argument name andValueis 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 encloseNamein quotes.

Example:'MaxIterations',30stops the NDT algorithm after 30 iterations.

Expected percentage of outliers with respect to a normal distribution, specified as a scalar in the range [0, 1). The NDT algorithm assumes a point is generated by the mixture of a normal distribution for inliers and a uniform distribution for outliers. A larger value of'OutlierRatio'reduces the influence of outliers.

Data Types:single|双

Maximum number of iterations before the NDT algorithm stops, specified as a nonnegative integer.

Data Types:single|双

Tolerance between consecutive NDT iterations, specified as a 2-element vector with nonnegative values. The vector, [TdiffRdiff], represents the tolerance of absolute difference in translation and rotation, respectively, estimated in consecutive NDT iterations.Tdiffmeasures the Euclidean distance between two translation vectors.Rdiffmeasures the angular difference in degrees. The algorithm stops when the difference between estimated rigid transformations in the most recent consecutive iterations falls below the specified tolerance values.

Data Types:single|双

Display progress information, specified as a logical0(false)或1(true). Set“详细”totrueto display progress information.

Data Types:logical

Output Arguments

collapse all

Pose of the sensor in the map, returned as arigid3dobject. The function confines the search space to the submap, specified by theSelectedSubmapproperty ofndtMap.

Tips

  • To improve the accuracy and efficiency of localization, consider downsampling the point cloud usingpcdownsamplebefore using this function.

References

Biber, P., and W. Strasser. “The Normal Distributions Transform: A New Approach to Laser Scan Matching.” InProceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453)Vol. 3, 2743–48. Las Vegas, Nevada, USA: IEEE, 2003. https://doi.org/10.1109/IROS.2003.1249285.

[1] Magnusson, Martin. "The Three-Dimensional Normal-Distributions Transform: An Efficient Representation forRegistration, Surface Analysis, and Loop Detection." PhD thesis, Örebro universitet, 2009. http://urn.kb.se/resolve?urn=urn:nbn:se:oru:diva-8458 urn:nbn:se:oru:diva-8458.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2021a

See Also

Objects

Functions