Adaptation of Controllers for Image-Based Homing - CiteSeerX

Adaptation of Controllers for Image-Based Homing Markos Zampoglou

Master of Science School of Informatics University of Edinburgh 2005

1

2

Abstract Following a study of the existing algorithms for image-based homing, and their strengths and weaknesses, a new, adaptive controller was designed. The controller achieves homing through gradient descent of the RMS surface by modelling C. Elegans chemotaxis with a first-order function. The controller parameters were evolved in a simulated environment, and were then tested and evaluated both in simulation and in the real world. A comparison also took place between this and other, established algorithms. The simulation and real world results indicate that homing through gradient descent does offer some advantages over the traditional homing algorithms. They also demonstrate that the taxis algorithm is a very efficient way to take advantage of the properties of the RMS surface in a known environment, although further research will be necessary before the controller becomes fully applicable.

3

Acknowledgements I would like to than Dr. Barbara Webb, my supervisor, for her guidance and advice throughout the course of this project. I would also like to thank Matthew Szenher, for his support, his eagerness to help and his insightful comments on all stages of my work.

4

Declaration I declare that this thesis was composed by myself, that the work contained herein is my own except where explicitly stated otherwise in the text, and that this work has not been submitted for any other degree or professional qualification except as specified. (Markos Zampoglou)

5

6

Table of Contents 1

Introduction ................................................................................................... 10 1.1 Overview ................................................................................................... 10 1.2 Existing approaches................................................................................ 10 1.3 Using a difference surface ..................................................................... 14 1.4 Gradient descent methods..................................................................... 15 1.5 A biologically inspired approach to gradient descent ........................ 17 1.6 Project aims.............................................................................................. 20 2 Methods .......................................................................................................... 21 2.1 Overview ................................................................................................... 21 2.2 The RMS surface for indoor environments ......................................... 21 2.3 Choice of the controller and adaptation method................................. 23 2.4 Implementation of the Genetic Algorithm ............................................ 24 2.4.1 Encoding ........................................................................................... 25 2.4.2 Evaluation function .......................................................................... 25 2.4.3 Evaluation in a simulated environment ........................................ 26 2.4.4 Selection Method............................................................................. 29 2.4.5 Crossover and mutation ................................................................. 30 3 Results ............................................................................................................ 31 3.1 Overview ................................................................................................... 31 3.2 Running the GA ....................................................................................... 31 3.3 Behavioural interpretation of the parameters...................................... 32 3.4 Evaluation of the evolved controllers in simulation ............................ 35 3.5 Evaluation in the real world.................................................................... 44 3.5.1 Overview ........................................................................................... 44 3.5.2 Robot setup and control code ....................................................... 45 3.5.2.1 The taxis algorithm .................................................................. 46 3.5.2.2 Image warping ......................................................................... 47 3.5.2.3 RunDown .................................................................................. 47 3.5.3 Characteristics of the RMS surface in the real world................. 48 3.5.4 Initial results ..................................................................................... 53 3.5.5 Adapting the parameters to the real-world surfaces.................. 53 3.5.5.1 Estimating the real-world surfaces ....................................... 53 3.5.5.2 On-line optimization results ................................................... 53 3.5.6 Real world runs ................................................................................ 54 3.5.6.1 Short-range homing in a bright static environment ............ 54 3.5.6.2 Long-range homing in a bright static environment............. 56

7 3.5.6.3 Moving landmarks ................................................................... 58 3.5.6.4 Placing an object close to the home position...................... 61 3.5.7 The size of the catchment area..................................................... 64 3.5.8 Computational complexity .............................................................. 65 4 Conclusion ..................................................................................................... 67 4.1 Project Summary ................................................................................. 67 4.2 Conclusions .......................................................................................... 68 Bibliography ........................................................................................................... 72 Appendix A: Matlab Code ................................................................................... 75 Appendix B: C Code ............................................................................................. 84

8

List of Figures Figure 1.1: The homing task ...................................................................................... 11 Figure 1.2: Capturing a horizon line .......................................................................... 11 Figure 1.3: The simplified artificial neural network model presented by Feree & Lockery. ............................................................................................................. 18 Figure 2.1: An image from the ‘chairs’ dataset (left) and the same image from the ‘twilight’ dataset (right) ..................................................................................... 22 Figure 2.2: Three characteristic RMS difference surfaces......................................... 23 Figure 3.1: A sample run with each controller on surface #5 .................................... 36 Figure 3.2: The mean number of steps for each surface ............................................ 38 Figure 3.3: The mean distance travelled for each surface.......................................... 39 Figure 3.5: The mean homing accuracy for each surface. ......................................... 40 Figure 3.6: Surface #7 from the evaluation runs........................................................ 41 Figure 3.7: A RunDown homing run on surface #8................................................... 42 Figure 3.8: The Koala robot used in the real-world experiments .............................. 46 Figure 3.9: RMS values for different situations......................................................... 49 Figure 3.10: The change on the RMS surface when the agent happens to pass too close to an object................................................................................................ 49 Figure 3.11: The angle estimate at various distances (in metres) from the home position............................................................................................................... 51 Figure 3.12: The RMS difference for the same route and home position, with the snapshot captured at different orientations ........................................................ 52 Figure 3.13: The RMS difference for all theoretical rotations on two images, taken from the home position with different orientations............................................ 52 Figure 3.14: One of the successful Taxis homing runs.............................................. 58 Figure 3.15: The moved landmarks experiment ........................................................ 59 Figure 3.16: The moved chairs scene......................................................................... 61 Figure 3.17: The lab area where the cardboard box was placed ................................ 62 Figure 3.18: The three RMS lines, before and after inserting the object ................... 63

9

List of Tables Table 3.1: The three controllers evolved.................................................................... 37 Table 3.2: The surfaces on which the controllers were tested ................................... 37 Table 3.3: The total success rate for each controller, on all 9000 homing runs, and the mean (variance) number of steps per run........................................................... 37 Table 3.4: The success rate for each controller and each surface, 1000 runs per surface ................................................................................................................ 38 Table 3.5: The results for 10 short-range homing runs (1 metre away), in a clear, bright area with no movement ........................................................................... 56 Table 3.6: The results for 10 long range (2 metres away) homing runs in a clear, bright area with no movement. .......................................................................... 56 Table 3.7: The results for 10 homing runs after moving the landmarks .................... 60 Table 3.8: The results for 5 runs with an object placed next to the home position. .. 64

10

Chapter 1 Introduction 1.1

Overview The goal of this project was to design and implement an adapting controller

for the purpose of image-based homing. The problem of visual homing can be stated as in (Szenher, 2004): An agent, equipped with a monocular camera and (usually) a compass, is located at a point S. The goal is for the agent to move to a point C, using a snapshot image taken from location C, the orientation information from the compass and the current visual information from the camera. (Fig. 1.1).

1.2

Existing approaches

The existing visual homing approaches can be divided in two groups: Featurebased and image-based methods. Most methods in both groups use similar visual information (Franz & Mallot, 2000). A spherical mirror is located above a camera pointing upwards. This gives an omnidirectional image of the environment. Assuming movement takes place on a flat surface, there is a circle on the mirror, centred on its central vertical axis, where the angle between the line of sight and the surface normal of the mirror is equal to the angle between the horizon plane and the mirror surface. For points projected on this circle, the line of sight is reflected on a direction parallel to the horizon plane. If the projection of an object falls on this circle, the projections of these objects will change their bearing in the circle as the

11 agent moves, but will never leave this circle. (Fig. 1.2). The visual information used in most approaches is the one-dimensional image created by unfolding this circle into a line. It is possible, however for homing methods to use 2-D images.

Figure 1.1: The homing task is moving the agent from S to C. Visual information can be the relative location of the projection landmark L.

Figure 1.2: Capturing a horizon line. If MA is parallel to the horizon plane, the projection of A will change it’s bearing in a circle, but will never move out of it. Feature-based methods usually extract a number of features from the snapshot image, and each current image. Feature matching is performed between the current and the snapshot image features. By comparing the relative bearing and size of corresponding features, a movement vector is extracted. In image-based methods, no segmentation or feature extraction takes place. Instead, the whole current image is compared to the snapshot image to determine the movement vector. In most of these algorithms orientation is assumed to be constant and identical to that of the snapshot image. It is very hard to actually maintain constant orientation with a moving robot.

12 However, with the use of a compass, the orientation can be determined in each step, and adjusted to match the home orientation. The two most commonly cited methods for feature-based homing are the snapshot model (Cartwright and Collett, 1983), (Hong et al 1992), and the Average Landmark Vector (ALV) model (Lambrinos et al, 2000), (Moller, 2000). In the original snapshot model, feature matching is performed based on bearing, and a unit length vector is estimated for each feature pair, orthogonal to the current image feature’s bearing. The sum of all the resulting vectors is the movement vector for the next step. The constant orientation assumption is used. A large number of variations for the snapshot model can be found in literature. Weber et al (1998) suggested taking into consideration the relative size of each feature, and computing the length of each vector accordingly, instead of assigning unit length to all. Rofer used greyscale instead of binary images and a Kohonen network for feature matching (1995), and perfected the method using colour images (1997). Gourichon et al (2002) also suggested using coloured snapshots to reduce matching errors. Finally, Benhamou et al (1995) created an approach where the agent predicted the change in feature locations for each possible move, and chose the one that minimized the total difference. In the ALV model, the bearings of all features in the reference image are calculated, and their vector sum is the only reference information kept. At each location, the bearings of all features are summed, and the difference between the home vector and the current one defines the motion direction for the next step. Concerning image-based homing, the image warping model suggested by Franz et al (1998) is such an approach that does not rely on the constant orientation assumption. However, the image warping model works under the assumption that all landmarks are situated at equal distances from the home location. Using a greyscale one-dimensional image taken from the current location, the model calculates all possible deformations of the image line, for all possible rotations and movement vectors. The agent then moves according to the movement vector which produced the image which best resembled the snapshot image. The measure of resemblance suggested by Franz et al is the dot product between the two lines, after they have been normalized by subtracting their mean from them.

13 Given rotation ψ, angle of the home vector α and the ratio of the agent’s distance from the home position d by the home position’s distance from the landmarks R (assumed equal for all) ρ=d/R, a pixel located at angle θ in the current image will be displaced by  ρ sin (θ − α )   − ψ (1)  1 − ρ cos(θ − α ) 

δ = arctan

By shifting all pixels by their respective δ values, we can build a reasonable approximation of the image, had the agent moved and rotated according to α and ψ, and had ρ been a good estimate of the actual agent and landmark distances. The dependence of feature-based homing algorithms on the accurate detection and localization of landmarks, combined with their need for constant orientation are their greatest weaknesses. Although feature extraction is easy in artificial environments, it is hard to track the landmarks in natural environments, both indoor and outdoor, having to deal with illumination changes, occlusion, and, often, the actual absence of clear shapes that can be used as landmarks. The need for constant orientation can be solved by using a compass, which makes this issue secondary. Image warping, being an image-based method, is significally more robust in the matching phase. However, the fundamental assumption that all landmarks are located at equal distances from the home position is problematic. It is clear that it does not hold in most, if not all cases. Up to a certain level, this is not a problem, since the resulting error is small and has been proven to decrease as the agent approaches the home position (Franz et al 1998). It is possible however, that, if the distances between the landmarks and the home positions vary too much, warping may not be able to find the best home vector. Additionally, being a brute force solution, image warping is extremely expensive in terms of computation.

14

1.3

Using a difference surface Recently, Zeil et al (2003) studied the properties of outdoor scenes for the

purpose of image homing. Using a panoramic camera, they took a panoramic image, which was used as the snapshot image. By capturing similar images from various positions in 3-D space and for different times of day, they were able to come to a number of significant conclusions. Instead of extracting the horizon line, the whole unfolded images were extracted, and then compared to the snapshot image using the root mean square difference function. That is, the difference between the current image and the snapshot image was determined by:

M

RMS =

N

∑∑ (I i =1 j =1

C

(i, j ) − I S (i, j ) ) M ×N

2

, (2)

where IC(i, j) the intensity function for pixel (i, j) in the current image, IS(i, j) the intensity function for pixel (i, j) in the snapshot image, and M×N the dimensions of the images. This function applies to greyscale images, but can be generalized for colour images, if needed. Zeil et al estimated the RMS difference for a grid of camera positions (x,y,z) in a given area, compared to a snapshot image taken from the centre of the area. The 3-dimensional function resulting from the (x,y) pairs and the corresponding RMS values will be referred to as the RMS difference surface. The resulting difference surface came up unimodal in both dimensions, for a small area around the home position. That is, in dimensions (x,y), movement towards the home position resulted in gradual reduction of the difference, reaching a global minimum in the target location, with no local minima whatsoever. The size of the area for which this held true (the “catchment area”) was up to 3m for the specific experiment. The same characteristic applied to the dimension z as well as rotation. Regardless of the relative position of the agent with respect to the goal position, the RMS difference was reduced as the current height approached the height at which the snapshot image was taken, and was also reduced as the current orientation

15 approached the orientation of the snapshot image. In both cases, the RMS difference reached a global minimum when the current and snapshot coordinates matched. Illumination changes caused by the sun’s movement resulted in disturbance of the catchment area. This can be attributed to the appearance and movement of shadows, rather than the change in the environment illumination, as illumination should change uniformly and not disturb the unimodality of the difference function. Cloud movements had the same effect, as the covering and revealing of the sun resulted in shadows disappearing and reappearing, respectively. In these cases, local maxima appeared on the difference surface, and sometimes the global minimum even appeared in a different location than the home position. As for the effects of background movement such as wind-driven vegetation, the temporal resolution of the images collected was not high enough to perform a study. As the authors suggested, it is possible to take advantage of the unimodal property of the catchment area to achieve image-based homing. The agent can determine its orientation by turning (or simply transforming the image) until the minimum RMS difference is encountered and move towards the home location by descending towards the point that gives the minimum RMS difference. The homing issue in this case can be regarded as a gradient descent problem. Using a gradient descent algorithm, both the orientation determination and the movement in the direction of the home location can be achieved.

1.4

Gradient descent methods The gradient descent problem to be solved has certain differences compared

to the typical form encountered in bibliography. In our case, the only data available is the RMS difference at the present position, as well as the RMS differences encountered in the past. The agent has no knowledge of its current (x,y) position, and the RMS values do not follow a certain function of (x,y), and as a result the future values cannot be predicted, nor are we able to estimate the local gradient through a derivative. Thus, the problem now is, given the current and the past RMS differences, how to move in each step in order to reduce the RMS difference.

16 The most popular gradient descent algorithms currently in use can be categorized in two groups. The Steepest Descent (Mitchell, 1997, pg. 89) algorithm and its variations work by calculating the partial derivative of the input function with respect to all dimensions and using it to estimate the direction of the steepest descent. On the other hand, the Conjugate Gradient Method (Mathworld, 04/8/05) performs descent by first moving in a random direction, and then moving in orthogonal directions for the distance that minimizes the input. That is, from a position (x, y), where the input is (x, y) the vector (d1, d2) is determined, where d1 and d2 can be either positive or negative, that minimizes f(x+d1, y+ d2). In the absence of local minima or maxima, this algorithm homes in a number of steps equal to the number of dimensions. There exist a large number of variations of these two algorithms such as the stochastic gradient descent (Kivinen et al, 2004), (Yang & Amari, 1996), the natural gradient descent for training neural networks (Ibnkahla & Yuan, 2003), (Ibnkahla, 2002), adaptive algorithms (Chatterjee et al, 2000), or hybrid algorithms (RoyChowdhury et al, 2000), (Wong & Chen 1999). None of them is particularly appropriate for visual homing, since they either need to estimate the partial derivatives, which is impossible since the only information available is the value of the function at a particular point, or they need to know how far to move until the function starts to increase again, which again is impossible. To test the truth of their observations, Zeil et al applied a variation of each of the two gradient descent methods, adapted to the particularities of the problem. The first one, “RunDown”, moves the agent in a random direction in small steps, capturing pictures, until the RMS difference starts to increase. The algorithm then selects a new direction, orthogonal to the previous one and moves the agent towards it. This is an implementation of conjugate gradient, only the precision at which the optimal distance to move is estimated is limited by the step length. The second, “Triangular”, takes images from three different positions (for 2-D gradient descent). The mean square difference is calculated for all three images. By comparing the three RMS values, it is possible to calculate a vector pointing towards the direction of the maximal gradient. The agent moves to the direction of the vector, and repeats the procedure. This algorithm actually uses the same idea as steepest descent, and

17 estimates an approximation of the local gradient at a point by getting three RMS differences around it. Although these algorithms are sufficient to test the properties of the catchment area, they are quite unreliable for application in actual homing. The first algorithm, by moving in random directions until the gradient starts to increase, will probably take steps in the wrong direction, and then spend some time trying to find the next best choice of direction. The second one spends quite a long time determining the gradient in a position, before choosing the appropriate direction. Also, Zeil et al observed that illumination changes can lead to the presence of local minima or maxima. Both of these algorithms can be easily distracted by local minima and then have to spend time recovering their course.

1.5

A biologically inspired approach to gradient

descent An alternative gradient descent method that should work under these constraints can be derived from a biologically inspired approach. Chemotaxis is a process used by some organisms to locate sources of food. Ferree and Lockery (1999) define it as “oriented movement in response to a chemical gradient”. In effect, during chemotaxis, an organism performs gradient ascent from an area of low concentration of a chemical substance, to the global maximum of concentration. Chemotaxis is used by organisms such as C. Elegans that use a single sensor to detect only the current concentration of the chemical substance, and thus solve an analogous problem. In (Ferree & Lockery, 1999), the neural network that controls the behaviour of C. elegans is studied, and the underlying computational rules are extracted. The biological network of 13 pairs of neurons is first modelled with a 6 neuron artificial neural network. This network has one input and two output neurons, and all connections are recurrent (Fig. 1.3).

