Systems and methods for odometry enhanced real-time cooperative relative pose estimation for cooperative LIDAR perception
System, methods, and other embodiments described herein relate to cooperative perception. In one embodiment, a method includes computing, at a first timestep, a base relative pose between an ego vehicle and a remote vehicle based upon respective point clouds of the ego vehicle and the remote vehicle. The method includes computing, at a second timestep, a relative pose between the ego vehicle and the remote vehicle based upon the base relative pose, a first temporal relative pose of the ego vehicle, and a second temporal relative pose received from the remote vehicle. The method includes generating a combined point cloud based upon a first point cloud of the ego vehicle, a second point cloud received from the remote vehicle, and the relative pose.
1 . A computing system for cooperative perception between an ego vehicle and a remote vehicle, the computing system comprising:
a processor; and
memory communicably coupled to the processor that stores instructions that, when executed by the processor, cause the processor to:
compute, at a first timestep, a base relative pose between the ego vehicle and the remote vehicle based upon respective point clouds of the ego vehicle and the remote vehicle;
compute, at a second timestep, a computed relative pose between the ego vehicle and the remote vehicle based upon the base relative pose, a first temporal relative pose of the ego vehicle, and a second temporal relative pose received from the remote vehicle, wherein the first temporal relative pose and the second temporal relative pose are respectively indicative of motion changes of the ego vehicle and the remote vehicle between the second timestep and the first timestep;
generate a combined point cloud based upon a first point cloud of the ego vehicle, a second point cloud received from the remote vehicle, and the computed relative pose; and
control the ego vehicle based upon the combined point cloud.
2 . The computing system of claim 1 , wherein the instructions fer to compute the base relative pose between the ego vehicle and the remote vehicle comprise further instructions that cause the processor to compute the base relative pose at a first frequency, and wherein the instructions further cause the processor to:
compute the first temporal relative pose based upon odometry data of the ego vehicle, wherein the first temporal relative pose is computed at a second frequency; and
receive the second temporal relative pose from the remote vehicle, wherein the second temporal relative pose is computed at the second frequency.
3 . The computing system of claim 2 , wherein the first frequency is less than the second frequency.
4 . The computing system of claim 1 , wherein the instructions to compute the base relative pose between the ego vehicle and the remote vehicle comprises further instructions that cause the processor to apply an iterative closest point (ICP) algorithm to the respective point clouds, and wherein the base relative pose is generated as a by-product of the ICP algorithm.
5 . The computing system of claim 1 , wherein the first temporal relative pose comprises a first translation vector and a first rotation matrix, wherein the second temporal relative pose comprises a second translation vector and a second rotation matrix, and wherein the base relative pose comprises a base translation vector and a base rotation matrix.
6 . The computing system of claim 5 , wherein the instructions to compute the computed relative pose between the ego vehicle and the remote vehicle comprise further instructions that cause the processor to:
generate a translation vector based upon the base translation vector summed with a difference between the first translation vector and the second translation vector; and
generate a rotation matrix based upon the base rotation matrix multiplied by a quotient of the first rotation matrix and the second rotation matrix, wherein the computed relative pose comprises the translation vector and the rotation matrix.
7 . The computing system of claim 6 , wherein the instructions to generate the combined point cloud comprise further instructions that cause the processor to:
generate a transformed point cloud based upon the translation vector, the rotation matrix, and the second point cloud; and
stitch the first point cloud to the transformed point cloud.
8 . A non-transitory computer-readable medium for cooperative perception between an ego vehicle and a remote vehicle and including instructions that, when executed by a processor, cause the processor to:
compute, at a first timestep, a base relative pose between the ego vehicle and the remote vehicle based upon respective point clouds of the ego vehicle and the remote vehicle;
compute, at a second timestep, a computed relative pose between the ego vehicle and the remote vehicle based upon the base relative pose, a first temporal relative pose of the ego vehicle, and a second temporal relative pose received from the remote vehicle, wherein the first temporal relative pose and the second temporal relative pose are respectively indicative of motion changes of the ego vehicle and the remote vehicle between the second timestep and the first timestep;
generate a combined point cloud based upon a first point cloud of the ego vehicle, a second point cloud received from the remote vehicle, and the computed relative pose; and
control the ego vehicle based upon the combined point cloud.
9 . The non-transitory computer-readable medium of claim 8 , wherein the instructions further cause the processor to:
compute the first temporal relative pose of the ego vehicle based upon odometry data generated by the ego vehicle.
10 . The non-transitory computer-readable medium of claim 8 , wherein the instructions further cause the processor to:
transmit a request to the remote vehicle;
receive the second point cloud from the remote vehicle based upon the request; and
receive the second temporal relative pose from the remote vehicle.
11 . The non-transitory computer-readable medium of claim 8 , wherein the first point cloud represents a first field of view of an environment of the ego vehicle and the remote vehicle, and wherein the combined point cloud represents a second field of view of the environment that is greater than the first field of view.
12 . The non-transitory computer-readable medium of claim 8 , wherein the instructions further cause the processor to:
generate the first point cloud by way of a first light detection and ranging (LIDAR) sensor of the ego vehicle, wherein the second point cloud is generated by a second LIDAR sensor of the remote vehicle.
13 . A method, comprising:
computing, at a first timestep, a base relative pose between an ego vehicle and a remote vehicle based upon respective point clouds of the ego vehicle and the remote vehicle;
computing, at a second timestep, a computed relative pose between the ego vehicle and the remote vehicle based upon the base relative pose, a first temporal relative pose of the ego vehicle, and a second temporal relative pose received from the remote vehicle, wherein the first temporal relative pose and the second temporal relative pose are respectively indicative of motion changes of the ego vehicle and the remote vehicle between the second timestep and the first timestep;
generating a combined point cloud based upon a first point cloud of the ego vehicle, a second point cloud received from the remote vehicle, and the computed relative pose; and
controlling the ego vehicle based upon the combined point cloud.
14 . The method of claim 13 , wherein computing the base relative pose includes computing the base relative pose at a first frequency, the method further comprising:
computing, at the second timestep, the first temporal relative pose based upon odometry data of the ego vehicle, wherein the first temporal relative pose is computed at a second frequency; and
receiving, at the second timestep, the second temporal relative pose from the remote vehicle, wherein the second temporal relative pose is computed by the remote vehicle at the second frequency, and
wherein computing the computed relative pose comprises computing the computed relative pose at the second frequency, and wherein the first frequency is less than the second frequency.
15 . The method of claim 13 , further comprising:
obtaining the respective point clouds of the ego vehicle and the remote vehicle.
16 . The method of claim 13 , wherein the first point cloud and the second point cloud are indicative of different regions of an environment of the ego vehicle and the remote vehicle.
17 . The method of claim 13 , wherein the computed relative pose comprises a translation vector and a rotation matrix, and
wherein generating the combined point cloud based upon the first point cloud, the second point cloud, and the computed relative pose comprises:
generating a transformed point cloud based upon the second point cloud, the translation vector, and the rotation matrix; and
stitching the first point cloud and the transformed point cloud together to generate the combined point cloud.
18 . The method of claim 13 , further comprising:
computing, at a third timestep, a second relative pose between the ego vehicle and the remote vehicle based upon the computed relative pose, a third temporal relative pose of the ego vehicle, and a fourth temporal relative pose received from the remote vehicle, wherein the third temporal relative pose and the fourth temporal relative pose are respectively indicative of second motion changes of the ego vehicle and the remote vehicle between the third timestep and the second timestep; and
generating a second combined point cloud based upon a third point cloud of the ego vehicle, a fourth point cloud received from the remote vehicle, and the second relative pose.