0 200 400 600 800 1000 1200 1400 1600
0 50000 100000 150000 200000
Error [m]
Distance Traveled [m]
TFE MFE 2 levels MFE 3 levels MFE 4 levels MFE 5 levels MFE 6 levels MFE 7 levels
Figure 7.13: Performance of Multi-Frame approach as a function of distance traveled for WLS.
ego-position estimation between the TFE and MFE estimation after a traveled distance of50km. The error using MFE remains quite small and, although an evident increase in the orientation error is observed, the position error is negligible in comparison to the error made with TFE, which exceeds the250meters.
When computing the absolute orientation with TLS, similar results are obtained.
Figure 7.15(a) shows the results of MFE using TLS, where it can be seen that same improvements factors, as with WLS, are achieved. Figure 7.15(b) shows the orienta-tion error as a funcorienta-tion of traveled distance. The curves also show an improvement with increasing level of integration, but in contrast to the ego-position error, there is almost a constant reduction of the error with every additional level of integration. In fact, it is the reduction of the orientation error that allows almost a linear relationship between traveled distance and ego-position error, as already observed by Olson et al.[OMSM03].
7.6 Integration of Filtered Data
Chapter 4 introduced how to optimally estimate the 3D position and 3D velocity of world points fusing optical flow, stereo and the motion parameters of the camera.
The simulation results showed that the iterative refinement over time of the posi-tion of 3D points with Kalman filters allows a better estimaposi-tion, than treating the individual measurements as being uncorrelated. It is therefore tempting to use the filter estimates when computing ego-motion and not the triangulated measurements.
108 Robust Real-Time 6D Ego-Motion Estimation
-500 -400 -300 -200 -100 0 100 200
-200 -100 0 100 200 300 400 500
Depth
Lateral Position Ground Truth Motion (GT)
Multi-Frame Estimation (MFE) Starting Point GT
Starting Point MFE End Position GT End Position MFE
(a) Integrated motion estimation for the first15kilometers traveled.
-400 -300 -200 -100 0 100 200 300 400
-200 -100 0 100 200 300 400 500 600
Depth
Lateral Position Ground Truth Motion (GT)
Multi-Frame Estimation (MFE) Two-Frame Estimation (TFE) Starting Point GT Starting Point MFE End Position GT End Position MFE Starting Point TFE End Position TFE
(b) Integrated motion estimation after100kilometers traveled.
Figure 7.14: Examples of integrated motion estimation using MFE and TFE.
7.6 Integration of Filtered Data 109
0 200 400 600 800 1000 1200 1400
0 50000 100000 150000 200000
Error [m]
Distance Traveled [m]
TFE MFE 2 levels MFE 3 levels MFE 4 levels MFE 5 levels MFE 6 levels MFE 7 levels
(a) Euclidean distance position error
0 5 10 15 20 25
0 50000 100000 150000 200000
Error [degrees]
Distance Traveled [m]
TFE MFE 2 levels MFE 3 levels MFE 4 levels MFE 5 levels MFE 6 levels MFE 7 levels
(b) Orientation error.
Figure 7.15: Performance of Multi-Frame approach as a function of distance traveled.
110 Robust Real-Time 6D Ego-Motion Estimation
However, the filter requires first the motion of the camera in order to be able to tell the optimalcurrent position of the points. In other words, at time tk the motion of the camera between timestk−1 andtk is required in order to obtain the new filtered position of the points, i.e. the required filtered 3D position at timetkis not available until ego-motion is computed. The ego-motion algorithm cannot, therefore, use the optimal estimation of the point for current timetk. However, previous filter outputs are available and can be used as measurements for the computation of absolute ori-entation. This means that when computing the absolute orientation between current and previous time, the points corresponding to the data setXk−1 will not be just the triangulated features points but the estimates provided by the Kalman filters. Ob-serve that not only the immediate previous data set may contain filtered data, but all previous data sets (Xk−2,Xk−3, . . .). This means that not only TFE benefits from the filter outputs, but also MFE.
One important aspect to consider when using the filter outputs for the estimation of ego-motion is the danger of falling into a positive feedback loop. Positive feedback loops enhance or amplify changes, moving a system away from its equilibrium state and make it more and more unstable. Suppose that our system (as shown in Figure 7.1) is in equilibrium. This means the system works stably, computing stereo and optical flow, obtaining the ego-motion of the vehicle from non-filtered (current time) and filtered (previous times) 3D point, and updating 3D point position and velocity of world points. Now let us suppose that a small perturbation is introduced to the system, for example, the tracking or stereo algorithm computes too many outliers (because of repetitive structure in the scene for example). The Ego-motion system will compute the motion of the camera but with less accuracy than normal. The points updated with Kalman Filter will also have a larger error because of the inaccuracy of the ego-motion parameters. The equilibrium is already broken. Every posterior cycle amplifies this effect and the system go into an uncontrollable state.
In order to avoid this we define a firewall4. A firewall defines a limit for the integration of information. The firewall in our system is achieved through a threshold for the age of the tracked features. The filtered 3D position of feature points that are younger than this threshold, are not stored in the 3D Point List. By doing this we avoid the computation of ego-motion with filtered young features. Instead of the filtered position, the triangulated 3D position of young feature points is used, which is not affected by a previous wrong computation of ego-motion. A relatively old feature point will have a converged state in the KF model and a more stable behavior with respect to wrong estimates.
Figure 7.16 shows the expected root mean square distance error as a function of the point distance, comparing the performance of using the ego-motion results against the results of using the real motion parameters (ground truth). Every curve shows the results for point tracked a specified number of times. The curve “Age 2 Est” shows the RMSE for points with age 2 (tracked 2 times). “Est” means that the ego-motion estimation was used for building theAkand bk matrices of the Kalman filter system model. If, instead, the ground truth motion is used for building the matrices, then the curve is labeled “GT”. The motion sequence was simulated as constant forward motion. Multi-frame estimation with level 3 was used and all parameters
4The termfirewallwas taken from Nisteret al.[NNB04] [NNB06]. In the papers, the authors use this term for defining a method that prevents the system falling into a positive feedback loop.