Motion monitor using lidar registration
A motion monitor includes a lidar device, a controller, and a circuit. The lidar device is configured to perform scans of an environment proximate the ego vehicle to generate lidar point clouds related to one or more objects in the environment. The controller is configured to transform the lidar point clouds with location transform matrices to generate location point sets, aggregate the location point sets in a duration of interest to generate aggregated point sets, register the location point sets to the aggregated point sets to generate correction transform matrices, update the location transform matrices with the correction transform matrices to generate updated location transform matrices, and generate motion sets based on the updated location transform matrices. The circuit is configured to receive the motion sets.
1. A motion monitor comprising:
a lidar device disposed in an ego vehicle and configured to perform a plurality of scans in a sequence over time of an environment proximate the ego vehicle to generate a plurality of lidar point clouds in the sequence related to one or more objects in the environment, wherein the sequence involves two or more scans of the plurality of scans at two or more different times;
a controller in communication with the lidar device and configured to transform the plurality of lidar point clouds with a plurality of location transform matrices to generate a plurality of location point sets in the sequence, aggregate the plurality of location point sets in the sequence in a duration of interest that includes the two or more scans at the two or more different times to generate a plurality of aggregated point sets, register the plurality of location point sets to the plurality of aggregated point sets to generate a plurality of correction transform matrices, update the plurality of location transform matrices with the plurality of correction transform matrices to generate a plurality of updated location transform matrices in the sequence, and generate a plurality of motion sets based on the plurality of updated location transform matrices, wherein the registration of the plurality of location point sets includes:
identification of a number of aggregated points among the plurality of aggregated point sets that are within a neighborhood proximate each current location point in the plurality of location point sets;
removal of the neighborhood in response to the number of the aggregated points being less than a lower threshold number;
reduction of the neighborhood to a random sample of the aggregated points within the neighborhood in response to the number of the aggregated points exceeding an upper threshold value;
calculation of a singular value decomposition of the aggregated points in the neighborhood to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, where the first singular vector and the second singular vector define a best-fit plane to the aggregated points, and the third singular vector is normal to the best-fit plane; and
filter the neighborhood in response to a ratio of the third singular value to the first singular value being greater than a ratio threshold value; and
a circuit in communication with the controller and configured to receive the plurality of motion sets.
2. The motion monitor according to claim 1 , wherein the registration of the plurality of location point sets in the sequence includes a non-causal registration that utilizes one or more future location point sets of the plurality of location point sets in the sequence to noncausally register one or more current location point sets of the plurality of location point sets in the sequence.
3. The motion monitor according to claim 1 , wherein the registration of the plurality of location point sets in the sequence is a causal registration that solely utilizes past location point sets of the plurality of location point sets in the sequence to causally register each current location point set of the plurality of location point sets in the sequence.
4. The motion monitor according to claim 1 , wherein the generation of the plurality of correction transform matrices is calculated using a least mean squares technique under a low angle correction assumption.
5. The motion monitor according to claim 1 , wherein the circuit is a storage device configured to record the plurality of motion sets, and the plurality of motion sets form a training dataset for an autonomous driving training system.
6. The motion monitor according to claim 1 , wherein the circuit is a drive system disposed in the ego vehicle, and configured to control movement of the ego vehicle in response to the plurality of motion sets.
7. The motion monitor according to claim 1 , further comprising a camera configured to sense the environment proximate the ego vehicle to generate a plurality of images.
8. The motion monitor according to claim 7 , wherein the plurality of location transform matrices aligns the plurality of motion sets with the plurality of images.
9. A method for motion estimation of an ego vehicle comprising:
scanning an environment proximate the ego vehicle in a plurality of scans in a sequence over time with a lidar device to generate a plurality of lidar point clouds in the sequence related to one or more objects in the environment, wherein the sequence involves two or more scans of the plurality of scans at two or more different times;
transforming the plurality of lidar point clouds with a plurality of location transform matrices to generate a plurality of location point sets in the sequence;
aggregating the plurality of location point sets in the sequence in a duration of interest that includes the two or more scans at the two or more different times to generate a plurality of aggregated point sets;
registering the plurality of location point sets to the plurality of aggregated point sets to generate a plurality of correction transform matrices, wherein the registration of the plurality of location point sets includes:
identifying a number of aggregated points among the plurality of aggregated point sets that are within a neighborhood proximate each current location point in the plurality of location point sets;
removing the neighborhood in response to the number of the aggregated points being less than a lower threshold number;
reducing the neighborhood to a random sample of the aggregated points within the neighborhood in response to the number of the aggregated points exceeding an upper threshold value;
calculating a singular value decomposition of the aggregated points in the neighborhood to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, where the first singular vector and the second singular vector define a best-fit plane to the aggregated points, and the third singular vector is normal to the best-fit plane; and
filtering the neighborhood in response to a ratio of the third singular value to the first singular value being greater than a ratio threshold value;
updating the plurality of location transform matrices with the plurality of correction transform matrices to generate a plurality of updated location transform matrices in the sequence;
generating a plurality of motion sets based on the plurality of updated location transform matrices; and
receiving the plurality of motion sets at a circuit.
10. The method according to claim 9 , wherein the registering of each one of the plurality of location point sets in the sequence includes a noncausal registering that utilizes one or more future location point sets of the plurality of location point sets in the sequence to noncausally register one or more current location point sets of the plurality of location point sets in the sequence.
11. The method according to claim 9 , wherein the registering of each one of the plurality of location point sets in the sequence includes a causal registering that solely utilizes past location point sets of the plurality of location point sets in the sequence to causally register each current location point set of the plurality of location point sets in the sequence.
12. The method according to claim 9 , wherein the generation of the plurality of correction transform matrices is calculated using a least mean squares technique under a low angle correction assumption.
13. The method according to claim 9 , wherein the circuit is a storage device configured to record the plurality of motion sets, and the plurality of motion sets form a training dataset for an autonomous driving training system.
14. The method according to claim 9 , wherein the circuit is a drive system disposed in the ego vehicle, and configured to control movement of the ego vehicle in response to the plurality of motion sets.
15. An ego vehicle comprising:
a lidar device configured to perform a plurality of scans in a sequence over time of an environment proximate the ego vehicle to generate a plurality of lidar point clouds in the sequence related to one or more objects in the environment, wherein the sequence involves two or more scans of the plurality of scans at two or more different times;
a controller in communication with the lidar device and configured to transform the plurality of lidar point clouds with a plurality of location transform matrices to generate a plurality of location point sets in the sequence, aggregate the plurality of location point sets in the sequence in a duration of interest that includes the two or more scans at the two or more different times to generate a plurality of aggregated point sets, register the plurality of location point sets to the plurality of aggregated point sets to generate a plurality of correction transform matrices, update the plurality of location transform matrices with the plurality of correction transform matrices to generate a plurality of updated location transform matrices in the sequence, and generate a plurality of motion sets based on the plurality of updated location transform matrices, wherein the registration of the plurality of location point sets includes:
identifying a number of aggregated points among the plurality of aggregated point sets that are within a neighborhood proximate each current location point in the plurality of location point sets;
removing the neighborhood in response to the number of the aggregated points being less than a lower threshold number;
reducing the neighborhood to a random sample of the aggregated points within the neighborhood in response to the number of the aggregated points exceeding an upper threshold value;
calculating a singular value decomposition of the aggregated points in the neighborhood to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, where the first singular vector and the second singular vector define a best-fit plane to the aggregated points, and the third singular vector is normal to the best-fit plane; and
filtering the neighborhood in response to a ratio of the third singular value to the first singular value being greater than a ratio threshold value;
a circuit in communication with the controller and configured to receive the plurality of motion sets and generate a plurality of commands in response to the plurality of motion sets; and
a drive system configured to control movement of the ego vehicle in response to the plurality of commands.
16. The ego vehicle according to claim 15 , wherein the registering of the plurality of location point sets in the sequence is a causal registering that solely utilizes past location point sets of the plurality of location point sets in the sequence to causally register each current location point set of the plurality of location point sets in the sequence.
17. The ego vehicle according to claim 15 , further comprising a camera configured to sense the environment proximate the ego vehicle to generate a plurality of images.
18. The ego vehicle according to claim 17 , wherein the plurality of location transform matrices aligns the plurality of motion sets with the plurality of images.
19. The ego vehicle according to claim 15 , wherein the registration of the plurality of location point sets in the sequence includes a non-causal registration that utilizes one or more future location point sets of the plurality of location point sets in the sequence to noncausally register one or more current location point sets of the plurality of location point sets in the sequence.
20. The ego vehicle according to claim 15 , wherein the generation of the plurality of correction transform matrices is calculated using a least mean squares technique under a low angle correction assumption.