Closed-form Linear Solution To Motion Estimation In Disparity Space

Transcription

1 Closed-form Linear Solution To Motion Estimation In Disparity Space Konstantinos G. Derpanis Vision Technologies Sarnoff Corporation Princeton, NJ 8543 Peng Chang ABSTRACT Real-time stereovision systems play an important role in automotive related applications. This paper concerns the problem of rigid motion estimation with a stereovision sensor. Given a set of corresponding 3D points in Euclidean space reconstructed from stereovision, efficient linear algorithms exist to solve for the rigid motion. However it has been well known that the noise in the Euclidean reconstruction from stereovision is heteroscedastic and anisotropic, therefore the linear algorithms are only sub-optimal. Recently a d-motion based algorithm has been developed to solve for the rigid motion directly in the disparity space, in which the noise can be approximated to be homogenous and isotropic. However this algorithm is nonlinear and requires iterative least-squares solution. By reformulating the problem, a closed-form linear solution is presented in this paper to solve for the rigid motion in disparity space. Synthetic experimental results show that this new algorithm outperforms the d-motion based algorithm in terms of both accuracy and computational cost. We believe that the closed-form linear solution is potentially very useful for applications making use of stereovision to estimate rigid motion. I. INTRODUCTION Stereovision based approaches have played a very important role in automotive related applications, and recently gained increased popularity due to the fact that the real-time stereovision systems are becoming increasingly affordable [], [2], [3], [4], [5], [6], [7]. In this paper, we focus on the problem of rigid motion estimation with a stereovision sensor. To estimate the rigid motion from a stereovision sensor, a typical approach often involves extracting and matching image feature points in subsequent images from a designated camera of the stereo pair. Meanwhile, the disparities of those points are obtained from the stereo matching process. As a result, the correspondences in 3D across time can be established, which provide enough information to infer the 3D rigid motion. Many previous approaches perform the motion estimation with the 3D reconstruction in Euclidean space [8], [9], []. Given corresponding 3D point sets, the rigid motion can be estimated with closed-form least-squares solutions as reviewed in []. However, least-squares algorithms are optimal only when the data is corrupted with independent and identically distributed (i.i.d.), zero mean and isotropic noise, and give poor estimation with the stereo-reconstructed 3D points, which are known to be corrupted with heteroscedastic and anisotropic noise. In [2], a heteroscedastic error in variables (HEIV) based algorithm is presented, which yields optimal rigid motion estimates with 3D points reconstructed from stereo, however such method requires estimation of the underlying uncertainty for each reconstructed 3D point, which itself is nontrivial. Alternatively, methods have been proposed to estimate the rigid motion directly in the disparity space, in which the noise can be reasonably assumed to be isotropic and i.i.d. [3], [4]. In [3], a d-motion based algorithm has been presented to solve for the rigid motion in disparity space directly. Unfortunately, this algorithm is nonlinear and uses an iterative weighted least-squares method, which is not guaranteed to converge. In addition, it is assumed to only handle small rotations. In this paper, we present a closed-form linear algorithm to solve for general rigid motion in disparity space. The synthetic experimental results clearly show its advantage over the previous approach. In our approach, the rigid motion estimation is based on discrete feature points. Alternatively, dense disparity maps and grey scale images can be used to perform the rigid motion estimation [5], [6]. Our primary concern is computational cost, while detailed comparison is beyond the scope of this paper. In Section II the disparity space formulation and the previous algorithm as in [3] are first reviewed. Then the closedform linear solution is presented, which come in two possible forms. In Section III synthesized experiments are carried out, and comparisons are made among the new algorithm and previous algorithms. In Section IV discussions are made and future work outlined. II. ESTIMATION IN DISPARITY SPACE Rather than estimating the rigid motion in the Euclidean space the points are corrupted by heteroscedastic noise, Demirdjian and Darrell [7] demonstrate that the estimation can be done in disparity space, the noise is purported to be independent and isotropic for parallel stereo images. We first review the results in their work.

