def sdf_gradient_wrt_to_twist(live_field, y_field, x_field, twist_vector, offset, voxel_size): sdf_gradient_wrt_to_voxel = np.zeros((1, 2)) if y_field - 1 < 0: post_sdf = live_field[y_field + 1, x_field] if post_sdf < -1: sdf_gradient_wrt_to_voxel[0, 1] = 0 else: sdf_gradient_wrt_to_voxel[0, 1] = post_sdf - live_field[y_field, x_field] elif y_field + 1 > live_field.shape[0] - 1: pre_sdf = live_field[y_field - 1, x_field] if pre_sdf < -1: sdf_gradient_wrt_to_voxel[0, 1] = 0 else: sdf_gradient_wrt_to_voxel[0, 1] = live_field[y_field, x_field] - pre_sdf else: pre_sdf = live_field[y_field - 1, x_field] post_sdf = live_field[y_field + 1, x_field] if (post_sdf < -1) or (pre_sdf < -1): sdf_gradient_wrt_to_voxel[0, 1] = 0 else: sdf_gradient_wrt_to_voxel[0, 1] = (post_sdf - pre_sdf) / 2 if x_field - 1 < 0: post_sdf = live_field[y_field, x_field + 1] if post_sdf < -1: sdf_gradient_wrt_to_voxel[0, 0] = 0 else: sdf_gradient_wrt_to_voxel[0, 0] = post_sdf - live_field[y_field, x_field] elif x_field + 1 > live_field.shape[1] - 1: pre_sdf = live_field[y_field, x_field - 1] if pre_sdf < -1: sdf_gradient_wrt_to_voxel[0, 0] = 0 else: sdf_gradient_wrt_to_voxel[0, 0] = live_field[y_field, x_field] - pre_sdf else: pre_sdf = live_field[y_field, x_field - 1] post_sdf = live_field[y_field, x_field + 1] if (post_sdf < -1) or (pre_sdf < -1): sdf_gradient_wrt_to_voxel[0, 0] = 0 else: sdf_gradient_wrt_to_voxel[0, 0] = (post_sdf - pre_sdf) / 2 x_voxel = (x_field + offset[0]) * voxel_size z_voxel = (y_field + offset[2]) * voxel_size point = np.array([[x_voxel, z_voxel, 1.]], dtype=np.float32).T twist_matrix_homo_inv = twist_vector_to_matrix2d(-twist_vector) trans = np.dot(twist_matrix_homo_inv, point) voxel_gradient_wrt_to_twist = np.array([[1, 0, trans[1]], [0, 1, -trans[0]]]) return np.dot(sdf_gradient_wrt_to_voxel / voxel_size, voxel_gradient_wrt_to_twist).reshape((1, -1))
def test_twist_vector_to_matrix2d06(self): vector = np.array([[100], [-100], [math.pi / 3]]) expected_matrix = np.array([[1 / 2, math.sqrt(3) / 2, -100], [-math.sqrt(3) / 2, 1 / 2, 100], [0, 0, 1]]) matrix = twist_vector_to_matrix2d(-vector) self.assertTrue(np.allclose(expected_matrix, matrix))
def calculate_gradient_wrt_twist(live_field, twist, array_offset, voxel_size=0.004): gradient_field = np.zeros((live_field.shape[0], live_field.shape[1], 3), dtype=np.float32) sdf_gradient_first_term = np.gradient(live_field) twist_matrix_homo_inv = twist_vector_to_matrix2d(-twist) y_voxel = 0.0 w_voxel = 1.0 for y_field in range(live_field.shape[0]): for x_field in range(live_field.shape[1]): x_voxel = (x_field + array_offset[0]) * voxel_size z_voxel = (y_field + array_offset[2]) * voxel_size # acts as "Z" coordinate point = np.array([[x_voxel, z_voxel, w_voxel]], dtype=np.float32).T trans = np.dot(twist_matrix_homo_inv, point) sdf_gradient_second_term = np.array([[1, 0, trans[1]], [0, 1, -trans[0]]]) gradient_field[y_field, x_field] = np.dot( np.array([ sdf_gradient_first_term[1][y_field, x_field], sdf_gradient_first_term[0][y_field, x_field] ]), sdf_gradient_second_term) gradient_field /= voxel_size return gradient_field
def test_matrix_a01(self): live_field = np.array([[1, 0, -1], [1, 0, -1], [1, 0, -1]]) twist_vector = np.array([[0.], [0.], [0.]]) sdf_gradient_first_term = np.gradient(live_field) twist_matrix_homo = twist_vector_to_matrix2d(twist_vector) sdf_gradient = np.zeros((live_field.shape[0], live_field.shape[1], 3)) for i in range(live_field.shape[0]): for j in range(live_field.shape[1]): trans = np.dot(np.linalg.inv(twist_matrix_homo), np.array([[i], [j], [1]])) sdf_gradient_second_term = np.array([[1, 0, -trans[1]], [0, 1, trans[0]]]) sdf_gradient[i, j] = np.dot( np.array([ sdf_gradient_first_term[0][i, j], sdf_gradient_first_term[1][i, j] ]), sdf_gradient_second_term) # print(sdf_gradient) expected_matrix_a = np.array([[0, 0, 0], [0, 9, 9], [0, 9, 15]]) matrix_a = np.zeros((3, 3)) # print(matrix_a) for x in range(live_field.shape[0]): for z in range(live_field.shape[1]): # print(sdf_gradient[x, z]) # print(np.dot(sdf_gradient[x, z], sdf_gradient[x, z])) matrix_a += np.dot(sdf_gradient[x, z][:, None], sdf_gradient[x, z][None, :]) self.assertTrue(np.allclose(expected_matrix_a, matrix_a))
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 test_twist_vector_to_matrix2d04(self): vector = np.array([[0], [0], [2 * math.pi]]) expected_matrix = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) matrix = twist_vector_to_matrix2d(vector) self.assertTrue(np.allclose(expected_matrix, matrix))