This first part of the project has flown by and taken us, at least a bit, by surprise! Our original research into the topic took a bit longer than expected, so we didn’t start working on the code until much later than our original plan. So far we have done a few things: tried to map using Paul’s prewritten Hector SLAM code and began writing and pulling code from the internet to get our Iterative closest point algorithm working. We have also rethought our team goals and definition of MVP so that it is more realistic to where we currently are, given the two weeks we have left until the final presentation.
One change we have made to our current plan is to ignore hector scan mapping for now to see if we can get a very low level basic ICP working with predrawn maps on the computer. We decided to do this because we were getting hung up on Hector scan and not doing enough on the actual ICP code we are writing for this project. After we get a basic ICP working, we will then revist Hector SLAM, hopefully this weekend.
Since we decided to stop struggling with Hector SLAM we started to research more about actually creating an ICP algorithm and started writing the code. For this first pass, some of the code has been pulled from from resources online, however as both of us have goals to actually understand the code we are creating, we both worked hard to fully understand the code we are pulling. We worked on two main functions: corner finding and transformations. These will allow us to find the corners in our map,and transform one map to try to match up with the other.
Corner Finding and Matching:
We decided to use Harris Corner Detection in order to establish key points for later comparison. Instead of generating our own algorithm, we used the opencv library version. Basically, this algorithm looks for points that transform the most during rigid transforms. There are several different parameters, but the one we are most concerned with at the moment is the blockSize. This determines the size of the block of pixels to search at a time for interesting points. Since we expect to have a large amount of data and want to keep computation time down, we will most likely increase this size and decrease the resolution of the map. This may sacrifice some precision, but we think it will be worth it as ICP is a very computationally heavy algorithm due to the many transforms and iterations inherent with each scan. Our current model works well when tested with images taken on a mobile phone, but there are some concerns about how it will work on lidar scans. Lidar scans have a lot less data that a normal image which may affect how well the algorithm works for our application. Some other changes to keep in mind are removing the dilate function, which essentially increases the size of the interesting points. If we decrease map resolution and increase blockSize, this might not be very necessary.
Figure 1: Red dots represent points of interest. They stay pretty consistent between images so will be good for comparing later
Our model outputs a matrix with each value corresponding to the pixel of the same row. Any value that is 1 instead of 0 indicates an interesting pixel. Then it is a matter of determining the row and column of each key point and finding the nearest neighbors along a diagonal. Next steps include writing this function with consultations to a library to avoid being bogged down in the functions instead of focusing on functionality.
Transformations:
I took the base code for these transformations and edited that to make it match up better with the corner and error code we wrote. This code had 4 main function, a translation function, random rotation function, random scale function, and a function to implement the transformation at hand. This code uses matrices and matrix math to perform these transformation by creating translation, rotation, and scaling matrices to then apply to the matrix with the points from the second image. The original code returned an error value, found from an error function, to determine how accurate the transformation was to move it closer to matching up. We changed it so that it just returned the new matrix, to be compared later in a different function. We also changed the random rotation and scale function to allow the user to give an angle or scale factor instead of randomly choosing, so we can decide if we want to do something like rotate the image 90 degrees.
So far these translations have not been tested on actual images, that is the next step, changing images we want to match up into matrices and testing these different transformations.
In Figure 2 you can see the translation function which finds the center of mass of each of the images, finds the difference of the matrices holding the com, and finds a translation matrix which are then used to translate the image.
Figure 2: Translation code used to shift the matrix holding the image points
We are currently still working to have these three functions work together so we can have a simple ICP working. THe last function we plan to have is one to double check that the function is not stuck in a local minima. It is possible our ICP will settle on what it thinks is the best match, but it could get better. So we will do a check by rotating one map a bit to check that how it is lined up is the most accurate lining. Finishing combining at testing this basic code on perdrawn maps will be done this weekend, as will collecting Hecto SLAM scanned maps so that we can be one a good path to move forward to better the algorithm and get it working with live robots.
Reference Code / Documentation:
Comments