Algorithm and code optimizations for real-time passive ranging by imaging detection on single DSP

Abstract

Monocular vision-based passive ranging system is attractive for potential applications in navigation, transportation and traffic control, robotics, and air defense. Conventionally system estimates target distance by using imaging direction and distance-related features, and the latter are extracted from the image sequence. It is not only challenging to DSP device hardware capability but also to programming optimization that to transplant the traditional feature extraction algorithm to a real-time system due to the complexity and time-consuming. In this paper, a simplified scale invariant feature transform image matching algorithm and code optimization procedure are developed, and the improvement of operation speed is demonstrated clearly. For an image sequence with size of $256\times 256$ pixels, the operation speed was improved to 25–27 frames per second from 4 s per frame by using a single C64x+ DSP core, which meets the requirements of real-time ranging algorithm perfectly while the ranging error is less than $7\,\%$.

Appendix

1.1 Ranging model

In this paper, we take the geography coordinate system o-xyz as the host coordinates and take the north, the west, and the upper direction as the positive direction of $x-$, $y-$, and $z-$ axis, respectively. It is a reasonable assumption that the state of our measure platform where the camera fixed is known by GPS or other inboard system. The known state of the platform includes the azimuth, the pitching, the radial distance to point $o$, the velocity, the acceleration, and the attitude.

The platform itself also constitutes another coordinate O-XYZ, i.e., the platform coordinate. If we take an aircraft as the platform, then the nose head, the right wing, and the cabin head is the positive $y-$, $x-$, and $z-$ axis of the platform coordinate in turn, and it is also a right-hand coordinate. The airborne measure platform in the geography coordinate is shown in Figure 2.

In the $n$-th sampling, the point $O$ with coordinates $(x_{n}, y_{n}, z_{n})$ in o-xyz coordinate, the position of the moving target in the measure platform coordinate is expressed as $(r_{n},\alpha _{n},\beta _{n})$ in spherical form. Here, $\alpha _{n}$ and $\beta _{n}$ are the target’s azimuth and pitching in measure platform coordinate. And the sightline of camera to the target in the geography coordinate could be expressed in terms of direction vector $(l_{n}, m_{n}, n_{n})$ as below.

$$\begin{aligned} \left( {\begin{array}{l} l_n \\ m_n \\ n_n \\ \end{array}} \right) =\left( {{\begin{array}{lll} {t_{11}^n }&{}\quad {t_{12}^n }&{}\quad {t_{13}^n } \\ {t_{21}^n }&{}\quad {t_{22}^n }&{}\quad {t_{23}^n } \\ {t_{31}^n }&{}\quad {t_{32}^n }&{}\quad {t_{33}^n } \\ \end{array} }} \right) \left( {\begin{array}{ll} \cos \alpha _n&{}\quad \cos \beta _n \\ \sin \alpha _n&{}\quad \cos \beta _n \\ \sin \beta _n \\ \end{array}} \right) \end{aligned}$$

(8)

Here, $\left( {{\begin{array}{lll} {t_{11}^n }&{}\quad {t_{12}^n }&{}\quad {t_{13}^n } \\ {t_{21}^n }&{}\quad {t_{22}^n }&{}\quad {t_{23}^n } \\ {t_{31}^n }&{}\quad {t_{32}^n }&{}\quad {t_{33}^n } \\ \end{array} }} \right)$ is the transposed matrix of the direction vector for $x-$, $y-$, and $z-$ axis in o-xyz coordinates.

It is supposed that there is a scale $x_{0}$ with one dimension in the target, which is invariable to the rotation of the camera in two adjacent sampling times. The projection of this scale in the focal plane is called the target’s feature linearity. Now, let us take into account the normal condition in which both the target and the measure platform are moving. Figure 3 illustrates the recursive ranging model based on the feature linearity.

Fig. 3

The airborne measurement platform in geography coordinates

In Fig. 4, $T$ and $S$ are the target and the surveyor (camera), respectively. Subscript $n$ or $n+1$ is the sampling time. $T_{n}T_{n+1}, S_{n}S_{n+1}$ is the moving trace of the target and the platform between the $n$-th and $(n+1)$-th sampling time, respectively, while $\varphi _{n}$ and $\varphi _{n+1}$ are the angles of the target’s trace to the camera’s sightline at each sampling time.

