Maneuverless passive range estimation using monocular image sequences

Abstract

A method to estimate range to a moving rigid body from a moving platform using a monocular camera. The method does not require the camera platform to maneuver in order to estimate range. The method relies on identification and tracking of certain principal features of the object. The method extracts a silhouette of an object from an obtained image and identifies two principal linear components of the silhouette. A normalized distance between the point of intersection of the two linear components and a centroid of the silhouette is computed, compared to a data set and used to determine a direction of movement of the object.

Description

BACKGROUND

Estimating the range to a mobile object using a monocular camera fixed to a moving platform has, in general, remained an unsolved problem for the past three decades. An effective solution to this problem can be used in autonomous collision detection and avoidance applications for unmanned vehicles, especially unmanned aerial vehicles and surface vehicles. The technology can also be used to provide situational awareness information to manned air and water crafts moving in dynamic environments. In this disclosure, we present a novel approach to compute the range or distance to a dynamic object using a sequence of monocular images captured from a moving sensor platform. In what follows, we shall use the term “intruder” to describe the dynamic object whose range is to be estimated.

Depending on the sensor resolution, imaging sensors can provide very accurate estimates of the relative bearing angle to the intruder. At large distances, an intruder may occupy only a few pixels on an image. The classical approach is to treat the intruder as a point target and use bearing angle measurements to the tracked point. However, this approach is, in general, not sufficient due to the inherent unobservability of the intruder dynamics. We use an example to illustrate this problem.

Let us assume that the sensor platform is initially located at (0,0) in a two-dimensional plane and moves along the positive y-axis with velocity given by v_s. Let us assume that the intruder initially located at (x₀, y₀) is moving with velocity (u, v) where u and v represent the X and Y components of the velocity. The position of the sensor at any time t is given by (0, v_st). The position of the intruder at time t is given by (x₀+ut, y₀+vt). The bearing angle to the intruder as measured from the sensor at any given time is, therefore, given by arctan (y₀+(v−v_s)t)/(x₀+ut). Now, let us assume there is another intruder located initially at (kx₀, ky₀) moving with velocity (ku, kv−(k−1)v_s), where k is a positive constant and not equal to 1. Now, notice that the relative bearing to the second intruder measured from the sensor at any time t is given by arctan (ky₀+(kv−(k−1)v_s)t−v_st)/(kx₀+kut) which is equal to arctan (ky₀+kvt−kv_st)/(kx₀+kut)=arctan (y₀+(v−v_s)t)/(x₀+ut). Therefore, two intruders with distinct trajectories (k≠1) generate the same bearing angle measurement at the sensor. This clearly illustrates the inherent observability problem present in bearing-only tracking. Please refer to FIG. 1 for a specific example. In this case, (x₀, y₀)=(500,500), k=2, v_s=100, u=−50 and v=50. The sensor position is shown by red circles. The first intruder's position is represented by blue plus signs. The second intruder (position denoted by black asterisks) is moving with velocity equal to (−50k, 50k−(k−1)100)=(−100,0). The angles θ(0), θ(1) and θ(2) denote the relative bearing to the two intruders at time instants 0, 1 and 2. Notice that both intruders subtend the same relative bearing angles with respect to the sensor, but k≠1 signifying that the two intruder trajectories are distinct as can be seen in the figure.

The conditions of unobservability in a bearing-only tracking problem have been extensively studied since the late 1970's. This body of research has established that the intruder state is observable, in general, only if the order of sensor dynamics is greater than the intruder dynamics. For an intruder moving with constant velocity, this implies that the sensor dynamics must involve an acceleration component. In the event that the sensor dynamics are not of a higher order, the sensor platform must execute a deliberate maneuver involving a higher order dynamics component to be able to estimate the range. With the growing use of Unmanned Aerial Vehicles (UAVs) in recent years, such a “maneuver-based” approach has been proposed as a solution to the passive camera based Sense and Avoid (SAA) problem: upon detecting an intruder, the UAV maneuvers in order to triangulate and resolve the position of the intruder. However, a maneuver-based approach is undesirable in many ways especially in military operations. It may lead to waste of fuel, loss in mission performance, and is in general bad airmanship.

Other research deals with choosing the right coordinate frames and filters for bearings-only tracking from the point of view of stability and unbiasedness. For example, an Extended Kalman Filter (EKF) applied to bearings-only target tracking has been theoretically analyzed and has established the reasons for the filter bias and instability. A modified polar coordinate system based Kalman Filter has also been proposed to separate the observable and unobservable dynamics. Further, a bank of EKFs each with a different initial range estimate referred to as the range-parameterized (RP) tracker has been proposed and shown to perform better than classical EKF implementations. More recently, particle filter solutions to bearings-only tracking problem are receiving considerable attention and they have been implemented to track both maneuvering and non-maneuvering targets. Particle filter and RP EKF have been compared and it has been shown that the particle filter is only marginally better than the Range Parameterized EKF but is considerably more robust to errors in the initial target range.

This disclosure presents a novel passive image based ranging approach that does not require the sensor platform to maneuver. Analytical results prove that the approach can be used to estimate the range to an intruder without any sensor maneuver under very general conditions. Tests conducted on real flight-test data have demonstrated the practicality of the approach.

The invention in this disclosure concerns a novel method to estimate the range to a moving rigid body from a mobile platform using monocular passive cameras such as electro-optical or infra-red camera mounted on the platform. The primary application of the proposed technology is in the area of collision detection and avoidance for unmanned vehicles. Unmanned aerial, surface and ground vehicles equipped with cameras can use the proposed invention to estimate the range to other moving objects in their vicinity. The range information can then be used to determine the trajectory of the moving objects and maintain a safe distance from them.

Generally, in a computerized system including a camera mounted on a moving vehicle, wherein the camera acquires consecutively in real time a plurality of images of a moving object within a field of view of the camera, a method for determining a range of said moving object from said moving vehicle comprises the steps of:

(a) detecting the moving object (intruder) within each of said plurality of images;

(b) identifying two feature points p₁ and p₂ on said detected object where p₁ and p₂ satisfy a certain geometric relationship with the velocity vector of the detected object including but not limited to that the two feature points represent a leading point of the detected object and a trailing point of the detected object; and

(c) recursively calculating the range to the object based on changes in the positions of the feature points p₁ and p₂ in the sequential images and further based on the assumption that the geometric relationship mentioned under item (b), including but not limited to the assumption that the detected object is traveling in a direction along a line connecting feature points p₁ and p₂, is valid

