This week we successfully built and mapped a maze using hector SLAM! We used cardboard to build a pretty straight forward maze so that we could start to test code on actual maps instead of images. After the maze was built, we got Paul’s Hector SLAM algorithm working and were able to map the maze. It turns out that it wasn’t working before because the bags weren’t saved correctly and contained a lot of useless information. This week we created two maps with the robot starting at either end of the maze so we could start to try and overlay the mazes.
Code Architecture
As for the individual sections, we made some decisions about how we are going to structure and implement our code. The overarching goal for this project is to recreate a basic ICP algorithm using our own code. We chose to focus on understanding the inner workings of this instead of using a premade scan matching algorithm. Last time, we were a little undecided on how much of our own algorithm we were going to create, but go big or go home.
We decided to split our code into a few different sections:
-Hector SLAM
-ICP -corner detection and error distance
-translation and rotation
-minima prevention
From here, the work was divided between the first two parts of the ICP algorithm and will be integrated at a later stage. The last one will be saved for later, once the beginning parts work in conjunction, and the hector SLAM script will be filled in once we want to start running in real time.
The Hector SLAM code that we are using was created by our professor, Paul Ruvolo. Although we are creating our own ICP, some of the individual pieces are being taken from the internet, as we talking about in the previous blog post.
Part 1: Corners and Error
Last post, we discussed the use of the Harris Corner Detection algorithm from openCV to find interesting points and visualize them. This week, we focused on using those data points to find the error distance to a nearby point. While it is not yet up and working, there has been a lot of progress and ideation to get to this point.
First off, we quickly discovered that the matrix is NOT binary and is composed of both positive and negative values. This doesn’t turn out to be too important later as its as simple as higher values being more important, but it surprised us at first! Secondly, we tested it on a completed map .pgm file and it worked! All of the walls were considered key points as shown in the picture below
From here, we go into the manipulation of the data. The corner detection algorithm basically outputs a matrix with values for each pixel that correlates to importance. The first step is to compare the two map matrices and make sure they have the same amount of key points. Then, we decided to go simple with the matching algorithm and just use the nearest key point, regardless of how “interesting” it may be. This will at least make the scans overlay each other, if not rotate.
This is where we encountered some problems. It is a proving a bit hard to conceptualize how best to sort through and find the closest point. The error distance (or distance between points) is x^2 + y^2, or the hypotenuse squared. This is easy enough to compute, but iterating in concentric circles around a point to search for a value is difficult. So far, it has been challenging to iterate through all of these points, but with a bit more testing numpy.where might be promising.
Part 2: Translation / Rotation
This week we have been working on getting the transformation code to work, without the rest of the code, on the maps. After a lot of debugging, we believe the translate code is working.The translate code took two maps as matrices as arguments...
and returned a new picture, supposedly of the first map translated to match the second map.
However, we are a bit confused because translation should just be moving the map, not rotating it. So why the output is the first map rotated to match the second is a mystery to us. We are going to look further into this conundrum to figure out what exactly the translation code is doing, since it is clearly doing something we didn’t expect. We have also started working to get the rotation code working so we can rotate one of the mazes to match the other. Theoretically, using corner matching and error distance with rotations, we should be able to match the maps since they shouldn’t require non-rigid transformations.
Next Steps
Next steps will be to finish getting all of the code written and working in individual sections and combine them. There is only one week left, which we realize and are wary of. We have scheduled a good number of meetings before this is due. Although this project has been slow moving so far, we are both determined to have a working project in the end. We believe we know where we are going and what we are doing, at this point, it is just doing what needs to be done!
Note
Talking to Paul in class and looking back at the source made us gain a better understanding of the transformation code. The code was originally written to transform locations of points. We assumed we should change that to do complete transforms on images. This is not what we want. When we read an image with cv2.imread in python, it reads the brightness values of all the pixels. This is not useful to us. We want to look at specific points in the image and move them. So we need to convert the maps into matrices that hold point values for the walls. With that, we will be able to transform each individual point which, together, should create the transformed image. This will require some research into how to find points above a certain threshold in python, or cv2, and save those points to a new matrix. Then test the points in the transform functions. Rotation seems the most important, so we will start with that. We think the translation function will require the point matching code to know which two points to look at in order to translate the right amount. That being said, there is still a lot to play with.
This also means that the work done on the translation code this week was not correct, this makes sense as to why the returned image was confusing. We also realized the second map was translating to match the first, so it makes since that the return map looks identical to the second map.
To start, we need to get the rotation and translation functions working. Once those function work, we need to get the ICP algorithm to work with the two maps images we have. We can either start that by using our two images, rotating them manually so they are close, then running ICP, or maybe just manually create another map by slightly shifitng or rotating one of the maps. We need to test to see how good the ICP algorithm actually is.
Comments