From 470ac63a2df487637d6e13cbd5741e54ec3f67f8 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Fri, 10 Dec 2021 02:26:50 +0100 Subject: [PATCH 1/3] rs_t265: skip duplicate incomplete framesets --- include/basalt/device/rs_t265.h | 2 +- src/device/rs_t265.cpp | 52 ++++++++++++++++++++++++++------- 2 files changed, 42 insertions(+), 12 deletions(-) diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h index 4d70d67..462c6d2 100644 --- a/include/basalt/device/rs_t265.h +++ b/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(); diff --git a/src/device/rs_t265.cpp b/src/device/rs_t265.cpp index dee3915..462da01 100644 --- a/src/device/rs_t265.cpp +++ b/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()) { - if (frame_counter++ % skip_frames != 0) return; + BASALT_ASSERT(fs.size() == NUM_CAMS); + + std::vector vfs; + for (int i = 0; i < NUM_CAMS; ++i) { + rs2::video_frame vf = fs[i].as(); + 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++) { - auto f = fs[i]; - if (!f.as()) { - std::cout << "Weird Frame, skipping" << std::endl; - continue; - } - auto vf = f.as(); + // std::cout << "Reading frame " << frame_counter << std::endl; - 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; From ec48ff22e512e83b861a5d45b6e8e12c4e935fa1 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Fri, 10 Dec 2021 02:27:35 +0100 Subject: [PATCH 2/3] vo: crash earlier for duplicate timestamps or numeric failure --- include/basalt/utils/vio_config.h | 2 +- src/utils/vio_config.cpp | 2 +- src/vi_estimator/sqrt_keypoint_vio.cpp | 12 ++++++++++++ src/vi_estimator/sqrt_keypoint_vo.cpp | 8 ++++++++ 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 87b1baf..07d39ef 100644 --- a/include/basalt/utils/vio_config.h +++ b/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; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 4e39f70..9491cc7 100644 --- a/src/utils/vio_config.cpp +++ b/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; diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index 71aff49..4cbc6cc 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -226,6 +226,10 @@ void SqrtKeypointVioEstimator::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::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::optimize() { Eigen::LDLT> 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) diff --git a/src/vi_estimator/sqrt_keypoint_vo.cpp b/src/vi_estimator/sqrt_keypoint_vo.cpp index 92838dd..e7fd066 100644 --- a/src/vi_estimator/sqrt_keypoint_vo.cpp +++ b/src/vi_estimator/sqrt_keypoint_vo.cpp @@ -1029,6 +1029,7 @@ void SqrtKeypointVoEstimator::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::optimize() { Eigen::LDLT> 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) From 700376d84698d19f99b3a78ce0418fd8d38aa7d1 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Fri, 10 Dec 2021 11:03:49 +0100 Subject: [PATCH 3/3] Make default config suitable for float. Implement better handling of non-finite increments. --- include/basalt/utils/vio_config.h | 2 - .../basalt/vi_estimator/sqrt_keypoint_vio.h | 2 + .../basalt/vi_estimator/sqrt_keypoint_vo.h | 2 + src/utils/vio_config.cpp | 8 +--- src/vi_estimator/sqrt_keypoint_vio.cpp | 41 +++++++++++-------- src/vi_estimator/sqrt_keypoint_vo.cpp | 37 ++++++++++------- 6 files changed, 54 insertions(+), 38 deletions(-) diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 07d39ef..cebf67f 100644 --- a/include/basalt/utils/vio_config.h +++ b/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; // currently unused - int vio_lm_pose_damping_variant; bool vio_scale_jacobian; diff --git a/include/basalt/vi_estimator/sqrt_keypoint_vio.h b/include/basalt/vi_estimator/sqrt_keypoint_vio.h index 376ef10..69494db 100644 --- a/include/basalt/vi_estimator/sqrt_keypoint_vio.h +++ b/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 processing_thread; diff --git a/include/basalt/vi_estimator/sqrt_keypoint_vo.h b/include/basalt/vi_estimator/sqrt_keypoint_vo.h index 555fb14..b20124b 100644 --- a/include/basalt/vi_estimator/sqrt_keypoint_vo.h +++ b/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 processing_thread; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 9491cc7..0e554ed 100644 --- a/src/utils/vio_config.cpp +++ b/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_min = 1e-32; + vio_lm_lambda_initial = 1e-4; + vio_lm_lambda_min = 1e-6; vio_lm_lambda_max = 1e2; - vio_lm_landmark_damping_variant = 0; // currently unused - 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)); diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index 4cbc6cc..55a7618 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -1202,21 +1202,32 @@ void SqrtKeypointVioEstimator::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> 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> 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"); + if (!inc_valid) { + std::cerr << "Still invalid inc after " << max_num_iter + << " iterations." << std::endl; + } } // backup state (then apply increment and check cost decrease) @@ -1295,8 +1306,8 @@ void SqrtKeypointVioEstimator::optimize() { relative_decrease, step_norminf); } - // TODO: consider to remove assert. For now we want to test if we even - // run into the l_diff <= 0 case ever in practice + // TODO: consider to remove assert. For now we want to test if we + // 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 @@ -1339,7 +1350,6 @@ void SqrtKeypointVioEstimator::optimize() { 1 - std::pow(2 * relative_decrease - 1, 3)); lambda = std::max(min_lambda, lambda); - constexpr Scalar initial_vee = Scalar(2.0); lambda_vee = initial_vee; it++; @@ -1368,7 +1378,6 @@ void SqrtKeypointVioEstimator::optimize() { } lambda = lambda_vee * lambda; - constexpr Scalar vee_factor = Scalar(2.0); lambda_vee *= vee_factor; // lambda = std::max(min_lambda, lambda); diff --git a/src/vi_estimator/sqrt_keypoint_vo.cpp b/src/vi_estimator/sqrt_keypoint_vo.cpp index e7fd066..968df71 100644 --- a/src/vi_estimator/sqrt_keypoint_vo.cpp +++ b/src/vi_estimator/sqrt_keypoint_vo.cpp @@ -1099,21 +1099,32 @@ void SqrtKeypointVoEstimator::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> 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> 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"); + if (!inc_valid) { + std::cerr << "Still invalid inc after " << max_num_iter + << " iterations." << std::endl; + } } // backup state (then apply increment and check cost decrease) @@ -1225,7 +1236,6 @@ void SqrtKeypointVoEstimator::optimize() { 1 - std::pow(2 * relative_decrease - 1, 3)); lambda = std::max(min_lambda, lambda); - constexpr Scalar initial_vee = Scalar(2.0); lambda_vee = initial_vee; it++; @@ -1253,7 +1263,6 @@ void SqrtKeypointVoEstimator::optimize() { } lambda = lambda_vee * lambda; - constexpr Scalar vee_factor = Scalar(2.0); lambda_vee *= vee_factor; // lambda = std::max(min_lambda, lambda);