Generation of Simulated Data

Figure 7.3: Simulated motion sequence

A very important feature of both versions of the SMC is also the benefit obtained from the integration of temporal data. Many methods ignore valuable previous infor-mation and just compute the best fit by considering only the instantaneous measured data. It is only possible to obtain a meaningful result if at most 50% of the data are outliers, i.e. such estimators have a maximum breakdown point of 50%. Never-theless, if previous information is integrated in the estimation process, the break-down point of robust estimators will not depend on the percentage of contaminated data, but more on the number of non-contaminated data, allowing breakdown points larger than50%. The simulation results in Section 7.4.6 show this behavior.

7.4.5 Generation of Simulated Data

In order to show the effectiveness of the SMC and compare it with the ordinary Least Squares weighting, a simulated motion sequence is used. The motion sequence was generated by computing the ego-motion of the cameras installed on a vehicle while the vehicle was traveling the path shown at the top-left of Figure 7.3. An optimal parameter set in sights of accuracy (and at the expense of real-time) was choosen in

92 Robust Real-Time 6D Ego-Motion Estimation

this step for the computation of ego-motion. The frame rate was 10 frames/second and the distance traveled was approximately of 1 km with a total of 1137 stereo images. The motion parameters were smoothed in order to reduce noise and the pitch and roll rate as well as the vertical translation were amplified by a factor of 1.33, in order to obtain a slightly rougher motion than normally expected in real traffic situations. Figure 7.3 shows plots of the motion sequence.

The image features and corresponding 3D points of the environment used as mea-surements in the Least Squares approach are generated according to the following procedure:

1. the feature image position (¯u_pi,v¯_pi)is randomly generated, equally distributed in the left image;

2. the depthZ¯pi of the corresponding 3D point is randomly generated with equal distribution between minimal and maximal distance(distmin, distmax);

3. the disparityd¯_pi of the feature and the corresponding lateral and height position ( ¯X_pi,Y¯_pi) of the 3D point are obtained from the data generated in the two previous points. At this point the world point position with coordinate p¯_i = ( ¯X_pi,Y¯_pi,Z¯_pi)^T and its projectionp¯⁰_i = (¯u_pi,¯v_pi,d¯_pi)^T are already generated;

4. noise is added top¯⁰_i in order to obtainp⁰_i = (u_pi, v_pi, d_pi)^T;

5. the image feature point p⁰_i is triangulated in order to obtain p_i = (X_pi, Y_pi, Z_pi)^T, i.e. the noisy version ofp¯_i;

6. the current world point position is obtained byx¯_i =R¯_kp¯_i+¯t_k whereR¯_kand

¯t_kare the inverse rotation matrix and translation vector of the camera obtained from the motion sequence at stepk;

7. the pointx¯_i is projected on the image obtainingx¯⁰_i = (¯u_xi,v¯_xi,d¯_xi)^T; 8. noise is added tox¯⁰_i in order to obtainx⁰_i = (u_xi, v_xi, d_xi)^T;

9. (when required) equally distributed noise in the range(−m, m)pixels is added to every component ofx⁰_i generating a potential outlier; and finally

10. x⁰_iis triangulated in order to obtainx_i = (X_xi, Y_xi, Z_xi)^T, i.e. the noisy version ofx¯_i.

The above procedure ensures uniform distribution in the position of the feature in the image plane and in the depth of the corresponding 3D world point. The noise of steps 4and 8has a Gaussian or Slash distribution according to the evaluation de-sired¹. A Slash distributed random variable can be obtained as a Gaussian random

1When testing robust estimators Slash distribution are preferred to Cauchy distribution, because of the smaller peakedness at the origin [Dav02].

7.4.6 Simulation Results 93

0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0.40

-10.0 -5.0 0.0 5.0 10.0

Probability

Slash Gaussian

Figure 7.4: Comparison of Slash and Gaussian p.d.f. assuming a mean of 0 and a variance of1.

variable with mean0and varianceσ_I²divided by a uniform random variable over the unit interval [0,1]. The Slash distribution has larger tails than the Gaussian distribu-tion, as Figure 7.4 shows. In either case, a zero-mean multi-variate noise term η is added to feature point vectors x¯⁰_i and p¯⁰_i and not to the world points as is usually done ([HJL⁺89]). 3D points are obtained with the noisy feature positions, generating the real error distribution expected in the triangulated points. If, as a consequence of noise or motion, an image feature results with a position outside the image coor-dinates or its corresponding 3D point depth lies outside the range(dist_min, dist_max), the point is discarded. This process is repeated untilN valid points are obtained.

In order to evaluate the performance of the ego-motion algorithm the Root-Mean-Square Error (RMSE) of the rotation angle and RMSE of the translation distance are used. The error in the translation distance is obtained as the Euclidean distance between real and estimated translation, i.e. ift¯E =kt−¯tkis the error in translation, the translation distance error is kt¯_Ek. Since rotations are elements of the group SO(3), a rotation error cannot be obtained just as the Euclidean distance of rotation parameters. The error measure used for the evaluation of the rotation error is the minimal angle of rotation needed, around some axis, to transform estimated rotation to true rotation [Kan94] [OK98] [EBW04]. If R is an estimation of the true rotation matrix R, then the error rotation matrix is¯ R¯E = RR¯^T. The rotation expressed in R¯_E is represented as the angle of rotation∆Ω around some axis¯l_r; ∆Ω is the error rotation angle.

7.4.6 Simulation Results

This section shows some experimental results using the simulated data of the pre-vious section. For all the tests; the minimal distance is dist_min = 3.63125 me-ters, which corresponds to a disparity of 80, pixels and the maximal distance dist_max = 141.62, which corresponds to a disparity of 2 pixels. The camera pa-rameters aref_u =f_v = 830,u₀ = 320,v₀ = 240,B = 0.35meters with an image size

94 Robust Real-Time 6D Ego-Motion Estimation

of640pixels width and480 pixels height. The number of pointsN is500 (when not stated otherwise).

The improvements achieved with the scalar and matrix versions of the SMC are shown by comparison with the standard weighting methods. When using the WLS approach, the results of the SMC defined in Equation 7.9 (labeled “WLS SMC” in the graphs) is compared with the results using the standard weighting expressed in Equation 7.7 (labeled “WLS Without SMC” in the graphs)². For the TLS approach, the results of the SMC of Equation 7.13 (labeled “TLS SMC” in the graphs) is compared with the results using the standard covariance matrices obtained with Equation 6.4 for points p and x (labeled “TLS Without SMC” in the graphs). Each point of the following plots were obtained running 10 times the whole motion sequence, and therefore the RMSE corresponds to11,370motion estimation steps.

Figures 7.5(a) and 7.5(b) show the performance of the methods under varying Gaussian image position noise. Both, for the rotational and translational plots, the SMC curves outperform the standard versions. As expected the TLS approach with-out SMC performs better than the WLS approach, since the covariance matrices allow the modeling of the direction and size of the error for each point independently, as already shown in Chapter 6. Nevertheless, comparing both SMC curves, the scalar LS performs better than the matrix weighted LS. This is an effect of the penalization imposed by the closed-form solution: rotation is found by projecting the optimal un-constrained solution into the parameter manifold, as already discussed in the Section 5.5 of Chapter 5. The real benefit of the TLS is given in the translational component, which is at least3times more accurate than the WLS approach.

The penalization imposed in the rotation matrix for the closed form solution of the TLS approach is small and when Gaussian noise is used instead of Slash noise, the TLS with SMC shows the more robust behavior as shown in Figures 7.6(a) and 7.6(b).

Observe that under Slash noise the scalar version of the SMC is the least affected in the translation estimation, since the error increases by a factor smaller than1.3.

The estimation accuracy with respect to the number of points in the sets behaves as expected, i.e. the accuracy becomes considerably worse with decreasing number of points [HJL⁺89]. Figure 7.7 and 7.8 show RMSE of rotation and translation re-spectively, for varying number of points and fixed Gaussian noise withσ_I = 0.4. The observations made for Figure 7.6 are also true for these graphs, with an exception.

The TLS SMC version shows a better performance for rotation estimation than the WLS with SMC, when the number of points decreases, demonstrating once again a more stable solution. In Figure 7.7(a) this can be seen when the number of points is less than65.

Figure 7.9 shows the results when step9of the procedure of the previous section is used for an increasing percentage of features. As with the previous evaluation, a zero-mean Gaussian noise termηwithσ_I² = 0.4² is added to the features points. The percentage of points, for which step9of the procedure is applied, is varied between 0 and 90. For this percentage of features, a uniform random value κ in the interval [−32,32] (i.e. ±5% of the image width) is added to the image vector generating potential outliers, and simulating correspondence errors. Figure 7.9(a) shows the rotational error as a function of the percentage of outliers generated in this way, while

2If Equation 7.7 is used instead of Equation 6.2, the same results are obtained.