Vehicle pose estimaton method, in-vehicle device and storage medium

Abstract

This application provides a vehicle pose estimation method, an in-vehicle device, and a storage medium. The method includes: using the point cloud of a first keyframe in a complex set of keyframes as a first point cloud; dividing the first point cloud into a complex set of voxels; determining the Gaussian distribution parameters corresponding to each voxel based on the first point cloud divided into each voxel, and determining the initial predicted pose of the first keyframe; using the point cloud of a second keyframe as a second point cloud, determining the correspondence between each point in the second point cloud and each voxel; determining the pose increment corresponding to the second keyframe based on the Gaussian distribution parameters of the voxels corresponding to each point in the second point cloud; and determining the current pose of the vehicle based on the pose increment and the initial predicted pose of the first keyframe. This method can improve the accuracy of vehicle pose estimation.

Claims

1. A vehicle pose estimation method, applied in in-vehicle equipment, wherein the improvement is that the method includes: The point cloud of the first keyframe in the complex keyframes is taken as the first point cloud, and the first point cloud is divided into a complex number of voxels. The first keyframe represents any keyframe in the complex keyframes. Based on the first point cloud divided into each voxel, the Gaussian distribution parameters corresponding to each voxel are determined, and the initial predicted pose of the first keyframe is determined. The point cloud of the second keyframe is taken as the second point cloud, and the correspondence between each point in the second point cloud and each voxel is determined, wherein the second keyframe represents the adjacent keyframe of the first keyframe in the complex keyframes. Based on the Gaussian distribution parameters of the voxels corresponding to each point in the second point cloud, the pose increment corresponding to the second keyframe is determined, including: determining the scoring function corresponding to each point in the second point cloud based on the Gaussian distribution parameters of each point and the voxels corresponding to each point; determining the gradient contribution and Hessian matrix corresponding to each point in the second point cloud based on the scoring function; obtaining a comprehensive gradient vector based on the gradient contribution and a comprehensive Hessian matrix based on the Hessian matrix corresponding to each point in the second point cloud; determining the pose increment corresponding to the second keyframe based on the comprehensive gradient vector and the comprehensive Hessian matrix; and determining the current pose of the vehicle based on the pose increment and the initial predicted pose of the first keyframe.

2. The vehicle pose estimation method according to claim 1, wherein, Determining the initial predicted pose of the first keyframe includes: predicting the initial predicted pose of the first keyframe based on the uniform velocity model and the temporally preceding adjacent frames of the first keyframe.

3. The vehicle pose estimation method according to claim 1, wherein, Determining the current pose of the vehicle based on the pose increment and the initial predicted pose of the first keyframe includes: determining the magnitude of the pose increment; if the magnitude is greater than or equal to a preset threshold, converting the pose increment into an incremental transformation matrix; updating the initial predicted pose based on the incremental transformation matrix to obtain the current pose of the vehicle.

4. The vehicle pose estimation method according to claim 3, wherein, The method further includes: if the module length is less than the preset threshold, determining the current pose of the vehicle based on the initial predicted pose.

5. The vehicle pose estimation method according to claim 1, wherein, Each voxel has a corresponding preset index. Dividing the first point cloud into multiple voxels includes: determining a world coordinate system based on a first coordinate system corresponding to the first point cloud; determining the index value of each point in the first point cloud in the world coordinate system based on the coordinate value of each point in the first point cloud in the world coordinate system and the side length of each voxel; determining the correspondence between each point in the first point cloud and each voxel based on the index value of each point in the first point cloud and the preset index of each voxel; and dividing each point in the first point cloud into the corresponding voxel based on the correspondence.

6. The vehicle pose estimation method according to claim 5, wherein, Determining the correspondence between each point in the second point cloud and each voxel includes: based on the initial predicted pose of the first keyframe, transforming the initial coordinates of each point in the second point cloud in the second coordinate system to the world coordinate system, where the second coordinate system represents the coordinate system corresponding to the second point cloud; determining the index value of each point in the second point cloud in the world coordinate system based on the coordinate value of each point in the second point cloud in the world coordinate system and the side length of each voxel; and determining the voxel corresponding to each point in the second point cloud according to the index value of each point in the second point cloud and the preset index of each voxel.

7. The vehicle pose estimation method according to claim 5, wherein, The step of determining the Gaussian distribution parameters corresponding to each voxel based on the first point cloud divided into each voxel includes: determining the mean vector corresponding to each voxel based on the average value of the coordinates of the points in the first point cloud within each voxel in the world coordinate system; calculating the covariance matrix corresponding to each voxel based on the coordinates of the points in the first point cloud within each voxel and the mean vector; and obtaining the Gaussian distribution parameters corresponding to each voxel based on the mean vector and the covariance matrix.

8. An in-vehicle device, the improvement of which includes: A storage device, a processor, and a computer program stored on the storage device and executable on the processor, wherein the processor, when executing the computer program, implements the vehicle pose estimation method as described in any one of claims 1 to 7.

9. A computer-readable storage medium, improved in that the computer-readable storage medium stores a computer program that, when executed by a processor, implements the vehicle pose estimation method as described in any one of claims 1 to 7.

Images