Tantalizingly close

So close and yet so far. That is how it feels at the moment. I found the bug I mentioned in the last post, and had hoped would be the last piece of the puzzle before I had camera ego motion calculations working, but it wasn't to be.

I now have a problem with the build up of error over consecutive frames. My sanity check to see if ego motion calculation is working is to run the system completely stationary ( the only option at the moment anyway ). Therefore the ego motion calculation between frames should render a 0, 0, 0, 0 result. i.e No movement in any of the 3 spatial dimensions and no rotation. Unfortunately I am getting results that look more like this after one iteration:
-----------------------------------------
New position after ego motion estimation:
X = -0.044358336825996
Y = 0.025654321647698
Z = -0.003337081060976
Theta = 0.431281320335258

Obviously subsequent iterations compound this error and it quickly spirals out of control.

Now I realise in the real system there will be the odometery feeding into this and the odometery and ego motion will be combined in a kalman filter to get the actual estimated motion. In this way the accumulative error won't matter so much as it will be dampened by the odometery each iteration.

My concern is that this error is far too big to start off with. I can accept (and was expecting) some error between frames, but in the above example the system believes it has moved 4cm to the left. 4cm is too much. What I have to do now is identify the source of this error. I suspect it may be something to do with excessive accuracy in the SIFT feature 3d position calculations. Any error between calculated feature locations could be being exacerbated by the least squares minimisation. That at least is my uneducated guess at this point. More investigation will hopefully shed some light on this issue.