The key aspect of the invention lies in the premise that if you can assume pure translational movement and assume a direction of motion based on the image, then you can recursively calculate the range without the order of sensor dynamics being greater than the intruder dynamics, i.e. without any extraneous maneuvering.

Accordingly, among the objects of the instant invention are: the provision of a maneuverless passive ranging method.

Another object of the invention is the provision of a ranging method which extracts feature points on a detected object and provides a geometric relationship between the spatial positions of the feature points and the direction of velocity of the object.

Other objects, features and advantages of the invention shall become apparent as the description thereof proceeds when considered in connection with the accompanying illustrative drawings.

DESCRIPTION OF THE DRAWINGS

In the drawings which illustrate the best mode presently contemplated for carrying out the present invention:

FIG. 1 is a graph showing the inherent observability problem present in bearing-only tracking;

FIG. 2 is an illustration of a moving sensor platform (unmanned aircraft) and a moving object (intruder) within the field of view of the camera mounted on the platform;

FIG. 3 is a schematic block diagram of a computer system in accordance with the invention;

FIG. 4 is an illustration of an image acquired by the camera and an associated silhouette extracted from the image (nose to tail orientation);

FIG. 5 is another illustration of an image acquired by the camera and an associated silhouette extracted from the image (wingtip to wingtip orientation);

FIG. 6 is a flowchart of an exemplary software implementation of the method of the present invention;

FIG. 7 is a flowchart of another exemplary software implementation of the method of the present invention;

FIG. 8 is a flowchart of a software implementation of a method of extracting a silhouette of the image and identifying the pixel locations i₁, j₁ and i₂, j₂ of feature points p₁, p₂; and

FIG. 9 is a flowchart of a software implementation of another method of the present invention.

DESCRIPTION OF THE PREFERRED EMBODIMENT

Referring to FIGS. 2-5, the invention in this disclosure concerns a novel method to estimate a range to a moving rigid body 10 from a mobile platform 12 using monocular passive cameras 14 such as electro-optical or infra-red camera mounted on the platform 12. The primary application of the proposed technology is in the area of collision detection and avoidance for unmanned vehicles. Unmanned aerial, surface and ground vehicles equipped with cameras can use the proposed invention to estimate the range to other moving objects in their vicinity. As illustrated in FIG. 2, the exemplary embodiment shows an unmanned aerial vehicle (UAV) 12 (moving platform) estimating range to a civilian aircraft 10 (moving rigid body/intruder). The range information can then be used to determine the trajectory of the moving objects and maintain a safe distance from them.

Generally the invention is implemented in a computerized system 16 including a CPU 18, memory 20 and a camera mounted on the platform 12. The system 16 communicates with the vehicles navigation system and receives inputs from the vehicles global positioning system/inertial measurement unit (GPS/IMU) 22. The estimated range derived from the system 16 can be output back to the navigation system for avoidance measures or output to a display 24.

The invention is described in detail herein. In Section 1, there are described some preliminary notions that are used throughout the document. In Section 2, there are presented some novel mathematical results which form the basis of the invention. In Section 3, the monocular passive ranging (MPR) method is described. In Section 4, there is described a novel image processing algorithm which can be used as a module of the MPR algorithm when the rigid body to which the range is to be estimated is an aircraft. In Section 5, another variant of the MPR method is described.

1. Preliminaries

In this section, we describe concepts and notation to be used throughout this document. For any time t, let the positions of two feature points on the intruder be (x₁(t), y₁(t), z₁(t)) and (x₂(t), y₂(t), z₂(t)) respectively, where x₁(t), y₁(t), z₁(t), x₂(t), y₂(t), z₂(t)ϵ, where is the set of real numbers. Here the coordinates are measured with respect to a fixed global frame of reference. Let the position of the sensor be (x_s(t), y_s(t), z_s(t)), where again x_s(t), y_s(t), z_s(t) ϵ . Let r_i(t)ϵ³ be the position vector of feature point i with respect to the sensor at time t, where iϵ{1,2}. In other words, we have,r_i(t):=[x_i(t)−x_s(t) y_i(t)−y_s(t) z_i(t)−z_s(t)].Given a vector Xϵ^N, let ∥X∥ denote the Euclidean norm of the vector. We let X(t)⁽ⁿ⁾ denote the nth derivative of X(t) with respect to time, assuming that the nth derivative exists. For notational convenience, we also assume that X(t)⁽⁰⁾ is equivalent to X(t). We will also use ∥X(t)∥ as an alternate representation of ∥X(t)∥⁽¹⁾. Similarly, we let ∥X(t)∥⁽ⁿ⁾ denote the nth derivative of ∥X(t)∥ with respect to time. In what follows, for simplicity we shall omit the dependence on time wherever it is clear from the context.

2. Novel Mathematical Conditions for Maneuverless Monocular Passive Ranging

In this section, we present the results on the observability of the intruder states. Intuitively speaking, an intruder state is said to be observable if the state can estimated from the available measurements. In particular, we are interested in observing the range to the intruder at a given time.

Lemma 1. Let the intruder be a rigid body undergoing a pure translational motion. Let (x₁(t), y₁(t), z₁(t)) and (x₂(t), y₂(t), z₂(t)) be the locations of two points on the intruder at time t. Assuming that the motion is sufficiently smooth for the first order derivatives to exist, we have {dot over (x)}₁={dot over (x)}₂; {dot over (y)}₁={dot over (y)}₂; ż₁=ż₂.

The proof is a straightforward consequence of the fact that the body is rigid and undergoes no rotation. We are now ready to state our main results on observability. The results are summarized by the following theorems.

Theorem 1. Let (x(t), y(t), z(t)) represent the trajectory of a given point on a mobile rigid intruder at time t. Likewise, let (x_s(t), y_s(t), z_s(t)) represent the trajectory of the sensor. Let r(t) denote the vector (x(t)−x_s(t), y(t)−y_s(t), z(t)−z_s(t)). Let the measurement vector be defined by h(t):=r(t)/∥r(t)∥ and let h_x(t), h_y(t) and h_z(t) denote the three components of h(t). Let the following conditions be true:

• (i) there exists a positive integer N≥2 such that r(t)⁽ⁱ⁾ exists for all t and for all iϵ{1, . . . ,2N−2};
• (ii) r(t)^(N) is equal to zero.
• (iii) h_x⁽ⁱ⁾, h_y⁽ⁱ⁾ and h_z⁽ⁱ⁾ are non zero for all i ϵ {0, . . . ,N−2}.

