diff --git a/.gitignore b/.gitignore index 1679c47..8137c61 100755 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,3 @@ __pycache__ -*/build/* +**/build/* .vscode diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py index 88b6c8d..b95a315 100755 --- a/mad_icp/apps/mad_icp.py +++ b/mad_icp/apps/mad_icp.py @@ -152,7 +152,8 @@ def main(data_path: Annotated[ p_th, b_min, b_ratio, num_keyframes, num_cores, realtime) estimate_file_name = estimate_path / "estimate.txt" - estimate_file = open(estimate_file_name, 'w') + estimate_file = open(estimate_file_name, 'a') + estimate_file.truncate(0) with InputDataInterface_lut[reader_type](data_path, min_range, max_range, topic=topic, sensor_hz=sensor_hz, apply_correction=apply_correction) as reader: t_start = datetime.now() diff --git a/mad_icp/apps/utils/utils.py b/mad_icp/apps/utils/utils.py index de091bc..16b94fc 100755 --- a/mad_icp/apps/utils/utils.py +++ b/mad_icp/apps/utils/utils.py @@ -31,7 +31,4 @@ def write_transformed_pose(estimate_file, lidar_to_world, lidar_to_base): base_to_lidar = np.linalg.inv(lidar_to_base) base_to_world = np.dot(lidar_to_base, np.dot(lidar_to_world, base_to_lidar)) - for row in range(3): - for col in range(4): - estimate_file.write(str(base_to_world[row, col]) + " ") - estimate_file.write("\n") \ No newline at end of file + np.savetxt(estimate_file, base_to_world[:3].reshape(-1, 12)) \ No newline at end of file