From 5bb572cdeb75afc494c8e19cbdb71767de74b72f Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Fri, 26 Jul 2019 16:50:15 +0000 Subject: [PATCH] Added Landmark database --- CMakeLists.txt | 1 + data/euroc_config.json | 5 +- data/euroc_ds_calib.json | 48 ++-- data/euroc_eucm_calib.json | 6 +- data/tumvi_512_config.json | 5 +- include/basalt/utils/vio_config.h | 5 +- include/basalt/vi_estimator/ba_base.h | 30 +-- .../basalt/vi_estimator/landmark_database.h | 107 +++++++++ scripts/install_mac_os_deps.sh | 3 +- src/mapper.cpp | 14 +- src/mapper_sim.cpp | 2 +- src/utils/vio_config.cpp | 7 +- src/vi_estimator/ba_base.cpp | 106 +++++++-- src/vi_estimator/keypoint_vio.cpp | 64 +++--- src/vi_estimator/landmark_database.cpp | 206 ++++++++++++++++++ src/vi_estimator/nfr_mapper.cpp | 7 +- test/src/test_vio.cpp | 6 +- 17 files changed, 506 insertions(+), 116 deletions(-) create mode 100644 include/basalt/vi_estimator/landmark_database.h create mode 100644 src/vi_estimator/landmark_database.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 80b7253..b56ac9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -214,6 +214,7 @@ add_library(basalt SHARED src/vi_estimator/vio_estimator.cpp src/vi_estimator/ba_base.cpp src/vi_estimator/nfr_mapper.cpp + src/vi_estimator/landmark_database.cpp src/utils/keypoints.cpp) diff --git a/data/euroc_config.json b/data/euroc_config.json index 06ec434..2fee0d4 100644 --- a/data/euroc_config.json +++ b/data/euroc_config.json @@ -12,11 +12,14 @@ "config.vio_max_kfs": 7, "config.vio_min_frames_after_kf": 5, "config.vio_new_kf_keypoints_thresh": 0.7, - "config.vio_max_iterations": 5, "config.vio_debug": false, "config.vio_obs_std_dev": 0.5, "config.vio_obs_huber_thresh": 1.0, "config.vio_min_triangulation_dist": 0.05, + "config.vio_outlier_threshold": 3.0, + "config.vio_filter_iteration": 4, + "config.vio_max_iterations": 7, + "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, diff --git a/data/euroc_ds_calib.json b/data/euroc_ds_calib.json index 210fd34..25c22a0 100644 --- a/data/euroc_ds_calib.json +++ b/data/euroc_ds_calib.json @@ -159,30 +159,30 @@ } ], "calib_accel_bias": [ - 0.001725405479279035, - 0.1500005286487319, - 0.06708820471592454, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "calib_gyro_bias": [ - -0.002186848441668376, - 0.024427823167917037, - 0.07668367023977922, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], + -0.003025405479279035, + 0.1200005286487319, + 0.06708820471592454, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "calib_gyro_bias": [ + -0.002186848441668376, + 0.020427823167917037, + 0.07668367023977922, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], "imu_update_rate": 200.0, "accel_noise_std": [ 0.016, diff --git a/data/euroc_eucm_calib.json b/data/euroc_eucm_calib.json index e23a5bb..347d6b0 100644 --- a/data/euroc_eucm_calib.json +++ b/data/euroc_eucm_calib.json @@ -159,8 +159,8 @@ } ], "calib_accel_bias": [ - 0.001725405479279035, - 0.1500005286487319, + -0.003025405479279035, + 0.1200005286487319, 0.06708820471592454, 0.0, 0.0, @@ -171,7 +171,7 @@ ], "calib_gyro_bias": [ -0.002186848441668376, - 0.024427823167917037, + 0.020427823167917037, 0.07668367023977922, 0.0, 0.0, diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index dc9114f..de31800 100644 --- a/data/tumvi_512_config.json +++ b/data/tumvi_512_config.json @@ -8,15 +8,18 @@ "config.optical_flow_epipolar_error": 0.005, "config.optical_flow_levels": 3, "config.optical_flow_skip_frames": 1, + "config.vio_max_states": 3, "config.vio_max_kfs": 7, "config.vio_min_frames_after_kf": 5, "config.vio_new_kf_keypoints_thresh": 0.7, - "config.vio_max_iterations": 5, "config.vio_debug": false, "config.vio_obs_std_dev": 0.5, "config.vio_obs_huber_thresh": 1.0, "config.vio_min_triangulation_dist": 0.05, + "config.vio_outlier_threshold": 3.0, + "config.vio_filter_iteration": 4, + "config.vio_max_iterations": 7, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index eed8667..6d79528 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -56,9 +56,12 @@ struct VioConfig { int vio_max_kfs; int vio_min_frames_after_kf; float vio_new_kf_keypoints_thresh; - int vio_max_iterations; bool vio_debug; + double vio_outlier_threshold; + int vio_filter_iteration; + int vio_max_iterations; + double vio_obs_std_dev; double vio_obs_huber_thresh; double vio_min_triangulation_dist; diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h index 43f961e..dfdb604 100644 --- a/include/basalt/vi_estimator/ba_base.h +++ b/include/basalt/vi_estimator/ba_base.h @@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once -#include +#include #include @@ -42,22 +42,6 @@ namespace basalt { class BundleAdjustmentBase { public: - // keypoint position defined relative to some frame - struct KeypointPosition { - TimeCamId kf_id; - Eigen::Vector2d dir; - double id; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - struct KeypointObservation { - int kpt_id; - Eigen::Vector2d pos; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - struct RelLinDataBase { std::vector> order; @@ -113,7 +97,10 @@ class BundleAdjustmentBase { double error; }; - void computeError(double& error) const; + void computeError(double& error, + std::map>>* + outliers = nullptr, + double outlier_threshold = 0) const; void linearizeHelper( Eigen::vector& rld_vec, @@ -125,6 +112,8 @@ class BundleAdjustmentBase { static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H, Eigen::VectorXd& b); + void filterOutliers(double outlier_threshold, int min_num_obs); + template static bool linearizePoint( const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos, @@ -397,10 +386,7 @@ class BundleAdjustmentBase { Eigen::map frame_poses; // Point management - Eigen::unordered_map kpts; - Eigen::map>> - obs; + LandmarkDatabase lmdb; double obs_std_dev; double huber_thresh; diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h new file mode 100644 index 0000000..40543af --- /dev/null +++ b/include/basalt/vi_estimator/landmark_database.h @@ -0,0 +1,107 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#pragma once + +#include + +namespace basalt { + +// keypoint position defined relative to some frame +struct KeypointPosition { + TimeCamId kf_id; + Eigen::Vector2d dir; + double id; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct KeypointObservation { + int kpt_id; + Eigen::Vector2d pos; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class LandmarkDatabase { + public: + // Non-const + void addLandmark(int lm_id, const KeypointPosition& pos); + + void removeKeyframes(const std::set& kfs_to_marg, + const std::set& poses_to_marg, + const std::set& states_to_marg_all); + + void addObservation(const TimeCamId& tcid_target, + const KeypointObservation& o); + + KeypointPosition& getLandmark(int lm_id); + + // Const + const KeypointPosition& getLandmark(int lm_id) const; + + std::vector getHostKfs() const; + + std::vector getLandmarksForHost( + const TimeCamId& tcid) const; + + const Eigen::map>>& + getObservations() const; + + bool landmarkExists(int lm_id) const; + + size_t numLandmarks() const; + + int numObservations() const; + + int numObservations(int lm_id) const; + + void removeLandmark(int lm_id); + + void removeObservations(int lm_id, const std::set& obs); + + private: + Eigen::unordered_map kpts; + Eigen::map>> + obs; + + std::unordered_map> host_to_kpts; + + int num_observations = 0; + Eigen::unordered_map kpts_num_obs; +}; + +} // namespace basalt diff --git a/scripts/install_mac_os_deps.sh b/scripts/install_mac_os_deps.sh index 88c445b..8e148c4 100755 --- a/scripts/install_mac_os_deps.sh +++ b/scripts/install_mac_os_deps.sh @@ -10,6 +10,7 @@ brew install \ tbb \ glew \ eigen \ - ccache + ccache \ + lz4 brew install llvm diff --git a/src/mapper.cpp b/src/mapper.cpp index 8b78723..8ff5f61 100644 --- a/src/mapper.cpp +++ b/src/mapper.cpp @@ -108,6 +108,7 @@ void detect(); void match(); void tracks(); void optimize(); +void filter(); constexpr int UI_WIDTH = 200; @@ -135,6 +136,10 @@ Button detect_btn("ui.detect", &detect); Button match_btn("ui.match", &match); Button tracks_btn("ui.tracks", &tracks); Button optimize_btn("ui.optimize", &optimize); + +pangolin::Var outlier_threshold("ui.outlier_threshold", 3.0, 0.01, 10); + +Button filter_btn("ui.filter", &filter); Button align_btn("ui.aling_svd", &alignButton); pangolin::OpenGlRenderState camera; @@ -325,6 +330,8 @@ int main(int argc, char** argv) { match(); tracks(); optimize(); + filter(); + optimize(); if (!result_path.empty()) { double error = alignButton(); @@ -562,7 +569,7 @@ void load_data(const std::string& calib_path, const std::string& cache_path) { void computeEdgeVis() { edges_vis.clear(); - for (const auto& kv1 : nrf_mapper->obs) { + for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) { for (const auto& kv2 : kv1.second) { Eigen::Vector3d p1 = nrf_mapper->getFramePoses() .at(kv1.first.frame_id) @@ -644,3 +651,8 @@ void tracks() { // mapper_point_ids); computeEdgeVis(); } + +void filter() { + nrf_mapper->filterOutliers(outlier_threshold, 4); + nrf_mapper->get_current_points(mapper_points, mapper_point_ids); +} diff --git a/src/mapper_sim.cpp b/src/mapper_sim.cpp index 5b824a6..859904c 100644 --- a/src/mapper_sim.cpp +++ b/src/mapper_sim.cpp @@ -479,7 +479,7 @@ void extractNonlinearFactors(basalt::MargData& m) { void computeEdgeVis() { edges_vis.clear(); - for (const auto& kv1 : nrf_mapper->obs) { + for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) { for (const auto& kv2 : kv1.second) { Eigen::Vector3d p1 = nrf_mapper->getFramePoses() .at(kv1.first.frame_id) diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 2196372..9b7c272 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -57,11 +57,14 @@ VioConfig::VioConfig() { vio_max_kfs = 7; vio_min_frames_after_kf = 5; vio_new_kf_keypoints_thresh = 0.7; - vio_max_iterations = 5; + vio_debug = false; vio_obs_std_dev = 0.5; vio_obs_huber_thresh = 1.0; vio_min_triangulation_dist = 0.05; + vio_outlier_threshold = 3.0; + vio_filter_iteration = 4; + vio_max_iterations = 7; mapper_obs_std_dev = 0.25; mapper_obs_huber_thresh = 1.5; @@ -118,6 +121,8 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.vio_new_kf_keypoints_thresh)); ar(CEREAL_NVP(config.vio_debug)); ar(CEREAL_NVP(config.vio_max_iterations)); + ar(CEREAL_NVP(config.vio_outlier_threshold)); + ar(CEREAL_NVP(config.vio_filter_iteration)); ar(CEREAL_NVP(config.vio_obs_std_dev)); ar(CEREAL_NVP(config.vio_obs_huber_thresh)); diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index a9cc7ae..92c4e22 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -120,7 +120,7 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, Eigen::Vector3d inc_p = rld.Hll.at(lm_idx) * (rld.bl.at(lm_idx) - H_l_p_x); - KeypointPosition& kpt = kpts[lm_idx]; + KeypointPosition& kpt = lmdb.getLandmark(lm_idx); kpt.dir -= inc_p.head<2>(); kpt.id -= inc_p[2]; @@ -128,10 +128,13 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, } } -void BundleAdjustmentBase::computeError(double& error) const { +void BundleAdjustmentBase::computeError( + double& error, + std::map>>* outliers, + double outlier_threshold) const { error = 0; - for (const auto& kv : obs) { + for (const auto& kv : lmdb.getObservations()) { const TimeCamId& tcid_h = kv.first; for (const auto& obs_kv : kv.second) { @@ -151,7 +154,8 @@ void BundleAdjustmentBase::computeError(double& error) const { [&](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 = kpts.at(kpt_obs.kpt_id); + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); Eigen::Vector2d res; @@ -159,6 +163,11 @@ void BundleAdjustmentBase::computeError(double& error) const { 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 = @@ -166,6 +175,10 @@ void BundleAdjustmentBase::computeError(double& error) const { error += (2 - huber_weight) * obs_weight * res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); + } } } }, @@ -180,13 +193,19 @@ void BundleAdjustmentBase::computeError(double& error) const { [&](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 = kpts.at(kpt_obs.kpt_id); + 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 = @@ -194,6 +213,10 @@ void BundleAdjustmentBase::computeError(double& error) const { error += (2 - huber_weight) * obs_weight * res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } } } }, @@ -216,7 +239,7 @@ void BundleAdjustmentBase::linearizeHelper( std::vector obs_tcid_vec; for (const auto& kv : obs_to_lin) { obs_tcid_vec.emplace_back(kv.first); - rld_vec.emplace_back(kpts.size(), kv.second.size()); + rld_vec.emplace_back(lmdb.numLandmarks(), kv.second.size()); } tbb::parallel_for( @@ -264,7 +287,8 @@ void BundleAdjustmentBase::linearizeHelper( [&](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 = kpts.at(kpt_obs.kpt_id); + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); Eigen::Vector2d res; Eigen::Matrix d_res_d_xi; @@ -318,7 +342,8 @@ void BundleAdjustmentBase::linearizeHelper( [&](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 = kpts.at(kpt_obs.kpt_id); + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); Eigen::Vector2d res; Eigen::Matrix d_res_d_p; @@ -401,14 +426,10 @@ void BundleAdjustmentBase::get_current_points( points.clear(); ids.clear(); - for (const auto& kv_kpt : kpts) { - Eigen::Vector3d pt_cam = - StereographicParam::unproject(kv_kpt.second.dir).head<3>(); - pt_cam /= kv_kpt.second.id; - + for (const auto& tcid_host : lmdb.getHostKfs()) { Sophus::SE3d T_w_i; - int64_t id = kv_kpt.second.kf_id.frame_id; + int64_t id = tcid_host.frame_id; if (frame_states.count(id) > 0) { PoseVelBiasStateWithLin state = frame_states.at(id); T_w_i = state.getState().T_w_i; @@ -421,16 +442,65 @@ void BundleAdjustmentBase::get_current_points( std::abort(); } - const Sophus::SE3d& T_i_c = calib.T_i_c[kv_kpt.second.kf_id.cam_id]; + const Sophus::SE3d& T_i_c = calib.T_i_c[tcid_host.cam_id]; + Eigen::Matrix4d T_w_c = (T_w_i * T_i_c).matrix(); - // std::cerr << "T_w_i\n" << T_w_i.matrix() << std::endl; + for (const KeypointPosition& kpt_pos : + lmdb.getLandmarksForHost(tcid_host)) { + Eigen::Vector4d pt_cam = + StereographicParam::unproject(kpt_pos.dir); + pt_cam[3] = kpt_pos.id; - points.emplace_back(T_w_i * T_i_c * pt_cam); + Eigen::Vector4d pt_w = T_w_c * pt_cam; - ids.emplace_back(kv_kpt.first); + points.emplace_back(pt_w.head<3>() / pt_w[3]); + ids.emplace_back(1); + } } } +void BundleAdjustmentBase::filterOutliers(double outlier_threshold, + int min_num_obs) { + double error; + std::map>> outliers; + computeError(error, &outliers, outlier_threshold); + + // std::cout << "============================================" << std::endl; + // std::cout << "Num landmarks: " << lmdb.numLandmarks() << " with outliners + // " + // << outliers.size() << std::endl; + + for (const auto& kv : outliers) { + int num_obs = lmdb.numObservations(kv.first); + int num_outliers = kv.second.size(); + + bool remove = false; + + if (num_obs - num_outliers < min_num_obs) remove = true; + + // std::cout << "\tlm_id: " << kv.first << " num_obs: " << num_obs + // << " outliers: " << num_outliers << " ["; + + for (const auto& kv2 : kv.second) { + if (kv2.second == -2) remove = true; + + // std::cout << kv2.second << ", "; + } + + // std::cout << "] " << std::endl; + + if (remove) { + lmdb.removeLandmark(kv.first); + } else { + std::set outliers; + for (const auto& kv2 : kv.second) outliers.emplace(kv2.first); + lmdb.removeObservations(kv.first, outliers); + } + } + + // std::cout << "============================================" << std::endl; +} + void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, const std::set& idx_to_keep, diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index e49c9db..6908a8c 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -70,8 +70,8 @@ KeypointVioEstimator::KeypointVioEstimator( marg_H(5, 5) = prior_weight; // small prior to avoid jumps in bias - marg_H.diagonal().segment<3>(9).array() = 1e1; - marg_H.diagonal().segment<3>(12).array() = 1e2; + marg_H.diagonal().segment<3>(9).array() = 1e2; + marg_H.diagonal().segment<3>(12).array() = 1e3; std::cout << "marg_H\n" << marg_H << std::endl; @@ -119,6 +119,8 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, ImuData::Ptr data; imu_data_queue.pop(data); + data->accel = calib.calib_accel_bias.getCalibrated(data->accel); + data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); while (true) { vision_data_queue.pop(curr_frame); @@ -163,14 +165,17 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, while (data->t_ns <= prev_frame->t_ns) { imu_data_queue.pop(data); - if (!data.get()) break; + data->accel = calib.calib_accel_bias.getCalibrated(data->accel); + data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); } while (data->t_ns <= curr_frame->t_ns) { meas->integrate(*data, accel_cov, gyro_cov); imu_data_queue.pop(data); if (!data.get()) break; + data->accel = calib.calib_accel_bias.getCalibrated(data->accel); + data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); } if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) { @@ -240,15 +245,15 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, for (const auto& kv_obs : opt_flow_meas->observations[i]) { int kpt_id = kv_obs.first; - auto it = kpts.find(kpt_id); - if (it != kpts.end()) { - const TimeCamId& tcid_host = it->second.kf_id; + if (lmdb.landmarkExists(kpt_id)) { + const TimeCamId& tcid_host = lmdb.getLandmark(kpt_id).kf_id; KeypointObservation kobs; kobs.kpt_id = kpt_id; kobs.pos = kv_obs.second.translation().cast(); - obs[tcid_host][tcid_target].push_back(kobs); + lmdb.addObservation(tcid_target, kobs); + // obs[tcid_host][tcid_target].push_back(kobs); if (num_points_connected.count(tcid_host.frame_id) == 0) { num_points_connected[tcid_host.frame_id] = 0; @@ -343,7 +348,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, kpt_pos.kf_id = tcidl; kpt_pos.dir = StereographicParam::project(p0_triangulated); kpt_pos.id = p0_triangulated[3]; - kpts[lm_id] = kpt_pos; + lmdb.addLandmark(lm_id, kpt_pos); num_points_added++; valid_kp = true; @@ -352,7 +357,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, if (valid_kp) { for (const auto& kv_obs : kp_obs) { - obs[tcidl][kv_obs.first].push_back(kv_obs.second); + lmdb.addObservation(kv_obs.first, kv_obs.second); } } } @@ -674,17 +679,16 @@ void KeypointVioEstimator::marginalize( Eigen::map>> obs_to_lin; - for (auto it = obs.cbegin(); it != obs.cend();) { + for (auto it = lmdb.getObservations().cbegin(); + it != lmdb.getObservations().cend();) { if (kfs_to_marg.count(it->first.frame_id) > 0) { for (auto it2 = it->second.cbegin(); it2 != it->second.cend(); ++it2) { if (it2->first.frame_id <= last_state_to_marg) obs_to_lin[it->first].emplace(*it2); } - it = obs.erase(it); - } else { - ++it; } + ++it; } double rld_error; @@ -701,15 +705,6 @@ void KeypointVioEstimator::marginalize( linearizeAbs(rel_H, rel_b, rld, aom, accum); } - - // remove points - for (auto it = kpts.cbegin(); it != kpts.cend();) { - if (kfs_to_marg.count(it->second.kf_id.frame_id) > 0) { - it = kpts.erase(it); - } else { - ++it; - } - } } linearizeAbsIMU(aom, accum.getH(), accum.getB(), imu_error, bg_error, @@ -809,16 +804,7 @@ void KeypointVioEstimator::marginalize( prev_opt_flow_res.erase(id); } - for (auto it = obs.begin(); it != obs.end(); ++it) { - for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) { - if (poses_to_marg.count(it2->first.frame_id) > 0 || - states_to_marg_all.count(it2->first.frame_id) > 0) { - it2 = it->second.erase(it2); - } else { - ++it2; - } - } - } + lmdb.removeKeyframes(kfs_to_marg, poses_to_marg, states_to_marg_all); AbsOrderMap marg_order_new; @@ -925,7 +911,7 @@ void KeypointVioEstimator::optimize() { double rld_error; Eigen::vector rld_vec; - linearizeHelper(rld_vec, obs, rld_error); + linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); BundleAdjustmentBase::LinearizeAbsReduce> lopt( aom); @@ -1022,6 +1008,10 @@ void KeypointVioEstimator::optimize() { } } + if (iter == config.vio_filter_iteration) { + filterOutliers(config.vio_outlier_threshold, 4); + } + if (inc.array().abs().maxCoeff() < 1e-4) break; // std::cerr << "LT\n" << LT << std::endl; @@ -1037,7 +1027,7 @@ void KeypointVioEstimator::optimize() { void KeypointVioEstimator::computeProjections( std::vector>& data) const { - for (const auto& kv : obs) { + for (const auto& kv : lmdb.getObservations()) { const TimeCamId& tcid_h = kv.first; for (const auto& obs_kv : kv.second) { @@ -1061,7 +1051,8 @@ void KeypointVioEstimator::computeProjections( [&](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 = kpts.at(kpt_obs.kpt_id); + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); Eigen::Vector2d res; Eigen::Vector4d proj; @@ -1084,7 +1075,8 @@ void KeypointVioEstimator::computeProjections( [&](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 = kpts.at(kpt_obs.kpt_id); + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); Eigen::Vector2d res; Eigen::Vector4d proj; diff --git a/src/vi_estimator/landmark_database.cpp b/src/vi_estimator/landmark_database.cpp new file mode 100644 index 0000000..decdb35 --- /dev/null +++ b/src/vi_estimator/landmark_database.cpp @@ -0,0 +1,206 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +namespace basalt { + +void LandmarkDatabase::addLandmark(int lm_id, const KeypointPosition &pos) { + kpts[lm_id] = pos; + host_to_kpts[pos.kf_id].emplace(lm_id); +} + +void LandmarkDatabase::removeKeyframes( + const std::set &kfs_to_marg, + const std::set &poses_to_marg, + const std::set &states_to_marg_all) { + // remove points + for (auto it = kpts.cbegin(); it != kpts.cend();) { + if (kfs_to_marg.count(it->second.kf_id.frame_id) > 0) { + auto num_obs_it = kpts_num_obs.find(it->first); + num_observations -= num_obs_it->second; + kpts_num_obs.erase(num_obs_it); + + it = kpts.erase(it); + } else { + ++it; + } + } + + for (auto it = obs.cbegin(); it != obs.cend();) { + if (kfs_to_marg.count(it->first.frame_id) > 0) { + it = obs.erase(it); + } else { + ++it; + } + } + + for (auto it = obs.begin(); it != obs.end(); ++it) { + for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) { + if (poses_to_marg.count(it2->first.frame_id) > 0 || + states_to_marg_all.count(it2->first.frame_id) > 0 || + kfs_to_marg.count(it->first.frame_id) > 0) { + for (const auto &v : it2->second) kpts_num_obs.at(v.kpt_id)--; + + it2 = it->second.erase(it2); + } else { + ++it2; + } + } + } + + for (auto it = host_to_kpts.cbegin(); it != host_to_kpts.cend();) { + if (kfs_to_marg.count(it->first.frame_id) > 0 || + states_to_marg_all.count(it->first.frame_id) > 0 || + poses_to_marg.count(it->first.frame_id) > 0) { + it = host_to_kpts.erase(it); + } else { + ++it; + } + } +} + +std::vector LandmarkDatabase::getHostKfs() const { + std::vector res; + for (const auto &kv : obs) res.emplace_back(kv.first); + return res; +} + +std::vector LandmarkDatabase::getLandmarksForHost( + const TimeCamId &tcid) const { + std::vector res; + for (const auto &v : host_to_kpts.at(tcid)) res.emplace_back(kpts.at(v)); + return res; +} + +void LandmarkDatabase::addObservation(const TimeCamId &tcid_target, + const KeypointObservation &o) { + auto it = kpts.find(o.kpt_id); + BASALT_ASSERT(it != kpts.end()); + + auto &obs_vec = obs[it->second.kf_id][tcid_target]; + + // Check that the point observation is inserted only once + for (const auto &oo : obs_vec) { + BASALT_ASSERT(oo.kpt_id != o.kpt_id); + } + + obs_vec.emplace_back(o); + + num_observations++; + kpts_num_obs[o.kpt_id]++; +} + +KeypointPosition &LandmarkDatabase::getLandmark(int lm_id) { + return kpts.at(lm_id); +} + +const KeypointPosition &LandmarkDatabase::getLandmark(int lm_id) const { + return kpts.at(lm_id); +} + +const Eigen::map > > + &LandmarkDatabase::getObservations() const { + return obs; +} + +bool LandmarkDatabase::landmarkExists(int lm_id) const { + return kpts.count(lm_id) > 0; +} + +size_t LandmarkDatabase::numLandmarks() const { return kpts.size(); } + +int LandmarkDatabase::numObservations() const { return num_observations; } + +int LandmarkDatabase::numObservations(int lm_id) const { + return kpts_num_obs.at(lm_id); +} + +void LandmarkDatabase::removeLandmark(int lm_id) { + auto it = kpts.find(lm_id); + BASALT_ASSERT(it != kpts.end()); + + host_to_kpts.at(it->second.kf_id).erase(lm_id); + + for (auto &kv : obs.at(it->second.kf_id)) { + int idx = -1; + for (size_t i = 0; i < kv.second.size(); ++i) { + if (kv.second[i].kpt_id == lm_id) { + idx = i; + break; + } + } + + if (idx >= 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)--; + } + } + + 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); +} + +void LandmarkDatabase::removeObservations(int lm_id, + const std::set &outliers) { + auto it = kpts.find(lm_id); + BASALT_ASSERT(it != kpts.end()); + + for (auto &kv : obs.at(it->second.kf_id)) { + if (outliers.count(kv.first) > 0) { + int idx = -1; + for (size_t i = 0; i < kv.second.size(); i++) { + if (kv.second[i].kpt_id == lm_id) { + idx = i; + break; + } + } + BASALT_ASSERT(idx >= 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)--; + } + } +} + +} // namespace basalt diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 361cc89..fa4ebcd 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -244,7 +244,7 @@ void NfrMapper::optimize(int num_iterations) { double rld_error; Eigen::vector rld_vec; - linearizeHelper(rld_vec, obs, rld_error); + linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); // SparseHashAccumulator accum; // accum.reset(aom.total_size); @@ -662,14 +662,15 @@ void NfrMapper::setup_opt() { pos.dir = StereographicParam::project(pos_3d); pos.id = pos_3d[3]; - kpts[kv.first] = pos; + lmdb.addLandmark(kv.first, pos); for (const auto& obs_kv : kv.second) { KeypointObservation ko; ko.kpt_id = kv.first; ko.pos = feature_corners.at(obs_kv.first).corners[obs_kv.second]; - obs[tcid_h][obs_kv.first].emplace_back(ko); + lmdb.addObservation(obs_kv.first, ko); + // obs[tcid_h][obs_kv.first].emplace_back(ko); } break; } diff --git a/test/src/test_vio.cpp b/test/src/test_vio.cpp index c36e97a..6ffa531 100644 --- a/test/src/test_vio.cpp +++ b/test/src/test_vio.cpp @@ -338,7 +338,7 @@ TEST(VioTestSuite, LinearizePointsTest) { basalt::ExtendedUnifiedCamera cam = basalt::ExtendedUnifiedCamera::getTestProjections()[0]; - basalt::KeypointVioEstimator::KeypointPosition kpt_pos; + basalt::KeypointPosition kpt_pos; Eigen::Vector4d point3d; cam.unproject(Eigen::Vector2d::Random() * 50, point3d); @@ -358,7 +358,7 @@ TEST(VioTestSuite, LinearizePointsTest) { p_trans = T_t_h * p_trans; - basalt::KeypointVioEstimator::KeypointObservation kpt_obs; + basalt::KeypointObservation kpt_obs; cam.project(p_trans, kpt_obs.pos); Eigen::Vector2d res; @@ -391,7 +391,7 @@ TEST(VioTestSuite, LinearizePointsTest) { test_jacobian( "d_res_d_p", d_res_d_p, [&](const Eigen::Vector3d& x) { - basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos; + basalt::KeypointPosition kpt_pos_new = kpt_pos; kpt_pos_new.dir += x.head<2>(); kpt_pos_new.id += x[2];