Fusion of Discrete and Continuous Epipolar Geometry for Visual Odometry and Localization

Transcription

1 Fusion of Discrete and Continuous Epipolar Geometry for Visual Odometry and Localization David Tick Computer Science Dept. University of Texas at Dallas Dallas, Texas 7 dqt81@utdallas.edu Jinglin Shen Electrical Engineering Dept. University of Texas at Dallas Dallas, Texas 7 sxj96@utdallas.edu Dr. Nicholas Gans Electrical Engineering Dept. University of Texas at Dallas Dallas, Texas 7 ngans@utdallas.edu Astract Localization is a critical prolem for uilding moile rootic systems capale of autonomous navigation. This paper descries a novel visual odometry method to improve the accuracy of localization when a camera is viewing a piecewise planar scene. Discrete and continuous Homography Matrices are used to recover position, heading, and velocity from images of coplanar feature points. A Kalman filter is used to fuse pose and velocity estimates and increase the accuracy of the estimates. Simulation results are presented to demonstrate the performance of the proposed method. I. INTRODUCTION Building moile rootic systems that are capale of realtime, autonomous navigation is a complex, multi-faceted prolem. One of the primary aspects of this prolem is the task of localization. Localization, or pose-estimation, refers to the task of estimating the velocity (oth angular and linear), position, and orientation of the root at any given instant. There are many estalished ways to approach the task of localization including wheel odometry [1], inertial sensors [1], GPS [1], sonar [], and IR/laser-ased range finding sensors []. There has also een significant development of localization techniques which are solely vision-ased. Visual odometry is a method of localization that uses one or more cameras to continuously capture images or video frames taken of a scene [4]. The frames are analyzed sequentially using various computer vision techniques. The analysis of these frames estimates angular and linear velocities of the camera etween each time step. These velocities can then e integrated over time to estimate how the camera has moved. Using this technique, an estimate of a moile root s speed, position, and heading (orientation) can e calculated and maintained over time. A pose estimate produced in this manner can e further enhanced y comining visual odometry with traditional wheel odometry and other sensor devices. This can e achieved through the use of various signal processing techniques called sensor fusion. The Kalman filter is particularly useful for fusing signals from multiple sensors and removing errors in localization that occur due to many factors such as sensor noise, quantization, flawed process modeling, and sensor ias or drift [1], []. Some vision-ased localization techniques are designed to calculate the pose of the camera relative to some well known reference oject that appears in each frame [6], [7]. Usually these techniques require the system to have accurate geometric information aout the scene and/or some reference oject(s) in the scene prior to execution. In many situations, there exists little accurate prior knowledge regarding a scene and the ojects therein. In such cases, two frames can e compared with one another ased on a set of feature points which exists in oth frames. Once a set of feature points is identified in oth frames, a homogeneous mapping of the feature points from one frame onto the next must exist. This mapping can e modeled as a linear transformation and encapsulates the rotation and translation of the camera that occurs etween the taking of each picture. In practice, the Essential Matrix or Euclidean Homography Matrix is used to estimate a camera s pose in terms of a set of rotational and translational transformations [8], [9]. Over the years, many control systems have een designed that utilize Essential or Homography Matrices in vision-ased rootic tasks [1] [1]. The Euclidean Homography Matrix can e used in oth a continuous as well as a discrete form [9]. The Homography Matrix, in its discrete form, has een widely used for many years y the community [14] [18]. However, it seems that there has not een as much work done regarding applications of the Homography Matrix in its continuous form. Of particular interest in this paper, is the application of the Euclidean Homography Matrix as a means of estimating velocity [19]. If one does a discrete homography-ased estimation of the camera position and orientation (i.e., pose), then one can also integrate the continuous estimate of the velocity at each time step to extract the pose as well. The pose estimate otained from integrating the continuous homographic estimate of angular velocity must agree with the discretely estimated position of the camera. In this paper, we propose a method which uses a single six degrees of freedom (6DOF), or eye in hand, camera to do homography-ased visual odometry. This system will capture video frames and identify sets of coplanar feature points in a scene. It will estimate the change in pose of the camera y using the discrete form of the Euclidean Homography Matrix. The system will also utilize the continuous form of the Euclidean Homography Matrix to estimate the velocity of the camera. Our system utilizes the well known Kalman filter to fuse these two estimates and remove error which accrues /1/$6. 1 IEEE

