added a preintegrated velocity cost to steam-lo which seems to improv…

…e performance slightly using the accelerometer.
utiasASRL · Dec 5, 2023 · 8a1c0c6 · 8a1c0c6
1 parent 988d7ba
commit 8a1c0c6
Show file tree

Hide file tree

Showing 12 changed files with 52,426 additions and 29 deletions.
diff --git a/results/newer_college_dataset/steamlio/01_short_experiment_tum.txt b/results/newer_college_dataset/steamlio/01_short_experiment_tum.txt
diff --git a/results/newer_college_dataset/steamlio/02_long_experiment_tum.txt b/results/newer_college_dataset/steamlio/02_long_experiment_tum.txt
diff --git a/results/newer_college_dataset/steamlio/05_quad_with_dynamics_tum.txt b/results/newer_college_dataset/steamlio/05_quad_with_dynamics_tum.txt
diff --git a/results/newer_college_dataset/steamlio/06_dynamic_spinning_tum.txt b/results/newer_college_dataset/steamlio/06_dynamic_spinning_tum.txt
diff --git a/results/newer_college_dataset/steamlio/07_parkland_mound_tum.txt b/results/newer_college_dataset/steamlio/07_parkland_mound_tum.txt
diff --git a/...ts/newer_college_dataset/steamlio/steam_icp.obelisk.krb.log.WARNING.20231204-165513.12538 b/...ts/newer_college_dataset/steamlio/steam_icp.obelisk.krb.log.WARNING.20231204-165513.12538
@@ -0,0 +1,101 @@
+Log file created at: 2023/12/04 16:55:13
+Running on machine: obelisk
+Running duration (h:mm:ss): 0:00:00
+Log line format: [IWEF]yyyymmdd hh:mm:ss.uuuuuu threadid file:line] msg
+W20231204 16:55:13.534565 12538 main.cpp:840] Logging to /home/krb/ASRL/temp/steam_icp/newer_college_dataset/steamlio
+W20231204 16:55:13.535012 12538 main.cpp:120] Parameter suspend_on_failure = 0
+W20231204 16:55:13.535081 12538 main.cpp:121] Parameter output_dir = /home/krb/ASRL/temp/steam_icp/newer_college_dataset/steamlio
+W20231204 16:55:13.535174 12538 main.cpp:124] Parameter eval_only = 0
+W20231204 16:55:13.535250 12538 main.cpp:130] Parameter dataset = NewerCollege
+W20231204 16:55:13.535326 12538 main.cpp:134] Parameter dataset_options.all_sequences = 1
+W20231204 16:55:13.535394 12538 main.cpp:135] Parameter dataset_options.root_path = /workspace/raid/krb/newer_college_dataset
+W20231204 16:55:13.535461 12538 main.cpp:136] Parameter dataset_options.sequence = 06_dynamic_spinning
+W20231204 16:55:13.535548 12538 main.cpp:137] Parameter dataset_options.init_frame = 0
+W20231204 16:55:13.535614 12538 main.cpp:138] Parameter dataset_options.last_frame = 100000
+W20231204 16:55:13.535679 12538 main.cpp:139] Parameter dataset_options.min_dist_sensor_center = 0.1
+W20231204 16:55:13.535768 12538 main.cpp:140] Parameter dataset_options.max_dist_sensor_center = 100
+W20231204 16:55:13.535851 12538 main.cpp:141] Parameter dataset_options.lidar_timestamp_round = 0
+W20231204 16:55:13.535929 12538 main.cpp:142] Parameter dataset_options.lidar_timestamp_round_hz = 400
+W20231204 16:55:13.536002 12538 main.cpp:166] Parameter visualization_options.odometry = 1
+W20231204 16:55:13.536079 12538 main.cpp:167] Parameter visualization_options.raw_points = 0
+W20231204 16:55:13.536151 12538 main.cpp:168] Parameter visualization_options.sampled_points = 1
+W20231204 16:55:13.536222 12538 main.cpp:169] Parameter visualization_options.map_points = 1
+W20231204 16:55:13.536289 12538 main.cpp:178] (YAML)Parameter visualization_options.T_sr = 
+1 0 0 0
+0 1 0 0
+0 0 1 0
+0 0 0 1
+W20231204 16:55:13.536412 12538 main.cpp:187] Parameter odometry = STEAMLO
+W20231204 16:55:13.536494 12538 main.cpp:208] Parameter odometry_options.init_num_frames = 20
+W20231204 16:55:13.536566 12538 main.cpp:209] Parameter odometry_options.init_voxel_size = 0.2
+W20231204 16:55:13.536638 12538 main.cpp:210] Parameter odometry_options.voxel_size = 0.5
+W20231204 16:55:13.536722 12538 main.cpp:211] Parameter odometry_options.init_sample_voxel_size = 1
+W20231204 16:55:13.536792 12538 main.cpp:212] Parameter odometry_options.sample_voxel_size = 1.5
+W20231204 16:55:13.536875 12538 main.cpp:214] Parameter odometry_options.size_voxel_map = 1
+W20231204 16:55:13.536942 12538 main.cpp:215] Parameter odometry_options.min_distance_points = 0.1
+W20231204 16:55:13.537029 12538 main.cpp:216] Parameter odometry_options.max_num_points_in_voxel = 20
+W20231204 16:55:13.537091 12538 main.cpp:217] Parameter odometry_options.max_distance = 400
+W20231204 16:55:13.537171 12538 main.cpp:218] Parameter odometry_options.min_number_neighbors = 20
+W20231204 16:55:13.537237 12538 main.cpp:219] Parameter odometry_options.max_number_neighbors = 20
+W20231204 16:55:13.537315 12538 main.cpp:220] Parameter odometry_options.voxel_lifetime = 10
+W20231204 16:55:13.537379 12538 main.cpp:222] Parameter odometry_options.num_iters_icp = 10
+W20231204 16:55:13.537451 12538 main.cpp:223] Parameter odometry_options.threshold_orientation_norm = 0.0001
+W20231204 16:55:13.537520 12538 main.cpp:224] Parameter odometry_options.threshold_translation_norm = 0.001
+W20231204 16:55:13.537604 12538 main.cpp:225] Parameter odometry_options.min_number_keypoints = 100
+W20231204 16:55:13.537669 12538 main.cpp:227] Parameter odometry_options.debug_print = 1
+W20231204 16:55:13.537745 12538 main.cpp:228] Parameter odometry_options.debug_path = /home/krb/ASRL/temp/steam_icp/newer_college_dataset/steamlio
+W20231204 16:55:13.537834 12538 main.cpp:357] Parameter odometry_options.steam.T_sr = 
+0000000000-1 -3.58979e-09 000000000000 000-0.006253
+03.58979e-09 0000000000-1 000000000000 00000.011775
+000000000000 000000000000 000000000001 000-0.028535
+000000000000 000000000000 000000000000 000000000001
+W20231204 16:55:13.538018 12538 main.cpp:368] Parameter odometry_options.steam.qc_diag = 500 500 500 050 050 050
+W20231204 16:55:13.538118 12538 main.cpp:370] Parameter odometry_options.steam.num_extra_states = 0
+W20231204 16:55:13.538209 12538 main.cpp:372] Parameter odometry_options.steam.power_planarity = 2
+W20231204 16:55:13.538296 12538 main.cpp:373] Parameter odometry_options.steam.p2p_max_dist = 0.5
+W20231204 16:55:13.538374 12538 main.cpp:375] Parameter odometry_options.steam.p2p_loss_func = L2
+W20231204 16:55:13.538466 12538 main.cpp:389] Parameter odometry_options.steam.p2p_loss_sigma = 0.1
+W20231204 16:55:13.538543 12538 main.cpp:391] Parameter odometry_options.steam.use_rv = 0
+W20231204 16:55:13.538609 12538 main.cpp:392] Parameter odometry_options.steam.merge_p2p_rv = 0
+W20231204 16:55:13.538687 12538 main.cpp:393] Parameter odometry_options.steam.rv_max_error = 2
+W20231204 16:55:13.538759 12538 main.cpp:395] Parameter odometry_options.steam.rv_loss_func = 
+W20231204 16:55:13.538794 12538 main.cpp:405] Parameter odometry_options.steam.rv_loss_func not specified. Using default value: CAUCHY
+W20231204 16:55:13.538847 12538 main.cpp:409] Parameter odometry_options.steam.rv_cov_inv = 0.1
+W20231204 16:55:13.538921 12538 main.cpp:410] Parameter odometry_options.steam.rv_loss_threshold = 0.05
+W20231204 16:55:13.539000 12538 main.cpp:412] Parameter odometry_options.steam.verbose = 1
+W20231204 16:55:13.539074 12538 main.cpp:413] Parameter odometry_options.steam.max_iterations = 10
+W20231204 16:55:13.539149 12538 main.cpp:414] Parameter odometry_options.steam.num_threads = 16
+W20231204 16:55:13.539222 12538 main.cpp:416] Parameter odometry_options.steam.delay_adding_points = 1
+W20231204 16:55:13.539288 12538 main.cpp:417] Parameter odometry_options.steam.use_final_state_value = 0
+W20231204 16:55:13.539366 12538 main.cpp:418] Parameter odometry_options.steam.use_imu = 1
+W20231204 16:55:13.539430 12538 main.cpp:419] Parameter odometry_options.steam.p0_bias_gyro = 0.01
+W20231204 16:55:13.539512 12538 main.cpp:420] Parameter odometry_options.steam.q_bias_gyro = 0.00016
+W20231204 16:55:13.539588 12538 main.cpp:427] Parameter odometry_options.steam.r_imu_ang = 0.004326 0.004326 0.004326
+W20231204 16:55:13.539688 12538 main.cpp:854] Creating directory /home/krb/ASRL/temp/steam_icp/newer_college_dataset/steamlio/
+W20231204 16:55:13.963835 12538 main.cpp:864] Running odometry on sequence: 01_short_experiment
+W20231204 18:07:08.912904 12538 main.cpp:1056] Average loading ..................... 8.27325 ms
+W20231204 18:07:08.912962 12538 main.cpp:1056] Average registration ................ 132.883 ms
+W20231204 18:07:08.912976 12538 main.cpp:1056] Average visualization ............... 140.613 ms
+W20231204 18:07:12.927845 12538 main.cpp:1066] Mean RPE : 49.1234
+W20231204 18:07:12.927934 12538 main.cpp:1067] Mean RPE 2D : 49.014
+W20231204 18:07:12.927948 12538 main.cpp:1068] Mean APE : 190.759
+W20231204 18:07:12.927958 12538 main.cpp:1069] Max APE : 336.599
+W20231204 18:07:12.927968 12538 main.cpp:1070] Mean Local Error : 0.0284364
+W20231204 18:07:12.927979 12538 main.cpp:1071] Max Local Error : 0.238053
+W20231204 18:07:12.927989 12538 main.cpp:1072] Index Max Local Error : 14613
+W20231204 18:07:12.927999 12538 main.cpp:1074] KITTI Summary : 49.01 & 0.0054 & 49.12 & 0.0283\\
+W20231204 18:07:12.928010 12538 main.cpp:1080] 
+W20231204 18:07:16.854820 12538 main.cpp:864] Running odometry on sequence: 02_long_experiment
+W20231204 18:30:11.673213 12538 main.cpp:1018] Shutting down due to ctrl-c.
+W20231204 18:30:11.673303 12538 main.cpp:1021] Average loading ..................... 11.407 ms
+W20231204 18:30:11.673352 12538 main.cpp:1021] Average registration ................ 112.586 ms
+W20231204 18:30:11.673382 12538 main.cpp:1021] Average visualization ............... 31.6966 ms
+W20231204 18:30:14.005360 12538 main.cpp:1027] Mean RPE : 24.1657
+W20231204 18:30:14.005765 12538 main.cpp:1028] Mean RPE 2D : 24.0474
+W20231204 18:30:14.005781 12538 main.cpp:1029] Mean APE : 68.8002
+W20231204 18:30:14.005810 12538 main.cpp:1030] Max APE : 160.041
+W20231204 18:30:14.005821 12538 main.cpp:1031] Mean Local Error : 0.0258589
+W20231204 18:30:14.005832 12538 main.cpp:1032] Max Local Error : 0.310537
+W20231204 18:30:14.005842 12538 main.cpp:1033] Index Max Local Error : 7239
+W20231204 18:30:14.005851 12538 main.cpp:1035] KITTI Summary : 24.05 & 0.0113 & 24.17 & 0.0487\\
+W20231204 18:30:14.005864 12538 main.cpp:1041] 
diff --git a/steam_icp/config/ncd_steamlio_config.yaml b/steam_icp/config/ncd_steamlio_config.yaml
@@ -29,12 +29,18 @@
       debug_print: true
       debug_path: /home/krb/ASRL/temp/steam_icp/newer_college_dataset/steamlio
       num_iters_icp: 10