Then the quantity ∥r(t)∥⁽¹⁾/∥r(t)∥ is observable if the following conditions are true at time t.

• (i)

$\frac{h_x^{\left(1\right)}}{h_y^{\left(1\right)}}\ne \frac{h_x}{h_y},$ and

• (ii) for N>2 there does not exist a set of real numbers {a₁, . . . , a_N−2} with at least one of them not equal to zero such that both the following conditions are true:

$\begin{array}{cc}{h}_x^{\left(N-1\right)}=\sum _{i=1}^{N-2}{h}_x^{\left(N-1-i\right)}{\alpha }_i,& \left(1\right)\\ h_y^{\left(N-1\right)}=\sum _{i=1}^{N-2}{h}_x^{\left(N-1-i\right)}{\alpha }_i.& \left(2\right)\end{array}$

Theorem 2. Let (x₁(t), y₁(t), z₁(t)) and (x₂(t), y₂(t), z₂(t)) be the locations of two points on a mobile rigid intruder at time t. Let the rigid body's motion be purely translational. Let [u_s(t) v_s(t) w_s(t)] and [u(t) v(t) w(t)] be the velocity vectors of the sensor and the intruder at time t. Let RϵSO(3) be a rotation matrix such that (1/∥[u(t) v(t) w(t)]∥)[u(t) v(t) w(t)]^T=(1/∥[x₁(t)−x₂(t) y₁(t)−y₂(t) z₁(t)−z₂(t)]∥)R[x₁(t)−x₂(t) y₁(t)−y₂(t) z₁(t)−z₂(t)]^T. Let r_i(t) denote the vector (x_i(t)−x_s(t),y_i(t)−y_s(t),z_i(t)−z_s(t)) for iϵ{1,2}. Let the measurements be given by h_i(t):=r_i(t)/∥r_it∥ for iϵ{1,2}. Let h_ix, h_iy and h_iz denote the x, y and z components of h_i. Suppose that the following conditions hold:

• (i)

$\frac{{r_1}^{\left(1\right)}}{r_1}$ is observable;

• (ii) {dot over (β)}₂≠0 and y₂≠π/2, where β_i=arctan

$\frac{h_{i_x}}{h_{i_y}}$ and

${\gamma }_i=\arctan \frac{h_{i_z}}{\sqrt{\left(h_{i_x}^2+h_{i_y}^2\right)}};$

• (iii) The rotation matrix R is known; and
• (iv) The velocity of the sensor is non-zero.

Then, the states of the intruder are observable as long as the vectors [u_s(t) v_s(t) w_s(t)] and [u(t) v(t) w(t)] are not parallel.

Theorem 3. Let the intruder be a rigid body undergoing a translation motion. Let (x₁(t), y₁(t), z₁(t)) and (x₂(t), y₂(t), z₂(t)) be the locations of two feature points on the intruder at time t. Let [u_s(t) v_s(t) w_s(t)] and [u(t) v(t) w(t)] be the velocity vector of the sensor and the intruder at time t. Let α ϵ [0, π/2] be the angle between the vectors [u(t) v(t) w(t)] and [x₁(t)−x₂(t) y₁(t)−y₂(t) z₁(t)−z₂(t)]. For parallel vectors, we assume that α=0 and for perpendicular vectors α=π/2.

Suppose that the following conditions hold:

• (i)

$\frac{{r_1}^{\left(1\right)}}{r_1}$ is observable;

• (ii) {dot over (β)}₂≠0, and y₂≠π/2;
• (iii) The angle α is known; and
• (iv) The velocity of the sensor is non-zero.

Then, the following statements hold true.

• (i) If α=0 and (u_s, v_s, w_s) is not parallel to the vector (x₁−x₂, y₁−y₂, z₁−z₂), then the intruder states are observable;
• (ii) If α=π/2 and the vector (u_s, v_s, w_s) is not perpendicular to the vector (x₁−x₂, y₁−y₂, z₁−z₂), then the intruder states are observable;

One of the critical assumptions in the above theorem is that (u_s, v_s, w_s) is not parallel or perpendicular to the vector(x₁−x₂, y₁−y₂, z₁−z₂). This again corresponds to events that are highly improbable Now that we have established the mathematical conditions for the range to an intruder to be observable, we present a novel monocular passive ranging method that does not require the sensor platform to maneuver.

3. A Novel Maneuverless Monocular Passive Ranging (MPR) Method

In this section, a novel MPR method based on the applications of Theorem 1 and Theorems 2 and 3 is described. We assume that there is a mobile platform containing a passive monocular camera system as the sensor. The sensor system is used to detect the presence of the other moving objects 10 in the vicinity and estimate the range to the objects. (See FIGS. 4 and 5) A number of algorithms for detection of objects in images have been proposed in the prior art and we assume that a suitable algorithm is available for use with our proposed invention—our invention concerns the estimation of range to a moving object (intruder) once it has been detected. The following steps describe our method assuming that detection has been accomplished and that there is no noise in any of the measurements.

• (i) Get the pixel locations (i₁, j₁) and (i₂, j₂) of two feature points in the image of the intruder, denoted for short by p₁ and p₂, respectively, such that either (1) the rotation matrix R is known or (2) the angle α is known and is equal to either 0 or π/2 radians The matrix R is defined in Theorem 2 (See FIG. 6—Case 1). The angle α as defined in Theorem 3 is equal to 0 or π/2 (See FIG. 7—Case 2).
• (ii) Assume a suitable value of N, where N is a positive integer such that r₁^(N)=0 is a good approximation of the dynamics of the spatial location of feature point p₁ on the intruder.
• (iv) Measure the angles β₁, β₂, y₁ and y₂ where the quantities are defined in Theorem 2. This can be accomplished by knowing the intrinsic parameters of the camera (focal length, field of view and the camera mounting angle), the extrinsic parameters of the camera (yaw, pitch and roll angles of the sensor platform obtained from the onboard IMU) and the locations p₁ and p₂ in the image plane. This is equivalent to the measurement of h_x1, h_y1, h_z1, h_x2, h_y2 and h_z2 where h_xi, h_yi and h_zi are the x, y and z components of r_i/∥r_i∥.
• (v) Estimate the first 2N−3 derivatives of h_xi, h_yi and h_zi using the current value of h_xi, h_yi and h_zi and their past values stored in the memory 20 Use the constructive proof of Theorem 1 to estimate ∥r₁∥⁽¹⁾/∥r₁∥. Notice that N≥2 because N=1 implies that the range is constant.
• (vi) Now we know ∥r₁∥⁽¹⁾/∥r₁∥ and the first derivatives of β₁, β₂, y₁ and y₂. Depending on whether we know R or whether we know that α is 0 or π/2 radians, use the method in the proofs of Theorem 2 and Theorem 3 respectively to estimate ∥r₁∥.

For ease of reference of the reader, the equations of dynamics and measurements for a Recursive Filter Implementation for Cases 1 and 2 are laid out below.

Recursive Filter Implementation for Section 3—Case (1)

Dynamics Equationsx₁^(N)−x_s^(N)=0, y₁^(N)−y_s^(N)=0, z₁^(N)−z_s^(N)=0x₂^(N)−x_s^(N)=0, y₂^(N)−y_s^(N)=0, z₂^(N)−z_s^(N)=0x₁⁽ⁱ⁾=x₂⁽ⁱ⁾, 1≤i≤N y₁⁽ⁱ⁾=y₂⁽ⁱ⁾, 1≤i≤N z₁⁽ⁱ⁾=z₂⁽ⁱ⁾, 1≤i≤N u(R₂₁(x₂−x₁)+R₂₂(y₂−y₁)+R₂₃(z₂z₁)) =v(R₁₁(x₂−x₁)+R₁₂(y₂y₁)+R₁₃(z₂−z₁))u(R₃₁(x₂−x₁)+R₃₂(y₂y₁)+R₃₃(z₂−z₁)) =w(R₁₁(x₂−x₁)+R₁₂(y₂y₁)+R₁₃(z₂−z₁))Here N is a suitable positive integer, the superscript (N) denotes the Nth derivative with respect to time, u=x₁⁽¹⁾, v=y₁⁽¹⁾, w=z₁⁽¹⁾ and R_ij is the ij element of the rotation matrix R, and (x_s, y_s, z_s) is the 3D position of the sensor platform.Measurement Equations

${\beta }_1=\arctan \frac{h_{1_x}}{h_{1_y}}$ ${\beta }_2=\arctan \frac{h_{2_x}}{h_{2_y}}$ ${\gamma }_1=\arctan \frac{h_{1_z}}{\sqrt{\left(h_{1_x}^2+h_{1_y}^2\right)}}$ ${\gamma }_2=\arctan \frac{h_{2_z}}{\sqrt{\left(h_{2_x}^2+h_{2_y}^2\right)}}$ $\mathrm{where}$ $h_{1_x}=\frac{x_1-x_s}{\sqrt{{\left(x_1-x_s\right)}^2+{\left(y_1-y_s\right)}^2+{\left(z_1-z_s\right)}^2}}$ $h_{1_y}=\frac{y_1-y_s}{\sqrt{{\left(x_1-x_s\right)}^2+{\left(y_1-y_s\right)}^2+{\left(z_1-z_s\right)}^2}}$ $h_{1_z}=\frac{z_1-z_s}{\sqrt{{\left(x_1-x_s\right)}^2+{\left(y_1-y_s\right)}^2+{\left(z_1-z_s\right)}^2}}$ $h_{2_x}=\frac{x_2-x_s}{\sqrt{{\left(x_2-x_s\right)}^2+{\left(y_2-y_s\right)}^2+{\left(z_2-z_s\right)}^2}}$ $h_{2_y}=\frac{y_2-y_s}{\sqrt{{\left(x_2-x_s\right)}^2+{\left(y_2-y_s\right)}^2+{\left(z_2-z_s\right)}^2}}$ $h_{2_z}=\frac{z_2-z_s}{\sqrt{{\left(x_2-x_s\right)}^2+{\left(y_2-y_s\right)}^2+{\left(z_2-z_s\right)}^2}}$ Knowing the dynamics and the measurement equations, standard non-linear filtering techniques such as the Extended Kalman Filter can be implemented to get an estimate of the states (x₁, y₁, z₁), (x₂, y₂, z₂), and (u, v, w). Let the estimates be denoted by ({circumflex over (x)}₁, ŷ₁, {circumflex over (z)}₁), ({circumflex over (x)}₂, ŷ₂, {circumflex over (z)}₂), and (û, {circumflex over (v)}, ŵ). Once these states are estimated, the estimated range is given as follows:

$r^{\mathrm{est}}=\sqrt{{\left(\frac{{\hat{x}}_1+{\hat{x}}_2}{2}-x_s\right)}^2+{\left(\frac{{\hat{y}}_1+{\hat{y}}_2}{2}-y_s\right)}^2+{\left(\frac{{\hat{z}}_1+{\hat{z}}_2}{2}-z_s\right)}^2}$ $V^{\mathrm{est}}=\sqrt{{\hat{u}}^2+{\hat{v}}^2+{\hat{w}}^2}$

Recursive Filter Implementation for Section 4.3—Case (2) and for Section 5 (below)

Dynamics Equationsx₁^(N)−x_s^(N)=0, y₁^(N)−y_s^(N)=0, z₁^(N)−z_s^(N)=0x₂^(N)−x_s^(N)=0, y₂^(N)−y_s^(N)=0, z₂^(N)−z_s^(N)=0x₁⁽ⁱ⁾=x₂⁽ⁱ⁾, 1≤i≤N y₁⁽ⁱ⁾=y₂⁽ⁱ⁾, 1≤i≤N z₁⁽ⁱ⁾=z₂⁽ⁱ⁾, 1≤i≤N If α=0 radians, then:v(x₂−x₁)=u(y₂−y₁)w(y₂−y₁)=v(z₂−z₁)If α=π/2 radians, then:u(x₂−x₁)+v(y₂−y₁)+w(z₂−z₁)=0.Here N is a suitable positive integer, the superscript (N) denotes the Nth derivative with respect to time, u=x₁⁽¹⁾, v=y₁⁽¹⁾, w=z₁⁽¹⁾ and (x_s, y_s, z_s) is the 3D position of the sensor platform.Measurement Equations

${\beta }_1=\arctan \frac{h_{1_x}}{h_{1_y}}$ ${\beta }_2=\arctan \frac{h_{2_x}}{h_{2_y}}$ ${\gamma }_1=\arctan \frac{h_{1_z}}{\sqrt{\left(h_{1_x}^2+h_{1_y}^2\right)}}$ ${\gamma }_2=\arctan \frac{h_{2_z}}{\sqrt{\left(h_{2_x}^2+h_{2_y}^2\right)}}$ $\mathrm{where}$ $h_{1_x}=\frac{x_1-x_s}{\sqrt{{\left(x_1-x_s\right)}^2+{\left(y_1-y_s\right)}^2+{\left(z_1-z_s\right)}^2}}$ $h_{1_y}=\frac{y_1-y_s}{\sqrt{{\left(x_1-x_s\right)}^2+{\left(y_1-y_s\right)}^2+{\left(z_1-z_s\right)}^2}}$ $h_{1_z}=\frac{z_1-z_s}{\sqrt{{\left(x_1-x_s\right)}^2+{\left(y_1-y_s\right)}^2+{\left(z_1-z_s\right)}^2}}$ $h_{2_x}=\frac{x_2-x_s}{\sqrt{{\left(x_2-x_s\right)}^2+{\left(y_2-y_s\right)}^2+{\left(z_2-z_s\right)}^2}}$ $h_{2_y}=\frac{y_2-y_s}{\sqrt{{\left(x_2-x_s\right)}^2+{\left(y_2-y_s\right)}^2+{\left(z_2-z_s\right)}^2}}$ $h_{2_z}=\frac{z_2-z_s}{\sqrt{{\left(x_2-x_s\right)}^2+{\left(y_2-y_s\right)}^2+{\left(z_2-z_s\right)}^2}}$ Knowing the dynamics and the measurement equations, standard non-linear filtering techniques such as the Extended Kalman Filter can be implemented to get an estimate of the states (x₁, y₁, z₁), (x₂, y₂, z₂), and (u, v, w). Let the estimates be denoted by ({circumflex over (x)}₁, ŷ₁, {circumflex over (z)}₁), ({circumflex over (x)}₂, ŷ₂, {circumflex over (z)}₂), and (û, {circumflex over (v)}, ŵ). Once these states are estimated, the estimated range is given as follows.If α=0 radians, then:

$r_{\alpha =0}^{\mathrm{est}}=\sqrt{{\left(\frac{{\hat{x}}_1+{\hat{x}}_2}{2}-x_s\right)}^2+{\left(\frac{{\hat{y}}_1+{\hat{y}}_2}{2}-y_s\right)}^2+{\left(\frac{{\hat{z}}_1+{\hat{z}}_2}{2}-z_s\right)}^2}$ $V_{\alpha =0}^{\mathrm{est}}=\sqrt{{\hat{u}}^2+{\hat{v}}^2+{\hat{w}}^2}$ If α=π/2 radians, then:

$r_{\alpha =\pi /2}^{\mathrm{est}}=\sqrt{{\left(\frac{{\hat{x}}_1+{\hat{x}}_2}{2}-x_s\right)}^2+{\left(\frac{{\hat{y}}_1+{\hat{y}}_2}{2}-y_s\right)}^2+{\left(\frac{{\hat{z}}_1+{\hat{z}}_2}{2}-z_s\right)}^2}$ $V_{\alpha =\pi /2}^{\mathrm{est}}=\sqrt{{\hat{u}}^2+{\hat{v}}^2+{\hat{w}}^2}$

In a practical situation, measurements are always corrupted by noise. In such cases, a recursive filter implementation, such as the Extended Kalman Filter, can be used to estimate the range and other states of the intruder.

An exemplary recursive EKF method is described below

Let

${\alpha }_{\mathrm{correlation}}=\frac{\left(\left(x_1-x_2\right)u+\left(y_1-y_2\right)v+\left(z_1-z_2\right)w\right)}{\left(x_1-x_2\right),\left(y_1-y_2\right),\left(z_1-z_2\right)u,v,w}$

Where the states x₁, y₁, z₁, x₂, y₂, z₂, u, v, w are recursively estimated without the knowledge of whether α=0 or απ/2

We determine thatα=π/2 if α_correlation≤δα=0 if α_correlation>1−δ

where δ is a tuning parameter

Furthermore, instead of using a single EKF, one could use a bank of EKF's, or a particle filter for recursively computing the range. In this case, one can compute α=0 or α=π/2 for each particle in the bank and then use a voting mechanism to determine if α=0 or α=π/2. If the number of particles with α=0 is larger than the number of particles for which α=π/2, then we make a determination that α=0 and vice versa.

4. Image Processing for Feature Point Identification in Aircraft Images

In the previous section, there is described a monocular passive ranging method. In step (i) of the method, two feature points on the intruder are identified such that the the rotation matrix R is known or that the angle α is known and is equal to either 0 or π/2 radians. In this section, a novel method to identify two feature points such that the angle α is equal to either 0 or π/2 radians is described. The method also provides an algorithm to check if the angle α is equal to 0 or π/2 radians. This method is applicable to the case when the intruder is an aircraft. A software implementation is laid out in FIG. 8. The basic steps involved are as follows.

• (i) Extraction of the intruder aircraft silhouette. FIGS. 4 and 5 demonstrate the silhouettes extracted by our algorithm on two sample images of commercial airliners downloaded from the internet. The left images show the original images whereas the right images show the respective silhouettes.
• (ii) Extraction of the two principal components of the silhouette. In order to extract the principal components, a line searching method similar to Hough transform is used. For this method, the binary aircraft silhouette image is used. First the straight line L₁ which contains the maximum number of silhouette pixels is estimated. This serves as the longer principal component. We also estimate a second principal component. This is defined as the straight line L₂ that contains the maximum number of silhouette pixels subject to the condition that it makes an angle of greater than 45 degree with the longer principal component. This approach has been observed to yield better results than more traditional principal component analyses approaches such as Singular Value Decomposition.

In FIGS. 4 and 5 (right image), the two principal components are indicated by the lines L₁ and L₂ on the silhouette. The end points L_1a, L_1b of the longer primary principal component correspond to either the end points of the fuselage (nose and tail) or to the wing tips. For example, in FIG. 4 (top right), the end points of the longer principal component line correspond to the head and the tail of the aircraft, whereas the end points L_1a, L_1b of the longer principal component in FIG. 5 correspond to the two wing tips of the aircraft.

