Find absolute pose of points in map
specifies the size of the local map used to refine the absolute pose, in addition to all
input arguments from the previous syntax. When you do not specify
absPose = findPose(___,
localMapSize, the function uses a map size defined by the
x, y, and z spatial extents of
all the input points enlarged on all sides by a spatial radius,
SearchRadius, centered around the estimated absolute pose
returns the optimized relative pose.
optimizedRelPose] = findPose(___)
returns the root mean squared error of the Euclidean distance between the aligned points.
Lower values indicate a more accurate alignment.
rmse] = findPose(___)
[___] = findPose(___,Name=Value) specifies
options using one or more name-value arguments in addition to any combination of arguments
from previous syntaxes. For example,
SearchRadius=4 sets the search
radius for point matching to
Find Absolute Pose of Points in LOAM Map
Create a map to store LOAM feature points.
voxelSize = 0.1; loamMap = pcmaploam(voxelSize);
velodyneFileReader object to read point cloud daata.
veloReader = velodyneFileReader("lidarData_ConstructionRoad.pcap","HDL32E");
Read the first point cloud into the workspace.
ptCloud1 = readFrame(veloReader,1);
Detect LOAM feature points.
points1 = detectLOAMFeatures(ptCloud1);
Downsample the less planar surface points to improve registration speed.
gridStep = 1; points1 = downsampleLessPlanar(points1,gridStep);
Add the LOAM points of the first point cloud to the map.
absPose = rigidtform3d; addPoints(loamMap,points1,absPose)
Read the fifth point cloud, and detect the LOAM feature points in it.
ptCloud2 = readFrame(veloReader,5); points2 = detectLOAMFeatures(ptCloud2);
Downsample the less planar surface points.
points2 = downsampleLessPlanar(points2,gridStep);
Get a relative pose estimate by using the
relPose = pcregisterloam(points2,points1);
Find the absolute pose of the points from the fifth point cloud in the map.
absPose = findPose(loamMap,points2,relPose);
Add the points from the fifth point cloud to the map.
Visualize the map.
relPose — Relative pose
Relative pose, specified as a
relPose input contains the relative pose between the
new input points and the last points added to the LOAM map
in the sensor frame. You can use the
pcregisterloam function to estimate the relative pose.
localMapSize — Local map size
Local map size, specified as a three-element vector of the form
dz]. When you do not specify
function uses a map size defined by the x, y, and
z spatial extents of all the input points enlarged on all sides by
a spatial radius,
SearchRadius, centered around the estimated
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.
findPose(loamMap,points,relPose,SearchRadius=4) sets the search radius
for point matching to
SearchRadius — Search radius for point matching
3 (default) | positive integer
Search radius for point matching, specified as a positive integer. When matching,
the function finds the closest edge and surface points within the specified radius.
For best results, set this value based on the certainty of the relative pose,
specified by the
relPose input. You can increase the value of
SearchRadius when there is greater uncertainty in the relative
pose input, but this can also decrease the registration speed.
MaxIterations — Maximum iterations before LOAM registration stops
20 (default) | positive integer
Maximum iterations before LOAM registration stops, specified as a positive integer.
Tolerance — Tolerance between consecutive LOAM iterations
[0.01,0.5] (default) | two-element vector
Tolerance between consecutive LOAM iterations, specified as a two-element vector, with nonnegative values, of the form [Tdiff Rdiff].
Tdiff — Tolerance for the estimated absolute difference in translation and rotation between consecutive LOAM iterations. Measures the Euclidean distance between two translation vectors.
Rdiff — Tolerance for the estimated absolute difference of the angular rotation between consecutive iterations, measured in degrees.
The algorithm stops when the difference between the estimates of the rigid transformations in the most recent consecutive iteration falls below the specified tolerance value.
Verbose — Display progress information
0 (default) |
Display progress information, specified as a numeric or logical
true). To display progress information, set
absPose — Absolute pose
Absolute pose for aligning new points to the existing points in a LOAM map, returned
function uses the absolute pose to align new points to the existing map.
optimizedRelPose — Optimized relative pose
Optimized relative pose, returned as a
rmse — Root mean squared error
Root mean squared error, returned as a positive scalar that described the Euclidean distance between the aligned points. Lower values indicate a more accurate alignment.
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Introduced in R2022b