System and method for generating three dimensional images using lidar and video measurements
View Patent ↗A system uses range and Doppler velocity measurements from a lidar system and images from a video system to estimate a six degree-of-freedom trajectory of a target. The system estimates this trajectory in two stages: a first stage in which the range and Doppler measurements from the lidar system along with various feature measurements obtained from the images from the video system are used to estimate first stage motion aspects of the target (i.e., the trajectory of the target); and a second stage in which the images from the video system and the first stage motion aspects of the target are used to estimate second stage motion aspects of the target. Once the second stage motion aspects of the target are estimated, a three-dimensional image of the target may be generated.
1. A system for generating a three-dimensional image of a target from lidar measurements and video images, the system comprising:
a lidar subsystem configured to direct at least two beams toward the target and that generates a range measurement and a Doppler velocity measurement for each of a plurality of points on the target for each of the at least two beams;
a video subsystem configured to provide a plurality of two-dimensional images of the target; and
a processor configured to:
receive, from the lidar subsystem, the range measurement and the Doppler velocity measurement for each of the plurality of points on the target,
receive, from the video subsystem, the plurality of images of the target,
estimate motion aspects of each of the plurality of points on the target during a first stage, wherein the motion aspects comprise translational velocity components expressed in three dimensions as v x trans , v y trans , and v z trans , and angular velocity components expressed in three dimensions as ω x , ω y , and ω z , wherein the processor is further configured to:
estimate the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the range measurements and the Doppler velocity measurements from the lidar subsystem, and
estimate the translational velocity components v x trans and v y trans and the angular velocity component ω z of each of the plurality of points on the target from two-dimensional position and velocity measurements of at least one feature of the target obtained from the plurality of images of the target and from the estimated velocity component v z trans and the estimated angular velocity components ω x and ω y ;
refine, during a second stage, the motion aspects of the target estimated in the first stage by:
transforming a first one of the plurality of images to a particular time using the motion aspects of the target estimated in the first stage,
transforming a second one of the plurality of images to the particular time using the motion aspects of the target estimated in the first stage, and
subtracting the transformed first one of the plurality of images at the particular time from the transformed second one of the plurality of images at the particular time to determine Δx ij , Δy ij , Δ⊖z ij ; and
generate the three-dimensional image of the target using the refined motion aspects.
2. The system of claim 1 , wherein the processor is configured to estimate first stage motion aspects of the target using a least squares estimator.
3. The system of claim 1 , wherein the lidar subsystem is configured to direct at least four beams toward a target and generate a range measurement and a Doppler velocity measurement for each of a plurality of points on the target for each of the at least four beams.
4. The system of claim 1 , wherein the at least two beams are displaced from one another by a known distance or angle.
5. The system of claim 3 , wherein at least two of the at least four beams are displaced from one another by a known distance or angle along a first axis and wherein at least two of the at least four beams are displaced from one another by a known distance or angle along a second axis.
6. The system of claim 5 , wherein the first axis and the second axis are orthogonal to one another.
7. The system of claim 1 , wherein the processor is configured to estimate the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the range measurements and the Doppler velocity measurements from the lidar subsystem and from position and velocity measurements of at least one feature of the target obtained from the plurality of images of the target.
8. The system of claim 7 , wherein the processor is configured to estimate the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the range measurements and the Doppler velocity measurements from the lidar subsystem, from position and velocity measurements of at least one feature of the target obtained from the plurality of images of the target, and from an estimate of a distance component to the at least one feature of the target.
9. The system of claim 8 , wherein the processor is further configured to estimate of the distance component to the at least one feature of the target by determining an average distance component based on the range measurements from the lidar subsystem.
10. The system of claim 7 , wherein the processor is further configured to iteratively estimate the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the estimated translational velocity component V z trans and the estimated angular velocity components ω x and ω y of each of the plurality of points on the target, the position and velocity measurements of the at least one feature of the target, and an estimate of a distance component to the at least one feature of the target.
11. The system of claim 10 , wherein during a first iteration, the processor is configured to estimate the distance component to the at least one feature of the target by determining an average distance component based on the range measurements from the lidar subsystem.
12. The system of claim 11 , wherein during a second iteration, the processor is configured to use the estimated distance component to the at least one feature of the target as determined during the first iteration.
13. A method for generating a three-dimensional image of a target from lidar measurements from a lidar subsystem and video images from a video subsystem, the method comprising:
receiving, from the lidar subsystem, a range measurement and a Doppler velocity measurement for each of a plurality of points on the target for each of at least two beams of the lidar subsystem;
receiving, from the video subsystem, a plurality of two-dimensional images of the target;
determining two-dimensional position and velocity measurements of at least one feature of the target using the plurality of images of the target;
estimating motion aspects of each of the plurality of points on the target during a first stage, wherein the motion aspects comprise translational velocity components expressed in three dimensions as v x trans , v y trans , and v z trans , and angular velocity components expressed in three dimensions as ω x , ω y , and ω z , wherein said estimating motion aspects further comprising:
estimating the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the range measurements and the Doppler velocity measurements from the lidar subsystem, and
estimating the translational velocity components v x trans and v y trans and the angular velocity component ω z of each of the plurality of points on the target from the position and velocity measurements of the at least one feature of the target determined from the plurality of images of the target and from the estimated velocity component v z trans and the estimated angular velocity components ω x and ω y ;
refining, during a second stage, the motion aspects of the target estimated in the first stage by:
transforming a first one of the plurality of images to a particular time using the motion aspects of the target estimated in the first stage,
transforming a second one of the plurality of images to the particular time using the motion aspects of the target estimated in the first stage, and
subtracting the transformed first one of the plurality of images at the particular time from the transformed second one of the plurality of images at the particular time to determine Δx ij , Δy ij , Δ⊖z ij ; and
generating a three-dimensional image of the target using the refined motion aspects of the target.
14. The method of claim 13 , wherein estimating the motion aspects of each of the plurality of points on the target further comprises estimating, using at least one least squares estimator, the motion aspects of each of the plurality of points on the target.
15. The method of claim 13 , wherein receiving, from the lidar subsystem, a range measurement and a Doppler velocity measurement for each of a plurality of points on the target for each of at least two beams of the lidar subsystem comprises receiving, from the lidar subsystem, a range measurement and a Doppler velocity measurement for each of a plurality of points on the target for each of at least four beams of the lidar subsystem.
16. The method of claim 13 , wherein estimating the translational velocity component v z trans and the angular velocity components ω x and ω y further comprises estimating the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the range measurements and the Doppler velocity measurements from the lidar subsystem and from the position and velocity measurements of the at least one feature of the target determined from the plurality of images of the target.
17. The method of claim 16 , wherein estimating the translational velocity component v z trans and the angular velocity components ω x and ω y further comprises:
iteratively estimating the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target from the estimated translational velocity component v z trans and the estimated angular velocity components ω x and ω y of each of the plurality of points on the target, the position and velocity measurements of the at least one feature of the target, and an estimate of a distance component to the at least one feature of the target.
18. The method of claim 17 , wherein iteratively estimating the translational velocity component v z trans and the angular velocity components ω x and ω y of each of the plurality of points on the target further comprises:
during a first iteration, estimating the distance component to the at least one feature of the target by determining an average distance component to the at least one feature of the target based on the range measurements from the lidar subsystem; and
during a second iteration, using the estimated distance component to the at least one feature of the target as determined during the first iteration.
19. The method of claim 13 , wherein refining the motion aspects of the target estimated in the first stage comprises mapping the plurality of images from the video subsystem onto the plurality of points on the target using the motion aspects of the target estimated in the first stage.