Accurate and Robust Ego-Motion Estimation using Expectation Maximization

Transcription

1 Accurate an Robust Ego-Motion Estimation using Expectation Maximization Gijs Dubbelman 1,, Wannes van er Mark 1 an Frans C.A. Groen Abstract A novel robust visual-oometry technique, calle EM-SE(3) is presente an compare against using the ranom sample consensus () for ego-motion estimation. In this contribution, stereo-vision is use to generate a number of minimal-set motion hypothesis. By using EM-SE(3), which involves expectation maximization on a local linearization of the rigi-boy motion group SE(3), a istinction can be mae between inlier an outlier motion hypothesis. At the same time a robust mean motion as well as its associate uncertainty can be compute on the selecte inlier motion hypothesis. The atasets use for evaluation consist of synthetic an large real-worl urban scenes, incluing several inepenently moving objects. Using these ata-sets, it will be shown that EM-SE(3) is both more accurate an more efficient than. Inex Terms Robust estimation, visual-oometry, Stereovision. I. INTRODUCTION In this article, the focus is on robust ego-motion estimation of a moving vehicle using an onboar stereo-rig, this is also known as stereo-base visual-oometry. Stereo processing allows estimation of the three imensional (3D) location an associate uncertainty of lanmarks observe by the stereo camera. Subsequently, 3D point clous can be obtaine for each stereo frame. By establishing the frameto-frame corresponences of lanmarks, the point clous of successive stereo-frames can be relate to each other. From these corresponing point clous the frame-to-frame motion of the stereo-rig can be estimate, Matei an Meer [1], Umeyama [3]. By concatenating all the frame-to-frame motion estimates, the pose of the stereo-rig an vehicle can be tracke. In general, vision base approaches for ego-motion estimation are susceptible to outlier lanmarks. Sources of outlier lanmarks range from sensor noise, corresponences errors, to inepenent moving objects such as cars or people that are visible in the camera views. In the past ecae the ranom sample consensus () metho, evelope by Fischler an Bolles [3], emerge as the golen stanar for robust parameter estimation in computer vision. Hence, it is use in many visual oometry systems, Maimone et al. [8], Nistér et al. [13], [1] an Olson et al. [15]. The iea behin is to estimate a large number of minimal-set (containing for example six lanmarks) motion hypothesis. For each motion hypothesis also a robust score is calculate, this score is base on Electro-Optical Systems 1, TNO Defence, Security an Safety, Oue Waalsorperweg 63, 59 JG The Hague, The Netherlans, {gijs.ubbelman, wannes.vanermark}@tno.nl Intelligent Systems Laboratory Amsteram (ISLA), University of Amsteram, Kruislaan 3, 198 SJ Amsteram, The Netherlans groen@science.uva.nl the alignment of the motion hypothesis with all lanmarks in the set. The best scoring minimal-set motion hypothesis is taken as the final robust estimate. Often, refinement of the initial estimate is performe using a more avance estimator. Since [3], many ifferent approaches have emerge, for example LmeS, Rosin [19], for an overview see Torr an Murray []. Most notably for the case of visual-oometry is preemptive, Nistér [1], which allows evaluating a larger number of minimal-set motion estimates in the same amount of computation time. Using some sort of multi-frame optimization or multi-frame lanmark tracking is also frequently use. In the extreme case, this leas to simultaneous localization an mapping (SLAM) approaches such as that of Elinas et al. []. In this paper however, the focus is on frame-to-frame approaches only. Closely relate to ego-motion estimation is the problem of pose estimation, which involves estimating the motion of an object from images taken by a (fixe) camera. Recently, alternative approaches to have emerge for robust pose estimation, Pennec et al. [18] an Subbarao et al. [1]. These methos use geometrical computation within Riemannian geometry or Lie algebra, Selig [], together with robust statistics to obtain reliable results. In line with this work, a novel algorithm calle EM-SE(3) is presente. It uses expectation maximization (EM) on a local linearization of the rigi-boy motion group i.e. SE(3). In this article it will be compare to using both synthetic an real-worl ata. II. EXPECTATION MAXIMIZATION IN SE(3) The goal of the EM-SE(3) algorithm is computing a robust mean motion M an its covariance Σ from a set of rigiboy transformations M = { M 1...M n}. For this, expectation maximization on the group of 3D Eucliean motion is use. EM is an iterative algorithm often use for fining the parameters of a mixture moel. Here, a similar approach is taken for fining the parameters of a two class moel i.e. inlier motion hypothesis versus outlier motion hypothesis. The inlier motion hypothesis are minimal-set motion estimates from the true ego-motion perturbe with Gaussian noise. The outlier motion hypothesis are erroneous minimal-set motion estimates, they are cause by inclusion of outlier lanmarks into the minimal-set. The class of outlier motions will be moele with an uniform istribution.

