Fixed a bug in icp mapping, changed the optimization level to O4, which made a big difference for the optical flow registration