+      init_voxel_size: 1.0
+      voxel_size: 1.0
+      init_sample_voxel_size: 1.0
+      sample_voxel_size: 1.0
       # sample_voxel_size: 1.0
       # min_distance_points: 0.05
       # max_num_points_in_voxel: 25
       # min_number_neighbors: 25
       # max_number_neighbors: 25
       # voxel_lifetime: 100
+      min_distance_points: 0.05
+      max_num_points_in_voxel: 100
 
       elastic:
         num_threads: 20

diff --git a/steam_icp/config/ncd_steamlo_config.yaml b/steam_icp/config/ncd_steamlo_config.yaml
@@ -13,9 +13,9 @@
     # DATASET OPTIONS ----
     dataset: NewerCollege
     dataset_options:
-      all_sequences: false
+      all_sequences: true
       root_path: /workspace/raid/krb/newer_college_dataset
-      sequence: "06_dynamic_spinning" #"boreas-2021-09-02-11-42"
+      sequence: "05_quad_with_dynamics" #"boreas-2021-09-02-11-42"
       init_frame: 0 # 5675 (highway 7 front image)
       last_frame: 100000 # 5680 (highway 7 front image)
       min_dist_sensor_center: 0.1
@@ -29,12 +29,18 @@
       debug_print: true
       debug_path: /home/krb/ASRL/temp/steam_icp/newer_college_dataset/steamlio
       num_iters_icp: 10
+      # init_voxel_size: 0.5
+      # voxel_size: 0.5
+      # init_sample_voxel_size: 0.5
+      # sample_voxel_size: 0.5
       # sample_voxel_size: 1.0
-      # min_distance_points: 0.05
-      # max_num_points_in_voxel: 25
       # min_number_neighbors: 25
       # max_number_neighbors: 25
       # voxel_lifetime: 100
+      # map:
+      # min_distance_points: 0.05
+      # max_num_points_in_voxel: 100
+      max_distance: 400.0
 
       elastic:
         num_threads: 20
@@ -48,40 +54,46 @@
         p0_vel: [1.0e-0, 1.0e-0, 1.0e-0, 1.0e-0, 1.0e-0, 1.0e-0]
         p0_accel: [1.0e-0, 1.0e-0, 1.0e-0, 1.0e-0, 1.0e-0, 1.0e-0]
         # ****************
+        p2p_loss_func: L2
+        num_extra_states: 0
 
         # qc_diag: [50.0,50.0,50.0,5.0,5.0,5.0]
         # ad_diag: [0.0,0.0,0.0,10.0,10.0,10.0]
         num_threads: 16
-        gravity: -9.81599  #-9.8042
+
         # r_imu_acc: [0.43000461, 0.33698219, 0.29589244]
         # r_imu_ang: [0.04898588, 0.06860401, 0.04319808]
         # r_imu_acc: [0.03418968, 0.01292128, 0.00767178]
         # r_imu_ang: [5.75866815e-06, 2.21540004e-05, 3.48544315e-06]
-        r_imu_acc: [0.00156, 0.00156, 0.00156]
-        r_imu_ang: [4.326e-03, 4.326e-03, 4.326e-03]
+        # r_imu_acc: [0.00156, 0.00156, 0.00156]
+
         # q_bias_accel: [1.0e-11, 4.0e-3, 1.0e-3]
-        q_bias_accel: [1.1236e-4, 1.1236e-4, 1.1236e-4]
-        p0_bias_accel: [1.0e-2, 1.0e-2, 1.0e-2]
+        # q_bias_accel: [1.1236e-4, 1.1236e-4, 1.1236e-4]
+        # p0_bias_accel: [1.0e-2, 1.0e-2, 1.0e-2]
+        r_imu_ang: [4.326e-03, 4.326e-03, 4.326e-03]
         q_bias_gyro: 1.6e-4
         p0_bias_gyro: 1.0e-2
         use_imu: true
+        use_accel: true
         max_iterations: 10
         verbose: true
         delay_adding_points: 1
-        qg_diag: [1.0e-6, 1.0e-6, 1.0e-6, 1.0e-7, 1.0e-7, 1.0e-6]  # motion model for T_mi
-        T_mi_init_only: false
-        T_mi_init_cov: [1.0e-4, 1.0e-4, 1.0e-4, 1.0e-6, 1.0e-6, 1.0e-8]
-        use_T_mi_prior_after_init: false
-        T_mi_prior_cov: [1.0e-2, 1.0e-2, 1.0e-3, 1.0, 1.0, 1.0e-4]
-        use_bias_prior_after_init: false
+
         pk_bias_accel: 1.0e-1  # prior after init
         pk_bias_gyro: 1.0e-1  # prior after init
         xi_ig: [0., 0., 0., -1.51869911e-02,  3.07697005e-02, 0.]  # sim: [0., 0., 0., -0.0197052, 0.0285345, 0.] 
-        p2p_loss_func: L2
-        num_extra_states: 0
-        acc_loss_func: CAUCHY
-        acc_loss_sigma: 1.0
+
         gyro_loss_func: L2
         gyro_loss_sigma: 1.0
         T_sr_vec: [ 0.01849613,  0.00982219, -0.028535  ,  0.        ,  0.        , 3.14159265]
-        # T_sr_vec: [ 0.,  0., 0.  ,  0.        ,  0.        , 0.]
+        # T_sr_vec: [ 0.,  0., 0.  ,  0.        ,  0.        , 0.]
+        r_imu_acc: [5.0e-02, 5.0e-02, 5.0e-02]
+        q_bias_accel: [1.1236e-1, 1.1236e-1, 1.1236e-1]
+        p0_bias_accel: [1.0e-3, 1.0e-3, 1.0e-3]
+        qg_diag: [1.0e-6, 1.0e-6, 1.0e-6, 1.0e-7, 1.0e-7, 1.0e-6]  # motion model for T_mi
+        T_mi_init_only: true
+        T_mi_init_cov: [1.0e-2, 1.0e-2, 1.0e-2, 1.0e-2, 1.0e-2, 1.0e-2]
+        T_mi_prior_cov: [1.0e-2, 1.0e-2, 1.0e-3, 1.0, 1.0, 1.0e-4]
+        acc_loss_func: L2
+        acc_loss_sigma: 1.0
+        gravity: -9.81599  #-9.8042
diff --git a/steam_icp/include/steam_icp/odometry/steam_lio.hpp b/steam_icp/include/steam_icp/odometry/steam_lio.hpp
@@ -48,7 +48,7 @@ class SteamLioOdometry : public Odometry {
     Eigen::Matrix<double, 3, 1> r_imu_acc = Eigen::Matrix<double, 3, 1>::Zero();
     Eigen::Matrix<double, 3, 1> r_imu_ang = Eigen::Matrix<double, 3, 1>::Zero();
     Eigen::Matrix<double, 6, 1> r_pose = Eigen::Matrix<double, 6, 1>::Zero();
-    Eigen::Matrix<double, 3, 1> p0_bias_accel = Eigen::Matrix<double, 3, 1>::Zero();
+    Eigen::Matrix<double, 3, 1> p0_bias_accel = Eigen::Matrix<double, 3, 1>::Ones();
     double pk_bias_accel = 0.0001;
     Eigen::Matrix<double, 3, 1> q_bias_accel = Eigen::Matrix<double, 3, 1>::Ones();
     double p0_bias_gyro = 0.0001;

diff --git a/steam_icp/include/steam_icp/odometry/steam_lo.hpp b/steam_icp/include/steam_icp/odometry/steam_lo.hpp
@@ -5,6 +5,7 @@
 #include "steam.hpp"
 #include "steam/problem/cost_term/imu_super_cost_term.hpp"
 #include "steam/problem/cost_term/p2p_const_vel_super_cost_term.hpp"
+#include "steam/problem/cost_term/preintegrated_accel_cost_term.hpp"
 #include "steam_icp/odometry.hpp"
 
 namespace steam_icp {
@@ -41,9 +42,20 @@ class SteamLoOdometry : public Odometry {
     bool use_final_state_value = false;
     // IMU
     bool use_imu = false;
+    bool use_accel = false;
     Eigen::Matrix<double, 3, 1> r_imu_ang = Eigen::Matrix<double, 3, 1>::Zero();
     double p0_bias_gyro = 0.0001;
     double q_bias_gyro = 0.0001;
+    // Accelerometer
+    double gravity = -9.8042;
+    Eigen::Matrix<double, 3, 1> r_imu_acc = Eigen::Matrix<double, 3, 1>::Zero();
+    Eigen::Matrix<double, 3, 1> p0_bias_accel = Eigen::Matrix<double, 3, 1>::Ones();
+    Eigen::Matrix<double, 3, 1> q_bias_accel = Eigen::Matrix<double, 3, 1>::Ones();
+    bool T_mi_init_only = true;
+    Eigen::Matrix<double, 6, 1> qg_diag = Eigen::Matrix<double, 6, 1>::Ones();
+    Eigen::Matrix<double, 6, 1> T_mi_init_cov = Eigen::Matrix<double, 6, 1>::Ones();
+    std::string acc_loss_func = "CAUCHY";
+    double acc_loss_sigma = 1.0;
   };
 
   SteamLoOdometry(const Options &options);
@@ -55,6 +67,7 @@ class SteamLoOdometry : public Odometry {
 
  private:
   void initializeTimestamp(int index_frame, const DataFrame &const_frame);
+  Eigen::Matrix<double, 6, 1> initialize_gravity(const std::vector<steam::IMUData> &imu_data_vec);
   void initializeMotion(int index_frame);
   std::vector<Point3D> initializeFrame(int index_frame, const std::vector<Point3D> &const_frame);
   void updateMap(int index_frame, int update_frame);
@@ -69,12 +82,14 @@ class SteamLoOdometry : public Odometry {
   // trajectory variables
   struct TrajectoryVar {
     TrajectoryVar(const steam::traj::Time &t, const steam::se3::SE3StateVar::Ptr &T,
-                  const steam::vspace::VSpaceStateVar<6>::Ptr &w, const steam::vspace::VSpaceStateVar<6>::Ptr &b)
-        : time(t), T_rm(T), w_mr_inr(w), imu_biases(b) {}
+                  const steam::vspace::VSpaceStateVar<6>::Ptr &w, const steam::vspace::VSpaceStateVar<6>::Ptr &b,
+                  const steam::se3::SE3StateVar::Ptr &T_m_i)
+        : time(t), T_rm(T), w_mr_inr(w), imu_biases(b), T_mi(T_m_i) {}
     steam::traj::Time time;
     steam::se3::SE3StateVar::Ptr T_rm;
     steam::vspace::VSpaceStateVar<6>::Ptr w_mr_inr;
     steam::vspace::VSpaceStateVar<6>::Ptr imu_biases;
+    steam::se3::SE3StateVar::Ptr T_mi;
   };
   std::vector<TrajectoryVar> trajectory_vars_;
   size_t to_marginalize_ = 0;