2 A. Representing SE(3) The EM-SE(3) algorithm requires the ability to compute a weighte mean translation an rotation from a set of rigi-boy transformations. Whereas, translations belong to a vector space, more precisely the Eucliean space, rotations o not. Therefore, computing the usual weighte arithmetic mean on a set of rotations is far from appropriate. In fact, for most mathematical representations of rotation, for instance rotation matrices an quaternions, it will not result in a proper rotation, unless the result is orthogonalize in the case of rotation matrices, or normalize in the case of quaternions, Gramkow []. Because a rigi-boy transformation often inclues rotation, it also oes not belong to a vector space. Therefore, the problem to be solve is that of mapping the space of rigiboy transformations to a space that is as close to a vector space as possible. Solutions for this problem can be foun in the fiel of ifferentiable manifols, Loring [6], particularly in Lie algebra, Riemannian geometry an Geometric algebra Dorst et al. [1]. These fiels of mathematics are still actively stuie an an introuction is beyon the scope of this text. Because the complete group structure of SE(3) is irrelevant when computing the mean motion, the problem becomes more straightforwar. Note that the set of motion hypothesis are estimates for the same real-worl motion at one particular point in time. Therefore, the rotations o not transform any of the other rotations or translations, as woul be the case when the motion hypothesis forme a chain of rigiboy transformations. This allows processing the translations separately from the rotations, hence only the rotational part has to be mappe. Keeping the rotations an translations separate when applying a metric on SE(3) is in line with the work by Park [16]. In this work rotations will represente using unit quaternions. Quaternions will be enote as q an consists of a one imensional real part q an a three imensional spatial part q = (q i i + q j j + q k k) given on the imaginary basis i, j, k (i = j = k = ijk = 1). Thus q = (q + q). Quaternion aition is simply the pairwise aition of their elements. The inverse of a unit quaternion is given by its conjugate q = (q q). The ientity quaternion is efine as e = (1 + i + j + k). A rotation aroun a normalize axis r with angle θ can be put into quaternion form with q = (cos(θ/) + sin(θ/) r). To rotate an arbitrary vector v it can be represente as a quaternion i.e. v = ( + v). Given the quaternion q we apply its rotation on v by using v = qvq. Where the quaternion prouct is efine as q 1 q = (q 1 q q 1 q + q 1 q + q q 1 + q 1 q ) an the ot an cross prouct are efine as usual. Note that the quaternion prouct is anti-commutative, associative an left/right istributive over aition. The space of unit quaternions can be thought of as the three imensional surface of an unit sphere in four imensional space. Because, rotations only have three egrees of freeom the manifol only has three imensions. Clearly, the rotation manifol is not a vector space an therefore the Eucliean istance between quaternions is far from appropriate. Using Riemannian geometry the points, i.e. rotations on the manifol, can be mappe to a vector space, Pennec [17]. This can be thought of as locally linearizing the unit sphere in D space. Therefore, this vector space is often calle the tangent space. To go from the group of rotations SO(3) to its tangent space so(3) at the ientity the so calle logarithmic mapping log : SO(3) so(3), log(q) = q can be use. Mapping back from the tangent space so(3) to SO(3) can be one using the exponential mapping exp : so(3) SO(3), exp(q) = q. For unit quaternions these mappings are q = log(q) = (arccos(q) q q ), q (,, ), q = an q (cos( q ) + sin( q ) q ), q = q = exp(q) =. (1,,, ), q = () Treating SO(3) as a vector space is only appropriate close to the mapping point (as is also the case with regular linearization). Therefore, it is useful to be able to efine the tangent space at other points than the ientity. For this a placement function, which is compose of quaternion operations, can be use. Placing q in the tangent space at q 1 can be one using (1) q = log q1 (q ) = log(q 1q ) (3) an retrieving q from the tangent space efine at q 1 can be accomplishe with q = exp q1 (q ) = q 1 exp(q ). () These operations are important because they allow linearization of SO(3) at a certain point of interest, for instance at the mean rotation q. This causes less linearization errors than always mapping at the ientity. The iea behin locally mapping SO(3) to a vector space is allowing to treat so(3) as if it were R 3 an subsequently use statistical methos esigne for R 3 on so(3). The problem that is solve here is that SO(3) is not an Eucliean space an therefore the Eucliean istance in not appropriate. Because the Eucliean istance is the basis for most statistical methos for geometric computation, it is assume that by transforming SO(3) to so(3) an treat it as R 3 statistical inferences can be mae about SO(3). For notational convenience an element of SE(3) consisting of a translation v an a rotation embee in an unit quaternion q is given by M = ( v, q). Furthermore, the localize logarithmic an exponential mappings on the use representation of SE(3) are efine as log M1 (M ) ( v, log q1 (q )) = ( v, q ) m, (5) exp M1 (m ) ( v, exp q1 (q )) = ( v, q ) M. (6)