18

Figure 1.3: The simplified artificial neural network model presented by Feree & Lockery. The presence of only one input neuron suggests that C. Elegans calculates gradients during movement, which is exactly what would be needed in the case of visual homing. Although C. elegans movement is sinusoidal, for simplicity, during the extraction of the computational rules, movement was considered straightforward, with only right or left turns. The network’s output from a biological point of view, are two voltage signals sent to the Ventral and Dorsal muscles to determine the nematode’s movement. In the simplified model, the turn rate of the nematode is determined by

dθ = γ (VD − VV ) , dt

(3)

where VD and VV the network output values and γ is a parameter whose value is determined by the particular nematodes physiology, such as body length and diameter. Movement is defined by

dx dy = υ cos θ , = υ sin θ , (4) dt dt where υ is the nematode’s forward moving speed. The network was linearized, so the resulting function for each neuron was

19 N dVi = ∑ AijV j + bi + ci (t ) (5) dt j =1

where Vi the voltage of neuron i, Aij the synaptic weight for synapse (i→j), bi the bias for neuron i and

ci (t ) = δ i1

κ sens C icell

C (t ) (6)

where δij is the Kronecker delta, C(t) is the input at time t, κsens is a constant parameter and C icell the whole-cell capacitance for neuron i.

dθ dC d 2C = Ω bias + z o C (t ) + z1 + z 2 2 + ... , (7) dt dt dt where C is the input from the sensor, and the parameters Ωbias, z0, z1, z2 etc are defined by the nematode’s physiology. This model suggests that second and higher order derivatives affect the nematode’s behaviour during chemotaxis. Experiments with various models, however, showed that this particular model can perform chemotaxis using only the constant, the actual input and the first derivative of the input. That is, the behaviour achieved by the neural network can be modelled by

dθ dC = Ω bias + z o C (t ) + z1 (8) dt dt However, the coefficients determine the behaviour of the model, and its ability to move in response to the gradient of the input. From this point of view, the function has to be optimized with respect to its ability to home. It is also possible that the same parameter set will not behave optimally in all environments, and a new parameter set will have to be estimated for each home position. Both the network in Figure 1.3 and equation (7) result in the same behaviour after optimization of their parameters. Both options could be used to achieve image

20 based homing, using the properties of the catchment area as described by Zeil et al. Two more options were considered as possible models for gradient descent using a single input. One was a model of C. Elegans chemotaxis presented by Dunn et al (2003), where a small neural network takes the chemical concentration as input and gives a binary output. The output makes the nematode move in a fairly straight course (named a “run” by Ferree and Lockery (1999)) while the concentration increases or perform a series of random sharp turns (“tumbles”) when the concentration decreases. The other model was bacteria chemotaxis, as described by Muller et al (2002). The agent in bacteria chemotaxis moves in straight segments of random length followed by turns in random angles. The distribution of the segment lengths and the turning angles is determined by the current and previous inputs. Both these models are partly stochastic. Dunn et al’s C. Elegans model is less dependent on random variables, but is extremely simple and its behaviour on a homing scheme would be much more unreliable than the deterministic C. Elegans model described by Ferree and Lockery. Therefore, the approach that was followed was to optimize Ferree and Lockery’s C. Elegans model to descend the RMS difference space of the catchment area.

1.6

Project aims In the light of what has been stated so far, we are now in position to clearly

formulate the issue to be addressed: The parameters of the C. Elegans controller described in (Ferree & Lockery, 1999) are to be optimized in order for it to be used for homing by gradient descent in the RMS difference surface. After choosing the most appropriate function form of the ones described by Ferree and Lockery, a suitable optimization method will have to be found, and the resulting controller is to be tested on simulation and in the real world. A comparison to other existing homing algorithms is also necessary, in order to evaluate the applicability and usefulness of our controller.

21

Chapter 2 Methods 2.1

Overview In this section, the controller implementation and optimization is described in

detail. In section 2.1 a short study of the properties of the image dataset used in the simulation is done, to confirm the claims of Zeil et al. Section 2.2 describes the control function and the optimization method chosen, and the reasons for the choice. Finally, in section 2.3, the optimization method is described in full detail, and the simulation built for the optimization purposes as well.

2.2

The RMS surface for indoor environments Before the controller optimization took place, the properties of the catchment

areas in indoor areas had to be tested, since Zeil et al had performed their study in an outdoor environment. A number of datasets were available, courtesy of Andrew Vardy, McGill, Canada, consisting of the same 3 x 5.1m area (an office room), three of them with the same pictures captured under different illumination levels (‘Original’, ‘Twilight’, ‘Night’), and another one where a number of chairs had been added to the room, causing anomalies in the shape of the RMS difference surface (‘Chairs’) (Fig. 2.1). The images corresponded to a real world grid with 30 cm distances between every two consecutive images.

22

Figure 2.1: An image from the ‘chairs’ dataset (left) and the same image from the ‘twilight’ dataset (right)

For a controller to be applicable, we have to ensure that homing can be achieved not only when the current illumination conditions are identical to the conditions under which the snapshot image was captured, but also when there are significant differences between the two. That is, the agent would ideally be able to home in a dark room using a snapshot taken in full daylight. To cover all possibilities, all possible RMS surfaces were formed, both by comparing a snapshot to the image set it was drawn from (e.g. a snapshot from the ‘Original’ set, compared to all the images in the ‘Original’ dataset) and comparing a snapshot to a different image set (e.g. a snapshot image from the ‘Original’ dataset, subtracted from all the images of the ‘Night’ dataset). The fundamental property of the global minimum appearing on the home position did hold true for all surfaces (Fig. 2.2A). Excluding the ‘chairs’ dataset, the unimodality of the surface also held true. All surfaces that included the ‘chairs’ dataset, as their current images, although they retained their general form, always had a number of local optima at places (Fig. 2.2B). At this point, it interesting to note that, when using a snapshot image from the ‘Chairs’ set and current images from the same set, local minima and maxima still appear (Fig. 2.2C). What’s special about this set is that the objects (chairs) are close enough to the camera, so that moving the agent causes significant deformations that disrupt the catchment area. In any case, the general shape of the surface suggests that homing could indeed be achieved, even with significant changes in the environment. What these datasets could not model, however, was the disturbance caused by objects moving during homing, such as people moving around, which is a common case in natural

23 environments. Also, since all the images were captured under the same orientation, they could not model the effects of rotation.

B

A

C Figure 2.2: Three characteristic RMS difference surfaces. A: A snapshot image from ‘Original’ subtracted from the ‘Original’ set. B: The same snapshot image subtracted from the ‘Chairs’ set. C: A snapshot from the ‘Chairs’ set subtracted from the ‘Chairs’ set.

2.3

Choice of the controller and adaptation method Ferree & Lockery (1999) present two models for nematode chemotaxis,

which essentially represent the same function in two different forms. The first one is a neural network controller, and the output function of each neuron is (5). The network has 44 parameters in total (Αij and bi for i,j=(1..6), u, κsens / C1cell ). The second mode; presented was the one described by equation (8), which was extracted from the neural network. This controller, keeping the terms up to the first derivative and counting the movement speed as well, was described by 4 parameters.

24 The initial intention was to train the neural network controller based on the desired behaviour we want to achieve. Supervised learning, however, can work only by giving the right answer for a given input and comparing it with the actual network output. This sort of information was unavailable. The C. Elegans moves in loops, during which it often moves significally up the gradient. This rules out the option of considering as correct the output that forces the agent to turn towards the direction of greatest descent. One possibility was to allow the agent to move for a number of steps and evaluate the average result of the behaviour, instead of a step-by-step evaluation. Even so, however, offering a ‘correct’ response to the inputs that constituted such a course was virtually impossible. In effect, all we knew we wanted was the agent to reach the home position in as few steps as possible. This sort of information clearly points out to either a reinforcement learning approach or a genetic algorithm. What these two approaches have in common is the fact that the information available concerns what we want the agent to do, but not how we want it to do it. Of these two options, the GA approach was chosen, since the nature of the controller (i.e. a function, either in the form of a neural network or otherwise) was very easy to optimize through a genetic algorithm. After the decision was taken to follow the evolutionary approach, the model to be implemented was reconsidered. Ferree and Lockery had shown that the neural network’s behaviour could be modelled by (8) without any apparent divergence from the behaviour achieved by the neural network. This function has a significant advantage from an evolutionary point of view: The number of parameters to be optimized is much smaller than the network’s (i.e. 4 including the movement speed compared to 44 for the neural network). Besides the fact that fewer parameters make the function easier to optimize, another advantage is that the resulting parameter set can be interpreted in behavioural terms much more easily than the corresponding neural network.

2.4

Implementation of the Genetic Algorithm When using GA to optimize a parameter set, there are a number of issues to

be decided (Mitchell, 1998, pg. 10): The population encoding, the evaluation

25 function, the selection method, the crossover method and the mutation method. Furthermore a number of parameters also have to be defined, including the population size, the crossover probability, and the mutation probability.

2.4.1 Encoding The most common encoding scheme is a bitstring, whose length depends on the number and the range of the variables to be optimized. In our case, however, this method would result in a restriction to the decimal precision of the variables, which is completely undesirable. Instead, each individual was encoded as four real numbers, [Ωbias Z0 Z1 u] giving the minimal range and precision restrictions.

2.4.2 Evaluation function Choosing the evaluation function is probably the most important issue in a GA, since it determines the goal of the algorithm, what we want the controller to do. In our case, the goal was clear: home in as few steps as possible. A simple way to quantify this, was to have each individual perform a run of maximum 500 steps, and define the value function as 500-steps, where steps is the number of steps taken until the agent reached the home position. Thus, the maximum value of an individual was 499. Individuals that failed to home, either by moving out of the simulation boundaries or by moving in pointless circles in the simulation space were given a value of 1 instead of 0. This, in almost all cases means that from one generation to the next only the individuals that were able to home in the first place will survive. Generally, simply scrapping the non-fit individuals is bad practice in GAs. In our case, however, due to the fundamental simplicity of the control function and the task, we can be sure that, in an adequately large randomly generated initial population, enough individuals will be already capable of homing, although probably not efficiently enough. The population size was experimentally chosen to be 200, being large enough to ensure that enough individuals will initially be able to home, and still small enough for evaluation to be computationally feasible. The value 1 is given to the individuals who cannot home instead of 0, just to ensure that, in the extreme case

26 where too few individuals are able to home, population diversity will be kept. In truth, that was never the case, although the GA had to be run several times during the project progress.

2.4.3 Evaluation in a simulated environment For the homing runs needed for the evaluation, a simulation of the environment and the agent’s behaviour in it was created. The reason for this is the speed advantage of simulated trials as compared to real-world trial runs. Parameter sets can be applied and evaluated at a much greater speed in a simulation than in the real world. However, the issue always to keep in mind when using a simulation to train or evolve a controller for real-world use is the resulting controller’s ability to generalize what it has learned, and achieve similar behaviour in the real world as in the simulation. To this end, the methodology described by Jacobi (1997) in his “Radical Envelope of Noise Hypothesis” was followed. According to that methodology, there are three steps to be taken: •

Identify a base set of robot-environment interactions, which have a

basis in reality, and explicitly separate them from the implementation aspects. •

Ensure that the implementation aspects vary significally from

evaluation to evaluation, so that the controller learns to ignore them and focus on the base set (i.e. ensure the simulation is base set exclusive). •

Ensure that every aspect of the base set varies slightly from evaluation

to evaluation so that the controller is forced to cope with the differences between the simulation and the real world which result from noise (i.e. ensure the simulation is base set robust). In our case, the base set consisted of only two interactions: The RMS difference input from the world, and the robot’s movement in it. The “world” in which the virtual agent moved was a surface of RMS differences. The images provided by Andy Vardy were used for that purpose. Before

27 the start of each evaluation, the home position was chosen, the images were unfolded and the RMS surface was created, in the form of a 2-D matrix. To normalize the input in roughly (0,1), a technique used by Zeil et al was used. The snapshot image had its pixels shuffled, so that a new random image was created, having the same pixel value distribution as the snapshot image but showing the maximum possible difference from it concerning the pixels positions. The RMS difference between the home and the random image was calculated, giving an estimate of the maximum RMS difference for that dataset. All RMS differences were then normalized through division by that maximum value. At each simulation time step, the agent reads the current normalized RMS difference from the grid position corresponding to its actual position, estimates the new turning angle using equation (8) and moves forward by U, where U the movement speed of the agent (the fourth parameter). In this point, a small alteration to the initial chemotaxis model was done: As in all forms of gradient descent or ascent, a decreasing step size is desirable, so that the agent can approach the goal rapidly and then slow down to locate it with the desired accuracy. This was achieved by moving the agent by CxU at each time step, instead of simply U, where C is the current input (normalized RMS diff.). Following the second principle from Jakobi’s hypothesis, the start position was then chosen randomly, at a specific distance R from the home position. The fixed distance R ensured equal opportunities for all individuals, while the fact that the start position was chosen randomly from the circle defined by R prevented possible adapting of the controller to a particular start position. Another issue that falls in the same category is the shape of the RMS surface. In the Informatics Research Proposal (Zampoglou, 2005) one possibility that was taken into consideration was that the difference surface might differ from one environment to the other concerning the steepness, the value range and the presence of local minima or maxima. It was also considered possible that the same controller (parameter set) would lead to different behaviours depending on the surface. To prevent adaptation to one type of surface, a number of different surfaces were used for evaluation. The various difference surfaces were built by taking a snapshot image from one dataset and comparing it with the images of another.

28 Finally, for the third step of Jakobi's hypothesis, gaussian noise was added to the RMS input, as well as the agent’s turning angle and displacement at each time step. The final parameters of the noise distribution were taken from the actual koala robot that was to be used for the real world experiments, and were N(0, 0.002) for the RMS input and N(0,0.0005) for both the turning angle (in rads) and the displacement (in the simulation 1 distance unit = 0.3 metres). In truth, the real world robot never gave any measurable error in either turning or moving, but some small noise was assumed to exist in both. One issue to be resolved was the density of the images in the dataset. The grid had images at 30 cm intervals. In most cases, the agent found itself in between images. Rounding the agent’s position to the closest image position led to a very crude and inaccurate simulation. The solution was given by interpolation. Since the map consisted of normalized RMS differences, it was very easy to take the three nearest positions, extract the plane function from their respective RMS values and estimate the value for the intermediate point. The underlying assumption was that the RMS value changed smoothly for these 30 cm between two images. Since all test results pointed towards this direction, there was no reason to assume otherwise. The usual Gaussian noise was added to the result of the interpolation. For the evaluation of individuals, after a number of RMS surfaces had been created, each individual was forced to perform one homing run on each surface. The start position was chosen randomly as described above, although the distance R from the home position was always the same. That did not seem to lead to any form of overfitting, since the variation in the RMS surfaces meant that the agent had to deal with different input patterns, although the actual distance from the goal was the same. Using just one run per surface from a random start position is probably insufficient to fully evaluate an individual. Because of the randomness involved, a complete evaluation would require several runs on each map, and statistical processing of the results. However, according to Fitzpatrick and Grefenstette (1988), when simulating noisy environments and dealing with such data, where evaluation is in fact sampling, it is preferable to superficially evaluate a large number of individuals than to perform exhaustive evaluation over a small range of individuals.

29 That is, for the same total number of evaluations, it is preferable to have a large population with few tests runs each, than to have a small population fully evaluated. Given the size of the problem and the computational restrictions, a population of 200 was sufficient to provide satisfactory results with one evaluation per individual per surface. A final issue was the criteria for determining a successful run. The criterion suggested by Zeil et al (2003) is a threshold for the RMS difference. When the input goes below a user-defined threshold, we can assume the algorithm has reached close enough to the home position. In the simulation, however, we had the luxury of knowing the actual agent position. Thus, it was possible to know the actual distance from home. During optimization, it was deemed more useful to consider homing successful judging from the actual distance. The threshold for the RMS difference refers directly to the RMS surface, and the RMS differences on and around the home position. Using the actual distance, we can force the controller to learn to home, without having to deal with the particularities of the RMS surface.

2.4.4 Selection Method The selection method used was Stochastic Universal Sampling (Mitchell 1998, pg. 166), which is a stochastic form of fitness proportional selection. In both selection methods, after all individuals have been evaluated, the values are divided by the sum of all the values in the population, to give the individuals’ fitness. The fitness of every individual is then replaced by the cumulative fitness of all the individuals up to that one. This means that the last individual in the list will have a cumulant fitness of 1. In fitness proportionate selection, N random numbers between 0 and 1 are generated, and for each number the individual whose fitness is just above it passes to the intermediate population. In Stochastic Universal Sampling, N equally distanced pointers are then selected, in the range 0-1 with the distance between two consecutive pointers always being 1/N and the position of the initial pointer randomly chosen in the range [0,1/N). For every pointer, the individual whose cumulative fitness is the smallest one that surpasses the pointer value passes in the intermediate population. In both cases, the number of offspring each individual

30 passes to the intermediate population is roughly proportional to its fitness, but Stochastic Universal Sampling ensures a higher degree of fairness in the selection, and a reduced dependence on randomness.