2 A. Disparity Space In homogeneous coordinates the rigid transformation (R rotation and t translation) that maps point P = (X, Y, Z) to P = (X, Y, Z ) is given as follows, ( ) ( ) ( ) P R t P = () Let ω = (x, y, d) be a point in disparity space, (x, y) represents the image coordinate of a point on the left image with respect to the optical center, and d the disparity. After a little matrix manipulation, we arrive at, ( ) ( ) ω P Γ (2) Γ = B (3) and indicates the transformation is up to a scale factor. And and represent the camera focal length expressed in the effective horizontal and vertical pixel size, respectively, and B the camera baseline. Both the baseline and focal length are assumed known. Similarly for the point P, ( ) ( ) ω P Γ (4) Substituting the inverse of (2) and (4) into (), yields ( ) ( ) ω ω H(R, t) ( R t H(R, t) = Γ (5) ) Γ (6) H(R, t) is a homography dependent on the 3D rotation R and translation t. B. Previous Nonlinear Algorithm As pointed out in the literature, the noise in the Euclidean space reconstructed from stereo is heteroscedastic and anisotropic, which in turn calls for HEIV (Heteroscedastic Error In Variables) based method for motion estimation. Alternatively, the noise in the disparity space is much betterbehaved, and can be reasonably assumed to be homogenous and isotropic, which allows for more efficient algorithms. We first review the algorithm proposed in [7], then present a closed-form linear solution to the motion estimation problem in disparity space, which turns out to be much more efficient and accurate in comparison. In [7] the covariance of the noise corrupting the disparity space coordinate is given by Λ = σ 2 I, I is the identity matrix. Given a list of N correspondences in the disparity space {(ω i, ω i ), i =...N}, the estimation of rigid motion in [7] is formulated as a non-linear least-squares minimization problem, and the error metric is based on the summarized squared error of the so called d-motion. In addition, the authors focus on the case of small motion and approximate the rotation R as follows, R = I + ω c ω b ω c ω a (7) ω b ω a Note that this matrix is not orthogonal and thus not truly a rotation matrix. A procedure to orthogonalize the estimated rotation matrix is summarized below. The total error metric E 2 is formulated as,, E 2 = i w 2 i P i u + v i (8) u = (t x, t y, t z, ω a, ω b, ω c ), (9) and w i is a scalar, v i a 3-vector and P i a 3 6 matrix which all depend on ω i and ω i. Given this form of the total error E 2, an iterative reweighted least-squares estimation algorithm is given: Algorithm Disparity space re-weighted least-squares estimation[7] : Initialize w i () =. Estimate u. 2: Evaluate w i (k + ) from the current solution for u. 3: Minimize E 2 (k + ) using a standard weighted leastsquares method while keeping w i fixed. 4: Termination estimation if E 2 (k + ) E 2 (k) ɛ then stop, else return to Step 2; ɛ is a user defined threshold. The author also pointed out that this approach is not guaranteed to converge. A problem with the approach as detailed above is that the rotation matrix (7) is not orthonormal. A procedure for converting the estimated rotation matrix (7) to the closest orthonormal matrix (in the Frobenius-norm sense ) is found by computing the SVD of the estimated rotation matrix and setting the matrix of singular values W to the identity matrix, yielding, ˆR = UWV () UIV () UV (2) In the case of larger motions, a global non-linear minimization method is required such as Levenberg-Marquardt [8], [9]. The closest matrix in the Frobenius-norm sense entails minimizing the least-squares distance between the individual components of two matrices.

