More work on the Motion Model
The motion model took a lot of time to complete.
First some additional observations
Notice that the poset is wrong due to the wrong keypoints showing up in plane 3,2 due to how large the noise is on plane 5 which is used to generate the keypoints. It corrects itself in time, nothing really I could do to fix it.
Notice that when the plane is moving, when it is fully visible at one end there is noise so the plane becomes bigger then it should be. Not a good way to fix this other then use some sort of filter on the movement (or readings since one is derived from the other). Currently I don't have any code that fixes this
The viewing frustum not being that realistic caused an error. When the robot is rotating the frustum is bigger diagonally then straight on so during a part of the turn a portion of the plane is visible and as it keeps on turning less is visible. The algorithm assumes that the end of the plane is what was seen during the turn and everything is wrong after that point.
This is very similar to the problem of being in a hallway and one moment some walls in front shrink. Its not really that solvable, but having a longer viewing distance at an angle is not realistic.
This was an interesting problem, note that in this simulation run there is no noise. The standard deviation used to calculate which keypoints are valid failed due to the formula used to compare the two. What happened was there were two very small numbers, and numerically this didn't work. The algorithm was later changed to fix this.
As the loop is being closed the question is should plane 2,8 which are the same be merged? Initially I thought yes, but it might not be that important as with noise the planes might be quite a bit different. In terms of merging I was thinking of just copying all the plane storage from 8 to 2 and removing 8 and its links. I later instead decided to when searching at the end of a motion model to allow for the possibility of more then one plane globally matching which is similar to merging. The other issue is that the merging algorithm would be somewhat n^2.
Two things are not working here. The first is that the plane size is different from the initial size, so the plane has to be upsized before doing the comparison. The second is that some keypoints where missing so they had to be derived. This led to some rework and it was decided to force keypoint generation during the xpoint computation algorithm. If after the computation a keypoint doesn't exist, a function compares the rltplane to every other rltplane until one could generate a key point for the correct interval (the same one which generate the xreference). Note the correct interval could be important as the keypoint could change.
This is something I didn't realize. An algorithm determines its time to make a motion model xpoint when a good poset is unavailable for a number of iterations. When running with noise, a few iterations have a poset interspersed with iterations without a poset until finally there is no poset. At this point I reworked the whole getposet thing to save every poset and when a motion model xpoint is created go back to the last good poset and use that to start the motion model.
The problem with this one is that x4 should link with x3 which contains the motion model and the correct size of plane 4. Instead plane x4 links with x2 which I don't want. This also caused the keypoints to wrong as they were generated at different intervals.
An interesting observation and problem. At first I thought the problem was due to plane 6 being seen way too early due to no occlusion and a very large frustum. In turns out the error was the keypoints where on wrong intervals just like above. I later forced the keypoints to be recalculated if they were done in a different interval from the one that is used to do some calculation.
The left picture for some reason the plane is smaller then it should be. The issue was in a function to split intervals when the size of a plane changes direction (e.g it gets big then small, need to split it when it is its biggest size). The reason for this is that the keypoints will be different. What happened here is that it really didn't switch the growth of size, but there was a small amount of noise that made it appear that it did. I added in some leeway and the problem is fixed.
The main problem that were solved from phase3a where that the keypoints where misbehaving. They were reworked at the expense of spending more time computing them which is necessary. The other issue is the transition from motion model xpoint to normal xpoint was causing problems when closing the loop. Normally what happens is that a bunch of global matches happen and the xpoint switches to one of the first ones when one of the first xpoints has more current plane references then the past one. With the motion model simulation this would occur right after the 4rth motion model. There was also the problem that plane 2 and 8 were the same. To solve this after every motion model if there is a global match, all the planes from that iteration run are compared against all possible good global matches (to solve the plane 2 and 8 problem) and if there is an xpoint that contains all of the viewed plane, it switched to this point. Otherwise it creates a new xpoint. This solution works. One issue is that every time a motion model happens a new xpoint must be created but it has to be this way because its unknown if the robot will follow the old path, however if it does follow the old path other then the motion model xpoint no other xpoints are created.
So at last the motion model works. I was actually surprised that the loop closed when there was noise as the poset was a bit off but I guess there was enough leeway in the plane matching.