Running OpenVINS for very long durations #481
Comments
|
As an initial strategy, I wanted to reset the filter state, and use the last-known state as a initialization point. Looking at #398, re-starting the ROS node is not an option for me. I modified the Serial ROS1 bag reader code to attempt this, I'm not too sure if I did this properly -- but if someone could sanity check my impl, it would be greatly appreciated :) Essentially, I modified the // After processing, we need to check if the filter has diverged. We do this by a simple check of the covariance for now.
std::shared_ptr<State> state = _app->get_state();
std::vector<std::shared_ptr<Type>> statevars;
statevars.push_back(state->_imu->pose()->p());
statevars.push_back(state->_imu->pose()->q());
Eigen::Matrix<double, 6, 6> covariance_posori = StateHelper::get_marginal_covariance(_app->get_state(), statevars);
// Check if the covariance is too large
const double frobnorm = covariance_posori.norm();
PRINT_INFO(CYAN "@ %0.4f | Covariance Frobenius Norm: %.4f from VIOManager %s\n" RESET, message.timestamp, frobnorm,
_app->name.c_str());
// TODO to be tuned!
constexpr double threshold_covariance = 1.0;
if (frobnorm > threshold_covariance) {
PRINT_ERROR("Covariance is too large! (%.3f) Resetting...\n", frobnorm);
// Get rid of all features and clones in the state so that it is as "clean" as possible
StateHelper::marginalize_all_clones(state);
StateHelper::marginalize_all_slam(state);
camera_queue.clear();
// Naive way, but we need to reset the state
reset_state = std::make_unique<Eigen::Matrix<double, 17, 1>>();
reset_state->block(1, 0, 4, 1) = state->_imu->quat(); // Apart from position, we re-initialize with the curr state
// reset_state->block(5, 0, 3, 1) = state->_imu->pos(); // cannot reset position so easily??
reset_state->block(5, 0, 3, 1) = Eigen::Vector3d::Zero(); // Reset position
reset_state->block(8, 0, 3, 1) = state->_imu->vel(); // take the current velocity
reset_state->block(11, 0, 3, 1) = state->_imu->bias_g(); // take the current biases
reset_state->block(14, 0, 3, 1) = state->_imu->bias_a(); //
// Destroy the current application and create a new one to ensure all state is "reset"
VioManagerOptions currParams = _app->get_params();
_app.reset();
_app = std::make_shared<VioManager>(currParams, "reset"+std::to_string(reset_counter++));
PRINT_INFO(YELLOW "Reset VIOManager %s\n" RESET, _app->name.c_str());
}Knowing the previous state, we can then "abuse" the // check if we need to reinit
if (reset_state) {
(*reset_state)(0, 0) = timestamp;
_app->initialize_with_gt(*reset_state);
reset_state.reset(); // Clear this so we do not reinit again
PRINT_INFO(YELLOW "Reinitialize VIOManager %s\n" RESET, _app->name.c_str());
PRINT_DEBUG(YELLOW "Number of states after reset: IMU clones: %d, SLAM features: %d\n" RESET, _app->get_state()->_clones_IMU.size(),
_app->get_state()->_features_SLAM.size());
} |
|
it is a good work thanks for sharing. i wanted to do a reset state like you did. but i haven't started yet. |
goldbattle
added
user-platform
|
This might be the covariance is becoming ill-defined, or the statistical chi2 checks start to fail at that point. Could you take a look at that point and see if the filter stops getting feature updates (MSCKF and SLAM)? I think one concern with your approach is that you will remove all the tracked features, so basically have non-continuous pose tracking. Probably a better method is to "update" with a fake GPS / mocap sensors to reduce the uncertainty of the system to remove this instability (if you want to avoid this issue with the global uncertainty being unbounded). |
Hello, thanks for the good work and great code!
I've been trying to get OpenVINS to run on large-scale, long datasets, in particular the Vision Benchmark in Rome dataset. This dataset is quite interesting as it contains handheld sequences > 20min and > 2km in length.
Initially, OpenVINS seems to do quite well, but as the trajectory gets longer, I find the covariance estimates also increase, and I guess without place recognition techniques to "anchor" it, the trajectory eventually drifts away Youtube video (skip to ~3:15 to see filter divergence).
Maybe this is a limitation of a pure EKF-based VIO system, but are there any things that i can try to tackle this issue? E.g force reinitialization when covariance increases, tuning configs, etc.? Thanks in advance!
Overall Config
IMU Config
Camera Config
The text was updated successfully, but these errors were encountered: