Eperdices TFM

• Project Name: Visual Self-Localization in the RoboCup with sampling-based algorithms.

• Authors: Eduardo Perdices (eperdices [at] gsyc [dot] es) and Jose María Cañas Plaza (jmplaza [at] gsyc [dot] es)

• Academic Year: 2009-2010

• Degree: Master

• Jde Version: jde-4.3

• SVN Repository: repository

• Tags: Nao robot, RoboCup, computer vision, location, montecarlo

• Technology: C, C++, JdeRobot, OpenCV, Nao Robot, BICA

• State: Developing

• Source License: GPLv3

• Documentation:

Masther Thesis: PDF - Docs_Sources
Slides Thesis: PDF - Slides_Sources

• Schemas:

RoboCup Self-Localization

2010.05.18. Real tests with evolutionary algorithm

Movement test:

Kidnapping:

2010.04.31. Webots tests with Monte Carlo algorithm

Movement test:

Kidnapping:

2010.04.15. Webots tests with evolutionary algorithm

Movement test:

Kidnapping without reset:

Kidnapping with reset:

2010.03.15. New explorers population

With the algorithm of the explorers that we show in 2010.02.19, we have realized that in some places of the field, the explorers selected are not the best. This is because in some places of the field, the real robot is too far from the candidate explorers. So that, to improve the algorithm, we now use more explorers in more places:

To save the efficiency of the algorithm, the health of all these explorers are calculated in two iteration, to keep a time average of 30 ms per iteration.

2010.03.11. Obstacles detection

To avoid false positives with obstacles, we have improved the obstacles detection algorithm, which now is based in the width and the height of big white parts of the image. When we detect an obstacle, we create a rectangle around it, and the points inside these rectangles are deleted. In the next video, we show an example of this new algorithm:

2010.03.08. Field border calculation

We have been using an algorithm to calculate the field border that covered the image each 10 columns and obtained the border field separately. The behavior of the algorithm seemed right, however, after testing the algorithm with obstacles, we have detected that it failed with long white lines:

To correct this wrong behavior, we have used an algorithm called convex hull, which uses as input the set of points detected in each column and returns the minimal subset of points that contains all the points of the input inside them. With this new algorithm, the result of the two previous images are:

2010.03.04. Real robot precision

To predict the behavior of the self-localization when we use it in the real competition, we have measured the error obtained with the real robot. Because of the big noise that we obtain in the field of the laboratory, we have placed the robot in positions with little noise, and these are the results:

Real--------------------Calculated---------------Error (mm)

(1610, -720)------------(1616, -853)-------------133

(1610, -1710)-----------(1565, -1554)------------162

(0, -600)---------------(-22, -537)--------------66

After that, we have placed the robot where the images had a big noise, with a lot of false lines, and the measure has been:


Real--------------------Calculated---------------Error (mm)

(0, -1850)--------------(-124, -1159)------------702

2010.03.01. Obstacles

Until now, we have located the robot when it's alone inside the field of the RoboCup, however, when we play, there are other robots around it (at the most 8 robots playing at the same time). Francisco Martín Rico is developing a component to detect the obstacles (robots, posts, people, etc) in the field using the camera and ultrasounds. Maybe, we will use this component to rule out points detected in the image, but, from the moment, we are going to know how this obstacles damage our algorithm.

In the next video, we are going to show a simulation with static players inside the field. As we can see, some of the robots are not taken into account, because the white points are over the end of the field, but other points are not discarded, and they affect the algorithm as false positives:

2010.02.24. Using FSRs

Apart from the camera sensors, the robot has other sensors, such as ultrasounds or force sensors. The force sensors (FSR) are located in the soles of both foots of the robot, so it can detect when it's touching the ground. With this sensor, we can know if the robot has fallen or if it's kidnapped.

So, we have created another component to control this sensor, that return a boolean to know if the robot is touching the ground. When it's not touching, we guess that the robot has been kidnapped and reset the algorithm.

2010.02.19. Explorers with A* algorithm

As we explained in the last section, one of the reason to use several images instead of just one, was because the explorers didn't work correctly. That is, when the robot is well located, the algorithm works very well and follows the movement of the robot the most of the time, however, it's difficult to locate well the robot in some places of the field because the explorers are ruled out with bad observations.

As we had seen, we can't use more images to calculate the health of these explorers, because the time increases linearly with the number of images analyzed. So, we have decided to create these explorers another way, with local searches and A* algorithm.

We create new races with the explorers in four iterations, in the first iteration, we create new explorers in 7 different positions of the field with different angles, as we can see in the next image:

