Imported from https://github.com/chaowang15/plane-opt-rgbd