3 C. Linear solution in disparity space In this section a novel linear solution is proposed for estimating the Euclidean rigid motion using the disparity space representation. Let (x i, y i, d i ) be the points in disparity space selected from the video frame at time t = j and (x i, y i, d i ) be their correspondents taken from the frame at t = j+, i =,..., n. Let m = ( x y d ) and ( x2 y 2 d 2 ) be the points in homogeneous coordinates (for clarity the index of the measurements has been dropped). The mapping from the points at t = j to t = j + is a 4 4 homography matrix [7] given by (5). Assuming that there is no noise present in the observations, the following constraint holds, x 2 = h m h 4 m y 2 = h 2 m h 4 m d 2 = h 3 m h 4 m (3) h k denotes the k th row of the homography H (index of measurements dropped for clarity). Rewriting (3) yields the following system of linear equations, Z i = and Z i θ = (4) x i y i d i... x i y i d i... x i... x 2i x i y 2i x i x 2i y i y 2i y i x 2i d i y 2i d i x 2i y 2i y i d i d 2i x i d 2i y i d 2i d i d 2i (5) θ = ( h h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9... h h h 2 h 3 h 4 h 5 h 6 ) (6) which is a vector consisting of the components of H. Given ( all the point correspondences we define matrix Z = ) Z. Z2 Zn Each match contributes three equations to the homography constraint, therefore the minimum number of measurement correspondences required for the estimation of the homography is five; due to the fact that the homography is defined up to a multiplicative scalar. A closed-form 2 estimate of Ĥ in the presence of noise is obtained by computing the smallest singular vector of the matrix Z. Since linear solutions for homography estimates are 2 In fact, this algorithm is stictly non-iterative if one discounts the solution of the eigensystem or the determination of the singular value decomposition which is used in computing the singular values. notoriously unstable for significant amounts of noise, the measurements in both video frames must be centered and scaled [2]. Note that the linearization introduces heteroscedastic noise [2], [22]; the normalization represents only a partial solution. From a statistical point of view better solutions exist (e.g., [2]) in the literature but they require the non-trivial estimation of the heteroscedastic noise characteristics prior to parameter estimation and are iterative computations which may preclude them from real-time applications. Let and represent the camera focal length and B the camera baseline, both are assumed to be known. The homography H in (6) can be expressed as follows, H(R, t) = r r 2 r 2 r 22 t y t xb r 3 B r 23 r 3 r 32 t z B r 33 (7) r ij represent the elements of the rotation matrix R and t = (t x, t y, y z ). Let the estimated homography be represented as follows, Ĥ = ĥ ĥ 2 ĥ 3 ĥ 4 ĥ 5 ĥ 6 ĥ 7 ĥ 8 ĥ 9 ĥ ĥ ĥ 2 ĥ 3 ĥ 4 ĥ 5 ĥ 6 (8) Given the estimated homography (8) and the homography parameterized in terms of the rigid motion parameters and the known camera parameters (7), the estimate of the rigid motion parameters are recovered as follows, ˆR = ĥ ĥ 2 ĥ 4 ĥ fy ĥ 5 ĥ 8 6 ĥ 3 ĥ 4 ĥ 6 ˆt = Bĥ3 B ĥ 7 Bĥ5 (9) (2) We term this approach the Normalized Disparity Space Direct Linear Transform (N). An alternative solution, is to fix h 9 = h = h 2 = and h = by leveraging the structure of the disparity space homography (7), this yields the following three constraints (two homogeneous plus a homogeneous constraint), Z i = Z i θ = c (2) x i y i d i... x i y i d i x i x 2i y i x 2i d i x 2i x 2i x i y 2i y i y 2i d i y 2i y 2i x i d 2i y i d 2i d i d 2i d 2i (22)