2.4.5 Crossover and mutation One-point crossover was chosen, since the small length of the genes did not suggest the need for a more complex approach. The crossover point is chosen anew randomly for each pair of individuals. However, the crossover probability in the final version of the algorithm was kept relatively low compared to most algorithms, at 0.6. That was so because all the parameters in a function are interconnected and, as a result, crossover mostly destroyed solutions rather than creating new ones. Mutation was the most important factor in forming solutions. The reason for that is that, since we are dealing with real numbers and not bit strings, the only way to cover as large a range of parameter values as possible would be to perform many small alterations to the parameters. While crossover ensured that radically new parameter combinations would be generated, it was mutation that contributed the most in exploring the parameter space. This was reflected in the high mutation rate, 0.6. The mutation scheme chosen was to change each parameter to be changed by a random value, drawn from a uniform probability distribution. For the first three variables (Ωbias Z0 Z1), the value range was (-0.05,0.05), while for the fourth variable (u), the range was (-0.0175,175). The ranges were chosen after experimentation, since they gave the best results.

31

Chapter 3 Results 3.1

Overview This section describes the results from the evaluation. Section 3.1 explains

the behaviour of the GA. In section 3.2, having come up with a number of control parameter sets after running the GA several times, a behavioural interpretation of each parameter is attempted. In section 3.3, some of the resulting controllers are exhaustively evaluated using the simulation described in section 2.3.3 and the results are analyzed. Finally, in section 3.4, the method is applied in the real world, and the results are studied and compared to other homing methods.

3.2

Running the GA The GA described in section 2, including the simulation of the homing runs,

was implemented in Matlab. The choice was made since Matlab offers very easy programming, good matrix manipulation, and also useful tools for data postprocessing. The related code appears in Appendix A. The GA In this GA, convergence does not occur in the usual sense. The frequent mutations by real numbers make impossible for the individuals of the population to become identical, or even similar to each other. Furthermore, even small mutations in the parameter affected the controller behaviour, meaning that there would be no full convergence even in the number of steps per homing run. The

32 number of steps per homing run was an inadequate criterion for another reason: It was affected by the start position, and the fast evaluation technique meant that we could not have certain knowledge of an individual’s homing efficiency. However, for every RMS surface set, the maximum value function practically stopped increasing after 200-250 generations, and

then kept oscillating around a certain value,

dependent on the surface set used. By performing an exhaustive evaluation of the population at that point, we end up with a controller that could home in a small number of steps, which however depended on the structure of the surface on which it was homing. In order to test the generalization capabilities of the controllers, some image combinations (e.g. a snapshot image from ‘Original’ subtracted from the ‘Twilight’ images) were not used for the evaluation of the controllers in the GA so they could be used for validating the results.

3.3

Behavioural interpretation of the parameters Having evolved a number of controllers which are able to lead to homing by

descending the RMS surface, it was deemed useful to study the role of each parameter in the control function. Ferree and Lockery (1999) attempt a similar interpretation of the function, when used to control chemotaxis (i.e. descent) of model nematodes on a surface where the input was determined by

(

− x2 + y2

C ( x, y ) = C 0 e

2 λ2

) (8)

where C0 the input at (0,0), and λ a factor that determines steepness. The typical behaviour of a nematode was to move up the gradient, and start moving in dense loops (dwell) when it reached the peak. The first observation they made was that, when they removed the zero-order term (that is, C) from (8), the upward movement remained, but the simulated nematode begun dwelling long before it reached the peak. An observation linked to that one was that, the bias term (Ωbias) and the first-order term (z1) always had opposite signs. This results in the two terms cancelling each other for a certain value of dC/dt > 0, and leading to a straight

33 movement. As Ferree and Lockery point out, the role of the first-order term causes a behaviour called klinotaxis, and is described as “a change in turning rate in response to the spatial gradient of a stimulus field”. The role of the zero order term is to change the turning rate proportionally to the input itself. This behaviour is defined as klinokinesis: “A change in turning rate in response to the scalar value of a stimulus field”. In the function used by Ferree and Lockery, the steepness grows smaller as we approach the centre. In the parameter values optimized in the chemotaxis model, the first-order term always had the same sign as the second-order term. This meant that, as the steepness grew smaller and the input grew larger, the first order-term kept the nematode moving for a while longer, until it reached the top, where it became so large that it enforced constant turning around in narrow circles. In the controllers evolved in our case, the properties were different as a result of the different surface and goal. The goal in the chemotaxis model was to collect as much input as possible in a given time limit. This encouraged moving to the peak as fast as possible, and then dwelling around it for the rest of the time. In our case, the goal was to simply reach the minimum as fast as possible. The fact that it was descent and not ascent we were interested in resulted in the bias and the first-order term having the same sign, so that they would counter each other and a fairly straight course would be maintained for certain values of dC/dt < 0. Another difference concerned the role of the zero-order term. Since in our case, the largest input and the smallest steepness were away from the goal, it was there that klinokinesis played the most important role. The sign of the zero-order term was in some cases the same and in some other the opposite of the other two parameters. In a homing run, when a controller had been optimized with respect to a surface, the zero-order term was used to adjust the turning angle, either by decreasing it or by increasing it, but almost always leading to the final turning rate being pushed towards zero, leading to a smoother course (we should not forget that we are dealing with angles, where increasing towards 360° can mean in fact reducing the final turning angle). As the input itself was reduced, the agent begins performing steeper turns, which can help reaching the home position with greater precision. In cases when the controller was not optimal for the given surface, the result often was movement in dense spirals,

34 indicating that the zero-order term had failed to impose the straight course it is supposed to cause. Another observation was the value range of the parameters. The bias was forced to cover a value range of –π to π, since it represented a constant angle. The zero-order term usually covered a similar value range. This was to be expected, since the input usually ranged from 0-0.8. The first-order term was always multiplied with dC/dt, which was in most cases, much smaller than C itself. That resulted in the firstorder term being, by absolute value, around 35-40. It was thus that it could counter the bias and force a straight course as the gradient remained negative. An issue that becomes now apparent is the controller’s direct dependence on the steepness and the value range of the RMS surface. The turning rate at each step is directly dependent on the input and the gradient, and thus for a different series of inputs, resulting from a different surface, the overall behaviour of the agent would be different for the same Ωbias, z0, and z1. The same applies for u, although the effect is indirect: The step size on a given surface affects the resulting first-order derivative between two steps. The presence of noise, especially in the input, and the use of several different surfaces ensured that the controllers evolved could home in all the training surfaces, as well as new ones resulting from other home-current image combinations. Still, the number of steps until homing varied depending on the surfaces used for the controller evaluation. That is, when a single surface was used for controller evolution, the number of steps was generally smaller during homing on that surface than when other surfaces were also used for evolution. This is due to the fact that there seems to be an ideal parameter set for each surface and, as a result, when several surfaces are used, the resulting controller is able to home as fast as possible in all of them, but not as fast as a controller perfectly adapted for each particular surface.

35

3.4

Evaluation

of

the

evolved

controllers

in

simulation Given the certain amount of dependency of the final controller on the initial random population, three different runs of the GA were done, and the three resulting controllers were thoroughly tested on a number of surfaces, including –but not limited to- the ones they were trained on, to fully evaluate their homing abilities. The parameters of the controllers appear in Table 3.1, while the maps used for the controller evaluation appear in Table 3.2. For the tests, the agent began each run at a distance of 1.2 metres from the centre, and initial orientation orthogonal to the actual home vector. Surfaces 1, 3, 4, 5, 7 and 8 were used for the training, while all of them were used for the measurements, with 1000 runs for each controller and each surface. The criteria used for evaluation were: Total number of steps (refers to total number of pictures captured, in the real world), success rate (failure means either wandering around the home position without reaching it, or moving outside the simulation boundaries), total distance travelled, total angle turned, and homing precision. For reasons of realism, the homing criterion during evaluation was an RMS threshold. In our case it was 0.42, being the best one for the surfaces we have to deal with. Of course, in the real world, it might not be so easy to estimate the best RMS difference threshold for a surface. The results on the performance of the taxis controllers on these surfaces, are by themselves of limited interest. It would be preferable to evaluate the controllers with respect to some other algorithm. The existence of the simulated RMS surface made the evaluation of any other RMS-based homing algorithm very easy. Thus, the RunDown algorithm was also implemented. The RunDown algorithm is one of the algorithms suggested by Zeil et al (2003) as a means of demonstrating the usefulness of the RMS surface in homing. At each time step, the agent moves forward by a predetermined step size, calculates the RMS difference and compares it to the difference in the previous step. If the new one is smaller, the agent moves forward another step. Otherwise, the agent turns 90° left, and then moves forward by the step length. A small alteration was done on Zeil et

36 al’s version of the algorithm. The movement step at each time is multiplied by the normalized RMS input, as in the taxis algorithm, in order to give a decreasing timestep. The RMS threshold was set to 0.42, as for the taxis algorithms. This leaves the step length as the only parameter to determine the algorithms behaviour. The optimal performance on these surfaces was achieved with a step length of 0.8, or 0.24 metres in the real world. A sample run of each controller on surface #5 appears on Fig. 3.1.

Figure 3.1: A sample run with each controller on surface #5. Upper left: Controller #1. Upper right: Controller #2. Bottom left: Controller #3. Bottom right: RunDown.

37 Table 3.1: The three controllers evolved

Controller Omega

Z0

Z1

U (in 0.3 m)

0.9485

1

0.6490

44.6288

0.6744

2

1.3821 -0.7951 37.0456

0.6492

3

1.3250 -0.7987 36.1076

0.9403

Table 3.2: The surfaces on which the controllers were tested. The surfaces marked with ‘*’ were used for evolving the controllers.

Surface # Snapshot image from Current Images from 1*

“Original”

“Original”

2

“Original”

“Twilight”

3*

“Original”

“Night”

4*

“Original”

“Chairs”

5*

“Night”

“Original”

6

“Twilight”

“Night”

7*

“Chairs”

“Original”

8*

“Chairs”

“Chairs”

9

“Night”

“Chairs”

Table 3.3: The total success rate for each controller, on all 9000 homing runs, and the mean (variance) number of steps per run

Controller Homing rate Mean number of steps (Var) 1

97.2%

15.519 (31.087)

2

98.6%

16.044 (31.132)

3

95.2%

12.22 (19.654)

Overall

97%

14.62 (51.234)

RunDown

92.7%

15.44 (70.36)

38 Table 3.4: The success rate for each controller and each surface, 1000 runs per surface

Controller Surface #

1

2

3

Overall RunDown

1

100%

100%

100%

100%

100%

2

100%

100%

100%

100%

100%

3

100%

100%

100%

100%

100%

4

90.6% 94.7% 84.9%

90.07%

78.6%

5

100%

100%

100%

100%

100%

6

100%

100%

100%

100%

100%

7

100%

100%

100%

100%

100%

8

95.4% 96.9% 83.3%

91.87%

78.2%

9

91.8% 95.7% 89.8%

92.43%

77.4%

Mean steps per run 35 30 1 2

20

3 15

Overall RunDown

10 5

ve ra ll

9

O

8

7

6

5

4

3

2

0 1

Steps

25

Surface

Figure 3.2: The mean number of steps for each surface

39 Mean distance travelled per run 4 3.5

Distance (metres)

3 1

2.5

2

2

3 Overall

1.5

RunDown

1 0.5

ve ra ll

9

O

8

7

6

5

4

3

2

1

0

Surface

Figure 3.3: The mean distance travelled for each surface

Mean angle turned per run 1600 1400

1

1000

2

800

3 Overall

600

RunDown

400 200

l ve ra l O

9

8

7

6

5

4

3

2

0 1

Degrees

1200

Surface

Figure 3.4: The mean angle turned

40 Mean homing precision per run 0.35 precision (metres)

0.3 1

0.25

2

0.2

3

0.15

Overall

0.1

RunDown

0.05

ve ra ll

9

O

8

7

6

5

4

3

2

1

0

Surface

Figure 3.5: The mean homing accuracy for each surface.

Table 3.3 shows the overall success rate, that is, homing runs that ended in the centre in less than 500 moves, for the three controllers. The rates are very high, but the information provided there is limited, considering that most of the surfaces were also used for evolving the controllers. An initial observation is that RunDown has a slightly smaller success rate. In RunDown, as in most gradient descent algorithms, the success rate is strongly affected by the step size. This is because, if the step size is too big, it might happen that the agent moves close to the home position but is unable to reach it with the desired precision. Thus, a reduction in the step size is usually bound to result in an increase of the accuracy. In homing, however, a decrease in the step size also means an increase in the number of steps ( i.e. in the pictures taken). Already, with this step size (0.24m), the number of steps is slightly larger than the average for the taxis controllers, although controllers #1 and #2 have a larger number of steps than RunDown. This leads to the second observation: Not all controllers are of equal efficiency. Controller #3 has the smallest success rate, but also the smallest number of steps per run. Controller #2 has the largest values in both, while controller #2 is in between. A closer look in Table 3.1 shows that this, again, is directly linked to the step length. Controller #3 has the largest one, while controller #2 has the smallest one. This results from the criterion for the controller evaluation: “Home in as few

41 steps as possible”, means that the success rate and the number of steps are both taken into account, and most resulting controllers will favour one over the other. A more detailed analysis of the homing success rate appears in Table 3.4. It is apparent that there is no direct link between the surfaces used to train the controllers, and the success rate. This is desirable, since it indicates that the controllers were not optimized for the specific surfaces used for the evolution, but were rather optimized for homing on surfaces which carry the general properties of these surfaces. There is in fact one common factor for the surfaces where performance is relatively low: They use as the current image set, the ‘Chairs’ dataset. This implies that, the true difficulty for homing comes when local optima appear, as a result of either appearing or disappearing objects (Surfaces 4, 9) or objects that are lying about the catchment area and sometimes come too close to the camera (Surface 8). This is commonplace for all homing algorithms, but in the RMS difference approach it takes the form of local minima or maxima in the surface. Surface #7, which uses a snapshot image from the ‘Chairs’ dataset and the current images from the ‘Original’ dataset, gave no difficulty for any controller. In fact, that RMS contains no local optima, although it is not perfectly smooth (Fig. 3.6). This suggests that, what causes the local optima is not the changes in the scene but instead, the presence of objects too close to the agent. These objects are deformed more radically as the agent moves, resulting in unexpected RMS values.

Figure 3.6: Surface #7 from the evaluation runs.

42 Another observation on the success rates is the relatively poor performance of RunDown on the surfaces which contain local optima. One reason for this is the ‘rigid’ behaviour of RunDown. Unlike taxis, where at each step the turning rate is completely different, and thus a local optimum usually simply causes a small divergence from the course, RunDown often gets trapped, moving around a local minimum in square patterns (Fig. 3.7).

Figure 3.7: A RunDown homing run on surface #8. In this run, it actually took 68 steps to home.

Fig. 3.2 shows the mean number of steps taken, separated by surface. By looking at the steps taken per run, we can see that, for all controllers and all surfaces, the number of steps is below 25. This number is not prohibitive, on the contrary it is quite encouraging for real-world application of the controller. Concerning the comparison to RunDown, the same pattern as before appears: RunDown is significally worse than Controller #1, slightly worse than the average, and better than the other two controllers. The exception is surface #9, where RunDown faces serious problems. Surface #9 has the most extreme local optima, and RunDown’s inability to deal with them is apparent.

