Fixed a bug in tsdf_surface that caused failures for 2D and created pose sampling