4 c i = d i (23) θ = ( h h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 3 h 4 h 5 h 6 ) (24) which is a vector consisting of the components of H. Here only four points correspondences (as opposed to five as in N) are required for the solution, given as, θ = Z c (25) Z = ( ) Z Z2 Zn ( ) and c = c,... c n. If more than four points matches are used, yielding an overdetermined system of equations, the inverse in (25) is replaced with the pseudoinverse. Given the solution to θ the estimate of the rigid motion parameters are recovered as follows, ˆR = ĥ ĥ 2 ĥ 4 ĥ fy ĥ 5 ĥ 8 6 ĥ 3 ĥ 4 ĥ 6 ˆt = Bĥ3 B ĥ 7 Bĥ5 (26) (27) We term this approach the Disparity Space Direct Linear Transform (). Both N and are followed by the step to orthogonalize the estimated rotation matrix as described in Section II-B. A disadvantage of the is that it does not allow for the inclusion of the Hartley normalization [2] as in the N solution. However, often in real applications, a necessary first step is to reject the outliers in the 3D correspondence set, which is usually achieved by a RANSAC process. When used as a front end to generate the initial motion hypothesis in the RANSAC process, the fact that requires one less correspondence point than N is appealing, since the smaller the number of minimum points required, the smaller amount of samples are required by the RANSAC process, which in turn translates into more efficient implementation. III. EXPERIMENTAL RESULTS Experiments with simulated data are carried out to compare the performance of proposed linear algorithms with the previous algorithms, in particular, the nonlinear algorithm in disparity space as in [23] and the well-known SVD based algorithm in Euclidean space as in [24]. Apparently the algorithm performance depends on many variables, such as noise level, underlying motion and even the scene structure. Here we simulate the scenario when a stereo sensor is mounted on a host car and detects imminent collision with other cars. The relative motion is critical in deciding whether the two cars will collide or not, and it can be estimated from the 3D point correspondence set from the stereo sensor. With this application in mind, a synthesized scene of points is used, which are randomly distributed inside a cuboid with roughly the size of a car. A random rigid motion is then generated, simulating a car moving on the ground plane. The 3D ground truth points are projected onto the stereo image planes, generating the ground truth points in disparity space. Then Gaussian noise (at the image level) with varying standard deviation is added to the points in disparity space, to obtain the noise corrupted correspondence set in disparity space, which is used as the input to the motion estimation algorithms in disparity space. By triangulation with the noise corrupted point in disparity space, we can obtain the noise corrupted 3D reconstruction in Euclidean space, which is used as the input to the SVD based algorithm for comparison. To obtain statistically meaningful comparison, at each noise level, we repeat the process for 5 times, and compare the absolute mean and standard deviation of the estimation error. The translation error is computed as the angle subtended by the ground truth translation vector and the estimated translation vector. The rotation error is computed as the difference in the ground truth rotation angle and the estimated rotation angle along the ground truth rotation axis. Figure shows the comparison of the N,, and with increasing noise levels, refers to the iterative least-squares algorithm, and is followed by a Levenberg-Marquardt nonlinear optimization. The underlying rigid motion is t = (.5.5) and with a rotation angle of 5 about the axis (), X, Z axis are parallel to the ground plane and Y axis vertical to the ground. Figure shows the N and algorithms have very similar performance under different noise levels, with N generating slightly better result in the absolute mean error in the estimate rotation angle. Both N and outperform the iterative least-squares method by a large margin. While the Levenberg-Marquardt optimization can improve the iterative least-squares result, its rotation angle estimation still has much larger standard deviation than N and algorithms. Figure 2 shows the comparison result under the same setting, except that the rotation angle is now increased to. As it can be seen from the figure, under large rotation, the Levenberg-Marquardt optimization cannot improve the iterative least-squares result, possibly due to the wrong starting point. Nonetheless N and still generate quite consistent motion estimates. IV. CONCLUSION AND DISCUSSION In this paper, we present a novel closed-form linear algorithm to solve for rigid motion in the disparity space. To our knowledge this is the first linear algorithm for rigid motion estimation in disparity space, which can be potentially very useful for real-time automotive applications, due to its high efficiency.

