CENG 499B Final Report Image based path mapping system for ...elec499/2004b/group01/docs/final.pdf · CENG 499B Final Report Image based path mapping system for autonomous robotics

CENG 499B

Final Report

Image based path mapping system for autonomous

robotics applications

Jordan Reynolds#0125068

Computer [email protected]

March 31, 2004

Contents

1 Introduction 11.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Problem Description . . . . . . . . . . . . . . . . . . . . 11.3 Overall system architecture . . . . . . . . . . . . . . . . 2

1.3.1 Implementation language . . . . . . . . . . . . . 21.3.2 System structure . . . . . . . . . . . . . . . . . . 21.3.3 Resulting toolbox . . . . . . . . . . . . . . . . . 5

2 System operational flow 62.1 Image capture . . . . . . . . . . . . . . . . . . . . . . . 62.2 Image filtering . . . . . . . . . . . . . . . . . . . . . . . 62.3 Image decomposition . . . . . . . . . . . . . . . . . . . . 82.4 Edge detection . . . . . . . . . . . . . . . . . . . . . . . 82.5 Line vectorization . . . . . . . . . . . . . . . . . . . . . 82.6 Corner feature detection . . . . . . . . . . . . . . . . . . 82.7 Feature correlation . . . . . . . . . . . . . . . . . . . . . 92.8 Point locating . . . . . . . . . . . . . . . . . . . . . . . 92.9 Map generation . . . . . . . . . . . . . . . . . . . . . . 9

3 Project Results 103.1 Environment results . . . . . . . . . . . . . . . . . . . . 10

3.1.1 Constrained environment . . . . . . . . . . . . . 103.1.2 Partially constrained environment . . . . . . . . . 133.1.3 Unconstrained environment . . . . . . . . . . . . 18

3.2 Computational complexity . . . . . . . . . . . . . . . . . 213.3 System successes . . . . . . . . . . . . . . . . . . . . . . 213.4 System shortcomings . . . . . . . . . . . . . . . . . . . . 223.5 Next steps . . . . . . . . . . . . . . . . . . . . . . . . . 22

4 System theory 244.1 Object Detection . . . . . . . . . . . . . . . . . . . . . . 24

4.1.1 Capture image . . . . . . . . . . . . . . . . . . . 24

i

CONTENTS

4.1.2 Filter image . . . . . . . . . . . . . . . . . . . . 244.1.3 Split image . . . . . . . . . . . . . . . . . . . . . 254.1.4 Edge detect . . . . . . . . . . . . . . . . . . . . 264.1.5 Find corners . . . . . . . . . . . . . . . . . . . . 264.1.6 Merge results . . . . . . . . . . . . . . . . . . . . 274.1.7 Matching corners . . . . . . . . . . . . . . . . . . 274.1.8 Calculating position of point in space . . . . . . . 284.1.9 Map generation . . . . . . . . . . . . . . . . . . 29

4.2 Local intersection of lines . . . . . . . . . . . . . . . . . 294.2.1 Identifying lines within an image . . . . . . . . . . 304.2.2 Finding intersections of lines . . . . . . . . . . . . 334.2.3 Removing unwanted intersections . . . . . . . . . 35

A Appendix 37A.1 Matlab code . . . . . . . . . . . . . . . . . . . . . . . . 37

A.1.1 test.m . . . . . . . . . . . . . . . . . . . . . . . 37A.1.2 remove row.m . . . . . . . . . . . . . . . . . . . 38A.1.3 camera open.m . . . . . . . . . . . . . . . . . . 39A.1.4 im cap.m . . . . . . . . . . . . . . . . . . . . . . 40A.1.5 camera close.m . . . . . . . . . . . . . . . . . . 41A.1.6 img filter.m . . . . . . . . . . . . . . . . . . . . 42A.1.7 img decompose.m . . . . . . . . . . . . . . . . . 43A.1.8 edge detect.m . . . . . . . . . . . . . . . . . . . 44A.1.9 identify corners.m . . . . . . . . . . . . . . . . . 45A.1.10 find lines.m . . . . . . . . . . . . . . . . . . . . . 46A.1.11 find corners.m . . . . . . . . . . . . . . . . . . . 47A.1.12 find lines and find corners helper functions . . . . 48A.1.13 find position.m . . . . . . . . . . . . . . . . . . . 58A.1.14 range objects.m . . . . . . . . . . . . . . . . . . 59A.1.15 create map.m . . . . . . . . . . . . . . . . . . . 60

A.2 Progress report 1 . . . . . . . . . . . . . . . . . . . . . . 61A.2.1 Summary . . . . . . . . . . . . . . . . . . . . . . 61A.2.2 Problem Description . . . . . . . . . . . . . . . . 61A.2.3 Proposed solution . . . . . . . . . . . . . . . . . 61A.2.4 Proposed implementation . . . . . . . . . . . . . 63A.2.5 Proposed Timeline . . . . . . . . . . . . . . . . . 65

A.3 Progress Report 2 . . . . . . . . . . . . . . . . . . . . . 67A.3.1 Summary . . . . . . . . . . . . . . . . . . . . . . 67A.3.2 Problem Description . . . . . . . . . . . . . . . . 67A.3.3 Overall system architecture . . . . . . . . . . . . 68A.3.4 Object Detection . . . . . . . . . . . . . . . . . . 71A.3.5 Uncompleted work . . . . . . . . . . . . . . . . . 74

ii

CONTENTS

A.3.6 Local intersection of lines . . . . . . . . . . . . . 74A.3.7 Next steps . . . . . . . . . . . . . . . . . . . . . 81A.3.8 Problems encountered . . . . . . . . . . . . . . . 81A.3.9 Project outlook . . . . . . . . . . . . . . . . . . 81

iii

List of Figures

1.1 Overall system architecture 1 of 2. . . . . . . . . . . . . 31.2 Overall system architecture 2 of 2. . . . . . . . . . . . . 41.3 Robot vision toolbox. . . . . . . . . . . . . . . . . . . . 5

2.1 Progression through algorithm (a) initial image, (b) singlecolor layer,(c) edge detect, (d) line vectorization, (e) cornerfeatures discovered, (f) correlation between corners . . . . 7

3.1 left (a) and right (b) images from fully constrained envi-ronment . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2 Match between points within the constrained image pair. . 123.3 Resulting probability map from fully constrained object. . 133.4 left (a) and right (b) images from partially constrained en-

vironment . . . . . . . . . . . . . . . . . . . . . . . . . 143.5 Partially constrained test environment overhead shot. . . . 143.6 Feature matches within the partially constrained test envi-

ronment. . . . . . . . . . . . . . . . . . . . . . . . . . . 163.7 Resulting probability map of partially constrained test en-

vironment. . . . . . . . . . . . . . . . . . . . . . . . . . 173.8 Overhead shot of unconstrained test environment. . . . . 193.9 left (a) and right (b) images from unconstrained environment 193.10 Probability result of the unconstrained environment. . . . 20

4.1 Stereo cameras mounted on test base. . . . . . . . . . . 254.2 Intersection of lines without local constraint (Note: the red

boxes indicate identified intersection of two lines). . . . . 274.3 2 and 2+1 connected points . . . . . . . . . . . . . . . . 314.4 2+1 connected points . . . . . . . . . . . . . . . . . . . 314.5 All cases of intersection of lines. . . . . . . . . . . . . . . 32

A.1 Frame for testing environment. . . . . . . . . . . . . . . 64A.2 Camera mount gantry. . . . . . . . . . . . . . . . . . . . 65A.3 Overall system architecture 1 of 2. . . . . . . . . . . . . 69

iv

LIST OF FIGURES

A.4 Overall system architecture 2 of 2. . . . . . . . . . . . . 70A.5 Stereo cameras mounted on test base. . . . . . . . . . . 71A.6 Intersection of lines without local constraint (Note: the red

boxes indicate identified intersection of two lines). . . . . 74A.7 2 and 2+1 connected points . . . . . . . . . . . . . . . . 76A.8 2+1 connected points . . . . . . . . . . . . . . . . . . . 76A.9 All cases of intersection of lines. . . . . . . . . . . . . . . 77

