Optical Navigation by the Method of Differences

1. lntroduction

Optical navigation rcfers to thc dctermination of the position and orientation of a camera analysis of the picture takcn by thc camcra. The objcctive of such analysis is to detcrminc somc or all of the six parameters ( thrce of posi­ tion and thrce of orientation) that dctcrmine thc position of that camera relative to somc lixed frame of rcference. In our method and in many others the lixed frame of reference is that of a second camera, so that the problem is that of image comparison.

Optical navigation has a number of applications in robotic tasks that require a knowlcdge of the position and orientation of the robot. This is because mechanical imper­ fections and environmental unccrtainty make it impossible to know cxactly how a robot will move in response to the commands sent to it and exactly what it will encounter in its surroundings. Such applications includc navigation of autonomaus roving vehicles and navigation of a robot arm relative to the object on which it is performing its task.

Tlti:.: n·:.:··:tr<'il W:t:.: :.: p<>ll :.:<>n·d hy 1111· u .. r( 'JI ,.:(' A<lvane<·d llc·"·arclt l'rnj<·<"l:.: 1\ '~' 'li<'.V (DO ll ). AIU'A Ord<'r No. :;;,u7. 111o11itor<'d hy tlll' Air 1-'orn· Avioni <:.: Litlu>r<l.lory lilul<·r Coulrad F:\:Hii&·8 · 1-K- 1 f>~ll. Tlw \'i<·w:.: <111<1 r<IIII'IH:.:ion:.: contai11<'d in thi:.: clnnllnc·nt. an· !Iu·~· · of illc' illll lu·r' and :.:honld nol h<' intl'l"pn·t.<·d a~ r< ' Jll'c·~··nt.ing IIJ<· olli­ cial polici<·:.:. ··il h<·r <'Xpn·,.:,.:··cl or in1pli<·<l. of tlH' Dd<'IIS<' Advafln·d ll< ·:.:~·;cr<'h l'roj<'<'l' t\g<'II<'Y or t Iu· I JS CoV<'rJilll<'llt.

The approaches to matehing for optical naviga­ tion may be divided into three categories: sparse two­ dimensional matching, continnous two-dimensional match­ ing, and three-dimensional matching. The sparsc two­ dimensional approach starts with a discrctc set of matehing points in the two images, and from them deduces the cam­ era motion. Thc question of how many points are nec­ essary to uniquely solve for the camera parameters has becn addressed by Tsai & Huang (1981) . With more points, t.he problem is overspecified and a Ieast-squares ap­ proach is required (Gennery, 1980) . The continuous two­ dimensional matehing approach starts with a whole image lield of matches ( the "optical flow lield"); Bruss & Horn (1983) have shown how how to determine the camera mo­ tion from the optical flow field, a~ain using a Ieast-squares formulation. Obtaining the optical flow ficld has been in­ vestigatcd by, for example, Horn & Schunck (1981) and Cornelius & Kauade (1983), among others. In thc three­ dimensional matehing approach, corresponding points in three uimensions ( obtained e.g. by stereo) are used to dc­ termine the camera motion; this technique was used by Moravec (1980) to navigate a rover.

These approachcs all split thc process into two stcps: linding the matches and using thosc matches to solve for the camera parameters. In this paper we show how t.o combine thc two stcps into one, by applying a gcneralized image matehing tcchnique that wc tcrm thc melhod of dif­ ferences. The method of diffcrences dircctly computcs the six camcra parametcrs, or any desircd s11bset of thcm, much as standard matehing techniljUes computc two paramcters (the x and y displacements). That is, thc camcra par<lm­ etcrs are explicitly includcd in thc matehing process. The mcthod takes advantage of the fact that, in many applica­ tions the approximate position and orientation of the cam­ era are known. Starting from that estimate we compute a better estimate by using thc imagc intensity gradient as a guide. By using an iterative scheme our cstimates converge to the correct value. The result is a tcchniqne that is fast and free of search.

In the remainder of the paper, we lirst describe the method of differcnces in a one-dimensional case, which serves to illustrate many of the issues. Then we show how the same technique can be used for multi-paramcter esti­ mation. Finally, we present some experimental results and draw some conclusions.

2. The technique

Parameter estimation by thc method of differences. The une-dimensional case illustrates the nature of the tech­ nique. Given two une-dimensional images I1 (x) and I2(x) related by a translation, so that I1(x) = l2(x + h), we wish to estimate the translation h. We do this by finding that h that minimizes the totalsquared error,

Sincc we want a local , non-searehing algoritbm, wc approxi­ mate J2 (x +h) using /2 (x) on tbe basis oflocal information, namely the derivative; this yields the approximation

where Dx denotes partial differentiation with respect to x. Tbis equation is quadratic in h, so we can differentiate with respect to Jl, set eqnal to zero, and solve thc resulting linear cquation for h, obtaining

We call this the method of differences because it is based on comparing the differeuce bctwecn thc images, I1(x) ­ I2 (:r), with the derivative Dxl2(x) (which will in fact be implemcntcd as a diffcrencc), to obtain an cstimate for the paramet.er h.

We have shown elsewhere (Lucas, 1985) how this method is easily cxtended to multi-parameter estimation, as required for navigation. Briefiy, the scalar disparity h is replaced by a vector of camcra paramcters; the deriva­ tives become gradients, and the division becomes a matrix inversion. The stability of the matrix inversion is investi­ gated in the work cited above, with the conclusion that the matehing points should be well-distributed in tluee-space to guarantee good numerical accuracy.

Iteration and smoothing. Two modifications are required to make the method work. First , because the method yields only an approximation h to the disparity h, we must use an iterative scheme to obtain an accurate result . The idea is to calculate an estimated disparity, move h by that amount , and calculate again.

Second, to improve tbe accuracy and range of valid­ ity of the linear estimate used in (2), we must smooth the image. This can be thought of as smoothing out purely lo­ cal bumps and wrinkles in the image intensity profile that would make a linear estimate accuratc only over a small range. This can be made more precisc by a Fourier anal­ ysis of (3); this shows that removing the high frequency components of the image by smoothing does indeed extend the range of convergence, in rough proportion to the size of the smoothing window (Lucas, 1985). This is because con­ vcrgence to the correct value with an image consisting of a pure sinc wave is possible only for disparities up to onc-half the wavelength of the sine wave; for !arger disparities, the algorithm will converge to the wrong value.

Since smoothing the image also reduces the accuracy of the method, it is necessary to use an iterative approach in which each successive step uses a less-smoothed image, in a sort of coarse-fine approach. This allows the algorithm to tolerate a !arge disparity yet yicld an accurate answer.

3. Experimental results

Onr experimental data consisted of three views of the same sccne taken by a camera mounted on the Stanford cart (Moravec, 1980); they arc shown in Fignre 1. The camera was monnted on a slider, -so we had accnrate knowledge of the relat ive positions of the cameras. The three views were pictures t.aken by the camera at the lcft , middle, and right slider positions, with 26 cm separating cach position. The left picture was used as the refercnce image, and a num­ ber of points p were sclected from thfs i.mage as reference points. These points correspond to the points x that the sum in (1) runs over. Then the right picture was used as the second image of a stereo pair to obtain ( essentially by hand) the distances z(p) of the reference points p . The metbod of differences was then used to determine position of thc middle camera. Since the cxact position of the mid­ dle camera was known, we could assess the accuracy of the metbod. Moreover, we could determine the range of conver­ gence by varying the initial estimate of the middle camera's position around the correct value.

Convergence range. The convergence range for both the one-dimensional case and the multi-dimensional case was investigated using these pictures. As predicted, the convergence ranged was found to increase in rough propor­ tion to the size of the smoothing window. The range for x and y motions was roughly ± 1 meter, and somewhat more in the z direction. The range for pan and tilt was approxi­ mately ± 10 degrees, and about ± 30 degrees for roll. Except for roll, these parameters are limited more by the angle of view of the camera than by the technique. For example, no matehing technique could work if there angle of view is so small and the motion between the cameras so !arge that there is no overlap between the pictures. When this point is reached, t.he smoothing window rcquired would be so !arge that each picture would be smoothed to a uniform gray. Nevertheless, these results are useful in that they ver­ ify that a useful range of convergence is obtainable using the method.

What is the relationship between these convergence ranges and the convergence ranges in the multi-parameter case? This is shown in Figure 2. We sec that if we solve for two parameters (pan and tilt, top graph), the range is smaller than the range that would be expected on the hasis of the one-paramcter results for pan anrl tilt alone; and if wc solve for all six parametcrs (bottom graph), it is smaller still. Nevcrthcless, the range is sti ll quite adt!quate for thc continuous feedback mode. Whethcr it is adcqnate for thc stop-and-go mode, which involves a !arger motion at each step, depcuds on the accuracy of thc arm and on the accuracy of other navigational aids that can providc the initial estimates.

Accuracy. To assess the accuracy under a variety of conditions, we select reference points nsing a variety of methods, including by band and by computer, resulting in several sets of data points of various sizes. Then we dou­ bled the number of sets of reference points by either apply­ ing or not applying a pruning process to the scts we had. This pruning process, which is described elsewhere (Lucas, 1985), was based on the method of differences a.nd served to improve the accuracy of the stereo ma.tches. It also elim­ inated some points as being unfit for use by the method, for example beca.use they were in a region of small gradient. The results are shown in Figure 3. Severa.l general trends are observable. First, using more points produces more ac­ cura.te results. Second, the pruning process can to improve the results, as evidenced by the left endpoints of the lines in the ligure being lower than the right endpoints. These two factors are of course in conflict, and the improvement due to the pruning process is apparent only provided the number of points is not reduced too much. Finally, the ac­ curacy does not seem to be affected much by the number of parameters solvcd for.

lmplementation. The implementation may be divided into two parts: smoothing a.nd camera parameter estimation. The smoothing must be done over a relatively !arge window, up to 65 x 65 in our experiments. It is the most time-consuming, pa.rt even though wc implemented it as uniform smoothing over a reetangular region, which by a well-known algorithm takes a constant number of oper­ ations (two additions and two subtract.ions) per pixel, re­ gardless of the size of the smoothing window. However, it is fairly weil understood how to build specia.l-purpose hard­ ware for doing smoothing quickly, essentially in real time.

The parameter estimation step is more interesting. Our implementation, in which no attention was paid to efficiency, requires approximately 3 to 1 ms per reference point pcr iteration on a VAX 11 /780. ln the continuous fcedback modc, only one itcration pcr time step would be uscd sincc only an approximate answer is needed. Thus 50 refcrence points (thc largest numbcr nsed in the exper­ iments rcportcd above) would requirc less than 200ms per time step. This flgure could probably be improvcd several­ fold by more careful coding and taking account of the fact that somc of the entries in the matrices to be inverterl are known a priori to be zero. This information, together with the fact that thc algorithm has a rcgular structure free of decision points that could easily be implementecl in special­ purpose hardware, suggcsts that it is feasible for real-time control of a robot.

4. Conclusions

We have demonstrated that the method of differcnces provides a useful techniquc for optical naviga.tion. We have shown that thc algorithm ca.n succcssfu lly dctermine all six camera. parametcrs. It convcrges to thc correct position given an cstimate within something on the order of a meter (less if more para.metcrs are solverl for), and converges to a result accurate to a centimeter or so (regardless of the number of parameters solved for). Moreover, it ca.n do so using 50 or less reference points. Bccause of the regular structure of the algorithm, the prospects of carrying out the calculations in real time with special-purpose hardware seem good.

5. References

1. R. Bruss and B. K. P. Horn, 1983. Passive navigation. Computer Vision, Graphics, and Image Processing, 21, 320.
2. Cafforio and F. Rocca, 1979. Tracking moving objects in television images. Signal Processing, 1, 133-140.
3. H. Cornelius and T. Kanade, 1983. Adapting optical­ flow to mcasurc object motion in reflectance and x-ray im­ age sequences. Proc. ACM SIGGRAPH/SIGART Work­ shop on Motion: Representation and Perception, Toronto, 50-58.
4. B. Gennery, 1980. Modeling thc cnvironmcnt of an ex­ ploring vehicle by mcans of stereo vision. PhD Thesis, De­ partment of Computer Scicnce, Stanforrl University.
5. K. P. Horn and B. G. Schunck, 1981. Determining opti­ ca.l flow. Artificial Intelligence, 17, 185-202.
6. 0. Limb ancl J . A. Murphy, 1975. Estimating the velocity of moviug images in telcvision signals. Computer Graphics and Image Processing, 4, 311-327.
7. D. Lucas, 1985. Generalized ima,ge matehing by the method of diJierences: algorithms and applications. PhD Thesis (in preparation), Computer Science Department, Carnegie-Mellon University.
8. D. Lucas and T. Kanade, 1981. .An iterative image regis­ tra.tion technique with an applica.tion to stereo vision. Proc. Scvcnth International Joint Conference on Artificial Intel­ ligence, Vancouver.
9. P. Mora.vec, 1980. Obstacle avoidance and navigation in the real world by a seeing robot rover. Tech. Rept. CMU­ RI-TR-3, Robotics Institute, Carnegie-Mellon University.
10. Y. Tsai and T. S. Huang, 1981. Uniqueness a.nd estima­ tion of 3-D motion parameters of rigid objects with curved surfaces. Proc. IEEE Conference on Pattern Recognition and Image Processing.