Merge branch 'landmark_database' into 'master'
Added Landmark database See merge request basalt/basalt!24
This commit is contained in:
commit
909fbd21b3
|
@ -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)
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
|
@ -10,6 +10,7 @@ brew install \
|
||||||
tbb \
|
tbb \
|
||||||
glew \
|
glew \
|
||||||
eigen \
|
eigen \
|
||||||
ccache
|
ccache \
|
||||||
|
lz4
|
||||||
|
|
||||||
brew install llvm
|
brew install llvm
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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));
|
||||||
|
|
|
@ -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,15 +442,64 @@ 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,
|
||||||
Eigen::VectorXd& abs_b,
|
Eigen::VectorXd& abs_b,
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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];
|
||||||
|
|
Loading…
Reference in New Issue