Range and intensity image-based terrain and vehicle relative pose estimation system

1. A navigation system comprising:

an image acquisition device for acquiring a range image of a target vehicle at least one processor; and

a memory including a target vehicle model and computer readable program code,

the processor and the computer readable program code configured to cause the navigation system to:

convert the range image to a point cloud having three dimensions;

compute a transform from the target vehicle model to the point cloud;

use the transform to estimate the target vehicle's attitude and position for capturing the target vehicle;

wherein the processor and the computer readable program code are configured to generate an a priori pose estimate of the target vehicle by:

comparing a target vehicle point cloud with the range image point cloud; and

generating a score reflecting a fit between the target vehicle point cloud and the range image point cloud; and

wherein the processor and the computer readable program code are configured to compare the target vehicle point cloud with the range image point cloud by:

creating a range histogram of the range image point cloud and determining an average of coordinate values of pixels in a selected range bin of the histogram;

determining a vector from an origin of the target vehicle point cloud to a center of mass of the range image point cloud;

rotating the range image point cloud around the target vehicle point cloud and determining the score reflecting a fit between the target vehicle model and the point cloud; and

using the rotated range image point cloud having the highest score as the a priori pose estimate of the target vehicle.

2. The navigation system of claim 1 , wherein the processor and the computer readable program code are configured to compute the transform from the target vehicle model to the point cloud by:

centering the target vehicle model at an origin of the point cloud; and

applying a translation and rotation to the point cloud to align with the target vehicle model.

3. The navigation system of claim 1 , wherein the processor and the computer readable program code are configured to correct errors identified while generating said priori pose estimate said errors are corrected by:

identifying closest points of the target vehicle model and the range image point cloud;

removing points from the range image point cloud residing outside the target vehicle model;

performing a least squares minimization to minimize any offset between the remaining closest points of the target vehicle model and the range image point cloud; and

utilizing the resulting transform to update the a priori pose.

4. The navigation system of claim 3 , wherein the processor and the computer readable program code are configured to identify the closest points of the target vehicle model and the range image point cloud using a k-dimensional tree search.

5. The navigation system of claim 3 , wherein the processor and the computer readable program code are configured to remove points from the range image point cloud residing outside the target vehicle model by:

defining a three dimensional boundary around the target vehicle point cloud; and

disregarding points in the range image point cloud residing outside the three dimensional boundary.

6. The navigation system of claim 3 , wherein the processor and the computer readable program code are configured to use the updated a priori pose to estimate the target vehicle's attitude and position for capturing the target vehicle.

7. A navigation system comprising:

an image acquisition device for acquiring a range image of a target vehicle; and

an integrated circuit programmed with computer readable program code in firmware to:

convert the range image to a point cloud having three dimensions;

compute a transform from a stored target vehicle model to the point cloud;

use the transform to estimate the target vehicle's attitude and position for capturing the target vehicle;

wherein the integrated circuit is programmed with computer readable program code in firmware to generate an a priori pose estimate of the target vehicle by:

comparing a target vehicle point cloud with the range image point cloud; and

generating a score reflecting a fit between the target vehicle point cloud and the range image point cloud; and

wherein the integrated circuit is programmed with computer readable program code in firmware to compare the target vehicle point cloud with the range image point cloud by:

creating a range histogram of the range image point cloud and determining an average of coordinate values of pixels in a selected range bin of the histogram;

determining a vector from an origin of the target vehicle point cloud to a center of mass of the range image point cloud;

rotating the range image point cloud around the target vehicle point cloud and determining the score reflecting a fit between the target vehicle model and the point cloud; and

using the rotated range image point cloud having the highest score as the a priori pose estimate of the target vehicle.

8. The navigation system of claim 7 , wherein the integrated circuit is programmed with computer readable program code in firmware to compute Rail the transform from the target vehicle model to the point cloud by:

centering the target vehicle model at an origin of the point cloud; and

applying a translation and rotation to the point cloud to align with the target vehicle model.

9. The navigation system of claim 7 , wherein the integrated circuit, is programmed with computer readable program code in firmware to correct errors identified while said integrated circuit generates said priori pose estimate, resulting from applying the a priori pose estimate to the range image point cloud said errors are corrected by:

identifying closest points of the target vehicle model and the range image point cloud;

removing points from the range image point cloud residing outside the target vehicle model;

performing a least squares minimization to minimize any offset between the remaining closest points of the target vehicle model and the range image point cloud; and

utilizing the resulting transform to update the a priori pose.

10. The navigation system of claim 9 , wherein the integrated circuit is programmed with computer readable program code in firmware to identify the closest points of the target vehicle model and the range image point cloud using a k-dimensional tree search.

11. The navigation system of claim 9 , wherein the integrated circuit is programmed with computer readable program code in firmware to remove points from the range image point cloud residing outside the target vehicle model by:

defining a three dimensional boundary around the target vehicle point cloud; and

disregarding points in the range image point cloud residing outside the three dimensional boundary.

12. The navigation system of claim 9 , wherein the integrated circuit is programmed with computer readable program code in firmware to use the updated a priori pose to estimate the target vehicle's attitude and position for capturing the target vehicle.

13. A navigation system comprising:

an image acquisition device for acquiring a range image of a target vehicle;

at least one processor;

a memory including a target vehicle model and computer readable program code; and

an iterative closest point function implemented by the processor and the computer readable program code and configured to:

convert the range image to a point cloud having three dimensions;

compute a transform from the target vehicle model to the point cloud;

use the transform to estimate the target vehicle's attitude and position for capturing the target vehicle;

said navigation system further comprising an acquisition function implemented by the processor and the computer readable program code and configured to generate an a priori pose estimate of the target vehicle by:

comparing a target vehicle point cloud with the range image point cloud; and

generating a score reflecting a fit between the target vehicle point cloud and the range image point cloud;

wherein the acquisition function is further configured to compare the target vehicle point cloud with the range image point cloud by:

creating a range histogram of the range image point cloud and determining an average of coordinate values of pixels in a selected range bin of the histogram;

determining a vector from an origin of the target vehicle point cloud to a center of mass of the range image point cloud;

rotating the range image point cloud around the target vehicle point cloud and determining the score reflecting a fit between the target vehicle model and the point cloud; and

using the rotated range image point cloud having the highest score as the a priori pose estimate of the target vehicle.

14. The navigation system of claim 13 , wherein the iterative closest point function is further configured to compute the transform from the target vehicle model to the point cloud by:

centering the target vehicle model at an origin of the point cloud; and

applying a translation and rotation to the point cloud to align with the target vehicle model.