TSDF Integration

Truncated Signed Distance Function (TSDF) integration is the key of dense volumetric scene reconstruction. It receives relatively noisy depth images from RGB-D sensors such as Kinect and RealSense, and integrates depth readings into the Voxel Block Grid given known camera poses. TSDF integration reduces noise and generates smooth surfaces.

The integration process mainly consists of two steps, (sparse) block selection and activation, and (dense) voxel value integration. An example can be found at examples/python/t_reconstruction_system/integrate.py.

Activation

In the activation step, we first locate blocks that contain points unprojected from the current depth image. In other words, it finds active blocks in the current viewing frustum. Internally, this is achieved by a frustum hash map that produces duplication-free block coordinates, and a block hash map that activates and query such block coordinates.

82# examples/python/t_reconstruction_system/integrate.py
83            frustum_block_coords = vbg.compute_unique_block_coordinates(
84                depth, depth_intrinsic, extrinsic, config.depth_scale,
85                config.depth_max)

Integration

Now we can process the voxels in the blocks at frustum_block_coords. This is done by projecting all such related voxels to the input images and perform a weighted average, which is a pure geometric process without hash map operations.

We may use optimized functions, along with raw depth images with calibration parameters to activate and perform TSDF integration, optionally with colors:

86# examples/python/t_reconstruction_system/integrate.py
87            if config.integrate_color:
88                color = o3d.t.io.read_image(color_file_names[i]).to(device)
89                vbg.integrate(frustum_block_coords, depth, color,
90                              depth_intrinsic, color_intrinsic, extrinsic,
91                              config.depth_scale, config.depth_max)
92            else:
93                vbg.integrate(frustum_block_coords, depth, depth_intrinsic,

Currently, to use our optimized function, we assume the below combinations of data types, in the order of depth image, color image, tsdf in voxel grid, weight in voxel grid, color in voxel grid in CPU

229template void IntegrateCPU<uint16_t, uint8_t, float, uint16_t, uint16_t>(
230        FN_ARGUMENTS);
231template void IntegrateCPU<uint16_t, uint8_t, float, float, float>(
232        FN_ARGUMENTS);
233template void IntegrateCPU<float, float, float, uint16_t, uint16_t>(
234        FN_ARGUMENTS);

and CUDA

255template void IntegrateCUDA<uint16_t, uint8_t, float, uint16_t, uint16_t>(
256        FN_ARGUMENTS);
257template void IntegrateCUDA<uint16_t, uint8_t, float, float, float>(
258        FN_ARGUMENTS);
259template void IntegrateCUDA<float, float, float, uint16_t, uint16_t>(
260        FN_ARGUMENTS);

For more generalized functionalities, you may extend the macros and/or the kernel functions and compile Open3D from scratch achieve the maximal performance (~100Hz on a GTX 1070), or follow Customized Integration and implement a fast prototype (~25Hz).

Surface extraction

You may use the provided APIs to extract surface points.

135# examples/python/t_reconstruction_system/integrate.py
136    vbg = integrate(depth_file_names, color_file_names, depth_intrinsic,
137                    color_intrinsic, extrinsics, config)
138
139    pcd = vbg.extract_point_cloud()
140    o3d.visualization.draw([pcd])

Note extract_triangle_mesh applies marching cubes and generate mesh. extract_point_cloud uses the similar algorithm, but skips the triangle face generation step.

Save and load

The voxel block grids can be saved to and loaded from .npz files that are accessible via numpy.

47# examples/python/t_reconstruction_system/integrate.py
48        vbg = o3d.t.geometry.VoxelBlockGrid.load(config.path_npz)
49        print('Saving to {}...'.format(config.path_npz))

The .npz file of the aforementioned voxel block grid contains the following entries:

  • attr_name_tsdf: stores the value buffer index: 0

  • attr_name_weight: stores the value buffer index: 1

  • attr_name_color: stores the value buffer index: 2

  • value_000: the tsdf value buffer

  • value_001: the weight value buffer

  • value_002: the color value buffer

  • key: all the active keys

  • block_resolution: 8

  • voxel_size: 0.0059 = 3.0 / 512

  • CUDA:0: the device