def register_one_rgbd_pair(s, t, color_files, depth_files, intrinsic,
with_opencv, config):
source_rgbd_image = read_rgbd_image(color_files[s], depth_files[s], True,
config)
target_rgbd_image = read_rgbd_image(color_files[t], depth_files[t], True,
config)
option = o3d.odometry.OdometryOption()
option.max_depth_diff = config["max_depth_diff"]
if abs(s - t) is not 1:
if with_opencv:
success_5pt, odo_init = pose_estimation(source_rgbd_image,
target_rgbd_image,
intrinsic, False)
if success_5pt:
[success, trans, info] = o3d.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
return [success, trans, info]
return [False, np.identity(4), np.identity(6)]
else:
odo_init = np.identity(4)
[success, trans, info] = o3d.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
return [success, trans, info]
def pairRegistration(self, pcNumA, pcNumB):
pcSource = self.pcDataset[pcNumA]
pcTarget = self.pcDataset[pcNumB]
if abs(pcNumA - pcNumB) > 1:
success, odometryInit = ope.pose_estimation(
self.rgbdDataset[pcNumA], self.rgbdDataset[pcNumB],
self.intrinsics, False)
if success:
[success, trans, info] = compute_rgbd_odometry(
self.rgbdDataset[pcNumA], self.rgbdDataset[pcNumB],
self.intrinsics, odometryInit,
RGBDOdometryJacobianFromHybridTerm(), self.odometryOption)
return [success, trans, info]
else:
return [False, np.identity(4), np.identity(6)]
else:
[success, trans, info] = compute_rgbd_odometry(
self.rgbdDataset[pcNumA], self.rgbdDataset[pcNumB],
self.intrinsics, np.identity(4),
RGBDOdometryJacobianFromHybridTerm(), self.odometryOption)
return [success, trans, info]
def register_one_rgbd_pair(s, t, color_files, depth_files, intrinsic,
with_opencv):
# read images
color_s = read_image(color_files[s])
depth_s = read_image(depth_files[s])
color_t = read_image(color_files[t])
depth_t = read_image(depth_files[t])
source_rgbd_image = create_rgbd_image_from_color_and_depth(
color_s, depth_s, depth_trunc=3.0, convert_rgb_to_intensity=True)
target_rgbd_image = create_rgbd_image_from_color_and_depth(
color_t, depth_t, depth_trunc=3.0, convert_rgb_to_intensity=True)
if abs(s - t) is not 1:
if with_opencv:
success_5pt, odo_init = pose_estimation(source_rgbd_image,
target_rgbd_image,
intrinsic, False)
if success_5pt:
[success, trans, info] = compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
RGBDOdometryJacobianFromHybridTerm(), OdometryOption())
return [success, trans, info]
return [False, np.identity(4), np.identity(6)]
else:
odo_init = np.identity(4)
[success, trans,
info] = compute_rgbd_odometry(source_rgbd_image, target_rgbd_image,
intrinsic, odo_init,
RGBDOdometryJacobianFromHybridTerm(),
OdometryOption())
return [success, trans, info]
def register_one_rgbd_pair(s, t, color_files, depth_files, intrinsic,
with_opencv):
# read images
color_s = read_image(color_files[s])
depth_s = read_image(depth_files[s])
color_t = read_image(color_files[t])
depth_t = read_image(depth_files[t])
source_rgbd_image = create_rgbd_image_from_color_and_depth(
color_s, depth_s)
target_rgbd_image = create_rgbd_image_from_color_and_depth(
color_t, depth_t)
# initialize_camera_pose
if abs(s - t) is not 1 and with_opencv:
success_5pt, odo_init = pose_estimation(source_rgbd_image,
target_rgbd_image, intrinsic,
False)
else:
odo_init = np.identity(4)
# perform RGB-D odometry
[success, trans,
info] = compute_rgbd_odometry(source_rgbd_image, target_rgbd_image,
intrinsic, odo_init,
RGBDOdometryJacobianFromHybridTerm(),
OdometryOption())
source = create_point_cloud_from_rgbd_image(source_rgbd_image, intrinsic)
target = create_point_cloud_from_rgbd_image(target_rgbd_image, intrinsic)
return [source, target, trans, info]
def register_one_rgbd_pair(s, t, color_files, depth_files,
intrinsic, with_opencv):
# read images
color_s = read_image(color_files[s])
depth_s = read_image(depth_files[s])
color_t = read_image(color_files[t])
depth_t = read_image(depth_files[t])
source_rgbd_image = create_rgbd_image_from_color_and_depth(color_s, depth_s,
depth_trunc = 3.0, convert_rgb_to_intensity = True)
target_rgbd_image = create_rgbd_image_from_color_and_depth(color_t, depth_t,
depth_trunc = 3.0, convert_rgb_to_intensity = True)
if abs(s-t) is not 1:
if with_opencv:
success_5pt, odo_init = pose_estimation(
source_rgbd_image, target_rgbd_image, intrinsic, False)
if success_5pt:
[success, trans, info] = compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic,
odo_init, RGBDOdometryJacobianFromHybridTerm(),
OdometryOption())
return [success, trans, info]
return [False, np.identity(4), np.identity(6)]
else:
odo_init = np.identity(4)
[success, trans, info] = compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
RGBDOdometryJacobianFromHybridTerm(), OdometryOption())
return [success, trans, info]
def register_rgbd_pairs(src_idx, dst_idx, color_files, depth_files, intrinsic):
src_img_col = Image(color_files[src_idx])
src_img_dep = Image(depth_files[src_idx])
dst_img_col = Image(color_files[dst_idx])
dst_img_dep = Image(depth_files[dst_idx])
src_img = create_rgbd_image_from_color_and_depth(src_img_col, src_img_dep, depth_trunc=0.05)
dst_img = create_rgbd_image_from_color_and_depth(dst_img_col, dst_img_dep, depth_trunc=0.05)
option = OdometryOption()
option.max_depth_diff = 0.3
# option.max_depth = 10
# option.min_depth = 0.3
if abs(src_idx-dst_idx) is not 1:
success_5pt, odo_init = pose_estimation(src_img, dst_img, intrinsic, False)
if success_5pt:
[success, Rt, info] = compute_rgbd_odometry(
src_img, dst_img, intrinsic, odo_init, RGBDOdometryJacobianFromHybridTerm(), option
)
return [success, Rt, info]
return [False, np.identity(4), np.identity(6)]
else:
odo_init = np.identity(4)
[success, Rt, info] = compute_rgbd_odometry(
src_img, dst_img, intrinsic, odo_init, RGBDOdometryJacobianFromHybridTerm(), option
)
return [success, Rt, info]
def register_one_rgbd_pair(s, t, color_files, depth_files,
intrinsic, with_opencv):
# read images
color_s = read_image(color_files[s])
depth_s = read_image(depth_files[s])
color_t = read_image(color_files[t])
depth_t = read_image(depth_files[t])
source_rgbd_image = create_rgbd_image_from_color_and_depth(color_s, depth_s)
target_rgbd_image = create_rgbd_image_from_color_and_depth(color_t, depth_t)
# initialize_camera_pose
if abs(s-t) is not 1 and with_opencv:
success_5pt, odo_init = pose_estimation(
source_rgbd_image, target_rgbd_image, intrinsic, False)
else:
odo_init = np.identity(4)
# perform RGB-D odometry
[success, trans, info] = compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic,
odo_init, RGBDOdometryJacobianFromHybridTerm(), OdometryOption())
source = create_point_cloud_from_rgbd_image(source_rgbd_image, intrinsic)
target = create_point_cloud_from_rgbd_image(target_rgbd_image, intrinsic)
return [source, target, trans, info]
def compute_odometry(sid, tid, source, target, intrinsic):
odo_option = open3d.odometry.OdometryOption()
odo_option.min_depth = 0.5
odo_option.max_depth = 4.0
odo_option.iteration_number_per_pyramid_level = open3d.utility.IntVector([15, 7, 4])
if tid == sid + 1:
print(sid, tid)
[success_hybrid_term, odometry_estimate, info] = open3d.odometry.compute_rgbd_odometry(source, target, intrinsic, np.identity(4), open3d.odometry.RGBDOdometryJacobianFromHybridTerm(), odo_option)
return success_hybrid_term, odometry_estimate, info
else:
success, odo_init = pose_estimation(frames_rgbd_colour[s - fragment[0]], frames_rgbd_colour[t - fragment[0]], kinect_intrinsic, False)
if not success:
return False, np.identity(4), np.identity(6)
print(sid, tid)
[success_hybrid_term, odometry_estimate, info] = open3d.odometry.compute_rgbd_odometry(source, target, intrinsic, odo_init, open3d.odometry.RGBDOdometryJacobianFromHybridTerm(), odo_option)
return success_hybrid_term, odometry_estimate, info