43 A similar pattern emerges from Fig. 3.3, which shows the mean distance travelled per homing run. However, in this case, RunDown is slightly worse than all the other controllers. The superiority of the taxis algorithms in the distance travelled is countered by their extremely high cost in turning, as shown on Fig. 3.4. This is to be expected, since the taxis controllers turn at every time step, even a little, while the RunDown algorithm often covers large distances by moving straight ahead. This is also reflected in the higher variances in the turning rates of the taxis controllers. As was mentioned above, the taxis algorithm is not as predictable in its behaviour as RunDown. In a surface which is not completely symmetrical, the course of the agent depends heavily on the start position. Even noise plays a role in leading to a different course at each run. This unpredictability is not necessarily a disadvantage, however, since it obviously increases the robustness of homing in the presence of local optima. As for homing precision, it is strictly determined by three parameters: The controller’s step length, the RMS values near the home position and the RMS threshold. This leads to homing precision being more or less the same in every homing run for the same surface and the same controller, which is reflected in the extremely low variance in the data. However, comparing the surface overall homing precisions, we see that there are clear differences between them. This results from the termination condition of the taxis algorithm: Some surfaces had a really low value on the home position (#1 and #8 actually had zero), so the RMS difference goes beyond the threshold quite far away from the goal. Ideally, the threshold would be adapted to the surface, but in the real world, this will not be possible, since we cannot know the conditions under which the agent might have to home. In these runs, the threshold was set to a global value, which gave the best overall results, and this by itself means that this problem might be even worse in the real world. There is one measure which was not used for evaluating the performance of the simulated runs: The size of the catchment area. The reason for this is that, for a controller which has been optimized with respect to a surface, the only limit for homing is that it cannot home if it starts in a position so far from home that the properties of the surface no longer hold. It is reasonable to predict that, in the real world, after a certain distance, the RMS difference will stop increasing, or will increase so little that the surface will be completely disrupted by the noise. Thus, the

44 catchment area for any RMS difference- based homing algorithm is solely determined by the distance for which the RMS difference keeps increasing as we move away from the target. In the Vardy images, this property held generally held for a distance of 4.8-5.1 metres (16-17 units of 0.3m). Summing up, we can say that, for the given experiment, the taxis controllers were on average as good as RunDown, if not better, for most criteria, excluding the total angle turned. However, in the presence of local optima, the performance of RunDown deteriorates significally, while taxis homing is able to maintain a relatively high performance. In any case, since the image sets all described the same area and were captured with the same equipment, it would be reasonable to assume that certain properties of the RMS surface were common in all cases. Thus the possibility appeared that the controllers might not be able to home in a new area, if the properties of the resulting surface differed significally. This could only be tested through real-world experiments.

3.5

Evaluation in the real world

3.5.1 Overview Having evolved a number of controllers, it was necessary to apply them in the real world and evaluate their ability to home. Section 3.5.2 describes the robot setup used and the control code written for the evaluation. Section 3.5.3 is concerned with the characteristics of the RMS surface in the real world environments where the runs took place. Section 3.5.4 explains the reasons why new controllers had to be evolved, and section 3.5.5 explains how this was done. Finally, in section 3.5.6 the results from the new controllers are presented.

45

3.5.2 Robot setup and control code For the real world evaluation, a Koala robot from the IPAB robot lab was used (Fig. 3.8). A webcam pointing to a panoramic mirror was set up for capturing the images. Unlike the setup described in figure 1.2, however, the camera was above the mirror, pointing downwards. This setup was easier to implement, and makes no actual difference to the panoramic images. The rig supporting the camera consisted of three narrow pillars. The ideal setup for panoramic images is a uniform transparent tube, so that no object appears in the image apart from the landmarks themselves. However, as described in section 3.5.3, the presence of the pillars did not cause any consistent disruption of the catchment area. The robot was controlled by commands in ASCII format sent through a serial cable. In all algorithms implemented, the webcam was linked to a PC through a USB cable, and at each homing step (since all algorithms were iterative), an image was captured, processed and a movement command was sent to the robot. For the image processing, calculations and sending the robot commands, a program was written in C (Appendix B). A significant piece of code was already available, courtesy of Matthew Szenher, IPAB, University of Edinburgh. This code included a set of general image processing functions, functions for the camera calibration and extracting the gaussian-smoothed horizon line from a panoramic image. An implementation of image warping for the Koala robot was also included in the code. For the needs of the project, further code had to be written. The functions written concerned unfolding a panoramic image completely (as opposed to just the horizon line), calculating the RMS difference between two images, generating the shuffled image necessary for estimating the maximum RMS difference, an implementation of the RunDown algorithm and an implementation of the Taxis algorithm. Finally, a simple function was written for estimating the RMS differences at equal distances while moving away from the home position along a straight line, which was used to estimate the RMS surface in the real world environments.

46 The Koala robot proved to be extremely reliable in performing the desired actions, provided some asymmetries were taken into account. That is, the robot turned faster when turning left than when turning right, and diverged slightly to the left when commanded to move forward. To ensure greater reliability, these particularities were measured and taken into account when sending the robot commands. After that, the robot proved completely consistent in its behaviour, and no measurable error was ever detected in its turning angle or its movement distance.

Figure 3.8: The Koala robot used in the real-world experiments 3.5.2.1 The taxis algorithm

The code written for the taxis algorithm simply implements the algorithm described so far. The four parameters (Omega, Z0, Z1, U) and the RMS threshold for termination are defined as constants within the function. Assuming the agent is already at a distance from the home position and already has the snapshot saved, an image is captured and the RMS difference C is calculated. The agent then moves forward by CxU and calculates the new RMS difference. This is done because, for

47 decision making, both C and dC (i.e. Cold-C) are necessary. From then on, at each step, the new RMS difference is estimated, C and dC are inserted in function (8) and dθ is estimated. The agent turns by dθ and moves forward by CxU until C becomes less than the preset threshold. When this happens, the agent assumes to be home. 3.5.2.2 Image warping

The code for image warping implements the algorithm exactly as it was described in section 1.1. The horizon line is first extracted from the snapshot image. Starting at any place, an image is captured, and the horizon line is extracted from it as well. Using (1), all possible deformations for all rotations, home vector angles and values of ρ are applied on the current horizon, and the one that most resembles the snapshot one is found. The criterion used is the dot product between the two horizons. To reduce the computational burden of the estimation, a recursive search is used, at increasing accuracy. In the first run, all combinations are tried for a precision of 10° for α and ρ, and a precision of 0.2 and value range of 0.2-2 for ρ. In consecutive recursions, the precision for each parameter is reduced by a factor of 5. When the best match is found, the home vector is kept and the agent commanded to move accordingly. The termination condition is the one suggested by Franz et al (1998): If the home vector is around 180° (in the code, if it is >170°), it is assumed that the agent has just passed over the home position and is turning back, so the run ends. The only parameter the algorithm needs is the step size, which is defined as a constant within the code. The step length in all runs mentioned was 0.25 metres. 3.5.2.3 RunDown

The RunDown algorithm was implemented exactly as it was described in section 3.3, including the modified step size. That is step length U was set to 0.3 metres, multiplied by the current input C. As with the taxis algorithm, the agent begun by taking a step forward as to actually have two RMS values to compare.

48

3.5.3 Characteristics of the RMS surface in the real world Having programmed the robot to perform all the tasks necessary to implement the taxis algorithm, an initial study was done on the properties of the RMS surface for the specific camera-mirror system in the area where the experiments would take place. That area was the IPAB robot lab, at JCMB, Kings Buildings. Although it would be interesting to try a range of completely different environments, that option did not exist. Variations in the experiments were introduced by choosing different locations in the lab, adjusting the environment’s illumination level and introducing new objects, or moving the old ones around. Concerning the introduction or movement of objects, this was done either immediately after capturing the home position and kept unchanged for the rest of the experiment, or also moved in between images. The RMS surface was estimated by taking rows of equally distanced (30 cm) images. In studying the RMS difference surface for these conditions, a number of observations were made: •

The fundamental properties (Global minimum at the home position,

gradual increase of RMS difference as we move away) of the RMS surface were present in all cases. •

The RMS surface was clearly not as steep as in the Vardy dataset.

Steepness was affected by changes in the illumination levels, decreasing as the illumination decreased, but even in the case of maximum possible lighting, the value range was significally narrower, and the slope smaller (Fig. 3.9). •

The introduction of new objects, or movement of old ones either after

taking the snapshot image or throughout the experiment, did very little to disturb the RMS surface. •

Moving the robot too close to an object led to a local maximum on the

RMS surface. This is because, as the area covered by the object on the image increased, it covered a large part of it and resulted in large differences. As the

49 robot moved on and away from the object, the landmarks reappeared and led to smaller RMS differences (Fig. 3.10). •

The radius of the catchment area reached up to 2.7 metres, and never

came below 1.5 metres (Fig. 3.11). Again, the radius was affected by the illumination levels, decreasing as the illumination decreased, but also from the presence of nearby objects.

Figure 3.9: RMS values for different situations: Blue Xs: The RMS difference for the ‘Original’ Vardy dataset. Black Os: RMS differences taken from the lab area under bright illumination. Black dots: RMS differences taken from the same area under low illumination.

Figure 3.10: The change on the RMS surface when the agent happens to pass too close to an object, resulting in occlusion of all other features. Os: No object. ‘.’s: A small cardboard box next to the agent’s route at 1m from the home position.

50 As was found, part of the reduced steepness of the RMS surface was also due to the issue of rotation and linked to the instability of the camera-mirror rig. For the image unfolding to take place, the position of the mirror centre in the image has to be known, and a relatively accurate estimate of the radius of the mirror’s projection has to be known as well. For the calibration, an edge detection program written by Matthew Szenher was used, locating the circle which described the mirror boundary and getting the centre and radius that best described it. Clearly, during a homing run, the orientation at which each image was captured usually differed from the orientation of the snapshot image. The properties of the RMS surface as described by Zeil et al (2003) hold true when all images have the same orientation as the snapshot image. To resolve this, the observation of Zeil et al concerning the image orientation was used. That is, that the image captured from a position with the same orientation as the snapshot image has the minimum RMS difference compared to all other images captured from the same position under every other possible orientation. In a panoramic image, one way to model orientation is to shift the unfolded image. Since the unfolding technique generated images of 360 pixels width, it was theoretically possible to model every rotation up to 1 degree accuracy, without having to recapture the image. By trying all 360 possible images and estimating the RMS difference for each one of them, it was possible to get the orientation difference from the snapshot image, and an estimate of the RMS for the current position, if the agent had the same orientation as when capturing the snapshot image. The estimate of the orientation difference was in every case extremely accurate, giving an error which ranged from 0-2 degrees near the home position to 15-20 degrees at 2.5 metres from the home position (Fig. 3.11).

51

Figure 3.11: The angle estimate at various distances (in metres) from the home position. True angle for black: 360 (or 0) degrees. True angle for blue: 90 degrees.

The estimation of the RMS value however was clearly problematic. This was due to the fact that the mirror could never be perfectly calibrated. The mirror-camera rig was quite unstable, thus the unfolding was never perfect, and often had a wavy instead of straight form. As a result, although the minimum RMS was found at the right orientation, it was always overestimated. The maximum distortion was located at the bottom and the top of the unfolded image (near the centre and the boundaries of the mirror), and was reduced by cutting off these parts of the image. Still, after this processing, a significant overestimation of the RMS difference compared to the home orientation image remained. Under maximum illumination and practically zero background movement between images, the RMS difference between two images taken from the same position under the same orientation was 0.01-0.1, while, a 180° rotation of the agent and equal shifting of the image to match the home one resulted in a difference of 0.3-0.45. In general, the value became very small when orientation differed by less than 5 degrees, and was generally uniform for all other angles. Also, as the distance from the home position increased, the RMS for the home orientation and all other possible angles tended to converge (Fig. 3.12). Since the case where the current orientation differed from the home orientation by less than 5 degrees was extremely rare in the homing runs, it was not taken into account in the estimation of

52 the RMS surface. However, whenever the agent happened to assume an orientation similar to the home one and was near the home position, a local minimum appeared (Fig. 3.13). Since this minimum was not directly linked to the spatial or temporal properties of the environment, it could not be predicted and was accepted as a form of noise.

Figure 3.12: The RMS difference for the same route and home position, with the snapshot captured at different orientations. Black: Matching snapshot and current orientations. Blue: 90º angle between the two.

Figure 3.13: The RMS difference for all theoretical rotations on two images, taken from the home position with different orientations. Black: Current agent orientation matches the snapshot orientation. Minimum of 0.08 at 0º. Blue: 180º turn, no movement. Minimum of 0.48 at 180º.

53

3.5.4 Initial results Initial application of the controllers determined by the parameter sets that were evolved using the Vardy dataset failed to home. The reason for that is that, despite all the efforts taken to make the controller independent of the particularities of the RMS surface, the controllers were evolved to work in a particular area, whose RMS surface came out different than the ones for the lab, under all conditions.

3.5.5 Adapting the parameters to the real-world surfaces 3.5.5.1 Estimating the real-world surfaces

Facing the need for a new surface (or set of surfaces) on which to evolve the new controllers, it was decided to take samples from the lab area and build as many surfaces as needed for the evaluations. The simplest way to do this, was to capture a snapshot image, have the agent turn at least 30 degrees to ensure the home orientation is lost, and have it move in a straight line capturing images and estimating the RMS in 30 cm intervals. The distance was chosen to match the distance in the Vardy dataset, for which the simulation was initially programmed. The RMS line that resulted from the measurements was then expanded into a circle, with the minimum at the home position and the maximum in the perimeter of the simulated catchment area. In truth, the RMS surface is never symmetrical. It was however considered more practical to build four different circular RMS surfaces, one for each direction the agent could move, and evolve it on them as individual surfaces, than to try to model the surface all on one matrix, especially given the lack of equipment and the time limitations. The study of the RMS surface is not of direct interest to this project, except as a means to optimize the parameters of the control function. 3.5.5.2 On-line optimization results

The initial plan was to capture four RMS lines for every scenario, build the corresponding symmetrical surfaces, and evolve the controller for each specific situation. After capturing a set of four surfaces for the first runs (bright illumination, static landmarks, clear field), however, it was discovered that the resulting final

54 population of 200 controllers contained a range of different controllers which could all home on the given surfaces, but some of which could also home on the new scenarios. That is, although the optimal controller for the initial scenario was very poor at homing under reduced illumination, some other controller from the population could home quite efficiently. One explanation for this, is that the controllers evolved for a surface, may not be all equally efficient for that surface (that’s what the evaluation step is for) but contain variations of the best controller, which can work well on other, similar surfaces. It is true that the surfaces in all the setups were much alike, in the value range and the steepness, and it is these two properties that play the most important role in the parameter choice for a controller. Thus, for each new scenario, one or two RMS lines were sampled around the home position, and expanded into surfaces. By running an exhaustive evaluation of the population mentioned above (30 runs on each surface for all 200 individuals), the most appropriate one was chosen. The results of the run demonstrate that this technique was indeed successful, and no need was there for running the GA anew.

3.5.6 Real world runs Having developed a technique for optimizing a controller for homing on the spot, experimental homing runs had to be done in varying environments in order to evaluate the controllers’ performance, both in absolute and relative terms. A series of different scenarios were set up, that were expected to be of interest, given the initial observations on the behaviour of the catchment area and the given strengths and weaknesses of the Image Warping algorithm. 3.5.6.1 Short-range homing in a bright static environment

The first scenario was the easiest one for homing. The room was brightly lit, and no items were close to the home position, or moving. The distance of the start position from the home position was 1m. As a result, the catchment area was smooth, with no local minima or maxima. The results from the homing runs appear on Table 3.5. It is clear that the RMS-based algorithms cannot compete with image warping.

55 Despite the high success rates, both RMS algorithms are very costly in distance travelled and number of steps taken. A fundamental reason for that, is the way image warping works: It is able to estimate the direction to move to get closer to the home position, from a single input, whereas, the RMS algorithms have to move around in order to find the direction of descent at each point. Although both algorithms take as much advantage from the properties of the surface as possible, it still is not enough to compete with image warping. Unlike the simulated runs, the performance of the taxis algorithm was significally slower than RunDown. Since this is a case of simulation-real world differences, one explanation for this is noise. The advantage of taxis is that it not only moves in response of the gradient, but its behaviour is different depending on the steepness and the actual input (which reflects the distance from the goal). Thus, it is able to move in a more efficient way towards the goal, unlike RunDown, which only cares about the direction of the slope, and often ends up spending moves in order to catch the gradient. However, this theoretical advantage of taxis can turn into a disadvantage when the noise disrupts the RMS surface too much, since then, the input and the gradient will misguide the taxis algorithm. This is not about local optima, but rather in changes in the steepness of the RMS function, which are not linked to a particular property of a position, but rather because of temporary disruptions. An example of such a case is the fact that the same position can give different RMS values, depending on the orientation (Fig. 3.12). Another such case is the possibility that the pillars might block a source of light, in which case, the change in the RMS difference is significant, without necessarily leading to a local maximum. Another such case is when the agent moves from a dark area into a brightly illuminated one, or vice versa. When this happens, the camera takes some time to adapt to the new lighting conditions, and the first image will be either too bright or too dark, resulting again in an unexpected RMS value. If the agent manages to remain in the same area, the camera adapts and the next RMS value is again as expected (though the gradient dC will probably be unexpected again, as a result of the previous image).These bright and dark areas proved to be a significant problem, since there seemed to be too many of them in the lab. Although it was not initially

56 apparent, even moving in front a window, or near the shadow of the desk, led to this phenomenon. Sometimes, when one of the above situations occurs, a local optimum will appear. In this case, both RMS algorithms faced significant difficulties. In many occasions, though, the RMS value was not altered enough to cause a local optimum, just a disruption of the slope. In these cases, RunDown moved on ignoring the issue, while taxis had to take steps in the wrong direction. This should explain the significally worse performance of the taxis algorithm. Table 3.5: The results for 10 short-range homing runs (1 metre away), in a clear, bright area with no movement Algorithm

Taxis

RunDown

Warping

% success

90%

80%

100%

# of steps

20.67 (45.11)

13.75 (29.43)

6.8 (6.76)

Distance travelled

4.211 (1.5)

2.525 (1.02)

1.776 (0.518)

Angle turned

1316.41 (346180)

360 (72900)

316.04 (105180)

Homing precision

0.4290 (0.08)

0.179 (0.01)

0.2876 (0.02)

3.5.6.2 Long-range homing in a bright static environment

The exact same experiment was repeated, only at a start position at 2 metres away from the home position. The results are presented at Table 3.6. Table 3.6: The results for 10 long range (2 metres away) homing runs in a clear, bright area with no movement. Algorithm

Taxis

RunDown

Warping

% success

40%

90%

60%

# of steps

22.25 (7.6875)

23.56 (112.025)

16.33 (18.56)

Distance travelled

5.988 (3.87)

4.489 (4.9)

3.814 (1.148)

Angle turned

1559.3 (150550)

580 (218000)

999.30 (246560)

Homing precision

0.4180 (0.046)

0.252 (0.016)

0.2647 (0.02)

57 There are significant, and partly unexpected differences from the short range scenario results. One significant difference is the obvious superiority of RunDown compared to the other two algorithms in terms of success rate. Each of the other two algorithms has its own reasons for breaking down when positioned at a large distance from the home position. The taxis algorithm has a significant problem with local maxima which disrupt it from its course. It is clear that, when it has a larger distance to cross, the number of these problems will increase, often to a level that stops it from homing. Another reason is that, since the steepness of the RMS difference is reduced as the distance from the target increases, the resulting local maxima will have a greater effect at larger distances. Near the home position, where the RMS gradient is much larger, the role these maxima play is greatly reduced. Image Warping, on the other hand, has a different sort of problem, which results from a combination of its fundamental assumption and its termination condition. As Franz et al (1998) show, the assumption that all landmarks have the same distance from the home position does not hold, but as the agent moves closer to the home position, the resulting error from this is reduced. Therefore, the farther away the agent is from the home position, the larger the error in the home vector estimated. If the first vectors point in a direction away from the actual home position, then, even if later on some vector points to the correct direction, it is possible that the agent will have to turn around to go there. This would activate the termination condition, and the agent would stop, away from the home position. Another important observation is that, while the distance was doubled, the total distance travelled only increased by 1.5 times. It seems that the distance traversed is not directly proportionate to the distance from the home position. This should be appointed to the fact that a significant part of the total distance travelled is spent on trying to approach the home position with accuracy. It is often the case that the agent gets really close to the home position, and then turns back abruptly. This happens because of the large difference in steepness between the area at 0.3-0.6 metres from the home position, and the rest of the catchment area. This also applied to the simulated runs, but the difference is much greater in the real world. When a taxis controller learns to home in a surface, it adapts to the most fundamental properties of the surface. If the steepness-value range changes too radically near the

58 home position, a controller that is able to get close to the home position might not be able to move directly to it afterwards. For the taxis algorithm, too large a decrease of the RMS input is as undesirable as a too small one, since using (8), a gradient that is too small will give a large angle, thus forcing the agent to turn around. This was the case in most homing runs, meaning that, in a normal homing run, the agent moves close to the home position, turns back, does a small loop as an initial response to the unexpected change and then homes (Fig. 3.14) This cost is more or less constant, meaning that a part of the distance travelled is always the same, regardless of the initial distance.

Figure 3.14: One of the successful Taxis homing runs. The agent deals with two local optima, moves towards the home position, then moves back, and re-approaches it successfully. 3.5.6.3 Moving landmarks

The ability of an algorithm to home when there have been changes in the scene between the capturing of the snapshot image and the homing run is an extremely important one, since it is often a situation the agents have to deal with. It is clear that, there is a level of changes after which the scene has so radically changed that no agent can home (i.e. moving or replacing practically everything). On the other hand, for small changes in the scene (e.g. moving two chairs around), all three

59 controllers implemented were able to home, with no apparent change to their performance. The important issue in this comparison is, which algorithm is the first to have its performance significally reduced after a certain level of changes in the scene. Although the level of change is not quantifiable, an experiment was set-up to demonstrate a fundamental weakness of the image warping algorithm. After capturing a snapshot image, the scene was altered by moving a number of chairs and a cardboard box around. One chair was also completely removed from the scene (Fig. 3.15). The new scene had some interesting properties with respect to all three algorithms. These are demonstrated in Figure 3.16. The home position was situated at point H, where the robot lies. By moving out and around the objects, the behaviour of image warping changed completely. Instead of moving towards the home position, it always homed in the position marked with T. The movement of the landmarks had changed the horizon line enough for the image warping to find the best match in that position, instead of the home position. The catchment area for that new trap point extended into all the visible area, and more than 2.5 metres behind the agent. Even when the trap point was not visible from the start position, but the actual home position was, the agent would initially move to the true home position, but then the trap point would be within homing range, and the agent would go on and home on the trap. Thus, the image warping algorithm was rendered completely useless.

A

B Figure 3.15: The moved landmarks experiment. A: The snapshot image. B: An image taken from the home position after the changes in the scene.

60 As for the RMS-based algorithms, the most significant change was the appearance of a local minimum at the position marked with M in Figure 3.16. This, in some way resembles the way that image warping was misguided. There are two fundamental differences, though. The first one was that the local minimum was not as deep as the global minimum, which had remained on the home position. An explanation for this is that the RMS surface is calculated using visual information from the entire scene, including floor and ceiling patterns. This means that, even when the landmarks happen to move in a pattern, as to misguide image warping, the RMS surface needs much more radical changes in the scene to lose the global minimum. The second concerns the short-sightedness of the RMS-based algorithms, which, in this case, seems to be a useful trait. When the start position was beyond or around the local minimum, the agent always became trapped, and spent the rest of the run moving in and out of the minimum, which was not deep enough to trigger the termination condition. When it started relatively away from the minimum (more than 0.5 metres), it always followed the steepest local descent towards the home position, without being misguided at all. The positions where this was done were: The area around 0.5 metres away from the minimum, in the direction of the home position, and any other part of the visible scene. On the opposite side of the minimum and the trap point, the catchment area extended at around 1.8-2.0 metres. The results displayed at Table 3.7 all had the start positions at 1.2 metres from the home position. It can be seen that, once again, the taxis algorithm fails to compete with RunDown. Table 3.7: The results for 10 homing runs after moving the landmarks Algorithm

Taxis

RunDown

% success

80%

90%

# of steps

22.75 (57.68)

13.33 (4.44)

Distance travelled

5.077 (3.676)

1.639 (0.099)

Angle turned Homing precision

1670.38 (592560) 209.99 (7200) 0.220 (0.013)

0.188 (0.006)

61

Figure 3.16: The moved chairs scene. H: The home position. M: The local minimum. T: The perceived home position for image warping. 3.5.6.4 Placing an object close to the home position

The final experiment concerned the presence of objects near the home position. It has already been mentioned that, passing close to an object caused a local maximum in the RMS surface. It would be interesting to see what happens to the RMS surface when an object is actually close to the home position. Another reason for this scenario is the fundamental assumption of Image Warping. Since it is assumed that all landmarks are situated at equal distances from the home position, it might be possible that the algorithm would have a hard time functioning with one large object right next to the home position and all the other landmarks at further distances. Thus, an open area was first selected in the IPAB lab and three lines were sampled from the RMS surface in three directions. A large cardboard box was then placed next to the home position (Fig. 3.17), the snapshot image was recaptured to include the box, and new RMS samples were taken from the same lines. The snapshot image was recaptured since this experiment does not concern moving

62 objects between the snapshot and the homing run, but a consistent, static situation. The results from the RMS sampling appear on figure 3.18. Although small changes do appear in the steepness and value range, the RMS surface seems to maintain its fundamental property. However, a local minimum has appeared on line C, at a distance of 1.8m. However, another way to see it is a local maximum at a distance of 1.5m. The alteration is small, compared to the shape of the surface before the object, but is enough to potentially misguide both algorithms. Another observation is that, for directions A and B, there is a significant reduction in the steepness close to the home position. This is exactly the opposite from what usually happens. An explanation for this is that the box is large enough to cover all vision in one side, and, being uniform in colour, a large part of the visible area is very similar in the home position and around it.

Figure 3.17: The lab area where the cardboard box was placed. The robot stands on the home position. A, B and C denote the three directions where the RMS differences were sampled as lines.

63

B

A

C Figure 3.18: The three RMS lines, before and after inserting the object. Blue ‘x’: Before the box. Black ‘o’: After the box.

To test the homing performance of the controllers in this environment, 5 runs with each algorithm were attempted, at a distance from the home position of 1.2m. The results appear on Table 3.8. As the table shows, the taxis algorithm’s performance was very poor, although the shape of the surfaces suggested that there shouldn’t be any serious problems, apart from the small local minimum that appeared on direction C. In fact, the cause for the increased number of failures was not the local minimum, but the taxis algorithm’s tendency to wander around a lot while homing, instead of moving directly towards the minimum. As a result of this, the agent would wander behind the box, thus losing sight of most landmarks, and ending trapped there. The increased number of steps in the runs is linked to the same reason. In the experiments where the agent was able to recover, it had to spend many extra moves in order to get back near the home position and home. RunDown, being more consistent in the way it approaches the home position, did not actually suffer a decrease in performance because of that. Finally, Image Warping was not in the least affected by the presence of the object. Since the error when the assumption does not

64 hold is reduced as we approach the home position, it could be that, 1.2 metres is close enough for Image Warping to home without any problems. Table 3.8: The results for 5 runs with an object placed next to the home position. Algorithm

Taxis

RunDown

Warping

% success

40%

80%

100%

# of steps

30 (49)

13 (29.5)

9.2 (11.76)

Distance travelled

6.9381 (2.692)

2.3813 (1.121)

2.0938 (0.91)

2276.4 (322860) 292.5 (420190)

492.1 (86784)

Angle turned Homing precision

0.2887 (0.04)

0.1784 (0.012)

0.1845 (0.0037)

3.5.7 The size of the catchment area It was mentioned in section 3.3 that, for a taxis controller which has been adapted for a particular surface, the catchment area extends as long as the RMS difference keeps increasing. In the experiments in a bright, static environment, the catchment area often extended as long as 2.7 metres. However, the taxis algorithm had an extremely low success rate even at 2 metres. That is because, the condition described in 3.3 holds in an environment where local maxima did not appear in random positions as a result of the agent’s behaviour, but were consistently linked with a position. In the real world, where these maxima popped up for reasons that had nothing to do with the location, the agent often became confused and was misled outside the catchment area, where it spent the rest of its moves. Besides, the estimation of the RMS surface cannot be perfect, but simply approximated. This means that the agent is not perfectly prepared for what it is going to encounter. We might thus say that the catchment area extends at around 2 metres from the home position at maximum, for the taxis algorithm. Image Warping also seems to break down at a bit more than 2 metres. RunDown, on the other hand, which is not affected by the value range, but only by the sign of the gradient, showed a more consistent behaviour. Local optima did cost it a few extra moves, but did not send it totally off-track. It might thus be

65 reasonable to assume that the catchment area for RunDown extends as long as the RMS difference keeps increasing, which is 2.7-3 metres in the best of cases.

3.5.8 Computational complexity The computational cost of RunDown and the taxis algorithm are similar, since the RMS difference estimation is identical, and the actual command estimation is simply one calculation (the sum of two products and a constant for taxis, and a comparison for RunDown). On the other hand Image Warping gets the command directly from the image processing, in the form of angle α. Thus, the actual cost comes from the RMS calculation for the two first algorithms, and from the vector comparisons for the third one. To estimate the RMS difference for the comparison under all 360 possible orientations, the total number of image comparisons is 360, and each comparison takes 360*H*3 calculations, where H the image height, 360 the image width and the three calculations are the subtraction, square and addition. Furthermore, to shift an image by N columns, it takes 2*N+360-N= 360+N operations. Thus or all orientations (0 to 359), it takes 360+362+…+719=149220 operations. In the implementation, the image height H was 70, making for 27216000 operations for all comparisons, and 27410220 operations in total. Image warping compares nα*nρ*nψ images, where nα , nρ and nψ the number of increments for α, ρ and ψ respectively, and compares them using 360*2 calculations for the normalization and 360*2 calculations for the comparison (multiplication and summation). In order to build each image, Image Warping transforms the current image using (1) for each pixel. The total number of operations necessary, including the auxiliary ones (modulus by 360, adding to the current pixel position, since δ only signifies the pixel shift) is 14. In our implementation, it is nα=36 nρ=36 and nψ=10 for the first recursion, and nα=3 nρ=3 and nψ=10 for the second recursion. This adds up in a total of 84564000 operations, which means that Image Warping takes 3.01 times more operations to come up with an answer. One thing to note is that, the difference in cost is made not in the number of pixel comparisons, but in the transformation cost. The number of pixels compared is

66 360*360*H for the RMS difference, and nα*nρ*nψ*360 for Image Warping, which, in the current implementation means that Image Warping takes 0.514 of the comparisons taken by RMS. Of course, our implementation of Image Warping, being a well-established algorithm, used a standard recursive implementation to search for the best values. The current RMS implementation performs a full search, since the behaviour of the RMS values with rotation was not known beforehand, and time did not permit improvements on this later on. From this point of view, a recursive implementation, with a step of 10 degrees, like in Image Warping, would practically take 10 times less calculations, further increasing the advantage over Image Warping to about 30 times faster. The computational superiority of the RMS algorithms over Image Warping, however, is not enough to guarantee faster homing. Since the real-world implementation of Image Warping took significally less steps than even the simulated taxis algorithm, this advantage is partly countered.

67

Chapter 4 Conclusion 4.1 Project Summary A new family of controller for image-based homing was created. By taking advantage of the behaviour of the RMS pixel difference between a snapshot and the current image, a model of C. Elegans chemotaxis was used to achieve homing in the form of gradient descent. Using a GA, the controller parameters were optimized on a set of RMS surfaces, which corresponded to an indoor environment under varying conditions. Following an evaluation of the controllers on these and other similar surfaces, it was demonstrated that a properly adapted taxis controller is able to home more effectively than RunDown, the best RMS-based homing algorithm proposed so far. The controller was then tested on a robot in real-world situations. Comparison with Image Warping, one of the best homing algorithms so far and RunDown demonstrated that the particular implementation of taxis homing was not as efficient as its simulated version, since both RunDown and Image Warping performed better. A number of reasons for this were found, and they are discussed in the next section. The real world experiments, however, demonstrated that the RMS-based algorithms are more robust to changes in the scene than Image Warping, suggesting that a better implementation would be applicable.

68

4.2 Conclusions Although it was not possible to build a controller that would compete with Image Warping, certain conclusions concerning the potential of the RMS-based algorithms can be derived from the data presented in Section 3. The significant differences in performance between the simulation and the real-world suggest that, if the sources of noise could be reduced, the real-world performance, especially of taxis homing, could significally increase. However, on comparing the simulated results of the RMS-based algorithms in Section 3.3 to the Image Warping performance in the simple real-world experiment of section 3.4.5.1 we can assume that, even if the realworld conditions were ideal, Image Warping would still be more efficient. On the other hand, the results of the experiment in section 3.4.5.3 demonstrate that the RMS-based algorithms have an advantage over Image Warping, in their increased tolerance in the movement of landmarks. In the real-world experiments, taxis took significally more time than RunDown to actually home, and also displayed a much smaller success rate. In the simulation, however, taxis demonstrated a clear superiority. An explanation that was offered for this was that local optima often appeared in places where they were not expected to, and would then abruptly disappear. In this inconsistent environment, the taxis algorithm was more likely to break down than RunDown, resulting in poor performance. A number of reasons were identified for the appearance of these local optima: •

The image was often over- or under-illuminated

•

Different agent orientations gave different RMS difference values for

the same position •

The mirror-camera pillars occasionally blocked a source of light,

causing an increased RMS difference. A solution for the first problem could be to pre-process the image, and normalize its intensity. An issue here is that the intensity was not changed uniformly

69 over the image. Thus, uniform normalization did very little to solve the issue. It is possible that some form of local normalization would help reduce the problem. As for the orientation issue, a simple solution would be to use a compass. The agent would move to a position, turn in order to match the home position and then capture an image to get the RMS difference. Then it would turn back to its previous orientation and perform the next move. Since however, as was observed in section 3.4.2, the relative orientation with respect to the home position can be estimated with great accuracy using an image, it might be possible for the agent to capture an image from a position, use it to find its relative orientation, turn in such a way as to match the home position image, and then capture a new image from that orientation, which it would use to estimate the RMS difference. It would then turn back to its previous orientation, and go on. Of course, this is going to cost extra not only in turning, but it would also double the number of images captured. The ultimate solution, in order to avoid the use of the compass, the extra turning and the extra images, would be to use a more reliable rig, which would guarantee that shifting would successfully model rotation. Finally, concerning the problem of the rig pillars, it should be quite easy to remove them from both images, and use only the clear part of the image for the RMS estimation. In any case, most researchers tend to use a glass cylinder, which ensures that the camera is supported without any object interfering with the images. These changes could make a properly adapted taxis controller work better than a RunDown controller, and also a good competitor of Image Warping in real world image-based homing, probably slower, but more robust to moving landmarks. One significant weakness that was identified for the taxis algorithm is its tendency to wander around a lot when homing. Although this is necessary in order to catch the fastest gradient and also to avoid local optima, it was shown that it can lead to the agent losing visual contact with the home position, as a result of occlusion. However, given the routes of the simulated homing runs, it could be assumed that, once the sources of unexpected local optima disappear, it will be possible for the controller to move towards the home position in a fairly straight route, avoiding the possibility of getting lost because of occlusion.

70 Concerning computational complexity, it was shown that, in the current implementations of the algorithms, the RMS-based ones are faster by a factor of 3, and, in a recursive implementation, they could be further sped up by a factor of 10. However, we should keep in mind that the RMS-based algorithms were both more costly in the number of steps, as well as the path length and the total angle turned. Since we are dealing with robots, it is often the case that the criteria that have to do with movement in the real world are usually the ones that reflect the time cost of the algorithm, compared to which, the computational cost is often negligible. We are thus not in a position to claim that the RMS-based algorithms are actually going to be faster to home. The taxis algorithm’s greatest weakness at his point is getting a properly adapted controller for a given situation. Sampling and evolving is extremely costly, and it is not applicable when the agent returns from a trip and tries to home, only to find out that the environment has changed since last time, and as a result, the RMS surface as well. In that case, sampling is impossible, since the agent cannot know where it is with respect to the home position. It was demonstrated that evolution is not necessary, and for changes in the same environment, the same set of controllers can be used, using simply 600 simulated evaluations (30 runs for 200 individuals), instead of running a full GA. Choosing the best one from this set, however, needs some sort of knowledge of the shape of the RMS surface. Was it possible to predict the shape of the RMS surface, we could be able to select a controller from the set, which, although it might not be optimal, it could lead the agent home. As was stated, the presence of local optima does not affect the controller selection. All that matters is the steepness and the value range, as a function of the distance from the goal. Study of the factors that affect the shape of the RMS surface is still going on, however, and no definite results have come up yet. If claims that the RMS values generally follow a specific function of the distance from the home position (Szenher, 2005) hold, it would be possible to get an estimate of the characteristics of the RMS surface in a location, and use it to select an appropriate controller. Until this becomes possible, however, the applicability of the controller is limited.

71 A more feasible solution to the optimization problem could be to build a controller which would be able to home in an area, regardless of the changes in illumination or the movement of objects. In the simulated runs, it was shown that, once a controller has been optimized for a certain surface (i.e. steepness and value range), the appearance of local minima does not create the need for a different controller. It was also shown that a controller can be evolved for a number of surfaces which correspond to the same area under varying conditions, and be able to home in any of them, or new conditions that might appear. It seems feasible then, that once an area is selected as the home location, an agent could collect samples of the RMS surfaces under different conditions (the major factor being illumination level) and evolve a controller which could home on them. The simulated results suggest that, a controller evolved to home on these surfaces would also be able to home on variations of them, thus providing an efficient homing controller for that area.

72

Bibliography Benhamou, S., Bovet, P., and Poucet, B., (1995). A model for place navigation in mammals. Journal of Theoretical Biology, 173:163-178. Cartwright B.A., & Collett, T.S. (1983), Landmark Learning in Bees. Journal of Computational Physiology, 151, 521-543 Chatterjee, C., Kang, Z., and RoyChowdhury, V. P. (2000). Algorithms for accelerated convergence of adaptive PCA. IEEE Transactions on Neural Networks, 11, 338 - 355 Dunn, N. A., Conery, J. S, Lockery, S. R., (2003), A Neural Network Model for Chemotaxis in Caenorhabditis elegans, Proceedings of the International Joint Conference on Neural Networks, 2574- 2578 Feree, T., and Lockery, S. (1999), Computational rules for Chemotaxis in the Nematode C. elegans. Journal of Computational Neuroscience, 6:263-277 Fitzpatrick, M. J., and Grefenstette, J. J., (1988). Genetic Algorithms in Noisy Environments, Machine Learning, 3, 101-120. Frantz, M. and Mallot, H. (2000). Biomimetic robot navigation. Robotics and Autonomous Systems, 30:133-153. Franz, M., Scolkopf, B. Mallot, H., and Bulthoff, H. (1998). Where did I take that snapshot? Scene-based homing by image matching. Biological Cybernetics, 79:191202. Gourichon S., Meyer, J.A., Pirim, P. (2002) Using Coloured Snapshots for ShortRange Guidance in Mobile Robots, International Journal of Robotics and Automation: Special Issue on Biologically Inspired Robotics, 17(4), 154-162 Hong, J., Tan, X., Pinnette, B., Weiss, R., and Riseman, E. (1992). Image-based homing. Proceedings of the 1991 IEEE International Conference on Robotics and Automation, 620-625

73 Ibnkahla, M. (2002). Natural gradient learning neural networks for adaptive inversion of Hammerstein systems. IEEE Signal Processing Letters, 9, 315 - 317 Ibnkahla, M. & Yuan, J (2003). A neural network MLSE receiver based on natural gradient descent: application to satellite communications. Proceedings of the Seventh International Symposium on Signal Processing and Its Applications, 2003. 1, 33-36 Jakobi, N. (1997). Evolutionary Robotics and the Radical Envelope of Noise Hypothesis, Adaptive Behaviour, 6, 325-368 Kivinen, J., Smola, A. J. and Williamson, R. C., (2004). Online Learning with Kernels. IEEE Transactions on Signal Processing, 52, 2165-2175 Lambrinos, D., Möller, R., Labhart, T., Pfeifer, R., Wehner, R., (2000), A mobile robot employing insect strategies for navigation, Robotics and Autonomous Systems 30, 39–64 Mathworld: Conjugate Gradient Descent method, visited http://mathworld.wolfram.com/ConjugateGradientMethod.html

28/11/2004,

Mitchell, M. (1998). An Introduction to Genetic Algorithms. MIT Press, 1998. Mitchell, T. M. (1997). Machine Learning. MIT Press & The McGraw-Hill Companies. Moller, R. (2000). Insect visual homing strategies in a robot with analog processing. Biological Cybernetics, 83, 231-243. Muller, S. D., Marchetto, J., Airaghi, S., Koumoutsakos, P., (2002), Optimization based on bacterial chemotaxis, IEEE transactions on evolutionary computation, 6:16-29 Rizzi, A, Bianco, G., and Cassinis, R. (1998) A bee-inspired visual homing using colour images. Robotics and autonomous Systems, 25:159-164. Rizzi, A., Duina, D., Inelli, S. and Cassinis, R. (2001) A novel visual landmark matching for a biologically inspired homing. Pattern Recognition Letters 22:13711378. Röfer, T. (1995). Image based homing using a self-organizing feature map. Proceedings of the International Conference on Artificial Neural Networks. EC2 & Cie., 1, 475-480 Röfer, T., (1997) Controlling a wheelchair with image-based homing, AISB Workshop on Spatial Reasoning in Mobile Robots and Animals, Technical Report, UMCS-97-4-1, Department of Computer Science, Manchester University.

74 RoyChowdhury, P., Singh, Y. P. and Chansarkar, R. A. (2000). Hybridization of Gradient Descent Algorithms with Dynamic Tunnelling Methods for Global Optimization, IEEE Transactions on Systems, Man, and Cybernetics – Part A: Systems and Humans, 30, 384-390. Szenher, M. (2004), Visual Homing, IAR-4 lecture notes, School of Informatics, University of Edinburgh, 2004. Szenher, M. (2005) Visual Homing in Natural Environments, (to appear in) Proceedings of TAROS, September 2005, London, UK. Weber, K., Venkatesh, S., Srinivasan, M. V. (1998) An Insect-Based Approach to Robotic Homing, Proceedings of the 14th International Conference on Pattern Recognition, 297-299. Wong C.-C. and Chen C.-C. (1999), A hybrid clustering and gradient descent approach for fuzzy modelling, IEEE Transactions on Systems, Man and Cybernetics, Part B, 29, 686 – 693 Yang, H. & Amari, S. (1996). A stochastic natural gradient descent algorithm for blind signal separation, Neural Networks for Signal Processing VI. Proceedings of the 1996 IEEE Signal Processing Society Workshop, 433-442. Zampoglou M. (2004), Learning or Evolving Controllers for Image-Based Homing, Informatics Research Review, University of Edinburgh, Dept. of Informatics, Supervisor: Dr. Barbara Webb, December 2004. Zampoglou, M. (2005). Learning or evolving controllers for image-based homing, Informatics Research Proposal, University of Edinburgh, Dept. of Informatics Supervisor: Dr Barbara Webb, December 2004 Zeil, J., Hoffmenn, M. and Chahl, J. S. (2003), Catchment areas of panoramic snapshots in outdoor scenes, Journal of the Optical Society of America A, 20:450469

75

Appendix A: Matlab Code unfoldimage.m function imout=unfoldimage(imin) % Unfolds a panoramic image. The parameters apply to the Vardy images. horizonRadius=145; % The distance of the horizon line from the mirror centre rBuffer=130; % Determines the width of the image (2*rBuffer) mirrorCenterR=282; mirrorCenterC=376; rInner=horizonRadius-rBuffer; rOuter=horizonRadius+rBuffer; for r=[rOuter:-1:rInner]+1 % For each distance from the centre (i.e. for each unfolded image row) for theta=1:360 % For each angle (i.e. for each rads=theta*pi/180; row=round(mirrorCenterR+r*sin(rads)); col=round(mirrorCenterC+r*cos(rads)); imout(r-rInner,round(theta),i,j)=imin(row,col,i,j); end end getalldifferences.m function diffmap=getalldifferences(homeim,images) % Builds an RMS surface of size MxN, by comparing 'homeim' to all the images in % 'images'. The latter is a (X,Y,M,N) matrix containinf MxN XxY images d=size(images); % Calculate all the RMS differences and put them in their corresponding % positions in 'diffmap' for i=1:d(3) for j=1:d(4) diffmap(i,j)=sqrt(sum(sum((double(homeim)-double(images(:,:,i,j))).^2))/(d(1)*d(2))); end end % Build a random image by shuffling the home image randomimage=reshape(homeim,d(1)*d(2),1); randind=randperm(d(1)*d(2)); randomimage=randomimage(randind); randomimage=reshape(randomimage,261,360);

76 % Estimate the maximum RMS difference by calculating the home image with % its shuffled version. maxdiff=sqrt(sum(sum((double(homeim)-double(randomimage)).^2))/(d(1)*d(2))); % Normalize the RMS differences diffmap=diffmap/maxdiff; evolvefunction.m % Generates and evolves a population of 200 individuals % It is a script, not a function, so as to be possible to break it while % running and still keep the population so far. % An array of size (X,Y,N) named maps must exist in the workspace, % containing N>1 RMS surfaces of dimensions (X,Y) rand('state',sum(100*clock)); popsize=200; % Generate random initial population. Each individual is [Omega, Z0, Z1, U] % Initial value ranges selected experimentally population=[rand(popsize,1)*6.28-3.14 rand(popsize,1)*20-10 rand(popsize,1)*120-60 rand(popsize,1)*3]; for i=1:20 % Run for 250 generations disp(i); values=evaluate(population,maps); % Evaluate the current population disp(max(values)); intermediate=selection(population,values); % Select the intermediate population crossed=crossover(intermediate); % Perform crossover on the controllers mutated=mutate(crossed); % Perform mutation on the controllers population=mutated; %replace the old population end values=exhausteval(population,maps); % Perform exhaustive evaluation on the final population clear popsize i intermediate crossed mutated; evaluate.m function [values]=evaluate(population,maps) % Performs a run on each surface of 'maps' and returns the fitness of all % individuals rand('state',sum(100*clock)); map_d=size(maps); % Locate the coordinates of the global minimum on the first surface. It is % assumed that all other surfaces also have their global minimum on the % same coordinates. [centre(1), centre(2)]=find(maps(:,:,1)==min(min(maps(:,:,1)))); d=size(population); for i=1:d(1) % For all individuals

77 values(i)=0; for j=1:map_d(3) % For all surfaces % Calculate the distance r between the start positions and the home % position. It is assumed that the home position is at least 2 % units away from the edge of the map. r=min([(map_d(1)-centre(1)-1) (centre(1)-1)]); % Randomly get the x coordinate of the start position x(j)=rand*2*r+centre(1)-r; % Decide in which half of the map the corresponding y coordinate % will be. half(j)=round(rand); % Calculate y using the circle function switch half(j) case 0 y(j)=sqrt(r^2-(x(j)-centre(1))^2)+centre(2); case 1 y(j)=sqrt(r^2-(x(j)-centre(1))^2)+centre(2); end % The initial orientation is orthogonal to the true home vector angle=atan(-(centre(2)-y(1))/(centre(1)-x(j))); init=[x(j) y(j) angle]; % Run the simulation for the current individual, map, and start % position values(i)=values(i)+derivsim(init, maps(:,:,j), population(i,:)); end if values(i)==0 values(i)=1; end end disp([sum(values>1) max(values)]); % Divide the values with their total sum, so that they represent their % probability of passing to the next generation (fitness) values=values./sum(values); selection.m function intermediate=selection(population,values) % Get the intermediate population from a given population and their % fitnesses through Stochastic Universal Sampling rand('state',sum(100*clock)); d=size(population); a=rand/d(1); % Create the first pointer, between 0 and 1/200. ind=1; for i=1:d(1) while sum(values(1:ind))Closest(1) OtherX=[ceil(pos(1)) Closest(2) map(ceil(pos(1)),Closest(2))]; else OtherX=[floor(pos(1)) Closest(2) map(floor(pos(1)),Closest(2))]; end C=(Closest(3)+OtherX(3))/2; end else if round(pos(2))~=pos(2) if pos(2)>Closest(2) OtherY=[Closest(1) ceil(pos(2)) map(Closest(1),ceil(pos(2)))]; else OtherY=[Closest(1) floor(pos(2)) map(Closest(1),floor(pos(2)))]; end C=(Closest(3)+OtherY(3))/2; else C=map(pos(1),pos(2)); end end % Add noise to the input C=C+randn*0.002; % Calculate the input gradient gradC=C-oldC; % Calculate the angle to turn (noise added here, instead of when the % move is performed). dtheta= Omega+Z0*C+Z1*gradC+randn*0.0005; % Turn the agent pos(3)=pos(3)+dtheta; MoveNoise=randn*0.0005; % Move the agent forward pos(1:2)=pos(1:2)+u*C*[cos(pos(3)) sin(pos(3))]+MoveNoise; % Ensure the angle added to the total angle sum is the shortest one % possible to get to the new orientation. i.e. modulate by 2*pi and % ensure that if the angle is >pi, the agent turns on the other % direction. tmptheta=mod(dtheta,2*pi); if tmptheta>pi tmptheta=tmptheta-2*pi; end if tmpthetaClosest(2) OtherY=[Closest(1),ceil(pos(2)),map(Closest(1),ceil(pos(2)))]; else OtherY=[Closest(1),floor(pos(2)),map(Closest(1),floor(pos(2)))]; end

82 coeffs=inv([Closest(1) Closest(2) 1;OtherX(1) OtherX(2) 1;OtherY(1) OtherY(2) 1])*[Closest(3);OtherX(3);OtherY(3)]; C=[pos(1) pos(2) 1]*coeffs; else if pos(1)>Closest(1) OtherX=[ceil(pos(1)) Closest(2) map(ceil(pos(1)),Closest(2))]; else OtherX=[floor(pos(1)) Closest(2) map(floor(pos(1)),Closest(2))]; end C=(Closest(3)+OtherX(3))/2; end else if round(pos(2))~=pos(2) if pos(2)>Closest(2) OtherY=[Closest(1) ceil(pos(2)) map(Closest(1),ceil(pos(2)))]; else OtherY=[Closest(1) floor(pos(2)) map(Closest(1),floor(pos(2)))]; end C=(Closest(3)+OtherY(3))/2; else C=map(pos(1),pos(2)); end end C=C+randn*0.002; gradC=C-oldC; % If the input has increased, turn 90 degrees. if gradC>0 dtheta= pi/2+randn*0.0005; else dtheta=randn*0.0005; end pos(3)=pos(3)+dtheta; MoveNoise=randn*0.0005; pos(1:2)=pos(1:2)+u*C*[cos(pos(3)) sin(pos(3))]+MoveNoise; totang=totang+dtheta; totdist=totdist+u*C++MoveNoise; pos(3)=mod(pos(3),2*pi); oldC=C; end if C height) && (col < edgeDetectedImage -> width)) edgeDetectedImage->data[row][col][0] = 128; } //end of TESTING //output labelled image, with mirror edge and horizon circle FileSaveImage ("edge_detected_image.pgm", edgeDetectedImage); //ask user to look at labelled image to determine if its okay printf ("Look at the image called edge_detected_image.ppm to determine if the mirror has been correctly detected. Has it (y/n)?"); scanf ("%s", yOrN); //if so, then stop calibrating if (strcmp (yOrN, "y") == 0) done_calibrating = 1; //free memory ImageDestroy ( edgeDetectedImage ); if (inCalibrationImage == 0) ImageDestroy ( imgPPM ); } } void DoImageWarpingOffline2 ( int mirrorCenterC, int mirrorCenterR, int mirrorRadius, int horizonRadius ) // Written by Matthew Szenher { int error; int width, height, depth; char fileName[100]; int row, col; int refRow = 5, refCol = 6; sprintf (fileName, "images15March05Test/snapshot.ppm"); Image * snapshotImgPPM = FileLoadImage (fileName, &error); Image * snapshotImgPGM = ConvertImage (snapshotImgPPM, (ImageFormat) pgm);