Fig. 4

The ranging model based on feature linearity

The focuses of optical system of the camera are supposed as $f_{n}$ and $f_{n+1}$ at the $n$-th and $(n+1)$-th sampling, and the length of the feature linearity in the camera focal plane is $L_{n}$ and $L_{n+1}$ at meantime. According to the geometry imaging principle, it could be concluded as following.

$$\begin{aligned} \frac{r_{n+1}}{r_n }=\frac{f_{n+1} }{f_n }\frac{L_n }{L_{n+1}}\frac{\sin \varphi _{n+1} }{\sin \varphi _n } \end{aligned}$$

(9)

1.2 Ranging algorithm

Based on Eq. 9, the following recursion passive ranging equation is derived.

$$\begin{aligned} C_4 r_{n+1}^4 +C_3 r_{n+1}^3 +C_2 r_{n+1}^2 +C_1 r_{n+1} +C_0 =0 \end{aligned}$$

(10)

Where

$$\begin{aligned} C_4&= H[1-(l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n )^{2}]\end{aligned}$$

(11)

$$\begin{aligned} C_3&= 2H\{l_{n+1} (x_{n+1} -x_n )+m_{n+1} (y_{n+1} -y_n )\nonumber \\&+n_{n+1} (z_{n+1} -z_n )-(l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n )\nonumber \\&\times [l_n (x_{n+1}\! -\!x_n )\!+\!m_n (y_{n+1} -y_n )\!+\!n_n (z_{n+1} -z_n )]\}\nonumber \\ \end{aligned}$$

(12)

$$\begin{aligned} C_2&= H\{[l_n (x_{n+1} -x_n )+m_n (y_{n+1} -y_n ) \nonumber \\&+n_n (z_{n+1} -z_n )]^{2}+(x_{n+1} -x_n )^{2}+(y_{n+1} -y_n )^{2}\nonumber \\&+(z_{n+1} -z_n )^{2}\} \end{aligned}$$

(13)

$$\begin{aligned} C_1&= 0\end{aligned}$$

(14)

$$\begin{aligned} C_0&= k_2 r_n^2 +k_1 r_n+k_0 \end{aligned}$$

(15)

$$\begin{aligned} H&= \left( \frac{f_n }{f_{n+1}}\right) ^{2}\left( \frac{L_{n+1} }{L_n }\right) ^{2}\frac{1}{r_n^2 } \end{aligned}$$

(16)

In Eq. 15

$$\begin{aligned} k_2&= (l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n )^{2}-1\end{aligned}$$

(17)

$$\begin{aligned} k_1&= 2\{l_n (x_{n+1} -x_n )+m_n (y_{n+1} -y_n ) \nonumber \\&+n_n (z_{n+1} -z_n )-(l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n )\nonumber \\&[l_{n+1} (x_{n+1} -x_n )+m_{n+1} (y_{n+1} -y_n ) \nonumber \\&+n_{n+1} (z_{n+1} -z_n )]\} \end{aligned}$$

(18)

$$\begin{aligned} k_0&= [l_{n+1} (x_{n+1} -x_n )+m_{n+1} (y_{n+1} -y_n ) \nonumber \\&+n_{n+1} (z_{n+1} -z_n )]^{2}-(x_{n+1} -x_n )^{2} \nonumber \\&-(y_{n+1}-y_n )^{2}-(z_{n+1} -z_n )^{2} \end{aligned}$$

(19)

Equation 10 is multiplied by $r_{n}^{2}$ in both sides, then

$$\begin{aligned}&C_4 r_n^2 r_{n+1}^4 +C_3 r_n^2 r_{n+1}^3 +C_2 r_n^2 r_{n+1}^2\nonumber \\&\quad +C_1 r_n^2 r_{n+1} +C_0 r_n^2 =0 \end{aligned}$$

(20)

Here, we denote