In the limits of field, we just search in the angles that look into the field, because in the other angles, the robot can't see anything that helps to its self-localization.

All these particles are inserted in a list ordered with an A* algorithm, to prepare the next iteration.

In the next iteration, we select the first 6 particles in the list and evolute them creating 35 new particles for every particle:

After 4 iteration, we obtain the first 6 explorers and try to create races with them the way we did until now.

We have tested this new algorithm, and we have checked that the behavior was not good enough when the robot only can see lines, so that, we have decided to create new explorers only when we have found a goal,

In the next videos we show the behavior of the algorithm with this new implementation. Besides, we show the explorers selected to be a candidate with white arrows:

In the first video we can watch how the robot is well localized, and how the algorithm is able to follow the real movement of the robot:

In the second one, we test the capacity of the algorithm to recover the robot position when it's kidnapped:

2010.02.10. Multiple images

We have realized that our algorithm depends too much of an instantaneous observation. For instance, when we are trying to select the best explorers in a concrete iteration, a real good explorer can be ruled out if the observation is not very good or even if the kinematics of the robot is not exact.

One way to avoid this problem, is to take into account several images in every iteration to increase the information of the world with different positions of the robot neck.

We have implemented this new idea, and the behavior of the algorithm increases, because a wrong observation doesn't influence the health of the particles. However, we have decided to put this new implementation aside temporally, because the time of every iteration increases a lot, since we need to calculate the health of X images instead of just one image.

2010.02.05. Movement of the robot

Sometimes, if the image hadn't any characteristic points and the robot was walking, a good localization was lost, because our race stayed in the last position but the robot continued walking. So that, we have found the need of updating the races with the movement of the robot. We have got this, using the "body" component of the player and checking the status of the variables v (walking straight) and w (turning), is spite of knowing that this variables are not very reliable.

Besides, we simulate the movement of the head using the "head" component, to simulate the behavior of the robot in a real situation.

In the next video, we can watch how this new features work. In this video, we also use the "go2pos" component, developed by Francisco Martin:

The error-reliability charts of this video are:

2010.02.02. Life of races

When we create a new race, we guess that this new race is in a good position, however, the race usually dies in the next iteration, because in the new observation there are positions better than the position of the race. It usually means that the new race was created in a wrong place, but sometimes the position of this new races was the correct one, but the observation wasn't good enough. Besides, replacing all the time the races, grow the compute time of the algorithm.

Because of these reasons, we have created a life ...

Another feature that we have just implemented is to save the races created 3 iterations and the winners 9 iterations. Doing this, we avoid deleting races is bad observations and besides, we save computation time.

2010.02.01. Main explorers

When we created the explorers in every iteration, this creation was done randomly with all the explorers. After testing the behaviour of the robot, we have noticed that sometimes the robot was in a certain position, but the localization was wrong because any of the explorers were created near that position.

To avoid it, we have decided to spread some of the explorers in concrete positions and others randomly. In the next image we show how we spread the explorers in known positions, this explorers look at both goals with a certain noise, to ensure that when the robot is looking at one of the goals, at least one explorer is in that position.

2010.01.26. Calibration component

Another functionality that we need to use the real robots, is a tool to calibrate the intrinsic and extrinsic parameters of the cameras. In JdeRobot he had a Schema to to this, but it's not very unpleasant to change from JManager to JdeRobot only to use this tool.

To fix that, we have created a new component in the player an JManager that uses the world model created and uses it to show how we should see the world with a certain camera configuration. In JManager we can change the intrinsic and extrinsic parameters of the camera to feedback the images.

This is a screenshot of this new component:

2010.01.22. World model

We have created another component to handle the world model in a easy way. To do this, we save in a external file the points, lines and circles that set the field of the RoboCup. This file is loaded when the player starts running and this information is shared with the rest of components.

The first usefulness of this component has been to show the field in the JManager. Until this moment, the field was created in opengl in JManager with local information, but with this new feature, we can ask for the lines to the player and show the information that we receive from it.

In the next image we show the current status of the world model and the opengl in JManager:

2010.01.18. Kinematics

To abstract the robot kinematics and the camera configuration we have created a new component of BICA called "Kinematics", which updates automatically the configuration of the camera with the current intrinsic and extrinsic parameters of the robot camera and provides many functions to obtain points in 2D and 3D using the progeo library.

With this new component, our code is independent of the robot and can be used with any other robot without changing the source code.

2010.01.14. Reliability

When the robot is already well located, it has to be more difficult to jump to another race, even when some observations are not perfect, but we have to allow jumping because the robot can be kidnapped. Because of that, the ease of change to another race has to be different depending on the current status of the robot.

