v02

tum_vi/eval.py

@@ -31,7 +31,7 @@ def absoluteOrientationMatrix(D, M):
     if(np.linalg.det(u) * np.linalg.det(vh)<0):
         I[2,2] = -1
     R = u @ I @ vh 
-#     R = R.T
+    R = R.T
     
     # Output of rotmodel has to be the vector. 
     # Comment with reference (2).
@@ -58,10 +58,29 @@ def absoluteOrientationMatrix(D, M):
     return R, T, T_GT, s
 
 
-def find_closest_times(res, gt, difference):
+def find_closest_times(res, gt, difference, time_measure="nano"):
+    '''
+    :param res:
+    :param gt:
+    :param difference:
+    :param time_measure:
+    :return:
+    '''
+
     gt_adapted = pd.DataFrame({"time[s]":[0], "tx":[0], "ty":[0], "tz":[0], "qx":[0], "qy":[0], "qz":[0], "qw":[0]})
     gt_adapted = gt_adapted.drop(0)
 
+    if time_measure == "milli":
+        res["time[s]"] = res["time[s]"] * 1000000
+    elif time_measure == "s":
+        res["time[s]"] = res["time[s]"] * 1000000000
+    elif time_measure == "nano":
+        pass
+
+    print(res["time[s]"])
+    print()
+    print(gt["# timestamp[ns]"])
+
     res = res.rename(columns={"x":"tx", "y":"ty", "z":"tz"})
     res_initial = res.copy()
     res = res_initial[0:0]
@@ -105,7 +124,9 @@ def plot_and_save(M, D, name):
     plot = plt.figure(figsize=(10,10))
     ax1 = plot.add_subplot(1, 1, 1)
     ax1.grid()
-    # ax1.set_aspect("equal")
+    ax1.set_xlabel('x [m]')
+    ax1.set_ylabel('y [m]')
+    #ax1.set_aspect("equal")
     ax1.plot(M[:, 0], M[:, 1], label="estimated")
     ax1.plot(D[:, 0],  D[:, 1], label="ground truth")
     plot.legend()
@@ -129,6 +150,9 @@ if __name__ == "__main__":
     parser.add_argument("--switch_gt", help="Use --switch_gt=0 to use first format or --switch_gt=1 to use second format", default=0)
     parser.add_argument("--slam_name", help="the name of the slam used for the processing. ORB-SLAM3 by default", default="orb_slam3")
     parser.add_argument("--savefile_folder", help="absolute or relative path to save the file", default=".")
+    parser.add_argument("--res_measure", help="time measurement of the resulting text file.", default="nano")
+    parser.add_argument("--separator", help="separator used in the estimated values file ", default=" ")
+    parser.add_argument("--sensor", help="lol no help. ahahhah")
 
     args = parser.parse_args()
     first_file = args.first_file
@@ -136,7 +160,9 @@ if __name__ == "__main__":
     second_file = args.second_file
     
     gt = read_file(first_file, switcher=switcher)
-    res = read_file(second_file, sep_=" ")
+    # TODO: Adapt for sep = ","
+    sep = args.separator
+    res = read_file(second_file, sep_=sep)
     
     tmp = gt["qw"]
     del gt["qw"]
@@ -145,8 +171,12 @@ if __name__ == "__main__":
     
     # by default is 20 mlns of ns
     difference = 20000000
-    res, gt_adapted = find_closest_times(res, gt, difference)
-    
+    time_measure = args.res_measure
+    if time_measure:
+        res, gt_adapted = find_closest_times(res, gt, difference, time_measure)
+    else:
+        res, gt_adapted = find_closest_times(res, gt, difference)
+
     D = gt_adapted[["tx", "ty", "tz"]].to_numpy()
     M = res[["tx", "ty", "tz"]].to_numpy()
     
@@ -158,13 +188,13 @@ if __name__ == "__main__":
     error = ATE_error(D, M_aligned)
     
     folder = args.savefile_folder
-    f = open(folder + "/" + args.slam_name + "_errors.txt", "w")
+    f = open(folder + "/" + args.slam_name + "_" + args.sensor + "_errors.txt", "w")
     f.write("RMSError using scaling parameter: " + str(error_GT) + " m.")
     f.write("\n")
     f.write("RMSError without scaling parameter: " + str(error) + " m.")
     f.close()
     
-    plot_and_save(M_aligned, D, folder + "/" + args.slam_name + "_trajectory.png")
-    plot_and_save(M_GT_aligned, D, folder + "/" + args.slam_name + "_trajectory_GT.png")
+    plot_and_save(M_aligned, D, folder + "/" + args.slam_name + "_" + args.sensor + "_trajectory.png")
+    plot_and_save(M_GT_aligned, D, folder + "/" + args.slam_name + "_" + args.sensor + "_trajectory_GT.png")
     
     print("End of processing. Success! Check the created plots and files with data")