Merge pull request #321 from rpng/fast_propagate

cache our fast propagate values so we don't reintegrate
rpng · Apr 15, 2023 · b9c2509 · b9c2509
2 parents d03e53f + b397178
commit b9c2509
Show file tree

Hide file tree

Showing 17 changed files with 332 additions and 227 deletions.
diff --git a/config/rpng_sim/kalibr_imucam_chain.yaml b/config/rpng_sim/kalibr_imucam_chain.yaml
@@ -25,4 +25,30 @@ cam1:
   distortion_model: radtan
   intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
   resolution: [752, 480]
-  rostopic: /cam1/image_raw
+  rostopic: /cam1/image_raw
+cam2:
+  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
+    - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975]
+    - [0.999557249008, 0.0149672133247, 0.025715529948, 0.124676986768]
+    - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [1]
+  camera_model: pinhole
+  distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
+  distortion_model: radtan
+  intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
+  resolution: [752, 480]
+  rostopic: /cam2/image_raw
+cam3:
+  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
+    - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556]
+    - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.2253689425024]
+    - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [0]
+  camera_model: pinhole
+  distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05]
+  distortion_model: radtan
+  intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
+  resolution: [752, 480]
+  rostopic: /cam3/image_raw
diff --git a/ov_init/src/utils/helper.h b/ov_init/src/utils/helper.h
@@ -137,16 +137,15 @@ class InitializerHelper {
    */
   static void gram_schmidt(const Eigen::Vector3d &gravity_inI, Eigen::Matrix3d &R_GtoI) {
 
-    // Now gram schmidt to find rot for grav
-    // https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process
-    // Get z axis, which alines with -g (z_in_G=0,0,1)
+    // This will find an orthogonal vector to gravity which is our local z-axis
+    // We need to ensure we normalize after each one such that we obtain unit vectors
     Eigen::Vector3d z_axis = gravity_inI / gravity_inI.norm();
     Eigen::Vector3d x_axis, y_axis;
     Eigen::Vector3d e_1(1.0, 0.0, 0.0);
     Eigen::Vector3d e_2(0.0, 1.0, 0.0);
     double inner1 = e_1.dot(z_axis) / z_axis.norm();
     double inner2 = e_2.dot(z_axis) / z_axis.norm();
-    if (fabs(inner1) >= fabs(inner2)) {
+    if (fabs(inner1) < fabs(inner2)) {
       x_axis = z_axis.cross(e_1);
       x_axis = x_axis / x_axis.norm();
       y_axis = z_axis.cross(x_axis);
@@ -159,6 +158,7 @@ class InitializerHelper {
     }
 
     // Original method
+    // https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process
     // x_axis = e_1 - z_axis * z_axis.transpose() * e_1;
     // x_axis = x_axis / x_axis.norm();
     // y_axis = ov_core::skew_x(z_axis) * x_axis;

diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch
@@ -17,6 +17,10 @@
     <arg name="num_slam"    default="50" />
     <arg name="num_pts"     default="100" />
     <arg name="max_cameras" default="2" />
+    <arg name="use_stereo"  default="true" />
+
+    <arg name="feat_dist_min" default="5.0" />
+    <arg name="feat_dist_max" default="7.0" />
 
     <arg name="freq_cam"    default="10" />
     <arg name="freq_imu"    default="400" />
@@ -57,6 +61,9 @@
         <param name="sim_freq_imu"           type="int"    value="$(arg freq_imu)" />
         <param name="sim_do_perturbation"    type="bool"   value="$(arg sim_do_perturbation)" />
 
+        <param name="sim_min_feature_gen_dist" type="double" value="$(arg feat_dist_min)" />
+        <param name="sim_max_feature_gen_dist" type="double" value="$(arg feat_dist_max)" />
+
         <param name="save_total_state"       type="bool"   value="$(arg dosave_state)" />
         <param name="filepath_est"           type="str"    value="$(arg path_state_est)" />
         <param name="filepath_std"           type="str"    value="$(arg path_state_std)" />
@@ -77,6 +84,7 @@
         <param name="calib_cam_timeoffset"   type="bool"   value="$(arg sim_do_calibration)" />
         <param name="max_clones"             type="int"    value="$(arg num_clones)" />
         <param name="max_slam"               type="int"    value="$(arg num_slam)" />
+        <param name="use_stereo"             type="bool"   value="$(arg use_stereo)" />
         <param name="max_cameras"            type="int"    value="$(arg max_cameras)" />
         <param name="feat_rep_msckf"         type="str"    value="$(arg feat_rep)" />
         <param name="feat_rep_slam"          type="str"    value="$(arg feat_rep)" />

diff --git a/ov_msckf/scripts/run_ros_eth.sh b/ov_msckf/scripts/run_ros_eth.sh
@@ -46,19 +46,19 @@ bagstarttimes=(
   "5" # 18
 )
 
-
 # location to save log files into
-save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_euroc/algorithms"
-save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_euroc/timings"
-bag_path="/media/patrick/RPNG FLASH 3/euroc_mav/"
-ov_ver="2.6.2"
-
+save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_euroc/algorithms"
+save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_euroc/timings"
+bag_path="/home/patrick/datasets/euroc_mav/"
+ov_ver="2.6.3"
 
 
 #=============================================================
 #=============================================================
 #=============================================================
 
+big_start_time="$(date -u +%s)"
+
 # Loop through all datasets
 for i in "${!bagnames[@]}"; do
 # Loop through all modes
@@ -118,3 +118,12 @@ done
 done
 
 
+
+
+# print out the time elapsed
+big_end_time="$(date -u +%s)"
+big_elapsed="$(($big_end_time-$big_start_time))"
+echo "BASH: script took $big_elapsed seconds in total!!";
+
+
+
diff --git a/ov_msckf/scripts/run_ros_kaist.sh b/ov_msckf/scripts/run_ros_kaist.sh
@@ -35,17 +35,18 @@ bagstarttimes=(
 )
 
 # location to save log files into
-save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_kaist/algorithms"
-save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_kaist/timings"
-bag_path="/media/patrick/RPNG FLASH 3/kaist/"
-ov_ver="2.6.2"
-
+save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_kaist/algorithms"
+save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_kaist/timings"
+bag_path="/home/patrick/datasets/euroc_mav/"
+ov_ver="2.6.3"
 
 
 #=============================================================
 #=============================================================
 #=============================================================
 
+big_start_time="$(date -u +%s)"
+
 # Loop through all datasets
 for i in "${!bagnames[@]}"; do
 # Loop through all modes
@@ -105,3 +106,12 @@ done
 done
 
 
+
+# print out the time elapsed
+big_end_time="$(date -u +%s)"
+big_elapsed="$(($big_end_time-$big_start_time))"
+echo "BASH: script took $big_elapsed seconds in total!!";
+
+
+
+
diff --git a/ov_msckf/scripts/run_ros_kaistvio.sh b/ov_msckf/scripts/run_ros_kaistvio.sh
diff --git a/ov_msckf/scripts/run_ros_tumvi.sh b/ov_msckf/scripts/run_ros_tumvi.sh
@@ -37,17 +37,18 @@ bagstarttimes=(
 )
 
 # location to save log files into
-save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_tum/algorithms"
-save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_tum/timings"
-bag_path="/media/patrick/RPNG FLASH 3/tum_vi/"
-ov_ver="2.6.2"
-
+save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_tumvi/algorithms"
+save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_tumvi/timings"
+bag_path="/home/patrick/datasets/tum_vi/"
+ov_ver="2.6.3"
 
 
 #=============================================================
 #=============================================================
 #=============================================================
 
+big_start_time="$(date -u +%s)"
+
 # Loop through all datasets
 for i in "${!bagnames[@]}"; do
 # Loop through all modes
@@ -107,3 +108,10 @@ done
 done
 
 
+
+# print out the time elapsed
+big_end_time="$(date -u +%s)"
+big_elapsed="$(($big_end_time-$big_start_time))"
+echo "BASH: script took $big_elapsed seconds in total!!";
+
+
diff --git a/ov_msckf/scripts/run_ros_uzhfpv.sh b/ov_msckf/scripts/run_ros_uzhfpv.sh
@@ -80,17 +80,18 @@ bagstarttimes=(
 )
 
 # location to save log files into
-save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_uzhfpv/algorithms"
-save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_uzhfpv/timings"
-bag_path="/media/patrick/RPNG FLASH 3/"
-ov_ver="2.6.2"
-
+save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_uzhfpv/algorithms"
+save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_uzhfpv/timings"
+bag_path="/home/patrick/datasets/euroc_mav/"
+ov_ver="2.6.3"
 
 
 #=============================================================
 #=============================================================
 #=============================================================
 
+big_start_time="$(date -u +%s)"
+
 # Loop through all datasets
 for i in "${!bagnames[@]}"; do
 # Loop through all modes
@@ -150,3 +151,10 @@ done
 done
 
 
+
+# print out the time elapsed
+big_end_time="$(date -u +%s)"
+big_elapsed="$(($big_end_time-$big_start_time))"
+echo "BASH: script took $big_elapsed seconds in total!!";
+
+