92 int i; for (i = 1; i PI) { dTheta=dTheta-2*PI; printf("was too large, is now %f\n",dTheta); } if (dThetaRMSThresh) // Turn in the appropriate direction { if (dTheta < 0) { durationOfTurning = 16.0 * PI * fabs(dTheta/PI*180.0) / 171.5; //(old: 16.0 * PI * fabs(dTheta) / 162.0) error = SendRobotCommand ("D,2,-2"); } else { durationOfTurning = 16.0 * PI * fabs(dTheta/PI*180.0) / 165.5; //(old: 16.0 * PI * fabs(dTheta) / 162.0) error = SendRobotCommand ("D,-2,2"); } Pause (durationOfTurning); error = SendRobotCommand ("D,0,0"); // Then move forward error = SendRobotCommand ("D,10,10"); Pause (durationOfMovement); error = SendRobotCommand ("D,0,0"); } Pause (1.0);

101 *OldC=C; } void UnfoldImage(Image * PanorImagePPM,char *OutFilename,int horizonRadius,int rBuffer,int mirrorCenterR,int mirrorCenterC) // Written by Markos Zampoglou // Takes an image, unfolds it and saves it in OutFilename { int rInner = horizonRadius - rBuffer; int rOuter = horizonRadius + rBuffer; Image * unwrappedSubImage = ImageCreate ( 360 /*PanorImage -> width*/, rBuffer * 2 + 1, pgm ); // Tell Matt about that! int error; int r, theta; Image * PanorImage; PanorImage = ConvertImage (PanorImagePPM, (ImageFormat) pgm); ImageDestroy ( PanorImagePPM ); for (r = rOuter; r >= rInner; r--) { for (theta = 0; theta < 360 ; theta++) // 1 column per degree, the unfolded image has width of 360 { float rads = theta * PI/180; int row = mirrorCenterR + r * sin (rads); int col = mirrorCenterC + r * cos (rads); unwrappedSubImage -> data [r - rInner][theta][0] = PanorImage -> data [row][col][0]; } //end of for r } //end of for theta FileSaveImage (OutFilename, unwrappedSubImage); } Image* ShiftImage (Image* CurrentImage, int Columns) // Written by Markos Zampoglou // Shifting the image by a number of columns equal to 'Columns', modelling // rotation by an equal amount of degrees { int i,j; int tmpCols[CurrentImage->height] [Columns]; for (i=CurrentImage->width-Columns;iwidth;i++) for (j=0;jheight;j++) tmpCols[j][i-CurrentImage->width+Columns]=CurrentImage->data[j][i]; for (i=CurrentImage->width-1;i>=Columns;i--) for (j=0;jheight;j++) CurrentImage->data[j][i]=CurrentImage->data[j][i-Columns]; for (i=0;idata[j][i]=tmpCols[j][i]; return(CurrentImage); } void BuildRandomImage(char * Filename) // Written by Markos Zampoglou // Shuffle image 'Filename', thus creating the random image, // and save it in 'Random_Image.ppm' { Image* HomeImagePPM;