5 2 Mean Translation Error.4 STD of Translation Error N N Mean Rotation Error.25 STD of Rotation Error N N Fig.. Comparison of N and with D-motion based methods under different noise levels To apply any rigid motion estimation algorithm to real applications, at least one hurdle has to be overcome, the outliers in the correspondence set. RANSAC has been a popular method in rejecting the outliers. The efficient closedform linear algorithm then can be used in the initial hypothesis generation step to reject the outliers, which otherwise can be extremely expensive if nonlinear method is used. After the inliers are located, more expensive and optimal nonlinear algorithm can be applied to achieve better result. REFERENCES [] Z. Sun, G. Bebis, and R. Miller, On-road vehicle detection using optical sensors: a review, 24. [2] M. Bertozzi, A. Broggi, A. Fascioli, and S. Nichele, Stereo vision-based vehicle detection, 2. [3] M. Suwa, Y. Wu, and M. Kobayashi, A stereo-based vehicle detection method under windy conditions, 2. [4] N. Srinivasa, A vision-based vehicle detection and tracking method for forward collision warning, 22. [5] D. Gavrila, Pedestrian detection from a moving vehicle, in European Conference on Computer Vision, 2, pp [6] M. Bertozzi, A. Broggi, R. Chapuis, F. Chausse, A. Fascioli, and A. Tibaldi, Shape based pedestrian detection and localization, 23. [7] S. Nedevschi, R. Danescu, T. Marita, F. Oniga, C. Pocol, S. Sobol, T. Graf, and R. Schmidt, Driving environment perception using stereovision, in IEEE Intelligent Vehicles Symposium, 25. [8] L. Matthies and S. Shafer, Error modeling in stereo navigation, IEEE Trans. on Rob. and Auto., vol. 3, pp , 987. [9] C. Olson, L. Matthies, M. Schoppers, and M. Maimone, Robust stereo ego-motion for long distance navigation, in CVPR, 999. [] P. Chang, T. Camus, and R. Mandelbaum, Stereo based vision system for automotive imminent collision detection, in IEEE Intelligent Vehicles Symposium, 24. [] D. Eggert, A. Lorusso, and R. Fisher, Estimating 3d rigid body transformations: a comparison of four major algorithms, Mach. Vis. and Appl., vol. 9, pp , 997. [2] B. Matei and P. Meer, Optimal rigid motion estimation and performance evaluation with bootstrap, in CVPR, 999. [3] D. Demirdjian and T. Darrell, Using multiple-hypothesis disparity maps and image velocity for 3-d motion estimation. IJCV, vol. 47, no. -3, pp , 22. [4] M. Agrawal, K. Konolige, and L. Iocchi, Real-time detection of independent motion using stereo, in Motion, 25. [5] M. Harville, A. Rahimi, T.Darrell, G. Gordon, and J. Woodfill, 3d pose tracking with linear depth and brightness constraints, in ICCV, 999. [6] R. Mandelbaum, G. Salgian, and H. Sawhney, Correlation-based estimation of ego-motion and structure from motion and stereo, in ICCV, 999. [7] D. Demirdjian and T. Darrell, Motion estimation from disparity images, in IEEE International Conference on Computer Vision, 2, pp. I: [8] K. Levenberg, A method for the solution of certain problems in least squares, in Quarterly of Applied Mathematics, vol. 2, 944. [9] D. Marquardt, An algorithm for least-squares estimation of nonlinear parameters, SIAM Journal on Applied Mathematics, vol., pp , 963. [2] R. Hartley, In defense of the eight-point algorithm, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 9, no. 6, pp , June 997.

6 3 Mean Translation Error 35 STD of Translation Error N N Mean Rotation Error 8 STD of Rotation Error N N Fig. 2. Comparison of N and with D-motion based methods under different noise levels undergoing large rotation [2] B. Matei and P. Meer, A general method for errors-in-variables problems in computer vision, in CVPR, 2, pp. II: [22] M. Mühlich and R. Mester, The role of total least squares in motion analysis, in European Conference on Computer Vision, 998. [23] D. Demirdjian and T. Darrell, Using multiple-hypothesis disparity maps and image velocity for 3-d motion estimation, Internation Journal of Computer Vision, vol. 47, no. -3, pp , April 22. [24] K. Arun, T. Huang, and S. Blostein, Least-squares fitting of two 3- D point sets, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 9, no. 5, pp , September 987.