diff --git a/data/euroc_config.json b/data/euroc_config.json index dcb40ff..1804a50 100644 --- a/data/euroc_config.json +++ b/data/euroc_config.json @@ -20,6 +20,9 @@ "config.vio_filter_iteration": 4, "config.vio_max_iterations": 7, "config.vio_enforce_realtime": false, + "config.vio_use_lm": false, + "config.vio_lm_lambda_min": 1e-32, + "config.vio_lm_lambda_max": 1e2, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, @@ -33,6 +36,9 @@ "config.mapper_second_best_test_ratio": 1.2, "config.mapper_bow_num_bits": 16, "config.mapper_min_triangulation_dist": 0.07, - "config.mapper_no_factor_weights": false + "config.mapper_no_factor_weights": false, + "config.mapper_use_lm": false, + "config.mapper_lm_lambda_min": 1e-32, + "config.mapper_lm_lambda_max": 1e3 } } diff --git a/data/euroc_config_no_weights.json b/data/euroc_config_no_weights.json index 91231c1..8743e73 100644 --- a/data/euroc_config_no_weights.json +++ b/data/euroc_config_no_weights.json @@ -20,6 +20,9 @@ "config.vio_filter_iteration": 4, "config.vio_max_iterations": 7, "config.vio_enforce_realtime": false, + "config.vio_use_lm": false, + "config.vio_lm_lambda_min": 1e-32, + "config.vio_lm_lambda_max": 1e2, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, @@ -33,6 +36,9 @@ "config.mapper_second_best_test_ratio": 1.2, "config.mapper_bow_num_bits": 16, "config.mapper_min_triangulation_dist": 0.07, - "config.mapper_no_factor_weights": true + "config.mapper_no_factor_weights": true, + "config.mapper_use_lm": false, + "config.mapper_lm_lambda_min": 1e-32, + "config.mapper_lm_lambda_max": 1e3 } } diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index 4ba4452..4210b70 100644 --- a/data/tumvi_512_config.json +++ b/data/tumvi_512_config.json @@ -21,6 +21,9 @@ "config.vio_filter_iteration": 4, "config.vio_max_iterations": 7, "config.vio_enforce_realtime": false, + "config.vio_use_lm": false, + "config.vio_lm_lambda_min": 1e-32, + "config.vio_lm_lambda_max": 1e2, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, @@ -34,6 +37,9 @@ "config.mapper_second_best_test_ratio": 1.2, "config.mapper_bow_num_bits": 16, "config.mapper_min_triangulation_dist": 0.07, - "config.mapper_no_factor_weights": false + "config.mapper_no_factor_weights": false, + "config.mapper_use_lm": false, + "config.mapper_lm_lambda_min": 1e-32, + "config.mapper_lm_lambda_max": 1e3 } } diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index 9a2fcf7..386cbb0 100644 --- a/include/basalt/utils/imu_types.h +++ b/include/basalt/utils/imu_types.h @@ -57,7 +57,9 @@ static const Eigen::Vector3d g(0, 0, -9.81); static const Eigen::Vector3d g_dir(0, 0, -1); } // namespace constants -struct PoseVelBiasStateWithLin : private PoseVelBiasState { +struct PoseVelBiasStateWithLin { + using VecN = PoseVelBiasState::VecN; + PoseVelBiasStateWithLin() { linearized = false; delta.setZero(); @@ -67,14 +69,14 @@ struct PoseVelBiasStateWithLin : private PoseVelBiasState { const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bias_gyro, const Eigen::Vector3d& bias_accel, bool linearized) - : PoseVelBiasState(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel), - linearized(linearized) { + : linearized(linearized), + state_linearized(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel) { delta.setZero(); - state_current = *this; + state_current = state_linearized; } PoseVelBiasStateWithLin(const PoseVelBiasState& other) - : PoseVelBiasState(other), linearized(false) { + : linearized(false), state_linearized(other) { delta.setZero(); state_current = other; } @@ -88,115 +90,152 @@ struct PoseVelBiasStateWithLin : private PoseVelBiasState { void applyInc(const VecN& inc) { if (!linearized) { - PoseVelBiasState::applyInc(inc); + state_linearized.applyInc(inc); } else { delta += inc; - state_current = *this; + state_current = state_linearized; state_current.applyInc(delta); } } inline const PoseVelBiasState& getState() const { if (!linearized) { - return *this; + return state_linearized; } else { return state_current; } } - inline const PoseVelBiasState& getStateLin() const { return *this; } + inline const PoseVelBiasState& getStateLin() const { + return state_linearized; + } inline bool isLinearized() const { return linearized; } inline const VecN& getDelta() const { return delta; } - inline int64_t getT_ns() const { return t_ns; } + inline int64_t getT_ns() const { return state_linearized.t_ns; } friend struct PoseStateWithLin; + inline void backup() { + backup_delta = delta; + backup_state_linearized = state_linearized; + backup_state_current = state_current; + } + + inline void restore() { + delta = backup_delta; + state_linearized = backup_state_linearized; + state_current = backup_state_current; + } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: bool linearized; VecN delta; + PoseVelBiasState state_linearized, state_current; - PoseVelBiasState state_current; + VecN backup_delta; + PoseVelBiasState backup_state_linearized, backup_state_current; friend class cereal::access; template void serialize(Archive& ar) { - ar(T_w_i); - ar(vel_w_i); - ar(bias_gyro); - ar(bias_accel); + ar(state_linearized.T_w_i); + ar(state_linearized.vel_w_i); + ar(state_linearized.bias_gyro); + ar(state_linearized.bias_accel); ar(state_current.T_w_i); ar(state_current.vel_w_i); ar(state_current.bias_gyro); ar(state_current.bias_accel); ar(delta); ar(linearized); - ar(t_ns); + ar(state_linearized.t_ns); } }; -struct PoseStateWithLin : private PoseState { +struct PoseStateWithLin { + using VecN = PoseState::VecN; + PoseStateWithLin() { linearized = false; delta.setZero(); }; PoseStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i) - : PoseState(t_ns, T_w_i), linearized(false) { + : linearized(false), pose_linearized(t_ns, T_w_i) { delta.setZero(); T_w_i_current = T_w_i; } PoseStateWithLin(const PoseVelBiasStateWithLin& other) - : PoseState(other.t_ns, other.T_w_i), - linearized(other.linearized), - delta(other.delta.head<6>()) { - T_w_i_current = T_w_i; - incPose(delta, T_w_i_current); + : linearized(other.linearized), + delta(other.delta.head<6>()), + pose_linearized(other.state_linearized.t_ns, + other.state_linearized.T_w_i) { + T_w_i_current = pose_linearized.T_w_i; + PoseState::incPose(delta, T_w_i_current); } inline const Sophus::SE3d& getPose() const { if (!linearized) { - return T_w_i; + return pose_linearized.T_w_i; } else { return T_w_i_current; } } - inline const Sophus::SE3d& getPoseLin() const { return T_w_i; } + inline const Sophus::SE3d& getPoseLin() const { + return pose_linearized.T_w_i; + } inline void applyInc(const VecN& inc) { if (!linearized) { - incPose(inc, T_w_i); + PoseState::incPose(inc, pose_linearized.T_w_i); } else { delta += inc; - T_w_i_current = T_w_i; - incPose(delta, T_w_i_current); + T_w_i_current = pose_linearized.T_w_i; + PoseState::incPose(delta, T_w_i_current); } } inline bool isLinearized() const { return linearized; } inline const VecN& getDelta() const { return delta; } - inline int64_t getT_ns() const { return t_ns; } + inline int64_t getT_ns() const { return pose_linearized.t_ns; } + + inline void backup() { + backup_delta = delta; + backup_pose_linearized = pose_linearized; + backup_T_w_i_current = T_w_i_current; + } + + inline void restore() { + delta = backup_delta; + pose_linearized = backup_pose_linearized; + T_w_i_current = backup_T_w_i_current; + } EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: bool linearized; VecN delta; - + PoseState pose_linearized; Sophus::SE3d T_w_i_current; + VecN backup_delta; + PoseState backup_pose_linearized; + Sophus::SE3d backup_T_w_i_current; + friend class cereal::access; template void serialize(Archive& ar) { - ar(T_w_i); + ar(pose_linearized.T_w_i); ar(T_w_i_current); ar(delta); ar(linearized); - ar(t_ns); + ar(pose_linearized.t_ns); } }; diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 6fd4ce4..b14be19 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -68,6 +68,10 @@ struct VioConfig { bool vio_enforce_realtime; + bool vio_use_lm; + double vio_lm_lambda_min; + double vio_lm_lambda_max; + double mapper_obs_std_dev; double mapper_obs_huber_thresh; int mapper_detection_num_points; @@ -81,5 +85,9 @@ struct VioConfig { int mapper_bow_num_bits; double mapper_min_triangulation_dist; bool mapper_no_factor_weights; + + bool mapper_use_lm; + double mapper_lm_lambda_min; + double mapper_lm_lambda_max; }; } // namespace basalt diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h index 19834eb..1c98ea6 100644 --- a/include/basalt/vi_estimator/ba_base.h +++ b/include/basalt/vi_estimator/ba_base.h @@ -368,6 +368,18 @@ class BundleAdjustmentBase { AccumT accum; }; + inline void backup() { + for (auto& kv : frame_states) kv.second.backup(); + for (auto& kv : frame_poses) kv.second.backup(); + lmdb.backup(); + } + + inline void restore() { + for (auto& kv : frame_states) kv.second.restore(); + for (auto& kv : frame_poses) kv.second.backup(); + lmdb.restore(); + } + // protected: PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const { auto it = frame_poses.find(t_ns); diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h index cd64527..5f7d555 100644 --- a/include/basalt/vi_estimator/keypoint_vio.h +++ b/include/basalt/vi_estimator/keypoint_vio.h @@ -228,6 +228,8 @@ class KeypointVioEstimator : public VioEstimatorBase, VioConfig config; + double lambda, min_lambda, max_lambda, lambda_vee; + int64_t msckf_kf_id; std::shared_ptr processing_thread; diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h index 40543af..4ffe783 100644 --- a/include/basalt/vi_estimator/landmark_database.h +++ b/include/basalt/vi_estimator/landmark_database.h @@ -44,7 +44,21 @@ struct KeypointPosition { Eigen::Vector2d dir; double id; + inline void backup() { + backup_dir = dir; + backup_id = id; + } + + inline void restore() { + dir = backup_dir; + id = backup_id; + } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + Eigen::Vector2d backup_dir; + double backup_id; }; struct KeypointObservation { @@ -92,6 +106,14 @@ class LandmarkDatabase { void removeObservations(int lm_id, const std::set& obs); + inline void backup() { + for (auto& kv : kpts) kv.second.backup(); + } + + inline void restore() { + for (auto& kv : kpts) kv.second.restore(); + } + private: Eigen::unordered_map kpts; Eigen::map> hash_bow_database; VioConfig config; + + double lambda, min_lambda, max_lambda, lambda_vee; }; } // namespace basalt diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 6ed56a1..eea0815 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -68,6 +68,10 @@ VioConfig::VioConfig() { vio_enforce_realtime = false; + vio_use_lm = false; + vio_lm_lambda_min = 1e-32; + vio_lm_lambda_max = 1e2; + mapper_obs_std_dev = 0.25; mapper_obs_huber_thresh = 1.5; mapper_detection_num_points = 800; @@ -81,6 +85,10 @@ VioConfig::VioConfig() { mapper_bow_num_bits = 16; mapper_min_triangulation_dist = 0.07; mapper_no_factor_weights = false; + + mapper_use_lm = false; + mapper_lm_lambda_min = 1e-32; + mapper_lm_lambda_max = 1e2; } void VioConfig::save(const std::string& filename) { @@ -132,6 +140,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.vio_enforce_realtime)); + ar(CEREAL_NVP(config.vio_use_lm)); + ar(CEREAL_NVP(config.vio_lm_lambda_min)); + ar(CEREAL_NVP(config.vio_lm_lambda_max)); + ar(CEREAL_NVP(config.mapper_obs_std_dev)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_detection_num_points)); @@ -145,5 +157,9 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.mapper_bow_num_bits)); ar(CEREAL_NVP(config.mapper_min_triangulation_dist)); ar(CEREAL_NVP(config.mapper_no_factor_weights)); + + ar(CEREAL_NVP(config.mapper_use_lm)); + ar(CEREAL_NVP(config.mapper_lm_lambda_min)); + ar(CEREAL_NVP(config.mapper_lm_lambda_max)); } } // namespace cereal diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 92c4e22..a4edc77 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -465,8 +465,9 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold, std::map>> outliers; computeError(error, &outliers, outlier_threshold); - // std::cout << "============================================" << std::endl; - // std::cout << "Num landmarks: " << lmdb.numLandmarks() << " with outliners + // std::cout << "============================================" << + // std::endl; std::cout << "Num landmarks: " << lmdb.numLandmarks() << " + // with outliners // " // << outliers.size() << std::endl; @@ -498,7 +499,8 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold, } } - // std::cout << "============================================" << std::endl; + // std::cout << "============================================" << + // std::endl; } void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, @@ -552,7 +554,8 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, // H_mm_inv = abs_H.bottomRightCorner(marg_size, marg_size) // .fullPivHouseholderQr() - // .solve(Eigen::MatrixXd::Identity(marg_size, marg_size)); + // .solve(Eigen::MatrixXd::Identity(marg_size, + // marg_size)); abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv; diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 397d037..3795149 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -53,7 +53,11 @@ KeypointVioEstimator::KeypointVioEstimator( frames_after_kf(0), g(g), initialized(false), - config(config) { + config(config), + lambda(config.vio_lm_lambda_min), + min_lambda(config.vio_lm_lambda_min), + max_lambda(config.vio_lm_lambda_max), + lambda_vee(2) { this->obs_std_dev = config.vio_obs_std_dev; this->huber_thresh = config.vio_obs_huber_thresh; this->calib = calib; @@ -949,35 +953,136 @@ void KeypointVioEstimator::optimize() { double error_total = rld_error + imu_error + marg_prior_error + ba_error + bg_error; + if (config.vio_debug) + std::cout << "[LINEARIZE] Error: " << error_total << " num points " + << std::endl; + lopt.accum.setup_solver(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); - Hdiag *= 1e-12; - for (int i = 0; i < Hdiag.size(); i++) - Hdiag[i] = std::max(Hdiag[i], 1e-12); - const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag); + bool converged = false; - // apply increment to poses - for (auto& kv : frame_poses) { - int idx = aom.abs_order_map.at(kv.first).first; - kv.second.applyInc(-inc.segment(idx)); - } + if (config.vio_use_lm) { // Use Levenberg–Marquardt + bool step = false; + int max_iter = 10; - // apply increment to states - for (auto& kv : frame_states) { - int idx = aom.abs_order_map.at(kv.first).first; - kv.second.applyInc(-inc.segment(idx)); - } + while (!step && max_iter > 0 && !converged) { + Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; + for (int i = 0; i < Hdiag_lambda.size(); i++) + Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); - // Update points - tbb::blocked_range keys_range(0, rld_vec.size()); - auto update_points_func = [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i != r.end(); ++i) { - const auto& rld = rld_vec[i]; - updatePoints(aom, rld, inc); + const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda); + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < 1e-4) converged = true; + + backup(); + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // apply increment to states + for (auto& kv : frame_states) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); + + double after_update_marg_prior_error = 0; + double after_update_vision_error = 0, after_update_imu_error = 0, + after_bg_error = 0, after_ba_error = 0; + + computeError(after_update_vision_error); + computeImuError(aom, after_update_imu_error, after_bg_error, + after_ba_error, frame_states, imu_meas, + gyro_bias_weight, accel_bias_weight, g); + computeMargPriorError(after_update_marg_prior_error); + + double after_error_total = + after_update_vision_error + after_update_imu_error + + after_update_marg_prior_error + after_bg_error + after_ba_error; + + double f_diff = (error_total - after_error_total); + double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB()); + + // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << + // std::endl; + + double step_quality = f_diff / l_diff; + + if (step_quality < 0) { + if (config.vio_debug) + std::cout << "\t[REJECTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc + << " Error: " << after_error_total << std::endl; + lambda = std::min(max_lambda, lambda_vee * lambda); + lambda_vee *= 2; + + restore(); + } else { + if (config.vio_debug) + std::cout << "\t[ACCEPTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc + << " Error: " << after_error_total << std::endl; + + lambda = std::max( + min_lambda, + lambda * + std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0))); + lambda_vee = 2; + + step = true; + } + max_iter--; } - }; - tbb::parallel_for(keys_range, update_points_func); + + if (config.vio_debug && converged) { + std::cout << "[CONVERGED]" << std::endl; + } + } else { // Use Gauss-Newton + Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda; + for (int i = 0; i < Hdiag_lambda.size(); i++) + Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); + + const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda); + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < 1e-4) converged = true; + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // apply increment to states + for (auto& kv : frame_states) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); + } if (config.vio_debug) { double after_update_marg_prior_error = 0; @@ -1014,8 +1119,7 @@ void KeypointVioEstimator::optimize() { << " bg_error: " << after_bg_error << " ba_error: " << after_ba_error << " marg prior: " << after_update_marg_prior_error - << " total: " << after_error_total << " max_inc " - << inc.array().abs().maxCoeff() << " error_diff " + << " total: " << after_error_total << " error_diff " << error_diff << " time : " << elapsed.count() << "(us), num_states " << frame_states.size() << " num_poses " << frame_poses.size() << std::endl; @@ -1029,7 +1133,7 @@ void KeypointVioEstimator::optimize() { filterOutliers(config.vio_outlier_threshold, 4); } - if (inc.array().abs().maxCoeff() < 1e-4) break; + if (converged) break; // std::cerr << "LT\n" << LT << std::endl; // std::cerr << "z_p\n" << z_p.transpose() << std::endl; @@ -1040,7 +1144,7 @@ void KeypointVioEstimator::optimize() { if (config.vio_debug) { std::cout << "=================================" << std::endl; } -} +} // namespace basalt void KeypointVioEstimator::computeProjections( std::vector>& data) const { diff --git a/src/vi_estimator/landmark_database.cpp b/src/vi_estimator/landmark_database.cpp index decdb35..b06b931 100644 --- a/src/vi_estimator/landmark_database.cpp +++ b/src/vi_estimator/landmark_database.cpp @@ -155,6 +155,8 @@ void LandmarkDatabase::removeLandmark(int lm_id) { host_to_kpts.at(it->second.kf_id).erase(lm_id); + std::set to_remove; + for (auto &kv : obs.at(it->second.kf_id)) { int idx = -1; for (size_t i = 0; i < kv.second.size(); ++i) { @@ -165,14 +167,22 @@ void LandmarkDatabase::removeLandmark(int lm_id) { } if (idx >= 0) { + BASALT_ASSERT(kv.second.size() > 0); + std::swap(kv.second[idx], kv.second[kv.second.size() - 1]); kv.second.resize(kv.second.size() - 1); num_observations--; kpts_num_obs.at(lm_id)--; + + if (kv.second.size() == 0) to_remove.insert(kv.first); } } + for (const auto &v : to_remove) { + obs.at(it->second.kf_id).erase(v); + } + BASALT_ASSERT_STREAM(kpts_num_obs.at(lm_id) == 0, kpts_num_obs.at(lm_id)); kpts_num_obs.erase(lm_id); kpts.erase(lm_id); @@ -183,6 +193,8 @@ void LandmarkDatabase::removeObservations(int lm_id, auto it = kpts.find(lm_id); BASALT_ASSERT(it != kpts.end()); + std::set to_remove; + for (auto &kv : obs.at(it->second.kf_id)) { if (outliers.count(kv.first) > 0) { int idx = -1; @@ -193,14 +205,21 @@ void LandmarkDatabase::removeObservations(int lm_id, } } BASALT_ASSERT(idx >= 0); + BASALT_ASSERT(kv.second.size() > 0); std::swap(kv.second[idx], kv.second[kv.second.size() - 1]); kv.second.resize(kv.second.size() - 1); num_observations--; kpts_num_obs.at(lm_id)--; + + if (kv.second.size() == 0) to_remove.insert(kv.first); } } + + for (const auto &v : to_remove) { + obs.at(it->second.kf_id).erase(v); + } } } // namespace basalt diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index fa4ebcd..b0c8c44 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -44,7 +44,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace basalt { NfrMapper::NfrMapper(const Calibration& calib, const VioConfig& config) - : config(config) { + : config(config), + lambda(config.mapper_lm_lambda_min), + min_lambda(config.mapper_lm_lambda_min), + max_lambda(config.mapper_lm_lambda_max), + lambda_vee(2) { this->calib = calib; this->obs_std_dev = config.mapper_obs_std_dev; this->huber_thresh = config.mapper_obs_huber_thresh; @@ -274,7 +278,7 @@ void NfrMapper::optimize(int num_iterations) { double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error; - std::cout << "iter " << iter + std::cout << "[LINEARIZE] iter " << iter << " before_update_error: vision: " << rld_error << " rel_error: " << lopt.rel_error << " roll_pitch_error: " << lopt.roll_pitch_error @@ -284,61 +288,132 @@ void NfrMapper::optimize(int num_iterations) { lopt.accum.print_info = true; lopt.accum.setup_solver(); - Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); - Hdiag.setConstant(Hdiag.size(), 1e-6); + const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); - const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag); + bool converged = false; - // apply increment to poses - for (auto& kv : frame_poses) { - int idx = aom.abs_order_map.at(kv.first).first; - BASALT_ASSERT(!kv.second.isLinearized()); - kv.second.applyInc(-inc.segment(idx)); + if (config.vio_use_lm) { // Use Levenberg–Marquardt + bool step = false; + int max_iter = 10; + + while (!step && max_iter > 0 && !converged) { + Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; + for (int i = 0; i < Hdiag_lambda.size(); i++) + Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); + + const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda); + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < 1e-5) converged = true; + + backup(); + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + BASALT_ASSERT(!kv.second.isLinearized()); + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); + + double after_update_vision_error = 0; + double after_rel_error = 0; + double after_roll_pitch_error = 0; + + computeError(after_update_vision_error); + computeRelPose(after_rel_error); + computeRollPitch(after_roll_pitch_error); + + double after_error_total = after_update_vision_error + after_rel_error + + after_roll_pitch_error; + + double f_diff = (error_total - after_error_total); + double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB()); + + std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl; + + double step_quality = f_diff / l_diff; + + if (step_quality < 0) { + std::cout << "\t[REJECTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc + << " vision_error: " << after_update_vision_error + << " rel_error: " << after_rel_error + << " roll_pitch_error: " << after_roll_pitch_error + << " total: " << after_error_total << std::endl; + lambda = std::min(max_lambda, lambda_vee * lambda); + lambda_vee *= 2; + + restore(); + } else { + std::cout << "\t[ACCEPTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc + << " vision_error: " << after_update_vision_error + << " rel_error: " << after_rel_error + << " roll_pitch_error: " << after_roll_pitch_error + << " total: " << after_error_total << std::endl; + + lambda = std::max( + min_lambda, + lambda * + std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0))); + lambda_vee = 2; + + step = true; + } + + max_iter--; + + if (after_error_total > error_total) { + std::cout << "increased error after update!!!" << std::endl; + } + } + } else { // Use Gauss-Newton + Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda; + for (int i = 0; i < Hdiag_lambda.size(); i++) + Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); + + const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda); + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < 1e-5) converged = true; + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + BASALT_ASSERT(!kv.second.isLinearized()); + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); } - // Update points - tbb::blocked_range keys_range(0, rld_vec.size()); - auto update_points_func = [&](const tbb::blocked_range& r) { - for (size_t i = r.begin(); i != r.end(); ++i) { - const auto& rld = rld_vec[i]; - updatePoints(aom, rld, inc); - } - }; - tbb::parallel_for(keys_range, update_points_func); - - double after_update_vision_error = 0; - double after_rel_error = 0; - double after_roll_pitch_error = 0; - - computeError(after_update_vision_error); - computeRelPose(after_rel_error); - computeRollPitch(after_roll_pitch_error); - - double after_error_total = - after_update_vision_error + after_rel_error + after_roll_pitch_error; - - double error_diff = error_total - after_error_total; - auto t2 = std::chrono::high_resolution_clock::now(); - auto elapsed = std::chrono::duration_cast(t2 - t1); - std::cout << "iter " << iter - << " after_update_error: vision: " << after_update_vision_error - << " rel_error: " << after_rel_error - << " roll_pitch_error: " << after_roll_pitch_error - << " total: " << after_error_total << " max_inc " - << inc.array().abs().maxCoeff() << " error_diff " << error_diff - << " time : " << elapsed.count() << "(us), num_states " - << frame_states.size() << " num_poses " << frame_poses.size() - << std::endl; + std::cout << "iter " << iter << " time : " << elapsed.count() + << "(us), num_states " << frame_states.size() << " num_poses " + << frame_poses.size() << std::endl; - if (after_error_total > error_total) { - std::cout << "increased error after update!!!" << std::endl; - } - - if (inc.array().abs().maxCoeff() < 1e-4) break; + if (converged) break; // std::cerr << "LT\n" << LT << std::endl; // std::cerr << "z_p\n" << z_p.transpose() << std::endl; @@ -508,7 +583,8 @@ void NfrMapper::match_all() { // std::cout << "Closest frames for " << tcid << ": "; for (const auto& otcid_score : results) { - // std::cout << otcid_score.first << "(" << otcid_score.second << ") "; + // std::cout << otcid_score.first << "(" << otcid_score.second << ") + // "; if (otcid_score.first.frame_id != tcid.frame_id && otcid_score.second > config.mapper_frames_to_match_threshold) { match_pair m;