102 Image* HomeImage; int i; int j; int TmpRandX1; int TmpRandY1; int TmpRandX2; int TmpRandY2; int TmpImValue; int error; HomeImagePPM = FileLoadImage (Filename, &error); HomeImage = ConvertImage (HomeImagePPM, (ImageFormat) pgm); ImageDestroy ( HomeImagePPM ); srand((unsigned int)time(NULL)); for(i=0; i height*HomeImage -> width)*20; i++) // Performs card deck shuffling, by interchanging positions for two randomly // selected pixels at each turn. The total number of changes is width*height*20, ensuring // an adequately large shuffling to destroy all spatial correlations between pixels { TmpRandX1=rand()*((HomeImage -> height)/(2147483647.0)); TmpRandY1=rand()*((HomeImage -> width)/(2147483647.0)); TmpRandX2=rand()*((HomeImage -> height)/(2147483647.0)); TmpRandY2=rand()*((HomeImage -> width)/(2147483647.0)); TmpImValue=HomeImage->data[TmpRandX1][TmpRandY1][0]; HomeImage->data[TmpRandX1][TmpRandY1][0]=HomeImage>data[TmpRandX2][TmpRandY2][0]; HomeImage->data[TmpRandX2][TmpRandY2][0]=TmpImValue; } FileSaveImage ("Random_Image.ppm", HomeImage); ImageDestroy(HomeImage); } double compute_RMS_diff ( Image * snapshotPGM, Image * currentPGM ) // Written by Markos Zampoglou // Simply computes the RMS difference between two images { int i, j; int intensityS, intensityC; double rms = 0; int diff; for (i = 0; i < snapshotPGM->height; i++) { for (j = 0; j < snapshotPGM->width; j++) { intensityS = snapshotPGM -> data[i][j][0]; intensityC = currentPGM -> data[i][j][0]; diff = intensityS - intensityC; rms = rms + diff * diff; } } return sqrt(rms/( (double)snapshotPGM->width * (double)snapshotPGM->height)); }

103 double ComputeMinRMSDiff (char * s_fileName, char * c_fileName) // Written by Markos Zampoglou // Estimates the RMS difference for all 360 rotations, and keeps the smallest one, // assuming that is the one referring to the snapshot image orientation. This is the // function used by the RMS-based algorithms for RMS diference estimation { int Count=0; int i, min_Index, error; double MinRMS,TempRMS; Image* Current_Image = FileLoadImage (s_fileName, &error); Image* Home_Image = FileLoadImage (c_fileName, &error); MinRMS=compute_RMS_diff (Current_Image,Home_Image); min_Index=0; // For all rotations for (i=1;i= rInner; r--) { for (theta = 0; theta < hImagePGM -> width; theta++) { float rads = theta * PI/180; int row = mirrorCenterR + r * sin (rads); int col = mirrorCenterC + r * cos (rads); unwrappedSubImage -> data [r - rInner][theta][0] = panoramicImagePGM -> data [row][col][0]; } //end of for r } //end of for theta Image * hImagePGMTemp = ImageCreate (hImagePGM -> width, hImagePGM -> height, pgm); //reduce subImage to 1-D horizon array for (theta = 0; theta < hImagePGM -> width; theta ++) {

107 int intensitySum = 0; for (r = 0; r data[r][theta][0]; } hImagePGMTemp ->data[0][theta][0] = intensitySum / (2 * rBuffer + 1); } //perform guassian smoothing on horizon array float gaussWin [] = {0.0045, 0.0072, 0.0109, 0.0160, 0.0225, 0.0303, 0.0393, 0.0490, 0.0587, 0.0675, 0.0746, 0.0792, 0.0808, 0.0792, 0.0746, 0.0675, 0.0587, 0.0490, 0.0393, 0.0303, 0.0225, 0.0160, 0.0109, 0.0072, 0.0045}; int filterLen = 25; //must be an odd number int i; float weightedSum; int intensityVal; int index; for (theta = 0; theta < hImagePGM -> width; theta ++) { weightedSum = 0.0; for (i = 0; i < filterLen; i++) { index = (int) fmod (hImagePGM -> width + theta + i - filterLen/2, hImagePGM -> width); intensityVal = hImagePGMTemp -> data [0][ index ][0]; weightedSum += (float) gaussWin [i] * (float) intensityVal; } hImagePGM -> data[0][theta][0] = (int) weightedSum; } ImageDestroy (unwrappedSubImage); ImageDestroy (hImagePGMTemp); } void transform_Current_Horizon_Image ( const Image * hCurrent, float rho, float alpha, float phi, Image * hTransformed ) // Written by Matthew Szenher // Transforms the horizon image for rho, alpha and phi. { float delta = 0; int theta; // do the tranformation for (theta = 0; theta < hCurrent -> width; theta++) { if ((1 - rho * cos ( (theta - alpha) * PI/180)) != 0) { delta = -phi*PI/180 + atan (rho * sin ( (theta - alpha) * PI/180 ) / (1 - rho * cos ( (theta alpha) * PI/180)) ); delta = delta * 180 / PI; //delta = (int) (delta); } else { delta = 0; } delta = (int) delta % (hCurrent -> width); hTransformed ->data[0][ (theta + (int) delta + hTransformed -> width) % hTransformed -> width

108 ][0] = hCurrent ->data[0][ theta ][0]; } } float compute_HorizonImage_Correlation ( const Image * hImage1, const Image * hImage2 ) // Written by Matthew Szenher // Computes the internal product between two horizon images (lines) { Image * normalizedHImage1 = ImageCopy (hImage1); Image * normalizedHImage2 = ImageCopy (hImage2); int mean1 = ComputeMeanIntensity (normalizedHImage1); int mean2 = ComputeMeanIntensity (normalizedHImage2); DoImageWithScalarArithmeticInImage (normalizedHImage1, mean1, subtract); DoImageWithScalarArithmeticInImage (normalizedHImage2, mean2, subtract); DoImageDotProduct (normalizedHImage2, normalizedHImage2); float corr = DoImageDotProduct (normalizedHImage1, normalizedHImage2) / sqrt (DoImageDotProduct (normalizedHImage1, normalizedHImage1) * DoImageDotProduct (normalizedHImage2, normalizedHImage2)); corr = 1.0 - fabs(corr); ImageDestroy (normalizedHImage1); ImageDestroy (normalizedHImage2); return corr; } /* END OF IMAGE WARPING CODE */ image.h #ifndef IMAGE_H #define IMAGE_H #define pi 3.14159 enum e_ImageFormat {ppm, pgm}; typedef enum e_ImageFormat ImageFormat; enum e_ImageArithmeticOp {add, subtract, multiply, divide}; typedef enum e_ImageArithmeticOp ImageArithmeticOp; typedef struct _Image { int width; int height; int depth; ImageFormat iFormat; int*** data; } Image; typedef struct _Mask { int width; int height; double** data; } Mask; void CheckMalloc (void *, const char *);

109 Image* ImageCreate(int width, int height, ImageFormat f); Image* UnsignedCharArray2Image (int width, int height, ImageFormat f, const unsigned char * in_arr); void ImageDestroy(Image* image); Image * ImageCopy(const Image* src_image); void ImageClear(Image* image); Image* FileLoadImage(const char* file_name, int *error); int FileSaveImage(const char * file_name, const Image* image); Image * ConvertImage (const Image* src_image, ImageFormat dst_format); int ImagesEqualSize (const Image* im1, const Image* im2); void PrintImageData ( const Image* ); int ComputeMeanIntensity (const Image* im); Image * DoImageWithScalarArithmetic (const Image* src_image, int scalar, ImageArithmeticOp op); void DoImageWithScalarArithmeticInImage (Image* src_image, int scalar, ImageArithmeticOp op); Image * DoImageWithImageArithmetic (const Image* src_image1, const Image* src_image2, ImageArithmeticOp op); void DoImageWithImageArithmeticInImage (Image* src_image1, const Image* src_image2, ImageArithmeticOp op); double DoImageDotProduct (const Image* src_image1, const Image* src_image2); /* IMAGE FEATURE ALGS*/ Image * FindEdgesSobel (const Image * inImg); Image * FindEdgesCanny (const Image * inImg); Image * FindCircleHough (const Image * inImg, int inRadius, int minCol, int maxCol, int *outR, int *outC, int *outRadius); /* MASK FUNCTIONS */ Mask * MaskCreate(int width, int height); Mask * MaskCreateGaussianZeroMeanNormalized (int width, int height, float sd); void MaskDestroy(Mask* m); void MaskClear(Mask* m); void PrintMaskData (const Mask* m1); #endif image.c #include #include #include #include "image.h" // Auxiliary Image Processing functions. Entirely written by Matthew Szenher void CheckMalloc (void *newMem, const char * comment) { if (newMem == 0) { printf ("Malloc failed. Exiting. %s\n", comment); exit (EXIT_FAILURE); } } Image* ImageCreate(int width, int height, ImageFormat f) {

110 Image * newI = (Image *) malloc ( sizeof (Image) ); newI -> width = width; newI -> height = height; newI -> iFormat = f; switch (f) { case ppm: newI -> depth = 3; break; case pgm: newI -> depth = 1; break; default: printf ("TRYING TO CREATE UNKNOWN IMAGE FORMAT\n"); break; } int r, c; newI -> data = (int ***) malloc (height * sizeof (int **)); CheckMalloc (newI -> data, "In ImageCreate."); for (r = 0; r < height; r++) { newI -> data[r] = (int **) malloc ( width * sizeof (int *)); CheckMalloc (newI -> data[r], "In ImageCreate - for r."); for (c = 0; c < width; c++) { newI -> data[r][c] = (int *) malloc (newI->depth * sizeof (int)); CheckMalloc (newI -> data[r][c], "In ImageCreate - for c."); } } return newI; } Image* UnsignedCharArray2Image (int width, int height, ImageFormat f, const unsigned char * in_arr) { Image* newI = ImageCreate(width, height, f); int unsignedCharArrInd = 0; int row, col, depth; for (row = 0; row < height; row++) for (col = 0; col < width; col++) for (depth = 0; depth < newI -> depth; depth++) { newI -> data[row][col][depth] = in_arr[unsignedCharArrInd]; unsignedCharArrInd++; } return newI; } void ImageDestroy(Image* image) { if (image != 0) { if (image -> data != 0) { int r, c, d;

111 for (r = 0; r < image->height; r++) { for (c = 0; c < image->width; c++) { free (image -> data[r][c]); image->data[r][c] = 0; } free (image -> data[r]); image->data[r] = 0; } free ( image -> data ); image->data = 0; } free (image); image = 0; } } Image * ImageCopy(const Image* src_image) { if (src_image == 0) { printf ("ATTEMPTING TO COPY NULL SOURCE\n"); return; } Image * dst_image = ImageCreate ( src_image -> width, src_image -> height, src_image -> iFormat ); //copy data int r, c, d; for (r = 0; r < dst_image->height; r++) for (c = 0; c < dst_image->width; c++) for (d = 0; d < dst_image -> depth; d++) dst_image -> data[r][c][d] = src_image -> data[r][c][d]; return dst_image; } void ImageClear(Image* image) { if (image -> data != 0) { int r, c, d; for (r = 0; r < image->height; r++) for (c = 0; c < image->width; c++) for (d = 0; d < image -> depth; d++) image -> data[r][c][d] = 0; } } Image* FileLoadImage(const char* file_name, int *error) { Image* newI = 0; *error = 0; //assume no errors

112 FILE * fp = fopen( file_name , "r" ); if (fp == NULL) { *error = 1; return 0; } int width, height; char iFormat[100]; fscanf( fp , "%s %d %d 255\n" , iFormat, &width, &height ); if (strcmp ("P6", iFormat) == 0) //ppm newI = ImageCreate (width, height, (ImageFormat) ppm); else if (strcmp ("P5", iFormat) == 0) //pgm newI = ImageCreate (width, height, (ImageFormat) pgm); else { printf ("ATTEMPTING TO OPEN IMAGE OF UNKNOWN FORMAT %s\n", file_name); return 0; } //read the data int r, c, d; unsigned char val; for (r = 0; r < height; r++) for (c = 0; c < width; c++) for (d = 0; d < newI -> depth; d++) { fscanf (fp, "%c", &val); newI -> data [r][c][d] = (int) val; } fclose(fp); return newI; } int FileSaveImage(const char * file_name, const Image* image) { if (image == 0) { printf ("ATTEMPTING TO SAVE NULL IMAGE\n"); return 0; } FILE * fp = fopen( file_name , "w" ); if (fp == NULL) return 0; int size, i; switch (image -> iFormat) { case ppm: fprintf( fp , "P6 %d %d 255\n" , image -> width, image -> height ); break; case pgm: fprintf( fp , "P5 %d %d 255\n" , image -> width, image -> height ); break; default: printf ("ATTEMPTING TO SAVE IMAGE OF UNKNOWN FORMAT %s\n", file_name); return 0; break; }

113 //print the data int r, c, d, val; for (r = 0; r < image -> height; r++) for (c = 0; c < image -> width; c++) for (d = 0; d < image -> depth; d++) { val = image -> data [r][c][d]; fprintf (fp, "%c", val); } fclose(fp); return 0; } Image * ConvertImage (const Image* src_image, ImageFormat dst_format) { Image * dst_image = 0; if ( src_image -> iFormat == dst_format ) { dst_image = ImageCopy(src_image); } else if (( src_image -> iFormat == (ImageFormat) ppm) && ( dst_format == (ImageFormat) pgm)) { dst_image = ImageCreate (src_image -> width, src_image -> height, (ImageFormat) pgm); //convert rgb data to intensity int r, c; int intensity; for (r = 0; r < src_image -> height; r++) { for (c = 0; c < src_image -> width; c++) { int red = src_image->data[r][c][0]; int green = src_image->data[r][c][1]; int blue = src_image->data[r][c][2]; intensity = ((red + green + blue) / 3); dst_image->data[r][c][0] = intensity; } } } else { printf ("CANNOT PERFORM REQUESTED IMAGE CONVERSION\n"); } return dst_image; } void PrintImageData (const Image* im1) { int r, c, d; for ( r = 0; r < im1 -> height; r++ ) { for ( c = 0; c < im1 -> width; c++) { for ( d = 0; d < im1 -> depth; d++) { printf ("%d ", im1 -> data [r][c][d]);

114 } } printf ("\n"); } } /* DO IMAGE COMPARISONS */ int ImagesEqualSize (const Image* im1, const Image* im2) { if ((im1 -> height == im2 -> height) && (im1 -> width == im2 -> width) && (im1 -> depth == im2 > depth)) return 1; else return 0; } /* END OF IMAGE COMPARISONS */ /* DO IMAGE ARITHMETIC AND STATS */ int ComputeMeanIntensity (const Image* im) { if (im -> depth != 1) return 0; else { int r, c; int sum = 0; for (r = 0; r < im -> height; r++) for (c = 0; c < im -> width; c++) sum += im->data[r][c][0]; return sum / (im->width * im->height); } } Image * DoImageWithScalarArithmetic (const Image* src_image, int scalar, ImageArithmeticOp op) { Image * newI = ImageCopy ( src_image ); int r,c,d; switch (op) { case add: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) newI->data[r][c][d] += scalar; break; case subtract: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) newI->data[r][c][d] -= scalar; break; case multiply: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) newI->data[r][c][d] *= scalar; break;

115 case divide: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) newI->data[r][c][d] /= scalar; break; } return newI; } void DoImageWithScalarArithmeticInImage (Image* src_image, int scalar, ImageArithmeticOp op) { int r, c, d; switch (op) { case add: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) src_image->data[r][c][d] += scalar; break; case subtract: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) src_image->data[r][c][d] -= scalar; break; case multiply: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) src_image->data[r][c][d] *= scalar; break; case divide: for (r = 0; r < src_image -> height; r++) for (c = 0; c < src_image -> width; c++) for (d = 0; d < src_image -> depth; d++) src_image->data[r][c][d] /= scalar; break; } } Image * DoImageWithImageArithmetic (const Image* src_image1, const Image* src_image2, ImageArithmeticOp op) { Image * newI = ImageCopy ( src_image1 ); int r,c,d; switch (op) { case add: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) newI->data[r][c][d] += src_image2->data[r][c][d]; break;

116 case subtract: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) newI->data[r][c][d] += src_image2->data[r][c][d]; break; case multiply: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) newI->data[r][c][d] *= src_image2->data[r][c][d]; break; case divide: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) newI->data[r][c][d] /= src_image2->data[r][c][d]; break; } return newI; } void DoImageWithImageArithmeticInImage (Image* src_image1, const Image* src_image2, ImageArithmeticOp op) { int r, c, d; switch (op) { case add: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) src_image1->data[r][c][d] += src_image2->data[r][c][d]; break; case subtract: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) src_image1->data[r][c][d] -= src_image2->data[r][c][d]; break; case multiply: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) src_image1->data[r][c][d] *= src_image2->data[r][c][d]; break; case divide: for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) src_image1->data[r][c][d] /= src_image2->data[r][c][d]; break;

117 } } double DoImageDotProduct (const Image* src_image1, const Image* src_image2) { int r,c,d; double dot = 0; for (r = 0; r < src_image1 -> height; r++) for (c = 0; c < src_image1 -> width; c++) for (d = 0; d < src_image1 -> depth; d++) dot += (double)src_image1->data[r][c][d] * (double)src_image2->data[r][c][d]; return dot; } Image * ConvolveImage (const Image* src_image, const Mask * m) { Image * newI = ImageCopy ( src_image ); //apply the mask to each source image point int r, c, d; for (r = 0; r < src_image -> height; r ++) { for (c = 0; c < src_image -> width; c ++) { for (d = 0; d < src_image -> depth; d ++) { //iterate through all mask values int rM, cM, rI, cI; float newPointVal = 0; float maskVal; int imageVal; int error; for (rM = 0; rM < m -> height; rM ++) { for (cM = 0; cM < m -> width; cM ++) { maskVal = m -> data[rM][cM]; rI = r + (rM - m->height/2); cI = c + (cM - m->width/2); if ((rI < 0) || (cI < 0) || (rI >= src_image -> height) || (cI >= src_image -> width)) imageVal = 0.0; else imageVal = src_image->data[rI][cI][d]; newPointVal += (float) imageVal * maskVal; } } //end of iteration through mask points newI -> data[r][c][d] = (int) newPointVal; } } } //end of iteration through all image points return newI; } /* IMAGE FEATURE ALGS */ Image * FindEdgesSobel (const Image * inImg) { //edge detection gradient threshold

118 int threshold1 = 60; //make the image greyscale if it isn't already Image * origImg = ConvertImage (inImg, pgm); //create an output image Image * outImg = ImageCreate (inImg -> width, inImg -> height, pgm); DoImageWithScalarArithmeticInImage (outImg, 255, add); //outImg is initially all white; edges will be black //smooth the image Mask * gaussMask = MaskCreateGaussianZeroMeanNormalized (5, 5, 1.4); Image * smoothedImg = ConvolveImage (origImg, gaussMask); MaskDestroy (gaussMask); //find gradients Mask *gxMask = MaskCreate (3,3); gxMask->data[0][0]=-1; gxMask->data[0][1]=0; gxMask->data[0][2]=1; gxMask->data[1][0]=-2; gxMask->data[1][1]=0; gxMask->data[1][2]=2; gxMask->data[2][0]=-1; gxMask->data[2][1]=0; gxMask->data[2][2]=1; Image * gxImg = ConvolveImage (smoothedImg, gxMask); Mask *gyMask = MaskCreate (3,3); gyMask->data[0][0]=1; gyMask->data[0][1]=2; gyMask->data[0][2]=1; gyMask->data[1][0]=0; gyMask->data[1][1]=0; gyMask->data[1][2]=0; gyMask->data[2][0]=-1; gyMask->data[2][1]=-2; gyMask->data[2][2]=-1; Image * gyImg = ConvolveImage (smoothedImg, gyMask); //compute magnitude of gradient at each pixel Image * gMag = ImageCreate (origImg -> width, origImg -> height, pgm); int r, c; for (r = 0; r < origImg -> height; r++) for (c = 0; c < origImg -> width; c++) gMag -> data[r][c][0] = sqrt (gxImg->data[r][c][0] * gxImg->data[r][c][0] + gyImg->data[r][c][0] * gyImg->data[r][c][0]); //find edges based on gradient magnitude (Sobel) for (r = 0; r < outImg -> height; r++) for (c = 0; c < outImg -> width; c++) if (gMag->data[r][c][0] > threshold1) outImg->data[r][c][0] = 0; else outImg->data[r][c][0] = 255; ImageDestroy (smoothedImg); ImageDestroy (gxImg); ImageDestroy (gyImg); MaskDestroy (gxMask); MaskDestroy (gyMask);

119 ImageDestroy (gMag); ImageDestroy (origImg); return outImg; } Image * FindEdgesCanny (const Image * inImg) { //edge detection gradient threshold int threshold1 = 60, threshold2 = 40; //make the image greyscale if it isn't already Image * origImg = ConvertImage (inImg, pgm); //create an output image Image * outImg = ImageCreate (inImg -> width, inImg -> height, pgm); DoImageWithScalarArithmeticInImage (outImg, 255, add); //outImg is initially all white; edges will be black //smooth the image Mask * gaussMask = MaskCreateGaussianZeroMeanNormalized (5, 5, 1.4); Image * smoothedImg = ConvolveImage (origImg, gaussMask); MaskDestroy (gaussMask); //find gradients Mask *gxMask = MaskCreate (3,3); gxMask->data[0][0]=-1; gxMask->data[0][1]=0; gxMask->data[0][2]=1; gxMask->data[1][0]=-2; gxMask->data[1][1]=0; gxMask->data[1][2]=2; gxMask->data[2][0]=-1; gxMask->data[2][1]=0; gxMask->data[2][2]=1; Image * gxImg = ConvolveImage (smoothedImg, gxMask); Mask *gyMask = MaskCreate (3,3); gyMask->data[0][0]=1; gyMask->data[0][1]=2; gyMask->data[0][2]=1; gyMask->data[1][0]=0; gyMask->data[1][1]=0; gyMask->data[1][2]=0; gyMask->data[2][0]=-1; gyMask->data[2][1]=-2; gyMask->data[2][2]=-1; Image * gyImg = ConvolveImage (smoothedImg, gyMask); //compute magnitude of gradient at each pixel Image * gMag = ImageCreate (origImg -> width, origImg -> height, pgm); int r, c; for (r = 0; r < origImg -> height; r++) for (c = 0; c < origImg -> width; c++) gMag -> data[r][c][0] = sqrt (gxImg->data[r][c][0] * gxImg->data[r][c][0] + gyImg->data[r][c][0] * gyImg->data[r][c][0]); //compute edge gradients Image * g = ImageCreate (origImg -> width, origImg -> height, pgm); for (r = 0; r < origImg -> height; r++)

120 for (c = 0; c < origImg -> width; c++) { int dir = 0; if (gxImg->data[r][c][0] == 0) if (gyImg->data[r][c][0] == 0) dir = 0; else dir = 90; else dir = (180 / pi) * atan (gyImg->data[r][c][0] / gxImg->data[r][c][0]); //now that gradient has been precisely calculated, pigeonhole it into 0, 45, 90, 135 or 180 if (((dir >= 0) && (dir = 157.5) && (dir 22.5) && (dir 67.5) && (dir data[r][c][0] = dir; } //perform nonmaximum suppression for (r = 0; r < origImg -> height; r++) for (c = 0; c < origImg -> width; c++) { //determine pixel gradient of current pixel int rInc = 0, cInc = 0; switch (g->data[r][c][0]) { case 0: rInc = 0; cInc = 1; break; case 45: rInc = -1; cInc = 1; break; case 90: rInc = -1; cInc = 0; break; case 135: rInc = -1; cInc = -1; break; default: printf ("ERROR IN EDGE GRADIENT!\n"); break; } //trace current edge in outImg int done = 0; int currR = r, currC = c; int followingEdgePixel = 0; while (!done) { if ((g->data[currR][currC][0] == g->data[r][c][0]) && (gMag->data[currR][currC][0] > threshold1)) { outImg -> data[currR][currC][0] = 0;

121 followingEdgePixel = 1; } else if ((g->data[currR][currC][0] == g->data[r][c][0]) && (gMag->data[currR][currC][0] > threshold2) && followingEdgePixel) { outImg -> data[currR][currC][0] = 0; followingEdgePixel = 1; } else followingEdgePixel = 0; currR += rInc; currC += cInc; if ((currR < 0) || (currC < 0) || (currR >= origImg -> height) || (currC >= origImg -> width) || (followingEdgePixel == 0)) done = 1; } //trace same edge in opposite direction done = 0; currR = r; currC = c; followingEdgePixel = 0; rInc *= -1; cInc *= -1; while (!done) { if ((g->data[currR][currC][0] == g->data[r][c][0]) && (gMag->data[currR][currC][0] > threshold1)) { outImg -> data[currR][currC][0] = 0; followingEdgePixel = 1; } else if ((g->data[currR][currC][0] == g->data[r][c][0]) && (gMag->data[currR][currC][0] > threshold2) && followingEdgePixel) { outImg -> data[currR][currC][0] = 0; followingEdgePixel = 1; } else followingEdgePixel = 0; currR += rInc; currC += cInc; if ((currR < 0) || (currC < 0) || (currR >= origImg -> height) || (currC >= origImg -> width) || (followingEdgePixel == 0)) done = 1; } } ImageDestroy (smoothedImg); ImageDestroy (gxImg); ImageDestroy (gyImg); MaskDestroy (gxMask); MaskDestroy (gyMask); ImageDestroy (gMag); ImageDestroy (origImg); ImageDestroy (g); return outImg; } Image * FindCircleHough (const Image * inImg, int inRadius, int minCol, int maxCol, int *outRow, int *outCol, int *outRadius) { //edge detect the input image Image * edgeDetectedImage = FindEdgesSobel (inImg); double largestAccumulatorValue = 0; int bestA = 0, bestB = 0, bestRadius = 0;

122 //Hough space counter for parameters //(a,b) is center of circle; 0 height); //loop control variables int a, b; int row, col; int radius; int radiusWindow = 2; //search for radius +- radiusWindow from inRadius for (radius = inRadius - radiusWindow; radius height; row++) for (col = 0; col < edgeDetectedImage -> width; col++) { if (edgeDetectedImage->data[row][col][0] == 0) { double inSqrt = 0; for (a = minCol; a < maxCol; a++) { inSqrt = (radius * radius - (col - a) * (col - a)); if (inSqrt >= 0) { inSqrt = sqrt (inSqrt); bPlus = (int) (row + inSqrt); bMinus = (int) (row - inSqrt); //increment accumulator and keep track of largest value in accumulator if ((bPlus >= 0) && (bPlus < inImg->height)) { accumulator->data[bPlus][a] += 1; //printf ("testing %d %d %d %d %d\n", col, row, a, bPlus, radius); } if ((bMinus >= 0) && (bMinus < inImg->height)) { accumulator->data[bMinus][a] += 1; //printf ("testing %d %d %d %d %d\n", col, row, a, bMinus, radius); } } //end of if-sqrt } //end of for-a } //end of if current pixel is an edge } // end of for-c, r //find the largest accumulator value for ( b = 0; b < accumulator -> height; b++ ) for ( a = 0; a < accumulator -> width; a++) {

123 if (accumulator->data[b][a] > largestAccumulatorValue) { largestAccumulatorValue = accumulator->data[b][a]; bestA = a; bestB = b; bestRadius = radius; } } printf ("a=%d b = %d lav = %f\n", bestA, bestB, largestAccumulatorValue); } //end of for-radius *outRow = bestB; *outCol = bestA; *outRadius = bestRadius; //draw the circle in the edge detected image and save (TESTING) int theta; //bestB = edgeDetectedImage -> height / 2; bestA = edgeDetectedImage->width/2; for (theta = 0; theta < 359; theta++) { row = bestB + bestRadius * sin (theta * pi / 180.0); col = bestA + bestRadius * cos (theta * pi / 180.0); if ((row >= 0) && (col >= 0) && (row < edgeDetectedImage -> height) && (col < edgeDetectedImage -> width)) edgeDetectedImage->data[row][col][0] = 128; } //end of TESTING //ImageDestroy (edgeDetectedImage); MaskDestroy (accumulator); return edgeDetectedImage; } /* END OF IMAGE PROCESSING ALGS */ /* MASK ALGS */ Mask * MaskCreate(int width, int height) { Mask * newM = (Mask *) malloc ( sizeof (Mask) ); newM -> width = width; newM -> height = height; int r; newM -> data = (double **) malloc (height * sizeof (double *)); for (r = 0; r < height; r++) { newM -> data[r] = (double *) malloc ( width * sizeof (double)); } return newM; } Mask * MaskCreateGaussianZeroMeanNormalized (int width, int height, float sd) { if (width % 2 == 0) width++; if (height % 2 == 0) height ++; Mask * gMask = MaskCreate (width, height); int r, c; double x, y, expVal; double sum = 0.0; for (r = 0; r < height; r++) for (c = 0; c < width; c++) { x = r - height/2; y = c - width/2;

124 expVal = -1 * (x * x + y * y) / (2 * sd * sd); gMask -> data[r][c] = (1.0/(2*pi*sd*sd)) * exp(expVal); sum += gMask -> data[r][c]; } //normalize so that area under curve is 1 double sum2 = 0.0; for (r = 0; r < height; r++) for (c = 0; c < width; c++) { gMask -> data[r][c] = gMask -> data[r][c] / sum; sum2 += gMask -> data[r][c]; } return gMask; } void MaskDestroy(Mask* m) { if (m != 0) { if (m -> data != 0) { int r, c; for (r = 0; r < m->height; r++) { free (m -> data[r]); } free ( m -> data ); } free (m); } } void MaskClear(Mask* m) { if (m -> data != 0) { int r, c; for (r = 0; r < m->height; r++) for (c = 0; c < m->width; c++) m -> data[r][c] = 0; } } void PrintMaskData (const Mask* m1) { int r, c; for ( r = 0; r < m1 -> height; r++ ) { for ( c = 0; c < m1 -> width; c++) { printf ("%f ", m1 -> data [r][c]); } printf ("\n"); } } serialio.h #include

125 #include /* String function definitions */ #include /* UNIX standard function definitions */ #include /* File control definitions */ #include /* Error number definitions */ #include /* POSIX terminal control definitions */ int open_port (); void config_port ( int, struct termios *, struct termios * ); void close_port ( int, struct termios ); serialio.c #include "serialio.h" // Serial communications functions. Entirely written by Matthew Szenher int open_port(void) { int fd; /* File descriptor for the port */ fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY); if (fd == -1) { /* * Could not open the port. */ perror("open_port: Unable to open /dev/ttyS0 - "); } else fcntl(fd, F_SETFL, 0); return (fd); } void config_port (int fd, struct termios *old_options, struct termios *new_options) { // * Get the current options for the port... tcgetattr(fd, old_options); *new_options = *old_options; //* Set the baud rates to 9600... cfsetispeed(new_options, B9600); cfsetospeed(new_options, B9600); //* Enable the receiver and set local mode... new_options->c_cflag |= (CLOCAL | CREAD); // * Disable the echo mode new_options->c_lflag &= ~(ECHO); // * Set the new options for the port... tcsetattr(fd, TCSANOW, new_options); //just to make sure, issue the same commands via unix system ("stty -F /dev/ttyS0 9600"); system ("stty -F /dev/ttyS0 -echo"); system ("stty -F /dev/ttyS0 min 1"); //system ("stty -F /dev/ttyS0"); } void close_port ( int fd, struct termios old_options ) { tcsetattr(fd, TCSANOW, &old_options); close (fd);

126 } v4linux.h /* Simple Video4Linux image grabber. */ #include #include #include #include #include #include #include #include #include #define FILE_NAME "/dev/video0" int open_camera (); int capture_image ( int, unsigned char*, int, int, int ); int close_camera ( int ); int get_camera_info ( int, struct video_capability *, struct video_window *, struct video_picture * ); v4linux.c #include "v4linux.h" // Camera control functions. Entirely written by Matthew Szenher. int open_camera () { int fd = open(FILE_NAME, O_RDONLY); if (fd < 0) { perror(FILE_NAME); return -1; } return fd; } int capture_image ( int fd, unsigned char* buffer, int width, int height, int bpp ) { read(fd, buffer, width * height * bpp); return 1; } int close_camera ( int fd ) { if (fd >= 0) { close (fd); return 1; } else return -1; } int get_camera_info ( int fd, struct video_capability* cap, struct video_window* win,

127 struct video_picture* vpic ) { if (ioctl(fd, VIDIOCGCAP, cap) < 0) { perror("VIDIOGCAP"); fprintf(stderr, "(" FILE_NAME " not a video4linux device?)\n"); return (-1); } if (ioctl(fd, VIDIOCGWIN, win) < 0) { perror("VIDIOCGWIN"); return -1; } if (ioctl(fd, VIDIOCGPICT, vpic) < 0) { perror("VIDIOCGPICT"); return -1; } if (cap->type & VID_TYPE_MONOCHROME) { vpic->depth=8; vpic->palette=VIDEO_PALETTE_GREY; /* 8bit grey */ if(ioctl(fd, VIDIOCSPICT, vpic) < 0) { vpic->depth=6; if(ioctl(fd, VIDIOCSPICT, vpic) < 0) { vpic->depth=4; if(ioctl(fd, VIDIOCSPICT, vpic) < 0) { fprintf(stderr, "Unable to find a supported capture format.\n"); return -1; } } } } else { vpic->depth=24; vpic->palette=VIDEO_PALETTE_RGB24; if(ioctl(fd, VIDIOCSPICT, vpic) < 0) { vpic->palette=VIDEO_PALETTE_RGB565; vpic->depth=16; if(ioctl(fd, VIDIOCSPICT, vpic)==-1) { vpic->palette=VIDEO_PALETTE_RGB555; vpic->depth=15; if(ioctl(fd, VIDIOCSPICT, vpic)==-1) { fprintf(stderr, "Unable to find a supported capture format.\n"); return -1; } } } } return 1; }