2 over time due to integration, noise, and quantization []. Section II clarifies the terminology used in this paper as well as formally defining and modeling the prolem. Section III explains the proposed approach at homographic visual odometry. We present simulation results in section IV that illustrate the effectiveness of the proposed system. Finally, in section V we espouse the various conclusions that we have reached. Z w F w X w (v 1, ω 1 ) (v... k-, ω k- ) Y w (v k-1, ω k-1 ) X F (t k-1 ) (v k, ω k ) Y Z F (t k ) II. BACKGROUND While there are certain formal conventions that exist with respect to the terminology used in vision-ased localization, there is no official standard. Thus, it is necessary that we define the terminology that we use in this paper. A. Formal Definition and Terminology Fig. 1 illustrates a camera, with attached reference frame, moving over time. The lowercase su-script attached to each axis lael indicates the frame of reference to which that axis elongs. Navigation sensors, including cameras, report measurements in terms of the moving ody frame, formally called F. These measurements are then rotated to otain localization with respect to the world frame, formally called F w. The z-axis of F is oriented along the optical axis, the x-axis is oriented along the horizontal direction of the image plane, and the y-axis is oriented parallel to the vertical direction of the image. The orientation and position (collectively referred to as pose) of a ody frame in terms of the world frame is expressed as a function of time y including the index numer for a given time step in parentheses. For example, in Fig. 1 F (t ) descries the pose of the ody frame F at time index t as measured from F (t ), while F (t k ) descries the pose of F at time t k with respect to F (t ). The changes in the pose of F that occur over the time interval [t, t 1,..., t k 1, t k ] is descried in terms of a translation vector T k R and a rotation matrix R k SO (), formally written as (T k, R k ), where SO () means Special Orthogonal Group of order three. At any given time t k, the instantaneous linear and angular velocities of the ody frame F are descried, as a pair of vectors (v k, ω k ), where v k R and ω k R. B. Pinhole Camera Model Fig. illustrates a camera taking two images from two different poses F and F (t). F is considered a static reference frame, such as the pose at time t =. Without loss of generality we take F w = F. F (t) is considered a moving frame or current frame. The changes which exist etween the two poses, as stated aove, are encapsulated y (T, R). Fig. also shows N 4 feature points that all lie in the plane π s. The D coordinates of each feature point as measured from the reference frame of the pose F (t) are defined in terms of a vector m j (t) R. Similarly, the D coordinates of each feature point as measured from the reference frame of the pose Fig. 1. m 1 1 m (v, ω ) F (t ) X F (t 1 ) Y F (t t k- ) ( T k, R k ) Translation and Rotation (Tk, Rk) of Body/Camera Frame F Y Z X Fig m t t (T, R) t Y t s m t 1 Z t Planar Scenes and Feature Points X t F are defined in terms of a vector m j R. Formally these vectors are given as m j R = [ x j, y j, z j ] T, j {1,..., N} m j (t) R = [x j (t), y j (t), z j (t)] T, j {1,..., N}. Two different images are captured y the camera at the two poses F and F (t). This is modeled y projecting the feature points onto the D image planes π and π (t). The coordinates of the feature points in these D planes are expressed as a normalized set of D coordinates, where depth along the Z-axis is set equal to one. The normalized image plane coordinates that result from this projection, as measured from the reference frame of the pose F (t) are defined in terms of a vector m j (t) R. Similarly, the normalized image plane coordinates of each feature point as measured from the reference frame of the pose F are defined in terms of a vector m j R. These vectors are expressed as [ T x m j R j = zj, y j zj, 1], j {1,..., N} m j (t) R = [ xj (t) z j (t), y j (t) z j (t), 1 ] T, j {1,..., N}.

