222 
3. 3-D STRUCTURE ESTIMATION USING AN EXTENDED KALMAN FILTER 
In order to measure the 3-D structure of a workpiece the spatial coordinates of the polygon vertices have to be 
determined. As known from conventional stereo vision the coordinates of a point in 3-D space can be computed by 
means of triangulation (Fig. 4). A point moving from PC to PS during a certain interval of time is observed twice in the 
image plane as P? and Pj. The motion parameters as well as the internal camera parameters are expected to be 
known. 
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
z fdr TE qu m 
real 3-D | estimated 3-D 
Pf point coordinates | point coordinates | 
focal length ;N | | 
\ known motion | | 
\ | | 
\ | motion | 
™ | | 
| cua nC | mathematical 
/ b cere p | ı : 
> 7 E eec 1 | | model | 
ps i perspective | | 
1 projection | 
/ > X | | | | 
: PS Y | Y 
measured 2-D | ; ( predicted 2-D | 
S Ci coordinates comparison image REN 
v : L. _— — — — zd 
y image plane Kalman Filter 
Fig. 4. Stereo triangulation. Fig. 5. 3-D coordinate estimation. 
A recursive extension of the stereo approach is illustrated in Fig. 5. Instead of inferring depth from multiple 2-D images 
an estimate of the spatial coordinates is successively updated by comparing measured image coordinates with 
predicted image coordinates. The prediction is based on a mathematical model of the three-dimensional motion and the 
optical projection. Thus, the reconstruction of 3-D structures from 2-D image sequences can be formulated in terms of a 
- nonlinear minimum variance estimation problem. 
Fig. 6 illustrates the mathematical model and the Kalman filter approach using state-space notation. The state vector of 
the system is defined as the spatial position of a single vertex with reference to the camera coordinate frame C 
7 
xo GCytat. 1). (1) 
The dynamic system is described in discrete time by 
with F, designating the known motion of the rigid object. Assuming constant translational and rotational motion vectors 
v - (v, V0) and o = (0,.0,,0,) the system matrix can be computed as 
kyky(1-cos®)+cosf ^X kyk,(1— cos0) - k, sing Kzk,(1-cos6)+k,sin6 v,T 
K,k, (1- cos6)+k, sin6 kyk,(1- cos 6) + cos 8 kzk,(1- cos6)- k, sin6 vyT 3 
k= kik; (1-cos6)—k, sin6 kyk,(1-cos0)- Kk, sin k,k,(1-cos0)-cos0 — v,T (3) 
0 0 0 1 
T denotes the interval of time between k and k+1, k = th, 0) = o/|w| defines the axis of rotation, and 6 =|w|-T 
indicates the covered angle of rotation. The process noise w, is assumed to be a zero mean gaussian white random 
process with covariance matrix Q,. The initial state estimate ist defined by the mean X) together with the error 
covariance matrix P,. 
IAPRS, Vol. 30, Part 5W1, ISPRS Intercommission Workshop "From Pixels to Sequences", Zurich, March 22-24 1995