3 Putting the translation an rotation together in a seven imensional vector allows for compact formulas an efficient computation. However, one has to note that they are istinct elements, an for example, computing the correlation between a imension in the translational part an a imension in the rotational part is prohibite. B. Expectation maximization in se(3) In orer to perform EM on SE(3), the weighte mean of a set of motion hypothesis {M 1...M n } must be calculate. Let us start by consiering the unweighe case. Given the logarithmic an exponential mappings efine in section II-A the mean motion M is obtaine by minimizing the following function: arg min M SE(3), ˆM log SE(3) ˆM ( M) log ˆM (M i ). (7) The challenge here is that two goals have to be met simultaneously. That is, minimizing the summe square ifference between M an the motion hypothesis in the set {M 1...M n }. Seconly, choosing a mapping point ˆM such that treating so(3) as R 3 is as appropriate as possible. If a mapping point with minimal summe square istance to the motion hypothesis in the set is use, the optimal linearization point an the mean motion are the same. This problem can be solve by using an iterative approach, see eq. 8. Firstly, linearization of SE(3) is performe at the ientity (or the motion estimate from the previous time-step) by using the logarithmic map. Then, the mean rotation is compute in se(3) an mappe back to SE(3) using the exponential map. In the next iteration, linearization of SE(3) is performe using the new mean. Again, the mean is compute in se(3) an mappe back to SE(3). This continues until convergence or until j has reache a maximum. w n log (M M j = exp Mj 1 i ) M j 1. (8) w n Because the minimal-set motion hypothesis can contain outliers, it is inappropriate to use eq. 8 irectly. Therefore, eq. 8 is embee in an EM algorithm. The benefit of EM over other robust statistical methos, such as M-estimators or mean shift, is that besies a robust motion estimate also a Monte Carlo estimate of the covariance is returne. Here, the primary goal of the EM algorithm is to make a istinction between inlier an outlier minimal-set motion hypothesis an subsequently computing the mean on the inliers only. Inlier motion hypothesis are moele with a single Gaussian istribution. The outlier motion hypothesis are moele with a fixe uniform istribution. The mixing weights of the inlier an outlier classes at iteration k of the EM algorithm are given by p(i) k an p(o) k = 1 p(i) k respectively. Furthermore, p(m i θi k ) is the probability that motion hypothesis M i is an inlier given the inlier parameters θi k = { m, Σ m} i.e. the mean motion with its covariance at iteration k. Similar notation is use for the constant pf. of the outliers i.e. p(m i θ O ). Note, that these probabilities are calculate on the logarithmic mappings of the motion samples. The parameters that will optimize are the inlier mixing { weight } an the inlier ensity parameters i.e. Ψ k = p(i) k, θi k. Given these quantities the log expectation of the motion samples can be written as Q(Ψ k, Ψ k 1 ) = I i log(p(i) k p(m i θi k)) + O i log(p(o) k p(m i θ O )), where the inlier weights I i an outlier weights O i are expresse as I i = O i = p(i) k 1 p(m i θ k 1 I ) p(i) k 1 p(m i θ k 1 I )+p(o) k 1 p(m i θ O ) p(o) k 1 p(m i θ O ) p(i) k 1 p(m i θ k 1 I )+p(o) k 1 p(m i θ O ) (9). (1) For clarity, log an exp have their usual meaning in formulas concerning probabilities. Since, p(m i θ I ) is Gaussian it is given by p(m i θ I ) = 1 (π) 1/6 Σ m e 1 (m i m) T Σ m (m i m). (11) Note that Σ m must have the form [ ] Σ v Σ m =. Σ q Thus, the translation an rotation are inepenent an p(m) = p( v)p(q). The goal is to maximize Q with respect to θi k i.e. the new mean motion mk an its covariance Σ k m. Taking the erivative of Q to m k, setting it to zero an solving for m k gives I i m i m k =. (1) I i This equation oes not take into account the errors introuce by the logarithmic mapping. Therefore, we choose to use eq. 8 for iteratively computing the weighte mean motion uring every EM iteration. Next optimizing Q with respect to Σ k m base on the new mean results in I i (m i m k )(m i m k ) T Σ k m =. (13) I i Clearly, computing the elements of the upper right block an the lower left block of Σ m is not necessary an can be set to zeros. Finally, Q will be optimize with respect to the inlier mixing weight p(i) k, this gives p(i) k = 1 I i. (1) n The EM algorithm iterates between computing the weights with eq. 1 given the current parameters Ψ k 1, i.e. the

