diff --git a/.gitignore b/.gitignore
index b5e0a7a67..fb1ed5e75 100644
--- a/.gitignore
+++ b/.gitignore
@@ -7,3 +7,4 @@ doxgen_generated
*.swp
*.swo
doc
+.vscode
diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox
index e6bcd1031..71327cca1 100644
--- a/docs/gs-installing.dox
+++ b/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
diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch
index c11c7b18d..e3da524a3 100644
--- a/ov_msckf/launch/serial.launch
+++ b/ov_msckf/launch/serial.launch
@@ -10,8 +10,9 @@
-
-
+
+
+
diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp
index e5403a1bd..9137a02da 100644
--- a/ov_msckf/src/run_simulation.cpp
+++ b/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 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);
diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp
index 106311883..2c1555e09 100644
--- a/ov_msckf/src/sim/Simulator.cpp
+++ b/ov_msckf/src/sim/Simulator.cpp
@@ -106,6 +106,9 @@ Simulator::Simulator(VioManagerOptions ¶ms_) {
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,9 +309,28 @@ 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 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);
@@ -316,19 +338,6 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto
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;
}
diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h
index 65ef0a4ee..05d0c5a05 100644
--- a/ov_msckf/src/sim/Simulator.h
+++ b/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 hist_true_bias_time;
std::vector hist_true_bias_accel;
std::vector hist_true_bias_gyro;
diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp
index 52f5730d3..b0ebf4f1a 100644
--- a/ov_msckf/src/state/Propagator.cpp
+++ b/ov_msckf/src/state/Propagator.cpp
@@ -94,6 +94,7 @@ void Propagator::propagate_and_clone(std::shared_ptr 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 last_w = Eigen::Matrix::Zero();
@@ -237,8 +238,8 @@ std::vector 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 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;
}