Adding plane size to the simulation
The above algorithm used a constant plane size and I only put in full planes into it. The next task is to put in partial planes and have the algorithm figure out the size. The big idea used is that in a plane there are corner points. The corner points that are consistent over time (not shrinking or growing) will have a consistent distance to another planes corner points. This is similar to looking at the average global movement and then seeing if a point moves with the movement or not and if so declare it to be static. Since I'm trying to avoid using the global location instead I will do a plane to plane comparison. It also integrates well into the architecture, of having planes be there largest possible size always so their midpoint is always valid. So since height should be consistent we only have 2 points per planes (not four). So if we do 4 tests, each of the 2 points per plane to each other only ones over time that are consistent should be valid. To do this I just store the average and use the standard deviation.
This is how I got it to work
Phase 1.
Create the concept of keypoints. Each plane has 2 points which are derived by the plane being unrolled to 0 degrees and having the left one be 1 and the right one by 2. This is necessary to label them as they have to be consistent when retrieved.
The keypoints are used to derive midpoints using the planes maximum size. So each plane gets 2 midpoints so compared the 2 by 2 gets 4 possible plane displacements. The one with the lowest standard deviation is the most consistent one and should be used as the position offset. The rotation does not need this treatment. Information such as the plane size and keypoint used is stored in the xpointref class.
The map building did not require any changes, but the getposet needed to check if the plane is not the same size of the map. Then it needs to get the keypoint to figure out how to expand the plane to get it to match to the map.
Phase 2.
Although the above would seem to work it has many cases where it doesn't. The first is the way the planes are compared in terms of their first appearance. This does not guarantee that a shrinking plane matches to a full plane. And what happens when two shrinking planes are compared? Occasionally the two planes are shrinking at the same rate and the wrong keypoints could be returned. One more problem is if in the same xpoint a plane growns and shrinks. I didn't think of this before but it happens when a plane first grows and then shrinks due to rotation. Normally the plane would be under another xpoint when it shrinks so it would work out.
The solution would be to take more care with the keypoints. After taking a long look at how to optimally compare planes I decided not to try since it might not be solvable without comparing every plane against each other. I realized that the map builder still works but the getposet doesn't. The solution was to add which interval the keypoint was created and if it is not the current iteration then try and generate the keypoints in the getposet. This is prefered because computexpoint will use the interval with two full planes rather then one with one shrinking one with greater accuraccy.
So getposet splits up all viewed planes into two categories. Full ones and smaller ones with valid keypoints and ones without valid keypoints. After the sort the ones without valid keypoints and compared to the ones with to try and find keypoints. If it does great, if not then just don't use the plane (this shouldn't happen).
There was a problem with two parallel planes shrinking at the same rate. The solution was that looking at the midpoint pairs and that case would have a very specific computation in terms of which midpoint pairs are valid and it is possible to test for this and make the keypoint invalid.
The last problem was with the growing and shrinking plane. The solution was to split the interval into growing and shrinking phases as this will guarantee the keypoints are static during the split interval. Not doing this would lead to inaccuracies.
Here is the picture and video of the end result. In this version you could see the planes growing and shrinking being reflected in the map. Two video versions one with noise and one without, just to show the algorithm does not have any errors as everything is exactly where it should be without noise.
Phase 2 1/2
One thing left to due is figure out how to link the plane that the loop has closed as plane 5 which is seen in the start gets mapped to x0 but x0 does not have any other planes in the same interval so the full size plane 5 never happens. The solution was to have plane 5 map to the current xpoint rather then globally but how to do that?
To do this I needed to add the following...
First I realized what happens when we backtrack? I only would change the current xpoint when a new xpoint is created. I realized that this wouldn't work too well backtracking. Backtracking worked earlier because it would match the planes globally and when creating a new xpoint it would traverse all of them to find the correct one to link to. But this is very inefficient. So instead I monitored at every point in time which xpoint had the higher percentage of rltplanes viewable at an iteration, this would be the current one. Doing this saved a bit of computation, mabey 5% mainly due to global matching being quick.
So besides that now I would have to figure out when a plane should belong to another xpoint even though it has been seen before. Add first I thought I would simply have to look if a globally matched plane belongs in the current xpoint. If so then check to make sure there isn't an overlap in intervals as this would signify that it belongs to a adjacent xpoint. At this point I simply link the plane to the current xpoint. This seemed that it would work but it didn't due to a degenerate case. During a rotation a previously unseen plane came in momentarily and then faded away and was added to a xpoint. The xpoint then failed to compute as adding it to the xpoint broke the rule about having all planes visible at the same time, as when computing two planes which had non matching intervals where compared. It would be solvable by changing the plane ordering in the xpoint but I realized that this could lead to problems.
So the solution was to add a new xpoint when a globally matched point should be in the local space. This worked, but worked too well as it caused a whole bunch of extra xpoints to be created as shown in the below 3 pictures. After thinking about the problem for a while I realized the only good solution was to cap planes to only getting linked when been seen globally if they are linked less then 2 times (this is adjustable). Which partially solved the problem. The map does not look as elegant as before but the xpoint don't need to be optimum for it to work, it just needs full coverage.
So the only other thing I needed to due is add some code to the mapping algorithm to always map the largest plane available.
Here is the video, notice plane 5 fills up. rltfullyconnected.avi
One further note, towards the end of the simulation when the map has been traversed the map starts to skew. This is because of the limited amount of planes stored and in particular this case the planes that were missing (were rolled over) were when plane 12 was fully seen. The only planes left where when it was shrinking, and it was matched against plane 14 (which is real life is probably not possible due to occlusion), and this created the error. I upped the plane storage and the error went away. I still might add some form of xpoint locking at some point.
