Plane segmentation and decimation of point clouds for 3D environment reconstruction