$$\begin{aligned} \left\{ {\begin{array}{l} C_{40} =C_4 r_n^2 \\ C_{30} =C_3 r_n^2 \\ C_{20} =C_2 r_n^2 \\ C_{10} =C_1 r_n^2 =0\quad (C_1 =0) \\ C_{00} =C_0 r_n^2 =k_2 r_n^4 +k_1 r_n^3 +k_0 r_n^2 \\ \end{array}} \right. \end{aligned}$$

(21)

And then the Eq. 20 can turn into as below.

$$\begin{aligned}&C_{40} r_{n+1}^4 +C_{30} r_{n+1}^3 +C_{20} r_{n+1}^2 +k_2 r_n^4\nonumber \\&\quad +k_1 r_n^3 +k_0 r_n^2 =0 \end{aligned}$$

(22)

Substitute the distance ratio of the adjacent sampling time, $\rho =r_{n}/r_{n+1}$, into Eq. 22, we can get

$$\begin{aligned}&(C_{40} +k_2 \rho ^{4})r_{n+1}^4 +(C_{30} +k_1 \rho ^{3})r_{n+1}^3 +(C_{20}\nonumber \\&\quad +k_0 \rho ^{2})r_{n+1}^2 =0 \end{aligned}$$

(23)

After reduction of Eq. 23, we can get

$$\begin{aligned} D_2 r_{n+1}^4 +D_1 r_{n+1}^3 +D_0 r_{n+1}^2 =0 \end{aligned}$$

(24)

Considering the target’s distance $r_{n+1}\ne 0$, so Eq. 24 can be written as

$$\begin{aligned} D_2 r_{n+1}^2 +D_1 r_{n+1}+D_0 =0 \end{aligned}$$

(25)

Equations 10–19 and 21, the coefficients of the ranging equation can be determined, where

$$\begin{aligned} D_2&= (\rho ^{4}-{H}^{\prime })[(l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n )^{2}-1]\end{aligned}$$

(26)

$$\begin{aligned} D_1&= 2{H}^{\prime }\{l_{n+1} (x_{n+1} -x_n )+m_{n+1} (y_{n+1} -y_n ) \nonumber \\&+\,n_{n+1} (z_{n+1} -z_n )-(l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n ) \nonumber \\&\times \, [l_n (x_{n+1} -x_n )+m_n (y_{n+1} -y_n ) \nonumber \\&+\,n_n (z_{n+1}-z_n )]\}+2\rho ^{3}\{l_n (x_{n+1} -x_n ) \nonumber \\&+\,m_n (y_{n+1} -y_n )+n_n (z_{n+1} -z_n ) \nonumber \\&-\,(l_{n+1} l_n +m_{n+1} m_n +n_{n+1} n_n )\times [l_{n+1} (x_{n+1} -x_n ) \nonumber \\&+\,m_{n+1} (y_{n+1} -y_n )+n_{n+1}\times (z_{n+1} -z_n )]\} \end{aligned}$$

(27)

$$\begin{aligned} D_0&= {H}^{\prime }\{[l_n (x_{n+1} -x_n )+m_n (y_{n+1} -y_n ) \nonumber \\&+\,n_n (z_{n+1} -z_n )]^{2}+(x_{n+1} -x_n )^{2}+(y_{n+1}-y_n )^{2} \nonumber \\&+\,(z_{n+1} -z_n )^{2}\}+\rho ^{2}\{[l_{n+1} (x_{n+1}-x_n ) \nonumber \\&+\,m_{n+1} (y_{n+1} -y_n )+n_{n+1} (z_{n+1} -z_n )]^{2} \nonumber \\&-\,(x_{n+1} -x_n )^{2}-(y_{n+1} -y_n )^{2}-(z_{n+1} -z_n )^{2}\}\nonumber \\\end{aligned}$$

(28)

$$\begin{aligned} {H}^{\prime }&= Hr_n^2 =\left( \frac{f_n }{f_{n+1} }\right) ^{2}\left( \frac{L_{n+1} }{L_n }\right) ^{2} \end{aligned}$$

(29)

In Eqs. 23–28, $\rho =L_{n+1}/L_{n}$ can be obtained from the target image matching. Between the $n$-th and the $(n+1)$-th frame of the image sequence, we can obtain three matched SIFT key points, and this is the minimum requirement for image tracking. In our experiment, $L_{n}$ or $L_{n+1}$ is taken as the radius of the circumcircle of three matched points.