diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index 4efb1d3b7..e5fe73514 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -28,11 +28,11 @@ feat_rep_aruco: "GLOBAL_3D" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. try_zupt: false -zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_chi2_multipler: 1 # set to 0 for only disp-based zupt_max_velocity: 0.1 -zupt_noise_multiplier: 50 -zupt_max_disparity: 0.5 # set to 0 for only imu-based -zupt_only_at_beginning: true +zupt_noise_multiplier: 1 +zupt_max_disparity: 0 # set to 0 for only imu-based +zupt_only_at_beginning: false # ================================================================== # ================================================================== diff --git a/ov_core/src/feat/FeatureHelper.h b/ov_core/src/feat/FeatureHelper.h index 6e08aa0d9..9d5fe836b 100644 --- a/ov_core/src/feat/FeatureHelper.h +++ b/ov_core/src/feat/FeatureHelper.h @@ -121,7 +121,7 @@ class FeatureHelper { * @param oldest_time Only compute disparity for ones newer (-1 to disable) */ static void compute_disparity(std::shared_ptr db, double &disp_mean, double &disp_var, int &total_feats, - double newest_time = 1, double oldest_time = 1) { + double newest_time = -1, double oldest_time = -1) { // Compute the disparity std::vector disparities; diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 2edd36e4a..048f49f7d 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -10,7 +10,7 @@ - + diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 17d12667a..db7a40c01 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -189,6 +189,13 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector // Replace with the simulated tracker trackSIM = std::make_shared(state->_cam_intrinsics_cameras, state->_options.max_aruco_features); trackFEATS = trackSIM; + // Need to also replace it in init and zv-upt since it points to the trackFEATS db pointer + initializer = std::make_shared(params.init_options, trackFEATS->get_feature_database()); + if (params.try_zupt) { + updaterZUPT = std::make_shared(params.zupt_options, params.imu_noises, trackFEATS->get_feature_database(), + propagator, params.gravity_mag, params.zupt_max_velocity, + params.zupt_noise_multiplier, params.zupt_max_disparity); + } PRINT_WARNING(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET); } @@ -208,6 +215,7 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector did_zupt_update = updaterZUPT->try_update(state, timestamp); } if (did_zupt_update) { + trackDATABASE->cleanup_measurements(timestamp); return; } } @@ -281,6 +289,7 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const did_zupt_update = updaterZUPT->try_update(state, message.timestamp); } if (did_zupt_update) { + trackDATABASE->cleanup_measurements(message.timestamp); return; } } @@ -558,7 +567,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // First do anchor change if we are about to lose an anchor pose updaterSLAM->change_anchors(state); - // Cleanup any features older then the marginalization time + // Cleanup any features older than the marginalization time if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) { trackFEATS->get_feature_database()->cleanup_measurements(state->margtimestep()); trackDATABASE->cleanup_measurements(state->margtimestep()); diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index f55cb40c3..c30ecef63 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -176,16 +176,18 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest R *= _zupt_noise_multiplier; // Next propagate the biases forward in time - // NOTE: G*Qd*G^t = dt*Qd*dt = dt*Qc + // NOTE: G*Qd*G^t = dt*Qd*dt = dt*(1/dt*Qc)*dt = dt*Qc Eigen::MatrixXd Q_bias = Eigen::MatrixXd::Identity(6, 6); - Q_bias.block(0, 0, 3, 3) *= dt_summed * _noises.sigma_wb; - Q_bias.block(3, 3, 3, 3) *= dt_summed * _noises.sigma_ab; + Q_bias.block(0, 0, 3, 3) *= dt_summed * _noises.sigma_wb_2; + Q_bias.block(3, 3, 3, 3) *= dt_summed * _noises.sigma_ab_2; // Chi2 distance check - // NOTE: we also append the propagation we "would do before the update" if this was to be accepted + // NOTE: we also append the propagation we "would do before the update" if this was to be accepted (just the bias evolution) // NOTE: we don't propagate first since if we fail the chi2 then we just want to return and do normal logic Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order); - P_marg.block(3, 3, 6, 6) += Q_bias; + if (model_time_varying_bias) { + P_marg.block(3, 3, 6, 6) += Q_bias; + } Eigen::MatrixXd S = H * P_marg * H.transpose() + R; double chi2 = res.dot(S.llt().solve(res)); @@ -207,18 +209,16 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest double time0_cam = state->_timestamp; double time1_cam = timestamp; int num_features = 0; - double average_disparity = 0.0; - double variance_disparity = 0.0; - FeatureHelper::compute_disparity(_db, time0_cam, time1_cam, average_disparity, variance_disparity, num_features); + double disp_avg = 0.0; + double disp_var = 0.0; + FeatureHelper::compute_disparity(_db, time0_cam, time1_cam, disp_avg, disp_var, num_features); // Check if this disparity is enough to be classified as moving - disparity_passed = (average_disparity < _zupt_max_disparity && num_features > 20); + disparity_passed = (disp_avg < _zupt_max_disparity && num_features > 20); if (disparity_passed) { - PRINT_INFO(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, - (int)num_features); + PRINT_INFO(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, disp_avg, _zupt_max_disparity, (int)num_features); } else { - PRINT_DEBUG(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, - (int)num_features); + PRINT_DEBUG(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, disp_avg, _zupt_max_disparity, (int)num_features); } } @@ -226,6 +226,7 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // We need to pass the chi2 and not be above our velocity threshold if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) { last_zupt_state_timestamp = 0.0; + last_zupt_count = 0; PRINT_DEBUG(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, _options.chi2_multipler * chi2_check); return false; @@ -236,8 +237,9 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Do our update, only do this update if we have previously detected // If we have succeeded, then we should remove the current timestamp feature tracks // This is because we will not clone at this timestep and instead do our zero velocity update - // We want to keep the tracks from the previous timestep, thus only delete measurements from the current timestep - if (last_zupt_state_timestamp > 0.0) { + // NOTE: We want to keep the tracks from the second time we have called the zv-upt since this won't have a clone + // NOTE: All future times after the second call to this function will also *not* have a clone, so we can remove those + if (last_zupt_count >= 2) { _db->cleanup_measurements_exact(last_zupt_state_timestamp); } @@ -309,5 +311,6 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Finally return last_zupt_state_timestamp = timestamp; + last_zupt_count++; return true; } \ No newline at end of file diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index a7087f6dd..804e21cec 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -142,6 +142,9 @@ class UpdaterZeroVelocity { /// Last timestamp we did zero velocity update with double last_zupt_state_timestamp = 0.0; + + /// Number of times we have called update + int last_zupt_count = 0; }; } // namespace ov_msckf