Improve SfM accuracy and precision

Summary
Improve SfM accuracy and precision