[BUG] Three sources of inaccuracy

[paddyobrien]

I've done some work using the code from this project so I wanted to give back a little and share a couple of issues I found in case they are useful to others. Upstreaming my fixes isn't really feasible so I'll just describe them here.

1. Making the image square prior to undistorting causes inaccurate undistortion and ultimately inaccurate tracking, I found the results were off by about 4mm and distances between two points on the same object changed as the object moved around the scene. These lines here should probably be flipped, or the "make_square" call just removed entirely as it doesn't really contribute anything.
2. During pose estimation the wrong intrinsic matrices are used when getting the essential matrix, here the intrinsics for camera 0 and 1 are always used rather than camera_i and camera_i + 1.
3. Also during pose estimation a call is made to "triangulate_points" here, this always passes two camera poses for the two cameras that are having their Fundamental matrix calculated. Within "triangulate_points" the projection matrices for these cameras are generated, however the wrong intrinsic matrices get used for this because "triangulate_points" assumes that the full set of cameras is always passed to it and matches the intrinsic matrices using the array index of the camera pose here. The fix I landed on for this was switch to passing projection matrices into triangulate_points rather than poses. This involves quite a few changes around the codebase.

I have made a bunch of other changes which can be found here: https://github.com/paddyobrien/weccap. I've tested this work fairly rigorously and found that the motion tracking is now accurate to a fraction of a mm at the scales I use!

Thanks again to the original author for all the great work and for sharing publicly.