diff --git a/scripts/basalt_capture_mocap.py b/scripts/basalt_capture_mocap.py index d66ebc0..09e269c 100755 --- a/scripts/basalt_capture_mocap.py +++ b/scripts/basalt_capture_mocap.py @@ -3,6 +3,7 @@ import sys import os import rospy +import argparse from geometry_msgs.msg import TransformStamped @@ -34,11 +35,18 @@ def listener(): if __name__ == '__main__': + + parser = argparse.ArgumentParser(description='Record Motion Capture messages from ROS (/vrpn_client/raw_transform).') + parser.add_argument('-d', '--dataset-path', required=True, help="Path to store the result") + args = parser.parse_args() + + dataset_path = args.dataset_path + out_file = None time_offset = None - if not os.path.exists(sys.argv[1]): - os.makedirs(sys.argv[1]) + if not os.path.exists(dataset_path): + os.makedirs(dataset_path) out_file = open(sys.argv[1] + '/data.csv', 'w') out_file.write('#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n') diff --git a/scripts/basalt_convert_kitti_calib.py b/scripts/basalt_convert_kitti_calib.py index 0d7a77d..78732a3 100755 --- a/scripts/basalt_convert_kitti_calib.py +++ b/scripts/basalt_convert_kitti_calib.py @@ -6,8 +6,13 @@ import numpy as np import os from string import Template import cv2 +import argparse -dataset_path = sys.argv[1] +parser = argparse.ArgumentParser(description='Convert KITTI calibration to basalt and save it int the dataset folder as basalt_calib.json.') +parser.add_argument('-d', '--dataset-path', required=True, help="Path to the dataset in KITTI format") +args = parser.parse_args() + +dataset_path = args.dataset_path print(dataset_path) diff --git a/scripts/basalt_response_calib.py b/scripts/basalt_response_calib.py index b1abc8f..23e1f21 100755 --- a/scripts/basalt_response_calib.py +++ b/scripts/basalt_response_calib.py @@ -4,11 +4,17 @@ import sys import math import os import cv2 +import argparse import numpy as np from matplotlib import pyplot as plt -dataset_path = sys.argv[1] +parser = argparse.ArgumentParser(description='Response calibration.') +parser.add_argument('-d', '--dataset-path', required=True, help="Path to the dataset in Euroc format") +args = parser.parse_args() + + +dataset_path = args.dataset_path print(dataset_path) @@ -23,7 +29,8 @@ imgs = [] # check image data. for timestamp in timestamps: path = dataset_path + '/mav0/cam0/data/' + str(timestamp) - img = cv2.imread(dataset_path + '/mav0/cam0/data/' + str(timestamp) + '.webp', cv2.IMREAD_GRAYSCALE)[:,:,0] + img = cv2.imread(dataset_path + '/mav0/cam0/data/' + str(timestamp) + '.webp', cv2.IMREAD_GRAYSCALE) + if len(img.shape) == 3: img = img[:,:,0] imgs.append(img) pixel_avgs.append(np.mean(img)) @@ -73,9 +80,10 @@ def print_error(): generated_imgs = irradiance[np.newaxis, :, :] * exposures[:, np.newaxis, np.newaxis] generated_imgs -= inv_resp[imgs] generated_imgs[imgs == 255] = 0 - print(np.sum(generated_imgs**2)) + print('Error', np.sum(generated_imgs**2)) for iter in range(5): + print('Iteration', iter) irradiance = opt_irradiance() print_error() inv_resp = opt_inv_resp() diff --git a/scripts/basalt_verify_dataset.py b/scripts/basalt_verify_dataset.py index 1cd3bc2..2d88015 100755 --- a/scripts/basalt_verify_dataset.py +++ b/scripts/basalt_verify_dataset.py @@ -3,10 +3,17 @@ import sys import math import os +import argparse import numpy as np -dataset_path = sys.argv[1] + +parser = argparse.ArgumentParser(description='Check the dataset. Report if any images are missing.') +parser.add_argument('-d', '--dataset-path', required=True, help="Path to the dataset in Euroc format") +args = parser.parse_args() + + +dataset_path = args.dataset_path print(dataset_path)