def test_sdf_generation10(self):
depth_image = np.ones((3, 3))
image_pixel_row = 1
offset = np.array([-1, -1, 1])
field_size = 3
narrow_band_width_voxels = 1
twist3d = np.zeros((6, 1))
twist3d[2] = -1
twist_matrix3d = twist_vector_to_matrix3d(twist3d)
intrinsic_matrix = np.array(
[
[1, 0, 1], # FX = 1 CX = 1
[0, 1, 1], # FY = 1 CY = 1
[0, 0, 1]
],
dtype=np.float32)
depth_camera = DepthCamera(intrinsics=DepthCamera.Intrinsics(
resolution=(3, 3), intrinsic_matrix=intrinsic_matrix),
depth_unit_ratio=1)
expected_field = np.array([[-999, -999, -999], [0, 0, 0], [-1, -1,
-1]])
field = tsdf_gen.generate_2d_tsdf_field_from_depth_image(
depth_image,
depth_camera,
image_pixel_row,
camera_extrinsic_matrix=twist_matrix3d,
field_size=field_size,
default_value=-999,
voxel_size=1,
array_offset=offset,
narrow_band_width_voxels=narrow_band_width_voxels)
self.assertTrue(np.allclose(expected_field, field))
def test_sdf_generation03(self):
depth_image = np.zeros((3, 3))
image_pixel_row = 1
offset = np.array([-1, -1, -1])
field_size = 3
narrow_band_width_voxels = 1
intrinsic_matrix = np.array(
[
[1, 0, 1], # FX = 1 CX = 1
[0, 1, 1], # FY = 1 CY = 1
[0, 0, 1]
],
dtype=np.float32)
depth_camera = DepthCamera(intrinsics=DepthCamera.Intrinsics(
resolution=(3, 3), intrinsic_matrix=intrinsic_matrix),
depth_unit_ratio=1)
expected_field = np.full((3, 3), -999)
field = tsdf_gen.generate_2d_tsdf_field_from_depth_image(
depth_image,
depth_camera,
image_pixel_row,
field_size=field_size,
default_value=-999,
voxel_size=1,
array_offset=offset,
narrow_band_width_voxels=narrow_band_width_voxels,
)
self.assertTrue(np.allclose(expected_field, field))
def test_sdf_2_sdf_optimizer01(self):
canonical_frame_path = "tests/testdata/depth_000000.exr"
live_frame_path = "tests/testdata/depth_000003.exr"
if not os.path.exists(canonical_frame_path) or not os.path.exists(
live_frame_path):
canonical_frame_path = "testdata/depth_000000.exr"
live_frame_path = "testdata/depth_000003.exr"
image_pixel_row = 240
intrinsic_matrix = np.array(
[
[570.3999633789062, 0, 320
], # FX = 570.3999633789062 CX = 320.0
[0, 570.3999633789062, 240
], # FY = 570.3999633789062 CY = 240.0
[0, 0, 1]
],
dtype=np.float32)
camera = DepthCamera(intrinsics=DepthCamera.Intrinsics(
resolution=(480, 640), intrinsic_matrix=intrinsic_matrix))
field_size = 32
offset = np.array([-16, -16, 93.4375])
data_to_use = ImageBasedSingleFrameDataset(
canonical_frame_path, # dataset from original sdf2sdf paper, reference frame
live_frame_path, # dataset from original sdf2sdf paper, current frame
image_pixel_row,
field_size,
offset,
camera)
# depth_interpolation_method = tsdf.DepthInterpolationMethod.NONE
out_path = "output/test_rigid_out"
sampling.set_focus_coordinates(0, 0)
narrow_band_width_voxels = 2.
iteration = 40
optimizer = sdf2sdfo.Sdf2SdfOptimizer2d(
verbosity_parameters=sdf2sdfo.Sdf2SdfOptimizer2d.
VerbosityParameters(print_max_warp_update=False,
print_iteration_energy=False),
visualization_parameters=sdf2sdfv.Sdf2SdfVisualizer.Parameters(
out_path=out_path,
save_initial_fields=False,
save_final_fields=False,
save_live_progression=True))
optimizer.optimize(data_to_use,
narrow_band_width_voxels=narrow_band_width_voxels,
iteration=iteration)
expected_twist = np.array([[-0.079572], [0.006052], [0.159114]])
twist = optimizer.optimize(
data_to_use,
narrow_band_width_voxels=narrow_band_width_voxels,
iteration=iteration)
self.assertTrue(np.allclose(expected_twist, twist, atol=10e-6))
def from_infinitam_format(file_handle_or_name):
if type(file_handle_or_name) == str:
file_handle = open(file_handle_or_name, 'r')
else:
file_handle = file_handle_or_name
color_resolution_line = file_handle.readline()
color_intrinsics_lines = [
file_handle.readline(),
file_handle.readline()
]
file_handle.readline()
depth_resolution_line = file_handle.readline()
depth_intrinsics_lines = [
file_handle.readline(),
file_handle.readline()
]
file_handle.readline()
extrinsics_lines = [
file_handle.readline(),
file_handle.readline(),
file_handle.readline()
]
file_handle.readline()
depth_ratio_and_offset_line = file_handle.readline()
color_camera = Camera.from_infinitam_format(color_resolution_line,
color_intrinsics_lines)
depth_camera = DepthCamera.from_infinitam_format(
depth_resolution_line, depth_intrinsics_lines, extrinsics_lines,
depth_ratio_and_offset_line)
return DepthCameraRig(cameras=(color_camera, depth_camera))
def main():
canonical_frame_path = "../Data/Synthetic_Kenny_Circle/depth_000000.exr"
live_frame_path = "../Data/Synthetic_Kenny_Circle/depth_000003.exr"
image_pixel_row = 240
intrinsic_matrix = np.array(
[
[570.3999633789062, 0, 320], # FX = 570.3999633789062 CX = 320.0
[0, 570.3999633789062, 240], # FY = 570.3999633789062 CY = 240.0
[0, 0, 1]
],
dtype=np.float32)
camera = DepthCamera(intrinsics=DepthCamera.Intrinsics(
resolution=(480, 640), intrinsic_matrix=intrinsic_matrix))
field_size = 32
# offset = np.array([-16, -16, 102.875])
offset = np.array([-16, -16, 93.4375])
data_to_use = ImageBasedSingleFrameDataset(
canonical_frame_path, # dataset from original sdf2sdf paper, reference frame
live_frame_path, # dataset from original sdf2sdf paper, current frame
image_pixel_row,
field_size,
offset,
camera)
# depth_interpolation_method = tsdf.DepthInterpolationMethod.NONE
out_path = "output/sdf_2_sdf"
sampling.set_focus_coordinates(0, 0)
narrow_band_width_voxels = 2.
iteration = 40
optimizer = sdf2sdfo.Sdf2SdfOptimizer2d(
verbosity_parameters=sdf2sdfo.Sdf2SdfOptimizer2d.VerbosityParameters(
print_max_warp_update=True, print_iteration_energy=True),
visualization_parameters=sdf2sdfv.Sdf2SdfVisualizer.Parameters(
out_path=out_path,
save_initial_fields=True,
save_final_fields=True,
save_live_progression=True))
optimizer.optimize(data_to_use,
narrow_band_width_voxels=narrow_band_width_voxels,
iteration=iteration)
return EXIT_CODE_SUCCESS
def test_sdf_generation11(self):
filename = "zigzag2_depth_00108.png"
depth_image = self.image_load_helper(filename)
image_pixel_row = 200
offset = np.array([-8, -8, 144], dtype=np.int32)
field_size = 16
narrow_band_width_voxels = 20
camera_intrinsic_matrix = np.array(
[[700., 0., 320.], [0., 700., 240.], [0., 0., 1.]],
dtype=np.float32)
camera_extrinsic_matrix = np.eye(4, dtype=np.float32)
depth_camera = DepthCamera(intrinsics=DepthCamera.Intrinsics(
(640, 480), intrinsic_matrix=camera_intrinsic_matrix),
depth_unit_ratio=0.001)
field = tsdf_gen.generate_2d_tsdf_field_from_depth_image(
depth_image,
depth_camera,
image_pixel_row,
camera_extrinsic_matrix=camera_extrinsic_matrix,
field_size=field_size,
default_value=-999,
voxel_size=0.004,
array_offset=offset,
narrow_band_width_voxels=narrow_band_width_voxels)
self.assertTrue(np.allclose(field, data.out_sdf_field01))
parameters = cpp_module.tsdf.Parameters2d()
parameters.interpolation_method = cpp_module.tsdf.FilteringMethod.NONE
parameters.projection_matrix = camera_intrinsic_matrix
parameters.array_offset = cpp_module.Vector2i(int(offset[0]),
int(offset[2]))
parameters.field_shape = cpp_module.Vector2i(field_size, field_size)
generator = cpp_module.tsdf.Generator2d(parameters)
field2 = generator.generate(depth_image,
np.identity(4, dtype=np.float32), 1)
self.assertTrue(np.allclose(field, field2, atol=1e-6))
def __init__(self,
calibration_file_path,
first_frame_path,
second_frame_path,
image_pixel_row,
field_size,
offset,
voxel_size=0.004,
focus_coordinates=(-1, -1, -1)):
super().__init__(focus_coordinates)
self.calibration_file_path = calibration_file_path
self.first_frame_path = first_frame_path
self.second_frame_path = second_frame_path
self.image_pixel_row = image_pixel_row
self.field_size = field_size
self.offset = offset
self.voxel_size = voxel_size
if os.path.exists(self.calibration_file_path) and os.path.isdir(
self.calibration_file_path):
self.rig = DepthCameraRig.from_infinitam_format(
self.calibration_file_path)
else:
camera_intrinsic_matrix = np.array(
[[700., 0., 320.], [0., 700., 240.], [0., 0., 1.]],
dtype=np.float32)
camera = DepthCamera(intrinsics=Camera.Intrinsics(
(480, 640), intrinsic_matrix=camera_intrinsic_matrix),
depth_unit_ratio=0.001)
self.rig = DepthCameraRig(cameras=(camera, ))
parameters_2d = cpp_module.tsdf.Parameters2d()
parameters_2d.interpolation_method = cpp_module.tsdf.FilteringMethod.NONE
parameters_2d.projection_matrix = self.rig.depth_camera.intrinsics.intrinsic_matrix.astype(
np.float32)
parameters_2d.array_offset = cpp_module.Vector2i(
int(self.offset[0]), int(self.offset[2]))
parameters_2d.field_shape = cpp_module.Vector2i(
self.field_size, self.field_size)
self.parameters_2d = parameters_2d
parameters_3d = cpp_module.tsdf.Parameters3d()
parameters_3d.interpolation_method = cpp_module.tsdf.FilteringMethod.NONE
parameters_3d.projection_matrix = self.rig.depth_camera.intrinsics.intrinsic_matrix.astype(
np.float32)
parameters_3d.array_offset = cpp_module.Vector3i(
int(self.offset[0]), int(self.offset[1]), int(self.offset[2]))
parameters_3d.field_shape = cpp_module.Vector3i(
self.field_size, self.field_size, self.field_size)
self.parameters_3d = parameters_3d
def test_operation_same_cpp_to_py(self):
canonical_frame_path = "tests/test_data/depth_000000.exr"
live_frame_path = "tests/test_data/depth_000003.exr"
if not os.path.exists(canonical_frame_path) or not os.path.exists(live_frame_path):
canonical_frame_path = "test_data/depth_000000.exr"
live_frame_path = "test_data/depth_000003.exr"
image_pixel_row = 240
intrinsic_matrix = np.array([[570.3999633789062, 0, 320], # FX = 570.3999633789062 CX = 320.0
[0, 570.3999633789062, 240], # FY = 570.3999633789062 CY = 240.0
[0, 0, 1]], dtype=np.float32)
camera = DepthCamera(intrinsics=DepthCamera.Intrinsics(resolution=(480, 640),
intrinsic_matrix=intrinsic_matrix))
voxel_size = 0.004
narrow_band_width_voxels = 2
field_size = 32
offset = np.array([[-16], [-16], [93]], dtype=np.int32)
eta = 0.01 # thickness, used to calculate sdf weight.
camera_pose = np.eye(4, dtype=np.float32)
shared_parameters = build_opt.Sdf2SdfOptimizer2dSharedParameters()
shared_parameters.rate = 0.5
shared_parameters.maximum_iteration_count = 8
# For verbose output from py version:
verbosity_parameters_py = sdf2sdfo_py.Sdf2SdfOptimizer2d.VerbosityParameters(True, True)
verbosity_parameters_cpp = sdf2sdfo_cpp.Sdf2SdfOptimizer2d.VerbosityParameters(True, True)
visualization_parameters_py = sdf2sdfv.Sdf2SdfVisualizer.Parameters()
visualization_parameters_py.out_path = "out"
# For C++ TSDF generator
tsdf_generation_parameters = sdf2sdfo_cpp.tsdf.Parameters2d(
depth_unit_ratio=0.001, # mm to meter
projection_matrix=intrinsic_matrix,
near_clipping_distance=0.05,
array_offset=sdf2sdfo_cpp.Vector2i(int(offset[0, 0]), int(offset[2, 0])),
field_shape=sdf2sdfo_cpp.Vector2i(field_size, field_size),
voxel_size=voxel_size,
narrow_band_width_voxels=narrow_band_width_voxels,
interpolation_method=sdf2sdfo_cpp.tsdf.FilteringMethod.NONE
)
# Read image for c++ optimizer, identical to python, which is done inside ImageBasedSingleFrameDataset class.
canonical_depth_image = cv2.imread(canonical_frame_path, cv2.IMREAD_UNCHANGED)
canonical_depth_image = canonical_depth_image.astype(np.uint16) # mm
canonical_depth_image = cv2.cvtColor(canonical_depth_image, cv2.COLOR_BGR2GRAY)
canonical_depth_image[canonical_depth_image == 0] = np.iinfo(np.uint16).max
live_depth_image = cv2.imread(live_frame_path, cv2.IMREAD_UNCHANGED)
live_depth_image = live_depth_image.astype(np.uint16) # mm
live_depth_image = cv2.cvtColor(live_depth_image, cv2.COLOR_BGR2GRAY)
live_depth_image[live_depth_image == 0] = np.iinfo(np.uint16).max
canonical_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(canonical_depth_image, camera, image_pixel_row,
field_size=field_size,
array_offset=offset,
narrow_band_width_voxels=narrow_band_width_voxels,
interpolation_method=sdf2sdfo_cpp.tsdf.FilteringMethod.NONE)
optimizer_cpp = build_opt.make_sdf_2_sdf_optimizer2d(
implementation_language=build_opt.ImplementationLanguage.CPP,
shared_parameters=shared_parameters,
verbosity_parameters_cpp=verbosity_parameters_cpp,
verbosity_parameters_py=verbosity_parameters_py,
visualization_parameters_py=visualization_parameters_py,
tsdf_generation_parameters_cpp=tsdf_generation_parameters)
twist_cpp = optimizer_cpp.optimize(image_y_coordinate=image_pixel_row,
canonical_field=canonical_field,
live_depth_image=live_depth_image,
eta=eta,
initial_camera_pose=camera_pose)
# For python optimizer
data_to_use = ImageBasedSingleFrameDataset( # for python
canonical_frame_path, # dataset from original sdf2sdf paper, reference frame
live_frame_path, # dataset from original sdf2sdf paper, current frame
image_pixel_row, field_size, offset, camera
)
optimizer_py = build_opt.make_sdf_2_sdf_optimizer2d(
implementation_language=build_opt.ImplementationLanguage.PYTHON,
shared_parameters=shared_parameters,
verbosity_parameters_cpp=verbosity_parameters_cpp,
verbosity_parameters_py=verbosity_parameters_py,
visualization_parameters_py=visualization_parameters_py,
tsdf_generation_parameters_cpp=tsdf_generation_parameters)
twist_py = optimizer_py.optimize(data_to_use,
voxel_size=0.004,
narrow_band_width_voxels=narrow_band_width_voxels,
iteration=shared_parameters.maximum_iteration_count,
eta=eta)
self.assertTrue(np.allclose(twist_cpp, transformation.twist_vector_to_matrix2d(twist_py), atol=1e-4))
