Merge branch 'demmeln/fix-rs-t265' into 'master'

fix timestamp issues with realsense t265

Closes #16

See merge request basalt/basalt!50

include/basalt/device/rs_t265.h

@@ -79,7 +79,7 @@ class RsT265Device {
 
   RsT265Device(bool manual_exposure, int skip_frames, int webp_quality,
                double exposure_value = 10.0);
-  ~RsT265Device();
+
   void start();
   void stop();
 

include/basalt/utils/vio_config.h

@@ -78,8 +78,6 @@ 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_pose_damping_variant;
 
   bool vio_scale_jacobian;
 

include/basalt/vi_estimator/sqrt_keypoint_vio.h

@@ -248,6 +248,8 @@ class SqrtKeypointVioEstimator : public VioEstimatorBase,
 
   VioConfig config;
 
+  constexpr static Scalar vee_factor = Scalar(2.0);
+  constexpr static Scalar initial_vee = Scalar(2.0);
   Scalar lambda, min_lambda, max_lambda, lambda_vee;
 
   std::shared_ptr<std::thread> processing_thread;

include/basalt/vi_estimator/sqrt_keypoint_vo.h

@@ -238,6 +238,8 @@ class SqrtKeypointVoEstimator : public VioEstimatorBase,
 
   VioConfig config;
 
+  constexpr static Scalar vee_factor = Scalar(2.0);
+  constexpr static Scalar initial_vee = Scalar(2.0);
   Scalar lambda, min_lambda, max_lambda, lambda_vee;
 
   std::shared_ptr<std::thread> processing_thread;

src/device/rs_t265.cpp

@@ -84,8 +84,6 @@ RsT265Device::RsT265Device(bool manual_exposure, int skip_frames,
   }
 }
 
-RsT265Device::~RsT265Device(){};
-
 void RsT265Device::start() {
   auto callback = [&](const rs2::frame& frame) {
     exportCalibration();
@@ -148,20 +146,47 @@ void RsT265Device::start() {
         }
       }
     } else if (auto fs = frame.as<rs2::frameset>()) {
-      if (frame_counter++ % skip_frames != 0) return;
+      BASALT_ASSERT(fs.size() == NUM_CAMS);
+
+      std::vector<rs2::video_frame> vfs;
+      for (int i = 0; i < NUM_CAMS; ++i) {
+        rs2::video_frame vf = fs[i].as<rs2::video_frame>();
+        if (!vf) {
+          std::cout << "Weird Frame, skipping" << std::endl;
+          return;
+        }
+        vfs.push_back(vf);
+      }
+
+      // Callback is called for every new image, so in every other call, the
+      // left frame is updated but the right frame is still from the previous
+      // timestamp. So we only process framesets where both images are valid and
+      // have the same timestamp.
+      for (int i = 1; i < NUM_CAMS; ++i) {
+        if (vfs[0].get_timestamp() != vfs[i].get_timestamp()) {
+          return;
+        }
+      }
+
+      // skip frames if configured
+      if (frame_counter++ % skip_frames != 0) {
+        return;
+      }
 
       OpticalFlowInput::Ptr data(new OpticalFlowInput);
       data->img_data.resize(NUM_CAMS);
 
-      for (int i = 0; i < NUM_CAMS; i++) {
+      //      std::cout << "Reading frame " << frame_counter << std::endl;
-        auto f = fs[i];
-        if (!f.as<rs2::video_frame>()) {
-          std::cout << "Weird Frame, skipping" << std::endl;
-          continue;
-        }
-        auto vf = f.as<rs2::video_frame>();
 
-        data->t_ns = vf.get_timestamp() * 1e6;
+      for (int i = 0; i < NUM_CAMS; i++) {
+        const auto& vf = vfs[i];
+
+        int64_t t_ns = vf.get_timestamp() * 1e6;
+
+        // at this stage both image timestamps are expected to be equal
+        BASALT_ASSERT(i == 0 || t_ns == data->t_ns);
+
+        data->t_ns = t_ns;
 
         data->img_data[i].exposure =
             vf.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) * 1e-6;
@@ -178,6 +203,11 @@ void RsT265Device::start() {
           val = val << 8;
           data_out[j] = val;
         }
+
+        //        std::cout << "Timestamp / exposure " << i << ": " <<
+        //        data->t_ns << " / "
+        //                  << int(data->img_data[i].exposure * 1e3) << "ms" <<
+        //                  std::endl;
       }
 
       last_img_data = data;

src/utils/vio_config.cpp

@@ -75,11 +75,9 @@ VioConfig::VioConfig() {
   vio_enforce_realtime = false;
 
   vio_use_lm = false;
-  vio_lm_lambda_initial = 1e-8;
+  vio_lm_lambda_initial = 1e-4;
-  vio_lm_lambda_min = 1e-32;
+  vio_lm_lambda_min = 1e-6;
   vio_lm_lambda_max = 1e2;
-  vio_lm_landmark_damping_variant = 0;
-  vio_lm_pose_damping_variant = 0;
 
   vio_scale_jacobian = true;
 
@@ -192,8 +190,6 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.vio_lm_lambda_initial));
   ar(CEREAL_NVP(config.vio_lm_lambda_min));
   ar(CEREAL_NVP(config.vio_lm_lambda_max));
