initial commit

tum_vi/.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
+# Editor-based HTTP Client requests
+/httpRequests/

tum_vi/.idea/inspectionProfiles/Project_Default.xml

@@ -0,0 +1,15 @@
+<component name="InspectionProjectProfileManager">
+  <profile version="1.0">
+    <option name="myName" value="Project Default" />
+    <inspection_tool class="PyPackageRequirementsInspection" enabled="true" level="WARNING" enabled_by_default="true">
+      <option name="ignoredPackages">
+        <value>
+          <list size="2">
+            <item index="0" class="java.lang.String" itemvalue="opencv-python" />
+            <item index="1" class="java.lang.String" itemvalue="numpy" />
+          </list>
+        </value>
+      </option>
+    </inspection_tool>
+  </profile>
+</component>

tum_vi/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

tum_vi/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (base)" project-jdk-type="Python SDK" />
+</project>

tum_vi/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/tum_vi.iml" filepath="$PROJECT_DIR$/.idea/tum_vi.iml" />
+    </modules>
+  </component>
+</project>

tum_vi/.idea/other.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="PySciProjectComponent">
+    <option name="PY_SCI_VIEW_SUGGESTED" value="true" />
+  </component>
+</project>

tum_vi/.idea/tum_vi.iml

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="TestRunnerService">
+    <option name="PROJECT_TEST_RUNNER" value="pytest" />
+  </component>
+</module>

tum_vi/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/../../.." vcs="Git" />
+  </component>
+</project>

tum_vi/dataset-corridor4_512_16/orb_slam3_errors.txt

@@ -0,0 +1,2 @@
+RMSError using scaling parameter: 0.7408121394602898 m.
+RMSError without scaling parameter: 1.4654736451659267 m.

tum_vi/dataset-magistrale5_512_16/orb_slam3_errors.txt

@@ -0,0 +1,2 @@
+RMSError using scaling parameter: 0.9834596487833537 m.
+RMSError without scaling parameter: 3.4435383108456405 m.

tum_vi/dataset-outdoors4_512_16/orb_slam3_errors.txt

@@ -0,0 +1,2 @@
+RMSError using scaling parameter: 0.8071616904061958 m.
+RMSError without scaling parameter: 14.478852427250354 m.

tum_vi/dso_tum_vi_examples.sh

@@ -0,0 +1,51 @@
+#! /bin/bash
+
+echo " "
+echo "Script for running DSO on Monocular TUM-VI dataset."
+echo "--------------------------------------------------------"
+
+export DIR_DATASET=/home/ivan/ivan/git/work_drivecast2/SLAM/datasets/tum_mono_vi
+export DSO_BINARY_DIR=/home/ivan/ivan/git/work_drivecast2/SLAM/dso/build/bin
+#export DIR_CORRIDOR4="$DIR_DATASET"/dataset-corridor4_512_16/dso/cam0
+#export DIR_OUTDOOR4=$DIR_DATASET/dataset-outdoors4_512_16/dso/cam0
+
+function show_help {
+  echo "--------------------------------------------------------"
+	echo "Example of usage: -d=dataset-corridor4_512_16 or --dataset=dataset-outdoors4_512_16. They are used to as the
+	  folder names in tum-vi dataset folder. Output file doesn't change the name. Don't forget to rename and move the
+	  result before the next run."
+ }
+
+if [[ ($1 == -h || $1 == --help) ]] ; then
+  show_help
+  exit 0
+fi
+
+for i in "$@"; do
+  case "$i" in
+    -d=*|--dataset=*)
+      export DATASET="${i#*=}"
+      export DATASET_DIR="$DIR_DATASET"/"$DATASET"
+#      echo "$DATASET:"
+      shift
+      ;;
+    -*|--*)
+      echo "Unknown option $i"
+      show_help
+      exit 1
+      ;;
+    *)
+      echo "Invalid usage."
+      show_help
+      exit 1
+      ;;
+  esac
+done
+
+echo "$DATASET dataset starting..."
+"$DSO_BINARY_DIR"/dso_dataset files="$DATASET_DIR"/dso/cam0/images calib="$DATASET_DIR"/dso/cam0/camera.txt gamma="$DATASET_DIR"/dso/cam0/pcalib.txt vignette="$DATASET_DIR"/dso/cam0/vignette.png preset=0 mode=0
+
+export FILENAME="${DATASET}_512_mono_dso.txt"
+mv result.txt "$DATASET"/"$FILENAME"
+
+echo "Success! Check $DATASET directory for the output file "