4 expectation step, an computing the new parameters Ψ k with eq. 8,13,1 given the new weights, i.e. the maximization step. This goes on until convergence or k has reache a maximum. III. STEREO VISION BASED MOTION ESTIMATION The motion hypothesis {M 1...M n } neee for the EM- SE(3) algorithm are estimate using stereo vision. It is assume that stereo images are rectifie accoring to the epipolar geometry of the use stereo-rig, Hartley an Zisserman [5]. To obtain the lanmarks neee for motion estimation, image feature corresponences must be establishe between successive stereo-frames an between the images in the stereo-frames themselves. To this purpose the Scale Invariant Feature Transform (SIFT), Lowe [7], is use. A threshol is applie on the istance between SIFT escriptors to ensure reliable matches between image features. Furthermore, the epipolar constraint, back-an-forth an left-to-right consistency are enforce. From an image point in the left image v l = [x l, y l ] an its corresponing point in the right image v r = [x r, y r ] their isparity can be obtaine with sub-pixel accuracy = x l x r. Using the isparity, the focal length of the left camera f an the stereo base line b, the 3D position of the lanmark image by v l an v r relative to the left camera can be recovere with v = [ v x = x lb, v y = y lb, v z = fb ] T. (15) For each reconstructe lanmark we also estimate its three imensional uncertainty as covariance matrix Σ v. This uncertainty is base on error-propagation of the image feature position uncertainty using the Jacobian J of the reconstruction function, Matthies an Shafer [11], [ ] Σ vl Σ v = J J, (16) Σ vr J = x l b + b y l b b fb x l b y l b fb. (17) Here, Σ vl an Σ vr are the image feature covariance matrices in the left an right images respectively. For our purposes, only the shape an relative magnitue of image feature position uncertainty is important. Therefore, it suffices to estimate Σ v with G 1 Σ v = s G 1. (18) Where s is the scale of the image feature in scale-space an G is the graient Grammian at v, Zhou et al. []. The stereo reconstruction proceure results in normally istribute uncertainty in lanmark positions. The uncertainty is significantly larger in the irection from the optical center to the lanmark position. Furthermore, the magnitue of the uncertainty increases with the istance from the lanmark to the camera. Clearly, the noise in lanmark position is anisotropic an inhomogeneous. For estimating motion parameters for these kin of error istributions the HEIV motion estimator was evelope by Matei an Meer [9], [1]. For successive stereo frames, the process escribe above can be use to obtain a set of corresponing lanmarks with their 3D position an relate uncertainty. This results in a set L of corresponing 3D points i.e. L = {L 1...L m }, where L i = ( v i,t 1, Σ i,t 1, v i,t, Σ i,t ). Then the HEIV motion estimator is use to generate n minimal-set motion hypothesis {M 1...M n }. Each motion hypothesis M is estimate on six lanmarks from the set L. The number of motion hypothesis n require to attain a probability of p that there is at least one motion hypothesis base on only inlier lanmarks, can be compute with n = log(1 p)/ log(1 (1 ε) 6 ). (19) Where ε is the probability of selecting an outlier lanmark. Often, it is useful to measure how well two timecorresponing 3D points align with a motion estimate. For this we use the Bhattacharyya istance. Given two points v t 1 an v t with their reconstruction uncertainties Σ t an Σ t 1, we apply the estimate motion M on v t an Σ t. The Bhattacharyya istance is then compute as D = 1 ( v t v t 1 )(Σ t + Σ t 1 )( v t v t 1 ) + 1 log( Σ t+σ t 1 Σ t Σ t 1 ). () IV. EXPERIMENTAL SETUP A synthetic ata-set, consisting of uniformly istritbute 3D points, is use to evaluate algorithm performances uner specific outlier conitions. A percentage of the 3D lanmarks is transforme accoring to the true ego-motion ˆM = (ˆ v, ˆq), whereas the other lanmarks are transforme with a ranom motion each to create outliers. The motion ˆM consist of translation an rotation along/aroun all axis. The 3D lanmarks from before an after the motion are projecte onto the imaging planes of a moele stereo camera. Subsequently, Gaussian noise is ae to the locations of the lanmark projections. These image points are use as input to the EM-SE(3) an algorithms. The process escribe above is repeate for k = 5 times for several outlier percentages. For each of the successive 5 trials the translation an rotation along/aroun each axis is increase, this simulates an accelerating platform. For the synthetic ataset, the robustly estimate ego-motion M can be compare against the true ego-motion ˆM. The use performance metric is the error expresse as a percentage of the grountruth. For translation this gives 1% k k v n ˆ v. (1) ˆ v n=1 An appropriate single value istance metric between rotations q an ˆq is the angle of the rotation q ˆq, this rotation brings the estimate rotation q onto the grountruth rotation ˆq, Gramkow []. For rotations this results in the performance metric 1% k k n=1 arccos(q n ˆq). () arccos(ˆq)