3 C. Euclidean Homography In this work, we focus on the planar Homography case, that is, all D feature points are on the same plane. Consider the two sets of points m(t) on plane π s, mentioned aove, the transformation etween the two sets is given y [9] m j = Rm j + T. (1) The relationship in (1) can e rewritten in terms of image points as [8] m j = (R + 1 d T n T )m j where n = [n x, n y, n z] T is the constant unit normal vector of plane π s measured in F, and d is the constant distance etween the optical center of the camera (i.e. the origin of F ) to the plane π s. We assume that d is known in this initial investigation. The matrix H d = R + 1 d T n T defines what is known as the discrete Homography Matrix. By using the four-point algorithm, the Homography Matrix H d (t) can e solved to recover the translation vector T (t) and the rotation matrix R(t) [8]. In the continuous case, image point m j (t) and its optical flow ṁ j (t) are measured instead of image pair m j and m j (t). The time derivative of m j (t) satisfies ṁ j = ˆωm j + v () where ˆω(t) R is the skew-symmetric matrix of ω(t). The relationship in () can e rewritten in terms of image points as [9] The matrix ṁ j (t) = (ˆω + 1 d vn T )m j (t). H c = ˆω + 1 d vn T is defined as the continuous Homography Matrix. Similar to the discrete form, a four-points algorithm gives the solution for linear velocity v(t) and angular velocity ω(t) of the camera. III. APPROACH As descried in section II, the translation and rotation of a camera can e recovered y applying the Homography Matrix method in its discrete form. Similarly, the linear and angular velocities of a camera can e otained using the continuous form. However, the estimation can e noisy due to possile sensor noise from the camera and numerical error produced during computation of the Homography Matrix. Integration of the continuous velocity estimate over time should agree with the discrete pose estimate, thus fusing velocity and pose estimates will reduce the effect of noise. A Kalman filter is uilt to perform the sensor fusion, including integration, and generate the final pose and velocity estimates. The reference feature points m j are taken as the points at initial time t. At each time step t k, feature points m j and m j (t k ) are used to calculate the Homography Matrices H d (t k ) and H c (t k ). The Homography Matrices are decomposed to solve for R k, T k, v k, ω k. The system states are the position and velocity of the camera. Roll, pitch, and yaw angles (denoted r(t k ), p(t k ), w(t k )) represent the camera s orientation instead of the recovered rotation matrix. The relation etween the angle rates ṙ(t k ), ṗ(t k ), ẇ(t k ) and angular velocity ω k = [ω x (t k ), ω y (t k ), ω z (t k )] T is given y [] ṗ = (ω x cos r ω y sin r) () ẇ = (ω x sin r + ω y cos r) sec p (4) ṙ = (ω x sin r + ω y cos r) tan p + ω z. () The system equations for the Kalman Filter are given y In (6), the state vector is given y x k = F k x k 1 + w k (6) y k = H k x k + v k. (7) x k = [x, y, z, p, w, r, v x, v y, v z, ω x, ω y, ω z ] T R 1 and w k R 1 is a normally distriuted random process with zero mean and covariance matrix Q k R 1 1. The state transition matrix F k R 1 1 and the process covariance matrix Q k can e found in the Appendix. A random walk process is used to determine Q k. In F k and Q k the term t is the frame rate of the camera, and the various σ terms are scaling terms for the states and are indicated as suscripts. In (7), the measurement matrix H k R 1 1 is an identity matrix since all components of the state vector are eing measured. The sensor noise vector v k R 1 is a normally distriuted random process with zero mean and covariance matrix R k R 1 1, which is a diagonal matrix, whose diagonal elements are decided according to the estimated measurement noise for each state vector component. Given the terms in (6) and (7) the Kalman filter is designed and updated in the typical manner []. Estimates from the update step are utilized to provide the pose and velocity estimates that are in turn used for localization of the camera. IV. RESULTS In this section, simulation results of the proposed Homographic visual odometry method are presented. A single camera oserves four static, coplanar feature points. The camera motion is generated so that the feature points always stay in the field of view. The linear velocity is sinusoidal along all degrees of freedom. The angular velocity is zero along the x-axis and sinusoidal along the other two axes. The frame rate of the camera is frames/second. Discrete and continuous Homography matrices compute the estimated camera position, orientation and velocities. Next, these homographic estimates are input into the Kalman filter, which fuses them, producing the final state estimate. The results are then plotted against the camera motion for comparison. In the first simulation, the fused results from the Kalman filter are compared with results which come directly from using only one of the Homography matrices. Gaussian noise with zero mean and variance equal to.1 is added. The