tum_vi/eval.py

@@ -0,0 +1,170 @@
+import numpy as np
+import pandas as pd 
+from matplotlib import pyplot as plt
+import argparse
+import sys
+
+def absoluteOrientationMatrix(D, M):
+    dmean = D.mean(1)
+    mmean = M.mean(1)
+    
+    print("dmean is: ", dmean[:, np.newaxis])
+    print("mmean is: ", mmean[:, np.newaxis])
+
+    D_centr = D - dmean[:, np.newaxis]
+    M_centr = M - mmean[:, np.newaxis]
+    
+    print("D_centr is: ", D_centr)
+    print("M_centr is: ", M_centr)
+
+    # Comment with reference (1).
+    # If you don't understand what's written here, plese refer to the links and the code below
+    H = np.zeros(shape=(3, 3))
+    for column in range(D.shape[1]):
+        H += np.outer(D_centr[:, column],  M_centr[:, column])
+#         print("H is: ", H)
+    print("H is: ", H.T)
+    
+    u, s, vh = np.linalg.svd(H.T)
+    #R = vh @ u.T
+    I = np.identity(3)
+    if(np.linalg.det(u) * np.linalg.det(vh)<0):
+        I[2,2] = -1
+    R = u @ I @ vh 
+#     R = R.T
+    
+    # Output of rotmodel has to be the vector. 
+    # Comment with reference (2).
+    rotmodel = R @ M_centr
+#     print(rotmodel)
+    dots = 0.0
+    norms = 0.0
+
+    for column in range(D_centr.shape[1]):
+        dots += np.dot(D_centr[:,column].transpose(),rotmodel[:,column])
+        normi = np.linalg.norm(M_centr[:,column])
+        norms += normi*normi
+    
+    # s has to be the scalar.
+    s = float(dots/norms)
+    
+    T = dmean - R @ mmean
+    T_GT = dmean - s * R @ mmean
+
+    print("The Rotation matrix is: \n", R, "\n")
+    print("The Translation matrix is: \n", T, "\n")
+    print("The scale factor is: ", s)
+    
+    return R, T, T_GT, s
+
+
+def find_closest_times(res, gt, difference):
+    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)
+
+    res = res.rename(columns={"x":"tx", "y":"ty", "z":"tz"})
+    res_initial = res.copy()
+    res = res_initial[0:0]
+
+    # Последний момент времени в res неадкватный, поэтому уберем его: 
+    # res = res.drop(len(res)-1)
+    all_gt_times = gt["# timestamp[ns]"].to_numpy()
+    
+    for i in range(len(res_initial)):
+        time_res = res_initial.iloc[i][0]
+        compar_res = np.where(all_gt_times < time_res)
+    #     print(compar_res[0])
+        if not compar_res[0].shape[0] == 0:
+            gt_time = gt.iloc[compar_res[0][-1]][0]
+    #     print(abs(gt_time - time_res) < difference)
+        if ( (not compar_res[0].shape[0] == 0) and (abs(gt_time - time_res) < difference) ):
+            index = compar_res[0][-1]
+        else:
+            continue
+
+    #     print(index)
+        closest_row = gt.iloc[index]
+        closest_row = closest_row.rename({ "# timestamp[ns]":"time[s]" })
+        closest_row = closest_row.rename(i)
+    #     print(closest_row)
+        gt_adapted = gt_adapted.append(closest_row)
+        res = res.append(res_initial.iloc[i])
+    
+    return res, gt_adapted
+
+
+def ATE_error(D, M):
+    diff = D.T - M.T
+    print("alignmentGT resuls is: \n", diff)
+    trans_error1 = np.sqrt(np.sum(np.multiply(diff,diff), 0))
+    error1 = np.sqrt(np.dot(trans_error1, trans_error1)/len(trans_error1))
+    return error1
+
+
+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.plot(M[:, 0], M[:, 1], label="estimated")
+    ax1.plot(D[:, 0],  D[:, 1], label="ground truth")
+    plot.legend()
+    plot.savefig(name)
+
+
+def read_file(filename, sep_=",", switcher=0):
+    file = pd.read_csv(filename, sep=sep_)
+    if switcher == "1":
+        file = file.rename(columns={ "#timestamp [ns]":"# timestamp[ns]", " p_RS_R_x [m]":"tx", " p_RS_R_y [m]":"ty",
+                   " p_RS_R_z [m]":"tz", " q_RS_w []":"qw", " q_RS_x []":"qx", " q_RS_y []":"qy",
+                        " q_RS_z []":"qz" })
+    return file
+    
+
+if __name__ == "__main__":
+    
+    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[ns],tx,ty,tz,qw,qx,qy,qz) or (format: #timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []). Use --switch_gt=0 to use first format or --switch_gt=1 to use second format')
+    parser.add_argument("second_file", help="estimated trajectory (format: time[s] x y z qx qy qz qw)")
+    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=".")
+
+    args = parser.parse_args()
+    first_file = args.first_file
+    switcher = args.switch_gt
+    second_file = args.second_file
+    
+    gt = read_file(first_file, switcher=switcher)
+    res = read_file(second_file, sep_=" ")
+    
+    tmp = gt["qw"]
+    del gt["qw"]
+    gt["qw"] = tmp
+    gt
+    
+    # by default is 20 mlns of ns
+    difference = 20000000
+    res, gt_adapted = find_closest_times(res, gt, difference)
+    
+    D = gt_adapted[["tx", "ty", "tz"]].to_numpy()
+    M = res[["tx", "ty", "tz"]].to_numpy()
+    
+    R, T, T_GT, s = absoluteOrientationMatrix(D.T, M.T)
+    M_GT_aligned = (s * R @ M.T).T + T_GT
+    M_aligned = (R @ M.T).T + T
+    
+    error_GT = ATE_error(D, M_GT_aligned)
+    error = ATE_error(D, M_aligned)
+    
+    folder = args.savefile_folder
+    f = open(folder + "/" + args.slam_name + "_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")
+    
+    print("End of processing. Success! Check the created plots and files with data")

