ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

vo: crash earlier for duplicate timestamps or numeric failure

This commit is contained in:
Nikolaus Demmel 2021-12-10 02:27:35 +01:00
parent 470ac63a2d
commit ec48ff22e5
4 changed files with 22 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/utils/vio_config.h
View File

@ -78,7 +78,7 @@ struct VioConfig {
double vio_lm_lambda_initial;
double vio_lm_lambda_min;
double vio_lm_lambda_max;
int vio_lm_landmark_damping_variant;
int vio_lm_landmark_damping_variant; // currently unused
int vio_lm_pose_damping_variant;
bool vio_scale_jacobian;

2
src/utils/vio_config.cpp
View File

@ -78,7 +78,7 @@ VioConfig::VioConfig() {
vio_lm_lambda_initial = 1e-8;
vio_lm_lambda_min = 1e-32;
vio_lm_lambda_max = 1e2;
vio_lm_landmark_damping_variant = 0;
vio_lm_landmark_damping_variant = 0; // currently unused
vio_lm_pose_damping_variant = 0;
vio_scale_jacobian = true;

12
src/vi_estimator/sqrt_keypoint_vio.cpp
View File

@ -226,6 +226,10 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
prev_frame->t_ns, last_state.getState().bias_gyro,
last_state.getState().bias_accel));
BASALT_ASSERT_MSG(prev_frame->t_ns < curr_frame->t_ns,
"duplicate frame timestamps?! zero time delta leads "
"to invalid IMU integration.");
while (data->t_ns <= prev_frame->t_ns) {
data = popFromImuDataQueue();
if (!data) break;
@ -1127,6 +1131,7 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
}
if (config.vio_debug) {
// TODO: num_points debug output missing
std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl;
@ -1205,6 +1210,13 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
inc = ldlt.solve(b);
stats.add("solve", t.reset()).format("ms");
// TODO: instead of crashing, backtrack and increase damping, but make
// sure it does not go unnoticed. (Note: right now, without further
// handling, Sophus would crash anyway when trying to apply and
// increment with NaNs or inf)
BASALT_ASSERT_MSG(!inc.array().isFinite().all(),
"numeric failure during");
}
// backup state (then apply increment and check cost decrease)

8
src/vi_estimator/sqrt_keypoint_vo.cpp
View File

@ -1029,6 +1029,7 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
stats.add("performQR", t.reset()).format("ms");
if (config.vio_debug) {
// TODO: num_points debug output missing
std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl;
@ -1106,6 +1107,13 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
inc = ldlt.solve(b);
stats.add("solve", t.reset()).format("ms");
// TODO: instead of crashing, backtrack and increase damping, but make
// sure it does not go unnoticed. (Note: right now, without further
// handling, Sophus would crash anyway when trying to apply and
// increment with NaNs or inf)
BASALT_ASSERT_MSG(!inc.array().isFinite().all(),
"numeric failure during");
}
// backup state (then apply increment and check cost decrease)