v

List of Tables

3.1 Position of corners from above image . . . . . . . . . . . 15

A.1 Progression of images through Local intersection of linesalgorithm. . . . . . . . . . . . . . . . . . . . . . . . . . 82

vi

Chapter 1

Introduction

1.1 Summary

This report outlines the progress made to date in the development of animage based path mapping system for an autonomous robot. This reportoutlines the design challenges faced to date and the methods used to over-come them. This report looks at the system first from a very high level,then in much more detail, then finally describes the algorithms developedto complete the object identification and positioning algorithms.

1.2 Problem Description

The goal for this term is to develop a low cost, extendable image processingand path mapping subsystem that can be plugged into any mobile roboticsplatform. This system should be able to plan a path through any arbitraryspace using only information provided by a set of stereo cameras.The deliverables for this project will be a system that can plot a path fromthe location of the cameras to a small red ball that can be placed anywherein the field of view of the cameras. This path must be a smooth path thatcan reasonably be traversed by a physical entity, eg. it cannot have anysharp corners or discontinuities.The resulting system is able to generate a map for navigation but does notendeavor to generate an equation for the actual path. Most of the workthis term has been in developing new algorithms to identify feature pointswithin an image, and correlate them between the stereo images generatedby two cameras

1

CHAPTER 1. INTRODUCTION

1.3 Overall system architecture

1.3.1 Implementation language

The initial plan was to develop part of the system in C and the remainderin Matlab. After investigating the Intel Image Processing primitives it wasdecided to move the whole project into Matlab at this phase. Ultimatelythis will produce a system that will only function on a theoretical level, butwill make developing the system much simpler. Matlab provides facilitiesto not only capture images, but easily display the results at each step inthe path planning process. This ability to view the results easily makesdevelopment much simpler.

1.3.2 System structure

Figures 1.1 and 1.2 show the overall system architecture being employedin this project. Each module is built as a Matlab script and run in a linearfashion by the Matlab GUI manager. The following sections describe indetail the work completed in each of the modules.

2


Figure 1.1: Overall system architecture 1 of 2.

3


Figure 1.2: Overall system architecture 2 of 2.

4


1.3.3 Resulting toolbox

Figure 1.3 shows the toolbox view of the system. As all the functionsare built in Matlab it was easiest to develop the system from the toolboxpoint of view, and then implement a simple test framework to verify thecomponents of the system. By using the toolbox methodology it makes iteasy to test each module on its own before integrating with the rest of thesystem, as well it makes it a much simpler job to swap out a module witha newer or more application specific one.

Figure 1.3: Robot vision toolbox.

5

Chapter 2

System operational flow

In this section the flow of a single image processing cycle is examined indetail from a fairly high level Chapter 4 takes a much more detailed lookat the individual algorithms used by the system. The following discussionwill flow in the same way an image flows through the system.

2.1 Image capture

The first step is to capture a set of images, this is accomplished using theMatlab data acquisition toolbox which allows for direct capture of imagesfrom any windows quick camera, and from several frame grabber cards.The capture routines grab a series of 20 images and discard the first 19,this is done to allow the camera to auto adjust to its environment. Eachcamera is initialized, the full set of 20 images are taken, then the camera isshut down before the other is initialized. This serialization of the capturemakes it unsuitable for a dynamic environment, but is necessary as windowsonly allows one camera at a time to be open.

2.2 Image filtering

After capturing the image it needs to be filtered to help reduce noise causedby the low cost imagers. The CCD imagers have uneven responses withinthe same camera, this is most apparent in the blue layer. By presenting apure blue image to the camera flares and dead zones can be seen in theresulting image. The exact filter is described in more detail in Chapter 4.It is enough now to know that the filter helps reduce effects caused bythese flares and dead zones.

6

CHAPTER 2. SYSTEM OPERATIONAL FLOW

(a) (b) (c)

(d) (e)

(f)

Figure 2.1: Progression through algorithm (a) initial image, (b) single colorlayer,(c) edge detect, (d) line vectorization, (e) corner features discovered,(f) correlation between corners

7


2.3 Image decomposition

The initial tests were done using a single gray scale image generated fromthe intensity image. Unfortunately the camera imagers do not produceenough color contrast on all layers to make the grayscale image useful.Because of this each color layer is considered separately and the resultsmerged to generate the best feature representation of the image. Matlabmakes the image decomposition a simple task, by allowing to pull eachlayer out of the three dimensional array representation of the whole image.

2.4 Edge detection

Each of the images generated in the previous step are next run through anedge detection algorithm. This is one of the few built in Matlab functionsused in this system. The method used was the Sobel edge detect, thisprovides the most noise free results in the least amount of time. ThePrewitt algorithm was experimented with, but tended to generate muchnoisier results making it harder to extract the real features of interestfrom the image. The edge detection has to be applied on six differentimages which slows the whole system down, and in a future iteration,higher quality cameras will be used providing usable intensity image, andtherefore allowing the edge detect to be accomplished in a single pass.

2.5 Line vectorization

The next step is to convert the arbitrary lines discovered in the previousstep to well defined vectors that we can use to gain more information.The vectorization algorithm is described in detail in chapter 4. For nowit is enough to know that it follows along the lines and converts them toa vector described by its start and end points. Once we have this set ofvectors the original images are not used anymore and we can deal with welldefined vector sets.

2.6 Corner feature detection

Once the lines are vectorized we can apply the local intersection of linesalgorithm to find corner features, the actual theory behind this functionis given in Chapter 4. This algorithm is modelled roughly on how humanbeings identify corners. We accomplish this task by first identifying lines,and then subconsciously finding the intersection of these lines. This systembehaves in a similar way.

8


We need to identify corner features in each of the image layers, and thenre-combine the results into a single set of data, this is accomplished byway of a local average. The local average finds the best approximation ofthe middle between a series of corners within a predefined region.

2.7 Feature correlation

Now we have a collection of features, we need to correlate these featuresbetween the two images before we can attempt to calculate their positionin space. This is accomplished by finding the best match between a smallregion around the point of interest and the same sized region around thepoints of all of the corner features in the other image. To help reduce theworkload here, only a band centered around the corner we are interestedin finding the mate is considered when looking for points in the secondimage.As well as simply matching the points, a value based on how closely thepoints match is used to decide how certain we are that a point really existsin space at that location.

2.8 Point locating

Now that the two images are correlated we can calculate the actual loca-tion of the points in space. This is achieved using a high order logarithmicapproximation of the location of a series of test points. This simultane-ously helps compensate for warpage in the image due to the lens and anyother non-linearities in the camera sensing elements.These approximations are generated before run time, and mean the calcu-lation of the location of a point is a simple matter of substitution into theequation.

2.9 Map generation

The map is generated from the data calculated in the previous step. Thispart of the system uses a user defined falloff to determine how large anarea each point will influence, in most cases a single point will influencean area approximately 30cm in diameter. Any data from other sensors canbe integrated into the map at this time (Eg. the location of obstaclesdetermined using sonar ranging can be added to the map.

9

Chapter 3

Project Results

3.1 Environment results

To test the final operability of the system three independent environmentswere used. The first is a fully constrained environment. This was designedusing a 3D cad package, and provides little information about the systemsability to position objects, but proves quite well the effectiveness of thepoint matching algorithm.The second environment is a partially constrained one. This is the op-erating environment laid out in the initial specification. It consists of afixed background, geometric objects, and strong front lighting designed tominimize shadowing.The final environment is a mostly unconstrained environment. Lightingis still used from the front, but shadows are present, and have an effecton the system results. The following sections discuss the operation of thesystem with respect to each of these environments in turn.

3.1.1 Constrained environment

A three dimensional cad program was used to generate a virtual environ-ment. This environment allows for testing and verification of the systemup to the point correlation step. The actual positions generated by thesystem are meaningless as the translation of the object between the twocamera views is meaningless. This is due to the fact that the cad pack-age does not have facilities for placing camera views at known locations,only for exporting a snapshot of the viewing portal. Figure 3.1.1 showsthe two test images used, it can be seen that the lack of translation androtation about all three axes is physically impossible and therefore providesa good test to the matching algorithm, but meaningless data when theactual locations of the corner features are calculated.

10

CHAPTER 3. PROJECT RESULTS

(a) (b)

Figure 3.1: left (a) and right (b) images from fully constrained environment

Figure 3.2 shows the results of the match1. From this we can see thesystem is able to achieve a better then 80% accuracy in matching pointsbetween the two images. Even in a fully constrained environment thematch is not perfect as the conversion from the cad packages proprietaryformat to .jpg for use in Matlab is far from a lossless one, and a great dealof noise is introduced into the image during the transfer.

Though mostly meaningless Figure 3.3 shows the probability map gen-erated by the system. It can be observed that there are four distinct areasof high probability, and these correspond to the four corners of the objectin the image.

1the blue lines connect the mated points between the two images

11


Figure 3.2: Match between points within the constrained image pair.

12


Figure 3.3: Resulting probability map from fully constrained object.

3.1.2 Partially constrained environment

The partially constrained environment is the operational environment ini-tially laid out in the specification. It consists of a geometric, or series ofgeometric objects in a well lit, shadow free environment. It also specifies afixed color, evenly lit background. The only deviation from this specifica-tion is the fact that the test environment has carpet as the floor surface.This introduces noise into the final image, and makes it harder to accom-plish a high quality edge detect.Figure 3.1.2 shows the camera captures from the partially constrained en-vironment shown in Figure 3.5. The system is able to match points withapproximately a 70% accuracy, which is a little on the low side, but mostvertical edges are identified by at least 3 corner features, and therefore wecan be, on the average guaranteed to get at least one valid position fromeach edge.

Table 3.1.2 shows the actual calculated locations of corners within theimage, the system is almost completely certain about three of the corners,while the other two are most certainly noise. Referring to Figure 3.6 wecan see three perfect matches, along with two mismatches2. Correlatingthese values to the results in Table 3.1.2 we see that corner features 1,2

2One of these mismatches is caused by the a shadow produced by the object on thebackdrop

13


(a) (b)

Figure 3.4: left (a) and right (b) images from partially constrained environ-ment

Figure 3.5: Partially constrained test environment overhead shot.

14


Table 3.1: Position of corners from above imageCorner X position Y position Certainty

1 2 24 100%2 -2 56 62%3 4 71 8%4 4 22 0%5 -1 41 91%

and 5 correspond to the three matches, while 3 and 4 correspond to themismatched points. In a future iteration of the software a dynamic thresh-old will be added to remove these obvious outliers from the resultant dataset. Note that the probability of 0 comes from the floor function used inthe calculation of the probabilities, and really indicates a probability of lessthen one.

Figure 3.7 shows the resulting probability map of the match set shownabove. In this case we see almost exactly what we would expect, a largeblob of high probability right at the location of the object, and a zeroprobability of collision elsewhere. This space is easily navigated now, usingany one of may predefined algorithms.

Only a single instance is demonstrated here, but during testing theblock was placed in may different positions within the space, and similarresults were observed. If necessary more information can be provided aboutthese test runs.

15


Figure 3.6: Feature matches within the partially constrained test environ-ment.

16


Figure 3.7: Resulting probability map of partially constrained test environ-ment.

17


3.1.3 Unconstrained environment

As a final test the systems behavior in an unconstrained environment wasmeasured. Figure 3.8 shows an overhead view of the completely uncon-strained environment.

Figure 3.1.3 shows the camera views of the unconstrained environ-ment3, many corner features have been picked out by the local intersectionof lines algorithm described later in this paper. These sets provide a worstcase input into the system, and as such should provide the weakest results.After experimenting in this environment a surprisingly good representationof the space was observed. Figure 3.10 shows the resulting probability map,the small cluster of points in the bottom left hand corner (the (60,50) and(20,50) region) represent the block present in the foreground of the testimage, and the line of points extending up and to the right are a noisyrepresentation of the perimeter of the robot platform in the background ofthe image4.The representation produced in this environment is far from perfect, butmore then suitable for basic navigation tasks. The use of other reactivesensors would allow a robotic platform to easily navigate within an envi-ronment similar to the test environment.

3the images shown in figure 3.1.3 vary slightly from those seen in the overhead shot4position 200 corresponds to roughly 8 feet from the camera position which is the exact

measured position of the platform

18


Figure 3.8: Overhead shot of unconstrained test environment.

(a) (b)

Figure 3.9: left (a) and right (b) images from unconstrained environment

19


Figure 3.10: Probability result of the unconstrained environment.

20


3.2 Computational complexity

The final system takes longer then expected to process a set of images,even disregarding the large overhead to capture the images one at a time.To determine if this loss of efficiency is coming from the core algorithms orwhether it is due to the use of Matlab a full analysis of the time complexityof the system was carried out.The time to discover and parameterize all the lines in the image growslinearly with the size of the image, thus for a (NxM) image the system willtake O(N ·M) worst case to find all the lines.Next the find corners algorithm is investigated. Since we compare eachcorner with every other corner at least once we get a growth of n + n −1 + ... + 2 + 1 or O

(n·(n+1)

2

)Once the corner features are identified we need to correlate the sets ofcorners which grows linearly with the number of corners discovered in theprevious step. Worst case is all corners exist within a narrow band on eachimage, and we will have to compare every corner with every other cornerin each direction yielding a growth in order O(c2)Once this is accomplished finding the position of the object in space andgenerating the map grow in a linear fashion, therefore are O(m)The overall growth of the system is primarily driven by the find lines step,and therefore the overall growth of the system can be approximated by agrowth of O(n2)5. This would lead one to believe that in a simple testenvironment such as was being used, yielding 30 to 40 lines, and 10 featurepoints in most cases that the long processing time is mainly a function ofMatlab’s functional overhead.

3.3 System successes

The system behaved in many ways much better then expected. The fol-lowing gives a brief outline of some of the system successes.

• The system behaves much better then expected in an unconstrainedenvironment, with only minimal reactive sensor output the roboticplatform should be able to navigate within any arbitrary space.

• The whole system was built from the ground up using proprietaryfunctions, therefore if the system every becomes commercially viable(after much more work), no royalties will be due.

• The complexity growth of the system makes possible using larger

5in this case n represents the number of lines discovered in the vectorization operation

21


images (in the order of 640x480) to achieve more precise resultswithout slowing down the processing time greatly.

• The pipelined architecture in the resulting system allows for easymulti-platform deployment which in turn allows for much greaterthroughput (eg it would be possible to break the operations up andrun them on separate computers thus providing more raw computerpower to analyze the image data.

3.4 System shortcomings

Some aspects of the system did not meet the original design concepts.The following is a brief outline of some of these shortcomings.

• The system is not capable of planning a path within the probabilityspace it generates, this is due to sheer lack of time.

• The system has not been ported to C and is not operating online.This is primarily due to the fact that none of the pre-built functionsthat were intended to be used with this system functioned properlyon the low contrast, low resolution imagers in the web cameras.

• The system takes longer to process images then expected, this ismostly due to the overhead imposed by Matlab.

• The system cannot take two simultaneous images, and therefore isunsuitable (in its current form) for dynamic systems.

• The two cameras had incredibly narrow fields of view, and this re-duced the field of view of the final system down to the order of 25o.Better cameras would reduce this problem.

3.5 Next steps

The next steps for this project are as follows :

• Replace the web cameras with proper analog cameras and a framegrabber card that is capable of capturing two images simultaneously.

• Port the code to C or C++ and deploy it on one of Unit006 off-boardprocessors. This means the platform will be responsible for capturingthe images and transmitting them to a remote machine for processingusing the wireless network link.

6Unit00 is the name of the robotics platform

22


• Fine tune all the degrees of freedom of the system, with more then25 tweakable options it is almost impossible to fine tune the wholesystem in the allotted time.

23

Chapter 4

System theory

4.1 Object Detection

4.1.1 Capture image

The capture of the images is achieved using facilities provided by Matlabitself. It is not possible to capture an image in Matlab using any WDM orVFW compatible cameras. Figure 4.1 shows the cameras mounted on theirtest base. The cameras themselves are low cost D-Link cameras capableof producing 320 × 240 color images, unfortunately these cameras havesome major contrast issues. The capture process initializes each camera inturn, captures an image, then shuts the camera down. This is done as theWDM will not allow for both cameras to be initialized simultaneously. Thisfact alone greatly slows the capture process to the order of approximately30sec per image pair.The imagers within the cameras can produce a well contrasted red colorlayer, but produces low contrast blue and green layers. This complicates theimage processing greatly as the initial plan called for using color informationfrom all layers. The result is far less precision then anticipated in the finalimages, making it hard to identify hard corners, and introducing a greatdeal of error into the final result.

4.1.2 Filter image

After capturing the image as much noise needs to be removed as possible.The floor surface is the largest single source of noise in the resulting images.The texture of the carpet shows quite clearly on the red color layers. Thefiltering is accomplished using the filter kernel shown in Equation (4.1)and the imfilter function provided by Matlab. The values used to createEquation (4.1) were found through trial and error, and seem to producethe best results, without unduely affecting the later steps of the process.

24

CHAPTER 4. SYSTEM THEORY

Figure 4.1: Stereo cameras mounted on test base.

Before deploying the system these values will be revisited to determine ifany further tweaking will improve the downstream results.

0.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.0125

(4.1)

The timing of this function is determined by Matlab, and therefore noalgorithmic tweaking will produce a faster result. In a production system,this function would be implemented by hand, and most likely provide amuch more efficient filter.

4.1.3 Split image

The image split is easily achieved in Matlab as images are handled asthree dimensional arrays. To split a channel off simply requires copyingthe desired layer into a temporary two dimensional array. The image isstored in the three dimensional array in the following form [x,y,layer] wherelayer is an integer between 1 and 3, with 1 being the red layer, 2 being thegreen layer, and 3 being the blue layer.

25


4.1.4 Edge detect

The edge detect makes use of the edge function provided by Matlab. Thistakes the image as input, as well as the desired edge detect algorithmtype. Through experimentation it was determined that either the Sobelor Prewitt methods provided the best results, with Prewitt being the finalchoice. The Prewitt method proved the best tradeoff of speed and results.The Canny method, which provided the sharpest edges, also produces agreat deal of noise that could not be differentiated from the points ofinterest in the final image.The Prewitt method uses a Prewitt approximation of the first derivative ofthe image, then uses this approximation to find maxima in the derivative.These maxima points represent points of rapid change in contrast in thecolor layer, and therefore rapid change in color. These points, due to thesimplifying assumptions can be determined to be edges.

4.1.5 Find corners

The find corners function is the heart of the system, this function willidentify and group related corners. The resulting groups will represent ge-ometric objects within the image itself. These objects can then be matchedbetween the two images, and distances measured.

Initial approach

The first approach taken was to use a Kanade-Lucas-Tomasi (KLT) orHarris1 transform to identify corners. Both of these approaches identifythe required corners, but do to the low contrast of the original image, andtherefore the very poor contrast of the resulting grayscale image, the resultsproduced more “noise” corners then viable ones. It was impossible to sortthe real corners from the ones produced by image noise, and therefore itwas impossible to identify objects.

Current approach

Section 4.2 describes the algorithm developed to find these corners. Thisalgorithm is the result of the greatest amount of work within this project,and is believed to be completely original.This algorithm, termed Local intersection of lines, works on the principalthat if lines within an image can be identified with reasonable accuracy,then the intersection of these lines can be used to identify corners. The

1The Harris transform implementation was produced by Dr. Philip Torr,http://wwwcms.brookes.ac.uk/ philiptorr/

26


constraint is that these corners must exists within the local neighborhoodof one or both of the lines, otherwise a series of points are created notrelated to any object (Figure 4.2 shows the result of the algorithm runwithout the local constraints).

Figure 4.2: Intersection of lines without local constraint (Note: the redboxes indicate identified intersection of two lines).

4.1.6 Merge results

The results of corners found on each layer are merged by averaging allpairs of corners that exists within a minimum vector distance from anothercorner. This further reduces the number of corners that need to be matchedwith the other images. The result is a set of corners that hopefully averageout to the most accurate representation of the corner.

4.1.7 Matching corners

Once we have a set of corner features identified for each image we need tomatch the features in each image. The algorithm that accomplishes thisis a band based region match. The band region match takes a subset of

27


the image around the identified corner, and compares it with each cornerin the other image. To reduce the number of required match calculationsonly corners that exist within a band in the second image is considered.This band match is effective as most of the difference between the twoimages is seen in x translation2 and very little y translation3.The match is achieved by calculating the sum of the pixel by pixel differencebetween the local area around each feature point (Refer to Equation (4.2).The grayscale image is used to accomplish this as it seems to provide thebest results. If the two local regions only differ by position, the result ofthis calculation will be zero. In most cases the image will only differ by apure x and y translation resulting in a small value being calculated. If thereis a rotation as well as a translation the variance will be slightly higher.After calculating the variance for every point in the matching band andremoving those not below a defined threshold, the lowest value is chosenas a match between the two corner features.

V ariance =1

(X · Y )·

X∑i=1

Y∑j=1

(I(2)i,j − I(1)i,j) (4.2)

The entire set of corners is iterated over in this fashion and the resultsstored in an array. Next the process is repeated interchanging the twoimages, this removes points that match in one direction, but not in theother. In this way we re-enforce symmetry between the two images. Finallythe real matching corners are chosen as the intersection of the two setsof corner matches Results = (Matchlefttoright) ∩ (Matchrighttoleft). Thevariance is also recorded with each match and will provide a measure ofcertainty for each point.

4.1.8 Calculating position of point in space

Once the features have been correlated the position of each point can becalculated in space, and a probability can be calculated. The position ofthe object in space is measured using a high order approximation of a set oftest data, and will change depending on the baseline of the cameras. Theapproximation also takes into account the warpage of the image caused bythe lenses. The equations in (4.3) represent the approximation and andare valid within the area of detection out to approximately 5M, at whichpoint the cameras resolution reduce the accuracy so low that the results

2the object appears at a different x position in the two images3 the y translation comes entirely from perspective rotation of the image

28


are meaningless.

position(x) = −36755.567 + 20873.78 · log(x)− 1.9709381 · y− 3943.1833 · (log(x))2 + 0.014525003 · y2

+ 0.25848237 · y · log(x) + 247.62209 · (log(x))3

+ 2.0378758x10−5 · y3 − 0.0040336311 · y2 · log(x)

+ 0.047144751 · y · (log(x))2

position(y) = −1047.067− 2.0850787 · x + 0.010763948 · x2

− 1.9176879x10−5 · x3 + 1013.2165 · log(y)

− 271.20435 · (log(y))2 + 23.73898 · (log(y))3

(4.3)

These two functions approximate the data with a r2 value of approx-imately 0.91, any higher r2 value would yield a non-contiguous function,and therefore would be unsuitable in this application.Finally we have a position, we now tie a certainty to it. This certainty isthe same as the variance calculated using equation 4.2.

4.1.9 Map generation

The final step is to generate the probability map. This function iteratesthrough the set of corner locations, placing the probability at the center,and falling off in all directions at a specified rate. This means that thefalloff from one point may add to the falloff of another corner. This simplyindicates that we are far more certain of an obstacle existing if we find twofeature points near the position.Any additional sensor data can now be integrated into the map, and thepath planning problem becomes a matter of plotting a path through theprobability space. Many search techniques could be used for this, the mostideal would be an A∗ search which would yield an optimal solution to thesearch problem.

4.2 Local intersection of lines

The local intersection of lines algorithm is a technique developed duringthe course of this project to identify the corners of a geometric object ina given image. The technique finds the intersection of lines and verifiesthat these intersections exists within the local area of the end of eitherline. This algorithm takes an image, splits it into its corresponding colorchannels, then finds the corners within each of these images. The final

29


results are them merged to produce a single set of corners for each inputimage. These corners can then be grouped together into objects using thelines discovered in an earlier step.

4.2.1 Identifying lines within an image

The first step is to identify lines within the image, this operation is per-formed separately on each color channel. After splitting the image intoits constituent channels a bi-directional Prewitt or Sobel edge detect isapplied. The result is a binary image representing all the points where adrastic change in color intensity can be seen.This now needs to be vectorized into a usable image. This is accomplishedin a two pass, four connected point by point search through the image.Figure 4.3 will be referred to in the next section. There are three possibleways for a point to connect to the current one that keeps the line “con-nected”, first it can be immediately adjacent, indicated by the red pixelin figure 4.3. Since two passes are made this pixel will either be foundat xi+1 in the horizontal pass, or yi+1 in the vertical path. The secondcase are diagonally connected points. These are indicated by the blackpixels in Figure 4.3, each of these positions are checked in turn, and theyare only checked if a pixel is not found that is directly connected. Finallythe 2 + 1 connected points are examined. These points are indicated bythe blue pixels in Figure 4.3 (or the black pixels in Figure 4.4). Thesepoints are included to help the algorithm find lines that are not physicallyconnected, and therefore can jump small gaps in the image (sometimescaused by bad areas in the imager). These 2+1 connected points are onlyexamined if nothing is found either directly or diagonally connected. If nopoint is found in any of these positions, the search moves into the directlyconnected space (red pixel in Figure 4.3), increments a counter indicatinghow many spaces have been encountered and continues.After a point is found it is added to a set of discovered points. One prob-lem encountered during the development of this algorithm is that it had atendency to follow a line around a corner. To fix this a fitness factor wasintroduced, this is calculated using Equation (4.6). This first calculates theslope of the current discovered line using Equation (4.4), next the averageof all slopes measured to this point is calculated using Equation (4.5). Thefitness factor is then compared against a threshold, if it is greater then theset threshold the line is terminated. This fitness factor insures that vari-ances in the line can be accommodated, but it will no longer follow a linearound a corner.Initially both the y intercept, and slope were examined, but small variancesin the slope, when the line is nearly vertical, has a huge affect on the inter-cept, and therefore does not tell us much about the changing of the line.

30


The algorithm continues in this fashion until one of two conditions are met.The first is too many gaps have been detected, this check is made afterevery space has been encountered, and only counts contiguous spaces. Theother condition is that the slope of the line being created has varied toofar from the average. Once the end of a line is discovered, the size of theline is compared against a threshold indicating the minimum line size. Ifthe line is longer then the given minimum, the start and end points of theline are added to a set of known lines. Finally all the pixels used in creatingthe line are removed from the image, this prevents segments of the sameline being found in subsequent passes.

Figure 4.3: 2 and 2+1 connected points12345

a b c d

Figure 4.4: 2+1 connected points12345

a b c d

m =y2 − y1

x2 − x1

(4.4)

mave =1

n·

n−1∑i=1

mi (4.5)

fitness factor =|mave −m| (4.6)

31


Figure 4.5: All cases of intersection of lines.

32


4.2.2 Finding intersections of lines

Once the lines have been found the points of intersection between themneed to be discovered. There are quite a number of possible cases, andeach one need be examined in turn. This part of the algorithm parsesthrough the entire set of edges comparing them pair by pair looking forlines that intersect, and who’s intersection fall within the local vicinity ofthe end of one of the lines. The following cases are applied to each pair oflines in turn until either an intersection is discovered, or it is determinedthat the lines do not cross anywhere in space. Please refer to Figure 4.5,which depicts visually all the possible cases.

Two arbitrary lines crossing in space

The first is the most general case, two arbitrary lines crossing in space.This case covers pairs of lines where neither line is horizontal or vertical.It does however cover two parallel lines in any orientation.Equation (4.7) gives the general equation for a line, we are looking forvalues of x and y that simultaneously solve the equation for either line.We can once again use Equation (4.4) to calculate the slope of the line, andusing the slope calculate the y intercept (b). Once we have solved thesevariables for each line (which are stored as a pair of points) we can attemptto find the intersection points. Equation (4.8) shows the equations we wishto solve simultaneously, m1 and b1 are the slope and intercept of the firstline respectively, m2 and b2 for the second line. If either the calculatedvalue for b or the calculated value for m are infinite, we know the line iseither horizontal or vertical, so this case does not apply.The condition we are looking for is a value of x and y that solves bothequations. We therefore have a set of four equations in four unknowns,Equation (4.9) shows these equations re-arranged having constants on theleft hand side, and the variables we wish to solve on the right. It is nowa simple matter of solving the matrix presented in Equation (4.10), it canbe seen that a pair of parallel lines will produce an infinite value due to thefact that two parallel lines have the same slope, and therefore:

b2 − b1

m1 −m2

=b2 − b1

0= ∞

If the infinite result is generated we know the lines have no intercept andcan move on to the next pair. Otherwise the intersection point can be

33


given as :

xintercept =b2 − b1

m1 −m2

yintercept =b2 ·m1 − b1 ·m2

m1 −m2

Finally a check need be made that the point is within the bounds of theimage, and that it is close to the end of one of the lines. Since this iscommon to all cases it will be described in section 4.2.3

y = m · x + b (4.7)

y1 = m1 · x1 + b1

y2 = m2 · x2 + b2

x1 = x2

y1 = y2

(4.8)

−b1 = m1 · x1 − y1

−b2 = m2 · x2 + y2

x1 = x2

y1 = y2

(4.9)

RREF

m1 0 −1 0 −b1

0 m2 0 −1 −b2

1 −1 0 0 00 0 1 −1 0

⇒

1 0 0 0 b2−b1

m1−m2

0 1 0 0 b2−b1m1−m2

0 0 1 0 b2·m1−b1·m2

m1−m2

0 0 0 1 b2·m1−b1·m2

m1−m2

(4.10)

One horizontal line, and one vertical line

This is the easiest case to handle, and can be identified by one line havingan infinite slope, and the other having an infinite y intercept. The pointsof intersection can be extracted from the lines, the vertical line providesthe x intercept, and the horizontal line provides the y. This case is testedfor before either of the following two, as a pair entering this case wouldalso fall into the other two.

xintercept = X component of the vertical line.

yintercept = Y component of the horizontal line.

Once again the tests given in section 4.2.3 are applied to test the validityof the corner.

34


One horizontal line, and one arbitrary line

In this case one line is horizontal, while the other is non horizontal orvertical. In this case the horizontal line contributes the value of the ycomponent, and the x value must be calculated. This can easily be doneusing Equation (4.7), the slope and intercept values coming from the non-horizontal line. Once again the tests of Section 4.2.3 must be applied toverify the correctness of the line.

yintercept = y component of the horizontal line.

xintercept =yintercept − b

m

One vertical line, and one arbitrary line

The final case is the one in which one line is vertical and the other is nonhorizontal nor vertical. In this final case the vertical line contributes thex component, and the y value must be calculated in a similar manner tosection 4.2.2.


yintercept = m · xintercept + b

4.2.3 Removing unwanted intersections

We have now calculated an intersection point between the line pair we arecurrently examining, the next step is to test the validity of this point. Thefirst test is to verify that it exists within the bounds of the image, that is,that the point falls within the following range:

0 ≥xintercept ≥ 320

0 ≥yintercept ≥ 240

Next we need to verify that the point exists on or near one of the linesused to detect it. This is done by calculating the euclidian distance betweenthe point and both of the line segments4. A first quick test is done usingEquation 4.11 to measure the distance between the endpoints of both linesand the point in question. If the point does not fall within the maximumrange (this can be due to the fact the point is on the line, but far awayfrom either endpoint) we must make the more expensive calculation todetermine the distance between the point and the line segment. To do thiswe must first calculate the projection of the point onto the line segment

4The algorithm used was based on the paper by :David Eberly available at :http://www.magic-software.com/Documentation/DistancePointLine.pdf

35


(note this technique is valid in n dimensions), this is done using Equation(4.12). The next step depends on where the point projected onto the linesegment, Equation (4.13) determines the final distance. If the point existson the line, the distance will be zero, otherwise it represents the shortestdistance between the point and the line. Once again this is compared tothe the threshold, if it is less the point is added to the set of valid corners,otherwise it is discarded.

vector distance = ‖p2 − p1‖ (4.11)

to =~M · (~P − ~B)

~M · ~M(4.12)

D =

‖~P − ~B‖ to ≤ 0

‖~P − ( ~B + to · ~M)‖ 0 < to < 1

‖~P − ( ~B + ~M)‖ to ≥ 1

(4.13)

36

default

Appendix A

Appendix

A.1 Matlab code

Please note, this code is proprietary, and is only provided here for evaluationreasons. The publishing of this code in this document should not be viewedas a waiver of the copyright.

A.1.1 test.m

37

default

APPENDIX A. APPENDIX

A.1.2 remove row.m

The remove row function is a helper function that removes a row from a2 dimensional array. This is used by almost all of the following functionsand scripts.

38

default


A.1.3 camera open.m

39

default


A.1.4 im cap.m

40

default


A.1.5 camera close.m

41

default


A.1.6 img filter.m

42

default


A.1.7 img decompose.m

43

default


A.1.8 edge detect.m

44

default


A.1.9 identify corners.m

Identify corners is a wrapper that converts the find lines and find cornersfrom scripts to a single function call. This helps clean up the user interfaceto the system, at the same time it eases the need to pass strange arraysof data back from these functions.

45

default


A.1.10 find lines.m

46

default


A.1.11 find corners.m

47

default


A.1.12 find lines and find corners helper functions

line to point distance.m

48

default


merge layers.m

49

default


merge lines.m

50

default


add corner to set.m

51

default


add to set.m

52

default


add to total.m

53

default


corelate sets.m

Correlate sets is another wrapper function, it hides a series of scripts behinda function API that makes passing data easier during development, whilestill providing a well defined API to the end user.

54

default


calculate similarity.m

55

default


match corners.m

56

default


extract region.m

57

default


A.1.13 find position.m

58

default


A.1.14 range objects.m

59

default


A.1.15 create map.m

60


A.2 Progress report 1

A.2.1 Summary

This report outlines the intended approach to develop an image based pathmapping system for an autonomous robot. The approach given here is onlyan outline of what might be the final design solution, and as such takesinto account any foreseeable problems that may be encountered. The finalsolution may not resemble that layer out here as many unforseen problemsare bound to appear during the design process.Also provided is a rough timeline intended only to provide a goal alongthe development path, again unforseen problems could quickly nullify thistimeline, and require re-consideration of what can be accomplished duringthe given time.

A.2.2 Problem Description

The goal for this term is to develop a low cost, extendable image processingand path mapping subsystem that can be plugged into any mobile roboticsplatform. This system should be able to plan a path through any arbitraryspace using only information provided by a set of stereo cameras.The deliverables for this project will be a system that can plot a path fromthe location of the cameras to a small red ball that can be placed anywherein the field of view of the cameras. This path must be a smooth path thatcan reasonably be traversed by a physical entity, eg. it cannot have anysharp corners or discontinuities.

A.2.3 Proposed solution

Simplifying Assumptions

To simplify the initial design and verification of the system as a whole thefollowing assumptions will be used:

1. The ”world” will consist of geometrically simple shapes, for examplecubes,spheres, and pyramidal shapes.

2. The environment will be evenly lit to help reduce problems introducedby shadows.

3. The platform will be stationary, since low cost USB cameras are beingused it is impossible to take two images at exactly the same time,and therefore each image would be taken at different points in space.

4. The background of the environment will consist of consistently litand colored surfaces.

61


Once the basic system is operational, time permitting, each of these sim-plifying assumptions will be removed in turn.

Image processing

The first step will be to capture the image and perform some basic imageprocessing steps, these include :

1. Image de-warping, to remove any curvature introduced into the imageby the lenes.

2. Image channel separation, to break the image into its separate colorchannels

3. Edge detect.

4. Image re-assembly

5. Object location

The first components of the image processing system will be providedby the Intel OCV1 libraries, which are a set of high level functions thatprovide basic image manipulation and processing functionality. The edgedetect components will most likely be implemented by hand, as they arefairly simple. A small convolution kernel will most likely suffice for thisproject but can be expanded fairly easily. While it may be possible toonly examine one color channel to gather all the relevant information,hooks will be left in the system to allow for examining each of the colorchannels separately. The final step in image processing will be to identifyand location objects in space. Using the information gathered throughthe use of stereo cameras, the approximate location of any object can becalculated as well as a ”bounding box” that defines the approximate spacethe object inhabits. Since most of the time the exact location of an objectis hard to determine, there will be a certainty figure assigned to the objectwhich specifies how sure the algorithm is that an object truly does exist atthat location.It is also possible that Matlab will provide all the same functionality if it isdetermined that the OCV libraries ar insufficient to complete any of thesetasks.

Path Planning

The path planning will take a data structure containing the location of allobjects found by the image processing algorithms as input, and output an

1OCV - Open Computer Vision libraries

62


equation representing the path to the endpoint. This system will take aprobabilistic approach to the path planning problem to help make it moreflexible in a real environment.The algorithm will generate a probability density map representing thelikelihood of a collision occurring at any point in space. This map will begenerated using the object location, the bounding box, and the certaintyfigure generated by the image processing algorithm. Once the map hasbeen generated a functional representation of this map will be generated,this will most likely be accomplished using Matlab functions. The resultwill be a function of two variables representing the same information asthe map mentioned above.Finally using a gradient decent technique, or possibly a piecewise splinetechnique the path function can be generated. This function can then beused by any motion control system to move through the space.

A.2.4 Proposed implementation

Operating environment

Figure A.1 shows the beginning framework of the testing environment.Fabric will be stretched across the frame to provide an even backgroundfor initial system testing. This will be removed during the later stages ofthe project and a real environment will be used.The target location will be marked in space using a 40mm ball painted in acontrasting color. This will allow experimenting with the dynamic systemby moving the ball around the space while the system remains online andobserving the change in the intended path.

Software design

Image processing

The image processing algorithms will be built in C under the Linux oper-ating environment. This system will run on up to two separate machinesand therefore will have to be designed from the ground up for networkdistribution. This functionality will be provided by the SIMPL2 interpro-cess messaging protocol. All attempts will be made to allow the system tooperate on a single computer for simplicity sake.The core image processing algorithm will be built around the functionalityprovided by the Intel OCV libraries, and if these fall short, Matlab functions.By using pre-optimized libraries, and not re-inventing the wheel a more ro-bust system should result. If Matlab functionality is used it will be harder

2SIMPL - Synchronous Interprocess Messaging Project, a network transparent inter-process messaging protocol built to mimic the behavior of the QnX messaging protocol.

63


Figure A.1: Frame for testing environment.

to distribute the components of the system across multiple processors, andinstead the cluster support in Matlab itself will be used.

Path planning

The path planning system will be implemented in Matlab, as it providesmany of the high level math and visualization functions required to com-plete this component of the project. The interface into the Matlab func-tionality will either be scripts or C code. The image toolbox may alsoprovide some of the functionality required in the image processing system.

Hardware design

The hardware design component of this project has already been com-pleted, Figure A.2 shows the camera mounts separated from the platform.The robotics platform this image processing system is intended to operatewith is a product of work completed in the previous project term (499A)and more information can be found at the Unit00 project homepage :http://www.ece.uvic.ca/ jreynold/robot/ .

64

http://www.ece.uvic.ca/~jreynold/robot/

http://www.ece.uvic.ca/~jreynold/robot/


Figure A.2: Camera mount gantry.

A.2.5 Proposed Timeline

65


66


A.3 Progress Report 2

A.3.1 Summary

This report outlines the progress made to date in the development of animage based path mapping system for an autonomous robot. This reportoutlines the design challenges faced to date and the methods used to over-come them. The project has fallen behind the intended schedule due tohaving to change the planned system significantly in several key areas, aswell as numerous hardware failures. The hope is that the system will stillbe completed and functional by the deadline.

A.3.2 Problem Description

The goal for this term is to develop a low cost, extendable image processingand path mapping subsystem that can be plugged into any mobile roboticsplatform. This system should be able to plan a path through any arbitraryspace using only information provided by a set of stereo cameras.The deliverables for this project will be a system that can plot a path fromthe location of the cameras to a small red ball that can be placed anywherein the field of view of the cameras. This path must be a smooth path thatcan reasonably be traversed by a physical entity, eg. it cannot have anysharp corners or discontinuities.

67


A.3.3 Overall system architecture

Implementation language

The initial plan was to develop part of the system in C and the remainderin Matlab. After investigating the Intel Image Processing primitives it wasdecided to move the whole project into Matlab at this phase. Ultimatelythis will produce a system that will only function on a theoretical level, butwill make developing the system much simpler. Matlab provides facilitiesto not only capture images, but easily display the results at each step inthe path planning process. This ability to view the results easily makesdevelopment much simpler.

System structure

Figures A.3 and A.4 show the overall system architecture being employedin this project. Each module is built as a Matlab script and run in a linearfashion by the Matlab GUI manager. The following sections describe indetail the work completed in each of the modules.

68


Figure A.3: Overall system architecture 1 of 2.

69


Figure A.4: Overall system architecture 2 of 2.

70


A.3.4 Object Detection

Capture image

The capture of the images is achieved using facilities provided by Matlabitself. It is not possible to capture an image in Matlab using any WDM orVFW compatible cameras. Figure A.5 shows the cameras mounted on theirtest base. The cameras themselves are low cost D-Link cameras capableof producing 320 × 240 color images, unfortunately these cameras havesome major contrast issues. The capture process initializes each camera inturn, captures an image, then shuts the camera down. This is done as theWDM will not allow for both cameras to be initialized simultaneously. Thisfact alone greatly slows the capture process to the order of approximately30sec per image pair.The imagers within the cameras can produce a well contrasted red colorlayer, but produces low contrast blue and green layers. This complicates theimage processing greatly as the initial plan called for using color informationfrom all layers. The result is far less precision then anticipated in the finalimages, making it hard to identify hard corners, and introducing a greatdeal of error into the final result.

Figure A.5: Stereo cameras mounted on test base.

71


Filter image

After capturing the image as much noise needs to be removed as possible.The floor surface is the largest single source of noise in the resulting images.The texture of the carpet shows quite clearly on the red color layers. Thefiltering is accomplished using the filter kernel shown in Equation (A.1)and the imfilter function provided by Matlab. The values used to createEquation (A.1) were found through trial and error, and seem to producethe best results, without unduely affecting the later steps of the process.Before deploying the system these values will be revisited to determine ifany further tweaking will improve the downstream results.

0.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.01250.0125 0.0125 0.0125 0.0125 0.0125

(A.1)

The timing of this function is determined by Matlab, and therefore noalgorithmic tweaking will produce a faster result. In a production system,this function would be implemented by hand, and most likely provide amuch more efficient filter.

Split image

The image split is easily achieved in Matlab as images are handled asthree dimensional arrays. To split a channel off simply requires copyingthe desired layer into a temporary two dimensional array. The image isstored in the three dimensional array in the following form [x,y,layer] wherelayer is an integer between 1 and 3, with 1 being the red layer, 2 being thegreen layer, and 3 being the blue layer.

Edge detect

The edge detect makes use of the edge function provided by Matlab. Thistakes the image as input, as well as the desired edge detect algorithmtype. Through experimentation it was determined that either the Sobelor Prewitt methods provided the best results, with Prewitt being the finalchoice. The Prewitt method proved the best tradeoff of speed and results.The Canny method, which provided the sharpest edges, also produces agreat deal of noise that could not be differentiated from the points ofinterest in the final image.The Prewitt method uses a Prewitt approximation of the first derivative of

72


the image, then uses this approximation to find maxima in the derivative.These maxima points represent points of rapid change in contrast in thecolor layer, and therefore rapid change in color. These points, due to thesimplifying assumptions can be determined to be edges.

Find corners

The find corners function is the heart of the system, this function willidentify and group related corners. The resulting groups will represent ge-ometric objects within the image itself. These objects can then be matchedbetween the two images, and distances measured.

Initial approach

The first approach taken was to use a Kanade-Lucas-Tomasi (KLT) orHarris3 transform to identify corners. Both of these approaches identifythe required corners, but do to the low contrast of the original image, andtherefore the very poor contrast of the resulting grayscale image, the resultsproduced more “noise” corners then viable ones. It was impossible to sortthe real corners from the ones produced by image noise, and therefore itwas impossible to identify objects.

Current approach

Section A.3.6 describes the algorithm developed to find these corners. Thisalgorithm is the result of the greatest amount of work within this project,and is believed to be completely original.This algorithm, termed Local intersection of lines, works on the principalthat if lines within an image can be identified with reasonable accuracy,then the intersection of these lines can be used to identify corners. Theconstraint is that these corners must exists within the local neighborhoodof one or both of the lines, otherwise a series of points are created notrelated to any object (Figure A.6 shows the result of the algorithm runwithout the local constraints).

Merge results

The results of corners found on each layer are merged by averaging allpairs of corners that exists within a minimum vector distance from anothercorner. This further reduces the number of corners that need to be matchedwith the other images.

3The Harris transform implementation was produced by Dr. Philip Torr,http://wwwcms.brookes.ac.uk/ philiptorr/

73


Figure A.6: Intersection of lines without local constraint (Note: the redboxes indicate identified intersection of two lines).

A.3.5 Uncompleted work

Group corners

Once the corners are identified, the need to be grouped into objects, thiswill be done using the lines found in the Local intersection of lines process.These lines can be used to tie corners together in an unconnected digraph.These graphs will then be used to match corners between the two stereoimages.

Remaining steps

The final steps will proceed as outlined in the initial design report.

A.3.6 Local intersection of lines

The local intersection of lines algorithm is a technique developed duringthe course of this project to identify the corners of a geometric object ina given image. The technique finds the intersection of lines and verifies

74


that these intersections exists within the local area of the end of eitherline. This algorithm takes an image, splits it into its corresponding colorchannels, then finds the corners within each of these images. The finalresults are them merged to produce a single set of corners for each inputimage. These corners can then be grouped together into objects using thelines discovered in an earlier step.

Identifying lines within an image

The first step is to identify lines within the image, this operation is per-formed separately on each color channel. After splitting the image intoits constituent channels a bi-directional Prewitt or Sobel edge detect isapplied. The result is a binary image representing all the points where adrastic change in color intensity can be seen.This now needs to be vectorized into a usable image. This is accomplishedin a two pass, four connected point by point search through the image.Figure A.7 will be referred to in the next section. There are three possibleways for a point to connect to the current one that keeps the line “con-nected”, first it can be immediately adjacent, indicated by the red pixelin figure A.7. Since two passes are made this pixel will either be foundat xi+1 in the horizontal pass, or yi+1 in the vertical path. The secondcase are diagonally connected points. These are indicated by the blackpixels in Figure A.7, each of these positions are checked in turn, and theyare only checked if a pixel is not found that is directly connected. Finallythe 2 + 1 connected points are examined. These points are indicated bythe blue pixels in Figure A.7 (or the black pixels in Figure A.8). Thesepoints are included to help the algorithm find lines that are not physicallyconnected, and therefore can jump small gaps in the image (sometimescaused by bad areas in the imager). These 2+1 connected points are onlyexamined if nothing is found either directly or diagonally connected. If nopoint is found in any of these positions, the search moves into the directlyconnected space (red pixel in Figure A.7), increments a counter indicatinghow many spaces have been encountered and continues.After a point is found it is added to a set of discovered points. One prob-lem encountered during the development of this algorithm is that it had atendency to follow a line around a corner. To fix this a fitness factor wasintroduced, this is calculated using Equation (A.4). This first calculatesthe slope of the current discovered line using Equation (A.2), next theaverage of all slopes measured to this point is calculated using Equation(A.3). The fitness factor is then compared against a threshold, if it isgreater then the set threshold the line is terminated. This fitness factorinsures that variances in the line can be accommodated, but it will nolonger follow a line around a corner.

75


Initially both the y intercept, and slope were examined, but small variancesin the slope, when the line is nearly vertical, has a huge affect on the inter-cept, and therefore does not tell us much about the changing of the line.The algorithm continues in this fashion until one of two conditions are met.The first is too many gaps have been detected, this check is made afterevery space has been encountered, and only counts contiguous spaces. Theother condition is that the slope of the line being created has varied toofar from the average. Once the end of a line is discovered, the size of theline is compared against a threshold indicating the minimum line size. Ifthe line is longer then the given minimum, the start and end points of theline are added to a set of known lines. Finally all the pixels used in creatingthe line are removed from the image, this prevents segments of the sameline being found in subsequent passes.

Figure A.7: 2 and 2+1 connected points12345

a b c d

Figure A.8: 2+1 connected points12345

a b c d

m =y2 − y1

x2 − x1

(A.2)

mave =1

n·

n−1∑i=1

mi (A.3)

fitness factor =|mave −m| (A.4)

76


Figure A.9: All cases of intersection of lines.

Finding intersections of lines

Once the lines have been found the points of intersection between themneed to be discovered. There are quite a number of possible cases, andeach one need be examined in turn. This part of the algorithm parsesthrough the entire set of edges comparing them pair by pair looking forlines that intersect, and who’s intersection fall within the local vicinity ofthe end of one of the lines. The following cases are applied to each pair oflines in turn until either an intersection is discovered, or it is determinedthat the lines do not cross anywhere in space. Please refer to Figure A.9,which depicts visually all the possible cases.

Two arbitrary lines crossing in space

The first is the most general case, two arbitrary lines crossing in space.This case covers pairs of lines where neither line is horizontal or vertical.It does however cover two parallel lines in any orientation.Equation (A.5) gives the general equation for a line, we are looking forvalues of x and y that simultaneously solve the equation for either line.

77


We can once again use Equation (A.2) to calculate the slope of the line,and using the slope calculate the y intercept (b). Once we have solvedthese variables for each line (which are stored as a pair of points) wecan attempt to find the intersection points. Equation (A.6) shows theequations we wish to solve simultaneously, m1 and b1 are the slope andintercept of the first line respectively, m2 and b2 for the second line. Ifeither the calculated value for b or the calculated value for m are infinite,we know the line is either horizontal or vertical, so this case does not apply.The condition we are looking for is a value of x and y that solves bothequations. We therefore have a set of four equations in four unknowns,Equation (A.7) shows these equations re-arranged having constants on theleft hand side, and the variables we wish to solve on the right. It is now asimple matter of solving the matrix presented in Equation (A.8), it can beseen that a pair of parallel lines will produce an infinite value due to thefact that two parallel lines have the same slope, and therefore:

b2 − b1

m1 −m2

=b2 − b1

0= ∞

If the infinite result is generated we know the lines have no intercept andcan move on to the next pair. Otherwise the intersection point can begiven as :

xintercept =b2 − b1

m1 −m2

yintercept =b2 ·m1 − b1 ·m2

m1 −m2

Finally a check need be made that the point is within the bounds of theimage, and that it is close to the end of one of the lines. Since this iscommon to all cases it will be described in section A.3.6

y = m · x + b (A.5)

y1 = m1 · x1 + b1

y2 = m2 · x2 + b2

x1 = x2

y1 = y2

(A.6)

−b1 = m1 · x1 − y1

−b2 = m2 · x2 + y2

x1 = x2

y1 = y2

(A.7)

78


RREF

m1 0 −1 0 −b1

0 m2 0 −1 −b2

1 −1 0 0 00 0 1 −1 0

⇒

1 0 0 0 b2−b1

m1−m2

0 1 0 0 b2−b1m1−m2

0 0 1 0 b2·m1−b1·m2

m1−m2

0 0 0 1 b2·m1−b1·m2

m1−m2

(A.8)

One horizontal line, and one vertical line

This is the easiest case to handle, and can be identified by one line havingan infinite slope, and the other having an infinite y intercept. The pointsof intersection can be extracted from the lines, the vertical line providesthe x intercept, and the horizontal line provides the y. This case is testedfor before either of the following two, as a pair entering this case wouldalso fall into the other two.


yintercept = Y component of the horizontal line.

Once again the tests given in section A.3.6 are applied to test the validityof the corner.

One horizontal line, and one arbitrary line

In this case one line is horizontal, while the other is non horizontal orvertical. In this case the horizontal line contributes the value of the ycomponent, and the x value must be calculated. This can easily be doneusing Equation (A.5), the slope and intercept values coming from the non-horizontal line. Once again the tests of Section A.3.6 must be applied toverify the correctness of the line.

yintercept = y component of the horizontal line.

xintercept =yintercept − b

m

One vertical line, and one arbitrary line

The final case is the one in which one line is vertical and the other is nonhorizontal nor vertical. In this final case the vertical line contributes thex component, and the y value must be calculated in a similar manner tosection A.3.6.


yintercept = m · xintercept + b

79


Removing unwanted intersections

We have now calculated an intersection point between the line pair we arecurrently examining, the next step is to test the validity of this point. Thefirst test is to verify that it exists within the bounds of the image, that is,that the point falls within the following range:

0 ≥xintercept ≥ 320

0 ≥yintercept ≥ 240

Next we need to verify that the point exists on or near one of the lines usedto detect it. This is done by calculating the euclidian distance between thepoint and both of the line segments4. A first quick test is done usingEquation A.9 to measure the distance between the endpoints of both linesand the point in question. If the point does not fall within the maximumrange (this can be due to the fact the point is on the line, but far awayfrom either endpoint) we must make the more expensive calculation todetermine the distance between the point and the line segment. To do thiswe must first calculate the projection of the point onto the line segment(note this technique is valid in n dimensions), this is done using Equation(A.10). The next step depends on where the point projected onto the linesegment, Equation (A.11) determines the final distance. If the point existson the line, the distance will be zero, otherwise it represents the shortestdistance between the point and the line. Once again this is compared tothe the threshold, if it is less the point is added to the set of valid corners,otherwise it is discarded.

vector distance = ‖p2 − p1‖ (A.9)

to =~M · (~P − ~B)

~M · ~M(A.10)

D =

‖~P − ~B‖ to ≤ 0

‖~P − ( ~B + to · ~M)‖ 0 < to < 1

‖~P − ( ~B + ~M)‖ to ≥ 1

(A.11)

Resulting system

Appendix ?? shows the bulk of the algorithm used to find lines, and Ap-pendix ?? shows the core of the find corners algorithm. Neither of these

4The algorithm used was based on the paper by :David Eberly available at :http://www.magic-software.com/Documentation/DistancePointLine.pdf

80


code segments are complete, but are presented here to give an idea howthe software operates.Table A.1 shows the results of running the red layer of a static imagethrough the algorithm. It can be seen that the system can pick out quitewell only the valid corner points, ignoring many of the noise points shownin Figure A.6. Note: The corners are indicated by the red squares in theimages.

A.3.7 Next steps

The next step is to perfect the merge set algorithm. After that the cal-culation of distance to the point will be relatively simple, as it is welldocumented. Finally, time permitting the path planning algorithms will bedeveloped.

A.3.8 Problems encountered

Many problems have been encountered during the development of thissystem to date.

1. The low cost cameras produce images of such poor contrast thatblack and white cameras may have been a better choice

2. The original plan to implement the core of the system using theIntel image processing libraries failed due to the fact that the displayfunctions did not behave as anticipated, and a Linux native GUIwould have to have been developed simply to display the imagesduring development.

3. Both development hardware platforms (the primary and the backup)have failed during development, and only one has been repaired atthe time of this report.

A.3.9 Project outlook

Due to the hardware failures the project is at least two weeks behindschedule, this will eat up the padding time left at the end of the allotteddevelopment plan. The hope is to have the whole system implemented bythe demo date, but if this is not possible, the system will be able to identifyand place objects in space at a minimum. The final steps are much betterunderstood then the work that has already been completed, and thereforehopefully will go much smoother.

81


Table A.1: Progression of images through Local intersection of lines algo-rithm.

Initial image

Prewitt edge detect

Line detect

Corner Detect82

Documents

CENG 499B Final Report Image based path mapping system for ...elec499/2004b/group01/docs/final.pdf · CENG 499B Final Report Image based path mapping system for autonomous robotics