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..dab25af528 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; @@ -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..56d8df468e 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..b0e6e89674 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..297bcaf8b8 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..24d100200a 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 1250f2ee3c..6df8d07b85 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; seq0 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): """ @@ -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 @@ -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 diff --git a/evaluation/evaluate_ate_scale.py b/evaluation/evaluate_ate_scale.py index 3eb73ebcf7..e08a8777c6 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,13 @@ import sys import numpy -import argparse 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). @@ -82,21 +84,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) - trans = data.mean(1) - rot * model.mean(1) - - model_alignedGT = s*rot * model + transGT - model_aligned = rot * model + trans - - alignment_errorGT = model_alignedGT - data + s = float(dots/norms) + trans = data.mean(1) - s * rot * model.mean(1) + model_aligned = s * rot * model + trans 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): """ @@ -115,115 +109,133 @@ 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__": - # parse command line - 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)) - 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() - 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() - 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.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.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_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) - 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 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 args.save_associations: - file = open(args.save_associations,"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") - 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: - 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) - label="" + # print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed) + 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) + 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 + # Dataset path + dataset_path = './Datasets/euroc/MachineHall/' + # Algorithms and subdatasets + 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: + groundtruth_path = dataset_path + sub_dataset +'/mav0/state_groundtruth_estimate0/data.csv' + 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 + # 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_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) + + 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) - ax.legend() + 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.set_xlabel('x [m]') - ax.set_ylabel('y [m]') - plt.axis('equal') - plt.savefig(args.plot,format="pdf") - - - + 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)) + + 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: + fig = plt.figure() + 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") + ax.legend() + 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) + 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") + plt.close() 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