From 28871767ea117d3698ffe9f209f3ea3f4ad810f4 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sat, 18 Nov 2023 18:50:21 -0800 Subject: [PATCH 01/13] add examples --- CMakeLists.txt | 10 +- .../Monocular-Inertial/mono_inertial_euroc.cc | 2 +- Examples/Stereo/stereo_euroc.cc | 2 +- Examples/euroc_examples.sh | 182 ++++++++++++++++++ Examples_old/ROS/ORB_SLAM3/src/ros_mono.cc | 2 +- 5 files changed, 190 insertions(+), 8 deletions(-) create mode 100644 Examples/euroc_examples.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d..da78e9877a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,20 +12,20 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -# Check C++11 or C++0x support +# Check C++14 or C++0x support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") + message(STATUS "Using flag -std=c++14.") elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") add_definitions(-DCOMPILEDWITHC0X) message(STATUS "Using flag -std=c++0x.") else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc index b9f320fd76..bff32b2790 100644 --- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc @@ -117,7 +117,7 @@ int main(int argc, char *argv[]) cout.precision(17); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true); + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, false); float imageScale = SLAM.GetImageScale(); double t_resize = 0.f; diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc index 1250f2ee3c..0aa89ce0bd 100644 --- a/Examples/Stereo/stereo_euroc.cc +++ b/Examples/Stereo/stereo_euroc.cc @@ -88,7 +88,7 @@ int main(int argc, char **argv) cout.precision(17); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO, true); + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO, false); cv::Mat imLeft, imRight; for (seq = 0; seq Date: Sun, 19 Nov 2023 09:33:44 -0800 Subject: [PATCH 02/13] add parameters instead of arguments --- evaluation/evaluate_ate_scale.py | 100 ++++++++++++++++--------------- 1 file changed, 53 insertions(+), 47 deletions(-) diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index 3eb73ebcf7..fbadae0065 100644 --- a/evaluation/evaluate_ate_scale.py +++ b/evaluation/evaluate_ate_scale.py @@ -135,75 +135,84 @@ def plot_traj(ax,stamps,traj,style,color,label): parser = argparse.ArgumentParser(description=''' This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. ''') - parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)') - parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)') - parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) - parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0) - parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 10000000 ns)',default=20000000) - parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)') - parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)') - parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)') - parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true') - parser.add_argument('--verbose2', help='print scale eror and RMSE absolute translational error in meters after alignment with and without scale correction', action='store_true') - args = parser.parse_args() - - first_list = associate.read_file_list(args.first_file, False) - second_list = associate.read_file_list(args.second_file, False) - - matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference)) + first_file = '/home/justmohsen/Documents/SLAM2/Datasets/EuRoc/MH01/mav0/state_groundtruth_estimate0/data.csv' + second_file = '/home/justmohsen/Documents/SLAM2/ORB_SLAM3/f_dataset-MH01_mono.txt' + # print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed) + is_verbose = True + # print scale eror and RMSE absolute translational error in meters after alignment with and without scale correction + is_verbose2 = True + # save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2) + is_save_associations = True + # save aligned second trajectory to disk (format: stamp2 x2 y2 z2) + is_save = True + # plot the first and the aligned second trajectory to an image (format: png) + is_plot = True + # maximally allowed time difference for matching entries (default: 10000000 ns) + max_time_difference_ns = 20000000 + # time offset added to the timestamps of the second file (default: 0.0) + time_offset = 0 + # scaling factor for the second trajectory (default: 1.0) + scale_factor = 1.0 + poses_associations_output_file = 'poses_associations.txt' + time_associations_output_file = 'time_associations.txt' + plot_results_output_file = 'results_output.png' + + first_list = associate.read_file_list(first_file, False) + second_list = associate.read_file_list(second_file, False) + + matches = associate.associate(first_list, second_list,float(time_offset),float(max_time_difference_ns)) if len(matches)<2: sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?") first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose() - second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose() + second_xyz = numpy.matrix([[float(value)*float(scale_factor) for value in second_list[b][0:3]] for a,b in matches]).transpose() dictionary_items = second_list.items() sorted_second_list = sorted(dictionary_items) - second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in sorted_second_list[i][1][0:3]] for i in range(len(sorted_second_list))]).transpose() # sorted_second_list.keys()]).transpose() + second_xyz_full = numpy.matrix([[float(value)*float(scale_factor) for value in sorted_second_list[i][1][0:3]] for i in range(len(sorted_second_list))]).transpose() # sorted_second_list.keys()]).transpose() rot,transGT,trans_errorGT,trans,trans_error, scale = align(second_xyz,first_xyz) second_xyz_aligned = scale * rot * second_xyz + trans second_xyz_notscaled = rot * second_xyz + trans second_xyz_notscaled_full = rot * second_xyz_full + trans - first_stamps = first_list.keys() + first_stamps = list(first_list.keys()) first_stamps.sort() first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose() - second_stamps = second_list.keys() + second_stamps = list(second_list.keys()) second_stamps.sort() - second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose() + second_xyz_full = numpy.matrix([[float(value)*float(scale_factor) for value in second_list[b][0:3]] for b in second_stamps]).transpose() second_xyz_full_aligned = scale * rot * second_xyz_full + trans - if args.verbose: - print "compared_pose_pairs %d pairs"%(len(trans_error)) - - print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) - print "absolute_translational_error.mean %f m"%numpy.mean(trans_error) - print "absolute_translational_error.median %f m"%numpy.median(trans_error) - print "absolute_translational_error.std %f m"%numpy.std(trans_error) - print "absolute_translational_error.min %f m"%numpy.min(trans_error) - print "absolute_translational_error.max %f m"%numpy.max(trans_error) - print "max idx: %i" %numpy.argmax(trans_error) + if is_verbose: + print(f"compared_pose_pairs %d pairs"%(len(trans_error))) + print(f"absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))) + print(f"absolute_translational_error.mean %f m"%numpy.mean(trans_error)) + print(f"absolute_translational_error.median %f m"%numpy.median(trans_error)) + print(f"absolute_translational_error.std %f m"%numpy.std(trans_error)) + print(f"absolute_translational_error.min %f m"%numpy.min(trans_error)) + print(f"absolute_translational_error.max %f m"%numpy.max(trans_error)) + print(f"max idx: %i" %numpy.argmax(trans_error)) else: # print "%f, %f " % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale) # print "%f,%f" % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale) - print "%f,%f,%f" % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale, numpy.sqrt(numpy.dot(trans_errorGT,trans_errorGT) / len(trans_errorGT))) + print("%f,%f,%f" % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale, numpy.sqrt(numpy.dot(trans_errorGT,trans_errorGT) / len(trans_errorGT)))) # print "%f" % len(trans_error) - if args.verbose2: - print "compared_pose_pairs %d pairs"%(len(trans_error)) - print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) - print "absolute_translational_errorGT.rmse %f m"%numpy.sqrt(numpy.dot(trans_errorGT,trans_errorGT) / len(trans_errorGT)) + if is_verbose2: + print(f"compared_pose_pairs %d pairs"%(len(trans_error))) + print(f"absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))) + print(f"absolute_translational_errorGT.rmse %f m"%numpy.sqrt(numpy.dot(trans_errorGT,trans_errorGT) / len(trans_errorGT))) - if args.save_associations: - file = open(args.save_associations,"w") + if is_save_associations: + file = open(poses_associations_output_file,"w") file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)])) file.close() - if args.save: - file = open(args.save,"w") + if is_save: + file = open(time_associations_output_file,"w") file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_notscaled_full.transpose().A)])) file.close() - if args.plot: + if is_plot: import matplotlib matplotlib.use('Agg') import matplotlib.pyplot as plt @@ -214,8 +223,8 @@ def plot_traj(ax,stamps,traj,style,color,label): plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth") plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") label="difference" - for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A): - ax.plot([x1,x2],[y1,y2],'-',color="red",label=label) + for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_notscaled.transpose().A): + ax.plot([x1,x2],[y1,y2],'-',color="violet",label=label, alpha=0.5, linewidth=0.05) label="" ax.legend() @@ -223,7 +232,4 @@ def plot_traj(ax,stamps,traj,style,color,label): ax.set_xlabel('x [m]') ax.set_ylabel('y [m]') plt.axis('equal') - plt.savefig(args.plot,format="pdf") - - - + plt.savefig(plot_results_output_file,format="png") From a6388b5e65d7912b53229455a1d49d5954f5121f Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 09:33:57 -0800 Subject: [PATCH 03/13] fix a typo in the evaluation job --- evaluation/evaluate_ate_scale.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index fbadae0065..bb8ab6932e 100644 --- a/evaluation/evaluate_ate_scale.py +++ b/evaluation/evaluate_ate_scale.py @@ -85,10 +85,10 @@ def align(model,data): s = float(dots/norms) transGT = data.mean(1) - s*rot * model.mean(1) - trans = data.mean(1) - rot * model.mean(1) + trans = data.mean(1) - s * rot * model.mean(1) model_alignedGT = s*rot * model + transGT - model_aligned = rot * model + trans + model_aligned = s * rot * model + trans alignment_errorGT = model_alignedGT - data alignment_error = model_aligned - data From 19eb7f99b3d06b0c2a0cb64557d844a1bd172ac0 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 09:34:20 -0800 Subject: [PATCH 04/13] update to list to use python3 --- evaluation/associate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/evaluation/associate.py b/evaluation/associate.py index 222b7c8c10..6edca8b9a2 100644 --- a/evaluation/associate.py +++ b/evaluation/associate.py @@ -85,8 +85,8 @@ def associate(first_list, second_list,offset,max_difference): matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) """ - first_keys = first_list.keys() - second_keys = second_list.keys() + first_keys = list(first_list.keys()) + second_keys = list(second_list.keys()) potential_matches = [(abs(a - (b + offset)), a, b) for a in first_keys for b in second_keys From 403c42e88a47435eb25ab1b6046e630c89172d3e Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 11:30:20 -0800 Subject: [PATCH 05/13] evaluate all dataset --- evaluation/evaluate_ate_scale.py | 195 +++++++++++++++---------------- 1 file changed, 94 insertions(+), 101 deletions(-) diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index bb8ab6932e..9b74326d61 100644 --- a/evaluation/evaluate_ate_scale.py +++ b/evaluation/evaluate_ate_scale.py @@ -32,9 +32,6 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# -# Requirements: -# sudo apt-get install python-argparse """ This script computes the absolute trajectory error from the ground truth @@ -43,8 +40,8 @@ import sys import numpy -import argparse import associate +import pandas def align(model,data): """Align two trajectories using the method of Horn (closed-form). @@ -82,21 +79,13 @@ def align(model,data): normi = numpy.linalg.norm(model_zerocentered[:,column]) norms += normi*normi - s = float(dots/norms) - - transGT = data.mean(1) - s*rot * model.mean(1) + s = float(dots/norms) trans = data.mean(1) - s * rot * model.mean(1) - - model_alignedGT = s*rot * model + transGT model_aligned = s * rot * model + trans - - alignment_errorGT = model_alignedGT - data alignment_error = model_aligned - data - - trans_errorGT = numpy.sqrt(numpy.sum(numpy.multiply(alignment_errorGT,alignment_errorGT),0)).A[0] trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0] - return rot,transGT,trans_errorGT,trans,trans_error, s + return rot,trans,trans_error, s def plot_traj(ax,stamps,traj,style,color,label): """ @@ -131,16 +120,10 @@ def plot_traj(ax,stamps,traj,style,color,label): if __name__=="__main__": - # parse command line - parser = argparse.ArgumentParser(description=''' - This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. - ''') - first_file = '/home/justmohsen/Documents/SLAM2/Datasets/EuRoc/MH01/mav0/state_groundtruth_estimate0/data.csv' - second_file = '/home/justmohsen/Documents/SLAM2/ORB_SLAM3/f_dataset-MH01_mono.txt' # print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed) - is_verbose = True - # print scale eror and RMSE absolute translational error in meters after alignment with and without scale correction - is_verbose2 = True + is_verbose = False + # save all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed) + is_save_verbose = True # save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2) is_save_associations = True # save aligned second trajectory to disk (format: stamp2 x2 y2 z2) @@ -153,83 +136,93 @@ def plot_traj(ax,stamps,traj,style,color,label): time_offset = 0 # scaling factor for the second trajectory (default: 1.0) scale_factor = 1.0 - poses_associations_output_file = 'poses_associations.txt' - time_associations_output_file = 'time_associations.txt' - plot_results_output_file = 'results_output.png' - - first_list = associate.read_file_list(first_file, False) - second_list = associate.read_file_list(second_file, False) - - matches = associate.associate(first_list, second_list,float(time_offset),float(max_time_difference_ns)) - if len(matches)<2: - sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?") - first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose() - second_xyz = numpy.matrix([[float(value)*float(scale_factor) for value in second_list[b][0:3]] for a,b in matches]).transpose() - dictionary_items = second_list.items() - sorted_second_list = sorted(dictionary_items) - - second_xyz_full = numpy.matrix([[float(value)*float(scale_factor) for value in sorted_second_list[i][1][0:3]] for i in range(len(sorted_second_list))]).transpose() # sorted_second_list.keys()]).transpose() - rot,transGT,trans_errorGT,trans,trans_error, scale = align(second_xyz,first_xyz) - - second_xyz_aligned = scale * rot * second_xyz + trans - second_xyz_notscaled = rot * second_xyz + trans - second_xyz_notscaled_full = rot * second_xyz_full + trans - first_stamps = list(first_list.keys()) - first_stamps.sort() - first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose() - - second_stamps = list(second_list.keys()) - second_stamps.sort() - second_xyz_full = numpy.matrix([[float(value)*float(scale_factor) for value in second_list[b][0:3]] for b in second_stamps]).transpose() - second_xyz_full_aligned = scale * rot * second_xyz_full + trans - - if is_verbose: - print(f"compared_pose_pairs %d pairs"%(len(trans_error))) - print(f"absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))) - print(f"absolute_translational_error.mean %f m"%numpy.mean(trans_error)) - print(f"absolute_translational_error.median %f m"%numpy.median(trans_error)) - print(f"absolute_translational_error.std %f m"%numpy.std(trans_error)) - print(f"absolute_translational_error.min %f m"%numpy.min(trans_error)) - print(f"absolute_translational_error.max %f m"%numpy.max(trans_error)) - print(f"max idx: %i" %numpy.argmax(trans_error)) - else: - # print "%f, %f " % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale) - # print "%f,%f" % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale) - print("%f,%f,%f" % (numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), scale, numpy.sqrt(numpy.dot(trans_errorGT,trans_errorGT) / len(trans_errorGT)))) - # print "%f" % len(trans_error) - if is_verbose2: - print(f"compared_pose_pairs %d pairs"%(len(trans_error))) - print(f"absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))) - print(f"absolute_translational_errorGT.rmse %f m"%numpy.sqrt(numpy.dot(trans_errorGT,trans_errorGT) / len(trans_errorGT))) - - if is_save_associations: - file = open(poses_associations_output_file,"w") - file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)])) - file.close() - - if is_save: - file = open(time_associations_output_file,"w") - file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_notscaled_full.transpose().A)])) - file.close() - - if is_plot: - import matplotlib - matplotlib.use('Agg') - import matplotlib.pyplot as plt - import matplotlib.pylab as pylab - from matplotlib.patches import Ellipse - fig = plt.figure() - ax = fig.add_subplot(111) - plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth") - plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") - label="difference" - for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_notscaled.transpose().A): - ax.plot([x1,x2],[y1,y2],'-',color="violet",label=label, alpha=0.5, linewidth=0.05) - label="" + # Dataset path + dataset_path = './Datasets/euroc/MachineHall/' + # Algorithms and subdatasets + algorithms = ['monocular'] + sub_datasets = ['MH01'] + # Go through all algoirhtms and datasets + for algorithm in algorithms: + for sub_dataset in sub_datasets: + groundtruth_path = dataset_path + sub_dataset +'/mav0/state_groundtruth_estimate0/data.csv' + orb_results_path = dataset_path + 'ORBSLAM3_Run/' + algorithm + '/' + sub_dataset + '_frames.txt' + evaluation_results_path = dataset_path + 'ORBSLAM3_Run/' + algorithm + '/' + sub_dataset + first_file = groundtruth_path + second_file = orb_results_path + # save evaluation output + poses_associations_output_file = evaluation_results_path + '_poses_associations.txt' + time_associations_output_file = evaluation_results_path + '_time_associations.txt' + estimation_error_summary_output_file = evaluation_results_path + '_evaluation_error.txt' + plot_results_output_file = evaluation_results_path + '_results_output.png' + + first_list = associate.read_file_list(first_file, False) + second_list = associate.read_file_list(second_file, False) + + matches = associate.associate(first_list, second_list,float(time_offset),float(max_time_difference_ns)) + if len(matches)<2: + sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?") + first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose() + second_xyz = numpy.matrix([[float(value)*float(scale_factor) for value in second_list[b][0:3]] for a,b in matches]).transpose() + dictionary_items = second_list.items() + sorted_second_list = sorted(dictionary_items) + + second_xyz_full = numpy.matrix([[float(value)*float(scale_factor) for value in sorted_second_list[i][1][0:3]] for i in range(len(sorted_second_list))]).transpose() # sorted_second_list.keys()]).transpose() + rot,trans,trans_error, scale = align(second_xyz,first_xyz) + + second_xyz_aligned = scale * rot * second_xyz + trans + first_stamps = list(first_list.keys()) + first_stamps.sort() + first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose() - ax.legend() + second_stamps = list(second_list.keys()) + second_stamps.sort() + second_xyz_full = numpy.matrix([[float(value)*float(scale_factor) for value in second_list[b][0:3]] for b in second_stamps]).transpose() + second_xyz_full_aligned = scale * rot * second_xyz_full + trans - ax.set_xlabel('x [m]') - ax.set_ylabel('y [m]') - plt.axis('equal') - plt.savefig(plot_results_output_file,format="png") + if is_verbose: + print(f"compared_pose_pairs %d pairs"%(len(trans_error))) + print(f"absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))) + print(f"absolute_translational_error.mean %f m"%numpy.mean(trans_error)) + print(f"absolute_translational_error.median %f m"%numpy.median(trans_error)) + print(f"absolute_translational_error.std %f m"%numpy.std(trans_error)) + print(f"absolute_translational_error.min %f m"%numpy.min(trans_error)) + print(f"absolute_translational_error.max %f m"%numpy.max(trans_error)) + print(f"max idx: %i" %numpy.argmax(trans_error)) + + if is_save_verbose: + headers = ["# pose paired compared", "rmse of absolute_translational_error", "mean of absolute_translational_error", "median of absolute_translational_error", "std of absolute_translational_error", "min of absolute_translational_error", "max of absolute_translational_error"] + output_df = pandas.DataFrame([len(trans_error), numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)), numpy.mean(trans_error), numpy.median(trans_error), numpy.std(trans_error), numpy.min(trans_error), numpy.max(trans_error)]) + output_df.index = headers + output_df.to_csv(estimation_error_summary_output_file) + + if is_save_associations: + file = open(poses_associations_output_file,"w") + file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)])) + file.close() + + if is_save: + file = open(time_associations_output_file,"w") + file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_aligned.transpose().A)])) + file.close() + + if is_plot: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + import matplotlib.pylab as pylab + from matplotlib.patches import Ellipse + fig = plt.figure() + ax = fig.add_subplot(111) + plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth") + plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") + label="difference" + for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A): + ax.plot([x1,x2],[y1,y2],'-',color="red",label=label, alpha=0.25) + label="" + + ax.legend() + + ax.set_xlabel('x [m]') + ax.set_ylabel('y [m]') + plt.axis('equal') + plt.savefig(plot_results_output_file,format="png") From a31835e59d797613d8d8806d33b9245b2538d7e3 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 11:32:18 -0800 Subject: [PATCH 06/13] change save name --- Examples/Monocular-Inertial/mono_inertial_euroc.cc | 4 ++-- Examples/Monocular-Inertial/mono_inertial_tum_vi.cc | 4 ++-- Examples/Monocular/mono_euroc.cc | 4 ++-- Examples/Monocular/mono_tum_vi.cc | 4 ++-- Examples/Stereo-Inertial/stereo_inertial_euroc.cc | 4 ++-- Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc | 4 ++-- Examples/Stereo/stereo_euroc.cc | 4 ++-- Examples/Stereo/stereo_tum_vi.cc | 4 ++-- Examples_old/Monocular-Inertial/mono_inertial_euroc.cc | 4 ++-- Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc | 4 ++-- Examples_old/Monocular/mono_euroc.cc | 4 ++-- Examples_old/Monocular/mono_tum_vi.cc | 4 ++-- Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc | 4 ++-- Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc | 4 ++-- Examples_old/Stereo/stereo_euroc.cc | 4 ++-- Examples_old/Stereo/stereo_tum_vi.cc | 4 ++-- 16 files changed, 32 insertions(+), 32 deletions(-) diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc index bff32b2790..7fc6b7accf 100644 --- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc @@ -235,8 +235,8 @@ int main(int argc, char *argv[]) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc index bd84ce0012..0487259bf2 100644 --- a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc +++ b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc @@ -253,8 +253,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc index 3a233129be..5165ac1a25 100644 --- a/Examples/Monocular/mono_euroc.cc +++ b/Examples/Monocular/mono_euroc.cc @@ -189,8 +189,8 @@ int main(int argc, char **argv) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Monocular/mono_tum_vi.cc b/Examples/Monocular/mono_tum_vi.cc index 2805c92522..448e268125 100644 --- a/Examples/Monocular/mono_tum_vi.cc +++ b/Examples/Monocular/mono_tum_vi.cc @@ -201,8 +201,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc index bb52223542..26a979bcc3 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc @@ -226,8 +226,8 @@ int main(int argc, char **argv) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc index 2abe61472b..11993740cc 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc @@ -259,8 +259,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc index 0aa89ce0bd..7614dca035 100644 --- a/Examples/Stereo/stereo_euroc.cc +++ b/Examples/Stereo/stereo_euroc.cc @@ -171,8 +171,8 @@ int main(int argc, char **argv) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc index 20a3ecf72d..63f9150243 100644 --- a/Examples/Stereo/stereo_tum_vi.cc +++ b/Examples/Stereo/stereo_tum_vi.cc @@ -209,8 +209,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc b/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc index ea1dceb805..d375db3a12 100644 --- a/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc @@ -234,8 +234,8 @@ int main(int argc, char *argv[]) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc index bd84ce0012..0487259bf2 100644 --- a/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc +++ b/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc @@ -253,8 +253,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular/mono_euroc.cc b/Examples_old/Monocular/mono_euroc.cc index 3a233129be..63f571d55a 100644 --- a/Examples_old/Monocular/mono_euroc.cc +++ b/Examples_old/Monocular/mono_euroc.cc @@ -189,8 +189,8 @@ int main(int argc, char **argv) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular/mono_tum_vi.cc b/Examples_old/Monocular/mono_tum_vi.cc index 2805c92522..448e268125 100644 --- a/Examples_old/Monocular/mono_tum_vi.cc +++ b/Examples_old/Monocular/mono_tum_vi.cc @@ -201,8 +201,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc index 25dbf3cf2b..0d90988d82 100644 --- a/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc +++ b/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc @@ -303,8 +303,8 @@ int main(int argc, char **argv) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc index 2abe61472b..11993740cc 100644 --- a/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc +++ b/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc @@ -259,8 +259,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo/stereo_euroc.cc b/Examples_old/Stereo/stereo_euroc.cc index 08a59e4272..48029924e6 100644 --- a/Examples_old/Stereo/stereo_euroc.cc +++ b/Examples_old/Stereo/stereo_euroc.cc @@ -258,8 +258,8 @@ int main(int argc, char **argv) // Save camera trajectory if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo/stereo_tum_vi.cc b/Examples_old/Stereo/stereo_tum_vi.cc index 3e296f6d21..60b90248e3 100644 --- a/Examples_old/Stereo/stereo_tum_vi.cc +++ b/Examples_old/Stereo/stereo_tum_vi.cc @@ -210,8 +210,8 @@ int main(int argc, char **argv) if (bFileName) { - const string kf_file = "kf_" + string(argv[argc-1]) + ".txt"; - const string f_file = "f_" + string(argv[argc-1]) + ".txt"; + const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; + const string f_file = string(argv[argc-1]) + "frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } From 777084f28e7e51b9bf353fe4afe4342b318cf232 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 12:59:39 -0800 Subject: [PATCH 07/13] Update evaluate_ate_scale.py --- evaluation/evaluate_ate_scale.py | 53 +++++++++++++++++++------------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index 9b74326d61..d4b1ffd94a 100644 --- a/evaluation/evaluate_ate_scale.py +++ b/evaluation/evaluate_ate_scale.py @@ -42,6 +42,11 @@ import numpy import associate import pandas +import matplotlib +from mpl_toolkits.mplot3d import Axes3D +matplotlib.use('Agg') +import matplotlib.pyplot as plt +import os def align(model,data): """Align two trajectories using the method of Horn (closed-form). @@ -104,19 +109,22 @@ def plot_traj(ax,stamps,traj,style,color,label): interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])]) x = [] y = [] + z = [] last = stamps[0] for i in range(len(stamps)): if stamps[i]-last < 2*interval: x.append(traj[i][0]) y.append(traj[i][1]) + z.append(traj[i][2]) elif len(x)>0: - ax.plot(x,y,style,color=color,label=label) + ax.plot(x,y,z,style,color=color,label=label) label="" x=[] y=[] + z=[] last= stamps[i] if len(x)>0: - ax.plot(x,y,style,color=color,label=label) + ax.plot(x,y,z,style,color=color,label=label) if __name__=="__main__": @@ -139,7 +147,7 @@ def plot_traj(ax,stamps,traj,style,color,label): # Dataset path dataset_path = './Datasets/euroc/MachineHall/' # Algorithms and subdatasets - algorithms = ['monocular'] + algorithms = ['monocular', 'stereo'] sub_datasets = ['MH01'] # Go through all algoirhtms and datasets for algorithm in algorithms: @@ -149,11 +157,16 @@ def plot_traj(ax,stamps,traj,style,color,label): evaluation_results_path = dataset_path + 'ORBSLAM3_Run/' + algorithm + '/' + sub_dataset first_file = groundtruth_path second_file = orb_results_path + # create the output directory if it doesn't exist + if not os.path.exists(evaluation_results_path): + os.makedirs(evaluation_results_path) + print('new directory is created for output') # save evaluation output - poses_associations_output_file = evaluation_results_path + '_poses_associations.txt' - time_associations_output_file = evaluation_results_path + '_time_associations.txt' - estimation_error_summary_output_file = evaluation_results_path + '_evaluation_error.txt' - plot_results_output_file = evaluation_results_path + '_results_output.png' + poses_associations_output_file = evaluation_results_path + '/poses_associations.txt' + time_associations_output_file = evaluation_results_path + '/time_associations.txt' + estimation_error_summary_output_file = evaluation_results_path + '/evaluation_error.txt' + plot_results_output_file = evaluation_results_path + '/trajectory_output.png' + plot_error_hist_output_file = evaluation_results_path + '/error_histogram.png' first_list = associate.read_file_list(first_file, False) second_list = associate.read_file_list(second_file, False) @@ -206,23 +219,21 @@ def plot_traj(ax,stamps,traj,style,color,label): file.close() if is_plot: - import matplotlib - matplotlib.use('Agg') - import matplotlib.pyplot as plt - import matplotlib.pylab as pylab - from matplotlib.patches import Ellipse fig = plt.figure() - ax = fig.add_subplot(111) + ax = fig.add_subplot(projection='3d') plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth") - plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") - label="difference" - for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A): - ax.plot([x1,x2],[y1,y2],'-',color="red",label=label, alpha=0.25) - label="" - + plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") ax.legend() - ax.set_xlabel('x [m]') ax.set_ylabel('y [m]') - plt.axis('equal') plt.savefig(plot_results_output_file,format="png") + position_error=first_xyz-second_xyz_aligned + plt.clf() + fig, ax = plt.subplots(3) + fig.suptitle('Error Histograms') + ax[0].hist(numpy.array(position_error[0,:]).flatten(), label='error in x', bins=100, color='blue') + ax[1].hist(numpy.array(position_error[1,:]).flatten(), label='error in y', bins=100, color='green') + ax[2].hist(numpy.array(position_error[2,:]).flatten(), label='error in z', bins=100, color='red') + ax[2].set_xlabel('error [m]') + fig.legend() + plt.savefig(plot_error_hist_output_file,format="png") From fceacd3039e0612b68dc0df651087e383287d582 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 13:00:04 -0800 Subject: [PATCH 08/13] Update System.cc --- src/System.cc | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/System.cc b/src/System.cc index 60d9c5185a..301f83cbc8 100644 --- a/src/System.cc +++ b/src/System.cc @@ -620,7 +620,7 @@ void System::SaveTrajectoryTUM(const string &filename) Eigen::Vector3f twc = Twc.translation(); Eigen::Quaternionf q = Twc.unit_quaternion(); - f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } f.close(); // cout << endl << "trajectory saved!" << endl; @@ -652,7 +652,7 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f t = Twc.translation(); f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t(0) << " " << t(1) << " " << t(2) - << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } @@ -759,14 +759,14 @@ void System::SaveTrajectoryEuRoC(const string &filename) Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } else { Sophus::SE3f Twc = ((*lit)*Trw).inverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f twc = Twc.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } // cout << "5" << endl; @@ -864,14 +864,14 @@ void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } else { Sophus::SE3f Twc = ((*lit)*Trw).inverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f twc = Twc.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } // cout << "5" << endl; @@ -982,7 +982,7 @@ void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) Eigen::Vector3f twb = Twb.translation(); Eigen::Quaternionf q = Twb.unit_quaternion(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } else { @@ -991,7 +991,7 @@ void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) Eigen::Vector3f twc = Twc.translation(); Eigen::Quaternionf q = Twc.unit_quaternion(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } // cout << "5" << endl; @@ -1098,7 +1098,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) Sophus::SE3f Twb = pKF->GetImuPose(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } else @@ -1106,7 +1106,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) Sophus::SE3f Twc = pKF->GetPoseInverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } } f.close(); @@ -1136,7 +1136,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap) Sophus::SE3f Twb = pKF->GetImuPose(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } else @@ -1144,7 +1144,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap) Sophus::SE3f Twc = pKF->GetPoseInverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; } } f.close(); From 7fb614089df4f1d3f5174830c8c77f8e964ac1ab Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 15:21:08 -0800 Subject: [PATCH 09/13] merge ground truth data --- evaluation/associate.py | 6 +++--- evaluation/evaluate_ate_scale.py | 6 +++--- evaluation/merge_ground_truth.py | 14 ++++++++++++++ 3 files changed, 20 insertions(+), 6 deletions(-) create mode 100644 evaluation/merge_ground_truth.py diff --git a/evaluation/associate.py b/evaluation/associate.py index 6edca8b9a2..3ac642941c 100644 --- a/evaluation/associate.py +++ b/evaluation/associate.py @@ -66,9 +66,9 @@ def read_file_list(filename,remove_bounds): lines = data.replace(","," ").replace("\t"," ").split("\n") if remove_bounds: lines = lines[100:-100] - list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] - list = [(float(l[0]),l[1:]) for l in list if len(l)>1] - return dict(list) + lines_list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] + lines_list = [(float(l[0]),l[1:]) for l in lines_list if len(l)>1] + return dict(lines_list) def associate(first_list, second_list,offset,max_difference): """ diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index d4b1ffd94a..b76d20cb35 100644 --- a/evaluation/evaluate_ate_scale.py +++ b/evaluation/evaluate_ate_scale.py @@ -147,13 +147,13 @@ def plot_traj(ax,stamps,traj,style,color,label): # Dataset path dataset_path = './Datasets/euroc/MachineHall/' # Algorithms and subdatasets - algorithms = ['monocular', 'stereo'] - sub_datasets = ['MH01'] + algorithms = ['monocular', 'stereo', 'monocular-inertial', 'stereo-inertial'] + sub_datasets = ['MH01_to_MH05'] # Go through all algoirhtms and datasets for algorithm in algorithms: for sub_dataset in sub_datasets: groundtruth_path = dataset_path + sub_dataset +'/mav0/state_groundtruth_estimate0/data.csv' - orb_results_path = dataset_path + 'ORBSLAM3_Run/' + algorithm + '/' + sub_dataset + '_frames.txt' + orb_results_path = dataset_path + 'ORBSLAM3_Run/' + algorithm + '/' + sub_dataset + '/results_frames.txt' evaluation_results_path = dataset_path + 'ORBSLAM3_Run/' + algorithm + '/' + sub_dataset first_file = groundtruth_path second_file = orb_results_path diff --git a/evaluation/merge_ground_truth.py b/evaluation/merge_ground_truth.py new file mode 100644 index 0000000000..ea8c078fc8 --- /dev/null +++ b/evaluation/merge_ground_truth.py @@ -0,0 +1,14 @@ +import pandas as pd + +sub_datasets = ['MH01', 'MH02', 'MH03', 'MH04', 'MH05'] +ground_truth_paths = [f'Datasets/euroc/MachineHall/{i}/mav0/state_groundtruth_estimate0/data.csv' for i in sub_datasets] +ground_truth_columns = ['timestamp', '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 []', 'v_RS_R_x [m s^-1]', 'v_RS_R_y [m s^-1]', 'v_RS_R_z [m s^-1]', 'b_w_RS_S_x [rad s^-1]', 'b_w_RS_S_y [rad s^-1]', 'b_w_RS_S_z [rad s^-1]', 'b_a_RS_S_x [m s^-2]', 'b_a_RS_S_y [m s^-2]', 'b_a_RS_S_z [m s^-2]'] +file_ground_truth_df_list = [] + +for file in ground_truth_paths: + file_ground_truth_df = pd.read_csv(file) + file_ground_truth_df_list.append(file_ground_truth_df) + +df = pd.concat(file_ground_truth_df_list) +output_dir = 'Datasets/euroc/MachineHall/MH01_to_MH05/mav0/state_groundtruth_estimate0/data.csv' +df.to_csv(output_dir, index=False) \ No newline at end of file From 03c21809908ca5583e596cc1066d765451e26866 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 15:23:07 -0800 Subject: [PATCH 10/13] fix a typo --- Examples/Monocular-Inertial/mono_inertial_euroc.cc | 2 +- Examples/Monocular-Inertial/mono_inertial_tum_vi.cc | 2 +- Examples/Monocular/mono_tum_vi.cc | 2 +- Examples/Stereo-Inertial/stereo_inertial_euroc.cc | 2 +- Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc | 2 +- Examples/Stereo/stereo_euroc.cc | 2 +- Examples/Stereo/stereo_tum_vi.cc | 2 +- Examples_old/Monocular-Inertial/mono_inertial_euroc.cc | 2 +- Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc | 2 +- Examples_old/Monocular/mono_euroc.cc | 2 +- Examples_old/Monocular/mono_tum_vi.cc | 2 +- Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc | 2 +- Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc | 2 +- Examples_old/Stereo/stereo_euroc.cc | 2 +- Examples_old/Stereo/stereo_tum_vi.cc | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc index 7fc6b7accf..dab25af528 100644 --- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc @@ -236,7 +236,7 @@ int main(int argc, char *argv[]) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc index 0487259bf2..56d8df468e 100644 --- a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc +++ b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc @@ -254,7 +254,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Monocular/mono_tum_vi.cc b/Examples/Monocular/mono_tum_vi.cc index 448e268125..b0e6e89674 100644 --- a/Examples/Monocular/mono_tum_vi.cc +++ b/Examples/Monocular/mono_tum_vi.cc @@ -202,7 +202,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc index 26a979bcc3..297bcaf8b8 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc @@ -227,7 +227,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc index 11993740cc..24d100200a 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc @@ -260,7 +260,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc index 7614dca035..6df8d07b85 100644 --- a/Examples/Stereo/stereo_euroc.cc +++ b/Examples/Stereo/stereo_euroc.cc @@ -172,7 +172,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc index 63f9150243..806b418516 100644 --- a/Examples/Stereo/stereo_tum_vi.cc +++ b/Examples/Stereo/stereo_tum_vi.cc @@ -210,7 +210,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc b/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc index d375db3a12..9b17b9d587 100644 --- a/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples_old/Monocular-Inertial/mono_inertial_euroc.cc @@ -235,7 +235,7 @@ int main(int argc, char *argv[]) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc index 0487259bf2..56d8df468e 100644 --- a/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc +++ b/Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc @@ -254,7 +254,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular/mono_euroc.cc b/Examples_old/Monocular/mono_euroc.cc index 63f571d55a..92f587a516 100644 --- a/Examples_old/Monocular/mono_euroc.cc +++ b/Examples_old/Monocular/mono_euroc.cc @@ -190,7 +190,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Monocular/mono_tum_vi.cc b/Examples_old/Monocular/mono_tum_vi.cc index 448e268125..b0e6e89674 100644 --- a/Examples_old/Monocular/mono_tum_vi.cc +++ b/Examples_old/Monocular/mono_tum_vi.cc @@ -202,7 +202,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc index 0d90988d82..b5a5506980 100644 --- a/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc +++ b/Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc @@ -304,7 +304,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc index 11993740cc..24d100200a 100644 --- a/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc +++ b/Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc @@ -260,7 +260,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo/stereo_euroc.cc b/Examples_old/Stereo/stereo_euroc.cc index 48029924e6..6e1fe0c041 100644 --- a/Examples_old/Stereo/stereo_euroc.cc +++ b/Examples_old/Stereo/stereo_euroc.cc @@ -259,7 +259,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } diff --git a/Examples_old/Stereo/stereo_tum_vi.cc b/Examples_old/Stereo/stereo_tum_vi.cc index 60b90248e3..e6f76dd824 100644 --- a/Examples_old/Stereo/stereo_tum_vi.cc +++ b/Examples_old/Stereo/stereo_tum_vi.cc @@ -211,7 +211,7 @@ int main(int argc, char **argv) if (bFileName) { const string kf_file = string(argv[argc-1]) + "_keyframes.txt"; - const string f_file = string(argv[argc-1]) + "frames.txt"; + const string f_file = string(argv[argc-1]) + "_frames.txt"; SLAM.SaveTrajectoryEuRoC(f_file); SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file); } From f42ac116188a7dabb1e3993e8d3fcc89c5773694 Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:12:13 -0800 Subject: [PATCH 11/13] add faster matching method --- evaluation/associate.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/evaluation/associate.py b/evaluation/associate.py index 3ac642941c..e4f8b1a392 100644 --- a/evaluation/associate.py +++ b/evaluation/associate.py @@ -102,6 +102,24 @@ def associate(first_list, second_list,offset,max_difference): matches.sort() return matches +def associate2(first_time_list, second_time_list, offset, max_difference): + """ + Associate two timestamp by finding the closest element in the second list to each element in the first list given some restirction max_difference + + Output: + matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) + + """ + first_time_list_numpy = numpy.array(first_time_list) + second_time_list_numpy = numpy.array(second_time_list) + match_first_indicies = numpy.array(list(range(len(first_time_list)))) + match_second_indicies = numpy.array([numpy.argmin(abs(a - (second_time_list_numpy + offset))) for a in first_time_list]) + match_time_diff = numpy.abs(first_time_list_numpy[match_first_indicies] - (second_time_list_numpy[match_second_indicies] + offset)) + match_time_first_list = list(first_time_list_numpy[match_first_indicies[numpy.where(match_time_diff < max_difference)[0]]]) + match_time_second_list = list(second_time_list_numpy[match_second_indicies[numpy.where(match_time_diff < max_difference)[0]]]) + + return match_time_first_list, match_time_second_list + if __name__ == '__main__': # parse command line From ab505061bacb43ef014234be84d8581bd402a3fd Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Sun, 19 Nov 2023 22:11:03 -0800 Subject: [PATCH 12/13] Update evaluate_ate_scale.py --- evaluation/evaluate_ate_scale.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index b76d20cb35..e08a8777c6 100644 --- a/evaluation/evaluate_ate_scale.py +++ b/evaluation/evaluate_ate_scale.py @@ -147,8 +147,8 @@ def plot_traj(ax,stamps,traj,style,color,label): # Dataset path dataset_path = './Datasets/euroc/MachineHall/' # Algorithms and subdatasets - algorithms = ['monocular', 'stereo', 'monocular-inertial', 'stereo-inertial'] - sub_datasets = ['MH01_to_MH05'] + algorithms = ['monocular', 'stereo'] + sub_datasets = ['MH01', 'MH02', 'MH03', 'MH04', 'MH05', 'MH01'] # Go through all algoirhtms and datasets for algorithm in algorithms: for sub_dataset in sub_datasets: @@ -162,11 +162,11 @@ def plot_traj(ax,stamps,traj,style,color,label): os.makedirs(evaluation_results_path) print('new directory is created for output') # save evaluation output - poses_associations_output_file = evaluation_results_path + '/poses_associations.txt' - time_associations_output_file = evaluation_results_path + '/time_associations.txt' - estimation_error_summary_output_file = evaluation_results_path + '/evaluation_error.txt' - plot_results_output_file = evaluation_results_path + '/trajectory_output.png' - plot_error_hist_output_file = evaluation_results_path + '/error_histogram.png' + poses_associations_output_file = evaluation_results_path + '/poses_associations_frames.txt' + time_associations_output_file = evaluation_results_path + '/time_associations_frames.txt' + estimation_error_summary_output_file = evaluation_results_path + '/evaluation_error_frames.txt' + plot_results_output_file = evaluation_results_path + '/trajectory_output_frames.png' + plot_error_hist_output_file = evaluation_results_path + '/error_histogram_frames.png' first_list = associate.read_file_list(first_file, False) second_list = associate.read_file_list(second_file, False) @@ -227,6 +227,7 @@ def plot_traj(ax,stamps,traj,style,color,label): ax.set_xlabel('x [m]') ax.set_ylabel('y [m]') plt.savefig(plot_results_output_file,format="png") + plt.close() position_error=first_xyz-second_xyz_aligned plt.clf() fig, ax = plt.subplots(3) @@ -237,3 +238,4 @@ def plot_traj(ax,stamps,traj,style,color,label): ax[2].set_xlabel('error [m]') fig.legend() plt.savefig(plot_error_hist_output_file,format="png") + plt.close() From d3ad2f11a68192011d7160ecd2e8ae7ce9e4f48f Mon Sep 17 00:00:00 2001 From: JustMohsen <53262801+JustMohsen@users.noreply.github.com> Date: Wed, 22 Nov 2023 15:27:12 -0800 Subject: [PATCH 13/13] Revert "Update System.cc" This reverts commit fceacd3039e0612b68dc0df651087e383287d582. --- src/System.cc | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/System.cc b/src/System.cc index 301f83cbc8..60d9c5185a 100644 --- a/src/System.cc +++ b/src/System.cc @@ -620,7 +620,7 @@ void System::SaveTrajectoryTUM(const string &filename) Eigen::Vector3f twc = Twc.translation(); Eigen::Quaternionf q = Twc.unit_quaternion(); - f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } f.close(); // cout << endl << "trajectory saved!" << endl; @@ -652,7 +652,7 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f t = Twc.translation(); f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t(0) << " " << t(1) << " " << t(2) - << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } @@ -759,14 +759,14 @@ void System::SaveTrajectoryEuRoC(const string &filename) Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } else { Sophus::SE3f Twc = ((*lit)*Trw).inverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f twc = Twc.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } // cout << "5" << endl; @@ -864,14 +864,14 @@ void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } else { Sophus::SE3f Twc = ((*lit)*Trw).inverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f twc = Twc.translation(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } // cout << "5" << endl; @@ -982,7 +982,7 @@ void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) Eigen::Vector3f twb = Twb.translation(); Eigen::Quaternionf q = Twb.unit_quaternion(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } else { @@ -991,7 +991,7 @@ void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) Eigen::Vector3f twc = Twc.translation(); Eigen::Quaternionf q = Twc.unit_quaternion(); - f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } // cout << "5" << endl; @@ -1098,7 +1098,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) Sophus::SE3f Twb = pKF->GetImuPose(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } else @@ -1106,7 +1106,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) Sophus::SE3f Twc = pKF->GetPoseInverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } } f.close(); @@ -1136,7 +1136,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap) Sophus::SE3f Twb = pKF->GetImuPose(); Eigen::Quaternionf q = Twb.unit_quaternion(); Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } else @@ -1144,7 +1144,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap) Sophus::SE3f Twc = pKF->GetPoseInverse(); Eigen::Quaternionf q = Twc.unit_quaternion(); Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } } f.close();