tum_vi/eval_data.sh

@@ -0,0 +1,30 @@
+#!/bin/bash
+
+PREFIX="/home/ivan/ivan/git/work_drivecast2/slam_algorithms_research/tum_vi"
+
+echo "Evaluation script for TUM-VI dataset. Don't forget to add the 'time[s] x y z qx qy qz qw' row to the top of the
+file which was obtained from ORB_SLAM3 or DSO"
+
+for i in "$@"; do
+    case $i in 
+        -p=*|--prefix=*)
+        PREFIX="{i#*=}"
+        shift
+        ;;
+    esac
+done
+
+#CORRIDOR4="dataset-corridor4_512_16"
+#echo "Evaluation of Corridor4 trajectory with Monocular sensor"
+#python eval.py "$PREFIX"/"$CORRIDOR4"/gt_imu.csv "$PREFIX"/"$CORRIDOR4"/f_dataset_corridor4_512_16_monoi_orb_slam3.txt --switch_gt=0 --slam_name="orb_slam3" --savefile_folder="$CORRIDOR4"
+
+#echo "---------------------------------------"
+#MAGISTRALE5="dataset-magistrale5_512_16"
+#echo "Evaluation of Magistrale 5 trajectory with Monocular sensor"
+#python eval.py "$PREFIX"/"$MAGISTRALE5"/gt_imu.csv "$PREFIX"/"$MAGISTRALE5"/f_dataset-magistrale5_512_monoi_orb_slam3.txt --switch_gt=0 --slam_name="orb_slam3" --savefile_folder="$MAGISTRALE5"
+
+echo "---------------------------------------"
+OUTDOORS4="dataset-outdoors4_512_16"
+echo "Evaluation of Outdoors 4 trajectory with Monocular sensor"
+python eval.py "$PREFIX"/"$OUTDOORS4"/gt_imu.csv "$PREFIX"/"$OUTDOORS4"/f_dataset-outdoors4_512_mono_orb_slam3.txt --switch_gt=0 --slam_name="orb_slam3" --savefile_folder="$OUTDOORS4"
+

tum_vi/orb_slam3_tum_vi_examples.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+pathDatasetTUM_VI='/home/ivan/ivan/git/work_drivecast2/SLAM/datasets/tum_mono_vi' #Example, it is necesary to change it by the dataset path
+orb_slam3_dir="/home/ivan/ivan/git/work_drivecast2/SLAM/ORB_SLAM3-1.0-release"
+#------------------------------------
+# Monocular Examples
+echo "Launching dataset-outdoors4_512_16 with Monocular sensor"
+./mono_tum_vi "$orb_slam3_dir"/Vocabulary/ORBvoc.txt "$orb_slam3_dir"/Examples/Monocular/TUM-VI.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data "$orb_slam3_dir"/Examples/Monocular/TUM_TimeStamps/dataset-outdoors4_512.txt dataset-outdoors4_512_mono