Main Content

undistortEgoMotion

Undistort point cloud affected by ego motion

Since R2023a

collapse all in page

    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.

    example

    Examples

    collapse all

    Open Live Script

    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)

    Figure contains an axes object. The axes object contains 3 objects of type scatter, line.

    Input Arguments

    collapse all

    Input point cloud, specified as a pointCloud 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.

    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 PropertypointTimestamps Value
    M-by-3 matrixM-element vector
    M-by-N-by-3 matrixM-by-N matrix

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

    Output Arguments

    collapse all

    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

    See Also

    Functions

    Objects