added normal estimation to increase plane segmentation accuracy