5 Also, a real-worl automotive ata-set containing approximately 8 images an spanning about 6 meters was use. It was recore using a stereo camera with a baseline of approximately centimeters. The stereo camera was mounte on our test vehicle RoboJeep. Furthermore, an image resolution of 6 by 8 pixels is use at 3 frames per secon. An impression of the stereo-images in the ataset is given in fig. 1. By concatenating the frame-to-frame Error % Error in translation 6 Percentage of outliers (a) Error % Error in rotation 6 Percentage of outliers (b) Fig.. Error on synthetic ata using 93 motion hypothesis, translation (a), rotation (b). Fig. 1. Two left images extracte from their stereo-pairs both containing an inepenently moving object. In both instances this is an approaching car. ego-motion estimates, the vehicle pose can be tracke. This enables that the complete riven trajectory can be reconstructe. In parallel to the stereo-frames also ifferential GPS (DGPS) positions were recore. Both the DGPS reaouts an the stereo-frames have synchronize time-stamps. This coul be exploite for comparing the estimate motion at any time with the DGPS base groun truth. Because of the inaccuracy an sparsity of the DGPS reaouts however, a ifferent performance metric is use. The ata-set encompasses an almost exact loop. Ieally, the final estimate pose shoul be near the starting pose. Therefore, the performance metric is the istance between the final estimate position an the starting position. In realistic conitions the number of motion hypothesis that can be generate is limite. Therefore, both algorithms only generate 93 motion hypothesis. When assuming an outlier percentage of 5, this ensures with a probability of.99 that at least one motion hypothesis was estimate on inlier lanmarks only, eq. 11. The pseuo coe of the EM- SE(3) an algorithms are given in appenix A. V. RESULTS Fig. shows the performance of both algorithms on the synthetic ata-set. It can be observe that the EM-SE(3) is more accurate for outlier percentages below 5 percent. While these ifferences may seem marginal, keeping the error as low as possible, especially for rotations, is crucial for estimating large real-worl trajectories. Also, note the breakown of both methos at an outlier percentage of 5. This is cause by the fact that only 93 motion hypothesis were use. To measure the computational complexity, both methos were evaluate using various parameter settings, the results are plotte in fig. 3. It can be seen that the novel EM-SE(3) algorithm requires less computational time an scales more favorably in the number of lanmarks. An algorithm that Average computation time (s) Number of minimal set hypothesis (a) Average computation time (s) Number of lanmarks Fig. 3. Time complexity in the number of minimal-set motion hypothesis (a), in the number of lanmarks (b). is faster can generate more minimal-set motion hypothesis in a given time-span making it potentially more robust an accurate. To evaluate the real-worl applicability of the propose methos an automotive ata-set is use. A top view of the estimate trajectories of both methos as well as DGPS is given in fig. The estimate trajectories show that EM-SE(3) is more accurate than. The 3D (thus incluing height) ifference in starting an ening location is for EM-SE(3) approximately 3 m an for 1 m. As percentage of the travele istance this gives.5% an 1.6% respectively. For one particular estimate, the rotation covariance matrix Σ q is visualize in fig. 5. It can be seen that the largest uncertainty is in rotation aroun an axis almost parallel to the Z axis i.e. roll. By converting the iagonal of Σ q to a rotation vector an extracting its Euler angles, the stanar eviation in pitch, heaing an roll can be approximate. Table I shows these stanar eviations in rotation an translation for several segments of the ata-set. It can clearly be seen TABLE I STANDARD DEVIATION IN ROTATION AND TRANSLATION ESTIMATED (b) ON-LINE BY THE EM-SE(3) ALGORITHM σ rotation σ translation (egrees) (millimeters) Pitch Heaing Roll X Y Z 1 Accelerating Cornering Accelerating Cornering