To handle this behavior, we have created two new variables, which measure the reliability of the current observation and the reliability of the current robot position.

The first one, measures the quality of the current observation, where the quality will be greater when the robot can see the goal, and lower when the robot can only see lines or a few characteristic points.

The second one grows or decrease depending on the quality of the observation and the probability of the winner race. Besides, when we change the winner race, the reliability is reset to 0.

With this reliability, we handle the ease of change from one race to another, so that, when the reliability is near 1.0, it's very difficult to change to another race, because we guess that our location is the correct one.

With this new variables, we have measure the error and the reliability of the calculated position of the robot, as we can see in the next image:

We can realize that when the error decreases, the reliability grows, and vice versa.

2009.12.23. Goal Blobs

With the field horizon, we have avoided false positives with the lines, but the goal points are still sensitive to false positives. To change this situation, we have decided to use the blobs of the goals calculated with the Perception component.

To detect the goal, this component segments the image using the blue and yellow color filter, and selects two blobs at the most (one for each post). Besides, it checks some values to valid the blob, such as the size of the blobs, the distance to the horizon, etc.

To valid a goal point, we check that our point is inside the limits of one of the blobs calculated, or discard it otherwise.

In the next image, we show an example of the blobs calculated from the source image:

2009.12.16. Field horizon

We have realized that the horizon calculated with the odometry of the robot (described before) is not enough when we are close to the field limits, because the field ends before our calculated horizon and some white false positives are detected. This happens for instance with the net of the goals, where a lot of white points can be detected.

So, we have decided to calculate the field horizon using the green colour filter and the Perception component. With this component, we calculate a horizon in the image covering the image every 10 columns and calculating lines between this columns that defines the horizon:

With this information, we add a new filter in our algorithm, deleting all the white points under the odometry-horizon line and over the field-horizon line. You can see an example in the next image:

2009.12.11. Debug the real robot

To debug the code of the robot, the team uses a tool called "Jmanager", that is programmed in Java and communicates with the robot using tcp and udp sockets. In this context, we have created a new flap in this tool to debug the location of the robot.

To do this, we have created several functions in the robot code to obtain the status of the robot in a concrete moment, and we have programmed a new user interface in Java. In the next image we show an example of our interface. On the left, we show the field with the valid races, and a circle around the winner race. And on the right, we can see two pictures, the first one with the source image, and the second one with a filtered image with the selected points.

2009.12.03. Testing the real robot

To use the robot in a real environment, we have reproduced in our laboratory a half-field of the RoboCup with real measures:

Since we need to check the behaviour of our algorithm in the real robot, we have created a new "Component" inside the code used for our team in the RoboCup. In this component we have adapted our current code to C++ to use it in the real robot.

We have realized that the time of an iteration of our evolutionary algorithm depends on the number of points detected in the real image, growing exponentially with the number of points. To avoid it, we have set a maximal number of points detected, 12 for lines and 8 for goals. In case we detect more points than the maximal, we rule out the rest of the points randomly.

In the next images we show in yellow the ruled out points using the real robot:

After this, we hace calculated the time of our algorithm. Using 6 races the algorithm takes about 80 ms every iteration, and using 4 races, it takes about 60 ms. Since the efficiency is one of the most important measures, we have decided to use 4 races, because the behaviour of the algorithm is similar with 4 and 6 races.

2009.11.30. Sample videos

We have recorded 3 videos with the new features implemented to check the correct behaviour of the evolutionary algorithm.

In the first video, you can watch the location in an easy scenario, where the robot look at the goal to calculate its position:

The second one, shows an example with symmetries, where the robot has several possible locations. First, it selects a wrong race, but after some time, it correct its position and select the correct one.

The last video, shows how the robot correct its location when we hijack it:

2009.11.24. Improving the health function

Until now, to calculate the health of a particle, we calculated the theoretical image and compared each point in the image with each line in the theoretical image, as we show in the next image:

After testing this solution in the real nao, we checked that it took the most of the time of the algorithm. So we have tried to improve the efficiency of this part of the algorithm.

To do it, we decided to precalculate the distances obtained in a file, which is loaded when the algorithm starts. In this file, we have two columns, the first one is a point in the field and the second one is the point nearest to the first one where a line is located. So, for all the position of the field (every 25 mm), we know where would be the closest line.

With our old implementation, when we looked for the closest line for a point of a particle, we created the theoretical image in 2D and we compared the distances between each line and our point. With this new approach, we need to calculate the position of the point in 3D, using the Progeo library and the odometry of the robot, after that, we search this point in the precalculated vector and recover the point in 3D closest to our point.

We could compare the distance in 3D of both points and calculate the health with this distance, but it depends too much of the distance in 3D between the robot and the point, so we have decided to project the closest point obtained and compare the distances in 2D, like we did in the old implementation.

In the next image we can see an example of this new algorithm:

Doing this for the lines and the goals we obtain the distances in a faster way and the behavior of the algorithm is almost the same.

2009.11.20. Points detected and memory

To avoid false positives, we calculate two imaginary lines to determine where the robot is able to detect points. The first line is the maximal horizon from the point of view of the robot, this horizon is always 6 meters in front of the robot, and all the white points detected over this lines are ruled out (shown as yellow):

The second line calculates where the robot's body can be seen, it avoids to detect white or blue points because of the color of the own robot, so all the points (both white and blue) under this line are ruled out too (shown as yellow):

Another improvement of our algorithm, is to remember where was our last positions, and determine the new position taking it into account. We have created this feature because of two reasons:

- If we are looking at some place (with or without goals) and we have two races with almost the same probability, it's possible that our location selects each iteration one of the races and our locations changes all the time from one race to another. If we remember which race has won more times, we can keep our position more stable.

- The second reason takes into account the symmetries, because if we look at some symmetric place, we can select the wrong race and our locations will be wrong. If we don't know where the robot was before that situation, it's impossible to calculate the correct location, however, if the robot was before in a not symmetric place, we can remember the last race selected and select the same race when we have symmetries and races with similar health.

2009.11.13. Evolutionary algorithm

If we can't detect the goal and the only characteristic points are lines, we will find different and symmetrical locations where our robot can be placed. So, if we use the Monte Carlo method we have been explaining, maybe the algorithm will select the wrong symmetric place and our robot will be lost.

In the next image, our algorithm selects one of the four corners of the field, maybe it's correct, but likely, it isn't:

To avoid theses cases, we need an algorithm which can manage symmetric points and saves different locations. To do this, we have developed a evolutionary algorithm based in other projects in the URJC robotics group.

With this new algorithm we have N "races" in different locations, and every race has M particles around him called "exploiters". To determinate where a new race deserves to be created we create randomly a great number of particles, called "explorers", and check if any of the new explorers has a better health (similar to the distance function in Monte Carlo) than any of our races.

The races can be deleted if their health is too low or fusioned if a race is very close to another.

With this new algorithm, in the case of symmetric places, we obtain N places where the robot can be located, what lets the robot recover his real position faster than in Monte Carlo.

When we use the evolutionary algorithm with an image where we detect one of the goals, the behaviour is very good too, because although it saves several races that have a high health, it usually selects the correct race very fast. This implementation also includes the movement model used in the Monte Carlo algorithm, what let the races change his position correctly if we move the robot.

2009.11.12. Monte Carlo with movement

Until now, we only took into account the movement of the angles tilt and roll, and the height Z of the robot, however, the odometry of the robot also provides the angle pan, and the position X-Z from the camera to the center of the robot. These three parameters can't be used to locate the robot, because the values are relative respect the robot, however, we can use the difference of theses angles respect our last measure and update the particles with these differences.

So, in our movement model, we have added this new values and refresh the position of the particles taking into account this parameters. This is very useful when we move the pan angle of the robot, because the particles update their position faster.

In the next video, we resample sometimes the particles to check how fast our algorithm is and move the camera to check our new feature:

2009.11.10. Local Search

With a local search, we should reach the position of the robot if the distance function wouldn't have local maximums and only would have a global maximum. We have tested this hypothesis throwing randomly a few particles and checking if all of them went to the global maximum with a local search.

However, we have checked that with a small local search, the most of the particles don't move a lot, what means, that our distance function has local maximums. To ensure this, we have plotted our distance function from every position of the field with the program "Gnuplot":

As we can see, there are a lot of local maximums, and that explains the behaviour of our algorithm. To avoid it, we have done our local search bigger and we skip some of this local maximum, but our search has a high computational cost and it doesn't work with all the particles.

In the next video, we can watch how this works. We use 3 particles and show the distance function with the debugger to check if it works correctly and how the local maximums affect the particles.

2009.11.04. Grid in images

We still detect too many points in the image, to improve the efficiency of this detection, we have changed the "jump" between every vertical and horizontal line depending on the position in the image. An object will be closed to us when we see it near the bottom of the image, and farther when it's near the top.

With this new grid, we detect 35 points, while with the old algorithm we detected 108:

2009.11.02. Elitism

We have detected that whenever a particle reach a great position, this position is lost in the next resampling, because the most of the particles are not in that position, and although the particle is one of the best, his portion in the roulette is very small.

To avoid losing this high probability particles, we have implement elitism in our algorithm, it means that the best particles always continue in the next population without using the roulette. With this new feature, we done some test:

His this new implementation, our robot is located correctly in his position, because the elitist particles are permanent. However, the most of the particles continue in the wrong position, and the elitist are not enough to move all the particles to the correct place.

2009.10.29. Improving detection and debug

We have improved our algorithm in many ways:

The first point to improve was to detect less points in the real image, because the most of the points detected were redundant. So, to avoid redundant information, when we cover the image in horizontal and vertical, if two points are very close, we calculate the average and we take them as the same point.

We show and instance of it in the next two image, with our old algorithm in this image we detected 147 points, and now we only detect 89 without losing information and improving the efficiency:

To debug better our algorithm and detect where the observation model has the highest probability, we have created a new tool that show what would be the highest probability in every position of the field with every angle (pan) of the robot. We can see and example in the next image:

We can see how the highest probability is in the correct place but there are a lot of places with too much probability, so, our observation model is not good enough.

2009.10.20. Resampling

The next step in the Monte Carlo method is to resample the particles taking into account the probability of every particle. We accumulate every particle inside a roulette depending of its probability and obtain the next population selecting the particles from the roulette in a random way.

The particles with more probability will have a greater portion of the roulette, so they will have more probability of being selected in the population. After some iteration, all the particles will be in a similar position, where the probability will be very high.

Using the second observation model and 500 particles, we have test our Monte Carlo method. Firstly, it seemed to work well, as we can see in the next example:

But after some test, it failed sometimes, as in the next image:

To know why the probability of our observation model was high in that point, we have put on top of the real image our theoretical image in that position:

The points detected in the real image are very similar to the lines in the theoretical image, so, even when some points don't match, the average distance is very good and the probability is hight. After this new observation, we have decided to change our observation model to take into account these cases.

2009.10.14. Second observation Model

The second observation model has another point of view, in this case we cover the real image every 10 rows and every 10 columns, from top to bottom and from left to right, as we can see in the next image:

Doing this, we select the points where the color change from an unknown color to a known color (white for lines, and blue and yellow for goals) and vice versa to select the borders of the lines and goals. In the following image, we can see what points were selected in the last image:

We cover all this points selected and calculate a distance from every point to the nearest line (with the same color) in the theoretical image. This way, we can calculate a probability for every particle.

Before implement the Monte Carlo method with this observation model, we have test this with a developed tool that allow select between two grades of freedom (x,y) and one grade of freedom (pitch angle).

If we set the pitch angle of the robot, we calculate what would be the probability in every point of the field. We can see an example of this in the next image, where we set the angle of the robot in 337 grades, we obtain the greatest probability (red) in the real point where the robot is located:

If we select the (x,y) position of the robot, the grade of freedom will be the pitch angle, so if we set the position (3025, 4050), we can see a graphic with the probability in every grade, and obtain the greatest probability where the grade is 330-340:

2009.10.07. First Observation Model

To compare the images, the first algorithm developed covers every line shown in the theoretical image and select 5 points from each line. Now, for every point (x,y), we take this pixel from the real image, and check if the color is the same than in the theoretical image.

Doing it for every point of every line, we can calculate the probability taking into account the positives and negatives pixels. Using this observation model, we have obtained these examples:

As we can see, the model is not too good, because a lot of positions has similar probabilities when they shouldn't. That's because of the comparison is very efficient but very poor.

2009.10.01. Theoretical Images

The observation model is the most important feature of the Monte Carlo method. To determinate the probability of every particle, we need to compare the real image with a theoretical image in some way.

The first step has been to develop a simulator to show how the theoretical images are created and compare it with the real images. We can see two examples next:

To determinate the precision of the simulation, we have put on top of the real image our simulated image:

2009.09.28. Monte Carlo Method

To locate our robot in the Robocup Standard Platform League, we are developing an algorithm which utilizes the Monte Carlo method. In this method we need an observation model and a movement model to calculate the position of the robot with probability and particles.

The location of the robot has 6 grades of freedom, the x,y,z position and the orientation (yaw, pitch and roll) of the camera. With the robot's odometry, we already know three of this grades of freedom, the height of the robot (z), the camera's yaw and camera's roll, so we need to calculate only three grades of freedom (x,y,pitch) to know the pose of our robot.