We present a novel yet simple 3D stereo vision tracking algorithm which computes the position and orientation of an object from the location of markers attached to the object. The novelty of this algorithm is that it does not assume that the markers are tracked synchronously. This provides a higher robustness to the noise in the data, missing points and outliers. The principle of the algorithm is to perform a simple gradient descent on the rigid body transformation describing the object position and orientation. This is proved to converge to the correct solution and is illustrated in a simple experimental setup involving two USB cameras

Billard, A.

Hersch, M.

Reichert, T.

We present a novel yet simple 3D stereo vision tracking algorithm which computes the position and orientation of an object from the location of markers attached to the object. The novelty of this algorithm is that it does not assume that the markers are tracked synchronously. This provides a higher robustness to the noise in the data, missing points and outliers. The principle of the algorithm is to perform a simple gradient descent on the rigid body transformation describing the object position and orientation. This is proved to converge to the correct solution and is illustrated in a simple experimental setup involving two USB cameras.LAS

Infoscience - École polytechnique fédérale de Lausanne

ITERATIVE RIGID BODY TRANSFORMATION ESTIMATION FORVISUAL 3-D OBJECT TRACKINGMicha Hersch, Thomas Reichert, Aude BillardLASA Laboratory, EPFL, 1015 Lausanne, Switzerlandmicha.hersch@ep.ch, aude.billard@ep.chKeywords: stereo vision tracking, rigid body transformation estimationAbstract: We present a novel yet simple 3D stereo vision tracking algorithm which computes the position andorientation of an object from the location of markers attached to the object. The novelty of this algorithmis that it does not assume that the markers are tracked synchronously. This provides a higher robustnessto the noise in the data, missing points and outliers. The principle of the algorithm is to perform a simplegradient descent on the rigid body transformation describing the object position and orientation. This is provedto converge to the correct solution and is illustrated in a simple experimental setup involving two USB cameras.1 INTRODUCTIONEstimating the 3-D rigid body transformationaligning two noisy sets of identifiable points is con-sidered a solved problem in computer vision. Indeed,various closed form solutions have been suggestedin the last two decades (Arun et al., 1987; Horn,1987; Walker et al., 1991), and those solutions havebeen widely used and compared (Eggert et al., 1997).However, in spite of those existing solutions, we ad-dress once again this problem and suggest an iterativesolution to the rigid body estimation problem. Ourbelief is that in many applications, an iterative solu-tion is preferable to a closed-form solution, especiallyif the rigid body transformation changes in time, forexample when tracking a moving object. The majorreasons for this is that an iterative solution would bemore robust to noise in the data and that and wouldnot assume synchronicity of the set of points.2 SETTING AND NOTATIONSWe consider a rigid body transformation T trans-forming a set of n vectors {xi} into another set of nvectors {yi}. This transformation is described by arotation R around an axis passing through the originand a translation V by a vector v:yi = T(xi) = R(xi)+v. (1)When considering a 3-D tracking application, therigid body transformation T can be used to describethe position and orientation of the tracked object, rel-atively to a reference position and orientation. Thereference positions of the n markers on the objectsmake the set of {xi}. The positions of those mark-ers when tracked by a stereo vision system constitutethe set of {yi}. It is assumed that the markers can bedistinguished one from another, for example by usingdifferent colors. If the object is moving, the evolutionof T yields the trajectory of the object.3 ROTATIONSIn this paper, we use the spinor representation ofrotations which is briefly recalled here, adopting theapproach described in (Hestenes, 1999). This repre-sentation is very similar to the quaternion representa-tion. The spinor q¯ representing the rotation R is givenby a scalar α and imaginary vector bi. The directionof b yields the rotation axis (passing through the ori-gin) and its norm is equal to sin(θ/2), where θ is therotation angle. The scalar α is given by cos(θ/2).Therotation of a vector x by a spinor q¯ is given by thefollowing equation.Rb(x)= (1−2bT b)x+2√(1−bT b)b×x+2(bT x)b,(2)where Rb denotes the rotation represented by b.4 ITERATIVE ESTIMATION OF ARIGID BODYTRANSFORMATIONWe now present to the algorithm for iteratively es-timating a rigid body transformation given a set of npoints {xi} and its noisy transform {yi}. The princi-ple of the algorithm is quite trivial. Starting from aninitial guess for the parameters b and v of the trans-formation, it consists simply on a gradient descent onthe squared distance between the measurement yi andthe transformed point Tb,v(xi)∆b =−ε ∂∂b12∣∣∣∣∣∣yi−Tb,v(xi)∣∣∣∣∣∣2(3)∆v =−ε ∂∂v12∣∣∣∣∣∣yi−Tb,v(xi)∣∣∣∣∣∣2, (4)where ε is the learning rate. One assumes that i takesvalues from 1 to n in a uniformly distributed manner.So at each time step, an index i is selected among theavailable points and b and v are update according to(3) and (4).The actual development of those two equations yields:∆b = 2ε(yi−Tb,v(xi))T (−2xibT −1√(1−bT b)(b×xi)bT +1√(1−bT b)xi ↑+bxTi +(bT xi)I)(5)∆v = ε(yi−Tb,v(xi)), (6)where I is the 3×3 identity matrix and the unary op-erator↑ is defined asx↑ .=∂∂b (b×x) =0 x(3) −x(2)−x(3) 0 x(1)x(2) −x(1) 0 , (7)with x = [x(1) x(2) x(3)]T .This concludes the description of the algorithm.For efficiency purposes, it is preferable to choose ref-erence positions so that the xi are centered on the ori-gin. This allows to reduce the influence of b on thecomputation of v.5 CONVERGENCEIn this section, we prove that if there exists a rigidbody transformation matching the two sets of points{xi} to {yi}, then the iterative algorithm describedabove will converge to it.Let T∗ be the true transformation mapping a finiteset of points {xi}= V into their corresponding image.If V contains at least three unaligned points, thereis only one such transformation. Let T 6= T∗ be thecurrent estimate of this transformation.We then define the following function E(T)E(T) =n∑i=1Ei(T), with Ei(T) =12‖Txi−T∗xi‖2 (8)Here and in the rest of this paper, the parenthesesaround xi are omitted to lighten the notation. Wealso define the vector p = [bT vT ]T to be the vectorparameterizing the transformation.We first show that the algorithm always convergesto a solution. If ε tends to zero and t is the time, thenε−1∆b and ε−1∆v tend respectively to ∂∂t b and∂∂t v .So the gradient descent of the algorithm means that∂∂t p =−∂∂p Ei(T). We thus have∂∂t E =∂∂tn∑i=1Ei =n∑i=1∂∂t Ei =n∑i=1∂∂pEi∂∂t p =n∑i=1∂∂pEi(−∂∂pEi) =n∑i=1−(∂∂pEi)2 ≤ 0The function E(T), being positive, the algorithmalways converges to a solution. It remains to beshown that this solution is correct.In order to show that the algorithm converges tothe right solution T∗, we show that for any T, T∗, V ,satisfying the conditions mentioned above, there is atransformation T† belonging to a neighborhood of Tsuch thatE(T†) < E(T) (9)This amounts to saying that there is no local minimumfor E(T). We assume, without loss of generality, thatthe xi are centered. Let us consider the transformationT† defined by translation vector v† and rotation R†v† = v+ ε(v∗−v) (10)R† = εR+ ◦R with ε > 0. (11)In the above expression εR+ is an infinitesimal rota-tion of unit rotation axis given byb+ = z∑iRxi×R∗xi (12)where z = ‖∑i Rxi×R∗xi‖−1. This means that R† isin the neighborhood of R. If ε is small enough, wehave, see (Altmann, 1986),R†x = Rx+ ε(b+×Rx). (13)Thus the variation in E when moving from T to T† isgiven by∆E = E(T†)−E(T) (14)= ∑i‖T†xi−T∗xi‖2−∑i‖Txi−T∗xi‖2= ∑i‖T†xi‖2−‖Txi‖2−2(T∗xi)T (T†xi−Txi)= ∑i‖R†xi +v†‖2−‖Rxi +v‖2−2(R∗xi +v∗)T(R†xi +v†−Rxi−v)= ∑i‖Rx+v+ ε(v∗−v+b+×Rxi)‖2−‖Rxi +v‖2−2(R∗xi +v∗)T(Rxi + εb+×Rxi +v+ ε(v∗−v)−Rxi−v)= ∑i2ε((v∗−v+b+×Rxi)T (Rxi +v)−(R∗xi +v∗)T (b+×Rxi +v∗−v))+O(ε2)(15)If ε is small enough, we can discard terms in O(ε2).∆E' ∑i2ε(v∗−v+b+×Rxi)T (Rxi−R∗xi +v−v∗)= 2ε(−n‖v∗−v‖2 +(v∗−v)T (∑iRxi−∑iR∗xi)+∑i(b+×Rxi)T (Rxi−R∗xi))= 2ε(−n‖v∗−v‖2 +∑i(b+×Rxi)T (Rxi−R∗xi))= −2ε(n‖v∗−v‖2 +∑i(b+×Rxi)T R∗xi)(16)We now show that the sum in (16) is also positive.Using the matrix representation of rotation,∑i(b+×Rxi)T R∗)= ∑i((z∑jRx j ×R∗x j)×Rxi)T R∗xi)= z∑i, j((Rx j ×R∗x j)×Rxi)T R∗xi= z∑i, j(R∗x j(Rx j)T Rxi−Rx j(R∗x j)T Rxi)T R∗xi= z∑i, jxTi RT Rx jxTj (R∗)T R∗xi−xTi RT R∗x jxTj RT R∗xi= zn∑ixTi Cxi−xTi RT R∗CRT R∗xi > 0 ∀R 6= R∗.(17)In the last equation C is the covariance matrix of thexi. The last inequality is justified by the fact that therotation vectortranslation vectoriterationsxzyxyz−2−1 0 1 2 3 4 5 6 7 8−0.2−0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0  50  100  150  200  0  50  100  150  200Figure 1: Convergence of the algorithm. The three vectorcomponents are indicated by x, y and z. The dotted horizon-tal lines are the true parameter values of rigid body trans-formation and the solid lines show the evolution of the esti-mated values using the learning algorithm.rotation matrix RT R∗ breaks the alignment betweenthe principal component of C and the direction ofmaximum variance in V . Putting (17) and (16) to-gether shows that E decreases when moving from Tto T†. There is thus no local minima in E, so E isa Lyapunov function of the system, which proves theconvergence.6 EXPERIMENTSThe first experiment aims at illustrating theconvergence properties of the algorithm describedabove and is performed in simulation. A rotationvector b∗ and a translation vector v∗ were randomlygenerated. The estimated rigid body transformationwas initialized to the identity b = v = 0 and thealgorithm was run on randomly generated points xi.The results can be seen in Figure 1. One sees thatboth b and v converge to b∗ and v∗ respectively, asis expected from the convergence properties studiedabove.The next experiment involves a tracking task ina real stereo vision setting made of two low qualityUSB cameras mounted on a fixed support. Threecolor patches were taped on the object to be tracked.A software, based on the OpenCV library can trackcolor blobs and locate them in three dimensions.The object was moved by hand, so the only in-formation about the position of the object is givenby the stereo vision system. So the real positionof the object is unknown, i.e., there is no ground truth.Using the data recorded from the stereo vi-sion system, the position and orientation of the0 500 1000 1500 2000 2500 300050525456586062Rotation angle (in degree)0 500 1000 1500 2000 2500 3000iterative solution closed-form solutionframes framesFigure 2: The behaviors of the tested algorithms in case ofnoise in the data. The iterative algorithm (left graphs) is lessnoisy than the closed-form algorithm. The object is static,and the same data was used on both algorithms.end-effector were computed using two differentalgorithms, the iterative one described in this paper(5) and (6) and the closed-form solution describedin (Horn, 1987). This algorithm finds the rigid bodytransformation by optimizing a least square criterionsimilar to E(T) defined in (8).In both cases, the data was taken as it is, withoutany preprocessing. The iterative algorithm wasinitialized using the closed-form algorithm on theinitial patch positions. In the absence of groundtruth, the precision of the tracking algorithm is notinvestigated. Rather, we compare the behaviors of theiterative and closed-form algorithmsThe first experiment was made with a static object.Using the same marker position data coming from thestereo vision software, we ran both algorithms to es-timate the position and the orientation of the object.The results can be seen in Figure 2. One sees that theiterative solution is much less sensitive to noise in thedata. This is because the closed-form solution has nomemory, whereas the iterative solution can only up-date its current estimate up to a certain amount, whichproduces a smoothing effect.The second experiment was made with a movingobject. In this experiment, the effect of missing pointsis investigated. Two different scenarios were tested.In the first scenario (periodic occlusion), a randomlyselected point was removed in each frame. In the sec-ond scenario (lasting occlusion) a given point was re-moved from the data for 10 consecutive frames. Theresults can be seen in Figure 3. One sees that for bothscenarios, the closed-form algorithm (dotted lines)cannot deal with the missing points as it requires atleast three concomitant points. To the contrary, theiterative algorithm (dashed-dotted line) can deal withthe missing points as it has no such requirement. Itcan follow pretty well the position given by the base-line (solid line). This baseline was obtained by usingthe closed-form algorithm and smoothing the result.0 10 20 30 40 50 60 700102030405060708090framesrotation angle (in degree)  20 25 30 35 40 45 50 55 6005101520253035404550frames  baseline iterative solution closed-form solutionperiodic occlusion lasting occlusionFigure 3: The behaviors of the tested algorithms in caseof occlusions. The iterative algorithm can deals well withpoints periodically missing and points missing for a numberof consecutive frames (lasting occulsions).7 DISCUSSIONThe results presented above show that an iterativesolution to a rigid body transformation can be advan-tageous in a tracking application. The main advan-tages come from the fact that the iterative solutiondoes not make the assumption that it has concomitantpoints. Moreover, it ensures a continuity in the es-timates, which is not guaranteed by the memorylessclosed-form solution. When using the iterative solu-tion suggested here, the learning rate must be care-fully chosen to be big enough to avoid loosing trackof the object, while remaining small enough to ensurea smooth estimate of the transformation.Although it was not investigated in this paper, we be-lieve that the suggested algorithm could be useful inother applications, especially in iterative algorithmslike ICP. This algorithm could also most probably beeasily extended to include uniform scaling of rigidbody transformations.REFERENCESAltmann, S. (1986). Rotations, Quaternions and DoubleGroups, chapter 4, page 80. Oxford University Press.Arun, K., Huang, T., and Blostein, S. (1987). Least-squaresfitting of two 3-d point sets. IEEE Transactions onPattern Analysis and Machine Intelligence.Eggert, D., Lorusso, A., and Fisher, R. (1997). Estimating3-d rigid body transformation: a comparison of fourmajor algorithms. Machine Vision and Applications.Hestenes, D. (1999). New Foudations for Classical Me-chanics, pages 277–305. Fundamental Theories ofPhysics. Kluwer Academic Publishers, 2 edition.Horn, B. (1987). Closed-form solution of absolute orien-tation using unit quaternions. Journal of the OpticalSociety of America A, 4(4):629–641.Walker, M., Shao, L., and Volz, R. (1991). Estimating 3-d location parameters using dual number quaternions.CVGIP: Image Understanding.

Iterative rigid body transformation estimation for visual 3-D object tracking

http://infoscience.epfl.ch/record/114052/files/hersch08iterative.pdf

Iterative rigid body transformation estimation for visual 3-D object tracking

Abstract

Similar works

Full text

Available Versions

Infoscience - École polytechnique fédérale de Lausanne