Added Landmark database

This commit is contained in:
Vladyslav Usenko 2019-07-26 16:50:15 +00:00
parent d981339d18
commit 5bb572cdeb
17 changed files with 506 additions and 116 deletions

View File

@ -214,6 +214,7 @@ add_library(basalt SHARED
src/vi_estimator/vio_estimator.cpp src/vi_estimator/vio_estimator.cpp
src/vi_estimator/ba_base.cpp src/vi_estimator/ba_base.cpp
src/vi_estimator/nfr_mapper.cpp src/vi_estimator/nfr_mapper.cpp
src/vi_estimator/landmark_database.cpp
src/utils/keypoints.cpp) src/utils/keypoints.cpp)

View File

@ -12,11 +12,14 @@
"config.vio_max_kfs": 7, "config.vio_max_kfs": 7,
"config.vio_min_frames_after_kf": 5, "config.vio_min_frames_after_kf": 5,
"config.vio_new_kf_keypoints_thresh": 0.7, "config.vio_new_kf_keypoints_thresh": 0.7,
"config.vio_max_iterations": 5,
"config.vio_debug": false, "config.vio_debug": false,
"config.vio_obs_std_dev": 0.5, "config.vio_obs_std_dev": 0.5,
"config.vio_obs_huber_thresh": 1.0, "config.vio_obs_huber_thresh": 1.0,
"config.vio_min_triangulation_dist": 0.05, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -159,8 +159,8 @@
} }
], ],
"calib_accel_bias": [ "calib_accel_bias": [
0.001725405479279035, -0.003025405479279035,
0.1500005286487319, 0.1200005286487319,
0.06708820471592454, 0.06708820471592454,
0.0, 0.0,
0.0, 0.0,
@ -171,7 +171,7 @@
], ],
"calib_gyro_bias": [ "calib_gyro_bias": [
-0.002186848441668376, -0.002186848441668376,
0.024427823167917037, 0.020427823167917037,
0.07668367023977922, 0.07668367023977922,
0.0, 0.0,
0.0, 0.0,

View File

@ -159,8 +159,8 @@
} }
], ],
"calib_accel_bias": [ "calib_accel_bias": [
0.001725405479279035, -0.003025405479279035,
0.1500005286487319, 0.1200005286487319,
0.06708820471592454, 0.06708820471592454,
0.0, 0.0,
0.0, 0.0,
@ -171,7 +171,7 @@
], ],
"calib_gyro_bias": [ "calib_gyro_bias": [
-0.002186848441668376, -0.002186848441668376,
0.024427823167917037, 0.020427823167917037,
0.07668367023977922, 0.07668367023977922,
0.0, 0.0,
0.0, 0.0,

View File

@ -8,15 +8,18 @@
"config.optical_flow_epipolar_error": 0.005, "config.optical_flow_epipolar_error": 0.005,
"config.optical_flow_levels": 3, "config.optical_flow_levels": 3,
"config.optical_flow_skip_frames": 1, "config.optical_flow_skip_frames": 1,
"config.vio_max_states": 3, "config.vio_max_states": 3,
"config.vio_max_kfs": 7, "config.vio_max_kfs": 7,
"config.vio_min_frames_after_kf": 5, "config.vio_min_frames_after_kf": 5,
"config.vio_new_kf_keypoints_thresh": 0.7, "config.vio_new_kf_keypoints_thresh": 0.7,
"config.vio_max_iterations": 5,
"config.vio_debug": false, "config.vio_debug": false,
"config.vio_obs_std_dev": 0.5, "config.vio_obs_std_dev": 0.5,
"config.vio_obs_huber_thresh": 1.0, "config.vio_obs_huber_thresh": 1.0,
"config.vio_min_triangulation_dist": 0.05, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -56,9 +56,12 @@ struct VioConfig {
int vio_max_kfs; int vio_max_kfs;
int vio_min_frames_after_kf; int vio_min_frames_after_kf;
float vio_new_kf_keypoints_thresh; float vio_new_kf_keypoints_thresh;
int vio_max_iterations;
bool vio_debug; bool vio_debug;
double vio_outlier_threshold;
int vio_filter_iteration;
int vio_max_iterations;
double vio_obs_std_dev; double vio_obs_std_dev;
double vio_obs_huber_thresh; double vio_obs_huber_thresh;
double vio_min_triangulation_dist; double vio_min_triangulation_dist;

View File

@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#pragma once #pragma once
#include <basalt/utils/imu_types.h> #include <basalt/vi_estimator/landmark_database.h>
#include <tbb/blocked_range.h> #include <tbb/blocked_range.h>
@ -42,22 +42,6 @@ namespace basalt {
class BundleAdjustmentBase { class BundleAdjustmentBase {
public: 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 { struct RelLinDataBase {
std::vector<std::pair<TimeCamId, TimeCamId>> order; std::vector<std::pair<TimeCamId, TimeCamId>> order;
@ -113,7 +97,10 @@ class BundleAdjustmentBase {
double error; double error;
}; };
void computeError(double& error) const; void computeError(double& error,
std::map<int, std::vector<std::pair<TimeCamId, double>>>*
outliers = nullptr,
double outlier_threshold = 0) const;
void linearizeHelper( void linearizeHelper(
Eigen::vector<RelLinData>& rld_vec, Eigen::vector<RelLinData>& rld_vec,
@ -125,6 +112,8 @@ class BundleAdjustmentBase {
static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H, static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H,
Eigen::VectorXd& b); Eigen::VectorXd& b);
void filterOutliers(double outlier_threshold, int min_num_obs);
template <class CamT> template <class CamT>
static bool linearizePoint( static bool linearizePoint(
const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos, const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
@ -397,10 +386,7 @@ class BundleAdjustmentBase {
Eigen::map<int64_t, PoseStateWithLin> frame_poses; Eigen::map<int64_t, PoseStateWithLin> frame_poses;
// Point management // Point management
Eigen::unordered_map<int, KeypointPosition> kpts; LandmarkDatabase lmdb;
Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
obs;
double obs_std_dev; double obs_std_dev;
double huber_thresh; double huber_thresh;

View File

@ -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 <basalt/utils/imu_types.h>
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<FrameId>& kfs_to_marg,
const std::set<FrameId>& poses_to_marg,
const std::set<FrameId>& 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<TimeCamId> getHostKfs() const;
std::vector<KeypointPosition> getLandmarksForHost(
const TimeCamId& tcid) const;
const Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>&
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<TimeCamId>& obs);
private:
Eigen::unordered_map<int, KeypointPosition> kpts;
Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
obs;
std::unordered_map<TimeCamId, std::set<int>> host_to_kpts;
int num_observations = 0;
Eigen::unordered_map<int, int> kpts_num_obs;
};
} // namespace basalt

View File

@ -10,6 +10,7 @@ brew install \
tbb \ tbb \
glew \ glew \
eigen \ eigen \
ccache ccache \
lz4
brew install llvm brew install llvm

View File

@ -108,6 +108,7 @@ void detect();
void match(); void match();
void tracks(); void tracks();
void optimize(); void optimize();
void filter();
constexpr int UI_WIDTH = 200; constexpr int UI_WIDTH = 200;
@ -135,6 +136,10 @@ Button detect_btn("ui.detect", &detect);
Button match_btn("ui.match", &match); Button match_btn("ui.match", &match);
Button tracks_btn("ui.tracks", &tracks); Button tracks_btn("ui.tracks", &tracks);
Button optimize_btn("ui.optimize", &optimize); Button optimize_btn("ui.optimize", &optimize);
pangolin::Var<double> outlier_threshold("ui.outlier_threshold", 3.0, 0.01, 10);
Button filter_btn("ui.filter", &filter);
Button align_btn("ui.aling_svd", &alignButton); Button align_btn("ui.aling_svd", &alignButton);
pangolin::OpenGlRenderState camera; pangolin::OpenGlRenderState camera;
@ -325,6 +330,8 @@ int main(int argc, char** argv) {
match(); match();
tracks(); tracks();
optimize(); optimize();
filter();
optimize();
if (!result_path.empty()) { if (!result_path.empty()) {
double error = alignButton(); double error = alignButton();
@ -562,7 +569,7 @@ void load_data(const std::string& calib_path, const std::string& cache_path) {
void computeEdgeVis() { void computeEdgeVis() {
edges_vis.clear(); edges_vis.clear();
for (const auto& kv1 : nrf_mapper->obs) { for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) {
for (const auto& kv2 : kv1.second) { for (const auto& kv2 : kv1.second) {
Eigen::Vector3d p1 = nrf_mapper->getFramePoses() Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
.at(kv1.first.frame_id) .at(kv1.first.frame_id)
@ -644,3 +651,8 @@ void tracks() {
// mapper_point_ids); // mapper_point_ids);
computeEdgeVis(); computeEdgeVis();
} }
void filter() {
nrf_mapper->filterOutliers(outlier_threshold, 4);
nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
}

View File

@ -479,7 +479,7 @@ void extractNonlinearFactors(basalt::MargData& m) {
void computeEdgeVis() { void computeEdgeVis() {
edges_vis.clear(); edges_vis.clear();
for (const auto& kv1 : nrf_mapper->obs) { for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) {
for (const auto& kv2 : kv1.second) { for (const auto& kv2 : kv1.second) {
Eigen::Vector3d p1 = nrf_mapper->getFramePoses() Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
.at(kv1.first.frame_id) .at(kv1.first.frame_id)

View File

@ -57,11 +57,14 @@ VioConfig::VioConfig() {
vio_max_kfs = 7; vio_max_kfs = 7;
vio_min_frames_after_kf = 5; vio_min_frames_after_kf = 5;
vio_new_kf_keypoints_thresh = 0.7; vio_new_kf_keypoints_thresh = 0.7;
vio_max_iterations = 5;
vio_debug = false; vio_debug = false;
vio_obs_std_dev = 0.5; vio_obs_std_dev = 0.5;
vio_obs_huber_thresh = 1.0; vio_obs_huber_thresh = 1.0;
vio_min_triangulation_dist = 0.05; 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_std_dev = 0.25;
mapper_obs_huber_thresh = 1.5; 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_new_kf_keypoints_thresh));
ar(CEREAL_NVP(config.vio_debug)); ar(CEREAL_NVP(config.vio_debug));
ar(CEREAL_NVP(config.vio_max_iterations)); 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_std_dev));
ar(CEREAL_NVP(config.vio_obs_huber_thresh)); ar(CEREAL_NVP(config.vio_obs_huber_thresh));

View File

@ -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); 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.dir -= inc_p.head<2>();
kpt.id -= inc_p[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<int, std::vector<std::pair<TimeCamId, double>>>* outliers,
double outlier_threshold) const {
error = 0; error = 0;
for (const auto& kv : obs) { for (const auto& kv : lmdb.getObservations()) {
const TimeCamId& tcid_h = kv.first; const TimeCamId& tcid_h = kv.first;
for (const auto& obs_kv : kv.second) { for (const auto& obs_kv : kv.second) {
@ -151,7 +154,8 @@ void BundleAdjustmentBase::computeError(double& error) const {
[&](const auto& cam) { [&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) { for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[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::Vector2d res;
@ -159,6 +163,11 @@ void BundleAdjustmentBase::computeError(double& error) const {
if (valid) { if (valid) {
double e = res.norm(); double e = res.norm();
if (outliers && e > outlier_threshold) {
(*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e);
}
double huber_weight = double huber_weight =
e < huber_thresh ? 1.0 : huber_thresh / e; e < huber_thresh ? 1.0 : huber_thresh / e;
double obs_weight = double obs_weight =
@ -166,6 +175,10 @@ void BundleAdjustmentBase::computeError(double& error) const {
error += error +=
(2 - huber_weight) * obs_weight * res.transpose() * res; (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) { [&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) { for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[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::Vector2d res;
bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res);
if (valid) { if (valid) {
double e = res.norm(); double e = res.norm();
if (outliers && e > outlier_threshold) {
(*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
}
double huber_weight = double huber_weight =
e < huber_thresh ? 1.0 : huber_thresh / e; e < huber_thresh ? 1.0 : huber_thresh / e;
double obs_weight = double obs_weight =
@ -194,6 +213,10 @@ void BundleAdjustmentBase::computeError(double& error) const {
error += error +=
(2 - huber_weight) * obs_weight * res.transpose() * res; (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<TimeCamId> obs_tcid_vec; std::vector<TimeCamId> obs_tcid_vec;
for (const auto& kv : obs_to_lin) { for (const auto& kv : obs_to_lin) {
obs_tcid_vec.emplace_back(kv.first); 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( tbb::parallel_for(
@ -264,7 +287,8 @@ void BundleAdjustmentBase::linearizeHelper(
[&](const auto& cam) { [&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) { for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[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::Vector2d res;
Eigen::Matrix<double, 2, POSE_SIZE> d_res_d_xi; Eigen::Matrix<double, 2, POSE_SIZE> d_res_d_xi;
@ -318,7 +342,8 @@ void BundleAdjustmentBase::linearizeHelper(
[&](const auto& cam) { [&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) { for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[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::Vector2d res;
Eigen::Matrix<double, 2, 3> d_res_d_p; Eigen::Matrix<double, 2, 3> d_res_d_p;
@ -401,14 +426,10 @@ void BundleAdjustmentBase::get_current_points(
points.clear(); points.clear();
ids.clear(); ids.clear();
for (const auto& kv_kpt : kpts) { for (const auto& tcid_host : lmdb.getHostKfs()) {
Eigen::Vector3d pt_cam =
StereographicParam<double>::unproject(kv_kpt.second.dir).head<3>();
pt_cam /= kv_kpt.second.id;
Sophus::SE3d T_w_i; 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) { if (frame_states.count(id) > 0) {
PoseVelBiasStateWithLin state = frame_states.at(id); PoseVelBiasStateWithLin state = frame_states.at(id);
T_w_i = state.getState().T_w_i; T_w_i = state.getState().T_w_i;
@ -421,14 +442,63 @@ void BundleAdjustmentBase::get_current_points(
std::abort(); 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<double>::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<int, std::vector<std::pair<TimeCamId, double>>> 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<TimeCamId> 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, void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,

View File

@ -70,8 +70,8 @@ KeypointVioEstimator::KeypointVioEstimator(
marg_H(5, 5) = prior_weight; marg_H(5, 5) = prior_weight;
// small prior to avoid jumps in bias // small prior to avoid jumps in bias
marg_H.diagonal().segment<3>(9).array() = 1e1; marg_H.diagonal().segment<3>(9).array() = 1e2;
marg_H.diagonal().segment<3>(12).array() = 1e2; marg_H.diagonal().segment<3>(12).array() = 1e3;
std::cout << "marg_H\n" << marg_H << std::endl; std::cout << "marg_H\n" << marg_H << std::endl;
@ -119,6 +119,8 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
ImuData::Ptr data; ImuData::Ptr data;
imu_data_queue.pop(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) { while (true) {
vision_data_queue.pop(curr_frame); 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) { while (data->t_ns <= prev_frame->t_ns) {
imu_data_queue.pop(data); imu_data_queue.pop(data);
if (!data.get()) break; 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) { while (data->t_ns <= curr_frame->t_ns) {
meas->integrate(*data, accel_cov, gyro_cov); meas->integrate(*data, accel_cov, gyro_cov);
imu_data_queue.pop(data); imu_data_queue.pop(data);
if (!data.get()) break; 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) { 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]) { for (const auto& kv_obs : opt_flow_meas->observations[i]) {
int kpt_id = kv_obs.first; int kpt_id = kv_obs.first;
auto it = kpts.find(kpt_id); if (lmdb.landmarkExists(kpt_id)) {
if (it != kpts.end()) { const TimeCamId& tcid_host = lmdb.getLandmark(kpt_id).kf_id;
const TimeCamId& tcid_host = it->second.kf_id;
KeypointObservation kobs; KeypointObservation kobs;
kobs.kpt_id = kpt_id; kobs.kpt_id = kpt_id;
kobs.pos = kv_obs.second.translation().cast<double>(); kobs.pos = kv_obs.second.translation().cast<double>();
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) { if (num_points_connected.count(tcid_host.frame_id) == 0) {
num_points_connected[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.kf_id = tcidl;
kpt_pos.dir = StereographicParam<double>::project(p0_triangulated); kpt_pos.dir = StereographicParam<double>::project(p0_triangulated);
kpt_pos.id = p0_triangulated[3]; kpt_pos.id = p0_triangulated[3];
kpts[lm_id] = kpt_pos; lmdb.addLandmark(lm_id, kpt_pos);
num_points_added++; num_points_added++;
valid_kp = true; valid_kp = true;
@ -352,7 +357,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
if (valid_kp) { if (valid_kp) {
for (const auto& kv_obs : kp_obs) { 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<TimeCamId, Eigen::vector<KeypointObservation>>> Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
obs_to_lin; 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) { if (kfs_to_marg.count(it->first.frame_id) > 0) {
for (auto it2 = it->second.cbegin(); it2 != it->second.cend(); for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
++it2) { ++it2) {
if (it2->first.frame_id <= last_state_to_marg) if (it2->first.frame_id <= last_state_to_marg)
obs_to_lin[it->first].emplace(*it2); obs_to_lin[it->first].emplace(*it2);
} }
it = obs.erase(it);
} else {
++it;
} }
++it;
} }
double rld_error; double rld_error;
@ -701,15 +705,6 @@ void KeypointVioEstimator::marginalize(
linearizeAbs(rel_H, rel_b, rld, aom, accum); 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, linearizeAbsIMU(aom, accum.getH(), accum.getB(), imu_error, bg_error,
@ -809,16 +804,7 @@ void KeypointVioEstimator::marginalize(
prev_opt_flow_res.erase(id); prev_opt_flow_res.erase(id);
} }
for (auto it = obs.begin(); it != obs.end(); ++it) { lmdb.removeKeyframes(kfs_to_marg, poses_to_marg, states_to_marg_all);
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;
}
}
}
AbsOrderMap marg_order_new; AbsOrderMap marg_order_new;
@ -925,7 +911,7 @@ void KeypointVioEstimator::optimize() {
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs, rld_error); linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt( BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt(
aom); 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; if (inc.array().abs().maxCoeff() < 1e-4) break;
// std::cerr << "LT\n" << LT << std::endl; // std::cerr << "LT\n" << LT << std::endl;
@ -1037,7 +1027,7 @@ void KeypointVioEstimator::optimize() {
void KeypointVioEstimator::computeProjections( void KeypointVioEstimator::computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& data) const { std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {
for (const auto& kv : obs) { for (const auto& kv : lmdb.getObservations()) {
const TimeCamId& tcid_h = kv.first; const TimeCamId& tcid_h = kv.first;
for (const auto& obs_kv : kv.second) { for (const auto& obs_kv : kv.second) {
@ -1061,7 +1051,8 @@ void KeypointVioEstimator::computeProjections(
[&](const auto& cam) { [&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) { for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[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::Vector2d res;
Eigen::Vector4d proj; Eigen::Vector4d proj;
@ -1084,7 +1075,8 @@ void KeypointVioEstimator::computeProjections(
[&](const auto& cam) { [&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) { for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[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::Vector2d res;
Eigen::Vector4d proj; Eigen::Vector4d proj;

View File

@ -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 <basalt/vi_estimator/landmark_database.h>
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<FrameId> &kfs_to_marg,
const std::set<FrameId> &poses_to_marg,
const std::set<FrameId> &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<TimeCamId> LandmarkDatabase::getHostKfs() const {
std::vector<TimeCamId> res;
for (const auto &kv : obs) res.emplace_back(kv.first);
return res;
}
std::vector<KeypointPosition> LandmarkDatabase::getLandmarksForHost(
const TimeCamId &tcid) const {
std::vector<KeypointPosition> 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<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation> > >
&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<TimeCamId> &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

View File

@ -244,7 +244,7 @@ void NfrMapper::optimize(int num_iterations) {
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs, rld_error); linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
// SparseHashAccumulator<double> accum; // SparseHashAccumulator<double> accum;
// accum.reset(aom.total_size); // accum.reset(aom.total_size);
@ -662,14 +662,15 @@ void NfrMapper::setup_opt() {
pos.dir = StereographicParam<double>::project(pos_3d); pos.dir = StereographicParam<double>::project(pos_3d);
pos.id = pos_3d[3]; pos.id = pos_3d[3];
kpts[kv.first] = pos; lmdb.addLandmark(kv.first, pos);
for (const auto& obs_kv : kv.second) { for (const auto& obs_kv : kv.second) {
KeypointObservation ko; KeypointObservation ko;
ko.kpt_id = kv.first; ko.kpt_id = kv.first;
ko.pos = feature_corners.at(obs_kv.first).corners[obs_kv.second]; 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; break;
} }

View File

@ -338,7 +338,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
basalt::ExtendedUnifiedCamera<double> cam = basalt::ExtendedUnifiedCamera<double> cam =
basalt::ExtendedUnifiedCamera<double>::getTestProjections()[0]; basalt::ExtendedUnifiedCamera<double>::getTestProjections()[0];
basalt::KeypointVioEstimator::KeypointPosition kpt_pos; basalt::KeypointPosition kpt_pos;
Eigen::Vector4d point3d; Eigen::Vector4d point3d;
cam.unproject(Eigen::Vector2d::Random() * 50, point3d); cam.unproject(Eigen::Vector2d::Random() * 50, point3d);
@ -358,7 +358,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
p_trans = T_t_h * p_trans; p_trans = T_t_h * p_trans;
basalt::KeypointVioEstimator::KeypointObservation kpt_obs; basalt::KeypointObservation kpt_obs;
cam.project(p_trans, kpt_obs.pos); cam.project(p_trans, kpt_obs.pos);
Eigen::Vector2d res; Eigen::Vector2d res;
@ -391,7 +391,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
test_jacobian( test_jacobian(
"d_res_d_p", d_res_d_p, "d_res_d_p", d_res_d_p,
[&](const Eigen::Vector3d& x) { [&](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.dir += x.head<2>();
kpt_pos_new.id += x[2]; kpt_pos_new.id += x[2];