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