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))
