Imported from https://github.com/shichaoy/cube_slam