6 Fig. 5. Rotation error ellipsoi in so(3), rotation samples are given with black ots, mean rotation is given by the magenta circle. that most uncertainty occurs in the estimate roll angle. Also note the larger eviations in translation over the Z axis when the vehicle is accelerating. Knowing the magnitue of these errors is crucial for optimally fusing the motion estimate with other motion or pose sensors. It shoul be note that for this application the covariance Σ q can be use irectly instea of the approximate stanar eviations in Euler angles. Fig.. Satellite view of the riven trajectory with the test vehicle in an urban environment. The DGPS coorinates are inicate in re. Results for both methos using 93 minimal-set motion estimates are inicate for EM-SE(3) in green an for in blue. VI. CONCLUSION A novel robust visual-oometry technique, calle EM- SE(3), has been presente an compare against. EM-SE(3) utilizes an expectation maximization algorithm over a local linearization of the rigi-boy motion group SE(3). The results on the use synthetic an real-worl atasets show that EM-SE(3) is both more efficient an more accurate. This contribution shows that robust an accurate statistical inferences can be mae on elements of SE(3). This is achieve through the interplay of robustly iscaring outlier minimal-set hypothesis, by means of expectation maximization, an the ability of computing an accurate weighte mean motion, by using Riemannian geometry. Because this technique works irectly with the motion hypothesis it is more efficient an scales better if the number of lanmarks is increase. For this research a basic approach is use. It has to be note that both efficiency an accuracy of can be improve by, for example, using avance preemptive methos or post optimization. In this work however, the choice has been mae to compare both methos in their purest forms. Nevertheless, the results show that EM-SE(3) is a powerful alternative. In future work, the EM-SE(3) algorithm will be evaluate on even more challenging urban an outoor ata-sets. Furthermore, guie sampling base on lanmark classification an tracking is a promising extension.

