Vehicle trajectory estimation for the generation of ground truth data
Techniques are disclosed to enable the generation of ground truth datasets that identify a continuous time movement trajectory of dynamic objects such as non-stationary vehicles. The techniques as described herein may utilize iterative closest point (ICP) processes to compute a rigid-body transform that describes a 3D correspondence between different pairs of temporal point cloud data sample sets at various discrete time periods that are identified with an object along a movement trajectory. From these rigid-body transforms at each of the discrete time periods, a pose graph optimization may be performed to compute the continuous time movement trajectory. The ground truth datasets may be used to enable training of machine learning models, which may be used by a vehicle computing system when navigating a driving environment.
1 . A computing device, comprising:
a memory configured to store computer-readable instructions; and
one or more processors configured to execute the computer-readable instructions stored in the memory to:
receive a plurality of temporal point cloud data sample sets greater than two, each respective one of the temporal point cloud data sample sets comprising a set of three-dimensional (3D) coordinates that represent an object in 3D space at a respective discrete time period within a discrete time movement trajectory of the object;
compute a first rigid-body transform dataset that comprises rigid-body transform data that identifies a mapping of 3D coordinates between each combination of respective pairs of temporal point cloud data sample sets from among the plurality of temporal point cloud data sample sets;
perform a pose graph optimization that utilizes the first rigid-body transform dataset as constraints to output a second rigid-body transform dataset that comprises a mapping of 3D coordinates between (i) a first one of the plurality of temporal point cloud data sample sets that is identified with a starting discrete time period of the discrete time movement trajectory of the object, and (ii) other ones of the plurality of temporal point cloud data sample sets that are identified with other respective discrete time periods of the discrete time movement trajectory of the object;
generate, based upon the plurality of temporal point cloud data sample sets and the second rigid-body transform dataset, a ground truth dataset that includes a computed continuous time movement trajectory of the object within a set of training image frames;
train, using the ground truth dataset, a machine learning model that predicts, at inference, a continuous time movement trajectory of one or more objects in acquired image frames when navigating a driving environment;
deploy the trained machine learning model in a vehicle control system of a vehicle that is configured to control at least one aspect of the operation of the vehicle; and
control, via the vehicle control system, a trajectory of the vehicle in response to predicting, using the trained machine learning model, a movement trajectory of the one or more other objects in the driving environment.
2 . The computing device of claim 1 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to:
compute the continuous time movement trajectory of the object over a time period from (i) the starting discrete time period of the discrete time movement trajectory of the object, and (ii) an ending discrete time period of the discrete time movement trajectory of the object.
3 . The computing device of claim 1 , wherein the vehicle control system comprises a safety system of the vehicle, and
wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to
perform, via the safety system of the vehicle, a vehicle-based function in response to predicting, at inference, the continuous time movement trajectory of the one or more objects in the acquired image frames when navigating the driving environment.
4 . The computing device of claim 1 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to:
compute the first rigid-body transform dataset by executing an Iterative Closest Point (ICP) algorithm on each respective pair of temporal point cloud data sample sets to identify, via a convergence of the ICP algorithm, the mapping of 3D coordinates between each respective pair of temporal point cloud data sample sets.
5 . The computing device of claim 4 , wherein:
the object in 3D space comprises a vehicle, and
the ICP algorithm comprises a point-to-plane ICP algorithm.
6 . The computing device of claim 4 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to:
compute the first rigid-body transform dataset by reducing a number of data samples from among each respective pair of temporal point cloud data sample sets used for computing, for each respective pair of temporal point cloud data sample sets, a respective rigid-body transform.
7 . The computing device of claim 6 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to:
reduce the number of data samples used for computing, for each respective pair of temporal point cloud data sample sets, a respective rigid-body transform by performing a filtering operation that removes outlier data samples by discarding data samples from among each respective pair of temporal point cloud data sample sets that do not share a common 3D region with one another.
8 . The computing device of claim 4 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to:
reduce a number of data samples used for computing, for each respective pair of temporal point cloud data sample sets, a respective rigid-body transform by performing a filtering operation that removes outlier data samples by using only the data samples from among each respective pair of temporal point cloud data sample sets that, when used for one or more ICP iterations of the ICP algorithm, result in an error value that is less than a threshold error value.
9 . The computing device of claim 1 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, further cause the computing device to:
perform the pose graph optimization by minimizing a sum of errors resulting from mapping the 3D coordinates between the first one of the plurality of temporal point cloud data sample sets and each of the plurality of temporal point cloud data sample sets identified with the other respective discrete time periods within the discrete time movement trajectory of the object.
10 . The computing device of claim 1 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, cause the computing device to compute the continuous time movement trajectory of the object by interpolating between ones of the plurality of temporal point cloud data sample sets identified with the second rigid-body transform dataset.
11 . The computing device of claim 1 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, cause the computing device to:
compute a projection of temporal point cloud data sample sets of the object to one or more time periods using the continuous time movement trajectory; and
generate the ground truth dataset by supplementing the set of training image frames with the projection of the temporal point cloud data sample sets of the object.
12 . The computing device of claim 1 , wherein the computer-readable instructions stored on the memory, when executed by the one or more processors, cause the computing device to control, via the vehicle control system of the vehicle, the trajectory of the vehicle to attempt to avoid a potential collision with the one or more other objects.
13 . A non-transitory computer-readable medium having instructions stored thereon that, when executed by processing circuitry of a computing device, cause the computing device to:
receive a plurality of temporal point cloud data sample sets greater than two, each respective one of the temporal point cloud data sample sets comprising a set of three-dimensional (3D) coordinates that represent an object in 3D space at a respective discrete time period within a discrete time movement trajectory of the object;
compute a first rigid-body transform dataset that comprises rigid-body transform data that identifies a mapping of 3D coordinates between each combination of respective pairs of temporal point cloud data sample sets from among the plurality of temporal point cloud data sample sets;
perform a pose graph optimization that utilizes the first rigid-body transform dataset as constraints to output a second rigid-body transform dataset that comprises a mapping of 3D coordinates between (i) a first one of the plurality of temporal point cloud data sample sets that is identified with a starting discrete time period of the discrete time movement trajectory of the object, and (ii) other ones of the plurality of temporal point cloud data sample sets that are identified with other respective discrete time periods of the discrete time movement trajectory of the object;
generate, based upon the plurality of temporal point cloud data sample sets and the second rigid-body transform dataset, a ground truth dataset that includes a computed continuous time movement trajectory of the object within a set of training image frames; and
train, using the ground truth dataset, a machine learning model that predicts, at inference, a continuous time movement trajectory of one or more objects in acquired image frames when navigating a driving environment;
deploy the trained machine learning model in a vehicle control system of a vehicle; and
control, via the vehicle control system of the vehicle, a trajectory of the vehicle in response to predicting, using the trained machine learning model, a movement trajectory of the one or more other objects in the driving environment.
14 . The non-transitory computer-readable medium of claim 13 , wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to:
compute the continuous time movement trajectory of the object over a time period from (i) the starting discrete time period of the discrete time movement trajectory of the object, and (ii) an ending discrete time period of the discrete time movement trajectory of the object.
15 . The non-transitory computer-readable medium of claim 13 , wherein the vehicle control system comprises a safety system of the vehicle, and
wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to
perform, via the safety system of the vehicle, a vehicle-based function in response to predicting, at inference, the continuous time movement trajectory of the one or more objects in the acquired image frames when navigating the driving environment.
16 . The non-transitory computer-readable medium of claim 13 , wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to:
compute the first rigid-body transform dataset by executing an Iterative Closest Point (ICP) algorithm on each respective pair of temporal point cloud data sample sets to identify, via a convergence of the ICP algorithm, the mapping of 3D coordinates between each respective pair of temporal point cloud data sample sets.
17 . The non-transitory computer-readable medium of claim 16 , wherein:
the object in 3D space comprises a vehicle, and
the ICP algorithm comprises a point-to-plane ICP algorithm.
18 . The non-transitory computer-readable medium of claim 16 , wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to compute the first rigid-body transform dataset by reducing a number of data samples from among each respective pair of temporal point cloud data sample sets used for computing, for each respective pair of temporal point cloud data sample sets, a respective rigid-body transform.
19 . The non-transitory computer-readable medium of claim 18 , wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to reduce the number of data samples used for computing, for each respective pair of temporal point cloud data sample sets, a respective rigid-body transform by performing a filtering operation that removes outlier data samples by discarding data samples from among each respective pair of temporal point cloud data sample sets that do not share a common 3D region with one another.
20 . The non-transitory computer-readable medium of claim 16 , wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to reduce a number of data samples used for computing, for each respective pair of temporal point cloud data sample sets, a respective rigid-body transform by performing a filtering operation that removes outlier data samples by using only the data samples from among each respective pair of temporal point cloud data sample sets that, when used for one or more ICP iterations of the ICP algorithm, result in an error value that is less than a threshold error value.
21 . The non-transitory computer-readable medium of claim 13 , wherein the computer-readable instructions, when executed by the processing circuitry, further cause the computing device to perform the pose graph optimization by minimizing a sum of errors resulting from mapping the 3D coordinates between the first one of the plurality of temporal point cloud data sample sets and each of the plurality of temporal point cloud data sample sets identified with the other respective discrete time periods within the discrete time movement trajectory of the object.
22 . The non-transitory computer-readable medium of claim 13 , wherein the computer-readable instructions, when executed by the processing circuitry, cause the computing device to control, via the vehicle control system of the vehicle, the trajectory of the vehicle to attempt to avoid a potential collision with the one or more other objects.