Fixed - Small edge case

When there are <1 imu messages at the end of integration this would break. Should just use the same measurement even if averaging.
leavesnight · Jul 25, 2018 · c14ca2c · c14ca2c
1 parent 17f9e2c
commit c14ca2c
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/cpi_compare/src/solvers/GraphSolver_IMU.cpp b/cpi_compare/src/solvers/GraphSolver_IMU.cpp
@@ -63,7 +63,7 @@ ImuFactorCPIv1 GraphSolver::createimufactor_cpi_v1(double updatetime, gtsam::Val
     // TODO: Clean this code, and use the mutex
     double dt_f = updatetime - imu_times.at(0);
     if (dt_f > 0) {
-        cpi.feed_IMU(imu_times.at(0), updatetime, imu_angvel.at(0), imu_linaccs.at(0), imu_angvel.at(1), imu_linaccs.at(1));
+        cpi.feed_IMU(imu_times.at(0), updatetime, imu_angvel.at(0), imu_linaccs.at(0), imu_angvel.at(0), imu_linaccs.at(0));
         imu_times.at(0) = updatetime;
         imucompound++;
     }
@@ -118,7 +118,7 @@ ImuFactorCPIv2 GraphSolver::createimufactor_cpi_v2(double updatetime, gtsam::Val
     // TODO: Clean this code, and use the mutex
     double dt_f = updatetime - imu_times.at(0);
     if (dt_f > 0) {
-        cpi.feed_IMU(imu_times.at(0), updatetime, imu_angvel.at(0), imu_linaccs.at(0), imu_angvel.at(1), imu_linaccs.at(1));
+        cpi.feed_IMU(imu_times.at(0), updatetime, imu_angvel.at(0), imu_linaccs.at(0), imu_angvel.at(0), imu_linaccs.at(0));
         imu_times.at(0) = updatetime;
         imucompound++;
     }