def test_2D_ewa_tsdf_generation4(self):
filename = "zigzag1_depth_00064.png"
depth_image = self.image_load_helper(filename)
camera_intrinsic_matrix = np.array(
[[700., 0., 320.], [0., 700., 240.], [0., 0., 1.]],
dtype=np.float32)
camera = cam.DepthCamera(intrinsics=cam.Camera.Intrinsics(
(640, 3), intrinsic_matrix=camera_intrinsic_matrix),
depth_unit_ratio=0.001)
field = \
ewa.generate_tsdf_2d_ewa_tsdf_inclusive(depth_image, camera, 200,
field_size=16,
array_offset=np.array([-232, -256, 490]),
voxel_size=0.004,
gaussian_covariance_scale=0.5
)
self.assertTrue(np.allclose(field, data.out_sdf_field04))
parameters = cpp_module.tsdf.Parameters2d()
parameters.interpolation_method = cpp_module.tsdf.FilteringMethod.EWA_VOXEL_SPACE_INCLUSIVE
parameters.projection_matrix = camera_intrinsic_matrix
parameters.array_offset = cpp_module.Vector2i(-232, 490)
parameters.field_shape = cpp_module.Vector2i(16, 16)
parameters.smoothing_factor = 0.5
generator = cpp_module.tsdf.Generator2d(parameters)
field2 = generator.generate(depth_image,
np.identity(4, dtype=np.float32), 1)
self.assertTrue(np.allclose(field2, data.out_sdf_field04, atol=1e-5))
def test_2d_ewa_tsdf_generation2(self):
filename = "zigzag2_depth_00108.png"
depth_image = self.image_load_helper(filename)
test_full_image = False
camera_intrinsic_matrix = np.array(
[[700., 0., 320.], [0., 700., 240.], [0., 0., 1.]],
dtype=np.float32)
camera = cam.DepthCamera(intrinsics=cam.Camera.Intrinsics(
(640, 480), intrinsic_matrix=camera_intrinsic_matrix),
depth_unit_ratio=0.001)
offset_full_image = np.array([-256, 0, 0])
chunk_x_start = 210
chunk_y_start = 103
chunk_size = 16
offset_chunk_from_image = np.array([chunk_x_start, 0, chunk_y_start])
offset_chunk = offset_full_image + offset_chunk_from_image
if test_full_image:
parameters = cpp_module.tsdf.Parameters2d()
parameters.projection_matrix = camera_intrinsic_matrix
parameters.field_shape = cpp_module.Vector2i(512, 512)
parameters.array_offset = cpp_module.Vector2i(
int(offset_full_image[0]), int(offset_full_image[2]))
parameters.interpolation_method = cpp_module.tsdf.FilteringMethod.EWA_IMAGE_SPACE
generator = cpp_module.tsdf.Generator2d(parameters)
field2 = generator.generate(depth_image,
np.identity(4, dtype=np.float32), 200)
chunk = field2[chunk_y_start:chunk_y_start + chunk_size,
chunk_x_start:chunk_x_start + chunk_size].copy()
else:
parameters = cpp_module.tsdf.Parameters2d()
parameters.interpolation_method = cpp_module.tsdf.FilteringMethod.EWA_IMAGE_SPACE
parameters.projection_matrix = camera_intrinsic_matrix
parameters.array_offset = cpp_module.Vector2i(
int(offset_chunk[0]), int(offset_chunk[2]))
parameters.field_shape = cpp_module.Vector2i(16, 16)
generator = cpp_module.tsdf.Generator2d(parameters)
chunk = generator.generate(depth_image,
np.identity(4, dtype=np.float32), 200)
self.assertTrue(np.allclose(chunk, data.out_sdf_chunk))
field = \
ewa.generate_tsdf_2d_ewa_image(depth_image, camera, 200,
field_size=chunk_size,
array_offset=offset_chunk,
voxel_size=0.004)
self.assertTrue(np.allclose(field, data.out_sdf_chunk, atol=2e-5))
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_nonrigid_optimization02(self):
sampling.set_focus_coordinates(0, 0)
field_size = 4
live_field_template = np.array(
[[1., 1., 0.49999955, 0.42499956],
[1., 0.44999936, 0.34999937, 0.32499936],
[1., 0.35000065, 0.25000066, 0.22500065],
[1., 0.20000044, 0.15000044, 0.07500044]],
dtype=np.float32)
live_field = live_field_template.copy()
canonical_field = np.array(
[[1.0000000e+00, 1.0000000e+00, 3.7499955e-01, 2.4999955e-01],
[1.0000000e+00, 3.2499936e-01, 1.9999936e-01, 1.4999935e-01],
[1.0000000e+00, 1.7500064e-01, 1.0000064e-01, 5.0000645e-02],
[1.0000000e+00, 7.5000443e-02, 4.4107438e-07, -9.9999562e-02]],
dtype=np.float32)
optimizer = make_optimizer(ComputeMethod.DIRECT, field_size, 2)
optimizer.optimize(live_field, canonical_field)
expected_live_field_out = np.array(
[[1., 1., 0.48917317, 0.43777004],
[1., 0.43342987, 0.3444094, 0.3287867],
[1., 0.33020678, 0.24566807, 0.22797936],
[1., 0.2261582, 0.17907946, 0.14683424]],
dtype=np.float32)
report1 = optimizer.get_convergence_report()
self.assertTrue(np.allclose(live_field, expected_live_field_out))
live_field = live_field_template.copy()
optimizer = make_optimizer(ComputeMethod.VECTORIZED, field_size, 2)
optimizer.optimize(live_field, canonical_field)
self.assertTrue(np.allclose(live_field, expected_live_field_out))
report2 = optimizer.get_convergence_report()
self.assertTrue(report1 == report2)
expected_warp_stats = \
cpp_module.WarpDeltaStatistics2d(0.272727, 0.0, 0.0684823, 0.0364445,
0.0167321, cpp_module.Vector2i(1, 2), False, False)
expected_diff_stats = \
cpp_module.TsdfDifferenceStatistics2d(0, 0.246834, 0.111843, 0.0812234, cpp_module.Vector2i(3, 3))
expected_report = cpp_module.ConvergenceReport2d(
2, True, expected_warp_stats, expected_diff_stats)
self.assertTrue(report2 == expected_report)
def test_2D_ewa_tsdf_generation1(self):
depth_image = np.zeros((3, 640), dtype=np.uint16)
depth_image[:] = np.iinfo(np.uint16).max
depth_image_region = np.array(
[[
3233, 3246, 3243, 3256, 3253, 3268, 3263, 3279, 3272, 3289,
3282, 3299, 3291, 3308, 3301, 3317, 3310, 3326
],
[
3233, 3246, 3243, 3256, 3253, 3268, 3263, 3279, 3272, 3289,
3282, 3299, 3291, 3308, 3301, 3317, 3310, 3326
],
[
3233, 3246, 3243, 3256, 3253, 3268, 3263, 3279, 3272, 3289,
3282, 3299, 3291, 3308, 3301, 3317, 3310, 3326
]],
dtype=np.uint16)
depth_image[:, 399:417] = depth_image_region
camera_intrinsic_matrix = np.array(
[[700., 0., 320.], [0., 700., 240.], [0., 0., 1.]],
dtype=np.float32)
camera = cam.DepthCamera(intrinsics=cam.Camera.Intrinsics(
(640, 3), intrinsic_matrix=camera_intrinsic_matrix),
depth_unit_ratio=0.001)
field = \
ewa.generate_tsdf_2d_ewa_image(depth_image, camera, 1,
field_size=16,
array_offset=np.array([94, -256, 804]),
voxel_size=0.004)
self.assertTrue(np.allclose(field, data.out_sdf_field01, atol=2e-5))
parameters = cpp_module.tsdf.Parameters2d()
parameters.interpolation_method = cpp_module.tsdf.FilteringMethod.EWA_IMAGE_SPACE
parameters.projection_matrix = camera_intrinsic_matrix
parameters.array_offset = cpp_module.Vector2i(94, 804)
parameters.field_shape = cpp_module.Vector2i(16, 16)
generator = cpp_module.tsdf.Generator2d(parameters)
field2 = generator.generate(depth_image,
np.identity(4, dtype=np.float32), 1)
self.assertTrue(np.allclose(field2, data.out_sdf_field01, atol=1e-6))
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 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 optimize(self, live_field, canonical_field):
self.visualizer = viz.SlavchevaVisualizer(len(live_field),
self.out_path,
self.visualization_settings)
self.focus_neighborhood_log = \
self.__generate_initial_focus_neighborhood_log(self.field_size)
self.log = OptimizationLog()
warp_field = np.zeros((self.field_size, self.field_size, 2),
dtype=np.float32)
max_warp = np.inf
max_warp_location = (0, 0)
iteration_number = 0
self.gradient_field = np.zeros_like(warp_field)
self.__run_checks(live_field, canonical_field, warp_field)
if self.adaptive_learning_rate_method == AdaptiveLearningRateMethod.RMS_PROP:
# exponentially decaying average of squared gradients
self.edasg_field = np.zeros_like(live_field)
# do some logging initialization that requires canonical data
for (x, y), log in self.focus_neighborhood_log.items():
log.canonical_sdf = canonical_field[y, x]
# write original raw live
self.visualizer.write_live_sdf_visualizations(canonical_field,
live_field)
# actually perform the optimization
while (iteration_number < self.min_iterations) or \
(iteration_number < self.max_iterations and
self.maximum_warp_length_lower_threshold < max_warp < self.maximum_warp_length_upper_threshold):
if self.compute_method == ComputeMethod.DIRECT:
max_warp, max_warp_location = \
self.__optimization_iteration_direct(live_field, canonical_field, warp_field)
elif self.compute_method == ComputeMethod.VECTORIZED:
max_warp, max_warp_location = \
self.__optimization_iteration_vectorized(live_field, canonical_field, warp_field)
# log energy aggregates
self.log.max_warps.append(max_warp)
self.log.data_energies.append(self.total_data_energy)
self.log.smoothing_energies.append(self.total_smoothing_energy)
self.log.level_set_energies.append(self.total_level_set_energy)
# print end-of-iteration output
level_set_energy_string = ""
if self.level_set_term_enabled:
level_set_energy_string = "; level set energy: {:5f}".format(
self.total_level_set_energy)
print(BOLD_RED,
"[Iteration ",
iteration_number,
" done],",
RESET,
" data energy: {:5f}".format(self.total_data_energy),
"; smoothing energy: {:5f}".format(
self.total_smoothing_energy),
level_set_energy_string,
"; total energy:",
self.total_data_energy + self.total_smoothing_energy +
self.total_level_set_energy,
"; max warp:",
max_warp,
"@",
max_warp_location,
sep="")
self.visualizer.write_all_iteration_visualizations(
iteration_number, warp_field, self.gradient_field, live_field,
canonical_field)
iteration_number += 1
# log end-of-optimization stats
if self.enable_convergence_status_logging:
self.log.convergence_status = cpp_extension.ConvergenceStatus(
iteration_number, float(max_warp),
cpp_extension.Vector2i(int(max_warp_location.x),
int(max_warp_location.y)),
iteration_number >= self.max_iterations,
bool(max_warp < self.maximum_warp_length_lower_threshold),
bool(max_warp > self.maximum_warp_length_upper_threshold))
del self.visualizer
self.visualizer = None
return live_field