对[1]的matlab实现，有所改动，在计算新的点是否属于平面时用点到平面的距离替换MSE，亲测精度不变计算时间大大减少。
referenece
[1]Junhao Xiao, Jianhua Zhang and Jianwei Zhang，'Fast Plane Detection for SLAM from Noisy Range Images in Both Structured and Unstructured Environments',IEEE International Conference on Mechatronics & Automation,2011:1768-1773