• (iii) Computation of the normalized distance between the point of intersection of the two principal components and the centroid of the silhouette. For example, in FIGS. 4, the point of intersection of the two principal components is indicated by an asterisk, and the centroid of the silhouette is indicated by a circle. The normalized distance is defined as the distance between the asterisk and the circle normalized by the length of the longer principal component which in this case is the length of line L₁ in the silhouette. The normalized distance can be used to check whether the end points correspond to the end points of the fuselage or to the wing tips or neither. The intuition is that if the end points are on the fuselage then the normalized distance should be larger than if the end points are the wing tips.
• (iv) Creation of a training data set from images for which the identity of the end points of the longer principal component is known. For this data set, the normalized distances are computed. We downloaded 50 aircraft images from the internet—25 corresponding to the case where the end points are on the fuselage and the remaining 25 corresponding to the case where the end points are the wing tips. The normalized distances are extracted for each of the images in the training data set. It is observed that these distances can be clustered into two intervals, [0,0.3] and [0.5,1]. The former and the latter intervals correspond to the wing tips and the fuselage images respectively. This is in accordance with our intuition.
• (v) Comparison of the extracted normalized distances in the test images with the normalized distances in the training data. For robust classification, instead of using a single test image we use a sequence of consecutive images, compute the normalized distances for each of the images, and then perform statistical T-tests to test the hypotheses of whether the mean of the normalized distances in the sequence of the test images is greater than 0.5 or less than 0.3. If the hypothesis that the mean is greater than 0.5 is true, then this implies that for the images in the sequence the end points correspond to the fuselage or equivalently that α is equal to zero radians. If the other hypothesis is true then it implies that the end points correspond to the wing tips or that α is equal to π/2 radians.

4a. Silhouette Extraction by Image Segmentation

The exemplary method below can be used as an alternative to the method set forth in FIG. 8 and paragraphs 93-98 described above.

A silhouette extraction module is used to identify the two feature points of the object in the field of view. This works by first performing image segmentation in a neighborhood of the detected aircraft to obtain the aircraft silhouette, and then identifying two feature points from the silhouette.

Segmentation:

The image segmentation module receives a sequence of image windows of a fixed size around the detected aircraft. The steps in the image segmentation process are:

1. Compute image features: These are extracted by

• • Performing Principal Components Analysis (PCA) on a small subsampled image.
  • Using the PCA to compute one or two color components with maximum variance from the input color image
  • Optionally, using the gradient of the first component.
  • Pixel coordinates are also used as features to ensure a connected silhouette.
  • Normalizing all features and weight them in a manner that improves the resulting segmentation.

2. Shape prior: The segmentation resulting from the previous frame is also used as a feature to maintain continuity of the silhouette. The shape prior is registered with the new image by shifting the shape prior till the centroids of the edge maps of the previous and current frame match. Optionally, the shift may be estimated by matching the brightest or darkest pixels in the current and previous frames.

3. Segmentation: A fast DBSCAN algorithm is used to segment the feature image into multiple clusters. The largest clusters and those touching the image boundary are marked as background and the rest are marked as foreground. The foreground cluster is the silhouette of the intruder aircraft.

Robust Feature Point identification:

The feature points of the silhouette are further identified in a manner robust to errors in silhouette extraction. A straight line is fit to the silhouette using least squares fitting. This is the principal axis of the silhouette. All silhouette points are projected to the principal axis and a truncated histogram of the points is calculated along the principal axis. The centroid and interquartile range (IQR) are calculated from the histogram. The robust feature point estimates are the two points a distance of IQR away from the centroid along the principal axis.

5. Likelihood Testing Based Range Estimation

In this section, another method for estimating the range to the intruder is described. The difference of this method from the method in Section 3 Case 2 (See FIG. 6 —Case 2) is that we only need to know that the angle α is either 0 or π/2 radians but it is not required to know which one of the two it is. A software implementation is laid out in FIG. 9. The sensor system is used to detect the presence of the other moving objects 10 in the vicinity and estimate the range to the objects. (See FIGS. 4 and 5) A number of algorithms for detection of objects in images have been proposed in the prior art and we assume that a suitable algorithm is available for use with our proposed invention—our invention concerns the estimation of range to a moving object (intruder) once it has been detected. The following steps describe our method assuming that detection has been accomplished and that there is no noise in any of the measurements.

• (i) Get the pixel locations (i₁, j₁) and (i₂, j₂) of two feature points in the image of the intruder, denoted for short by p₁ and p₂, respectively, such that the angle α is equal to either 0 or π/2 radians but it is not known which one of the two it is. If the intruder is an aircraft, steps (i) and (ii) of the method in Section 4 can be used to find the end points of the longer principal component of the silhouette of the intruder in the image. These end points serve as points p₁ and p₂.
• (ii) Assuming the angle α is equal to zero, estimate the range to the intruder by application of the method described in Section 3. Once the range to the intruder is estimated, other states such as the range rate can also be estimated since ∥r(t)∥⁽¹⁾/ ∥r(t)∥ is observable according to Theorem 1. Using the knowledge of the bearing angles, and bearing angle derivatives, estimate the velocity of the intruder. Let the estimated range and magnitude of the velocity of the intruder be denoted by r_α=0^est and V_α=0^est.
• (iii) Assuming the angle α is equal to π/2, estimate the range and range rate of the intruder. Using the knowledge of the bearing angles, and bearing angle derivatives, estimate the velocity of the intruder. Let the estimated range and magnitude of the velocity of the intruder be denoted by r_α=π/2^est and V_α=π/2^est.
• (iv) Let p_range(r_α=0^est) be the likelihood of the range estimate being equal to r_α=0^est based on prior sensor detection ranges. For example, if r_α=0^est is equal to 10 nautical miles and the sensor detection range can never be more than 5 nautical miles, then the likelihood p_range(r_α=0^est) should be small. Likewise, if r_α=0^est is a small value then too the likelihood p_range(r_α=0^est) should be small because then the intruder should have been detected earlier. Likewise, let p_range(r_α=π/2^est) be the likelihood of the range estimate being equal to r_α=π/2^est. Similarly, one can estimate likelihoods p_speed(V_α=0^est) and p_speed(V_α=π/2^est) based on typical speeds of the intruders in the domain of operation.
• (v) If

$\frac{p_{\mathrm{range}}\left(r_{\alpha =0}^{\mathrm{est}}\right)p_{\mathrm{speed}}\left(V_{\alpha =0}^{\mathrm{est}}\right)}{p_{\mathrm{range}}\left(r_{\alpha =\pi /2}^{\mathrm{est}}\right)p_{\mathrm{speed}}\left(V_{\alpha =\pi /2}^{\mathrm{est}}\right)}$ is greater than a certain threshold M then α is equal to 0 and the correct range estimate is r_α=0^est.

If, on the other hand,

$\frac{p_{\mathrm{range}}\left(r_{\alpha =0}^{\mathrm{est}}\right)p_{\mathrm{speed}}\left(V_{\alpha =0}^{\mathrm{est}}\right)}{p_{\mathrm{range}}\left(r_{\alpha =\pi /2}^{\mathrm{est}}\right)p_{\mathrm{speed}}\left(V_{\alpha =\pi /2}^{\mathrm{est}}\right)}$ is less than a certain threshold m then α is equal to π/2 and the correct range estimate is r_α=π/2^est.

