From b3304884c692b5ea9f61fb6c8732ee0c3680c64f Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 7 Aug 2019 16:26:13 +0200 Subject: [PATCH 01/14] small update to types --- include/basalt/utils/imu_types.h | 72 ++++++++++++++++++-------------- 1 file changed, 41 insertions(+), 31 deletions(-) diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index 9a2fcf7..a8b3c87 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,27 +90,29 @@ 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; @@ -117,86 +121,92 @@ struct PoseVelBiasStateWithLin : private PoseVelBiasState { bool linearized; VecN delta; - PoseVelBiasState state_current; + PoseVelBiasState state_linearized, 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; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: bool linearized; VecN delta; + PoseState pose_linearized; Sophus::SE3d 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); } }; From 29e1a76407fbee1c5041b70e038da4193b5a11fc Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 7 Aug 2019 16:45:25 +0200 Subject: [PATCH 02/14] added backup funtions to types --- include/basalt/utils/imu_types.h | 33 +++++++++++++++++-- .../basalt/vi_estimator/landmark_database.h | 14 ++++++++ thirdparty/basalt-headers | 2 +- 3 files changed, 46 insertions(+), 3 deletions(-) diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index a8b3c87..386cbb0 100644 --- a/include/basalt/utils/imu_types.h +++ b/include/basalt/utils/imu_types.h @@ -116,13 +116,27 @@ struct PoseVelBiasStateWithLin { 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; + VecN backup_delta; + PoseVelBiasState backup_state_linearized, backup_state_current; + friend class cereal::access; template @@ -190,14 +204,29 @@ struct PoseStateWithLin { inline const VecN& getDelta() const { return delta; } 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 diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h index 40543af..f506b8c 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 { diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 2ae558c..df6b2ca 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 2ae558ce2b494d21908391b89cf13df93d0dcfa9 +Subproject commit df6b2ca48658a3261370cbf9799ed69f7692525a From 227694d572990d3f25bce6cd45c6396e19580e0d Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 7 Aug 2019 16:54:35 +0200 Subject: [PATCH 03/14] added restore and backup functions --- include/basalt/vi_estimator/ba_base.h | 12 ++++++++++++ include/basalt/vi_estimator/landmark_database.h | 8 ++++++++ src/vi_estimator/keypoint_vio.cpp | 3 +++ 3 files changed, 23 insertions(+) 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/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h index f506b8c..4ffe783 100644 --- a/include/basalt/vi_estimator/landmark_database.h +++ b/include/basalt/vi_estimator/landmark_database.h @@ -106,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 Date: Wed, 7 Aug 2019 17:19:27 +0200 Subject: [PATCH 04/14] Added LM to VIO --- include/basalt/vi_estimator/keypoint_vio.h | 2 + src/vi_estimator/keypoint_vio.cpp | 120 ++++++++++++++++----- 2 files changed, 94 insertions(+), 28 deletions(-) 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/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 29e181b..65b22b2 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(1e-6), + min_lambda(1e-12), + max_lambda(10), + lambda_vee(2) { this->obs_std_dev = config.vio_obs_std_dev; this->huber_thresh = config.vio_obs_huber_thresh; this->calib = calib; @@ -941,38 +945,99 @@ void KeypointVioEstimator::optimize() { double error_total = rld_error + imu_error + marg_prior_error + ba_error + bg_error; + 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; + bool step = false; + int max_iter = 10; - // 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)); - } + 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); - // 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)); - } + const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda); + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < 1e-4) converged = true; - // 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); + 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)); } - }; - tbb::parallel_for(keys_range, update_points_func); - // backup(); - // restore(); + // 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) { + 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 { + 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--; + } + + if (converged) { + std::cout << "[CONVERGED]" << std::endl; + } if (config.vio_debug) { double after_update_marg_prior_error = 0; @@ -1009,8 +1074,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; @@ -1024,7 +1088,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; From 8ffa903eb2e945a24f153fffb9eb4851c4d63db4 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 7 Aug 2019 17:21:52 +0200 Subject: [PATCH 05/14] Added LM to VIO --- src/vi_estimator/keypoint_vio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 65b22b2..b010a2c 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -56,7 +56,7 @@ KeypointVioEstimator::KeypointVioEstimator( config(config), lambda(1e-6), min_lambda(1e-12), - max_lambda(10), + max_lambda(100), lambda_vee(2) { this->obs_std_dev = config.vio_obs_std_dev; this->huber_thresh = config.vio_obs_huber_thresh; From 29ed498aa0615ec952718566405b469892ad2eee Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Thu, 8 Aug 2019 14:37:07 +0200 Subject: [PATCH 06/14] new param --- src/vi_estimator/keypoint_vio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index b010a2c..5b7ac4c 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -55,7 +55,7 @@ KeypointVioEstimator::KeypointVioEstimator( initialized(false), config(config), lambda(1e-6), - min_lambda(1e-12), + min_lambda(1e-14), max_lambda(100), lambda_vee(2) { this->obs_std_dev = config.vio_obs_std_dev; From 43c99145920b3fdee3e475fd455120606758e7e4 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Thu, 8 Aug 2019 14:56:51 +0200 Subject: [PATCH 07/14] parallel version of compute error --- src/vi_estimator/ba_base.cpp | 211 +++++++++++++++++++---------------- 1 file changed, 117 insertions(+), 94 deletions(-) diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 92c4e22..2b823bb 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -37,6 +37,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + namespace basalt { Sophus::SE3d BundleAdjustmentBase::computeRelPose(const Sophus::SE3d& T_w_i_h, @@ -129,101 +131,119 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, } void BundleAdjustmentBase::computeError( - double& error, + double& error_all, std::map>>* outliers, double outlier_threshold) const { - error = 0; + error_all = 0; + std::vector obs_tcid_vec; for (const auto& kv : lmdb.getObservations()) { - const TimeCamId& tcid_h = kv.first; - - for (const auto& obs_kv : kv.second) { - const TimeCamId& tcid_t = obs_kv.first; - - if (tcid_h != tcid_t) { - PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id); - PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id); - - Sophus::SE3d T_t_h_sophus = - computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id], - state_t.getPose(), calib.T_i_c[tcid_t.cam_id]); - - Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); - - std::visit( - [&](const auto& cam) { - for (size_t i = 0; i < obs_kv.second.size(); i++) { - const KeypointObservation& kpt_obs = obs_kv.second[i]; - const KeypointPosition& kpt_pos = - lmdb.getLandmark(kpt_obs.kpt_id); - - Eigen::Vector2d res; - - bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); - - if (valid) { - double e = res.norm(); - - if (outliers && e > outlier_threshold) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e); - } - - double huber_weight = - e < huber_thresh ? 1.0 : huber_thresh / e; - double obs_weight = - huber_weight / (obs_std_dev * obs_std_dev); - - error += - (2 - huber_weight) * obs_weight * res.transpose() * res; - } else { - if (outliers) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); - } - } - } - }, - calib.intrinsics[tcid_t.cam_id].variant); - - } else { - // target and host are the same - // residual does not depend on the pose - // it just depends on the point - - std::visit( - [&](const auto& cam) { - for (size_t i = 0; i < obs_kv.second.size(); i++) { - const KeypointObservation& kpt_obs = obs_kv.second[i]; - const KeypointPosition& kpt_pos = - lmdb.getLandmark(kpt_obs.kpt_id); - - Eigen::Vector2d res; - - bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); - if (valid) { - double e = res.norm(); - - if (outliers && e > outlier_threshold) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); - } - - double huber_weight = - e < huber_thresh ? 1.0 : huber_thresh / e; - double obs_weight = - huber_weight / (obs_std_dev * obs_std_dev); - - error += - (2 - huber_weight) * obs_weight * res.transpose() * res; - } else { - if (outliers) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); - } - } - } - }, - calib.intrinsics[tcid_t.cam_id].variant); - } - } + obs_tcid_vec.emplace_back(kv.first); } + + std::mutex res_mutex; + + tbb::parallel_for( + tbb::blocked_range(0, obs_tcid_vec.size()), + [&](const tbb::blocked_range& range) { + double error = 0; + + for (size_t r = range.begin(); r != range.end(); ++r) { + auto kv = lmdb.getObservations().find(obs_tcid_vec[r]); + const TimeCamId& tcid_h = kv->first; + + for (const auto& obs_kv : kv->second) { + const TimeCamId& tcid_t = obs_kv.first; + + if (tcid_h != tcid_t) { + PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id); + PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id); + + Sophus::SE3d T_t_h_sophus = + computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id], + state_t.getPose(), calib.T_i_c[tcid_t.cam_id]); + + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = + linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); + + if (valid) { + double e = res.norm(); + + if (outliers && e > outlier_threshold) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e); + } + + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += (2 - huber_weight) * obs_weight * + res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); + } + } + } + }, + calib.intrinsics[tcid_t.cam_id].variant); + + } else { + // target and host are the same + // residual does not depend on the pose + // it just depends on the point + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); + if (valid) { + double e = res.norm(); + + if (outliers && e > outlier_threshold) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } + + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += (2 - huber_weight) * obs_weight * + res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } + } + } + }, + calib.intrinsics[tcid_t.cam_id].variant); + } + } + } + + std::scoped_lock l(res_mutex); + error_all += error; + }); } void BundleAdjustmentBase::linearizeHelper( @@ -465,8 +485,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 +519,8 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold, } } - // std::cout << "============================================" << std::endl; + // std::cout << "============================================" << + // std::endl; } void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, @@ -552,7 +574,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; From 3d6a4099cf86e3ef5550e30fdbbb79c470988931 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Thu, 8 Aug 2019 18:02:21 +0200 Subject: [PATCH 08/14] Added LM to mapping --- include/basalt/vi_estimator/nfr_mapper.h | 2 + src/vi_estimator/nfr_mapper.cpp | 144 +++++++++++++++-------- 2 files changed, 98 insertions(+), 48 deletions(-) diff --git a/include/basalt/vi_estimator/nfr_mapper.h b/include/basalt/vi_estimator/nfr_mapper.h index c458d12..e939ffa 100644 --- a/include/basalt/vi_estimator/nfr_mapper.h +++ b/include/basalt/vi_estimator/nfr_mapper.h @@ -205,5 +205,7 @@ class NfrMapper : public BundleAdjustmentBase { std::shared_ptr> hash_bow_database; VioConfig config; + + double lambda, min_lambda, max_lambda, lambda_vee; }; } // namespace basalt diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index fa4ebcd..88bcbb0 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(1e-10), + min_lambda(1e-32), + max_lambda(100), + 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 @@ -285,60 +289,103 @@ void NfrMapper::optimize(int num_iterations) { lopt.accum.setup_solver(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); - Hdiag.setConstant(Hdiag.size(), 1e-6); - const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag); + bool converged = false; + bool step = false; + int max_iter = 10; - // 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)); + 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; + } } - // 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 +555,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; From be249f6354a9eb471b1afe33e08b5b8a560fe40a Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Thu, 8 Aug 2019 18:45:39 +0200 Subject: [PATCH 09/14] non-parallel compute_error --- src/vi_estimator/ba_base.cpp | 198 +++++++++++-------------- src/vi_estimator/landmark_database.cpp | 19 +++ src/vi_estimator/nfr_mapper.cpp | 4 +- 3 files changed, 111 insertions(+), 110 deletions(-) diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 2b823bb..f5d8f33 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -131,119 +131,101 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, } void BundleAdjustmentBase::computeError( - double& error_all, + double& error, std::map>>* outliers, double outlier_threshold) const { - error_all = 0; + error = 0; - std::vector obs_tcid_vec; for (const auto& kv : lmdb.getObservations()) { - obs_tcid_vec.emplace_back(kv.first); + const TimeCamId& tcid_h = kv.first; + + for (const auto& obs_kv : kv.second) { + const TimeCamId& tcid_t = obs_kv.first; + + if (tcid_h != tcid_t) { + PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id); + PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id); + + Sophus::SE3d T_t_h_sophus = + computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id], + state_t.getPose(), calib.T_i_c[tcid_t.cam_id]); + + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); + + if (valid) { + double e = res.norm(); + + if (outliers && e > outlier_threshold) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e); + } + + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += + (2 - huber_weight) * obs_weight * res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); + } + } + } + }, + calib.intrinsics[tcid_t.cam_id].variant); + + } else { + // target and host are the same + // residual does not depend on the pose + // it just depends on the point + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); + if (valid) { + double e = res.norm(); + + if (outliers && e > outlier_threshold) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } + + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += + (2 - huber_weight) * obs_weight * res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } + } + } + }, + calib.intrinsics[tcid_t.cam_id].variant); + } + } } - - std::mutex res_mutex; - - tbb::parallel_for( - tbb::blocked_range(0, obs_tcid_vec.size()), - [&](const tbb::blocked_range& range) { - double error = 0; - - for (size_t r = range.begin(); r != range.end(); ++r) { - auto kv = lmdb.getObservations().find(obs_tcid_vec[r]); - const TimeCamId& tcid_h = kv->first; - - for (const auto& obs_kv : kv->second) { - const TimeCamId& tcid_t = obs_kv.first; - - if (tcid_h != tcid_t) { - PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id); - PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id); - - Sophus::SE3d T_t_h_sophus = - computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id], - state_t.getPose(), calib.T_i_c[tcid_t.cam_id]); - - Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); - - std::visit( - [&](const auto& cam) { - for (size_t i = 0; i < obs_kv.second.size(); i++) { - const KeypointObservation& kpt_obs = obs_kv.second[i]; - const KeypointPosition& kpt_pos = - lmdb.getLandmark(kpt_obs.kpt_id); - - Eigen::Vector2d res; - - bool valid = - linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); - - if (valid) { - double e = res.norm(); - - if (outliers && e > outlier_threshold) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e); - } - - double huber_weight = - e < huber_thresh ? 1.0 : huber_thresh / e; - double obs_weight = - huber_weight / (obs_std_dev * obs_std_dev); - - error += (2 - huber_weight) * obs_weight * - res.transpose() * res; - } else { - if (outliers) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); - } - } - } - }, - calib.intrinsics[tcid_t.cam_id].variant); - - } else { - // target and host are the same - // residual does not depend on the pose - // it just depends on the point - - std::visit( - [&](const auto& cam) { - for (size_t i = 0; i < obs_kv.second.size(); i++) { - const KeypointObservation& kpt_obs = obs_kv.second[i]; - const KeypointPosition& kpt_pos = - lmdb.getLandmark(kpt_obs.kpt_id); - - Eigen::Vector2d res; - - bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); - if (valid) { - double e = res.norm(); - - if (outliers && e > outlier_threshold) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); - } - - double huber_weight = - e < huber_thresh ? 1.0 : huber_thresh / e; - double obs_weight = - huber_weight / (obs_std_dev * obs_std_dev); - - error += (2 - huber_weight) * obs_weight * - res.transpose() * res; - } else { - if (outliers) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); - } - } - } - }, - calib.intrinsics[tcid_t.cam_id].variant); - } - } - } - - std::scoped_lock l(res_mutex); - error_all += error; - }); } void BundleAdjustmentBase::linearizeHelper( 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 88bcbb0..949119d 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -47,7 +47,7 @@ NfrMapper::NfrMapper(const Calibration& calib, const VioConfig& config) : config(config), lambda(1e-10), min_lambda(1e-32), - max_lambda(100), + max_lambda(1000), lambda_vee(2) { this->calib = calib; this->obs_std_dev = config.mapper_obs_std_dev; @@ -288,7 +288,7 @@ void NfrMapper::optimize(int num_iterations) { lopt.accum.print_info = true; lopt.accum.setup_solver(); - Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); + const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); bool converged = false; bool step = false; From 4e4901dfa407a68fd62ef9a962fb7f83d0259c58 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Fri, 9 Aug 2019 11:51:04 +0200 Subject: [PATCH 10/14] update config --- src/vi_estimator/keypoint_vio.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 5b7ac4c..c50c5dd 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -54,8 +54,8 @@ KeypointVioEstimator::KeypointVioEstimator( g(g), initialized(false), config(config), - lambda(1e-6), - min_lambda(1e-14), + lambda(1e-10), + min_lambda(1e-32), max_lambda(100), lambda_vee(2) { this->obs_std_dev = config.vio_obs_std_dev; From aaa82fba3c09562fbedc7038cf02df5b16da599b Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 12 Aug 2019 19:14:47 +0200 Subject: [PATCH 11/14] Added LM options --- data/euroc_config.json | 8 +- data/tumvi_512_config.json | 8 +- include/basalt/utils/vio_config.h | 8 ++ src/utils/vio_config.cpp | 16 +++ src/vi_estimator/keypoint_vio.cpp | 161 ++++++++++++++++++------------ 5 files changed, 137 insertions(+), 64 deletions(-) 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/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/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/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/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index c50c5dd..9941f75 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -54,9 +54,9 @@ KeypointVioEstimator::KeypointVioEstimator( g(g), initialized(false), config(config), - lambda(1e-10), - min_lambda(1e-32), - max_lambda(100), + 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; @@ -945,18 +945,107 @@ void KeypointVioEstimator::optimize() { double error_total = rld_error + imu_error + marg_prior_error + ba_error + bg_error; - std::cout << "[LINEARIZE] Error: " << error_total << " num points " - << std::endl; + if (config.vio_debug) + std::cout << "[LINEARIZE] Error: " << error_total << " num points " + << std::endl; lopt.accum.setup_solver(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); bool converged = false; - bool step = false; - int max_iter = 10; - while (!step && max_iter > 0 && !converged) { - Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; + 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-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--; + } + + 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); @@ -964,8 +1053,6 @@ void KeypointVioEstimator::optimize() { 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; @@ -987,56 +1074,6 @@ void KeypointVioEstimator::optimize() { } }; 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) { - 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 { - 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--; - } - - if (converged) { - std::cout << "[CONVERGED]" << std::endl; } if (config.vio_debug) { @@ -1099,7 +1136,7 @@ void KeypointVioEstimator::optimize() { if (config.vio_debug) { std::cout << "=================================" << std::endl; } -} +} // namespace basalt void KeypointVioEstimator::computeProjections( std::vector>& data) const { From 7ebb7baf7c23806337b5a2b98e7f7e947256ac15 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 12 Aug 2019 19:24:25 +0200 Subject: [PATCH 12/14] update mapper --- src/vi_estimator/nfr_mapper.cpp | 154 +++++++++++++++++++------------- 1 file changed, 91 insertions(+), 63 deletions(-) diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 949119d..b0c8c44 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -45,9 +45,9 @@ namespace basalt { NfrMapper::NfrMapper(const Calibration& calib, const VioConfig& config) : config(config), - lambda(1e-10), - min_lambda(1e-32), - max_lambda(1000), + 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; @@ -291,11 +291,95 @@ void NfrMapper::optimize(int num_iterations) { const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); bool converged = false; - bool step = false; - int max_iter = 10; - while (!step && max_iter > 0 && !converged) { - Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; + 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); @@ -303,8 +387,6 @@ void NfrMapper::optimize(int num_iterations) { 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; @@ -321,60 +403,6 @@ void NfrMapper::optimize(int num_iterations) { } }; 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; - } } auto t2 = std::chrono::high_resolution_clock::now(); From f4f16ddc56f766fd0c1c1099efbf7732ba19a805 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 12 Aug 2019 19:33:00 +0200 Subject: [PATCH 13/14] small fix --- src/vi_estimator/ba_base.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index f5d8f33..a4edc77 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -37,8 +37,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include - namespace basalt { Sophus::SE3d BundleAdjustmentBase::computeRelPose(const Sophus::SE3d& T_w_i_h, From a2eeb002906af0193ab01b0d78ac9f9a0fac26f2 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 12 Aug 2019 19:36:44 +0200 Subject: [PATCH 14/14] update config --- data/euroc_config_no_weights.json | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 } }