ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'landmark_database' into 'master' 

Added Landmark database

See merge request basalt/basalt!24
This commit is contained in:
Vladyslav Usenko 2019-07-26 16:50:15 +00:00
parent d981339d18 5bb572cdeb
commit 909fbd21b3
17 changed files with 506 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -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)

5
data/euroc_config.json
View File

@ -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,

6
data/euroc_ds_calib.json
View File

@ -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,

6
data/euroc_eucm_calib.json
View File

@ -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,

5
data/tumvi_512_config.json
View File

@ -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,

5
include/basalt/utils/vio_config.h
View File

@ -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;

30
include/basalt/vi_estimator/ba_base.h
View File

@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <basalt/utils/imu_types.h>
#include <basalt/vi_estimator/landmark_database.h>
#include <tbb/blocked_range.h>
@ -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<std::pair<TimeCamId, TimeCamId>> order;
@ -113,7 +97,10 @@ class BundleAdjustmentBase {
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(
Eigen::vector<RelLinData>& 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 <class CamT>
static bool linearizePoint(
const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
@ -397,10 +386,7 @@ class BundleAdjustmentBase {
Eigen::map<int64_t, PoseStateWithLin> frame_poses;
// Point management
Eigen::unordered_map<int, KeypointPosition> kpts;
Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
obs;
LandmarkDatabase lmdb;
double obs_std_dev;
double huber_thresh;

107
include/basalt/vi_estimator/landmark_database.h Normal file
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

3
scripts/install_mac_os_deps.sh
View File

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

14
src/mapper.cpp
View File

@ -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<double> 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);
}

2
src/mapper_sim.cpp
View File

@ -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)

7
src/utils/vio_config.cpp
View File

@ -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));

106
src/vi_estimator/ba_base.cpp
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);
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<int, std::vector<std::pair<TimeCamId, double>>>* 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<TimeCamId> 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<double, 2, POSE_SIZE> 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<double, 2, 3> 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<double>::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,14 +442,63 @@ 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<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,

64
src/vi_estimator/keypoint_vio.cpp
View File

@ -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<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) {
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<double>::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<TimeCamId, Eigen::vector<KeypointObservation>>>
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<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs, rld_error);
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> 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<Eigen::vector<Eigen::Vector4d>>& 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;

206
src/vi_estimator/landmark_database.cpp Normal file
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

7
src/vi_estimator/nfr_mapper.cpp
View File

@ -244,7 +244,7 @@ void NfrMapper::optimize(int num_iterations) {
double rld_error;
Eigen::vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs, rld_error);
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
// SparseHashAccumulator<double> accum;
// accum.reset(aom.total_size);
@ -662,14 +662,15 @@ void NfrMapper::setup_opt() {
pos.dir = StereographicParam<double>::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;
}

6
test/src/test_vio.cpp
View File

@ -338,7 +338,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
basalt::ExtendedUnifiedCamera<double> cam =
basalt::ExtendedUnifiedCamera<double>::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];