MAZE SCANNER DOCUMENTATION

MAIN IDEA:

The main idea of the project was to successfully implement Iterative Closest Point (ICP) in order to line up maps taken by a robot. The very end goal, which was not reached, was to have two robots able to go through a maze, real time, and determine where the other robot was to go find it. We ended up with all the components of the ICP algorithm, but not fully working together.

The main steps were to find the important points in each image and compute the error distance between the points, then rotate and translate one map iteratively until the error distance was at a minimum.

PROCESS:

The first step was to make the maze, which we did out of cardboard, a fairly straightforward maze. We then drove a robot through the maze from one side to create a map, then created a different map by driving the robot back through the other direction. We created the maps using Hector SLAM, which is a type of mapping that used Lidar. Our maps were pretty clear and similar to one another, except rotated almost 180 degrees.

Our actual code to perform ICP was written in piece. We wrote functions to find the points and plot them and tested that on regular pictures before taking the maps. After getting this working with maps, we wrote code to cut down the number of important points so both maps held the same number of points. We also wrote code to find the error between the points determined to be the closest to one another in the different images. We also wrote code to rotate and translate the images. All of these segments were written and tested individually before being combined to work together for a full ICP implementation. In the next section, we will talk more in depth about each of these functionalities.

CODE ARCHITECTURE:

3 Classes:

• PointMatching:

  • findPoints visualizes the image from the given file, and uses the cv2 built in cornerHarris detection function. Harris point detection essentially looks for points that change the most during rigid transforms.  

  • arrangePoints looks at these points found by corner detection to find the key points. The points from the corner detection are held in a matrix and represented as numbers that correspond to the importance of that point. arrangePoints sets the points under a certain threshold equal to zero. This function then saves all the point locations (x,y) to an array.

  • limitPoints randomly removes points from the matrix to make sure the arays holding the important point locations are the same length. It shuffles the array, then removes the first however many points where the number of points to remove is equal to the difference of the length of the two lists.

  • matchPoints looks at the nonzero values in each point array and looks at the error distance between each point pair to find the closest point. It then creates an array that holds point pairs of those deemed closest. This array is what is sent to the Transformations class to be transformed to attempt to line up the images.

• Transformations

  • translate is used to move the x and y coordinates by the a and y average distance values to move translate one image closer to the other.

  • rot if used to rotate a point. It creates the rotation matrix, then multiplies the point by the transpose of that matrix in order to rotate the point to its new position.

  • apply_trans_to_points identifies the moving points in the array of point pairs, then based on the transform variable, will either rotate or translate the points from the moving point set. It then creates a new list to hold these transformed values.

  • avg_dist find the distance between all x and all y point pairs, and saves those distances to two different arrays. It then finds the average of these distances to find the average x change and average y change of the points. The output is used to translate the moving points.

  • icp is what runs the actual ICP. if the updated error found is less than some threshold, the array of points that corresponds will be returned. However, if the error is above the threshold, the moving point set will be translated, the rotated to try to minimize the distance. SVD was not used for lack of time. Instead, angles to rotate were chosen randomly between 0 and 90. After ever translation and rotation, the new moving points locations and old fixed array are sent back to the PointMatching class to refind and match points, hoping to minimize error distance.

• Visualize

  • ​build_img simply creates a matrix with the important points with the value 0 and the rest valued 255 so that the transformed matrix can be visualized as black points on a white background

  • show_img uses cv2 to actually open the image of the given file or matrix

ALGORITHM:

Iterative Closest Point (ICP) uses translation and rotation to minimize the error distance between matched points. This can be used with sets of important points in transformed images in order to line the images up, as we did with the maps. This can be shown on slide 3 of https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf.

Where P and X are the sets of points to align, Np is the number of points in P, R is the rotation and t is the translation. We translated first to generally align the two maps. We then iteratively rotated random amounts and translated to realign the rotated map. After each iteration, we checked to see if the minimum error was smaller or larger. We continued to iteratively test and correct until the minimum error reached below a certain threshold, which we assumed would align the maps the best.

To translate, typically you find the center of masses of each set of points, then subtract the com from each point in both sets. To calculate this, we found the average distance between all the matched x values and the average distance between all the matched y values. We then added those average distances to every x and y value in the moving set.

Often ICP algorithms will use singular value decomposition (SVD)  to determine the optimal rigid transformation to match points up. This is because SVD will break a matrix down into rotation matrices. Using SVD on a matrix gieves you [U, S, V] which can be used to find the optimal rotation with the equation VU^T where T is the transpose. SVD can be done if two point sets are recentered so their coms are on the origin, or at least on a shared point on top of one another. Theoretically, the transformation that needs to be made can be found by -R(com of moving matrix)+(com of fixed matrix). This would rotate the matrix then translate it to the fixed matrix.

As described before, we achieve this by translating the matrix, then rotating it. The plan was to then recalculate the important points and their error, and continue translating a rotating new important points found until the average error was under a certain value.

GOALS AND SETBACKS

We started this project with very clear goals, a set schedule, and a huge drive to have a working project in the end. Unfortunately, as is typical, we fell behind on our schedule quickly and unfortunately in the end, though our drive is still here, did not complete all of our goals. As a team we wanted a working MVP, to do a project applicable to the real world robotics, and to understand all of the code. I think that as a team, we successfully achieved two of our three goals. Our project is very applicable to the field of robotics. Many people are working on robot localization and swarm robotics. One part of this is knowing where the other robots are. One way this could be done is by creating maps and comparing maps from different robots to determine where the others are. This is exactly what we were working on. We feel we have gained an understanding of ICP and mapping that could help us talk to others in the robotics field. We also understand all of the code. Throughout the project when writing blogs, we discussed what each other was writing. We also asked a lot of advice and questions of one another in our final sprint to try and finish. We feel as though we do understand one another's code and could write it again, if prompted.

Unfortunately, our set, clear schedule fell to the wayside and things did not get done when we wanted them to, meaning that in the end, we were unable to complete our goal of having a working algorithm. We have a lot of working parts of the algorithm, but when converted to object oriented, they no longer worked the way they used to. So we must have done something to mess with some of the functionality.

One major setback we encountered was that of time. We had a clear schedule with some buffer space, so we thought we would be okay. However, as it does, life and other classes got in the way and we didn’t have as much time as we thought we would. In the last week we did a lot of cramming to try to finish, unfortunately, we were unable to complete our goal.

NEXT STEPS

The first step would be to fix the bugs the appeared after moving to object oriented code. It seems that somehow when plotting the important points, they are now far in the corner instead of the middle of the screen as before, as it should be. After this is fixed, the actual ICP iterations of transforming then refinding points and recalculating the error to do more transforms until the average error reaches below some threshold. Once that was in place, we would want to move past our MVP to see if it could work on partial maps or if the maps being rearranged could also hold a point to represent where the robot is so we could see where the two robots in the map are.

RESOURCES

http://nghiaho.com/?page_id=671

https://igl.ethz.ch/projects/ARAP/svd_rot.pdf

https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf