fix first imu reading not getting passed in simulation

.gitignore

@@ -7,3 +7,4 @@ doxgen_generated
 *.swp
 *.swo
 doc
+.vscode

docs/gs-installing.dox

@@ -192,7 +192,7 @@ You can disable this with `catkin build -DENABLE_ARUCO_TAGS=OFF` or `cmake -DENA
 
 
 
-@subsection gs-install-ceres Ceres Solver
+@section gs-install-ceres Ceres Solver
 
 Ceres solver @cite ceres-solver is required for dynamic initialization and backend optimization.
 Please refer to their [documentation](http://ceres-solver.org/installation.html#linux) for specifics to your platform.
@@ -205,6 +205,8 @@ To install we can perform the following:
 @par Ceres Source Installation
     Try to first build with your system with `sudo apt-get install libceres-dev`.
     Only fall back onto this if it does not allow you to compile, or want a newer version!
+    You will need to build from source if there is an Eigen miss-match:
+    "Failed to find Ceres - Found Eigen dependency, but the version of Eigen found (3.3.7) does not exactly match the version of Eigen Ceres was compiled with (3.3.4)."
 
 @code{.shell-session}
 sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev

ov_msckf/launch/serial.launch

@@ -10,8 +10,9 @@
     <arg name="use_stereo"  default="true" />
     <arg name="bag_start"   default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
     <arg name="dataset"     default="V1_02_medium" /> <!-- V1_01_easy, V1_02_medium, V2_02_medium -->
-    <arg name="bag"         default="/media/patrick/RPNG FLASH 3/$(arg config)/$(arg dataset).bag" />
-<!--    <arg name="bag"         default="/datasets/$(arg config)/$(arg dataset).bag" />-->
+    <!-- <arg name="bag"         default="/media/patrick/RPNG FLASH 3/$(arg config)/$(arg dataset).bag" /> -->
+    <arg name="bag"         default="/home/patrick/datasets/$(arg config)/$(arg dataset).bag" />
+    <!-- <arg name="bag"         default="/datasets/$(arg config)/$(arg dataset).bag" /> -->
 
     <!-- saving trajectory path and timing information -->
     <arg name="dosave"      default="false" />

ov_msckf/src/run_simulation.cpp

@@ -111,8 +111,10 @@ int main(int argc, char **argv) {
   //===================================================================================
 
   // Get initial state
+  // NOTE: we are getting it at the *next* timestep so we get the first IMU message
+  double next_imu_time = sim->current_timestamp() + 1.0 / params.sim_freq_imu;
   Eigen::Matrix<double, 17, 1> imustate;
-  bool success = sim->get_state(sim->current_timestamp(), imustate);
+  bool success = sim->get_state(next_imu_time, imustate);
   if (!success) {
     PRINT_ERROR(RED "[SIM]: Could not initialize the filter to the first state\n" RESET);
     PRINT_ERROR(RED "[SIM]: Did the simulator load properly???\n" RESET);

ov_msckf/src/sim/Simulator.cpp

@@ -106,6 +106,9 @@ Simulator::Simulator(VioManagerOptions &params_) {
   hist_true_bias_time.push_back(timestamp_last_imu);
   hist_true_bias_accel.push_back(true_bias_accel);
   hist_true_bias_gyro.push_back(true_bias_gyro);
+  hist_true_bias_time.push_back(timestamp_last_imu + 1.0 / params.sim_freq_imu);
+  hist_true_bias_accel.push_back(true_bias_accel);
+  hist_true_bias_gyro.push_back(true_bias_gyro);
 
   // Our simulation is running
   is_running = true;
@@ -306,29 +309,35 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto
   gravity << 0.0, 0.0, params.gravity_mag;
   Eigen::Vector3d accel_inI = R_GtoI * (a_IinG + gravity);
 
-  // Now add noise to these measurements
+  // Calculate the bias values for this IMU reading
+  // NOTE: we skip the first ever bias since we have already appended it
   double dt = 1.0 / params.sim_freq_imu;
   std::normal_distribution<double> w(0, 1);
+  if (has_skipped_first_bias) {
+
+    // Move the biases forward in time
+    true_bias_gyro(0) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu);
+    true_bias_gyro(1) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu);
+    true_bias_gyro(2) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu);
+    true_bias_accel(0) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu);
+    true_bias_accel(1) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu);
+    true_bias_accel(2) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu);
+
+    // Append the current true bias to our history
+    hist_true_bias_time.push_back(timestamp_last_imu);
+    hist_true_bias_gyro.push_back(true_bias_gyro);
+    hist_true_bias_accel.push_back(true_bias_accel);
+  }
+  has_skipped_first_bias = true;
+
+  // Now add noise to these measurements
   wm(0) = omega_inI(0) + true_bias_gyro(0) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu);
   wm(1) = omega_inI(1) + true_bias_gyro(1) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu);
   wm(2) = omega_inI(2) + true_bias_gyro(2) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu);
   am(0) = accel_inI(0) + true_bias_accel(0) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu);
   am(1) = accel_inI(1) + true_bias_accel(1) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu);
   am(2) = accel_inI(2) + true_bias_accel(2) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu);
 
-  // Move the biases forward in time
-  true_bias_gyro(0) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu);
-  true_bias_gyro(1) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu);
-  true_bias_gyro(2) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu);
-  true_bias_accel(0) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu);
-  true_bias_accel(1) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu);
-  true_bias_accel(2) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu);
-
-  // Append the current true bias to our history
-  hist_true_bias_time.push_back(timestamp_last_imu);
-  hist_true_bias_gyro.push_back(true_bias_gyro);
-  hist_true_bias_accel.push_back(true_bias_accel);
-
   // Return success
   return true;
 }

ov_msckf/src/sim/Simulator.h

@@ -199,6 +199,7 @@ class Simulator {
   Eigen::Vector3d true_bias_gyro = Eigen::Vector3d::Zero();
 
   // Our history of true biases
+  bool has_skipped_first_bias = false;
   std::vector<double> hist_true_bias_time;
   std::vector<Eigen::Vector3d> hist_true_bias_accel;
   std::vector<Eigen::Vector3d> hist_true_bias_gyro;

ov_msckf/src/state/Propagator.cpp

@@ -94,6 +94,7 @@ void Propagator::propagate_and_clone(std::shared_ptr<State> state, double timest
       dt_summed += prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp;
     }
   }
+  assert(std::abs((time1 - time0) - dt_summed) < 1e-4);
 
   // Last angular velocity (used for cloning when estimating time offset)
   Eigen::Matrix<double, 3, 1> last_w = Eigen::Matrix<double, 3, 1>::Zero();
@@ -237,8 +238,8 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
     if (imu_data.at(i + 1).timestamp > time0 && imu_data.at(i).timestamp < time0) {
       ov_core::ImuData data = Propagator::interpolate_data(imu_data.at(i), imu_data.at(i + 1), time0);
       prop_data.push_back(data);
-      // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n",
-      // (int)i,data.timestamp-prop_data.at(0).timestamp,time0-prop_data.at(0).timestamp);
+      // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n", (int)i, data.timestamp - prop_data.at(0).timestamp,
+      //             time0 - prop_data.at(0).timestamp);
       continue;
     }
 
@@ -247,7 +248,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
     // Then we should just append the whole measurement time to our propagation vector
     if (imu_data.at(i).timestamp >= time0 && imu_data.at(i + 1).timestamp <= time1) {
       prop_data.push_back(imu_data.at(i));
-      // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp);
+      // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n", (int)i, imu_data.at(i).timestamp - prop_data.at(0).timestamp);
       continue;
     }