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