The previous method requires the knowledge of the likelihood functions. Any suitable likelihood function can be used as long as it satisfied two properties. First, it should be a positive real valued function. Second, the function must assume a strictly greater value for an argument that is more likely to occur than another argument that is less likely to occur. The particular choice of the likelihood function depends on the needs of the user and domain of operation. For example, for use in an unmanned aircraft collision avoidance application, the likelihood functions should be designed after a careful study of the types of intruder aircraft operating in the airspace and their performance capabilities as well as the type of passive sensor being used. Likewise, the thresholds M and m are tunable design parameters and should be chosen based on the application scenario and system objectives.

6. Collision Detection

Previously we established that one can compute r¹(t) and r²(t)—the range to the wto feature points using the method described in the patent. We now present a method to predict an imminent collision.

We introduce the following notation. Let t_current be the time at which the range to the feature points is computed. Define constants T_safe and R_safe as safe time and safe distance which allow for the either the ownship or the intruder to maneuver and avoid an impending collision. These constants depend on the dynamics of the ownship and the intruder. They are defined by the needs of the application. We determine that a collision is imminent if at a critical time t*≥t_current in the future if |rⁱ(t*)|≤R_safe for i=1 or 2, t*−t_current≥0 and t*−t_current≤T_safe

In general case one can determine t* as

$\underset{t}{\mathrm{argmin}}r^i\left(t\right)$ for i=1 or 2. Depending on the motion of the intruder and ownship t* might not be unique. We check for the collision detection condition for t* that is greater than t_current and closest to it among all the possible t*.

For the special case where intruder and the ownship are not accelerating i.e. both of them have linear motion then t* is computed using the formula

$t^{*}=-\underset{i}{\mathrm{argmax}}\left[\frac{\left(x_i-x_s\right)+\left(y_i-y_s\right)+\left(z_i-z_s\right)}{\left(u-u_s\right)+\left(v-v_s\right)+\left(w-w_s\right)}\right]+t_{\mathrm{current}},i=1,2$

It can therefore be seen that the present invention provides a maneuverless passive ranging method which is based on extracting feature points on a detected object and assuming a direction of movement based on those selected feature points. For these reasons, the instant invention is believed to represent a significant advancement in the art which has substantial commercial merit.

While there is shown and described herein certain specific structure embodying the invention, it will be manifest to those skilled in the art that various modifications and rearrangements of the parts may be made without departing from the spirit and scope of the underlying inventive concept and that the same is not limited to the particular forms herein shown and described except insofar as indicated by the scope of the appended claims.

Claims

1. In a computerized system including a camera mounted on a moving vehicle, a method of predicting a collision between a foreign moving object and said moving vehicle, the method comprising the steps of: acquiring with said camera, consecutively in real time, a plurality of images of said foreign moving object;detecting said foreign moving object within each of said plurality of images;identifying two related feature points p1 and p2 on said detected foreign moving object within each of said plurality of images;calculating ranges r1(t) and r2(t) to said identified feature points p1 and p2 on said detected foreign moving object based on changes in the positions of the feature points p1 and p2 in the sequential images; anddetermining that a collision condition is imminent if at a time t*≥tcurrent in the future if |ri(t*)|≤Rsafe for i=1 or 2, t*−tcurrent≥0 and t*−tcurrent≤Tsafe where tcurrent is the time at which the ranges r1(t) and r2(t) to the feature points p1 and p2 are computed andwhere Tsafe and Rsafe define a safe time and a safe distance which allow for the moving vehicle or the foreign moving object to maneuver to avoid a collision.

2. The method of claim 1 wherein

3. The method of claim 1 further comprising the step of checking for said collision condition for t* that is greater than tcurrent and closest to it among all possible t*.

4. The method of claim 2 further comprising the step of checking for said collision condition for t* that is greater than tcurrent and closest to it among all possible t*.

5. The method of claim 1 where

6. The method of claim 2 where

7. The method of claim 6 further comprising the step of checking for said collision condition for t* that is greater than tcurrent and closest to it among all possible t*.

8. The method of claim 1 wherein said collision condition is selectively used by said computerized system to automatically alter a trajectory of said moving vehicle.

9. The method of claim 6 wherein said collision condition is selectively used by said computerized system to automatically alter a trajectory of said moving vehicle.

10. In a computerized system including a camera mounted on a moving vehicle, a method of predicting a collision between a foreign moving object and said moving vehicle, the method comprising the steps of acquiring with said camera, consecutively in real time, a plurality of images of said foreign moving object;detecting said foreign moving object within each of said plurality of images;identifying two feature points p1 and p2 on said detected foreign moving object within each of said plurality of images;calculating ranges r1(t) and r2(t) to said identified feature points p1 and p2 on said detected foreign moving object based on changes in the positions of the feature points p1 and p2 in the sequential images; anddetermining that a collision condition is imminent if at a time t*≥tcurrent in the future if |ri(t*)|≤Rsafe for i=1 or 2, t*−tcurrent≥0 and t*−tcurrent≤Tsafe where tcurrent is the time at which the ranges r1(t) and r2(t) to the feature points p1 and p2 are computed andwhere Tsafe and Rsafe define a safe time and a safe distance which allow for the moving vehicle or the foreign moving object to maneuver to avoid a collision;wherein:the two feature points p1 and p2 on said detected foreign moving object satisfy a predetermined geometric relationship with a velocity vector of said detected foreign moving object, said step of identifying said feature points p1 and p2 further comprising the step of determining pixel locations (i1, j1) and (i2, j2) of said feature points p1 and p2 within said image; andthe calculating ranges step further includes recursively calculating the range of said detected foreign moving object based on changes in the positions of the feature points p1 and p2 in the sequential images and further based on said predetermined geometric relationship, said step of recursively calculating said range further comprising the step of converting said pixel locations to relative azimuth and elevation angles denoted by β1, β2, y1, y2,wherein the step of recursively calculating said range comprises recursively calculating said range where an angle α is known and is equal to either 0 or π/2 radians, and the following conditions are defined:(x1, y1, z1) and (x2, y2, z2) represent 3D locations of said feature points (i1, j1) and (i2, j2);[u, v, w] represents a 3D velocity vector of the object;angle α is the angle between the vectors [u v w] and [x1−x2 y1−y2 z1−z2]; anda determination of whether α is equal to either 0 or π/2 is made using an Extended Kalman Filter (EKF) where