4 diagonal values of R k used in our simulations are [.1,.1,.1,.1,.1,.1,.1,.1,.1,.1,.1,.1]. Fig. through Fig. 6 show the estimated camera position, orientation, linear and angular velocities respectively. The dashed line (lue) represents the value of the simulated trajectory, and the two solid lines (green and red) represent the estimate otained directly from Homography(H) and the fused estimate produced y the Kalman filter(kf). Fig. and Fig. 4 show that given a random initial state, the estimated values of position and orientation converge toward the trajectory quickly, and thereafter estimation error remains small for oth the Homography and the Kalman filtered estimates. Fig. and Fig. 6, show that the velocity estimates from Kalman filter track the values quite well, while the continuous Homography estimate is much noiser. Also, note that a lag exists etween the signals and the filtered estimates. Large values of the diagonal elements in the measurement covariance matrix R k cause this lag. Larger values in R k make the estimated trajectories more accurate, ut increase the lag of the Kalman filter. There is a trade-off etween the system s roustness against noise, and the overall speed of the system. In the second simulation, we investigate the roustness of our proposed method y looking at its performance when different amounts of noise are added. Fig. 7 through Fig. 1 once again show estimated camera position, orientation, linear and angular velocities respectively. The dashed line (lue) represents the value of the simulated trajectory, and the two solid lines (green and red) represent the fused estimate produced y the Kalman filter with no noise added(n=), and with Gaussian noise having zero mean and variance equal to. added(n=.). Comparison with the set of results from the first simulation, reveals that the accuracy of the position and orientation estimates are not severely impacted y the introduction of more noise. However, the velocity estimates do ecome significantly noisier as the variance of the sensor noise increases. These results indicate that our proposed system is sensitive to noise in its velocity estimate. There are several approaches which could mitigate this noise sensitivity. One possile solution is to utilize a Fixed Interval Kalman Smoother [1]. As an example, the effect of this smoother on velocity estimation is shown in Fig. 11 and Fig. 1. Note that the velocity estimates are drastically improved when this method is employed. However, this solution that is not usale in real time. Another solution, which is capale of eing run in real time, is to add more feature points to the set. While this would likely provide some additional roustness against noise, it should e noted that in this initial investigation we are evaluating performance of the proposed system in the worst case situation of having only the minimum set of four feature points. V. CONCLUSION In this paper, we present a novel visual odometry method for localization of a moile rootic system. Continuous and discrete forms of the Euclidean Homography Matrix are used Posx(m) Posy(m) Posz(m) Angx(rad) Angy(rad) Angz(rad) Fig.. Filter vs DH: Position Estimate (noise variance =.1) Fig. 4. Filter vs DH: Orientation Estimate (noise variance =.1) Vx(m/s) Vy(m/s) Vz(m/s) Fig.. Filter vs CH: Linear Velocity Estimate (noise variance =.1)

5 Wx(rad/s) Vx(m/s) estimated(n=) estimated(n=.) Wy(rad/s).. Vy(m/s) Wz(rad/s).. Vz(m/s) Fig. 6. Filter vs CH: Angular Velocity Estimate (noise variance =.1) Fig. 9. Filtered Linear Velocity Estimate with Camera Noise Added Posx(m) estimated(n=) estimated(n=.) Wx(rad/s).. estimated(n=) estimated(n=.) Posy(m) Wy(rad/s) Posz(m) Wz(rad/s) Fig. 7. Filtered Position Estimate with Camera Noise Added Fig. 1. Filtered Angular Velocity Estimate with Camera Noise Added Angx(rad).. estimated(n=) estimated(n=.) Vx(m/s) estimate(kf) estimate(ks) Angy(rad).. Vy(m/s) Angz(rad).. Vz(m/s) Fig. 8. Filtered Orientation Estimate with Camera Noise Added Fig. 11. Smoothed Estimate of Filtered Linear Velocity w/noise Added

