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_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 generate_2d_canonical_field(self,
narrow_band_width_voxels=20.,
method=tsdf_gen.GenerationMethod.NONE):
canonical_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(self.depth_image0, self.depth_camera, self.image_pixel_row,
field_size=self.field_size,
array_offset=self.offset,
narrow_band_width_voxels=narrow_band_width_voxels,
generation_method=method)
return canonical_field
def generate_2d_sdf_canonical(self, method=tsdf_gen.GenerationMethod.NONE):
rig = DepthCameraRig.from_infinitam_format(self.calibration_file_path)
depth_camera = rig.depth_camera
depth_image0 = cv2.imread(self.first_frame_path, cv2.IMREAD_UNCHANGED)
max_depth = np.iinfo(np.uint16).max
depth_image0[depth_image0 == 0] = max_depth
canonical_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(depth_image0, depth_camera, self.image_pixel_row,
field_size=self.field_size, array_offset=self.offset,
generation_method=method,
voxel_size=self.voxel_size)
return canonical_field
def generate_2d_live_field(self,
method=tsdf_gen.GenerationMethod.NONE,
narrow_band_width_voxels=20.,
twist=np.zeros((6, 1))):
twist_matrix = twist_vector_to_matrix3d(twist)
live_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(self.depth_image1, self.depth_camera, self.image_pixel_row,
camera_extrinsic_matrix=twist_matrix,
field_size=self.field_size,
array_offset=self.offset,
narrow_band_width_voxels=narrow_band_width_voxels,
generation_method=method)
return live_field
def generate_2d_canonical_field(self,
narrow_band_width_voxels=20.,
method=tsdf_gen.GenerationMethod.NONE):
depth_image0 = cv2.imread(self.first_frame_path, -1)
depth_image0 = cv2.cvtColor(depth_image0, cv2.COLOR_BGR2GRAY)
depth_image0 = depth_image0.astype(float) # cm
canonical_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(depth_image0, self.depth_camera, self.image_pixel_row,
field_size=self.field_size,
array_offset=self.offset,
narrow_band_width_voxels=narrow_band_width_voxels,
generation_method=method)
return canonical_field
def generate_2d_canonical_field(
self,
narrow_band_width_voxels=20.,
method=cpp_module.tsdf.FilteringMethod.NONE):
depth_image0 = cv2.imread(self.first_frame_path, -1)
depth_image0 = depth_image0.astype(np.uint16) # mm
depth_image0 = cv2.cvtColor(depth_image0, cv2.COLOR_BGR2GRAY)
depth_image0[depth_image0 == 0] = np.iinfo(np.uint16).max
canonical_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(depth_image0, self.depth_camera, self.image_pixel_row,
field_size=self.field_size,
array_offset=self.offset,
narrow_band_width_voxels=narrow_band_width_voxels,
interpolation_method=method)
return canonical_field
def generate_2d_live_field(self,
method=tsdf_gen.GenerationMethod.NONE,
narrow_band_width_voxels=20.,
twist=np.zeros((6, 1))):
depth_image1 = cv2.imread(self.second_frame_path, -1)
depth_image1 = cv2.cvtColor(depth_image1, cv2.COLOR_BGR2GRAY)
depth_image1 = depth_image1.astype(float) # cm
twist_matrix = twist_vector_to_matrix3d(twist)
live_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(depth_image1, self.depth_camera, self.image_pixel_row,
camera_extrinsic_matrix=twist_matrix,
field_size=self.field_size,
array_offset=self.offset,
narrow_band_width_voxels=narrow_band_width_voxels,
generation_method=method)
return live_field
def generate_2d_live_field(self,
method=cpp_module.tsdf.FilteringMethod.NONE,
narrow_band_width_voxels=20.,
twist=np.zeros((6, 1))):
depth_image1 = cv2.imread(self.second_frame_path, -1)
depth_image1 = depth_image1.astype(np.uint16) # mm
depth_image1 = cv2.cvtColor(depth_image1, cv2.COLOR_BGR2GRAY)
depth_image1[depth_image1 == 0] = np.iinfo(np.uint16).max
twist_matrix = twist_vector_to_matrix3d(twist)
live_field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(depth_image1, self.depth_camera, self.image_pixel_row,
camera_extrinsic_matrix=twist_matrix,
field_size=self.field_size,
array_offset=self.offset,
narrow_band_width_voxels=narrow_band_width_voxels,
interpolation_method=method)
return live_field
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 _generate_2d_sdf_aux_image(self,
depth_image,
method=cpp_module.tsdf.FilteringMethod.NONE,
smoothing_coefficient=1.0,
use_cpp=False):
max_depth = np.iinfo(np.uint16).max
depth_image[depth_image == 0] = max_depth
if not use_cpp:
field = \
tsdf_gen.generate_2d_tsdf_field_from_depth_image(depth_image, self.rig.depth_camera,
self.image_pixel_row, field_size=self.field_size,
array_offset=self.offset,
interpolation_method=method,
voxel_size=self.voxel_size,
smoothing_coefficient=smoothing_coefficient)
else:
self.parameters_2d.smoothing_factor = smoothing_coefficient
self.parameters_2d.interpolation_method = method
generator = cpp_module.tsdf.Generator2d(self.parameters_2d)
field = generator.generate(depth_image,
np.identity(4, dtype=np.float32),
self.image_pixel_row)
return field
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))
def main():
save_profile = False
fraction_field = False
image_choice = 2
if fraction_field or image_choice == 1:
image_path = "/media/algomorph/Data/Reconstruction/synthetic_data/zigzag/depth/depth_00064.png"
z_offset = 480 # zigzag - 64
else:
image_path = "/media/algomorph/Data/Reconstruction/synthetic_data/zigzag2/input/depth_00108.png"
z_offset = 0 # zigzag2 - 108
# depth_interpolation_method = gen.GenerationMethod.NONE
# depth_interpolation_method = gen.GenerationMethod.EWA_IMAGE
# depth_interpolation_method = gen.GenerationMethod.EWA_IMAGE_CPP
# depth_interpolation_method = gen.GenerationMethod.EWA_TSDF
# depth_interpolation_method = gen.GenerationMethod.EWA_TSDF_INCLUSIVE
depth_interpolation_method = gen.GenerationMethod.EWA_TSDF_CPP
if save_profile:
im = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)
sl = im[200]
plt.figure(figsize=(40, 30))
plt.plot(sl, "k")
filename = "../output/image_slice_plot.png"
plt.savefig(filename)
plt.clf()
plt.close("all")
else:
if fraction_field:
voxel_size = 0.004
field_size = 16
# field_size = 64
# offset = np.array([94, -256, 804])
offset = np.array([-232, -256, 490])
rig = DepthCameraRig.from_infinitam_format(
"/media/algomorph/Data/Reconstruction/synthetic_data/zigzag/inf_calib.txt"
)
depth_camera = rig.depth_camera
depth_image0 = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)
max_depth = np.iinfo(np.uint16).max
depth_image0[depth_image0 == 0] = max_depth
field = \
gen.generate_2d_tsdf_field_from_depth_image(depth_image0, depth_camera, 200,
field_size=field_size,
array_offset=offset,
generation_method=depth_interpolation_method,
voxel_size=voxel_size,
smoothing_coefficient=0.5
)
print(repr(field))
else:
voxel_size = 0.004
field_size = 512
rig = DepthCameraRig.from_infinitam_format(
"/media/algomorph/Data/Reconstruction/synthetic_data/zigzag/inf_calib.txt"
)
depth_camera = rig.depth_camera
depth_image0 = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)
max_depth = np.iinfo(np.uint16).max
depth_image0[depth_image0 == 0] = max_depth
field = \
gen.generate_2d_tsdf_field_from_depth_image(depth_image0, depth_camera, 200,
field_size=field_size,
array_offset=np.array([-256, -256, z_offset]),
generation_method=depth_interpolation_method,
voxel_size=voxel_size,
smoothing_coefficient=0.5)
print(repr(field[103:119, 210:226]))
# print(repr(field[102:120, 209:226]))
viz.visualize_field(field, view_scaling_factor=2)
return EXIT_CODE_SUCCESS
