Total Least Squares

[hokiespurs]

@nosliwg
Here is an example code using Total Least Squares for the camera pose estimation. If you want to add the initial guess at pose as a constraint in the least squares solution, the jacobian and B matrix functions will both need to be modified slightly, but it's definitely doable. I think it would be an eye(6) added to the bottom of the Jacobian, and an -eye(6) added on the blkdiag for the B.

@Levi-Gorrell has some really cool ideas and has been developing some similar code as well, so I thought a sort of "sandbox" repo would be a good spot to collaborate.

[KateBrodie]

@hokiespurs this is awesome! Thanks for adding this. Excited to see where the community takes this! @nosliwg @Levi-Gorrell if you guys add to it, feel free to upload to the repo for all to see!!

When you guys get some spare time, could you add some wiki documentation? It can be brief, but support information to help the users understand what this code is and does (even if it's just in sandbox mode) would be great! @hokiespurs you could include some of the figures from our brief phone call last week.

@allisonpenko @mpalmsten @RobHolman @SRHarrison @jstanleyx we have our first user submitted repo from @hokiespurs Is there anything else we'd like @hokiespurs @nosliwg @Levi-Gorrell to do while they work on some total propagated uncertainty code? I know they have lots of ideas for cleaner, more sophisticated toolboxes, but I encouraged them to start collaborating from the beginning on here so code could remain version controlled and not be emailed back and forth.