6 Wx(rad/s) Wy(rad/s) Wz(rad/s).... estimate(kf) estimate(ks) actual Fig Smoothed Estimate of Filtered Angular Velocity w/noise Added to recover position and velocity information from camera images. A Kalman filter is used to fuse the velocity and pose estimates. This reduces the effects of noise and improves the estimate. Simulations were performed to explore the performance of the proposed method. When sensor noise is not considered, the estimated positions and velocities show little error from the signals. Even when noise is introduced to the camera model, the velocity estimates produced y our proposed method are dramatically etter than the velocity estimates otained directly from the continuous Homography Matrix. Carefully choosing the parameters of the Kalman filter allows the user to alance the accuracy and responsiveness of the system. Furthermore, there exist oth offline and real-time solutions that mitigate the effects of noise sensitivity. There are several avenues open for future work. In the short term, experiments will e performed of the presented method. This will require accurate and fast detection and tracking of features. Low levels of tracking error will e important to mitigate sensor noise. In the longer term, a method must e determined to handle large scale movements of the root and camera such that feature points can e allowed to enter and leave the camera field of view without disrupting the estimation. Finally this method will e incorporated with other localization and odometry methods, including inertial measurement units and wheel encoders. REFERENCES [1] G. Dudek and M. Jenkin, Inertial sensors, gps, and odometry, in Springer Handook of Rootics, 8, pp [] L. Kleeman and R. Kuc, Sonar sensing, in Springer Handook of Rootics, 8, pp [] R. B. Fisher and K. Konolige, Range sensors, in Springer Handook of Rootics, 8, pp [4] D. Nister, O. Naroditsky, and J. Bergen, Visual odometry, in Computer Vision and Pattern Recognition, 4. CVPR 4. Proceedings of the 4 IEEE Computer Society Conference on, vol. 1, 7 4, pp. I 6 I 69 Vol.1. [] R. G. Brown, Introduction to Random Signal Analysis and Kalman Filtering. John Wiley & Sons, 198. [6] D. DeMenthon and L. S. Davis, Model-ased oject pose in lines code, in European Conf. on Computer Vision, 199, pp. 4. F k = Q k = APPENDIX KALMAN FILTER MATRICES 1 t 1 t 1 t 1 cos (r) t sin (r) t sin(r) 1 cos(p) cos(r) t cos(p) t 1 sin (r) tan (p) t cos (r) tan (p) t t (σx t ) (σx t ) ( ) σy t ( ) σy t (σz t ) (σz t ) ( ) σp t ( ) σp t (σw t ) (σw t ) (σr t ) (σx t ) σx t ( ) σy t σy t (σz t ) σz t ( ) σp t σp t (σw t ) σw t (σr t ) (σr t ) σr t [7] L. Quan and Z.-D. Lan, Linear n-point camera pose determination, IEEE Trans. Pattern Anal. Mach. Intell., vol. 1, no. 8, pp , [8] O. D. Faugeras and F. Lustman, Motion and structure from motion in a piecewise planar environment, Int. J. Pattern Recog. and Artificial Intell., vol., no., pp. 48 8, [9] Y. Ma, S. Soatto, J. Koseck, and S. Sastry, An Invitation to -D Vision. Springer, 4. [1] E. Malis, F. Chaumette, and S. Boudet, -1/D visual servoing, IEEE Trans. Root. Autom., vol. 1, no., pp. 8, [11] C. Taylor and J. Ostrowski, Roust vision-ased pose control, in Proc. IEEE Int. Conf. Rootics and Automation,, pp [1] Y. Fang, D. Dawson, W. Dixon, and M. de Queiroz, Homographyased visual servoing wheeled moile roots, in Proc. IEEE Conf. on Decision and Control,, pp [1] Y. Fang, W. E. Dixon, D. M. Dawson, and P. Chawda, Homographyased visual servo regulation of moile roots, IEEE Trans. Syst., Man, Cyern., vol., no., pp ,. [14] Z. Zhang and A. Hanson, D reconstruction ased on homography mapping, in Proc. ARPA Image Understanding Workshop Palm Springs CA, [1] N. Daucher, M. Dhome, J. Laprest, and G. Rives, Speed command a rootic system y monocular pose estimate, in Proc. IEEE/RSJ Int. Conf. Intelligent Roots and Systems, 1997, pp. 4. [16] C. Baillard and A. Zisserman, Automatic reconstruction planar models from multiple views, in Proc. IEEE Conf. Computer Vision and Pattern Recognition, 1999, pp [17] E. Malis and F. Chaumette, 1/D visual servoing with respect to unknown ojects through a new estimation scheme camera displacement, Int. J. Computer Vision, vol. 7, no. 1, pp ,. [18] K. Okada, S. Kagami, M. Inaa, and H. Inoue, Plane segment finder: algorithm, implementation and applications, in Proc. IEEE Int. Conf. Rootics and Automation, 1, pp [19] V. Chitrakaran, D. M. Dawson, W. E. Dixon, and J. Chen, Identification a moving oject s velocity with a fixed camera, Automatica, vol. 41, no., pp. 6,. [] D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology (nd Edition)l. Institution of Engineering and Technology, 4. [1] H. Rauch, F. Tung, and C. Strieel, Maximum likelihood estimates of linear dynamic systems, AIAA, vol. (8), pp , 196.