undistortEgoMotion

Undistort point cloud affected by ego motion

Since R2023a

Syntax

ptCloudOut = undistortEgoMotion(ptCloudIn,relTform,pointTimestamps,sweepTime)

Description

ptCloudOut = undistortEgoMotion(ptCloudIn,relTform,pointTimestamps,sweepTime) undistorts a point cloud, ptCloudIn, using the transformation relTform, the timestamp of each point pointTimestamps, and the lidar sweep time sweepTime.

undistortEgoMotion undistorts a point cloud affected by the motion of the sensor during the lidar sweep, assuming that the sensor sweeps at a constant speed when collecting the point cloud data. To undistort the point cloud, the function transforms the points in ptCloudIn back to where they would have been detected if the lidar sensor had not moved while completing the lidar sweep.

Examples

Undistort Point Cloud

Create a velodyneFileReader object, and read PCAP-formatted data into the workspace.

veloReader = velodyneFileReader("lidarData_ConstructionRoad.pcap","HDL32E");

Read the point cloud to undistort.

frameToUndistort = 38;
prevPtCloud = readFrame(veloReader,frameToUndistort - 1);
[ptCloud,pointTimestamps] = readFrame(veloReader,frameToUndistort);

Estimate the motion of the vehicle during the lidar sweep. The estimated motion can come from other sensors, such as an IMU or GPS. In this case, the estimated motion comes from point cloud registration.

gridStep = 1;
relTform = pcregisterloam(ptCloud,prevPtCloud,gridStep);

Undistort the point cloud.

startTime = veloReader.Timestamps(frameToUndistort);
endTime = veloReader.CurrentTime;
undistortedPtCloud = undistortEgoMotion(ptCloud,relTform,pointTimestamps,[startTime endTime]);

Visualize the point cloud before and after motion compensation.

figure
pcshowpair(ptCloud,undistortedPtCloud)
view(2)
hold on

Visualize where the Lidar sweep starts and ends with a red line.

plot3([0 0],[0 ptCloud.YLimits(2)],[0 0],"r",LineWidth=1)

Input Arguments

ptCloudIn — Input point cloud

pointCloud object

Input point cloud, specified as a pointCloud object.

relTform — Relative transformation

rigidtform3d object

Relative transformation, specified as a rigidtform3d object. The relTform transformation represents the relative motion of the sensor from the position where it ends the sweep to the position where it started the sweep.

pointTimestamps — Timestamp of each point

M-element vector of duration objects | M-by-N matrix of duration objects

Timestamp of each point, specified as an M-element vector or an M-by-N matrix of duration objects. The size of the pointTimestamps input depends on the size of the Location property of the ptCloudIn input.

Location Property	pointTimestamps Value
M-by-3 matrix	M-element vector
M-by-N-by-3 matrix	M-by-N matrix

sweepTime — Sweep start and end time

2-element vector of duration objects

Sweep start and end time, specified as a 2-element vector of duration objects of the form [startTime endTime].

Output Arguments

ptCloudOut — Undistorted point cloud

pointCloud object

Undistorted point cloud, returned as a pointCloud object. The size and type of the Location property of ptCloudOut is equal to the size and type of the Location property of ptCloudIn.

References

[1] Shoemake, Ken. " Animating Rotation with Quaternion Curves." ACM SIGGRAPH Computer Graphics 19, no. 3 (July 1985): 245–54.

Extended Capabilities

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

GPU Code Generation
Generate CUDA® code for NVIDIA® GPUs using GPU Coder™.

Version History

Introduced in R2023a