Aaranda-pfc-itis

• Project Name:

• Authors: Álvaro Aranda Jiménez (aaranda [at] alumnos [dot] urjc [dot] es)

• Academic Year: 2008-2009

• Degree: Undergree

• Jde Version: jde 4.3.0

• SVN Repository: https://svn.jde.gsyc.es/users/aaranda/pfc-itis

• Tags: webots, opencv

• Technology: c, c++, opencv, python, jde suite

• State: Developing

• Source License: GPLv3

• Document License:

First steps with Webots and OpenCV

Color filtering using Webots's camera

Detecting borders with canny

Detecting borders using sobel

Detecting lines using hough

Python, wxPython and wxGlade

Showing an image and using some buttons

My first window with wxGlade

Detecting corners in a football field using OpenCV (T's and L's)

In this part I pretend to detect all corners of a football field, distinguishing between T and L corners. In a L corner, the lines by which is composed shares an end, this don't pass in T corner, where all ends are different.

Detecting corners without difference between T's and L's

Detecting corners without difference between T's and L's

Execution times:

real 0m0.371s

user 0m0.344s

sys 0m0.024s

Detecting corners without difference between T's and L's over Webots simulator

Detecting corners without difference between T's and L's, obtaining only one mark per corner(without duplicates)

Execution times:

real 0m0.378s

user 0m0.356s

sys 0m0.024s

Detected corners differentiating T's and L's

Detected corners differentiating T's and L's

Execution times:

real 0m0.392s

user 0m0.364s

sys 0m0.028s

Detecting corners differentiating T's and L's over Webots simulator. Final step

Kalman 2D

The Kalman filter is an efficient recursive filter that estimates the state of a linear dynamic system from a series of noisy measurements.

It has two distinct phases: Predict and Update(or Correction). The predict phase uses the state estimate from the previous timestep to produce an estimate of the state at the current timestep. This predicted state estimate is also known as the a priori state estimate because, although it is an estimate of the state at the current timestep, it does not include observation information from the current timestep. In the update phase, the current a priori prediction is combined with current observation information to refine the state estimate. This improved estimate is termed the a posteriori state estimate.

Only center point of color filtering recognition

First time I tried only to show the media of points recognized by a dynamic color filter. On the video we can see the parameters of this dynamic color filter, based on hsv. Moreover, the filtered points are displayed with its real color, and unfiltered points are displayed in grey shade.

First Kalman approximation, only with correction step

This video shows the first implementation of kalman algorithm. It only includes the correction step, and we can see that the kalman observation arrives, in a probabilistic way, to the actual observation. At this time, the uncertainty is not shown, the kalman correction only guarantees the state.

Kalman algorithm with circle of probability, including also correction and prediction step(slow)

In this first approximation using correction and prediction step, I try with its in a non perfect form of use, I mean, the cases for its activation where not the final ones, only trials. For this, correction step only activates when the kalman's correction where near to measures media. On other side, prediction step is activated when the prediction step where far to measures media, also sleeping a bit between 2 of this iterations. It's the first time I used a circle to represent the probability.

Kalman algorithm with circle of probability, including also correction and prediction step(fast)

This time I used the camera for record the example. With the camera I've better results, almost more clear results. Now, the prediction and correction step are well distinguished. The correction step is activated when the object filtered(something red) is detected. When there is no object detected, the prediction step becomes on, only growing the uncertainty of the circle.

Kalman algorithm with ellipse of probability(including anisotropic probability)

In this version, I have a two-step color filtering to only get red observations (this is configurable). I can also modify filtering parameters in the execution, by the gui.

The prediction step, only comes up alone when the actual observation is null, when no object is being filtered. Then, the state stop and the uncertainty raises.

When any red points is being filtered, the correction step comes up(also with prediction step). Now, by using kalman estimating equations, the state correct its position iteration by iteration, and the uncertainty raises up when the state is moving on, an decreases when the state is a few iterations in a near position.