-  ar(CEREAL_NVP(config.vio_lm_landmark_damping_variant));
-  ar(CEREAL_NVP(config.vio_lm_pose_damping_variant));
 
   ar(CEREAL_NVP(config.vio_scale_jacobian));
 

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;
@@ -1197,14 +1202,32 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
 
           stats.add("get_dense_H_b", t.reset()).format("ms");
 
-          if (config.vio_lm_pose_damping_variant == 1) {
+          int iter = 0;
+          bool inc_valid = false;
+          constexpr int max_num_iter = 3;
+
+          while (iter < max_num_iter && !inc_valid) {
             VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
-            H.diagonal() += Hdiag_lambda;
+            MatX H_copy = H;
+            H_copy.diagonal() += Hdiag_lambda;
+
+            Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
+            inc = ldlt.solve(b);
+            stats.add("solve", t.reset()).format("ms");
+
+            if (!inc.array().isFinite().all()) {
+              lambda = lambda_vee * lambda;
+              lambda_vee *= vee_factor;
+            } else {
+              inc_valid = true;
+            }
+            iter++;
           }
 
-          Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+          if (!inc_valid) {
-          inc = ldlt.solve(b);
+            std::cerr << "Still invalid inc after " << max_num_iter
-          stats.add("solve", t.reset()).format("ms");
+                      << " iterations." << std::endl;
+          }
         }
 
         // backup state (then apply increment and check cost decrease)
@@ -1283,8 +1306,8 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
                              relative_decrease, step_norminf);
           }
 
-          // TODO: consider to remove assert. For now we want to test if we even
+          // TODO: consider to remove assert. For now we want to test if we
-          // run into the l_diff <= 0 case ever in practice
+          // even run into the l_diff <= 0 case ever in practice
           // BASALT_ASSERT_STREAM(l_diff > 0, "l_diff " << l_diff);
 
           // l_diff <= 0 is a theoretical possibility if the model cost change
@@ -1327,7 +1350,6 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
               1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
           lambda = std::max(min_lambda, lambda);
 
-          constexpr Scalar initial_vee = Scalar(2.0);
           lambda_vee = initial_vee;
 
           it++;
@@ -1356,7 +1378,6 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
           }
 
           lambda = lambda_vee * lambda;
-          constexpr Scalar vee_factor = Scalar(2.0);
           lambda_vee *= vee_factor;
 
           //        lambda = std::max(min_lambda, lambda);

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;
@@ -1098,14 +1099,32 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
 
         stats.add("get_dense_H_b", t.reset()).format("ms");
 
-        if (config.vio_lm_pose_damping_variant == 1) {
+        int iter = 0;
+        bool inc_valid = false;
+        constexpr int max_num_iter = 3;
+
+        while (iter < max_num_iter && !inc_valid) {
           VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
-          H.diagonal() += Hdiag_lambda;
+          MatX H_copy = H;
+          H_copy.diagonal() += Hdiag_lambda;
+
+          Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
+          inc = ldlt.solve(b);
+          stats.add("solve", t.reset()).format("ms");
+
+          if (!inc.array().isFinite().all()) {
+            lambda = lambda_vee * lambda;
+            lambda_vee *= vee_factor;
+          } else {
+            inc_valid = true;
+          }
+          iter++;
         }
 
-        Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+        if (!inc_valid) {
-        inc = ldlt.solve(b);
+          std::cerr << "Still invalid inc after " << max_num_iter
-        stats.add("solve", t.reset()).format("ms");
+                    << " iterations." << std::endl;
+        }
       }
 
       // backup state (then apply increment and check cost decrease)
@@ -1217,7 +1236,6 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
             1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
         lambda = std::max(min_lambda, lambda);
 
-        constexpr Scalar initial_vee = Scalar(2.0);
         lambda_vee = initial_vee;
 
         it++;
@@ -1245,7 +1263,6 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
         }
 
         lambda = lambda_vee * lambda;
-        constexpr Scalar vee_factor = Scalar(2.0);
         lambda_vee *= vee_factor;
 
         //        lambda = std::max(min_lambda, lambda);