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/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 87b1baf..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; - 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/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; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 4e39f70..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; - 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 71aff49..55a7618 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; @@ -1197,14 +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"); + if (!inc_valid) { + std::cerr << "Still invalid inc after " << max_num_iter + << " iterations." << std::endl; + } } // backup state (then apply increment and check cost decrease) @@ -1283,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 @@ -1327,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++; @@ -1356,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 92838dd..968df71 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; @@ -1098,14 +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"); + if (!inc_valid) { + std::cerr << "Still invalid inc after " << max_num_iter + << " iterations." << std::endl; + } } // backup state (then apply increment and check cost decrease) @@ -1217,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++; @@ -1245,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);