vo: crash earlier for duplicate timestamps or numeric failure

include/basalt/utils/vio_config.h

@@ -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;

src/utils/vio_config.cpp

@@ -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;

src/vi_estimator/sqrt_keypoint_vio.cpp

@@ -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)

src/vi_estimator/sqrt_keypoint_vo.cpp

@@ -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)