Relative Mapping
One of the largest problems of any of the above algorithm is its reliance in storing absolute coordinates and updating them every time step. If the coordinates
are off after a single time step it might be unrecoverable. Also this introduces the potential for small accumulation errors creating large problems in time.
I Created an algorithm which just tracks relative coordinates of planes and only uses absolute coordinates to match newly seen planes against a map to see if they have been seen before. It seems to work. Here are some pictures
This shows the first try at relative mapping using 4 planes that are visible all the time. Notice it doesn't have any bad long term accumulation errors as the map (blue planes) pretty much stay where they are meaning the global position is only off by the error of the reading which is being compared to the map.
Even though this algorithm is straight forward, implementing it was difficult. The 4 plane simulation only has a single connection point and can't deal with new points. In order to debug the plane adding algorithm I made a test bench that was separate from the robot simulation that adds planes based on time and not robot movement which made debugging a lot easier.
Just before trying the algorithm on a full map I played around with a smaller one. As luck would have it, the simulation create a rare error as the robot pivots exactly in line with some of the planes. Since the planes had some noise to them the planes as seen by the robot had constantly changing normals. There are two choices to be made. Either all planes can be matched irregardless of their normal orientation (back, or forward) or for each orientation a separate plane is created. The separate plane is a better long term choice although slightly more difficult as if a robot would be in another room it would not link to a plane from an adjacent room (If the wall is very thin). When there was not any choice the algorithm worked, or when the robot would not pivot coplanar to the planes. The problem was the global maps plane had the wrong normal, and after this was fixed it worked fine. Took a long time to solve though. Notice that the robot has no problem backtracking, and there are two planes for one of the walls per each orientation. This was before I changed the code to only view planes with normals facing the robot just to make sure in case it would happen the code would work.
After all the bugs were worked out, and planes switched to viewing only when facing the correct way I did a full map. It is able to "close the loop" with mild noise (see below discussion on problems with noise). The dirty version has a bunch of debugging information, the clean version has less.
Problems with noise
There was an interesting issue with noise. Noise is suppose to have a static mean, however sometimes with a limited amount of readings it does not which creates problems.
The picture on the left compare planes that are all on the same iteration. The picture on the right compares planes using each individual overlapping iteration which are different as there are more readings that way. There is less noise if they are on all the same iteration because when they are on the different iteration they are exposed to different noise. Its an interesting question on how to choose the plane comparison pairs but comparing on the minimum interval removes this problem. When they are compared on each pairs interval the pairs are chosen as to when they are first seen so the interval range of the first plane is different from the one that is last viewed even though they have a smaller interval where they are both viewable.
There is a second way to correct this error. To do this when all the planes are traversed the end coordinate should be the start. If there is error is it possible to backtrack this error which is not really correct as it is hard to know which plane created the error but it somewhat cleans it up. It makes individual pair interval algorithm more accurate, and seems to help the minimum interval one too.
So where does the noise come in after the corrections? I started to look at the noise model, and I turned off rotation error but not position error Doing this means there is pretty much no errors.
This picture has no position noise but rotation noise. There is not much error but there is some.
So I took a look at what was going on. Even with 50-100 readings there was actually a significant amount of rotation given with planes that are coplanar. With the position noise even a small rotation error makes the position error worse. I changed noise models from instead of using the c rand() function each plane rotated there rotation noise to the previous median noise value but with only 3 discrete values, -noise, 0, noise. Doing this nearly eliminates the noise. In the above videos the robot is able to close the loop since the map is mostly accurate.
The first picture shows the same time step as all the other. The next three show a sequence in time.
When the robot is about to traverse the same area (or backtrack) it uses the position estimate to translate any newly seen plane. This plane is then matched to the global map and if there is a match the plane is linked to it. Loop closing could only occur of the robot position is accurate enough so when the plane is translated so it matches the the one on the map.
Closing the loop is not entirely important since there could always be a big enough map so that very small errors will cause the loop closing not to work as the plane matching bounds are fairly small. It is very possible to have a separate algorithm to detect when the loop is closed and make adjustments.
Demonstrating how sensitive the algorithm is to the interval used.
Here is an interesting bug but it show the potential problems with the algorithm and the fact that I'm not locking in any values right now. What happened is there was a bug when deciding what interval to use, the forward one or the reverse. The algorithm wants the interval that has the largest plane size, but also looks at maximum viewable iterations if the plane size is somewhat close. The bug happened when it used the wrong variable and fed in the wrong interval.
So why did the interval lead to the large bug, it shouldn’t but the noise was the culprit. The plane was small enough initially such that the noise fed in the wrong keypoints into the algorithm. What happened was there was two potential locations based on plane point pairs, normally the wrong one would have the much higher standard deviation and gets ignored, however due to the fact that the plane is small and there is only a few readings this didn’t happen. To make thinks worse the noise was larger in the correct reading from random chance I suppose so it picked the wrong one. After enough readings came in the wrong version would eventually have a larger std deviation and not be used. The real fix was in the interval selection and after that was fixed it work. This should not really occur unless there is a really weird issue with the interval is too small although I don’t exactly know how this would happen in real life since data is only stored when seen in a fairly large array 500 readings worth, but if only a few readings a large error could happen. (although we could ignore smaller planes since they might be a large source of error in general).
Note that it did auto correct using the wrong interval, as the error doesn’t accumulate in my algorithm.
one other things to note
One is what happens when there is not enough plane storage? I had an issue when the first viewed plane which is used to place the map at (0,0,0) ran out of storage and was not available. This pretty much caused it to stop working so I stored the first plane offset and uses it when the plane is not visible. I played around with size but when it is too small I think that the plane stops storing all of the relevant intervals and this causes the algorithm not to work. There is also an error if you leave the robot looking at a plane which only is part of a group for too long, eventually there is not any more links between the plane and ones that are not visible and it does not work.
I suppose the solution is to lock xpoint and xpointref at a certain point but then the algorithm would be continuous. I am leaving it as is, as I could make the plane size quite high without worrying about memory but I suppose I could add the locking at some point in time. (currently fixed)
Next up -> right now all the planes are either fully visible or not visible at all. This is not realistic so I am looking at doing some motion modeling to deal with this issue.