Motion Model
What is the motion model? The first thing I thought that when only one partial plane is visible the program would have to switch to just figuring out where the next position given a restriction to the motion (e.i only go forward, no side to side motion which would cause the problem to be unconstrained), and using that information to figure out location. Unfortunately that would mean current location is based on a previous so the global position would be reused which is not ideal.
As I was thinking about it, I realized that the only underlying mechanism that is necessary is to figure out how to expand the plane size. The previous xpoint is going to dictate the partial plane position and rotation.
So what is important is the original relationship of the partial plane does not change. So the motion model locks the plane in the previous position and just figures out based on the movement restrictions and the current reading has the plane expanded? As soon as another plane is sited then the motion model turns off.
In summary when only one partial plane is observed by the getposet mapbuilding function not being able to figure out where we are, a special type of xpoint is created at the last known good global position (this subjects it to a bit of error). That xpoint is a subclass and its compute xpoint uses the current plane reading midpoint as a point in a distance to plane equation to the known partial plane at the last good global position iteration. This distance combined with any rotation difference gives a poset. The poset is then used to figure out of the plane expanded. As soon as another plane is seen the motion model switchs off. There is the potential for some error from the initial point to the fact that we somewhat use absolute coordinates but we always coorispond to the known plane location so this should minimize any error.
Some interesting observations when coding the algorithm
The smaller plane at 90 degrees is assigned plane 7 not plane 6. I was wondering why and I realized that plane 6 was a canceled plane early when plane 5 was observed a few times in partial form so the unused partial form was plane 6 and it was removed since it was observed only a limited amount of times. Nice to know that is working as it hasn't been tested up to this point
The following seems like a bad bug but it isn't. With only a few reading the noise distorts which keypoints are the correct ones, and given how big plane 5 is, it assumes keypoint 1 not two is the accurate one and it expands the plane from the other side. The algorithm self corrects a few iterations later. This does not happen when there is no noise. Since the global position is not that important it should not really matter when this happens. There could be a global mismatch I suppose in the long term it would only cause a duplicate plane of which the mismatch occured.
There is no short term solution. The long term would be to store the moving average of the keypoint and when it is determined go back in time and correct it. This is very possible by storing several iterations worth of raw data and then removing all data past a certain iteration and rerunning it with some information such as keypoints locked. This is somewhat simular to the xpoint locking after enough readings.
The above problem was pretty major. When doing the planecompare the best interval is used. X3 has 3 planes 3,5,6. When comparing plane 3 and 5 it uses an interval before the rotation which is the most accurate one. At this interval plane 5 is full size so it returns a differences of 30,-30 rather then 5,-30. When chaining the plane compares a large error occurs due to the comparison between plane 5 and 6 uses a smaller size of plane 5. An easy solution would be to use the interval after the xpoint is created but this interval would most likely be less accurate since plane 3 is not observed for that long. Instead I check the size of the plane in adject plane compares and then using the correct keypoint and size difference I recalculate the offset. This does appear to solve the problem, and compares to using the less accurate interval it is a more elegant solution.
This sequence is very interesting. The motion model turns off and going back to the normal mode. X2 is created. One frame later X3 is created. The reason for this is earlier it was found out that adding a global match plane may be there for a very short time and it would mess of up how many iterations are available for a plane compare. X0 is not adjacent to X2 which is one case where it would not make an extra xpoint so that at least makes some sense. So why X4? If you notice X0 is the current xpoint which is incorrect and then plane 6 is seen which is not in X0 so another new xpoint is created. The only error is the X4 so why?
The problem was caused by X0 being the current plane. The issue was when X3 is created it recharts plane 3,5,6 into the chart which figures out which xpoint has the greatest percentage of matches. This causes X0 to win. The solution is to only chart a planes reference once per iteration which solves this. After the fix the last picture "rltwhyx4fixed" shows that it is corrected.
One more small issue. Before the motion model switches on which preempts the getbestxpoint from running X0 is the current xpoint. The reason is because pretty much all the xpoints have the one reference but X0 is the first on the list so it gets used. I added some code that if there is a tie and the previous xpoint is included in the tight pick the previous xpoint. The difference is seen in the last one as X4 links to X3 not X0, which I suppose is more ideal.
I included video of the motion model so far
Next Up Continued Work on the Motion Model