The most important improve this time is that the uncertainty is painted as a ellipse, and not as a circle. Moreover I have added an algorithm to calculate the uncertainty in a anisotropic way, in the future i will integrate this algorithm in the kalman equations.

Ellipse improvement

In this step I wanted to show that the uncertainty is not lineal, now it can range its orientation. Before, the ellipse can only range in the x and y axis, and now it can change its axis, 360º.

The other important improve is the addition of the velocity in the state(constant). This imply that I have to change all the algorithm, the matrices and some operations. This was a heavy actualization, but it was necessary . Now, in the prediction step we can see that the uncertainty grows and moves with constant velocity.

First Kalman algorithm with various detected elements

This time I have tried to recognize various points and filter it. Now i'm testing with 2 detected points, using different color filters, later I will try with N elements and not using this way of filter elements with the color.

Also I add the velocity vector. This is quite useful to know the movement of the filter.

First Kalman algorithm with N detected elements

Now I implement a really N kalman filter. I use now an own algorithm based on opencv to detect intersections. Each intersection detected will be followed as an interesting point, one filter for every intersection. So i won't use the color filter only any longer.

This algorithm to detect intersections is based on canny and hough. I detected that hough filter doesn't work properly with gazebo simulator, I demonstrate this in the first video. Anyway, I have adjust my algorithm to minimize this. In the other videos we can see the result, for a first aproximation of N-Kalman filter is ok. Later I have to perform ellipses nervousness and use other algorithms to detect intersections that don't work with hough.

The operation of the N-Kalman filter is this: when an intersection is detected I assign a Kalman filter to it. If the intersection is not detected for a period of time, I destroy the filter assigned. Otherwise, if the intersections is followed for a period time I begin painting it in the scene. I detect if the intersection is the same in the next moment by looking at the near zone.

Kalman algorithm with N detected elements

Using Shi & Tomasi detection algorithm

Now I have improve the ellipse, now its reliable. Also I have changed my corner detection algorithm, I'm using Shi&Tomasi. With this I obtain properly detection, I don't have the hough problem now. Moreover, I have changed my test battery, now I follow the corners of a polygon, this is more useful and clear.

This is almost a final version for Kalman 2D filter, I can improve it using Mahalanobis distance to detect similar corners by the iterations or use a pool of colors to paint different filters. Also I can use FAST algorithm to detect corners, this is faster and I trust that robust.

Using Fast detection algorithm

I have integrated the Fast algorithm for corner detection. This filter is quite faster than Shi & Tomasi, next I`ll show a comparison. It try to evaluate each point looking each point around it, but it's better to evaluate only certains points around it, this is an improvement. Only the evaluation of 4 points around each evaluated point is enough(0º, 90º,180º and 270º points). If 3 of this points are higher or lower simultaneously of a threshold, the evaluated point is a corner.

In my tests, I have realized that althought this algorithm is faster than others, it's also robust. I can obtain better corner recognition with Fast than with Shi & Tomasi. This videos show that Fast is quite good to find corners.

This video shows a extreme fast iteration of my schema, maximun 10ms. Now, the processor is fully stressed, and my code iteration is near 5 ms detecting 10 corners. This is quite good time and quite good results working at 100Hz, here is the video:

Comparing Shi & Tomasi and Fast detection algorithms

I have made a table comparing this two corner detection algorithms. This way it's easy to see that Fast is quite faster, also I can contribute saying that it's also quite robust, it has better detection range.

Algorithm / Cycle	18 ms	33 ms
Fast (10 corners)	7,82 ms	10,25 ms
Fast (3 corners)	7,26 ms	9,13 ms
Shi & Tomasi (10 corners)	11,45 ms	17,34 ms
Shi & Tomasi (3 corners)	10,76 ms	15,68 ms
Shi & Tomasi (10 corners) with Harris detection	9,64 ms	16,86 ms
Shi & Tomasi (3 corners) with Harris detection	9,31 ms	16,11 ms

New interface

Here I show a sample video with an actualized interface.

RANSAC

RANSAC is an abbreviation for "RANdom SAmple Consensus". It is an iterative method to estimate parameters of a mathematical model from a set of observed data which contains outliers. It is a non-deterministic algorithm in the sense that it produces a reasonable result only with a certain probability, with this probability increasing as more iterations are allowed.

A basic assumption is that the data consists of "inliers", I mean, data whose distribution can be explained by some set of model parameters, and "outliers" which are data that do not fit the model. In addition to this, the data can be subject to noise. The outliers can come, for example, from extreme values of the noise or from erroneous measurements or incorrect hypotheses about the interpretation of data. RANSAC also assumes that, given a (usually small) set of inliers, there exists a procedure which can estimate the parameters of a model that optimally explains or fits this data.

Line model

At the top I'm painting all data points, all in blue except the 2 points that are selected in every iterations as consensus set(better points to paint the line with less error in the model).

At the bottom, I'm painting all data points also, but now green points are points that fits well with the model, and red ones that don't fit so fine. The line is the model, the best in every iteration. If we have few iterations, the model will vary a lot, and not always will be fine. On the other side, with enough iterations, the model won't vary and it will fit properly, and model's error will be often the least.

Few iterations

Now we can see that the model varies because we have few iterations and the consensus set is different after iterations.

Enough iterations

Here we can see that the model doesn't vary. We have a lot of iterations and the program is able to get the best consensus set with less error, and find that way the best model for data.

Calculating KRT

I have implemented an algorithm to get the KRT matrix, giving only a set of 2Dpoints and its 3Dpoints asociated(a set of pixels and its points asociated). The way to obtain it is to solve an equation like Ax=B, where A is the pixel, x is the KRT matrix and B is the 3Dpoint.

In a mathematical way, we have to find the value of 12 unknows, because KRT matrix is 4x3. I can do this solving another equation's system, calculating a file of unknowns each time. I need at least 4 points and pixels to do that.

Ransac is used to get the better KRT matrix as possible. I multiply each pixel with the KRT matrix, and I get a 3Dpoint. I compare that 3Dpoint with the pixel's asociated point, I get the error between them. The KRT matrix with less error will be the better.

This is the result. After an iteration and using test point, I get the KRT matrix.

Manual KRT

I've implemented my schema to obtain the KRT matrix in another way. Now I have a manual path to get it, If we click on the gui's top image, my code get the relative clicked position.

We've a configuration file where we can put our selected points. In the video sample, I've configured it with all pattern points. So if I clicked on a pattern point, the system recognize it's a configured point and it become a candidate point for Ransac algorithm. I need at least 4 points to launch Ransac and get the KRT calibration matrix.

Automatic KRT

Now I have done an implementation to get the KRT calibration matrix automatically. So, I don't need a static object and camera anymore, now the scene could change. I use a color filter with the scene to get patterns. This patterns are colored squares, one into another(different colors).

We've a configuration file here also, there I've to put the 3D points(the reference system is a corner of the box) and the colors of its points. In the dinamic algorithm, we obtain the 2D points automatically and when we push the calibration button, we obtain the KRT matrix as a result of the association of 2D and 3D points(looking at same patterns).

Here are new videos, with more scene movement and changes to prove it's good.

This last one includes new debug text, now it's clearer to understand the way of the implemetation.

When I start the schema's execution, the 3D files are loaded, and they're shown on the screen. Later, I begin Ransac Auto. Then the image is filtered by color, and every iteration I capture the patterns. The time used in this part is almost the same like in a normal image color filter, it's good. When I click the button the Ransac algorithm is released and we can see in the screen the KRT matrix obtained. I also show the number of recognized points that goes to ransac and the best selected ones by the algorithm.

The higher problem of using a color filter to get the patterns is that the recognition depends on the light conditions.

Automatic KRT & Kalman filter

Now I have associated a Kalman filter at every detected mark of the pattern. In the first video we can see only this association, an at the second video I I have shown both the association and the ransac execution. Therefore, this is a automatic gauge that use Kalman filter for all processed marks of the pattern.