Cleaned up the code, decided with Sammy to improve the precision of the first estimates and add factors at camera rate. Converges well now