7 REFERENCES [1] L. Dorst, D. Fontijne, an S. Mann, Geometric Algebra for Computer Science (An Object-Oriente Approach to Geometry). Morgan Kaufmann, 7. [] P. Elinas, R. Sim, an J. J. Little, σslam: Stereo vision slam using the rao-blackwellise particle filter an a novel mixture proposal istribution, in IEEE International Conference on Robotics an Automation, 6, pp [3] M. A. Fischler an R. C. Bolles, Ranom sample consensus: a paraigm for moel fitting with applications to image analysis an automate cartography, Communications of the ACM, vol., no. 6, pp , June [] C. Gramkow, On averaging rotations, International Journal of Computer Vision, 1. [5] R. I. Hartley an A. Zisserman, Multiple View Geometry in Computer Vision, n e. Cambrige University Press, ISBN: ,. [6] W. T. Loring, An Introuction to Manifols, S. Axler an K. Ribet, Es. Springer, 7. [7] D. G. Lowe, Distinctive image features from scale-invariant keypoints, International Journal of Computer Vision, vol. 6, no., pp ,. [8] M. Maimone, Y. Cheng, an L. Matthies, Two years of visual oometry on the mars exploration rovers: Fiel reports, Journal of Fiel Robotics, vol., no. 3, pp , March 7. [9] B. C. Matei an P. Meer, Optimal rigi motion estimation an performance evaluation with bootstrap, in IEEE Computer Society Conference on Computer Vision an Pattern Recognition, vol. 1, no. 1, 1999, p. 35. [1], Estimation of nonlinear errors-in-variables moels for computer vision applications, IEEE Transactions on Pattern Analysis an Machine Intelligence, vol. 8, no. 1, pp , October 6. [11] L. Matthies an S. A. Shafer, Error moeling in stereo navigation, IEEE Journal of Robotics an Automation, vol. 3, no. 3, pp. 39 8, June [1] D. Nistér, Preemptive ransac for live structure an motion estimation, Machine Vision an Applications, vol. 16, no. 5, pp , November 5. [13] D. Nistér, O. Naroitsky, an J. Bergen, Visual oometry, in IEEE Computer Society Conference on Computer Vision an Pattern Recognition, vol. 1,, pp [1], Visual oometry for groun vehicle applications, Journal of Fiel Robotics, vol. 3, no. 1, pp. 3, January 6. [15] C. F. Olson, L. H. Matthies, M. Schoppers, an M. W. Maimoneb, Rover navigation using stereo ego-motion, Robotics an Autonomous Systems, vol. 3, no., pp. 15 9, June 3. [16] F. Park, Distance metrics on the rigi-boy motions with applications to mechanism esign, Transactions of the ASME, vol. 117, pp. 8 5, March [17] X. Pennec, Computing the mean of geometric features: Application to the mean rotation, INRIA, Tech. Rep. 3371, March [18] X. Pennec, C. R. G. Guttmann, an J.-P. Thirion, Feature-base registration of meical images: Estimation an valiation of the pose accuracy, Lecture Notes in Computer Science, vol. 196/1998, pp , 6. [19] P. L. Rosin, Robust pose estimation, IEEE Transaction on Systems, Man, an Cybernetics, vol. 9, no., pp , April [] J. M. Selig, Geometrical Methos in Robotics, D. Gries an F. B. Schneier, Es. Springer, [1] R. Subbarao, Y. Genc, an P. Meer, Nonlinear mean shift for robust pose estimation, in IEEE Workshop on Applications of Computer Vision, February 7, pp [] P. Torr an D. Murray, The evelopment an comparison of robust methos for estimating the funamental matrix, International Journal of Computer Vision, vol., no. 3, pp. 71 3, [3] S. Umeyama, Least-squares estimation of transformation parameters between twopoint patterns, IEEE Transactions on Pattern Analysis an Machine Intelligence, vol. 13, no., pp , [] X. S. Zhou, D. Comaniciu, an A. Gupta, An information fusion framework for robust shape tracking, IEEE Transactions on Pattern Analysis an Machine Intelligence, vol. 7, no. 1, pp. 1 15, January 5. Algorithm 1 EM-SE(3) 1) Use the SIFT matching proceure as escribe in III an create the set L of time corresponing three imensional points with their uncertainty i.e. L = { L 1...L n} using eq. 15,16,17 an 18. ) Preict the current motion base on the previous motion with M t = M t 1 an Σ t = Σ t 1 + Σ. This will be the starting point for EM optimization. 3) Create 93 minimal-set motion hypothesis with: for 93 iterations o - Take at ranom 6 points from L. - Estimate the motion on these six points using the HEIV estimator [9]. en for This results in the set of motion hypothesis M = {M 1...M 93 }. 7) Start the EM algorithm: for iterations o - Compute the weights I i an O i for each motion sample given the current mean motion using eq Compute a new mean motion M base on the new weights using eq Compute the motion covariance matrix Σ m base on the new mean an the weights using eq Compute the inlier mixing weight p(i) base on the new mean an covariance using eq. 1. en for 8) Return the mean motion M an its covariance matrix Σ m. Algorithm 1) Use the SIFT matching proceure as escribe in III an create the set L of time corresponing three imensional points with their uncertainty i.e. L = { L 1...L n} using eq. 15,16,17 an 18. ) Create 93 minimal subset motion hypothesis with: for 93 iterations o - Take at ranom 6 points from L. - Estimate the motion on these six points using the HEIV estimator [9]. - Calculate the Bhattacharyya istance given the current motion hypothesis for each point in T. The score of the current motion hypothesis is the number of lanmarks with a Bhattacharyya istance less than 1.5. en for This results in the set of motion hypothesis M = {M 1...M 93 } with their scores. 3) Select from M that motion estimate M with the highest score as the final robust estimate. APPENDIX A