Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
2026-09-15 14:13:49 +08:00
commit 7e4ef6f98b
7002 changed files with 1452249 additions and 0 deletions
View File
+137
View File
@@ -0,0 +1,137 @@
import os
import glob
import pandas as pd
from copy import deepcopy
import warnings
def update_args(args):
args = deepcopy(args)
if args.type == "egobody":
if args.root is None:
warnings.warn(
"args.root flag is set to None, using root directory in source"
)
args.root = "/path/to/egobody"
if args.seqs is None:
args.seqs = get_egobody_seqs(args.root, args.split)
args.img_dirs = [
glob.glob(f"{args.root}/egocentric_color/{seq}/**/PV")[0]
for seq in args.seqs
]
return args
if args.type == "3dpw":
if args.root is None:
warnings.warn(
"args.root flag is set to None, using root directory in source"
)
args.root = "/path/to/3DPW"
if args.split == "val":
args.split = "validation"
if args.seqs is None:
args.seqs = get_3dpw_seqs(args.root, args.split)
args.img_name = "imageFiles"
elif args.type == "posetrack":
if args.root is None:
warnings.warn(
"args.root flag is set to None, using root directory in source"
)
args.root = "/path/to/posetrack/posetrack2018/posetrack_data"
args.img_name = f"images/{args.split}"
elif args.type == "davis":
if args.root is None:
warnings.warn(
"args.root flag is set to None, using root directory in source"
)
args.root = "/path/to/DAVIS"
args.img_name = "JPEGImages/Full-Resolution"
args.split = ""
else:
assert args.root is not None
args.img_name = args.img_name if args.img_name is not None else "images"
args.split = ""
args.img_dirs, args.seqs = get_all_img_dirs(
f"{args.root}/{args.img_name}", args.seqs
)
return args
def isimage(f):
ext = os.path.splitext(f)[-1].lower()
return ext in [".png", ".jpg"]
def get_all_img_dirs(data_root, seqs=None):
"""
returns all image directories in root, recursively searching, and the sequence names
"""
seqs = seqs if seqs is not None else []
img_dirs = []
def get_name(p):
return p.removeprefix(f"{data_root}/")
for root, _, files in os.walk(data_root):
num_imgs = len([f for f in files if isimage(f)])
if num_imgs > 0:
name = get_name(root)
if len(seqs) == 0 or name in seqs:
img_dirs.append(root)
img_dirs = sorted(img_dirs)
seqs = [get_name(d) for d in img_dirs]
return img_dirs, seqs
def get_custom_seqs(data_root):
img_dir = f"{data_root}/images"
if not os.path.isdir(img_dir):
return []
return sorted(os.listdir(img_dir))
def get_egobody_seqs(data_root, split):
split_file = f"{data_root}/data_splits.csv"
df = pd.read_csv(split_file)
if split not in df.columns:
print(f"{split} not in {split_file}")
return []
return sorted(df[split].dropna().tolist())
def get_3dpw_seqs(data_root, split):
split_dir = f"{data_root}/sequenceFiles/{split}"
if not os.path.isdir(split_dir):
return []
seq_files = sorted(os.listdir(split_dir))
return [os.path.splitext(f)[0] for f in seq_files]
def get_posetrack_seqs(data_root, split):
split_dir = f"{data_root}/images/{split}"
if not os.path.isdir(split_dir):
return []
return sorted(os.listdir(split_dir))
def get_davis_seqs(data_root):
img_root = f"{data_root}/JPEGImages/Full-Resolution"
if not os.path.isdir(img_root):
return []
return sorted(os.listdir(img_root))
def get_img_dir(data_type, data_root, seq, img_name):
if data_type == "posetrack":
return f"{data_root}/{img_name}/{seq}"
if data_type == "3dpw":
return f"{data_root}/imageFiles/{seq}"
if data_type == "davis":
return f"{data_root}/JPEGImages/Full-Resolution/{seq}"
if data_type == "custom":
return f"{data_root}/images/{seq}" # custom sequence
return None
+124
View File
@@ -0,0 +1,124 @@
import os
import joblib
import json
import cv2
import numpy as np
def export_hamer_predictions(res_path, target_dir):
"""
exports hamer output results into a directory for each track
of JSON files with each frame's mano prediction
"""
os.makedirs(target_dir, exist_ok=True)
print(f"exporting hamer predictions from {res_path} to {target_dir}")
tracklet_data = joblib.load(res_path)
for frame, track_data in tracklet_data.items():
name = os.path.splitext(frame.split("/")[-1])[0]
print(track_data['tid'])
track_dicts = unpack_frame(track_data)
for tid, pred_dict in track_dicts.items():
track_dir = os.path.join(target_dir, f"{tid:03d}")
os.makedirs(track_dir, exist_ok=True)
pred_path = os.path.join(track_dir, f"{name}_mano.json")
with open(pred_path, "w") as f:
json.dump(pred_dict, f, indent=1)
def export_vitpose_keypoints(res_path, target_dir):
"""
exports each track of each frame into its own json of keypoints
"""
os.makedirs(target_dir, exist_ok=True)
print(f"exporting keypoints from {res_path} to {target_dir}")
tracklet_data = joblib.load(res_path)
for frame, track_data in tracklet_data.items():
name = os.path.splitext(frame.split("/")[-1])[0]
tids = track_data["tid"]
valid_tids = track_data["tracked_ids"]
kps_all = track_data["extra_data"]
# mask_paths = track_data["mask_name"]
for i, tid in enumerate(tids):
if tid not in valid_tids:
continue
track_dir = os.path.join(target_dir, f"{tid:03d}")
os.makedirs(track_dir, exist_ok=True)
kp_path = os.path.join(track_dir, f"{name}_keypoints.json")
kp_dict = {
"people": [
{
"pose_keypoints_2d": kps_all[i],#.tolist(),
# "mask_path": mask_paths[i],
}
],
}
with open(kp_path, "w") as f:
json.dump(kp_dict, f, indent=1)
def export_shot_changes(res_path, target_path):
"""
Exports the phalp output of shot changes into a JSON file
dict with the shot index for each frame in the sequence
"""
os.makedirs(os.path.dirname(target_path), exist_ok=True)
print(f"exporting shot changes to {target_path}")
tracklet_data = joblib.load(res_path)
frames = sorted(tracklet_data.keys())
# print(frames)
# print(tracklet_data[frames[0]].keys())
# for frame in frames:
# print(frame, tracklet_data[frame]["shot"])
shots = np.cumsum([tracklet_data[frame]["shot"] for frame in frames])
shots = shots.astype(int).tolist()
shot_dict = {frame.split("/")[-1]: shot for frame, shot in zip(frames, shots)}
with open(target_path, "w") as f:
json.dump(shot_dict, f, indent=1)
def unpack_frame(track_data):
"""
Unpack one frame of track data from the phalp output into JSON files for each track.
Track data contains body pose, global translation and rotation,
and keypoints for all tracks in the frame.
Each track is unpacked into folders with a JSON for each frame.
"""
out_dict = {}
for i, tid in enumerate(track_data["tid"]):
if tid not in track_data["tracked_ids"]:
continue
cam_trans = track_data["cam_trans"][i].squeeze() # (3,)
mano = track_data["mano"][i]
betas = mano["betas"].squeeze() # (10,)
hand_pose = mano["hand_pose"].squeeze() # (23, 3, 3)
is_right = mano["is_right"]
print(is_right, tid, track_data["tid"], track_data["tracked_ids"])
assert is_right == tid, f'{is_right}, {tid}'
hand_pose_aa = np.stack(
[cv2.Rodrigues(x)[0].squeeze() for x in hand_pose], axis=0
) # (23, 3)
global_orient = mano["global_orient"].squeeze() # (3, 3)
global_orient_aa = cv2.Rodrigues(global_orient)[0].squeeze() # (3,)
out_dict[tid] = {
"betas": betas.tolist(),
"body_pose": hand_pose_aa.tolist(),
"global_orient": global_orient_aa.tolist(),
"cam_trans": cam_trans.tolist(),
"is_right": is_right
}
return out_dict
def export_sequence_results(hamer_res_path, track_dir, shot_path):
print('exporting sequence results in export_hamer.py...')
export_hamer_predictions(hamer_res_path, track_dir)
export_vitpose_keypoints(hamer_res_path, track_dir)
export_shot_changes(hamer_res_path, shot_path)
+206
View File
@@ -0,0 +1,206 @@
import argparse
import imageio
import os
import subprocess
def split_frame(videopath,
out_dir,
fps=30,
ext="jpg",
down_scale=1,
start_sec=0,
end_sec=-1,
overwrite=False,
**kwargs,):
import cv2
base_name = videopath.replace('.mp4', '').replace('.MP4', '').replace('.avi', '')
if os.path.isfile(videopath):
print(f'processing {videopath} .....')
# filepath = os.path.dirname(videopath)
# videoname = os.path.basename(videopath)[:-4]
if not os.path.exists(videopath):
raise('{} not exists!'.format(videopath))
vc = cv2.VideoCapture(videopath)
path = out_dir
if not os.path.exists(path):
os.makedirs(path)
if vc.isOpened():
rval , frame = vc.read()
if '.MOV' not in videopath:
cv2.imencode('.jpg',frame)[1].tofile(path +'/' + str(1).zfill(6) + '.jpg')
else:
cv2.imencode('.jpg',frame[::-1, ::-1])[1].tofile(path +'/' + str(0).zfill(6) + '.jpg')
else:
rval = False
Frame = 2
timeF = 1
while rval:
rval,frame = vc.read()
if rval==False:
break
if '.MOV' not in videopath:
cv2.imencode('.jpg',frame)[1].tofile(path +'/' + str(Frame).zfill(6) + '.jpg')
else:
cv2.imencode('.jpg',frame[::-1, ::-1])[1].tofile(path +'/' + str(Frame).zfill(6) + '.jpg')
Frame += 1
elif os.path.isdir(videopath):
print()
print('moving ', base_name , 'to ', out_dir)
os.system(f"cp -r {videopath} {out_dir}")
raise ValueError
return 0
else:
print(os.path.abspath(videopath), os.path.exists(videopath))
raise ValueError(f"{videopath} does not exist or is not a valid file or directory.")
return 0
def video_to_frames(
path,
out_dir,
fps=30,
ext="jpg",
down_scale=1,
start_sec=0,
end_sec=-1,
overwrite=False,
**kwargs,
):
"""
:param path
:param out_dir
:param fps
:param down_scale (optional int)
"""
path = str(path)
base_name = path.replace('.mp4', '').replace('.MP4', '').replace('.avi', '')
print(path, base_name)
print(os.path.isfile(path), os.path.isdir(base_name))
if os.path.isfile(path):
os.makedirs(out_dir, exist_ok=True)
arg_str = f"-copyts -qscale:v 2 -vf fps={fps}"
if down_scale != 1:
arg_str = f"{arg_str},scale='iw/{down_scale}:ih/{down_scale}'"
if start_sec > 0:
arg_str = f"{arg_str} -ss {start_sec}"
if end_sec > start_sec:
arg_str = f"{arg_str} -to {end_sec}"
yn = "-y" if overwrite else "-n"
cmd = f"ffmpeg -i {path} {arg_str} {out_dir}/%06d.{ext} {yn}"
print(cmd)
elif os.path.isdir(base_name):
os.system(f"cp -r {base_name} {out_dir}")
return 0
else:
print(path, os.path.exists(path))
raise ValueError
return subprocess.call(cmd, shell=True, stdin=subprocess.PIPE)
# def video_to_frames(
# path,
# out_dir,
# fps=30,
# ext="jpg",
# down_scale=1,
# start_sec=0,
# end_sec=-1,
# overwrite=False,
# **kwargs,
# ):
# """
# :param path
# :param out_dir
# :param fps
# :param down_scale (optional int)
# """
# path = str(path)
# base_name = path.replace('.mp4', '').replace('.MP4', '').replace('.avi', '')
# print(path, base_name)
# print(os.path.isfile(path), os.path.isdir(base_name))
# if os.path.isfile(path):
# os.makedirs(out_dir, exist_ok=True)
# arg_str = f"-copyts -qscale:v 2 -vf fps={fps}"
# if down_scale != 1:
# arg_str = f"{arg_str},scale='iw/{down_scale}:ih/{down_scale}'"
# if start_sec > 0:
# arg_str = f"{arg_str} -ss {start_sec}"
# if end_sec > start_sec:
# arg_str = f"{arg_str} -to {end_sec}"
# yn = "-y" if overwrite else "-n"
# # Changed %06d to start from 0 instead of 1
# cmd = f"ffmpeg -i {path} {arg_str} -start_number 0 {out_dir}/%06d.{ext} {yn}"
# print(cmd)
# elif os.path.isdir(base_name):
# os.system(f"cp -r {base_name} {out_dir}")
# return 0
# else:
# print(path, os.path.exists(path))
# raise ValueError
# return subprocess.call(cmd, shell=True, stdin=subprocess.PIPE)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--data_root", type=str, required=True, help="path to video")
parser.add_argument(
"--out_root", type=str, required=True, help="output dir for frames"
)
parser.add_argument(
"--seqs",
nargs="*",
default=None,
help="[optional] sequences to run, default runs all available",
)
parser.add_argument("--fps", type=int, default=30, help="fps to extract frames")
parser.add_argument(
"--ext", type=str, default="jpg", help="output filetype for frames"
)
parser.add_argument(
"--down_scale", type=int, default=1, help="scale to extract frames"
)
parser.add_argument(
"-ss", "--start_sec", type=float, default=0, help="seconds to start_sec"
)
parser.add_argument(
"-es", "--end_sec", type=float, default=-1, help="seconds to end_sec"
)
parser.add_argument(
"-y", "--overwrite", action="store_true", help="overwrite if already exist"
)
args = parser.parse_args()
seqs_all = os.listdir(args.data_root)
if args.seqs is None:
args.seqs = seqs_all
for seq in args.seqs:
path = os.path.join(args.data_root, seq)
print(f"EXTRACTING FRAMES FROM {path}")
assert os.path.isfile(path)
seq_name = os.path.splitext(os.path.basename(path.rstrip("/")))[0]
out_dir = os.path.join(args.out_root, seq_name)
video_to_frames(
path,
out_dir,
args.fps,
args.ext,
args.down_scale,
args.start_sec,
args.end_sec,
args.overwrite,
)
+122
View File
@@ -0,0 +1,122 @@
import os
import subprocess
import multiprocessing as mp
from concurrent import futures
from preproc.datasets import update_args
from preproc.export_hamer import export_sequence_results
ROOT_DIR = os.path.abspath(f"{__file__}/../../../")
SRC_DIR = os.path.join(ROOT_DIR, "third-party/hamer")
def launch_hamer(gpus, seq, img_dir, res_dir, name, datatype, overwrite=False):
"""
run hamer using GPU pool
"""
cur_proc = mp.current_process()
print("PROCESS", cur_proc.name, cur_proc._identity)
# 1-indexed processes
# worker_id = cur_proc._identity[0] - 1 if len(cur_proc._identity) > 0 else 0
# gpu = gpus[worker_id % len(gpus)]
gpu = gpus[0]
HAMER_DIR = SRC_DIR
print("HAMER DIR", HAMER_DIR)
cmd_args = [
f"cd {HAMER_DIR};",
f"CUDA_VISIBLE_DEVICES={gpu}",
"python -u run.py",
f"--img_folder {img_dir} ",
f"--res_folder {res_dir}/demo_{name}.pkl ",
f"--batch_size=48 --side_view --save_mesh --full_frame",
f"--type {datatype}",
f"--checkpoint {ROOT_DIR}",
"--render"
]
cmd = " ".join(cmd_args)
print(cmd)
return subprocess.call(cmd, shell=True)
def process_seq(
gpus,
out_root,
seq,
img_dir,
out_name="hamer_out",
datatype=None,
track_name="track_preds",
shot_name="shot_idcs",
overwrite=False,
):
"""
Run and export HAMER results
"""
name = os.path.basename(seq)
res_root = f"{out_root}/{out_name}/{seq}"
os.makedirs(res_root, exist_ok=True)
res_dir = os.path.join(res_root, "results")
res_path = f"{res_root}/{name}.pkl"
if overwrite or not os.path.isfile(res_path):
res = launch_hamer(gpus, seq, img_dir, res_dir, name, datatype, overwrite)
print(f'rename {res_dir}/demo_{name}.pkl into ', res_path)
os.rename(f"{res_dir}/demo_{name}.pkl", res_path)
assert res == 0, "HAMER FAILED"
# export the HAMER predictions
track_dir = f"{out_root}/{track_name}/{seq}"
shot_path = f"{out_root}/{shot_name}/{seq}.json"
export_sequence_results(res_path, track_dir, shot_path)
return 0
def get_out_dir(src_root, src_dir, src_token, out_token):
"""
:param src_root (str) root of all data
:param src_dir (str) img input dir
:param src_token (str) parent name of image input dir
:param out_token (str) name of output dir
"""
src_suffix = src_dir.removeprefix(src_root)
out_dir = f"{out_root}/{src_suffix}"
return out_dir.replace(src_token, out_token)
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--type", default="posetrack", help="dataset to process")
parser.add_argument("--root", default=None, help="root dir of data, default None")
parser.add_argument("--split", default="val", help="split of dataset, default val")
parser.add_argument(
"--img_name", default=None, help="input image directory name, default None"
)
parser.add_argument("--seqs", nargs="*", default=None)
parser.add_argument("--gpus", nargs="*", default=[0])
parser.add_argument("-y", "--overwrite", action="store_true")
args = parser.parse_args()
args = update_args(args)
out_root = f"{args.root}/slahmr/{args.split}"
print(f"running phalp on {len(args.img_dirs)} image directories")
if len(args.gpus) > 1:
with futures.ProcessPoolExecutor(max_workers=len(args.gpus)) as exe:
for img_dir, seq in zip(args.img_dirs, args.seqs):
exe.submit(
process_seq,
args.gpus,
out_root,
seq,
img_dir,
overwrite=args.overwrite,
)
else:
for img_dir, seq in zip(args.img_dirs, args.seqs):
process_seq(args.gpus, out_root, seq, img_dir, overwrite=args.overwrite)
+186
View File
@@ -0,0 +1,186 @@
import os
import glob
import json
import numpy as np
import subprocess
from concurrent import futures
import multiprocessing as mp
from preproc.datasets import update_args
SRC_DIR = os.path.abspath(f"{__file__}/../")
def isimage(path):
ext = os.path.splitext(path)[-1].lower()
return ext == ".png" or ext == ".jpg" or ext == ".jpeg"
def split_frames_equal(img_dir, seq_len=-1):
"""
split the sequence into subsequences of seq_len
returns start and end indices for each subsequence
"""
if seq_len == -1: # don't split
return [(0, -1)]
image_list = sorted(list(filter(isimage, os.listdir(img_dir))))
num_imgs = len(image_list)
splits = list(range(0, num_imgs, seq_len))
splits[-1] = num_imgs # add remainder to last subseq
return list(zip(splits[:-1], splits[1:]))
def split_frames_shots(img_dir, shot_path, pad_shot=0, min_len=0):
"""
split the sequence into subsequences according by shots in the video
returns start and end indices for each subsequence
"""
if shot_path is None or not os.path.isfile(shot_path):
print(f"{shot_path} DOES NOT EXIST, USING WHOLE SEQUENCE")
return [(0, -1)], [0]
image_list = sorted(list(filter(isimage, os.listdir(img_dir))))#[:500]
num_frames = len(image_list)
with open(shot_path, "r") as f:
shot_dict = json.load(f)
# print(shot_dict.keys())
shot_idcs = np.array([shot_dict[name] for name in image_list])
subseqs = []
for i in np.unique(shot_idcs):
idcs = np.where(shot_idcs == i)[0]
start, end = idcs.min(), idcs.max() + 1
if pad_shot > 0:
if start > 0:
start = start + pad_shot
if end < num_frames:
end = end - pad_shot
if (end - start) < min_len: # this shot is too short
continue
subseqs.append((start, end))
return subseqs, np.unique(shot_idcs)
def split_sequence(args, img_dir, seq):
subseqs = split_frames_equal(img_dir, args.seq_len)
return subseqs, [None for _ in subseqs]
def get_out_dir(args, seq, shot_idx=None, start=0, end=-1):
if "egobody" in args.type or "3dpw" in args.type:
if args.use_intrins:
out_name = "cameras_intrins"
else:
out_name = "cameras_default"
else:
out_name = "cameras"
if args.seq_len > 0:
out_name = f"{out_name}_split"
out_dir = f"{args.root}/slahmr/{args.split}/{out_name}/{seq}"
if args.seq_len > 0:
return f"{out_dir}/{start}-{end}"
if args.type == "posetrack" and shot_idx is not None:
return f"{out_dir}/shot-{shot_idx}"
return out_dir
def get_intrins_path(data_type, data_root, seq):
raise NotImplementedError
def get_command(img_dir, out_dir, start=0, end=-1, intrins_path=None, overwrite=False):
cmd_args = [
f"python {SRC_DIR}/run_slam.py",
"-i",
img_dir,
"--map_dir",
out_dir,
"--disable_vis",
]
if start != 0:
cmd_args += ["--start", str(start)]
if end != -1:
cmd_args += ["--end", str(end)]
if intrins_path is not None:
cmd_args += ["--intrins_path", intrins_path]
if overwrite:
cmd_args.append("--overwrite")
cmd = " ".join(cmd_args)
return cmd
def check_intrins(data_type, data_root, intrins_path, img_dir):
assert intrins_path is not None
out_name = intrins_path.split(data_root)[1].strip("/").split("/")[0]
out_root = f"{data_root}/{out_name}"
return intrins_path
def get_slam_command(args, img_dir, seq, shot_idx=None, start=0, end=-1):
out_dir = get_out_dir(args, seq, shot_idx, start, end)
intrins_path = None
if args.use_intrins:
intrins_path = get_intrins_path(args.type, args.root, seq)
if intrins_path is not None:
intrins_path = check_intrins(args.type, args.root, intrins_path, img_dir)
return get_command(
img_dir,
out_dir,
start=start,
end=end,
intrins_path=intrins_path,
overwrite=args.overwrite,
)
def launch_job(gpus, cmd):
cur_proc = mp.current_process()
print(cur_proc.name, cur_proc._identity)
worker_id = cur_proc._identity[0] - 1
gpu = gpus[worker_id % len(gpus)]
cmd = f"CUDA_VISIBLE_DEVICES={gpu} {cmd}"
print(cmd)
subprocess.call(cmd, shell=True)
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--type", default="posetrack", help="dataset to process")
parser.add_argument(
"--root", default=None, help="root of data to process, default None"
)
parser.add_argument(
"--split", default="val", help="split of dataset to process, default val"
)
parser.add_argument("--gpus", nargs="*", default=[0])
parser.add_argument("--seqs", nargs="*", default=None)
parser.add_argument("--seq_len", type=int, default=-1)
parser.add_argument(
"--use_intrins", action="store_true", help="use GT intrinsics if available"
)
parser.add_argument("-y", "--overwrite", action="store_true")
args = parser.parse_args()
args = update_args(args)
print(f"Running SLAM on {len(args.seqs)} sequences")
with futures.ProcessPoolExecutor(max_workers=len(args.gpus)) as ex:
for img_dir, seq in zip(args.img_dirs, args.seqs):
subseqs, shot_idcs = split_sequence(args, img_dir, seq)
for shot, (start, end) in zip(shot_idcs, subseqs):
cmd = get_slam_command(args, img_dir, seq, shot, start, end)
print(cmd)
ex.submit(launch_job, args.gpus, cmd)
+414
View File
@@ -0,0 +1,414 @@
import os
ROOT_DIR = os.path.abspath(f"{__file__}/../../../")
SRC_DIR = os.path.join(ROOT_DIR, "third-party/DROID-SLAM")
print("PROJ SRC", ROOT_DIR)
print("DROID SRC", SRC_DIR)
import sys
sys.path.append(SRC_DIR)
sys.path.append(f"{SRC_DIR}/droid_slam")
import glob
import shutil
import argparse
from tqdm import tqdm
import json
import cv2
import trimesh
import numpy as np
import torch
from lietorch import SE3
from droid import Droid
import droid_backends
def get_image(image):
return image.permute(1, 2, 0).cpu().numpy()[..., ::-1]
def show_image(image):
image = image.permute(1, 2, 0).cpu().numpy()
cv2.imshow("image", image / 255.0)
cv2.waitKey(1)
def get_hwf(test_path):
test_img = cv2.imread(test_path)
H, W, _ = test_img.shape
F = 0.5 * (H + W)
return H, W, F
def isimage(path):
ext = os.path.splitext(path)[-1].lower()
return ext == ".png" or ext == ".jpg" or ext == ".jpeg"
def get_image_files(img_dir, stride, start=0, end=-1):
image_list = sorted(list(filter(isimage, os.listdir(img_dir))))
end = len(image_list) + 1 + end if end < 0 else end
return [os.path.join(img_dir, name) for name in image_list[start:end:stride]]
def load_intrins(intrins_path, image_files, start=0, end=-1):
N = len(image_files)
H, W, F = get_hwf(image_files[0])
# intrinsics for default FOV
default = torch.tensor([F, F, W / 2, H / 2, W, H])[None].repeat(N, 1)
if intrins_path is None:
return default
assert os.path.isfile(intrins_path), f"{intrins_path} DOES NOT EXIST"
assert intrins_path.endswith(".txt"), f"{intrins_path} INCORRECT EXT"
print(f"LOADING INTRINSICS FROM {intrins_path}")
intrins = torch.from_numpy(np.loadtxt(intrins_path).astype(np.float32))
print(intrins.shape, N)
if intrins.ndim < 2: # assume intrins is same for all frames
intrins = intrins[None].repeat(N, 1)
else:
end = N + 1 + end if end < 0 else end
intrins = intrins[start:end]
print(intrins.shape)
if intrins.shape[1] == 4: # fx, fy, cx, cy, adding on W and H
ones = torch.ones_like(intrins[:, :1])
intrins = torch.cat([intrins, ones * W, ones * H], dim=-1)
return intrins
def image_stream(image_files, intrins_all):
"""image generator"""
N = len(image_files)
assert intrins_all.shape[0] == N
assert intrins_all.shape[1] >= 4
for t, imfile in enumerate(image_files):
image = cv2.imread(imfile)
if image is None:
print(imfile, "is none, exiting")
sys.exit(1)
h0, w0, _ = image.shape
h1 = int(h0 * np.sqrt((384 * 512) / (h0 * w0)))
w1 = int(w0 * np.sqrt((384 * 512) / (h0 * w0)))
image = cv2.resize(image, (w1, h1))
image = image[: h1 - h1 % 8, : w1 - w1 % 8]
image = torch.as_tensor(image).permute(2, 0, 1)
fx, fy, cx, cy = intrins_all[t, :4]
sx, sy = w1 / w0, h1 / h0
frame_intrins = torch.as_tensor([fx * sx, fy * sy, cx * sx, cy * sy])
yield t, image[None], frame_intrins
def unpack_video(video, depth_filter_thresh=0.005):
"""
extract the video of keyframe disps and poses
returns:
(T) timestamps
(T, 7) world2cam pose for each keyframe
(T, H, W) disps
(T, H, W, 3) images
(T, H, W) mask of valid pixels
(4) intrinsics (fx, fy, cx, cy)
"""
with torch.no_grad():
with video.get_lock():
# dirty frames have disparity normalized over all previous frames
# should probably change this in visualizer
t = video.counter.value
print(f"{t} keyframes in map")
# normalization happens after all frames have been processed
if t < 1:
print("no keyframes")
return
# taken from visualization.py#80
tstamps = video.tstamp[:t]
dirty_index = torch.arange(t, device=video.poses.device)
poses = torch.index_select(video.poses, 0, dirty_index) # (T, 7)
disps = torch.index_select(video.disps, 0, dirty_index) # (T, H, W)
images = torch.index_select(video.images, 0, dirty_index) # (T, 3, H, W)
images = images[:, [2, 1, 0], 3::8, 3::8].permute(0, 2, 3, 1) / 255.0
intrins = video.intrinsics[0] # (4,)
# get mask of static points
thresh = depth_filter_thresh * torch.ones_like(disps.mean(dim=[1, 2]))
count = droid_backends.depth_filter(
video.poses, video.disps, intrins, dirty_index, thresh
)
disp_mean = disps.mean(dim=(1, 2), keepdim=True).clamp_min(0.2)
valid = (count >= 2) & (disps > 0.5 * disp_mean) # (T, H, W)
return {
"tstamps": tstamps.cpu(),
"w2c": poses.cpu(),
"disps": disps.cpu(),
"images": images.cpu(),
"valid": valid.cpu(),
"intrins": intrins.cpu(),
}
def get_keyframe_map(video_dict):
"""
:param video_dict (dict) unpacked droid slam video
:returns
(T, 4, 4) camera to world matrix
(N, 3) valid points in world
(N, 3) colors of valid points
"""
poses = video_dict["w2c"]
images = video_dict["images"]
valid = video_dict["valid"]
t = len(poses)
c2w = SE3(poses).inv()
# `iproj` only has a cuda backend; if we pass in CPU tensors, it will silently
# return zeros
points = droid_backends.iproj(
c2w.data.cuda(),
video_dict["disps"].cuda(),
video_dict["intrins"].cuda(),
)
valid_pts = torch.cat([points[i, valid[i]] for i in range(t)], dim=0)
valid_rgb = torch.cat([images[i, valid[i]] for i in range(t)], dim=0)
return (
c2w.matrix().detach().cpu(),
valid_pts.detach().cpu(),
valid_rgb.detach().cpu(),
)
def get_frame_cameras(droid, img_paths, intrins_all):
N = len(img_paths)
t = droid.video.counter.value
print(f"{t} keyframes in map")
if t > 1:
with torch.no_grad():
# localize all frames and get edges into keyframe graph
# returns 7D tensor (3D trans, 4D quat)
c2w = droid.terminate(image_stream(img_paths, intrins_all))
c2w = torch.from_numpy(c2w.astype(np.float32))
return SE3(c2w).inv().matrix()
return torch.eye(4)[None].repeat(N, 1, 1)
def save_camera_json(path, extrins, intrins, yup=False):
"""
:param path
:param extrins (N, 4, 4)
:param intrins (N, 4)
"""
N = extrins.shape[0]
if yup:
T = torch.tensor(
[[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]],
dtype=torch.float32,
)
extrins = torch.matmul(T[None], extrins)
with open(path, "w") as f:
json.dump(
{
"rotation": extrins[:, :3, :3].reshape((N, 9)).tolist(),
"translation": extrins[:, :3, 3].tolist(),
"intrinsics": intrins.tolist(),
},
f,
indent=1,
)
def save_pcl(path, points, colors, yup=False):
assert len(points) == len(colors)
if yup:
T = torch.tensor(
[[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]],
dtype=torch.float32,
)
points = torch.einsum("ij,...j->...i", T, points) # (*, 3)
points = points.reshape(-1, 3).numpy()
colors = colors.reshape(-1, 3).numpy()
obj = trimesh.Trimesh(vertices=points, vertex_colors=colors)
with open(path, "wb") as f:
f.write(trimesh.exchange.ply.export_ply(obj))
def save_keyframe_map(out_dir, kf_nodes):
"""
Saves everything with x right y up z back
(converts from x right y down z forward)
:param out_dir
:param kf_nodes (dict)
"""
c2w, points, colors = get_keyframe_map(kf_nodes)
intrins = kf_nodes["intrins"].detach().cpu()
print("c2w", c2w.shape)
print("points", points.shape)
print("colors", colors.shape)
os.makedirs(out_dir, exist_ok=True)
save_pcl(f"{out_dir}/map_points.ply", points, colors)
save_camera_json(f"{out_dir}/map_cameras.json", c2w, intrins)
print(f"saved {len(c2w)} keyframes to {out_dir}")
def save_cameras(map_dir, frame_w2c, intrins):
os.makedirs(map_dir, exist_ok=True)
print("SAVING CAMERA INTRINS AND EXTRINS IN", map_dir)
# save all camera extrinsics and intrinsic
W, H = intrins[0, 4], intrins[0, 5]
focal = intrins[:, :2].mean()
print("INTRINS", intrins[0])
np.savez(
f"{map_dir}/cameras.npz",
height=H,
width=W,
focal=focal,
intrins=intrins[:, :4],
w2c=frame_w2c,
)
# save visualization
frame_intrins = torch.tensor([focal, focal, W / 2, H / 2, W, H])
frame_c2w = torch.linalg.inv(frame_w2c)
save_camera_json(f"{map_dir}/frame_cameras.json", frame_c2w, intrins)
def main(args):
args.stereo = False
torch.multiprocessing.set_start_method("spawn")
droid = None
img_paths = get_image_files(
args.img_dir, args.stride, start=args.start, end=args.end
)
if args.map_dir is not None:
os.makedirs(args.map_dir, exist_ok=True)
camera_file = os.path.join(args.map_dir, "cameras.npz")
if os.path.isfile(camera_file) and not args.overwrite:
print(glob.glob(f"{args.map_dir}/*.npz"), "already exist, skipping")
return
intrins_all = load_intrins(
args.intrins_path, img_paths, start=args.start, end=args.end
)
print("intrins shape", intrins_all.shape, "num images", len(img_paths))
for t, image, intrinsics in tqdm(image_stream(img_paths, intrins_all)):
if t < args.t0:
continue
if not args.disable_vis:
show_image(image[0])
if droid is None:
args.image_size = [image.shape[2], image.shape[3]]
droid = Droid(args)
droid.track(t, image, intrinsics=intrinsics)
if args.map_dir is None:
return
# save cameras
frame_w2c = get_frame_cameras(droid, img_paths, intrins_all)
save_cameras(args.map_dir, frame_w2c, intrins_all)
# save keyframe cameras and points
kf_nodes = unpack_video(droid.video)
save_keyframe_map(args.map_dir, kf_nodes)
def get_slam_parser():
parser = argparse.ArgumentParser()
parser.add_argument("--stride", default=1, type=int, help="frame stride")
parser.add_argument("--t0", default=0, type=int, help="starting frame")
parser.add_argument("--weights", default=f"{ROOT_DIR}/_DATA/droid.pth")
parser.add_argument("--buffer", type=int, default=512)
parser.add_argument("--image_size", default=[240, 320])
parser.add_argument("--disable_vis", action="store_true")
parser.add_argument(
"--beta",
type=float,
default=0.3,
help="weight for translation / rotation components of flow",
)
parser.add_argument(
"--filter_thresh",
type=float,
default=1.0,
help="how much motion before considering new keyframe",
)
parser.add_argument("--warmup", type=int, default=8, help="number of warmup frames")
parser.add_argument(
"--keyframe_thresh",
type=float,
default=4.0,
help="threshold to create a new keyframe",
)
parser.add_argument(
"--frontend_thresh",
type=float,
default=16.0,
help="add edges between frames whithin this distance",
)
parser.add_argument(
"--frontend_window", type=int, default=25, help="frontend optimization window"
)
parser.add_argument(
"--frontend_radius",
type=int,
default=2,
help="force edges between frames within radius",
)
parser.add_argument(
"--frontend_nms", type=int, default=1, help="non-maximal supression of edges"
)
parser.add_argument("--backend_thresh", type=float, default=22.0)
parser.add_argument("--backend_radius", type=int, default=2)
parser.add_argument("--backend_nms", type=int, default=3)
parser.add_argument("--upsample", action="store_true")
return parser
if __name__ == "__main__":
parser = get_slam_parser()
parser.add_argument(
"-i", "--img_dir", required=True, type=str, help="path to image directory"
)
parser.add_argument(
"-o", "--map_dir", type=str, default=None, help="path to output directory"
)
parser.add_argument(
"-y", "--overwrite", action="store_true", help="overwrite existing cameras"
)
parser.add_argument(
"-g", "--save_graph", action="store_true", help="save the frame graph"
)
parser.add_argument(
"--intrins_path", default=None, help="path to camera intrinsics"
)
parser.add_argument("--start", default=0, type=int, help="start frame")
parser.add_argument("--end", default=-1, type=int, help="end frame")
args = parser.parse_args()
main(args)