added icp point to plane optimization