- Email: [email protected]
Recursive Motion Recovery Based on Dynamic Vision
Proceedings of the 17th World Congress The International Federation of Automatic Control Seoul, Korea, July 6-11, 2008
Recursive Motion Recovery Based on Dynamic Vision Xinkai Chen Department of Electronic and Information Systems, Shibaura Institute of Technology, 307 Fukasaku, Minuma-ku, Saitama-shi, Saitama 337-8570, Japan (Tel: +81-48-687-5085; e-mail: [email protected]) Abstract: The motion recovery for a class of movements in the space by using stereo vision is considered by observing at least three points in this paper. The considered motion equation can cover a wide class of practical movements in the space. The observability of this class of movement is clarified. The estimations of the motion parameters which are all time-varying are developed in the proposed algorithm based on the second method of Lyapunov. The assumptions about the perspective system are reasonable, and the convergence conditions are intuitive and have apparently physical interpretations. The proposed recursive algorithm requires minor a priori knowledge about the system and can alleviate the noises in the image data. Furthermore, the proposed algorithm is modified to deal with the occlusion phenomenon. Simulation results show the proposed algorithm is effective even in the presence of measurement noises. 1. INTRODUCTION In the study of machine vision, observing the motion and the structure of a moving object in the space by using the image data with the aid of CCD camera(s) has been studied recently. The motion treated in this field is composed of a rotation part and a translation part. A very typical method is the application of the extended Kalman filter (EKF). Numerous successful results have been reported until now where the formulation is based on a discrete expression of the motion, and the observability conditions are derived based on the perspective observations of a group of points (Azarbayejani and Pentland, 1995; Chiuso, et al, 2002). Such a recursive algorithm obviously alleviates the noises in the image data in contrast to the non-recursive methods based on solving a set of nonlinear algebraic equations (Kanatani, 1990). It should be mentioned that some theoretical convergence conditions of discrete EKF have been established both as observer and filter (Reif, et al, 1998). The observation problem for continuous time perspective systems has been studied in the point of view of dynamical system theory by Ghosh, et al (2000) and Loucks (1994). A necessary and sufficient condition for the perspective observability is given by Dayawansa, et al (1994) for the case that the motion parameters are constants. For the movements with piecewise constant motion parameters, the perspective observability problems are clarified by Soatto (1997) for the cases of observing one point or a group of points. Furthermore, for the observer design, some simple formulations for observing the position of a moving object are proposed in Chen and Kano (2002, 2004) and Jankovic, et al (1995). The proposed observers are guaranteed to converge in an arbitrarily large (but bounded) set of initial conditions, and since the convergence is exponential it is believed that the performance of the new observers are reliable, robust and would quickly compute the position on real data.
978-3-902661-00-5/08/$20.00 © 2008 IFAC
This paper considers the problem of motion recovery for a class of movements under perspective observation. Naturally, the motions are formulated in continuous-time settings and the so-called motion parameters are assumed to be all timevarying. The motion parameters are estimated by using image data observed through pin-hole camera with constant focal length (normalized to unity). The basic and important idea is to analyze the extent to which we can develop a scheme that is guaranteed to converge by observing minimum number of points. A dynamical systems approach is employed since it provides us with powerful mathematical tools, and a nonlinear observer is developed based on the second method of Lyapunov (Satry and Bodson, 1989). The considered motion equation can cover a wide class of practical movements in the space. The observability of this class of movement is clarified by observing three points. The estimation of the motion parameter is developed in this paper. The formulated problem can be converted into the observation of a dynamical system with nonlinearities. It should be noted that smoothened image data instead of the measured one is used in the proposed formulation in order to alleviate the noises in the image data. The assumptions about the perspective system are reasonable, and the convergence conditions are intuitive and have apparently physical interpretations. The attraction of the new method lies in that the algorithm is very simple, easy to be implemented practically. Furthermore, the proposed method requires minor a priori knowledge about the system and can cope with a much more general class of perspective systems. It should be noted that the changing of focal length is not considered in this paper. Furthermore, the algorithm is modified to deal with the occlusion phenomenon. Simulation results show the proposed algorithm is effective even in the presence of measurement noises. 2. PROBLEM STATEMENT Consider the movement of the object described by
7594
10.3182/20080706-5-KR-1001.2938
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
ω1 (t ) ω 2 (t )⎤ ⎡ x1 (t ) ⎤ ⎡b1 (t ) ⎤ ⎡ x1 (t ) ⎤ ⎡ 0 d ⎢ ⎢ ⎥ x 2 (t )⎥ = ⎢ − ω1 (t ) ω 3 (t )⎥⎥ ⎢⎢ x 2 (t )⎥⎥ + ⎢⎢b2 (t )⎥⎥ (1) 0 dt ⎢ ⎢⎣ x3 (t ) ⎥⎦ ⎢⎣− ω 2 (t ) − ω 3 (t ) 0 ⎥⎦ ⎢⎣ x3 (t )⎥⎦ ⎢⎣b3 (t )⎥⎦ where x(t ) = [x1 , x 2 , x3 ] is the position; ω i (t ) and bi (t ) (i = 1, 2, 3) are the motion parameters.
θ (t ) = [b1 , b2 , b3 , ω1 , ω 2 , ω 3 ]T
T Δ = [θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ] ,
(6)
and
T
It is supposed that the observed position by Camera 1 is defined by y (t ) = [ y1 (t ),
⎡x y 2 (t )] = ⎢ 1 , ⎣ x3
x2 ⎤ ⎥ x3 ⎦
⎡φ (t ) ⎤ ⎡ y φ (t ) = ⎢ 1 ⎥ = ⎢ 3 ⎣φ2 (t )⎦ ⎢⎣ 0
[
y * (t ) = y1* (t ),
]
x2 − n ⎤ ⎥, x3 ⎦
y 2 1 + y1 − y1 y1 y 2
2
y1 y 2 ⎤ 2⎥ 1 + y2 ⎥⎦ (7)
Thus, the first two equations in (5) can be rewritten as (2)
⎡ y1 (t ) ⎤ ⎢ y (t )⎥ = φ (t ) ⋅ θ (t ) . ⎣ 2 ⎦
and the observed position by Camera 2 is defined by ⎡x −m y 2* (t ) = ⎢ 1 , ⎣ x3
− y1 y3 − y 2 y3
0 y3
(8)
Similarly, for y * (t ) , it gives (3)
where m and n are constants. The perspective observations are defined in (2) and (3). The combination of the observations in (2) together with (3) is called “stereo vision”. In this paper, we make the following assumptions. (A1). m and n are known constants with m 2 + n 2 ≠ 0 . (A2). The motion parameters ω i (t ) and bi (t ) (i = 1, 2, 3) are bounded. (A3). x3 (t ) meets the condition x3 (t ) > η > 0 , where η is a constant. (A4). y (t ) and y * (t ) are bounded.
⎡ y1* (t )⎤ * ⎢ * ⎥ = φ (t ) ⋅ θ (t ) y t ( ) ⎣ 2 ⎦ with ⎡φ * (t )⎤ φ * (t ) = ⎢ 1* ⎥ ⎣φ 2 (t )⎦ ⎡y =⎢ 3 ⎣0
0 y3
− y1* y 3 − y 2* y 3
y 2* + ny 3 1 + y1* ( y1* + my 3 ) y1* ( y 2* + ny 3 ) ⎤ ⎥ * * * y 2 ( y1 + my 3 ) 1 + y 2* ( y 2* + my 3 ) ⎦ − y1 − my 3
(10) From (2) and (3), y 3 (t ) can be calculated by the average y 3 (t ) = m
Remark 1. It is easy to see that assumptions (A3) and (A4) are reasonable by referring to the practical systems. The purpose of this paper is to estimate the motion parameters ω i (t ) and bi (t ) (i = 1, 2, 3) by using the perspective observations. 3. FORMULATION OF THE MOTION RECOVERY Define
(9)
y1 − y1* y 2 − y 2* + n . m2 + n2 m2 + n2
(11)
Thus, φ (t ) and φ * (t ) are available. In the following, the vectors φ (t ) ⋅ θ (t ) and φ * (t ) ⋅ θ (t ) are estimated in section 3.1 by using the perspective observations defined in (2) and (3). Then, the motion parameters ω i (t ) and bi (t ) (i = 1, 2, 3) are estimated in section 3.2 by using the stereo observation of at least three points. 3.1 Identification of φ (t )θ (t ) and φ * (t )θ (t )
y 3 (t ) =
1 . x3 (t )
(4)
In the following, the observer of system (8) is formulated. We consider the system described by
Then, equation (1) can be transformed as ⎧ y1 (t ) = ω 2 + ω1 y2 + ω 2 y12 + ω3 y1 y2 + b1 y3 − b3 y1 y3 ⎪ 2 ⎨ y 2 (t ) = ω3 − ω1 y1 + ω 2 y1 y2 + ω3 y2 + b2 y3 − b3 y2 y3 . ⎪ y (t ) = ω y y + ω y y − b y 2 2 1 3 3 2 3 3 3 ⎩ 3
⎡ yˆ1 (t ) ⎤ ⎡ w1 (t ) ⎤ ⎢ ⎥=⎢ ⎥, ⎣⎢ yˆ 2 (t )⎦⎥ ⎣ w2 (t )⎦ (5)
⎡ yˆ 1 (0) ⎤ ⎡ y1 (0) ⎤ ⎢ˆ ⎥=⎢ ⎥; ⎣ y 2 ( 0) ⎦ ⎣ y 2 ( 0) ⎦
w i (t) = −( f i + αi )wi (t) + λˆi (t) ⋅ sign( yi − yˆ i ) + f iαi ( yi − yˆ i ) (13) λˆi (t ) = β i y i − yˆ i + α i ri (t ) ; (14)
(
ri (t ) = y i − yˆ i ,
Let
7595
(12)
)
(15)
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
(
)
+ λˆ1 (t ) − λ1 (e1 (t ) ⋅ sign(e1 (t )) + α1 e1 (t ) )
where f i ,α i , β i are positive constants, wi (0) can be any small constants, and ri (0) is chosen as ri (0) = 0 .
= − f1 r12 (t ) + r1 (t )η1 (t ) − (e1 (t ) + α 1e1 (t ) )λˆ1 (t ) ⋅ sign(e1 (t ))
(
)
+ λˆ1 (t ) − λ1 (e1 (t ) ⋅ sign(e1 (t )) + α1 e1 (t ) )
Let ⎡ w (t ) ⎤ w(t ) = ⎢ 1 ⎥ . ⎣ w2 (t ) ⎦
= − f 1 r12 (t ) + r1 (t )η1 (t ) − λ1e1 (t ) ⋅ sign(e1 (t )) − α 1λ1 e1 (t )
(16)
(25) Integrating the both sides of (25) from 0 to t yields
The next theorem is obtained. Theorem 1. All the generated signals in (12)-(15) are uniformly bounded and w(t ) is the asymptotic estimate of φ (t ) ⋅ θ (t ) , i.e. lim (φ (t ) ⋅ θ (t ) − w(t ) ) = 0 .
V (t ) = V (0) − f1 ∫ r12 (τ )dτ + ∫ (e1 (τ ) + α 1e1 (τ ) )η1 (τ )dτ t
0
t
− λ1 e1 (t ) − α1λ1 ∫ e1 (τ ) dτ 0
= V ( 0) − f
t
∫r
2 1 0 1
(τ )dτ + e1 (t )η1 (t ) − e1 (0)η1 (0)
+ ∫ e1 (τ )(− η1 (τ ) − α 1η1 (τ ) )dτ t
(17)
t →∞
t
0
0
t
− λ1 e1 (t ) − α 1λ1 ∫ e1 (τ ) dτ
Proof. For simplicity, we only give the proof for i=1. Let
0
≤ V (0) − f1 ∫ r12 (τ )dτ + e1 (t ) (η1 (t ) − λ1 ) −e1 (0)η1 (0) t
e1 (t ) = y1 (t ) − yˆ 1 (t ) .
(18)
0
+ ∫ e1 (τ ) (η1 (τ ) + α 1 η1 (τ ) − α 1λ1 (τ ) )dτ t
Differentiating e1 (t ) yields
0
t
e1 (t ) = φ1 (t ) ⋅ θ (t ) − w1 (t ), e1 (0) = 0 .
≤ V (0) − f 1 ∫ r12 (τ )dτ − e1 (0)η1 (0)
(26)
0
(19)
Thus, it can be seen that V (t ) and the integral
Now, define
r1 (t ) = e1 (t ) + α 1e1 (t ) .
(20)
Differentiating r (t ) yields
bounded. Therefore, r1 (t ) → 0 as t → ∞ . By the definition of r1 (t ) , it gives e1 (t ) → 0 and e1 (t ) → 0 as t → ∞ . The theorem is proved.
)
⎡ yˆ * (t ) ⎤ ⎡ w* (t ) ⎤ yˆ * (t ) = ⎢ 1* ⎥ = ⎢ 1* ⎥ , ⎣⎢ yˆ 2 (t )⎦⎥ ⎣ w2 (t ) ⎦
(21)
with
η1 (t ) =
d (φ1θ ) + ( f1 + α 1 )(φ1θ ) . dt
(22)
The uniformly boundedness of η1 (t ) and η1 (t ) can be easily derived by using the assumptions. Thus, there exist constants λ1 > 0 such that
η1 +
1
α1
η1 < λ1 .
(
(27)
Similar to Theorem 1, it can be concluded that w * (t ) is uniformly bounded and
(
)
lim φ * (t ) ⋅ θ (t ) − w * (t ) = 0 , t →∞
(28)
i.e. w* (t ) is the asymptotic estimate of φ * (t )θ (t ) .
)
2 1 2 1 ˆ r1 (t ) + λ1 (t ) − λ1 . 2 2β1
(24)
V (t ) = r1 (t ) η1 (t ) − f1 r1 (t ) − λˆ1 (t ) ⋅ sign(e1 (t ))
3.2 Identification of θ (t ) Relations (17) and (28) tell us that, by observing one point via stereo vision, four relations about θ (t ) can be obtained. It
Differentiating V (t ) yields
(
⎡ yˆ1* (0) ⎤ ⎡ y1* (0) ⎤ ⎢ * ⎥=⎢ * ⎥ ⎣ yˆ 2 (0)⎦ ⎣ y 2 (0) ⎦
⎡ w * (t ) ⎤ * where ⎢ 1* ⎥ Δ = w (t ) can be defined by referring (13)-(15) w t ( ) ⎣ 2 ⎦ by using the obtained image data y * (t ) from Camera 2.
(23)
Now, consider the Lyapunov candidate V (t ) =
(τ )dτ are
Similarly to (10), construct the equation
d r1 (t ) = (φ1θ − w1 ) + α 1 (φ1θ − w1 ) dt = η1 (t ) − f 1 r1 (t ) + λˆ1 (t ) sign(e1 )
(
t 2 0 1
∫r
) 7596
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
⎡ φ (t ) ⎤ can be easily checked that the rank of the matrix ⎢ * ⎥ is ⎣φ (t )⎦ three. It can be argued that the relations about θ (t ) can be increased by increasing the observation points. Since there are six entries in θ (t ) , it can be argued that at least two points are needed to get a solution of θ (t ) .
Now, suppose p points are observed. For the j-th point, we ⎡ φ ( j ) (t ) ⎤ ⎡ φ (t ) ⎤ ⎡ w(t ) ⎤ denote the obtained ⎢ * ⎥ and ⎢ * ⎥ as ⎢ *( j ) ⎥ and ⎣φ (t )⎦ ⎣ w (t )⎦ ⎣φ (t )⎦
respectively replaced by the generated smooth signals yˆ ( j ) (t ) and yˆ *( j ) (t ) , since y ( j ) (t ) − yˆ ( j ) (t ) → 0 and y *( j ) (t ) − yˆ *( j ) (t ) → 0 . As to the value of y3( j ) (t ) in Φ (t ) ,
although it can be calculated in (11) by using the image data, we use a smoothed signal yˆ 3( j ) (t ) to replace it in order to mitigate the influence of measurement noises. The signal yˆ 3( j ) (t ) is generated as follows. ( j) ⎛ y ( j ) − y *( j) ⎞ y ( j) − y* yˆ3( j) = λˆ(3 j ) (t ) sign ⎜ m 1 2 1 2 + n 2 2 22 − yˆ 3( j ) ⎟ (32) ⎜ ⎟ m +n m +n ⎝ ⎠
⎡ w ( j ) (t ) ⎤ ⎢ *( j ) ⎥ , respectively. ⎣ w (t ) ⎦
λˆ(3 j ) (t ) = γ ⋅ m
Define ⎡ w (1) (t ) ⎤ ⎡ φ (1) (t ) ⎤ ⎢ *(1) ⎥ ⎢ *(1) ⎥ ⎢ w (t ) ⎥ ⎢ φ (t ) ⎥ ⎢ # ⎥ ⎢ # ⎥ ⎢ ( j) ⎥ ⎢φ ( j ) (t ) ⎥ Φ (t ) = ⎢ *( j ) ⎥ , W (t ) = ⎢ w (t ) ⎥ . ⎢ w*( j ) (t ) ⎥ ⎢φ (t ) ⎥ ⎥ ⎢ ⎢ # ⎥ ⎢ # ⎥ ⎢φ ( p ) (t ) ⎥ ⎢ ( p) ⎥ ⎢ *( p ) ⎥ ⎢ w (t ) ⎥ ⎢⎣φ (t ) ⎥⎦ ⎢⎣ w*( p ) (t )⎥⎦
( j)
y1( j ) − y1* m2 + n2
( j)
+n
y 2( j ) − y 2* m2 + n2
− yˆ 3( j ) ,
(33)
where γ is a positive constant. Lemma 2. The generated signals yˆ 3( j ) (t ) and λˆ3 (t ) are uniformly bounded and
(29)
(
)
lim y 3( j ) (t ) − yˆ 3( j ) (t ) = 0 . t →∞
(34)
Proof. By the assumptions (A2)-(A4), it can be seen that 2
ω2 y1( j ) y3( j ) + ω3 y2( j) y3( j ) − b3 y3( j) is uniformly bounded, i.e. there exists a constant λ(3 j ) such that
By Theorem 1, it gives
lim(Φ (t ) ⋅ θ (t ) − W (t ) ) = 0 .
2
ω 2 y1( j ) y3( j ) + ω3 y 2( j ) y3( j ) − b3 y3( j ) < λ(3 j ) .
(30)
t →∞
About the rank of the matrix Φ (t ) , we have the next lemma.
(35)
By considering the Lyapunov candidate
(
)
(λ
)
Lemma 1. The matrix Φ (t ) is of full rank if and only if at least three points are not on a same line. Proof. The proof is omitted.
and differentiating it, the lemma can be easily proved.
Lemma 1 means that at least three points are needed in the proposed formulation.
The recursive algorithms of deriving yˆ ( j ) (t ) , yˆ *( j ) (t ) and
Theorem 2. If at least three observed points are not on a same line, then the motion parameters are observable and it holds
{
(
lim θ ( t ) − Φ T ( t )Φ ( t ) t→ ∞
(
)
−1
}
Φ T ( t )W ( t ) = 0 ,
(31)
)
−1
i.e. Φ T ( t )Φ ( t ) Φ T ( t )W ( t ) is the asymptotic estimate of the vector θ (t ) .
2
L(t ) = y 3( j ) (t ) − yˆ 3( j ) (t ) +
1
γ
( j) 3
− λˆ(3 j ) (t )
2
(36)
yˆ 3( j ) (t ) obviously alleviate the noises in the image data. By
replacing y ( j ) (t ) , y *( j ) (t ) and y3( j ) (t ) in the matrix ⎡ φ ( j ) (t ) ⎤ ( j) *( j ) ( j) ⎢ *( j ) ⎥ with yˆ (t ) , yˆ (t ) and yˆ 3 (t ) recpectively, we φ t ( ) ⎣ ⎦ ⎡ φˆ ( j ) (t ) ⎤ ⎡ φˆ ( j ) (t ) ⎤ get a matrix ⎢ *( j ) ⎥ . By combining the matrices ⎢ *( j ) ⎥ ⎢⎣φˆ (t )⎥⎦ ⎢⎣φˆ (t )⎥⎦
together for all j, we get the matrix Φˆ (t ) expressed as
Since the image data is directly used in Φ (t ) , the measurement noise will directly influence the accuracy of the estimation. In the practical application of the proposed algorithm, the image data y ( j ) (t ) and y *( j ) (t ) can be
7597
(
) (
)
(
) (
)
T T T T Φˆ (t ) = ⎡ φˆ (1) (t ) , φˆ*(1) (t ) , ", φˆ ( j ) (t ) , φˆ*( j ) (t ) ,
⎢⎣
(
) (
)
T T " , φˆ ( p ) (t ) , φˆ *( p ) (t ) ⎤ ⎥⎦
T
(37)
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
If Φ (t ) is of full rank, then Φˆ (t ) is of full rank when t is large enough. By using Theorem 1, Theorem 2 and Lemma 2, it can be concluded that
{
(
lim θ ( t ) − Φˆ T ( t )Φˆ ( t ) t→ ∞
)
−1
}
Φˆ T ( t )W ( t ) = 0 .
(38)
Thus, the motion parameters can be recovered by observing at least three points.
defined in (29) in which w ( j ) (t ) is replaced by wˆ ( j ) (t ) ; wˆ ( j ) (t ) can be similarly derived by a procedure defined in
(12)-(15), where the corresponding image data should be replaced by the virtual data yˆˆ1( j ) (t ) and yˆˆ 2( j ) (t ) ; the initial values at the instant τ ( j ) when the j-th point begins to be not visible should be chosen as
4. CONSIDERATION OF OCCLUSION In the practical applications, the occurrence of occlusion is inevitable. Thus, the algorithm should be modified in order to cope with this phenomenon. In the occurrence of occlusion, the image data of the observed point is not available. Thus, the image data should be replaced by some other virtual signals.
yˆˆ1 (τ ( j ) ) = yˆ1 (τ ( j ) − 0) ;
(42a)
yˆˆ 2 (τ ( j ) ) = yˆ 2 (τ ( j ) − 0) ;
(42b)
yˆˆ 3 (τ ( j ) ) = yˆ 3 (τ ( j ) − 0) .
(42c)
If the j-th point is not visible by the Camera 2 defined in (3), then the virtual signal for y *( j ) (t ) should be similarly derived. Furthermore, if the j-th point is not visible by both of the two cameras, the virtual signals for y ( j ) (t ) , y *( j ) (t ) and y3( j ) (t ) should be similarly derived.
Suppose the j-th point is not visible by the Camera 1 defined in (2). The method formulated in Section 3 of deriving yˆ ( j ) (t ) and yˆ 3( j ) (t ) is no longer useful. By referring to the dynamics in (5), the virtual signals for y ( j ) (t ) and y3( j ) (t ) are constructed by the following dynamical system
⎧ ˆˆ ˆˆ ˆˆ 2 ˆˆ ˆˆ ˆ ˆ ˆ ˆ ⎪ y1 (t ) = ω2 (t ) + ω1 (t ) y2 (t ) + ω2 (t ) y1 (t ) + ω3 (t ) y1 (t ) y2 (t ) ⎪ + bˆ1 (t ) yˆˆ 3 (t ) − bˆ3 (t ) yˆˆ1 (t ) yˆˆ 3 (t ) ⎪ ⎪ ˆ ˆ ˆ ˆ 2 ˆ ⎨ yˆ 2 (t ) = ωˆ 3 (t ) − ωˆ1 (t ) yˆ1 (t ) + ωˆ 2 (t ) yˆ1 (t ) yˆ 2 (t ) + ωˆ 3 (t ) yˆ 2 (t ) ⎪ + bˆ2 (t ) yˆˆ 3 (t ) − bˆ3 (t ) yˆˆ 2 (t ) yˆˆ 3 (t ) ⎪ ⎪ 2 ⎪ yˆˆ 3 (t ) = ωˆ 2 (t ) yˆˆ1 (t ) yˆˆ 3 (t ) + ωˆ 3 (t ) yˆˆ 2 (t ) yˆˆ 3 (t ) − bˆ3 (t ) yˆˆ 3 (t ) ⎩
[
θˆ(t ) = bˆ1 (t ), bˆ2 (t ), bˆ3 (t ), ωˆ 1 (t ), ωˆ 2 (t ), ωˆ 3 (t ) where defined by
]
T
The convergence of the computed motion parameters can be assured, if the total length of the intervals on which the data from two cameras is available is much longer than the total length of the intervals on which at least one camera is occluded. 5. SIMULATION RESULTS The simulation is done by the software Simulink in Matlab. The sampling period Δ is chosen as Δ = 0.02 . The measured image data at the sampling point kΔ is corrupted by a random noise which is in the range of 0.01 y (kΔ ) (or correspondingly 0.01 y * (kΔ) ). Consider the movement of the object described by
(39)
− 4 0.5⎤ ⎡ x1 (t ) ⎤ ⎡ 0 ⎤ ⎡ x1 (t ) ⎤ ⎡ 0 d ⎢ ⎥ ⎥ ⎥ ⎥⎢ ⎢ x2 (t )⎥ = ⎢ 4 0 0.4⎥ ⎢ x2 (t )⎥ + ⎢⎢sin t ⎥ dt ⎢ ⎢⎣ x3 (t ) ⎥⎦ ⎢⎣− 0.5 − 0.4 0 ⎥⎦ ⎢⎣ x3 (t ) ⎥⎦ ⎢⎣sin t ⎥⎦
is
(43)
+
ˆ θˆ ( t ) = ⎛⎜ Φˆ ( t ) ⎞⎟ Wˆ ( t ) ; ⎝
⎠
(40)
ˆ Φˆ (t ) denotes the corresponding matrix of Φˆ (t ) defined in ˆ (37) where φˆ ( j ) (t ) is replaced by φˆ ( j ) (t ) defined by ⎡ yˆˆ ( j ) ˆ φˆ ( j ) (t) = ⎢ 3 ⎣⎢ 0
( A )+
0 ˆyˆ ( j ) 3
(
2 − yˆˆ1( j ) yˆˆ 3( j ) yˆˆ 2( j ) 1 + yˆˆ1( j ) yˆˆ1( j ) yˆˆ 2( j ) ⎤ (41) 2⎥ − yˆˆ 2( j ) yˆˆ 3( j ) − yˆˆ1( j ) yˆˆ1( j ) yˆˆ 2( j ) 1 + yˆˆ 2( j ) ⎦⎥
)
−1
denotes A T A A T if A is of full rank, or the pseudo-inverse (Golub and Van Loan, 1996) of it if A is not of full rank; Wˆ (t ) denotes the corresponding vector of W (t )
Four points starting at [− 1, 1, 1] , [1, − 2, 2] , [0, 3, 3] and T
T
T
[2, 0, 2]T
are observed. It is assumed that the third and fourth points are not visible by both of the two cameras during the time period [1, 6]. Fig. 1 shows the difference between the real image data y1( 4) (t ) and its virtual value yˆˆ 1( 4 ) (t ) for the fourth point. The differences y ( 4 ) (t ) − yˆˆ ( 4) (t ) , y ( 4) (t ) − yˆˆ ( 4) (t ) , and 2
2
3
3
y (t ) − yˆˆ i(3) (t ) ( i = 1, 2, 3) are similar to that in Fig. 1. It can be seen that the virtual value of the image data during the occlusion is effectively constructed.
7598
( 3) i
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
The simulation results are shown in Figures 2-3. simulation results of the differences ω 2 (t ) − ωˆ 2 (t ) ω 3 (t ) − ωˆ 3 (t ) are very similar to that in Figure 2. simulation results of b (t ) − bˆ (t ) and b (t ) − bˆ (t ) is
The and The
very 1 1 3 3 similar to that in Fig. 3. It can be seen that very good estimates for the motion parameters are obtained even in the presence of measurement noises.
6. CONCLUSIONS The motion recovery for a class of movements in the space by using stereo vision has considered by observing multiple (at least three) points in this paper. The considered motion equation can cover a wide class of practical movements in the space. The estimations of the motion parameters which are all time-varying have been developed based on the second method of Lyapunov. The assumptions about the perspective system are reasonable, and the convergence conditions are intuitive and have apparently physical interpretations. The proposed method requires minor a priori knowledge about the system and can cope with a much more general class of perspective systems. Furthermore, the algorithm has been modified to deal with the occlusion phenomenon. Simulation results have shown that the proposed algorithm is effective even in the presence of measurement noises. REFERENCES
Fig.1 The difference y1( 4) (t ) − yˆˆ1( 4) (t ) for the fourth point.
Fig. 2. The difference between ω1 (t ) and ωˆ 1 (t ) .
Fig. 3. The difference between b2 (t ) and bˆ2 (t ) .
Azarbayejani, A. and A. Pentland (1995). Recursive estimation of motion, structure and focal length. IEEE Trans. on Pattern Analysis and Machine Intelligence, 17, 562-575. Chen, X. and H. Kano (2002). A new state observer for perspective systems. IEEE Trans. Automatic Control, 47, 658-663. Chen, X. and H. Kano (2004). State Observer for a class of nonlinear systems and its application to machine vision. IEEE Trans. Automatic Control, 49, 2085-2091. Chiuso, A., P. Favaro, H. Jin, and S. Soatto (2002). Structure from motion causally integrated over time. IEEE Trans Pattern Analysis & Machine Intelligence, 24, 523-535. Dayawansa, W., B.K. Ghosh, , C. Martin and X. Wang (1994). A necessary and sufficient condition for the perspective observability problem. Systems & Control Letters, 25, 159-166. Ghosh, B.K., H. Inaba, and S. Takahashi (2000). Identification of Riccati dynamics under perspective and orthographic observations. IEEE Trans. on Automatic Control, 45, 1267-1278. Jankovic, M. and B.K. Ghosh (1995). Visually guided ranging from observation of points, lines and curves via an identifier based nonlinear observer. Systems & Control Letters, 25, 63-73. Kanatani, K. (1990). Group-Theoretical Methods in Image Understanding. Springer-Verlag. Loucks, E.P. (1994). A perspective System Approach to Motion and Shape Estimation in Machine Vision. Ph.D Thesis, Washington Univ.. Reif, K., F. Sonnemann and R. Unbehauen (1998). An EKFbased nonlinear observer with a prescribed degree of stability. Automatica, 34, 1119-1123. Satry, S. and M. Bodson (1989). Adaptive Control, Stability, Convergence, and Robustness. Prentice Hall, Englewood Cliffs, New Jersey. Soatto, S. (1997). 3-D structure from visual motion: Modelling, representation and observability. Automatica, 33, 1287-1321. Golub, G.H. and C.F. Van Loan (1996). Matrix Computations. The Johns Hopkins University Press.
7599
Recursive Motion Recovery Based on Dynamic Vision Xinkai Chen Department of Electronic and Information Systems, Shibaura Institute of Technology, 307 Fukasaku, Minuma-ku, Saitama-shi, Saitama 337-8570, Japan (Tel: +81-48-687-5085; e-mail: [email protected]) Abstract: The motion recovery for a class of movements in the space by using stereo vision is considered by observing at least three points in this paper. The considered motion equation can cover a wide class of practical movements in the space. The observability of this class of movement is clarified. The estimations of the motion parameters which are all time-varying are developed in the proposed algorithm based on the second method of Lyapunov. The assumptions about the perspective system are reasonable, and the convergence conditions are intuitive and have apparently physical interpretations. The proposed recursive algorithm requires minor a priori knowledge about the system and can alleviate the noises in the image data. Furthermore, the proposed algorithm is modified to deal with the occlusion phenomenon. Simulation results show the proposed algorithm is effective even in the presence of measurement noises. 1. INTRODUCTION In the study of machine vision, observing the motion and the structure of a moving object in the space by using the image data with the aid of CCD camera(s) has been studied recently. The motion treated in this field is composed of a rotation part and a translation part. A very typical method is the application of the extended Kalman filter (EKF). Numerous successful results have been reported until now where the formulation is based on a discrete expression of the motion, and the observability conditions are derived based on the perspective observations of a group of points (Azarbayejani and Pentland, 1995; Chiuso, et al, 2002). Such a recursive algorithm obviously alleviates the noises in the image data in contrast to the non-recursive methods based on solving a set of nonlinear algebraic equations (Kanatani, 1990). It should be mentioned that some theoretical convergence conditions of discrete EKF have been established both as observer and filter (Reif, et al, 1998). The observation problem for continuous time perspective systems has been studied in the point of view of dynamical system theory by Ghosh, et al (2000) and Loucks (1994). A necessary and sufficient condition for the perspective observability is given by Dayawansa, et al (1994) for the case that the motion parameters are constants. For the movements with piecewise constant motion parameters, the perspective observability problems are clarified by Soatto (1997) for the cases of observing one point or a group of points. Furthermore, for the observer design, some simple formulations for observing the position of a moving object are proposed in Chen and Kano (2002, 2004) and Jankovic, et al (1995). The proposed observers are guaranteed to converge in an arbitrarily large (but bounded) set of initial conditions, and since the convergence is exponential it is believed that the performance of the new observers are reliable, robust and would quickly compute the position on real data.
978-3-902661-00-5/08/$20.00 © 2008 IFAC
This paper considers the problem of motion recovery for a class of movements under perspective observation. Naturally, the motions are formulated in continuous-time settings and the so-called motion parameters are assumed to be all timevarying. The motion parameters are estimated by using image data observed through pin-hole camera with constant focal length (normalized to unity). The basic and important idea is to analyze the extent to which we can develop a scheme that is guaranteed to converge by observing minimum number of points. A dynamical systems approach is employed since it provides us with powerful mathematical tools, and a nonlinear observer is developed based on the second method of Lyapunov (Satry and Bodson, 1989). The considered motion equation can cover a wide class of practical movements in the space. The observability of this class of movement is clarified by observing three points. The estimation of the motion parameter is developed in this paper. The formulated problem can be converted into the observation of a dynamical system with nonlinearities. It should be noted that smoothened image data instead of the measured one is used in the proposed formulation in order to alleviate the noises in the image data. The assumptions about the perspective system are reasonable, and the convergence conditions are intuitive and have apparently physical interpretations. The attraction of the new method lies in that the algorithm is very simple, easy to be implemented practically. Furthermore, the proposed method requires minor a priori knowledge about the system and can cope with a much more general class of perspective systems. It should be noted that the changing of focal length is not considered in this paper. Furthermore, the algorithm is modified to deal with the occlusion phenomenon. Simulation results show the proposed algorithm is effective even in the presence of measurement noises. 2. PROBLEM STATEMENT Consider the movement of the object described by
7594
10.3182/20080706-5-KR-1001.2938
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
ω1 (t ) ω 2 (t )⎤ ⎡ x1 (t ) ⎤ ⎡b1 (t ) ⎤ ⎡ x1 (t ) ⎤ ⎡ 0 d ⎢ ⎢ ⎥ x 2 (t )⎥ = ⎢ − ω1 (t ) ω 3 (t )⎥⎥ ⎢⎢ x 2 (t )⎥⎥ + ⎢⎢b2 (t )⎥⎥ (1) 0 dt ⎢ ⎢⎣ x3 (t ) ⎥⎦ ⎢⎣− ω 2 (t ) − ω 3 (t ) 0 ⎥⎦ ⎢⎣ x3 (t )⎥⎦ ⎢⎣b3 (t )⎥⎦ where x(t ) = [x1 , x 2 , x3 ] is the position; ω i (t ) and bi (t ) (i = 1, 2, 3) are the motion parameters.
θ (t ) = [b1 , b2 , b3 , ω1 , ω 2 , ω 3 ]T
T Δ = [θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ] ,
(6)
and
T
It is supposed that the observed position by Camera 1 is defined by y (t ) = [ y1 (t ),
⎡x y 2 (t )] = ⎢ 1 , ⎣ x3
x2 ⎤ ⎥ x3 ⎦
⎡φ (t ) ⎤ ⎡ y φ (t ) = ⎢ 1 ⎥ = ⎢ 3 ⎣φ2 (t )⎦ ⎢⎣ 0
[
y * (t ) = y1* (t ),
]
x2 − n ⎤ ⎥, x3 ⎦
y 2 1 + y1 − y1 y1 y 2
2
y1 y 2 ⎤ 2⎥ 1 + y2 ⎥⎦ (7)
Thus, the first two equations in (5) can be rewritten as (2)
⎡ y1 (t ) ⎤ ⎢ y (t )⎥ = φ (t ) ⋅ θ (t ) . ⎣ 2 ⎦
and the observed position by Camera 2 is defined by ⎡x −m y 2* (t ) = ⎢ 1 , ⎣ x3
− y1 y3 − y 2 y3
0 y3
(8)
Similarly, for y * (t ) , it gives (3)
where m and n are constants. The perspective observations are defined in (2) and (3). The combination of the observations in (2) together with (3) is called “stereo vision”. In this paper, we make the following assumptions. (A1). m and n are known constants with m 2 + n 2 ≠ 0 . (A2). The motion parameters ω i (t ) and bi (t ) (i = 1, 2, 3) are bounded. (A3). x3 (t ) meets the condition x3 (t ) > η > 0 , where η is a constant. (A4). y (t ) and y * (t ) are bounded.
⎡ y1* (t )⎤ * ⎢ * ⎥ = φ (t ) ⋅ θ (t ) y t ( ) ⎣ 2 ⎦ with ⎡φ * (t )⎤ φ * (t ) = ⎢ 1* ⎥ ⎣φ 2 (t )⎦ ⎡y =⎢ 3 ⎣0
0 y3
− y1* y 3 − y 2* y 3
y 2* + ny 3 1 + y1* ( y1* + my 3 ) y1* ( y 2* + ny 3 ) ⎤ ⎥ * * * y 2 ( y1 + my 3 ) 1 + y 2* ( y 2* + my 3 ) ⎦ − y1 − my 3
(10) From (2) and (3), y 3 (t ) can be calculated by the average y 3 (t ) = m
Remark 1. It is easy to see that assumptions (A3) and (A4) are reasonable by referring to the practical systems. The purpose of this paper is to estimate the motion parameters ω i (t ) and bi (t ) (i = 1, 2, 3) by using the perspective observations. 3. FORMULATION OF THE MOTION RECOVERY Define
(9)
y1 − y1* y 2 − y 2* + n . m2 + n2 m2 + n2
(11)
Thus, φ (t ) and φ * (t ) are available. In the following, the vectors φ (t ) ⋅ θ (t ) and φ * (t ) ⋅ θ (t ) are estimated in section 3.1 by using the perspective observations defined in (2) and (3). Then, the motion parameters ω i (t ) and bi (t ) (i = 1, 2, 3) are estimated in section 3.2 by using the stereo observation of at least three points. 3.1 Identification of φ (t )θ (t ) and φ * (t )θ (t )
y 3 (t ) =
1 . x3 (t )
(4)
In the following, the observer of system (8) is formulated. We consider the system described by
Then, equation (1) can be transformed as ⎧ y1 (t ) = ω 2 + ω1 y2 + ω 2 y12 + ω3 y1 y2 + b1 y3 − b3 y1 y3 ⎪ 2 ⎨ y 2 (t ) = ω3 − ω1 y1 + ω 2 y1 y2 + ω3 y2 + b2 y3 − b3 y2 y3 . ⎪ y (t ) = ω y y + ω y y − b y 2 2 1 3 3 2 3 3 3 ⎩ 3
⎡ yˆ1 (t ) ⎤ ⎡ w1 (t ) ⎤ ⎢ ⎥=⎢ ⎥, ⎣⎢ yˆ 2 (t )⎦⎥ ⎣ w2 (t )⎦ (5)
⎡ yˆ 1 (0) ⎤ ⎡ y1 (0) ⎤ ⎢ˆ ⎥=⎢ ⎥; ⎣ y 2 ( 0) ⎦ ⎣ y 2 ( 0) ⎦
w i (t) = −( f i + αi )wi (t) + λˆi (t) ⋅ sign( yi − yˆ i ) + f iαi ( yi − yˆ i ) (13) λˆi (t ) = β i y i − yˆ i + α i ri (t ) ; (14)
(
ri (t ) = y i − yˆ i ,
Let
7595
(12)
)
(15)
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
(
)
+ λˆ1 (t ) − λ1 (e1 (t ) ⋅ sign(e1 (t )) + α1 e1 (t ) )
where f i ,α i , β i are positive constants, wi (0) can be any small constants, and ri (0) is chosen as ri (0) = 0 .
= − f1 r12 (t ) + r1 (t )η1 (t ) − (e1 (t ) + α 1e1 (t ) )λˆ1 (t ) ⋅ sign(e1 (t ))
(
)
+ λˆ1 (t ) − λ1 (e1 (t ) ⋅ sign(e1 (t )) + α1 e1 (t ) )
Let ⎡ w (t ) ⎤ w(t ) = ⎢ 1 ⎥ . ⎣ w2 (t ) ⎦
= − f 1 r12 (t ) + r1 (t )η1 (t ) − λ1e1 (t ) ⋅ sign(e1 (t )) − α 1λ1 e1 (t )
(16)
(25) Integrating the both sides of (25) from 0 to t yields
The next theorem is obtained. Theorem 1. All the generated signals in (12)-(15) are uniformly bounded and w(t ) is the asymptotic estimate of φ (t ) ⋅ θ (t ) , i.e. lim (φ (t ) ⋅ θ (t ) − w(t ) ) = 0 .
V (t ) = V (0) − f1 ∫ r12 (τ )dτ + ∫ (e1 (τ ) + α 1e1 (τ ) )η1 (τ )dτ t
0
t
− λ1 e1 (t ) − α1λ1 ∫ e1 (τ ) dτ 0
= V ( 0) − f
t
∫r
2 1 0 1
(τ )dτ + e1 (t )η1 (t ) − e1 (0)η1 (0)
+ ∫ e1 (τ )(− η1 (τ ) − α 1η1 (τ ) )dτ t
(17)
t →∞
t
0
0
t
− λ1 e1 (t ) − α 1λ1 ∫ e1 (τ ) dτ
Proof. For simplicity, we only give the proof for i=1. Let
0
≤ V (0) − f1 ∫ r12 (τ )dτ + e1 (t ) (η1 (t ) − λ1 ) −e1 (0)η1 (0) t
e1 (t ) = y1 (t ) − yˆ 1 (t ) .
(18)
0
+ ∫ e1 (τ ) (η1 (τ ) + α 1 η1 (τ ) − α 1λ1 (τ ) )dτ t
Differentiating e1 (t ) yields
0
t
e1 (t ) = φ1 (t ) ⋅ θ (t ) − w1 (t ), e1 (0) = 0 .
≤ V (0) − f 1 ∫ r12 (τ )dτ − e1 (0)η1 (0)
(26)
0
(19)
Thus, it can be seen that V (t ) and the integral
Now, define
r1 (t ) = e1 (t ) + α 1e1 (t ) .
(20)
Differentiating r (t ) yields
bounded. Therefore, r1 (t ) → 0 as t → ∞ . By the definition of r1 (t ) , it gives e1 (t ) → 0 and e1 (t ) → 0 as t → ∞ . The theorem is proved.
)
⎡ yˆ * (t ) ⎤ ⎡ w* (t ) ⎤ yˆ * (t ) = ⎢ 1* ⎥ = ⎢ 1* ⎥ , ⎣⎢ yˆ 2 (t )⎦⎥ ⎣ w2 (t ) ⎦
(21)
with
η1 (t ) =
d (φ1θ ) + ( f1 + α 1 )(φ1θ ) . dt
(22)
The uniformly boundedness of η1 (t ) and η1 (t ) can be easily derived by using the assumptions. Thus, there exist constants λ1 > 0 such that
η1 +
1
α1
η1 < λ1 .
(
(27)
Similar to Theorem 1, it can be concluded that w * (t ) is uniformly bounded and
(
)
lim φ * (t ) ⋅ θ (t ) − w * (t ) = 0 , t →∞
(28)
i.e. w* (t ) is the asymptotic estimate of φ * (t )θ (t ) .
)
2 1 2 1 ˆ r1 (t ) + λ1 (t ) − λ1 . 2 2β1
(24)
V (t ) = r1 (t ) η1 (t ) − f1 r1 (t ) − λˆ1 (t ) ⋅ sign(e1 (t ))
3.2 Identification of θ (t ) Relations (17) and (28) tell us that, by observing one point via stereo vision, four relations about θ (t ) can be obtained. It
Differentiating V (t ) yields
(
⎡ yˆ1* (0) ⎤ ⎡ y1* (0) ⎤ ⎢ * ⎥=⎢ * ⎥ ⎣ yˆ 2 (0)⎦ ⎣ y 2 (0) ⎦
⎡ w * (t ) ⎤ * where ⎢ 1* ⎥ Δ = w (t ) can be defined by referring (13)-(15) w t ( ) ⎣ 2 ⎦ by using the obtained image data y * (t ) from Camera 2.
(23)
Now, consider the Lyapunov candidate V (t ) =
(τ )dτ are
Similarly to (10), construct the equation
d r1 (t ) = (φ1θ − w1 ) + α 1 (φ1θ − w1 ) dt = η1 (t ) − f 1 r1 (t ) + λˆ1 (t ) sign(e1 )
(
t 2 0 1
∫r
) 7596
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
⎡ φ (t ) ⎤ can be easily checked that the rank of the matrix ⎢ * ⎥ is ⎣φ (t )⎦ three. It can be argued that the relations about θ (t ) can be increased by increasing the observation points. Since there are six entries in θ (t ) , it can be argued that at least two points are needed to get a solution of θ (t ) .
Now, suppose p points are observed. For the j-th point, we ⎡ φ ( j ) (t ) ⎤ ⎡ φ (t ) ⎤ ⎡ w(t ) ⎤ denote the obtained ⎢ * ⎥ and ⎢ * ⎥ as ⎢ *( j ) ⎥ and ⎣φ (t )⎦ ⎣ w (t )⎦ ⎣φ (t )⎦
respectively replaced by the generated smooth signals yˆ ( j ) (t ) and yˆ *( j ) (t ) , since y ( j ) (t ) − yˆ ( j ) (t ) → 0 and y *( j ) (t ) − yˆ *( j ) (t ) → 0 . As to the value of y3( j ) (t ) in Φ (t ) ,
although it can be calculated in (11) by using the image data, we use a smoothed signal yˆ 3( j ) (t ) to replace it in order to mitigate the influence of measurement noises. The signal yˆ 3( j ) (t ) is generated as follows. ( j) ⎛ y ( j ) − y *( j) ⎞ y ( j) − y* yˆ3( j) = λˆ(3 j ) (t ) sign ⎜ m 1 2 1 2 + n 2 2 22 − yˆ 3( j ) ⎟ (32) ⎜ ⎟ m +n m +n ⎝ ⎠
⎡ w ( j ) (t ) ⎤ ⎢ *( j ) ⎥ , respectively. ⎣ w (t ) ⎦
λˆ(3 j ) (t ) = γ ⋅ m
Define ⎡ w (1) (t ) ⎤ ⎡ φ (1) (t ) ⎤ ⎢ *(1) ⎥ ⎢ *(1) ⎥ ⎢ w (t ) ⎥ ⎢ φ (t ) ⎥ ⎢ # ⎥ ⎢ # ⎥ ⎢ ( j) ⎥ ⎢φ ( j ) (t ) ⎥ Φ (t ) = ⎢ *( j ) ⎥ , W (t ) = ⎢ w (t ) ⎥ . ⎢ w*( j ) (t ) ⎥ ⎢φ (t ) ⎥ ⎥ ⎢ ⎢ # ⎥ ⎢ # ⎥ ⎢φ ( p ) (t ) ⎥ ⎢ ( p) ⎥ ⎢ *( p ) ⎥ ⎢ w (t ) ⎥ ⎢⎣φ (t ) ⎥⎦ ⎢⎣ w*( p ) (t )⎥⎦
( j)
y1( j ) − y1* m2 + n2
( j)
+n
y 2( j ) − y 2* m2 + n2
− yˆ 3( j ) ,
(33)
where γ is a positive constant. Lemma 2. The generated signals yˆ 3( j ) (t ) and λˆ3 (t ) are uniformly bounded and
(29)
(
)
lim y 3( j ) (t ) − yˆ 3( j ) (t ) = 0 . t →∞
(34)
Proof. By the assumptions (A2)-(A4), it can be seen that 2
ω2 y1( j ) y3( j ) + ω3 y2( j) y3( j ) − b3 y3( j) is uniformly bounded, i.e. there exists a constant λ(3 j ) such that
By Theorem 1, it gives
lim(Φ (t ) ⋅ θ (t ) − W (t ) ) = 0 .
2
ω 2 y1( j ) y3( j ) + ω3 y 2( j ) y3( j ) − b3 y3( j ) < λ(3 j ) .
(30)
t →∞
About the rank of the matrix Φ (t ) , we have the next lemma.
(35)
By considering the Lyapunov candidate
(
)
(λ
)
Lemma 1. The matrix Φ (t ) is of full rank if and only if at least three points are not on a same line. Proof. The proof is omitted.
and differentiating it, the lemma can be easily proved.
Lemma 1 means that at least three points are needed in the proposed formulation.
The recursive algorithms of deriving yˆ ( j ) (t ) , yˆ *( j ) (t ) and
Theorem 2. If at least three observed points are not on a same line, then the motion parameters are observable and it holds
{
(
lim θ ( t ) − Φ T ( t )Φ ( t ) t→ ∞
(
)
−1
}
Φ T ( t )W ( t ) = 0 ,
(31)
)
−1
i.e. Φ T ( t )Φ ( t ) Φ T ( t )W ( t ) is the asymptotic estimate of the vector θ (t ) .
2
L(t ) = y 3( j ) (t ) − yˆ 3( j ) (t ) +
1
γ
( j) 3
− λˆ(3 j ) (t )
2
(36)
yˆ 3( j ) (t ) obviously alleviate the noises in the image data. By
replacing y ( j ) (t ) , y *( j ) (t ) and y3( j ) (t ) in the matrix ⎡ φ ( j ) (t ) ⎤ ( j) *( j ) ( j) ⎢ *( j ) ⎥ with yˆ (t ) , yˆ (t ) and yˆ 3 (t ) recpectively, we φ t ( ) ⎣ ⎦ ⎡ φˆ ( j ) (t ) ⎤ ⎡ φˆ ( j ) (t ) ⎤ get a matrix ⎢ *( j ) ⎥ . By combining the matrices ⎢ *( j ) ⎥ ⎢⎣φˆ (t )⎥⎦ ⎢⎣φˆ (t )⎥⎦
together for all j, we get the matrix Φˆ (t ) expressed as
Since the image data is directly used in Φ (t ) , the measurement noise will directly influence the accuracy of the estimation. In the practical application of the proposed algorithm, the image data y ( j ) (t ) and y *( j ) (t ) can be
7597
(
) (
)
(
) (
)
T T T T Φˆ (t ) = ⎡ φˆ (1) (t ) , φˆ*(1) (t ) , ", φˆ ( j ) (t ) , φˆ*( j ) (t ) ,
⎢⎣
(
) (
)
T T " , φˆ ( p ) (t ) , φˆ *( p ) (t ) ⎤ ⎥⎦
T
(37)
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
If Φ (t ) is of full rank, then Φˆ (t ) is of full rank when t is large enough. By using Theorem 1, Theorem 2 and Lemma 2, it can be concluded that
{
(
lim θ ( t ) − Φˆ T ( t )Φˆ ( t ) t→ ∞
)
−1
}
Φˆ T ( t )W ( t ) = 0 .
(38)
Thus, the motion parameters can be recovered by observing at least three points.
defined in (29) in which w ( j ) (t ) is replaced by wˆ ( j ) (t ) ; wˆ ( j ) (t ) can be similarly derived by a procedure defined in
(12)-(15), where the corresponding image data should be replaced by the virtual data yˆˆ1( j ) (t ) and yˆˆ 2( j ) (t ) ; the initial values at the instant τ ( j ) when the j-th point begins to be not visible should be chosen as
4. CONSIDERATION OF OCCLUSION In the practical applications, the occurrence of occlusion is inevitable. Thus, the algorithm should be modified in order to cope with this phenomenon. In the occurrence of occlusion, the image data of the observed point is not available. Thus, the image data should be replaced by some other virtual signals.
yˆˆ1 (τ ( j ) ) = yˆ1 (τ ( j ) − 0) ;
(42a)
yˆˆ 2 (τ ( j ) ) = yˆ 2 (τ ( j ) − 0) ;
(42b)
yˆˆ 3 (τ ( j ) ) = yˆ 3 (τ ( j ) − 0) .
(42c)
If the j-th point is not visible by the Camera 2 defined in (3), then the virtual signal for y *( j ) (t ) should be similarly derived. Furthermore, if the j-th point is not visible by both of the two cameras, the virtual signals for y ( j ) (t ) , y *( j ) (t ) and y3( j ) (t ) should be similarly derived.
Suppose the j-th point is not visible by the Camera 1 defined in (2). The method formulated in Section 3 of deriving yˆ ( j ) (t ) and yˆ 3( j ) (t ) is no longer useful. By referring to the dynamics in (5), the virtual signals for y ( j ) (t ) and y3( j ) (t ) are constructed by the following dynamical system
⎧ ˆˆ ˆˆ ˆˆ 2 ˆˆ ˆˆ ˆ ˆ ˆ ˆ ⎪ y1 (t ) = ω2 (t ) + ω1 (t ) y2 (t ) + ω2 (t ) y1 (t ) + ω3 (t ) y1 (t ) y2 (t ) ⎪ + bˆ1 (t ) yˆˆ 3 (t ) − bˆ3 (t ) yˆˆ1 (t ) yˆˆ 3 (t ) ⎪ ⎪ ˆ ˆ ˆ ˆ 2 ˆ ⎨ yˆ 2 (t ) = ωˆ 3 (t ) − ωˆ1 (t ) yˆ1 (t ) + ωˆ 2 (t ) yˆ1 (t ) yˆ 2 (t ) + ωˆ 3 (t ) yˆ 2 (t ) ⎪ + bˆ2 (t ) yˆˆ 3 (t ) − bˆ3 (t ) yˆˆ 2 (t ) yˆˆ 3 (t ) ⎪ ⎪ 2 ⎪ yˆˆ 3 (t ) = ωˆ 2 (t ) yˆˆ1 (t ) yˆˆ 3 (t ) + ωˆ 3 (t ) yˆˆ 2 (t ) yˆˆ 3 (t ) − bˆ3 (t ) yˆˆ 3 (t ) ⎩
[
θˆ(t ) = bˆ1 (t ), bˆ2 (t ), bˆ3 (t ), ωˆ 1 (t ), ωˆ 2 (t ), ωˆ 3 (t ) where defined by
]
T
The convergence of the computed motion parameters can be assured, if the total length of the intervals on which the data from two cameras is available is much longer than the total length of the intervals on which at least one camera is occluded. 5. SIMULATION RESULTS The simulation is done by the software Simulink in Matlab. The sampling period Δ is chosen as Δ = 0.02 . The measured image data at the sampling point kΔ is corrupted by a random noise which is in the range of 0.01 y (kΔ ) (or correspondingly 0.01 y * (kΔ) ). Consider the movement of the object described by
(39)
− 4 0.5⎤ ⎡ x1 (t ) ⎤ ⎡ 0 ⎤ ⎡ x1 (t ) ⎤ ⎡ 0 d ⎢ ⎥ ⎥ ⎥ ⎥⎢ ⎢ x2 (t )⎥ = ⎢ 4 0 0.4⎥ ⎢ x2 (t )⎥ + ⎢⎢sin t ⎥ dt ⎢ ⎢⎣ x3 (t ) ⎥⎦ ⎢⎣− 0.5 − 0.4 0 ⎥⎦ ⎢⎣ x3 (t ) ⎥⎦ ⎢⎣sin t ⎥⎦
is
(43)
+
ˆ θˆ ( t ) = ⎛⎜ Φˆ ( t ) ⎞⎟ Wˆ ( t ) ; ⎝
⎠
(40)
ˆ Φˆ (t ) denotes the corresponding matrix of Φˆ (t ) defined in ˆ (37) where φˆ ( j ) (t ) is replaced by φˆ ( j ) (t ) defined by ⎡ yˆˆ ( j ) ˆ φˆ ( j ) (t) = ⎢ 3 ⎣⎢ 0
( A )+
0 ˆyˆ ( j ) 3
(
2 − yˆˆ1( j ) yˆˆ 3( j ) yˆˆ 2( j ) 1 + yˆˆ1( j ) yˆˆ1( j ) yˆˆ 2( j ) ⎤ (41) 2⎥ − yˆˆ 2( j ) yˆˆ 3( j ) − yˆˆ1( j ) yˆˆ1( j ) yˆˆ 2( j ) 1 + yˆˆ 2( j ) ⎦⎥
)
−1
denotes A T A A T if A is of full rank, or the pseudo-inverse (Golub and Van Loan, 1996) of it if A is not of full rank; Wˆ (t ) denotes the corresponding vector of W (t )
Four points starting at [− 1, 1, 1] , [1, − 2, 2] , [0, 3, 3] and T
T
T
[2, 0, 2]T
are observed. It is assumed that the third and fourth points are not visible by both of the two cameras during the time period [1, 6]. Fig. 1 shows the difference between the real image data y1( 4) (t ) and its virtual value yˆˆ 1( 4 ) (t ) for the fourth point. The differences y ( 4 ) (t ) − yˆˆ ( 4) (t ) , y ( 4) (t ) − yˆˆ ( 4) (t ) , and 2
2
3
3
y (t ) − yˆˆ i(3) (t ) ( i = 1, 2, 3) are similar to that in Fig. 1. It can be seen that the virtual value of the image data during the occlusion is effectively constructed.
7598
( 3) i
17th IFAC World Congress (IFAC'08) Seoul, Korea, July 6-11, 2008
The simulation results are shown in Figures 2-3. simulation results of the differences ω 2 (t ) − ωˆ 2 (t ) ω 3 (t ) − ωˆ 3 (t ) are very similar to that in Figure 2. simulation results of b (t ) − bˆ (t ) and b (t ) − bˆ (t ) is
The and The
very 1 1 3 3 similar to that in Fig. 3. It can be seen that very good estimates for the motion parameters are obtained even in the presence of measurement noises.
6. CONCLUSIONS The motion recovery for a class of movements in the space by using stereo vision has considered by observing multiple (at least three) points in this paper. The considered motion equation can cover a wide class of practical movements in the space. The estimations of the motion parameters which are all time-varying have been developed based on the second method of Lyapunov. The assumptions about the perspective system are reasonable, and the convergence conditions are intuitive and have apparently physical interpretations. The proposed method requires minor a priori knowledge about the system and can cope with a much more general class of perspective systems. Furthermore, the algorithm has been modified to deal with the occlusion phenomenon. Simulation results have shown that the proposed algorithm is effective even in the presence of measurement noises. REFERENCES
Fig.1 The difference y1( 4) (t ) − yˆˆ1( 4) (t ) for the fourth point.
Fig. 2. The difference between ω1 (t ) and ωˆ 1 (t ) .
Fig. 3. The difference between b2 (t ) and bˆ2 (t ) .
Azarbayejani, A. and A. Pentland (1995). Recursive estimation of motion, structure and focal length. IEEE Trans. on Pattern Analysis and Machine Intelligence, 17, 562-575. Chen, X. and H. Kano (2002). A new state observer for perspective systems. IEEE Trans. Automatic Control, 47, 658-663. Chen, X. and H. Kano (2004). State Observer for a class of nonlinear systems and its application to machine vision. IEEE Trans. Automatic Control, 49, 2085-2091. Chiuso, A., P. Favaro, H. Jin, and S. Soatto (2002). Structure from motion causally integrated over time. IEEE Trans Pattern Analysis & Machine Intelligence, 24, 523-535. Dayawansa, W., B.K. Ghosh, , C. Martin and X. Wang (1994). A necessary and sufficient condition for the perspective observability problem. Systems & Control Letters, 25, 159-166. Ghosh, B.K., H. Inaba, and S. Takahashi (2000). Identification of Riccati dynamics under perspective and orthographic observations. IEEE Trans. on Automatic Control, 45, 1267-1278. Jankovic, M. and B.K. Ghosh (1995). Visually guided ranging from observation of points, lines and curves via an identifier based nonlinear observer. Systems & Control Letters, 25, 63-73. Kanatani, K. (1990). Group-Theoretical Methods in Image Understanding. Springer-Verlag. Loucks, E.P. (1994). A perspective System Approach to Motion and Shape Estimation in Machine Vision. Ph.D Thesis, Washington Univ.. Reif, K., F. Sonnemann and R. Unbehauen (1998). An EKFbased nonlinear observer with a prescribed degree of stability. Automatica, 34, 1119-1123. Satry, S. and M. Bodson (1989). Adaptive Control, Stability, Convergence, and Robustness. Prentice Hall, Englewood Cliffs, New Jersey. Soatto, S. (1997). 3-D structure from visual motion: Modelling, representation and observability. Automatica, 33, 1287-1321. Golub, G.H. and C.F. Van Loan (1996). Matrix Computations. The Johns Hopkins University Press.
7599