def run(config):
print("refine rough registration of fragments.")
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
ply_file_names = get_file_list(
join(config["path_dataset"], config["folder_fragment"]), ".ply")
make_posegraph_for_refined_scene(ply_file_names, config)
optimize_posegraph_for_refined_scene(config["path_dataset"], config)
path_dataset = config['path_dataset']
n_fragments = len(ply_file_names)
# Save to trajectory
poses = []
pose_graph_fragment = o3d.io.read_pose_graph(
join(path_dataset, config["template_refined_posegraph_optimized"]))
for fragment_id in range(len(pose_graph_fragment.nodes)):
pose_graph_rgbd = o3d.io.read_pose_graph(
join(path_dataset,
config["template_fragment_posegraph_optimized"] % fragment_id))
for frame_id in range(len(pose_graph_rgbd.nodes)):
frame_id_abs = fragment_id * \
config['n_frames_per_fragment'] + frame_id
pose = np.dot(pose_graph_fragment.nodes[fragment_id].pose,
pose_graph_rgbd.nodes[frame_id].pose)
poses.append(pose)
traj_name = join(path_dataset, config["template_global_traj"])
write_poses_to_log(traj_name, poses)
def optimize_posegraph_for_refined_scene(path_dataset, config):
pose_graph_name = join(path_dataset, config["template_refined_posegraph"])
pose_graph_optimized_name = join(
path_dataset, config["template_refined_posegraph_optimized"])
run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
max_correspondence_distance = config["voxel_size"] * 1.4,
preference_loop_closure = \
config["preference_loop_closure_registration"])
def run(config):
print("register fragments.")
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
ply_file_names = get_file_list(
join(config["path_dataset"], config["folder_fragment"]), ".ply")
make_clean_folder(join(config["path_dataset"], config["folder_scene"]))
make_posegraph_for_scene(ply_file_names, config)
optimize_posegraph_for_scene(config["path_dataset"], config)
def leave_note():
msg = nlg.gen_message()
name = None
while name is None or os.path.exists(file.join(name)):
name = nlg.gen_message_name()
with open(file.join(name + ".txt"), 'w+') as note:
note.write(msg)
print('Check your mail! I left you a note!')
def optimize_posegraph_for_fragment(path_dataset, fragment_id, config):
pose_graph_name = join(path_dataset,
config["template_fragment_posegraph"] % fragment_id)
pose_graph_optimized_name = join(
path_dataset,
config["template_fragment_posegraph_optimized"] % fragment_id)
run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
max_correspondence_distance = config["max_depth_diff"],
preference_loop_closure = \
config["preference_loop_closure_odometry"])
def make_pointcloud_for_fragment(path_dataset, color_files, depth_files,
fragment_id, n_fragments, intrinsic, config):
mesh = integrate_rgb_frames_for_fragment(
color_files, depth_files, fragment_id, n_fragments,
join(path_dataset,
config["template_fragment_posegraph_optimized"] % fragment_id),
intrinsic, config)
pcd = o3d.geometry.PointCloud()
pcd.points = mesh.vertices
pcd.colors = mesh.vertex_colors
pcd_name = join(path_dataset,
config["template_fragment_pointcloud"] % fragment_id)
o3d.io.write_point_cloud(pcd_name, pcd, False, True)
def make_posegraph_for_refined_scene(ply_file_names, config):
pose_graph = o3d.io.read_pose_graph(
join(config["path_dataset"],
config["template_global_posegraph_optimized"]))
n_files = len(ply_file_names)
matching_results = {}
for edge in pose_graph.edges:
s = edge.source_node_id
t = edge.target_node_id
transformation_init = edge.transformation
matching_results[s * n_files + t] = \
matching_result(s, t, transformation_init)
if config["python_multi_threading"] == True:
from joblib import Parallel, delayed
import multiprocessing
import subprocess
MAX_THREAD = min(multiprocessing.cpu_count(),
max(len(pose_graph.edges), 1))
results = Parallel(n_jobs=MAX_THREAD)(
delayed(register_point_cloud_pair)(
ply_file_names, matching_results[r].s, matching_results[r].t,
matching_results[r].transformation, config)
for r in matching_results)
for i, r in enumerate(matching_results):
matching_results[r].transformation = results[i][0]
matching_results[r].information = results[i][1]
else:
for r in matching_results:
(matching_results[r].transformation,
matching_results[r].information) = \
register_point_cloud_pair(ply_file_names,
matching_results[r].s, matching_results[r].t,
matching_results[r].transformation, config)
pose_graph_new = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph_new.nodes.append(
o3d.pipelines.registration.PoseGraphNode(odometry))
for r in matching_results:
(odometry, pose_graph_new) = update_posegraph_for_scene(
matching_results[r].s, matching_results[r].t,
matching_results[r].transformation, matching_results[r].information,
odometry, pose_graph_new)
print(pose_graph_new)
o3d.io.write_pose_graph(
join(config["path_dataset"], config["template_refined_posegraph"]),
pose_graph_new)
def compute_initial_registration(s, t, source_down, target_down, source_fpfh,
target_fpfh, path_dataset, config):
if t == s + 1: # odometry case
print("Using RGBD odometry")
pose_graph_frag = o3d.io.read_pose_graph(
join(path_dataset,
config["template_fragment_posegraph_optimized"] % s))
n_nodes = len(pose_graph_frag.nodes)
transformation_init = np.linalg.inv(pose_graph_frag.nodes[n_nodes -
1].pose)
(transformation, information) = \
multiscale_icp(source_down, target_down,
[config["voxel_size"]], [50], config, transformation_init)
else: # loop closure case
(success, transformation,
information) = register_point_cloud_fpfh(source_down, target_down,
source_fpfh, target_fpfh,
config)
if not success:
print("No reasonable solution. Skip this pair")
return (False, np.identity(4), np.zeros((6, 6)))
print(transformation)
if config["debug_mode"]:
draw_registration_result(source_down, target_down, transformation)
return (True, transformation, information)
def copy():
"""Clone this file... lol"""
# use `file.join` to make paths relative to `file.home`
me = open(file.join(constants.MAIN_FILE_NAME), 'r')
copy_indexes = [
file.get_file_index(e) for e in os.listdir(file.home)
if os.path.isfile(file.join(e))
]
last_copy_index = max(copy_indexes)
# choose 'thank-you_0.exe' or 'thank-you_0.py'
copy_name = '%s_%d.%s' % (constants.MAIN_FILE_BASE, last_copy_index + 1,
constants.MAIN_FILE_EXT)
new_me = open(file.join(copy_name), 'w')
new_me.write(me.read())
me.close()
new_me.close()
def run(config):
clog.debug("making fragments ...")
make_clean_folder(join(config["path_dataset"], config["folder_fragment"]))
[color_files, depth_files] = get_rgbd_file_lists(config["path_dataset"])
n_files = len(color_files)
n_fragments = int(math.ceil(float(n_files) / \
config['n_frames_per_fragment']))
for fragment_id in range(n_fragments):
process_single_fragment(fragment_id, color_files, depth_files, n_files,
n_fragments, config)
def clean_directory():
"""Revert to original state"""
c = 0
for e in os.listdir(file.home):
if re.fullmatch(r'(%s_\d+\.py)|(.+\.txt)' % constants.MAIN_FILE_BASE,
e) and e != constants.MAIN_FILE_NAME:
os.remove(file.join(e)) # make `e` relative to home
c += 1
return c
def scalable_integrate_rgb_frames(path_dataset, intrinsic, config):
[color_files, depth_files] = get_rgbd_file_lists(path_dataset)
n_files = len(color_files)
n_fragments = int(math.ceil(float(n_files) / \
config['n_frames_per_fragment']))
volume = o3d.integration.ScalableTSDFVolume(
voxel_length=config["tsdf_cubic_size"] / 512.0,
sdf_trunc=0.04,
color_type=o3d.integration.TSDFVolumeColorType.RGB8)
pose_graph_fragment = o3d.io.read_pose_graph(
join(path_dataset, config["template_refined_posegraph_optimized"]))
for fragment_id in range(len(pose_graph_fragment.nodes)):
pose_graph_rgbd = o3d.io.read_pose_graph(
join(path_dataset,
config["template_fragment_posegraph_optimized"] %
fragment_id))
for frame_id in range(len(pose_graph_rgbd.nodes)):
frame_id_abs = fragment_id * \
config['n_frames_per_fragment'] + frame_id
print(
"Fragment %03d / %03d :: integrate rgbd frame %d (%d of %d)." %
(fragment_id, n_fragments - 1, frame_id_abs, frame_id + 1,
len(pose_graph_rgbd.nodes)))
rgbd = read_rgbd_image(color_files[frame_id_abs],
depth_files[frame_id_abs], False, config)
pose = np.dot(pose_graph_fragment.nodes[fragment_id].pose,
pose_graph_rgbd.nodes[frame_id].pose)
volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))
mesh = volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
if config["debug_mode"]:
o3d.visualization.draw_geometries([mesh])
mesh_name = join(path_dataset, config["template_global_mesh"])
o3d.io.write_triangle_mesh(mesh_name, mesh, False, True)
def make_posegraph_for_fragment(path_dataset, sid, eid, color_files,
depth_files, fragment_id, n_fragments,
intrinsic, with_opencv, config, pose_data):
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
pose_graph = o3d.registration.PoseGraph()
trans_odometry = np.identity(4)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(trans_odometry))
for s in range(sid, eid):
for t in range(s + 1, eid):
# odometry
if t == s + 1:
print(
"Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
% (fragment_id, n_fragments - 1, s, t))
[success, trans,
info] = register_one_rgbd_pair(s, t, color_files, depth_files,
intrinsic, with_opencv, config,
pose_data)
trans_odometry = np.dot(trans, trans_odometry)
trans_odometry_inv = np.linalg.inv(trans_odometry)
pose_graph.nodes.append(
o3d.registration.PoseGraphNode(trans_odometry_inv))
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s - sid,
t - sid,
trans,
info,
uncertain=False))
# keyframe loop closure
if s % config['n_keyframes_per_n_frame'] == 0 \
and t % config['n_keyframes_per_n_frame'] == 0:
print(
"Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
% (fragment_id, n_fragments - 1, s, t))
[success, trans,
info] = register_one_rgbd_pair(s, t, color_files, depth_files,
intrinsic, with_opencv, config,
pose_data)
if success:
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s - sid,
t - sid,
trans,
info,
uncertain=True))
o3d.io.write_pose_graph(
join(path_dataset,
config["template_fragment_posegraph"] % fragment_id), pose_graph)
def run(config):
print("making fragments from RGBD sequence.")
make_clean_folder(join(config["path_dataset"], config["folder_fragment"]))
[color_files, depth_files] = get_rgbd_file_lists(config["path_dataset"])
n_files = len(color_files)
n_fragments = int(math.ceil(float(n_files) / \
config['n_frames_per_fragment']))
pose_data = pd.read_csv(join(config["path_dataset"], "camera_pose.csv"))
if config["python_multi_threading"]:
from joblib import Parallel, delayed
import multiprocessing
import subprocess
MAX_THREAD = min(multiprocessing.cpu_count(), n_fragments)
Parallel(n_jobs=MAX_THREAD)(delayed(process_single_fragment)
(fragment_id, color_files, depth_files,
n_files, n_fragments, config, pose_data)
for fragment_id in range(n_fragments))
else:
for fragment_id in range(n_fragments):
process_single_fragment(fragment_id, color_files, depth_files,
n_files, n_fragments, config, pose_data)
def get_str(location, name, default_value=''):
path = file.join(location, name)
try:
with open(path, 'r') as f:
return f.read()
except (FileNotFoundError, ValueError) as e:
# error-specific messages
if type(e) == FileNotFoundError:
print('Hmm... it looks like someone *deleted a file*! Who would DO THAT??')
elif type(e) == ValueError:
print("Someone changed the data.. it's invalid")
with open(path, 'w+') as f:
f.write(default_value)
return default_value
def make_posegraph_for_scene(ply_file_names, config):
pose_graph = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
n_files = len(ply_file_names)
matching_results = {}
for s in range(n_files):
for t in range(s + 1, n_files):
matching_results[s * n_files + t] = matching_result(s, t)
if config["python_multi_threading"].lower() == "true":
from joblib import Parallel, delayed
import multiprocessing
import subprocess
MAX_THREAD = min(multiprocessing.cpu_count(),
max(len(matching_results), 1))
results = Parallel(n_jobs=MAX_THREAD)(delayed(
register_point_cloud_pair)(ply_file_names, matching_results[r].s,
matching_results[r].t, config)
for r in matching_results)
for i, r in enumerate(matching_results):
matching_results[r].success = results[i][0]
matching_results[r].transformation = results[i][1]
matching_results[r].information = results[i][2]
else:
for r in matching_results:
(matching_results[r].success, matching_results[r].transformation,
matching_results[r].information) = \
register_point_cloud_pair(ply_file_names,
matching_results[r].s, matching_results[r].t, config)
for r in matching_results:
if matching_results[r].success:
(odometry, pose_graph) = update_posegraph_for_scene(
matching_results[r].s, matching_results[r].t,
matching_results[r].transformation,
matching_results[r].information, odometry, pose_graph)
o3d.io.write_pose_graph(
join(config["path_dataset"], config["template_global_posegraph"]),
pose_graph)
def get_bool(location, name, default_value=False):
path = file.join(location, name)
try:
with open(path, 'r') as f:
s = f.read().strip().lower()
if s == 'true':
return True
elif s == 'false':
return False
else:
raise ValueError('Not a boolean value (with manual conversion)')
except (FileNotFoundError, ValueError) as e:
# error-specific messages
if type(e) == FileNotFoundError:
print('Hmm... it looks like someone *deleted a file*! Who would DO THAT??')
elif type(e) == ValueError:
print("Someone changed the data.. it's invalid")
with open(path, 'w+') as f:
f.write(str(default_value))
return default_value
def run(config):
print("slac non-rigid optimization.")
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
# dataset path and slac subfolder path
# slac default subfolder for 0.050 voxel size: `dataset/slac/0.050/`.
path_dataset = config["path_dataset"]
slac_folder = join(path_dataset, config["subfolder_slac"])
# Read RGBD images.
[color_files, depth_files] = get_rgbd_file_lists(config["path_dataset"])
if len(color_files) != len(depth_files):
raise ValueError(
"The number of color images {} must equal to the number of depth images {}."
.format(len(color_files), len(depth_files)))
# Read optimized pose graph. [Generated by `register` stage].
posegraph = o3d.io.read_pose_graph(
join(slac_folder, config["template_optimized_posegraph_slac"]))
# If camera intrinsic is not provided,
# the default PrimeSense intrinsic is used.
if config["path_intrinsic"]:
intrinsic = o3d.io.read_pinhole_camera_intrinsic(
config["path_intrinsic"])
else:
intrinsic = o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
focal_length = intrinsic.get_focal_length()
principal_point = intrinsic.get_principal_point()
intrinsic_t = o3d.core.Tensor([[focal_length[0], 0, principal_point[0]],
[0, focal_length[1], principal_point[1]],
[0, 0, 1]])
device = o3d.core.Device(
'CUDA:0' if o3d.core.cuda.is_available() else 'CPU:0')
voxel_grid = o3d.t.geometry.TSDFVoxelGrid(
{
"tsdf": o3d.core.Dtype.Float32,
"weight": o3d.core.Dtype.UInt16,
"color": o3d.core.Dtype.UInt16
}, config["tsdf_cubic_size"] / 512.0, config["sdf_trunc"], 16,
config["block_count"], device)
# Load control grid.
ctr_grid_keys = o3d.core.Tensor.load(slac_folder + "ctr_grid_keys.npy")
ctr_grid_values = o3d.core.Tensor.load(slac_folder + "ctr_grid_values.npy")
ctr_grid = o3d.t.pipelines.slac.control_grid(3.0 / 8,
ctr_grid_keys.to(device),
ctr_grid_values.to(device),
device)
fragment_folder = join(path_dataset, config["folder_fragment"])
k = 0
for i in range(len(posegraph.nodes)):
fragment_pose_graph = o3d.io.read_pose_graph(
join(fragment_folder, "fragment_optimized_%03d.json" % i))
for node in fragment_pose_graph.nodes:
pose_local = node.pose
extrinsic_local_t = o3d.core.Tensor(np.linalg.inv(pose_local))
pose = np.dot(posegraph.nodes[i].pose, node.pose)
extrinsic_t = o3d.core.Tensor(np.linalg.inv(pose))
depth = o3d.t.io.read_image(depth_files[k]).to(device)
color = o3d.t.io.read_image(color_files[k]).to(device)
rgbd = o3d.t.geometry.RGBDImage(color, depth)
print('Integrating Frame {:3d}'.format(k))
rgbd_projected = ctr_grid.deform(rgbd, intrinsic_t,
extrinsic_local_t,
config["depth_scale"],
config["max_depth"])
voxel_grid.integrate(rgbd_projected.depth, rgbd_projected.color,
intrinsic_t, extrinsic_t,
config["depth_scale"], config["max_depth"])
k = k + 1
if (config["save_output_as"] == "pointcloud"):
pcd = voxel_grid.extract_surface_points().to(o3d.core.Device("CPU:0"))
save_pcd_path = join(slac_folder, "output_slac_pointcloud.ply")
o3d.t.io.write_point_cloud(save_pcd_path, pcd)
else:
mesh = voxel_grid.extract_surface_mesh().to(o3d.core.Device("CPU:0"))
mesh_legacy = mesh.to_legacy()
save_mesh_path = join(slac_folder, "output_slac_mesh.ply")
o3d.io.write_triangle_mesh(save_mesh_path, mesh_legacy)
def run(config):
print("slac non-rigid optimisation.")
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
path_dataset = config['path_dataset']
ply_file_names = get_file_list(
join(config["path_dataset"], config["folder_fragment"]), ".ply")
if (len(ply_file_names) == 0):
raise RuntimeError(
"No fragment found in {}, please make sure the reconstruction_system has finished running on the dataset."
.format(join(config["path_dataset"], config["folder_fragment"])))
pose_graph_fragment = o3d.io.read_pose_graph(
join(path_dataset, config["template_refined_posegraph_optimized"]))
# SLAC optimizer parameters.
slac_params = o3d.t.pipelines.slac.slac_optimizer_params(
max_iterations=config["max_iterations"],
voxel_size=config["voxel_size"],
distance_threshold=config["distance_threshold"],
fitness_threshold=config["fitness_threshold"],
regularizer_weight=config["regularizer_weight"],
device=o3d.core.Device(str(config["device"])),
slac_folder=path_dataset + config["folder_slac"])
# SLAC debug option.
debug_option = o3d.t.pipelines.slac.slac_debug_option(False, 0)
# Run the system.
pose_graph_updated = o3d.pipelines.registration.PoseGraph()
# rigid optimization method.
if (config["method"] == "rigid"):
pose_graph_updated = o3d.t.pipelines.slac.run_rigid_optimizer_for_fragments(
ply_file_names, pose_graph_fragment, slac_params, debug_option)
elif (config["method"] == "slac"):
pose_graph_updated, ctrl_grid = o3d.t.pipelines.slac.run_slac_optimizer_for_fragments(
ply_file_names, pose_graph_fragment, slac_params, debug_option)
hashmap = ctrl_grid.get_hashmap()
active_buf_indices = hashmap.get_active_buf_indices().to(
o3d.core.Dtype.Int64)
key_tensor = hashmap.get_key_tensor()[active_buf_indices]
key_tensor.save(
join(slac_params.get_subfolder_name(), "ctr_grid_keys.npy"))
value_tensor = hashmap.get_value_tensor()[active_buf_indices]
value_tensor.save(
join(slac_params.get_subfolder_name(), "ctr_grid_values.npy"))
else:
raise RuntimeError(
"Requested optimization method {}, is not implemented. Implemented methods includes slac and rigid."
.format(config["method"]))
# Write updated pose graph.
o3d.io.write_pose_graph(
join(slac_params.get_subfolder_name(),
config["template_optimized_posegraph_slac"]), pose_graph_updated)
# Write trajectory for slac-integrate stage.
fragment_folder = join(path_dataset, config["folder_fragment"])
params = []
for i in range(len(pose_graph_updated.nodes)):
fragment_pose_graph = o3d.io.read_pose_graph(
join(fragment_folder, "fragment_optimized_%03d.json" % i))
for node in fragment_pose_graph.nodes:
pose = np.dot(pose_graph_updated.nodes[i].pose, node.pose)
param = o3d.camera.PinholeCameraParameters()
param.extrinsic = np.linalg.inv(pose)
params.append(param)
trajectory = o3d.camera.PinholeCameraTrajectory()
trajectory.parameters = params
o3d.io.write_pinhole_camera_trajectory(
slac_params.get_subfolder_name() + "/optimized_trajectory_" +
str(config["method"]) + ".log", trajectory)
