June 21 end of week commit
Before adding cost function that set camera extrinsic to be fixed
Added linear solver(not tested) and fixed a bug in matlab simulation
1. Reorganized the code. 2. Adding unit test scripts