Fixed conversion from 3D to 2D
Fixed what is put into BadPointsIndex
Corrected the division (from 4k to 4m)
Merged from vins-integration branch