11. The method of claim 10 wherein α is estimated using a bank of EKFs, and α=0 or α=π/2 is computed for each filter in each bank and then a voting mechanism is used to determine if α=0 or απ/2 where if the number of filters with α=0 is larger than the number of filters for which α=π/2, then α=0, andif the number of filters with α=π/2 is larger than the number of filters for which α=0, then α=π/2.

12. The method of claim 10 wherein α is estimated using a particle filter, and α=0 or α=π/2 is computed for each particle in the filter and then a voting mechanism is used to determine if α=0 or α=π/2 where if the number of particles with α=0 is larger than the number of particles for which α=π/2, then α=0, andif the number of particles with α=π/2 is larger than the number of particles for which α=0, then α=π/2.

13. In a computerized system including a camera mounted on a moving vehicle, a method of predicting a collision between a foreign moving object and said moving vehicle, the method comprising the steps of: acquiring with said camera, consecutively in real time, a plurality of images of said foreign moving object;detecting said foreign moving object within each of said plurality of images;identifying two feature points p1 and p2 on said detected foreign moving object within each of said plurality of images;calculating ranges r1(t) and r2(t) to said identified feature points p1 and p2 on said detected foreign moving object based on changes in the positions of the feature points p1 and p2 in the sequential images; anddetermining that a collision condition is imminent if at a time t*≥tcurrent in the future if |ri(t*)|≤Rsafe for i=1 or 2, t*−tcurrent≥0 and t*−tcurrent≤Tsafe where tcurrent is the time at which the ranges r1(t) and r2(t) to the feature points p1 and p2 are computed andwhere Tsafe and Rsafe define a safe time and a safe distance which allow for the moving vehicle or the foreign moving object to maneuver to avoid a collision;wherein the two feature points p1 and p2 on said detected foreign moving object satisfy a predetermined geometric relationship with a velocity vector of said detected foreign moving object, said step of identifying said feature points p1 and p2 further comprising the step of determining pixel locations (i1, j1) and (i2, j2) of said feature points p1 and p2 within said image; andwherein the calculating the ranges step further includes recursively calculating the range of said detected foreign moving object based on changes in the positions of the feature points p1 and p2 in the sequential images and further based on said predetermined geometric relationship,wherein the step of recursively calculating said range comprises recursively calculating said range where an angle α is known and is equal to either 0 or π/2 radians, and the following conditions are defined:(x1, y1, z1) and (x2, y2, z2) represent 3D locations of said feature points (i1, j1) and (i2, j2);[u, v, w] represents a 3D velocity vector of the object;angle α is the angle between the vectors [u v w] and [x1−x2 y1−y2 z1−z2];said step of determining pixel locations (i1, j1) and (i2, j2) comprises extracting a silhouette of said moving object by image segmentation.

14. The method of claim 13 wherein said step of image segmentation comprises the steps of: receiving a sequence of images of fixed size around said detected moving object;computing image features from said images by extracting principal components from said images and performing principal component analysis (PCA) on a sub-sample of images;using the PCA to compute at least one color component with a maximum variance from an input color image;normalizing said image features; andweighting the image features to improve segmentation;segmenting the image features into a plurality of clusters;identifying the largest clusters;marking the largest clusters and those clusters which touch an image boundary as background;marking the remaining clusters as foreground and identifying said remaining clusters as said silhouette of said moving object.

15. The method of claim 14 wherein a segmentation result from a previous image of said sequence of images is used to maintain continuity of said silhouette between images.

16. The method of claim 15, wherein said silhouette from said previous image (shape prior) is registered with a current image by shifting the shape prior until a centroid of an edge map of the previous image matches the current image.

17. The method of claim 16 wherein said step of shifting is estimated by matching a brightest or darkest pixel in the current image and the previous image.

18. In a computerized system including a camera mounted on a moving vehicle, a method of predicting a collision between a foreign moving object and said moving vehicle, the method comprising the steps of: acquiring with said camera, consecutively in real time, a plurality of images of said foreign moving object;detecting said foreign moving object within each of said plurality of images;identifying two feature points p1 and p2 on said detected foreign moving object within each of said plurality of images;calculating ranges r1(t) and r2(t) to said identified feature points p1 and p2 on said detected foreign moving object based on changes in the positions of the feature points p1 and p2 in the sequential images; anddetermining that a collision condition is imminent if at a time t*≥tcurrent in the future if |ri(t*)|≤Rsafe for i=1 or 2, t*−tcurrent≥0 and t*−tcurrent≤Tsafe where tcurrent is the time at which the ranges r1(t) and r2(t) to the feature points p1 and p2 are computed andwhere Tsafe and Rsafe define a safe time and a safe distance which allow for the moving vehicle or the foreign moving object to maneuver to avoid a collision;wherein the two feature points p1 and p2 are two robust feature points p1 and p2 on said detected foreign moving object, where p1 and p2 satisfy a predetermined geometric relationship with a velocity vector of said detected foreign moving object, said step of identifying said feature points p1 and p2 further comprising the step of determining pixel locations (i1, j1) and (i2, j2) of said robust feature points p1 and p2 within said image; andwherein the calculating the ranges step further includes recursively calculating the range of said object based on changes in the positions of the robust feature points p1 and p2 in the sequential images and further based on said predetermined geometric relationship, said step of recursively calculating said range further comprising the step of converting said pixel locations to relative azimuth and elevation angles denoted by β1, β2, y1, y2, andwherein said step of determining pixel locations (i1, j1) and (i2, j2) of said robust feature points p1 and p2 within said image comprises the steps ofextracting a silhouette of said moving object comprising a plurality of pixels associated with said detected foreign moving object,fitting a straight line to the silhouette using a least squares method wherein said straight line defines a principal axis of said silhouette,projecting all silhouette points to the principal axis,calculating a truncated histogram of said silhouette points, andcalculating a centroid and interquartile range (IQR) from said histogram,wherein said robust feature points p1 and p2 comprise two points having a distance of IQR away from the centroid along the principal axis.

19. The method of claim 18 wherein the step of recursively calculating said range further comprises recursively calculating said range where an angle α is known and is equal to either 0 or π/2 radians, and the following conditions are defined: (x1, y1, z1) and (x2, y2, z2) represent 3D locations of said feature points (i1, j1) and (i2, j2);[u, v, w] represents a 3D velocity vector of the object; andangle α is the angle between the vectors [u v w] and [x1−x2 y1−y2 z1−z2].