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

Merge branch 'stereo' into 'master' 

Added stereo (no IMU) odometry and KITTI evaluation

See merge request basalt/basalt!33
This commit is contained in:
Vladyslav Usenko 2019-08-23 08:01:40 +00:00
parent fc0800514b 18fe854d88
commit 27b9377285
25 changed files with 2211 additions and 229 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
.gitlab-ci.yml
View File

@ -104,6 +104,21 @@ stages:
- cd scripts/eval_full
- ./run_evaluations.sh
.eval_euroc_template: &eval_kitti_definition
stage: eval
parallel: 10
tags: [docker, dataset-eval]
variables:
GIT_STRATEGY: none
artifacts:
paths:
- scripts/eval_full/eval_results_kitti/*
expire_in: 1 week
script:
- dpkg -i deb_bionic/*.deb
- cd scripts/eval_full
- ./run_evaluations_kitti.sh
bionic-release-compile:
<<: *prepare_docker_definition
<<: *compile_test_package_definition
@ -205,8 +220,22 @@ eval_euroc:
- master
allow_failure: false
# evaluate on KITTI sequences
eval_kitti_master:
<<: *eval_kitti_definition
only:
- master
# evaluate on KITTI sequences
eval_kitti:
<<: *eval_kitti_definition
when: manual
except:
- master
allow_failure: false
# aggregate results for all EuRoC sequences
gen_results_euroc:
gen_results:
stage: results
variables:
GIT_STRATEGY: none
@ -216,11 +245,15 @@ gen_results_euroc:
artifacts:
paths:
- euroc_results.txt
- kitti_results.txt
script:
- cd scripts/eval_full
- ./gen_results.py eval_results > euroc_results.txt
- cat euroc_results.txt
- ./gen_results_kitti.py eval_results_kitti > kitti_results.txt
- cat kitti_results.txt
- mv euroc_results.txt ../../
- mv kitti_results.txt ../../
# deploy deb packages
deploy:

6
CMakeLists.txt
View File

@ -211,6 +211,7 @@ add_library(basalt SHARED
src/optical_flow/optical_flow.cpp
src/vi_estimator/keypoint_vio.cpp
src/vi_estimator/keypoint_vio_linearize.cpp
src/vi_estimator/keypoint_vo.cpp
src/vi_estimator/vio_estimator.cpp
src/vi_estimator/ba_base.cpp
src/vi_estimator/nfr_mapper.cpp
@ -250,6 +251,9 @@ target_link_libraries(basalt_vio basalt pangolin)
add_executable(basalt_time_alignment src/time_alignment.cpp)
target_link_libraries(basalt_time_alignment basalt pangolin)
add_executable(basalt_kitti_eval src/kitti_eval.cpp)
target_link_libraries(basalt_kitti_eval)
find_package(realsense2 QUIET)
if(realsense2_FOUND)
add_executable(basalt_rs_t265_record src/rs_t265_record.cpp src/device/rs_t265.cpp)
@ -261,7 +265,7 @@ endif()
install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper_sim_naive basalt_mapper basalt_opt_flow basalt_vio basalt
install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper_sim_naive basalt_mapper basalt_opt_flow basalt_vio basalt_kitti_eval basalt
EXPORT BasaltTargets
RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib

45
data/kitti_config.json Normal file
View File

@ -0,0 +1,45 @@
{
"value0": {
"config.optical_flow_type": "frame_to_frame",
"config.optical_flow_detection_grid_size": 30,
"config.optical_flow_max_recovered_dist2": 0.04,
"config.optical_flow_pattern": 51,
"config.optical_flow_max_iterations": 5,
"config.optical_flow_epipolar_error": 0.001,
"config.optical_flow_levels": 4,
"config.optical_flow_skip_frames": 1,
"config.vio_max_states": 3,
"config.vio_max_kfs": 7,
"config.vio_min_frames_after_kf": 1,
"config.vio_new_kf_keypoints_thresh": 0.7,
"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.vio_enforce_realtime": false,
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5,
"config.mapper_detection_num_points": 800,
"config.mapper_num_frames_to_match": 30,
"config.mapper_frames_to_match_threshold": 0.04,
"config.mapper_min_matches": 20,
"config.mapper_ransac_threshold": 5e-5,
"config.mapper_min_track_length": 5,
"config.mapper_max_hamming_distance": 70,
"config.mapper_second_best_test_ratio": 1.2,
"config.mapper_bow_num_bits": 16,
"config.mapper_min_triangulation_dist": 0.07,
"config.mapper_no_factor_weights": false,
"config.mapper_use_factors": true,
"config.mapper_use_lm": true,
"config.mapper_lm_lambda_min": 1e-32,
"config.mapper_lm_lambda_max": 1e3
}
}

198
include/basalt/io/dataset_io_kitti.h Normal file
View File

@ -0,0 +1,198 @@
/**
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/io/dataset_io.h>
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#include <opencv2/highgui/highgui.hpp>
namespace basalt {
class KittiVioDataset : public VioDataset {
size_t num_cams;
std::string path;
std::vector<int64_t> image_timestamps;
std::unordered_map<int64_t, std::string> image_path;
// vector of images for every timestamp
// assumes vectors size is num_cams for every timestamp with null pointers for
// missing frames
// std::unordered_map<int64_t, std::vector<ImageData>> image_data;
Eigen::vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns;
public:
~KittiVioDataset(){};
size_t get_num_cams() const { return num_cams; }
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; }
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; }
const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps;
}
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data;
}
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
std::vector<ImageData> get_image_data(int64_t t_ns) {
std::vector<ImageData> res(num_cams);
const std::vector<std::string> folder = {"/image_0/", "/image_1/"};
for (size_t i = 0; i < num_cams; i++) {
std::string full_image_path = path + folder[i] + image_path[t_ns];
if (file_exists(full_image_path)) {
cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
if (img.type() == CV_8UC1) {
res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
const uint8_t *data_in = img.ptr();
uint16_t *data_out = res[i].img->ptr;
size_t full_size = img.cols * img.rows;
for (size_t i = 0; i < full_size; i++) {
int val = data_in[i];
val = val << 8;
data_out[i] = val;
}
} else {
std::cerr << "img.fmt.bpp " << img.type() << std::endl;
std::abort();
}
}
}
return res;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
friend class KittiIO;
};
class KittiIO : public DatasetIoInterface {
public:
KittiIO() {}
void read(const std::string &path) {
if (!fs::exists(path))
std::cerr << "No dataset found in " << path << std::endl;
data.reset(new KittiVioDataset);
data->num_cams = 2;
data->path = path;
read_image_timestamps(path + "/times.txt");
if (file_exists(path + "/poses.txt")) {
read_gt_data_pose(path + "/poses.txt");
}
}
void reset() { data.reset(); }
VioDatasetPtr get_data() { return data; }
private:
void read_image_timestamps(const std::string &path) {
std::ifstream f(path);
std::string line;
while (std::getline(f, line)) {
if (line[0] == '#') continue;
std::stringstream ss(line);
double t_s;
ss >> t_s;
int64_t t_ns = t_s * 1e9;
std::stringstream ss1;
ss1 << std::setfill('0') << std::setw(6) << data->image_timestamps.size()
<< ".png";
data->image_timestamps.emplace_back(t_ns);
data->image_path[t_ns] = ss1.str();
}
}
void read_gt_data_pose(const std::string &path) {
data->gt_timestamps.clear();
data->gt_pose_data.clear();
int i = 0;
std::ifstream f(path);
std::string line;
while (std::getline(f, line)) {
if (line[0] == '#') continue;
std::stringstream ss(line);
Eigen::Matrix3d rot;
Eigen::Vector3d pos;
ss >> rot(0, 0) >> rot(0, 1) >> rot(0, 2) >> pos[0] >> rot(1, 0) >>
rot(1, 1) >> rot(1, 2) >> pos[1] >> rot(2, 0) >> rot(2, 1) >>
rot(2, 2) >> pos[2];
data->gt_timestamps.emplace_back(data->image_timestamps[i]);
data->gt_pose_data.emplace_back(Eigen::Quaterniond(rot), pos);
i++;
}
}
std::shared_ptr<KittiVioDataset> data;
};
} // namespace basalt

17
include/basalt/utils/imu_types.h
View File

@ -86,7 +86,11 @@ struct PoseVelBiasStateWithLin {
delta.setZero();
}
void setLinTrue() { linearized = true; }
void setLinTrue() {
linearized = true;
BASALT_ASSERT(delta.isApproxToConstant(0));
state_current = state_linearized;
}
void applyInc(const VecN& inc) {
if (!linearized) {
@ -163,8 +167,9 @@ struct PoseStateWithLin {
delta.setZero();
};
PoseStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i)
: linearized(false), pose_linearized(t_ns, T_w_i) {
PoseStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i,
bool linearized = false)
: linearized(linearized), pose_linearized(t_ns, T_w_i) {
delta.setZero();
T_w_i_current = T_w_i;
}
@ -178,6 +183,12 @@ struct PoseStateWithLin {
PoseState::incPose(delta, T_w_i_current);
}
void setLinTrue() {
linearized = true;
BASALT_ASSERT(delta.isApproxToConstant(0));
T_w_i_current = pose_linearized.T_w_i;
}
inline const Sophus::SE3d& getPose() const {
if (!linearized) {
return pose_linearized.T_w_i;

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

@ -237,6 +237,26 @@ class BundleAdjustmentBase {
Eigen::MatrixXd& marg_H,
Eigen::VectorXd& marg_b);
void computeDelta(const AbsOrderMap& marg_order,
Eigen::VectorXd& delta) const;
void linearizeMargPrior(const AbsOrderMap& marg_order,
const Eigen::MatrixXd& marg_H,
const Eigen::VectorXd& marg_b, const AbsOrderMap& aom,
Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
double& marg_prior_error) const;
void computeMargPriorError(const AbsOrderMap& marg_order,
const Eigen::MatrixXd& marg_H,
const Eigen::VectorXd& marg_b,
double& marg_prior_error) const;
static Eigen::VectorXd checkNullspace(
const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
const AbsOrderMap& marg_order,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses);
/// Triangulates the point and returns homogenous representation. First 3
/// components - unit-length direction vector. Last component inverse
/// distance.

14
include/basalt/vi_estimator/keypoint_vio.h
View File

@ -97,15 +97,6 @@ class KeypointVioEstimator : public VioEstimatorBase,
const Eigen::Vector3d& gyro_bias_weight,
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g);
void linearizeMargPrior(const AbsOrderMap& aom, Eigen::MatrixXd& abs_H,
Eigen::VectorXd& abs_b,
double& marg_prior_error) const;
void computeMargPriorError(double& marg_prior_error) const;
void computeDelta(const AbsOrderMap& marg_order,
Eigen::VectorXd& delta) const;
// int64_t propagate();
// void addNewState(int64_t data_t_ns);
@ -114,11 +105,6 @@ class KeypointVioEstimator : public VioEstimatorBase,
void optimize();
void checkMargNullspace() const;
static Eigen::VectorXd checkNullspace(
const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
const AbsOrderMap& marg_order,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses);
int64_t get_t_ns() const {
return frame_states.at(last_state_t_ns).getState().t_ns;

202
include/basalt/vi_estimator/keypoint_vo.h Normal file
View File

@ -0,0 +1,202 @@
/**
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 <memory>
#include <thread>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <basalt/imu/preintegration.h>
#include <basalt/io/dataset_io.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/test_utils.h>
#include <basalt/camera/generic_camera.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <basalt/utils/sophus_utils.hpp>
#include <basalt/vi_estimator/ba_base.h>
#include <basalt/vi_estimator/vio_estimator.h>
namespace basalt {
class KeypointVoEstimator : public VioEstimatorBase,
public BundleAdjustmentBase {
public:
typedef std::shared_ptr<KeypointVoEstimator> Ptr;
static const int N = 9;
typedef Eigen::Matrix<double, N, 1> VecN;
typedef Eigen::Matrix<double, N, N> MatNN;
typedef Eigen::Matrix<double, N, 3> MatN3;
KeypointVoEstimator(double int_std_dev,
const basalt::Calibration<double>& calib,
const VioConfig& config);
void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg,
const Eigen::Vector3d& ba);
void initialize(const Eigen::Vector3d& bg, const Eigen::Vector3d& ba);
virtual ~KeypointVoEstimator() { processing_thread->join(); }
void addIMUToQueue(const ImuData::Ptr& data);
void addVisionToQueue(const OpticalFlowResult::Ptr& data);
bool measure(const OpticalFlowResult::Ptr& data, bool add_frame);
// int64_t propagate();
// void addNewState(int64_t data_t_ns);
void marginalize(const std::map<int64_t, int>& num_points_connected);
void optimize();
void checkMargNullspace() const;
int64_t get_t_ns() const {
return frame_states.at(last_state_t_ns).getState().t_ns;
}
const Sophus::SE3d& get_T_w_i() const {
return frame_states.at(last_state_t_ns).getState().T_w_i;
}
const Eigen::Vector3d& get_vel_w_i() const {
return frame_states.at(last_state_t_ns).getState().vel_w_i;
}
const PoseVelBiasState& get_state() const {
return frame_states.at(last_state_t_ns).getState();
}
PoseVelBiasState get_state(int64_t t_ns) const {
PoseVelBiasState state;
auto it = frame_states.find(t_ns);
if (it != frame_states.end()) {
return it->second.getState();
}
auto it2 = frame_poses.find(t_ns);
if (it2 != frame_poses.end()) {
state.T_w_i = it2->second.getPose();
}
return state;
}
// const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); }
void computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& res) const;
inline void setMaxStates(size_t val) { max_states = val; }
inline void setMaxKfs(size_t val) { max_kfs = val; }
Eigen::vector<Sophus::SE3d> getFrameStates() const {
Eigen::vector<Sophus::SE3d> res;
for (const auto& kv : frame_states) {
res.push_back(kv.second.getState().T_w_i);
}
return res;
}
Eigen::vector<Sophus::SE3d> getFramePoses() const {
Eigen::vector<Sophus::SE3d> res;
for (const auto& kv : frame_poses) {
res.push_back(kv.second.getPose());
}
return res;
}
Eigen::map<int64_t, Sophus::SE3d> getAllPosesMap() const {
Eigen::map<int64_t, Sophus::SE3d> res;
for (const auto& kv : frame_poses) {
res[kv.first] = kv.second.getPose();
}
for (const auto& kv : frame_states) {
res[kv.first] = kv.second.getState().T_w_i;
}
return res;
}
const Sophus::SE3d& getT_w_i_init() { return T_w_i_init; }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
bool take_kf;
int frames_after_kf;
std::set<int64_t> kf_ids;
int64_t last_state_t_ns;
// Input
Eigen::map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
std::map<int64_t, int> num_points_kf;
// Marginalization
AbsOrderMap marg_order;
Eigen::MatrixXd marg_H;
Eigen::VectorXd marg_b;
Eigen::Vector3d gyro_bias_weight, accel_bias_weight;
size_t max_states;
size_t max_kfs;
Sophus::SE3d T_w_i_init;
bool initialized;
VioConfig config;
double lambda, min_lambda, max_lambda, lambda_vee;
int64_t msckf_kf_id;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

2
include/basalt/vi_estimator/landmark_database.h
View File

@ -73,6 +73,8 @@ class LandmarkDatabase {
// Non-const
void addLandmark(int lm_id, const KeypointPosition& pos);
void removeFrame(const FrameId& frame);
void removeKeyframes(const std::set<FrameId>& kfs_to_marg,
const std::set<FrameId>& poses_to_marg,
const std::set<FrameId>& states_to_marg_all);

3
include/basalt/vi_estimator/vio_estimator.h
View File

@ -101,7 +101,8 @@ class VioEstimatorFactory {
static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config,
const Calibration<double>& cam,
double int_std_dev,
const Eigen::Vector3d& g);
const Eigen::Vector3d& g,
bool use_imu);
};
double alignSVD(const std::vector<int64_t>& filter_t_ns,

130
scripts/convert_kitti_calib.py Executable file
View File

@ -0,0 +1,130 @@
#!/usr/bin/env python3
import sys
import math
import numpy as np
import os
from string import Template
import cv2
dataset_path = sys.argv[1]
print(dataset_path)
kitti_calib_file = dataset_path + '/calib.txt'
calib_template = Template('''{
"value0": {
"T_imu_cam": [
{
"px": 0.0,
"py": 0.0,
"pz": 0.0,
"qx": 0.0,
"qy": 0.0,
"qz": 0.0,
"qw": 1.0
},
{
"px": $px,
"py": 0.0,
"pz": 0.0,
"qx": 0.0,
"qy": 0.0,
"qz": 0.0,
"qw": 1.0
}
],
"intrinsics": [
{
"camera_type": "pinhole",
"intrinsics": {
"fx": $fx0,
"fy": $fy0,
"cx": $cx0,
"cy": $cy0
}
},
{
"camera_type": "pinhole",
"intrinsics": {
"fx": $fx1,
"fy": $fy1,
"cx": $cx1,
"cy": $cy1
}
}
],
"resolution": [
[
$rx,
$ry
],
[
$rx,
$ry
]
],
"vignette": [],
"calib_accel_bias": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"calib_gyro_bias": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"imu_update_rate": 0.0,
"accel_noise_std": [0.0, 0.0, 0.0],
"gyro_noise_std": [0.0, 0.0, 0.0],
"accel_bias_std": [0.0, 0.0, 0.0],
"gyro_bias_std": [0.0, 0.0, 0.0],
"cam_time_offset_ns": 0
}
}
''')
with open(kitti_calib_file, 'r') as stream:
lines = (' '.join([x.strip('\n ') for x in stream.readlines() if x.strip('\n ') ])).split(' ')
if len(lines) != 52:
print('Issues loading calibration')
print(lines)
P0 = np.array([float(x) for x in lines[1:13]]).reshape(3,4)
P1 = np.array([float(x) for x in lines[14:26]]).reshape(3,4)
print('P0\n', P0)
print('P1\n', P1)
tx = -P1[0,3]/P1[0,0]
img = cv2.imread(dataset_path + '/image_0/000000.png')
rx = img.shape[1]
ry = img.shape[0]
values = {'fx0': P0[0,0], 'fy0': P0[1,1], 'cx0': P0[0,2], 'cy0': P0[1,2], 'fx1': P1[0,0], 'fy1': P1[1,1], 'cx1': P1[0,2], 'cy1': P1[1,2], 'px': tx, 'rx': rx, 'ry': ry}
calib = calib_template.substitute(values)
print(calib)
with open(dataset_path + '/basalt_calib.json', 'w') as stream2:
stream2.write(calib)

74
scripts/eval_full/gen_results_kitti.py Executable file
View File

@ -0,0 +1,74 @@
#!/usr/bin/env python
import os
import sys
import json
datasets = ['Seq.', '00', '02', '03','04', '05', '06','07', '08', '09', '10']
lengths = [100, 200, 300, 400, 500, 600, 700, 800]
# Other results.
vo = {
'trans_error': {},
'rot_error': {}
}
for l in lengths:
vo['trans_error'][l] = ['Trans. error [%] ' + str(l) + 'm.']
vo['rot_error'][l] = ['Rot. error [deg/m] ' + str(l) + 'm.']
out_dir = sys.argv[1]
mean_values = {
'mean_trans_error' : 0.0,
'mean_rot_error' : 0.0,
'total_num_meas' : 0.0
}
def load_data(x, prefix, key, mean_values):
fname = out_dir + '/' + prefix + '_' + key + '.txt'
if os.path.isfile(fname):
with open(fname, 'r') as f:
j = json.load(f)
res = j['results']
for v in lengths:
num_meas = res[str(v)]['num_meas']
trans_error = res[str(v)]['trans_error']
rot_error = res[str(v)]['rot_error']
x['trans_error'][int(v)].append(round(trans_error, 5))
x['rot_error'][int(v)].append(round(rot_error, 5))
if num_meas > 0:
mean_values['mean_trans_error'] += trans_error*num_meas
mean_values['mean_rot_error'] += rot_error*num_meas
mean_values['total_num_meas'] += num_meas
else:
for v in lengths:
x['trans_error'][int(v)].append(float('inf'))
x['rot_error'][int(v)].append(float('inf'))
for key in datasets[1:]:
load_data(vo, 'rpe', key, mean_values)
row_format ="{:>24}" + "{:>10}" * (len(datasets)-1)
datasets_short = [x[:5] for x in datasets]
print '\nVisual Odometry (Stereo)'
print row_format.format(*datasets_short)
for l in lengths:
print row_format.format(*(vo['trans_error'][l]))
print
for l in lengths:
print row_format.format(*(vo['rot_error'][l]))
print('Mean translation error [%] ', mean_values['mean_trans_error']/mean_values['total_num_meas'])
print('Mean rotation error [deg/m] ', mean_values['mean_rot_error']/mean_values['total_num_meas'])

24
scripts/eval_full/run_evaluations_kitti.sh Executable file
View File

@ -0,0 +1,24 @@
#!/bin/bash
set -e
set -x
DATASET_PATH=/data/kitti_odom_grey/sequences
DATASETS=(00 02 03 04 05 06 07 08 09 10)
folder_name=eval_results_kitti
mkdir $folder_name
for d in ${DATASETS[$CI_NODE_INDEX-1]}; do
echo $d
basalt_vio --dataset-path $DATASET_PATH/$d --cam-calib $DATASET_PATH/$d/basalt_calib.json \
--dataset-type kitti --show-gui 0 --config-path /usr/etc/basalt/kitti_config.json --result-path $folder_name/vo_$d --save-trajectory kitti --use-imu 0
mv trajectory_kitti.txt $folder_name/kitti_$d.txt
basalt_kitti_eval --traj-path $folder_name/kitti_$d.txt --gt-path $DATASET_PATH/$d/poses.txt --result-path $folder_name/rpe_$d.txt
done

3
src/io/dataset_io.cpp
View File

@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <basalt/io/dataset_io.h>
#include <basalt/io/dataset_io_euroc.h>
#include <basalt/io/dataset_io_kitti.h>
#include <basalt/io/dataset_io_rosbag.h>
#include <basalt/io/dataset_io_uzh.h>
@ -49,6 +50,8 @@ DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo(
return DatasetIoInterfacePtr(new RosbagIO);
} else if (dataset_type == "uzh") {
return DatasetIoInterfacePtr(new UzhIO);
} else if (dataset_type == "kitti") {
return DatasetIoInterfacePtr(new KittiIO);
} else {
std::cerr << "Dataset type " << dataset_type << " is not supported"
<< std::endl;

214
src/kitti_eval.cpp Normal file
View File

@ -0,0 +1,214 @@
/**
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 <CLI/CLI.hpp>
#include <basalt/utils/sophus_utils.hpp>
#include <cereal/archives/json.hpp>
#include <cereal/types/string.hpp>
namespace cereal {
template <class Archive, class T, class C, class A>
inline void save(Archive& ar, std::map<std::string, T, C, A> const& map) {
for (const auto& i : map) ar(cereal::make_nvp(i.first, i.second));
}
template <class Archive, class T, class C, class A>
inline void load(Archive& ar, std::map<std::string, T, C, A>& map) {
map.clear();
auto hint = map.begin();
while (true) {
const auto namePtr = ar.getNodeName();
if (!namePtr) break;
std::string key = namePtr;
T value;
ar(value);
hint = map.emplace_hint(hint, std::move(key), std::move(value));
}
}
} // namespace cereal
Eigen::vector<Sophus::SE3d> load_poses(const std::string& path) {
Eigen::vector<Sophus::SE3d> res;
std::ifstream f(path);
std::string line;
while (std::getline(f, line)) {
if (line[0] == '#') continue;
std::stringstream ss(line);
Eigen::Matrix3d rot;
Eigen::Vector3d pos;
ss >> rot(0, 0) >> rot(0, 1) >> rot(0, 2) >> pos[0] >> rot(1, 0) >>
rot(1, 1) >> rot(1, 2) >> pos[1] >> rot(2, 0) >> rot(2, 1) >>
rot(2, 2) >> pos[2];
res.emplace_back(Eigen::Quaterniond(rot), pos);
}
return res;
}
void eval_kitti(const std::vector<double>& lengths,
const Eigen::vector<Sophus::SE3d>& poses_gt,
const Eigen::vector<Sophus::SE3d>& poses_result,
std::map<std::string, std::map<std::string, double>>& res) {
auto lastFrameFromSegmentLength = [](std::vector<float>& dist,
int first_frame, float len) {
for (int i = first_frame; i < (int)dist.size(); i++)
if (dist[i] > dist[first_frame] + len) return i;
return -1;
};
std::cout << "poses_gt.size() " << poses_gt.size() << std::endl;
std::cout << "poses_result.size() " << poses_result.size() << std::endl;
// pre-compute distances (from ground truth as reference)
std::vector<float> dist_gt;
dist_gt.emplace_back(0);
for (size_t i = 1; i < poses_gt.size(); i++) {
const auto& p1 = poses_gt[i - 1];
const auto& p2 = poses_gt[i];
dist_gt.emplace_back(dist_gt.back() +
(p2.translation() - p1.translation()).norm());
}
const size_t step_size = 10;
for (size_t i = 0; i < lengths.size(); i++) {
// current length
float len = lengths[i];
double t_error_sum = 0;
double r_error_sum = 0;
int num_meas = 0;
for (size_t first_frame = 0; first_frame < poses_gt.size();
first_frame += step_size) {
// for all segment lengths do
// compute last frame
int32_t last_frame =
lastFrameFromSegmentLength(dist_gt, first_frame, len);
// continue, if sequence not long enough
if (last_frame == -1) continue;
// compute rotational and translational errors
Sophus::SE3d pose_delta_gt =
poses_gt[first_frame].inverse() * poses_gt[last_frame];
Sophus::SE3d pose_delta_result =
poses_result[first_frame].inverse() * poses_result[last_frame];
// Sophus::SE3d pose_error = pose_delta_result.inverse() * pose_delta_gt;
double r_err = pose_delta_result.unit_quaternion().angularDistance(
pose_delta_gt.unit_quaternion()) *
180.0 / M_PI;
double t_err =
(pose_delta_result.translation() - pose_delta_gt.translation())
.norm();
t_error_sum += t_err / len;
r_error_sum += r_err / len;
num_meas++;
}
std::string len_str = std::to_string((int)len);
res[len_str]["trans_error"] = 100.0 * t_error_sum / num_meas;
res[len_str]["rot_error"] = r_error_sum / num_meas;
res[len_str]["num_meas"] = num_meas;
}
}
int main(int argc, char** argv) {
std::vector<double> lengths = {100, 200, 300, 400, 500, 600, 700, 800};
std::string result_path;
std::string traj_path;
std::string gt_path;
CLI::App app{"KITTI evaluation"};
app.add_option("--traj-path", traj_path,
"Path to the file with computed trajectory.")
->required();
app.add_option("--gt-path", gt_path,
"Path to the file with ground truth trajectory.")
->required();
app.add_option("--result-path", result_path, "Path to store the result file.")
->required();
app.add_option("--eval-lengths", lengths, "Trajectory length to evaluate.");
try {
app.parse(argc, argv);
} catch (const CLI::ParseError& e) {
return app.exit(e);
}
const Eigen::vector<Sophus::SE3d> poses_gt = load_poses(gt_path);
const Eigen::vector<Sophus::SE3d> poses_result = load_poses(traj_path);
if (poses_gt.empty() || poses_gt.size() != poses_result.size()) {
std::cerr << "Wrong number of poses: poses_gt " << poses_gt.size()
<< " poses_result " << poses_result.size() << std::endl;
std::abort();
}
std::map<std::string, std::map<std::string, double>> res_map;
eval_kitti(lengths, poses_gt, poses_result, res_map);
{
cereal::JSONOutputArchive ar(std::cout);
ar(cereal::make_nvp("results", res_map));
std::cout << std::endl;
}
if (!result_path.empty()) {
std::ofstream os(result_path);
{
cereal::JSONOutputArchive ar(os);
ar(cereal::make_nvp("results", res_map));
}
os.close();
}
}

12
src/opt_flow.cpp
View File

@ -66,6 +66,7 @@ constexpr int UI_WIDTH = 200;
void draw_image_overlay(pangolin::View& v, size_t cam_id);
void load_data(const std::string& calib_path);
bool next_step();
bool prev_step();
pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1500);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
@ -73,6 +74,7 @@ pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
using Button = pangolin::Var<std::function<void(void)>>;
Button next_step_btn("ui.next_step", &next_step);
Button prev_step_btn("ui.prev_step", &prev_step);
pangolin::Var<bool> continue_btn("ui.continue", true, false, true);
// Opt flow variables
@ -342,3 +344,13 @@ bool next_step() {
return false;
}
}
bool prev_step() {
if (show_frame > 0) {
show_frame = show_frame - 1;
show_frame.Meta().gui_changed = true;
return true;
} else {
return false;
}
}

4
src/rs_t265_vio.cpp
View File

@ -168,8 +168,8 @@ int main(int argc, char** argv) {
opt_flow_ptr = basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib);
t265_device->image_data_queue = &opt_flow_ptr->input_queue;
vio = basalt::VioEstimatorFactory::getVioEstimator(vio_config, calib, 0.0001,
basalt::constants::g);
vio = basalt::VioEstimatorFactory::getVioEstimator(
vio_config, calib, 0.0001, basalt::constants::g, true);
vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
t265_device->imu_data_queue = &vio->imu_data_queue;

181
src/vi_estimator/ba_base.cpp
View File

@ -569,4 +569,185 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
abs_H.resize(0, 0);
abs_b.resize(0);
}
void BundleAdjustmentBase::computeDelta(const AbsOrderMap& marg_order,
Eigen::VectorXd& delta) const {
size_t marg_size = marg_order.total_size;
delta.setZero(marg_size);
for (const auto& kv : marg_order.abs_order_map) {
if (kv.second.second == POSE_SIZE) {
BASALT_ASSERT(frame_poses.at(kv.first).isLinearized());
delta.segment<POSE_SIZE>(kv.second.first) =
frame_poses.at(kv.first).getDelta();
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
BASALT_ASSERT(frame_states.at(kv.first).isLinearized());
delta.segment<POSE_VEL_BIAS_SIZE>(kv.second.first) =
frame_states.at(kv.first).getDelta();
} else {
BASALT_ASSERT(false);
}
}
}
void BundleAdjustmentBase::linearizeMargPrior(const AbsOrderMap& marg_order,
const Eigen::MatrixXd& marg_H,
const Eigen::VectorXd& marg_b,
const AbsOrderMap& aom,
Eigen::MatrixXd& abs_H,
Eigen::VectorXd& abs_b,
double& marg_prior_error) const {
// Assumed to be in the top left corner
BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size);
// Check if the order of variables is the same.
for (const auto& kv : marg_order.abs_order_map)
BASALT_ASSERT(aom.abs_order_map.at(kv.first) == kv.second);
size_t marg_size = marg_order.total_size;
abs_H.topLeftCorner(marg_size, marg_size) += marg_H;
Eigen::VectorXd delta;
computeDelta(marg_order, delta);
abs_b.head(marg_size) += marg_b;
abs_b.head(marg_size) += marg_H * delta;
marg_prior_error = 0.5 * delta.transpose() * marg_H * delta;
marg_prior_error += delta.transpose() * marg_b;
}
void BundleAdjustmentBase::computeMargPriorError(
const AbsOrderMap& marg_order, const Eigen::MatrixXd& marg_H,
const Eigen::VectorXd& marg_b, double& marg_prior_error) const {
BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size);
Eigen::VectorXd delta;
computeDelta(marg_order, delta);
marg_prior_error = 0.5 * delta.transpose() * marg_H * delta;
marg_prior_error += delta.transpose() * marg_b;
}
Eigen::VectorXd BundleAdjustmentBase::checkNullspace(
const Eigen::MatrixXd& H, const Eigen::VectorXd& b,
const AbsOrderMap& order,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses) {
BASALT_ASSERT(size_t(H.cols()) == order.total_size);
size_t marg_size = order.total_size;
Eigen::VectorXd inc_x, inc_y, inc_z, inc_roll, inc_pitch, inc_yaw;
inc_x.setZero(marg_size);
inc_y.setZero(marg_size);
inc_z.setZero(marg_size);
inc_roll.setZero(marg_size);
inc_pitch.setZero(marg_size);
inc_yaw.setZero(marg_size);
int num_trans = 0;
Eigen::Vector3d mean_trans;
mean_trans.setZero();
// Compute mean translation
for (const auto& kv : order.abs_order_map) {
Eigen::Vector3d trans;
if (kv.second.second == POSE_SIZE) {
mean_trans += frame_poses.at(kv.first).getPoseLin().translation();
num_trans++;
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
mean_trans += frame_states.at(kv.first).getStateLin().T_w_i.translation();
num_trans++;
} else {
std::cerr << "Unknown size of the state: " << kv.second.second
<< std::endl;
std::abort();
}
}
mean_trans /= num_trans;
double eps = 0.01;
// Compute nullspace increments
for (const auto& kv : order.abs_order_map) {
inc_x(kv.second.first + 0) = eps;
inc_y(kv.second.first + 1) = eps;
inc_z(kv.second.first + 2) = eps;
inc_roll(kv.second.first + 3) = eps;
inc_pitch(kv.second.first + 4) = eps;
inc_yaw(kv.second.first + 5) = eps;
Eigen::Vector3d trans;
if (kv.second.second == POSE_SIZE) {
trans = frame_poses.at(kv.first).getPoseLin().translation();
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
trans = frame_states.at(kv.first).getStateLin().T_w_i.translation();
} else {
BASALT_ASSERT(false);
}
trans -= mean_trans;
Eigen::Matrix3d J = -Sophus::SO3d::hat(trans);
J *= eps;
inc_roll.segment<3>(kv.second.first) = J.col(0);
inc_pitch.segment<3>(kv.second.first) = J.col(1);
inc_yaw.segment<3>(kv.second.first) = J.col(2);
if (kv.second.second == POSE_VEL_BIAS_SIZE) {
Eigen::Vector3d vel = frame_states.at(kv.first).getStateLin().vel_w_i;
Eigen::Matrix3d J_vel = -Sophus::SO3d::hat(vel);
J_vel *= eps;
inc_roll.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(0);
inc_pitch.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(1);
inc_yaw.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(2);
}
}
inc_x.normalize();
inc_y.normalize();
inc_z.normalize();
inc_roll.normalize();
inc_pitch.normalize();
inc_yaw.normalize();
// std::cout << "inc_x " << inc_x.transpose() << std::endl;
// std::cout << "inc_y " << inc_y.transpose() << std::endl;
// std::cout << "inc_z " << inc_z.transpose() << std::endl;
// std::cout << "inc_yaw " << inc_yaw.transpose() << std::endl;
Eigen::VectorXd inc_random;
inc_random.setRandom(marg_size);
inc_random.normalize();
Eigen::VectorXd xHx(7), xb(7);
xHx[0] = 0.5 * inc_x.transpose() * H * inc_x;
xHx[1] = 0.5 * inc_y.transpose() * H * inc_y;
xHx[2] = 0.5 * inc_z.transpose() * H * inc_z;
xHx[3] = 0.5 * inc_roll.transpose() * H * inc_roll;
xHx[4] = 0.5 * inc_pitch.transpose() * H * inc_pitch;
xHx[5] = 0.5 * inc_yaw.transpose() * H * inc_yaw;
xHx[6] = 0.5 * inc_random.transpose() * H * inc_random;
xb[0] = inc_x.transpose() * b;
xb[1] = inc_y.transpose() * b;
xb[2] = inc_z.transpose() * b;
xb[3] = inc_roll.transpose() * b;
xb[4] = inc_pitch.transpose() * b;
xb[5] = inc_yaw.transpose() * b;
xb[6] = inc_random.transpose() * b;
std::cout << "nullspace x_trans: " << xHx[0] << " + " << xb[0] << std::endl;
std::cout << "nullspace y_trans: " << xHx[1] << " + " << xb[1] << std::endl;
std::cout << "nullspace z_trans: " << xHx[2] << " + " << xb[2] << std::endl;
std::cout << "nullspace roll : " << xHx[3] << " + " << xb[3] << std::endl;
std::cout << "nullspace pitch : " << xHx[4] << " + " << xb[4] << std::endl;
std::cout << "nullspace yaw : " << xHx[5] << " + " << xb[5] << std::endl;
std::cout << "nullspace random : " << xHx[6] << " + " << xb[6] << std::endl;
return xHx + xb;
}
} // namespace basalt

157
src/vi_estimator/keypoint_vio.cpp
View File

@ -82,8 +82,8 @@ KeypointVioEstimator::KeypointVioEstimator(
gyro_bias_weight = calib.gyro_bias_std.array().square().inverse();
accel_bias_weight = calib.accel_bias_std.array().square().inverse();
max_states = std::numeric_limits<int>::max();
max_kfs = std::numeric_limits<int>::max();
max_states = config.vio_max_states;
max_kfs = config.vio_max_kfs;
opt_started = false;
@ -427,127 +427,6 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
return true;
}
Eigen::VectorXd KeypointVioEstimator::checkNullspace(
const Eigen::MatrixXd& H, const Eigen::VectorXd& b,
const AbsOrderMap& order,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses) {
BASALT_ASSERT(size_t(H.cols()) == order.total_size);
size_t marg_size = order.total_size;
Eigen::VectorXd inc_x, inc_y, inc_z, inc_roll, inc_pitch, inc_yaw;
inc_x.setZero(marg_size);
inc_y.setZero(marg_size);
inc_z.setZero(marg_size);
inc_roll.setZero(marg_size);
inc_pitch.setZero(marg_size);
inc_yaw.setZero(marg_size);
int num_trans = 0;
Eigen::Vector3d mean_trans;
mean_trans.setZero();
// Compute mean translation
for (const auto& kv : order.abs_order_map) {
Eigen::Vector3d trans;
if (kv.second.second == POSE_SIZE) {
mean_trans += frame_poses.at(kv.first).getPoseLin().translation();
num_trans++;
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
mean_trans += frame_states.at(kv.first).getStateLin().T_w_i.translation();
num_trans++;
} else {
std::cerr << "Unknown size of the state: " << kv.second.second
<< std::endl;
std::abort();
}
}
mean_trans /= num_trans;
double eps = 0.01;
// Compute nullspace increments
for (const auto& kv : order.abs_order_map) {
inc_x(kv.second.first + 0) = eps;
inc_y(kv.second.first + 1) = eps;
inc_z(kv.second.first + 2) = eps;
inc_roll(kv.second.first + 3) = eps;
inc_pitch(kv.second.first + 4) = eps;
inc_yaw(kv.second.first + 5) = eps;
Eigen::Vector3d trans;
if (kv.second.second == POSE_SIZE) {
trans = frame_poses.at(kv.first).getPoseLin().translation();
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
trans = frame_states.at(kv.first).getStateLin().T_w_i.translation();
} else {
BASALT_ASSERT(false);
}
trans -= mean_trans;
Eigen::Matrix3d J = -Sophus::SO3d::hat(trans);
J *= eps;
inc_roll.segment<3>(kv.second.first) = J.col(0);
inc_pitch.segment<3>(kv.second.first) = J.col(1);
inc_yaw.segment<3>(kv.second.first) = J.col(2);
if (kv.second.second == POSE_VEL_BIAS_SIZE) {
Eigen::Vector3d vel = frame_states.at(kv.first).getStateLin().vel_w_i;
Eigen::Matrix3d J_vel = -Sophus::SO3d::hat(vel);
J_vel *= eps;
inc_roll.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(0);
inc_pitch.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(1);
inc_yaw.segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(2);
}
}
inc_x.normalize();
inc_y.normalize();
inc_z.normalize();
inc_roll.normalize();
inc_pitch.normalize();
inc_yaw.normalize();
// std::cout << "inc_x " << inc_x.transpose() << std::endl;
// std::cout << "inc_y " << inc_y.transpose() << std::endl;
// std::cout << "inc_z " << inc_z.transpose() << std::endl;
// std::cout << "inc_yaw " << inc_yaw.transpose() << std::endl;
Eigen::VectorXd inc_random;
inc_random.setRandom(marg_size);
inc_random.normalize();
Eigen::VectorXd xHx(7), xb(7);
xHx[0] = 0.5 * inc_x.transpose() * H * inc_x;
xHx[1] = 0.5 * inc_y.transpose() * H * inc_y;
xHx[2] = 0.5 * inc_z.transpose() * H * inc_z;
xHx[3] = 0.5 * inc_roll.transpose() * H * inc_roll;
xHx[4] = 0.5 * inc_pitch.transpose() * H * inc_pitch;
xHx[5] = 0.5 * inc_yaw.transpose() * H * inc_yaw;
xHx[6] = 0.5 * inc_random.transpose() * H * inc_random;
xb[0] = inc_x.transpose() * b;
xb[1] = inc_y.transpose() * b;
xb[2] = inc_z.transpose() * b;
xb[3] = inc_roll.transpose() * b;
xb[4] = inc_pitch.transpose() * b;
xb[5] = inc_yaw.transpose() * b;
xb[6] = inc_random.transpose() * b;
std::cout << "nullspace x_trans: " << xHx[0] << " + " << xb[0] << std::endl;
std::cout << "nullspace y_trans: " << xHx[1] << " + " << xb[1] << std::endl;
std::cout << "nullspace z_trans: " << xHx[2] << " + " << xb[2] << std::endl;
std::cout << "nullspace roll : " << xHx[3] << " + " << xb[3] << std::endl;
std::cout << "nullspace pitch : " << xHx[4] << " + " << xb[4] << std::endl;
std::cout << "nullspace yaw : " << xHx[5] << " + " << xb[5] << std::endl;
std::cout << "nullspace random : " << xHx[6] << " + " << xb[6] << std::endl;
return xHx + xb;
}
void KeypointVioEstimator::checkMargNullspace() const {
checkNullspace(marg_H, marg_b, marg_order, frame_states, frame_poses);
}
@ -731,7 +610,8 @@ void KeypointVioEstimator::marginalize(
linearizeAbsIMU(aom, accum.getH(), accum.getB(), imu_error, bg_error,
ba_error, frame_states, imu_meas, gyro_bias_weight,
accel_bias_weight, g);
linearizeMargPrior(aom, accum.getH(), accum.getB(), marg_prior_error);
linearizeMargPrior(marg_order, marg_H, marg_b, aom, accum.getH(),
accum.getB(), marg_prior_error);
// Save marginalization prior
if (out_marg_queue && !kfs_to_marg.empty()) {
@ -867,25 +747,6 @@ void KeypointVioEstimator::marginalize(
}
}
void KeypointVioEstimator::computeDelta(const AbsOrderMap& marg_order,
Eigen::VectorXd& delta) const {
size_t marg_size = marg_order.total_size;
delta.setZero(marg_size);
for (const auto& kv : marg_order.abs_order_map) {
if (kv.second.second == POSE_SIZE) {
BASALT_ASSERT(frame_poses.at(kv.first).isLinearized());
delta.segment<POSE_SIZE>(kv.second.first) =
frame_poses.at(kv.first).getDelta();
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
BASALT_ASSERT(frame_states.at(kv.first).isLinearized());
delta.segment<POSE_VEL_BIAS_SIZE>(kv.second.first) =
frame_states.at(kv.first).getDelta();
} else {
BASALT_ASSERT(false);
}
}
}
void KeypointVioEstimator::optimize() {
if (config.vio_debug) {
std::cout << "=================================" << std::endl;
@ -947,8 +808,8 @@ void KeypointVioEstimator::optimize() {
linearizeAbsIMU(aom, lopt.accum.getH(), lopt.accum.getB(), imu_error,
bg_error, ba_error, frame_states, imu_meas,
gyro_bias_weight, accel_bias_weight, g);
linearizeMargPrior(aom, lopt.accum.getH(), lopt.accum.getB(),
marg_prior_error);
linearizeMargPrior(marg_order, marg_H, marg_b, aom, lopt.accum.getH(),
lopt.accum.getB(), marg_prior_error);
double error_total =
rld_error + imu_error + marg_prior_error + ba_error + bg_error;
@ -1007,7 +868,8 @@ void KeypointVioEstimator::optimize() {
computeImuError(aom, after_update_imu_error, after_bg_error,
after_ba_error, frame_states, imu_meas,
gyro_bias_weight, accel_bias_weight, g);
computeMargPriorError(after_update_marg_prior_error);
computeMargPriorError(marg_order, marg_H, marg_b,
after_update_marg_prior_error);
double after_error_total =
after_update_vision_error + after_update_imu_error +
@ -1082,7 +944,8 @@ void KeypointVioEstimator::optimize() {
computeImuError(aom, after_update_imu_error, after_bg_error,
after_ba_error, frame_states, imu_meas,
gyro_bias_weight, accel_bias_weight, g);
computeMargPriorError(after_update_marg_prior_error);
computeMargPriorError(marg_order, marg_H, marg_b,
after_update_marg_prior_error);
double after_error_total =
after_update_vision_error + after_update_imu_error +

36
src/vi_estimator/keypoint_vio_linearize.cpp
View File

@ -37,42 +37,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace basalt {
void KeypointVioEstimator::linearizeMargPrior(const AbsOrderMap& aom,
Eigen::MatrixXd& abs_H,
Eigen::VectorXd& abs_b,
double& marg_prior_error) const {
// Assumed to be in the top left corner
BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size);
// Check if the order of variables is the same.
for (const auto& kv : marg_order.abs_order_map)
BASALT_ASSERT(aom.abs_order_map.at(kv.first) == kv.second);
size_t marg_size = marg_order.total_size;
abs_H.topLeftCorner(marg_size, marg_size) += marg_H;
Eigen::VectorXd delta;
computeDelta(marg_order, delta);
abs_b.head(marg_size) += marg_b;
abs_b.head(marg_size) += marg_H * delta;
marg_prior_error = 0.5 * delta.transpose() * marg_H * delta;
marg_prior_error += delta.transpose() * marg_b;
}
void KeypointVioEstimator::computeMargPriorError(
double& marg_prior_error) const {
BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size);
Eigen::VectorXd delta;
computeDelta(marg_order, delta);
marg_prior_error = 0.5 * delta.transpose() * marg_H * delta;
marg_prior_error += delta.transpose() * marg_b;
}
void KeypointVioEstimator::linearizeAbsIMU(
const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
double& imu_error, double& bg_error, double& ba_error,

907
src/vi_estimator/keypoint_vo.cpp Normal file
View File

@ -0,0 +1,907 @@
/**
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/utils/assert.h>
#include <basalt/vi_estimator/keypoint_vo.h>
#include <basalt/optimization/accumulator.h>
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
#include <chrono>
namespace basalt {
KeypointVoEstimator::KeypointVoEstimator(
double int_std_dev, const basalt::Calibration<double>& calib,
const VioConfig& config)
: take_kf(true),
frames_after_kf(0),
initialized(false),
config(config),
lambda(config.vio_lm_lambda_min),
min_lambda(config.vio_lm_lambda_min),
max_lambda(config.vio_lm_lambda_max),
lambda_vee(2) {
this->obs_std_dev = config.vio_obs_std_dev;
this->huber_thresh = config.vio_obs_huber_thresh;
this->calib = calib;
// Setup marginalization
marg_H.setZero(POSE_SIZE, POSE_SIZE);
marg_b.setZero(POSE_SIZE);
double prior_weight = 1.0 / (int_std_dev * int_std_dev);
// prior on pose
marg_H.diagonal().setConstant(prior_weight);
std::cout << "marg_H\n" << marg_H << std::endl;
gyro_bias_weight = calib.gyro_bias_std.array().square().inverse();
accel_bias_weight = calib.accel_bias_std.array().square().inverse();
max_states = config.vio_max_states;
max_kfs = config.vio_max_kfs;
vision_data_queue.set_capacity(10);
imu_data_queue.set_capacity(300);
}
void KeypointVoEstimator::initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
const Eigen::Vector3d& vel_w_i,
const Eigen::Vector3d& bg,
const Eigen::Vector3d& ba) {
UNUSED(vel_w_i);
UNUSED(bg);
UNUSED(ba);
initialized = true;
T_w_i_init = T_w_i;
last_state_t_ns = t_ns;
frame_poses[t_ns] = PoseStateWithLin(t_ns, T_w_i, true);
marg_order.abs_order_map[t_ns] = std::make_pair(0, POSE_SIZE);
marg_order.total_size = POSE_SIZE;
marg_order.items = 1;
}
void KeypointVoEstimator::initialize(const Eigen::Vector3d& bg,
const Eigen::Vector3d& ba) {
auto proc_func = [&, bg, ba] {
OpticalFlowResult::Ptr prev_frame, curr_frame;
bool add_pose = false;
while (true) {
vision_data_queue.pop(curr_frame);
if (config.vio_enforce_realtime) {
// drop current frame if another frame is already in the queue.
while (!vision_data_queue.empty()) vision_data_queue.pop(curr_frame);
}
if (!curr_frame.get()) {
break;
}
// Correct camera time offset
// curr_frame->t_ns += calib.cam_time_offset_ns;
while (!imu_data_queue.empty()) {
ImuData::Ptr d;
imu_data_queue.pop(d);
}
if (!initialized) {
Eigen::Vector3d vel_w_i_init;
last_state_t_ns = curr_frame->t_ns;
frame_poses[last_state_t_ns] =
PoseStateWithLin(last_state_t_ns, T_w_i_init, true);
marg_order.abs_order_map[last_state_t_ns] =
std::make_pair(0, POSE_SIZE);
marg_order.total_size = POSE_SIZE;
marg_order.items = 1;
std::cout << "Setting up filter: t_ns " << last_state_t_ns << std::endl;
std::cout << "T_w_i\n" << T_w_i_init.matrix() << std::endl;
initialized = true;
}
if (prev_frame) {
add_pose = true;
}
measure(curr_frame, add_pose);
prev_frame = curr_frame;
}
if (out_vis_queue) out_vis_queue->push(nullptr);
if (out_marg_queue) out_marg_queue->push(nullptr);
if (out_state_queue) out_state_queue->push(nullptr);
finished = true;
std::cout << "Finished VIOFilter " << std::endl;
};
processing_thread.reset(new std::thread(proc_func));
}
void KeypointVoEstimator::addVisionToQueue(const OpticalFlowResult::Ptr& data) {
vision_data_queue.push(data);
}
bool KeypointVoEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
const bool add_pose) {
if (add_pose) {
const PoseStateWithLin& curr_state = frame_poses.at(last_state_t_ns);
last_state_t_ns = opt_flow_meas->t_ns;
PoseStateWithLin next_state(opt_flow_meas->t_ns, curr_state.getPose());
frame_poses[last_state_t_ns] = next_state;
}
// save results
prev_opt_flow_res[opt_flow_meas->t_ns] = opt_flow_meas;
// Make new residual for existing keypoints
int connected0 = 0;
std::map<int64_t, int> num_points_connected;
std::unordered_set<int> unconnected_obs0;
for (size_t i = 0; i < opt_flow_meas->observations.size(); i++) {
TimeCamId tcid_target(opt_flow_meas->t_ns, i);
for (const auto& kv_obs : opt_flow_meas->observations[i]) {
int kpt_id = kv_obs.first;
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>();
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;
}
num_points_connected[tcid_host.frame_id]++;
if (i == 0) connected0++;
} else {
if (i == 0) {
unconnected_obs0.emplace(kpt_id);
}
}
}
}
if (double(connected0) / (connected0 + unconnected_obs0.size()) <
config.vio_new_kf_keypoints_thresh &&
frames_after_kf > config.vio_min_frames_after_kf)
take_kf = true;
if (config.vio_debug) {
std::cout << "connected0 " << connected0 << " unconnected0 "
<< unconnected_obs0.size() << std::endl;
}
if (take_kf) {
// Triangulate new points from stereo and make keyframe for camera 0
take_kf = false;
frames_after_kf = 0;
kf_ids.emplace(last_state_t_ns);
TimeCamId tcidl(opt_flow_meas->t_ns, 0);
int num_points_added = 0;
for (int lm_id : unconnected_obs0) {
// Find all observations
std::map<TimeCamId, KeypointObservation> kp_obs;
for (const auto& kv : prev_opt_flow_res) {
for (size_t k = 0; k < kv.second->observations.size(); k++) {
auto it = kv.second->observations[k].find(lm_id);
if (it != kv.second->observations[k].end()) {
TimeCamId tcido(kv.first, k);
KeypointObservation kobs;
kobs.kpt_id = lm_id;
kobs.pos = it->second.translation().cast<double>();
// obs[tcidl][tcido].push_back(kobs);
kp_obs[tcido] = kobs;
}
}
}
// triangulate
bool valid_kp = false;
const double min_triang_distance2 =
config.vio_min_triangulation_dist * config.vio_min_triangulation_dist;
for (const auto& kv_obs : kp_obs) {
if (valid_kp) break;
TimeCamId tcido = kv_obs.first;
const Eigen::Vector2d p0 = opt_flow_meas->observations.at(0)
.at(lm_id)
.translation()
.cast<double>();
const Eigen::Vector2d p1 = prev_opt_flow_res[tcido.frame_id]
->observations[tcido.cam_id]
.at(lm_id)
.translation()
.cast<double>();
Eigen::Vector4d p0_3d, p1_3d;
bool valid1 = calib.intrinsics[0].unproject(p0, p0_3d);
bool valid2 = calib.intrinsics[tcido.cam_id].unproject(p1, p1_3d);
if (!valid1 || !valid2) continue;
Sophus::SE3d T_i0_i1 =
getPoseStateWithLin(tcidl.frame_id).getPose().inverse() *
getPoseStateWithLin(tcido.frame_id).getPose();
Sophus::SE3d T_0_1 =
calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.cam_id];
if (T_0_1.translation().squaredNorm() < min_triang_distance2) continue;
Eigen::Vector4d p0_triangulated =
triangulate(p0_3d.head<3>(), p1_3d.head<3>(), T_0_1);
if (p0_triangulated.array().isFinite().all() &&
p0_triangulated[3] > 0 && p0_triangulated[3] < 3.0) {
KeypointPosition kpt_pos;
kpt_pos.kf_id = tcidl;
kpt_pos.dir = StereographicParam<double>::project(p0_triangulated);
kpt_pos.id = p0_triangulated[3];
lmdb.addLandmark(lm_id, kpt_pos);
num_points_added++;
valid_kp = true;
}
}
if (valid_kp) {
for (const auto& kv_obs : kp_obs) {
lmdb.addObservation(kv_obs.first, kv_obs.second);
}
}
}
num_points_kf[opt_flow_meas->t_ns] = num_points_added;
} else {
frames_after_kf++;
}
optimize();
marginalize(num_points_connected);
if (out_state_queue) {
const PoseStateWithLin& p = frame_poses.at(last_state_t_ns);
PoseVelBiasState::Ptr data(
new PoseVelBiasState(p.getT_ns(), p.getPose(), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()));
out_state_queue->push(data);
}
if (out_vis_queue) {
VioVisualizationData::Ptr data(new VioVisualizationData);
data->t_ns = last_state_t_ns;
BASALT_ASSERT(frame_states.empty());
for (const auto& kv : frame_poses) {
data->frames.emplace_back(kv.second.getPose());
}
get_current_points(data->points, data->point_ids);
data->projections.resize(opt_flow_meas->observations.size());
computeProjections(data->projections);
data->opt_flow_res = prev_opt_flow_res[last_state_t_ns];
out_vis_queue->push(data);
}
last_processed_t_ns = last_state_t_ns;
return true;
}
void KeypointVoEstimator::checkMargNullspace() const {
checkNullspace(marg_H, marg_b, marg_order, frame_states, frame_poses);
}
void KeypointVoEstimator::marginalize(
const std::map<int64_t, int>& num_points_connected) {
BASALT_ASSERT(frame_states.empty());
if (true) {
// Marginalize
AbsOrderMap aom;
// remove all frame_poses that are not kfs and not the current frame
std::set<int64_t> non_kf_poses;
for (const auto& kv : frame_poses) {
if (kf_ids.count(kv.first) == 0 && kv.first != last_state_t_ns) {
non_kf_poses.emplace(kv.first);
} else {
aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE);
// Check that we have the same order as marginalization
if (marg_order.abs_order_map.count(kv.first) > 0)
BASALT_ASSERT(marg_order.abs_order_map.at(kv.first) ==
aom.abs_order_map.at(kv.first));
aom.total_size += POSE_SIZE;
aom.items++;
}
}
for (int64_t id : non_kf_poses) {
frame_poses.erase(id);
lmdb.removeFrame(id);
prev_opt_flow_res.erase(id);
}
auto kf_ids_all = kf_ids;
std::set<int64_t> kfs_to_marg;
while (kf_ids.size() > max_kfs) {
int64_t id_to_marg = -1;
{
std::vector<int64_t> ids;
for (int64_t id : kf_ids) {
ids.push_back(id);
}
for (size_t i = 0; i < ids.size() - 2; i++) {
if (num_points_connected.count(ids[i]) == 0 ||
(num_points_connected.at(ids[i]) / num_points_kf.at(ids[i]) <
0.05)) {
id_to_marg = ids[i];
break;
}
}
}
if (id_to_marg < 0) {
std::vector<int64_t> ids;
for (int64_t id : kf_ids) {
ids.push_back(id);
}
int64_t last_kf = *kf_ids.crbegin();
double min_score = std::numeric_limits<double>::max();
int64_t min_score_id = -1;
for (size_t i = 0; i < ids.size() - 2; i++) {
double denom = 0;
for (size_t j = 0; j < ids.size() - 2; j++) {
denom += 1 / ((frame_poses.at(ids[i]).getPose().translation() -
frame_poses.at(ids[j]).getPose().translation())
.norm() +
1e-5);
}
double score =
std::sqrt((frame_poses.at(ids[i]).getPose().translation() -
frame_poses.at(last_kf).getPose().translation())
.norm()) *
denom;
if (score < min_score) {
min_score_id = ids[i];
min_score = score;
}
}
id_to_marg = min_score_id;
}
kfs_to_marg.emplace(id_to_marg);
non_kf_poses.emplace(id_to_marg);
kf_ids.erase(id_to_marg);
}
// std::cout << "marg order" << std::endl;
// aom.print_order();
// std::cout << "marg prior order" << std::endl;
// marg_order.print_order();
if (config.vio_debug) {
std::cout << "non_kf_poses.size() " << non_kf_poses.size() << std::endl;
for (const auto& v : non_kf_poses) std::cout << v << ' ';
std::cout << std::endl;
std::cout << "kfs_to_marg.size() " << kfs_to_marg.size() << std::endl;
for (const auto& v : kfs_to_marg) std::cout << v << ' ';
std::cout << std::endl;
std::cout << "last_state_t_ns " << last_state_t_ns << std::endl;
std::cout << "frame_poses.size() " << frame_poses.size() << std::endl;
for (const auto& v : frame_poses) std::cout << v.first << ' ';
std::cout << std::endl;
}
if (!kfs_to_marg.empty()) {
// Marginalize only if last state is a keyframe
BASALT_ASSERT(kf_ids_all.count(last_state_t_ns) > 0);
size_t asize = aom.total_size;
double marg_prior_error;
DenseAccumulator accum;
accum.reset(asize);
{
// Linearize points
Eigen::map<TimeCamId,
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
obs_to_lin;
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) {
obs_to_lin[it->first].emplace(*it2);
}
}
++it;
}
double rld_error;
Eigen::vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs_to_lin, rld_error);
for (auto& rld : rld_vec) {
rld.invert_keypoint_hessians();
Eigen::MatrixXd rel_H;
Eigen::VectorXd rel_b;
linearizeRel(rld, rel_H, rel_b);
linearizeAbs(rel_H, rel_b, rld, aom, accum);
}
}
linearizeMargPrior(marg_order, marg_H, marg_b, aom, accum.getH(),
accum.getB(), marg_prior_error);
// Save marginalization prior
if (out_marg_queue && !kfs_to_marg.empty()) {
// int64_t kf_id = *kfs_to_marg.begin();
{
MargData::Ptr m(new MargData);
m->aom = aom;
m->abs_H = accum.getH();
m->abs_b = accum.getB();
m->frame_poses = frame_poses;
m->frame_states = frame_states;
m->kfs_all = kf_ids_all;
m->kfs_to_marg = kfs_to_marg;
for (int64_t t : m->kfs_all) {
m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t));
}
out_marg_queue->push(m);
}
}
std::set<int> idx_to_keep, idx_to_marg;
for (const auto& kv : aom.abs_order_map) {
if (kv.second.second == POSE_SIZE) {
int start_idx = kv.second.first;
if (kfs_to_marg.count(kv.first) == 0) {
for (size_t i = 0; i < POSE_SIZE; i++)
idx_to_keep.emplace(start_idx + i);
} else {
for (size_t i = 0; i < POSE_SIZE; i++)
idx_to_marg.emplace(start_idx + i);
}
} else {
BASALT_ASSERT(false);
}
}
if (config.vio_debug) {
std::cout << "keeping " << idx_to_keep.size() << " marg "
<< idx_to_marg.size() << " total " << asize << std::endl;
std::cout << " frame_poses " << frame_poses.size() << " frame_states "
<< frame_states.size() << std::endl;
}
Eigen::MatrixXd marg_H_new;
Eigen::VectorXd marg_b_new;
marginalizeHelper(accum.getH(), accum.getB(), idx_to_keep, idx_to_marg,
marg_H_new, marg_b_new);
for (auto& kv : frame_poses) {
if (!kv.second.isLinearized()) kv.second.setLinTrue();
}
for (const int64_t id : kfs_to_marg) {
frame_poses.erase(id);
prev_opt_flow_res.erase(id);
}
lmdb.removeKeyframes(kfs_to_marg, kfs_to_marg, kfs_to_marg);
AbsOrderMap marg_order_new;
for (const auto& kv : frame_poses) {
marg_order_new.abs_order_map[kv.first] =
std::make_pair(marg_order_new.total_size, POSE_SIZE);
marg_order_new.total_size += POSE_SIZE;
marg_order_new.items++;
}
marg_H = marg_H_new;
marg_b = marg_b_new;
marg_order = marg_order_new;
BASALT_ASSERT(size_t(marg_H.cols()) == marg_order.total_size);
Eigen::VectorXd delta;
computeDelta(marg_order, delta);
marg_b -= marg_H * delta;
if (config.vio_debug) {
std::cout << "marginalizaon done!!" << std::endl;
std::cout << "======== Marg nullspace ==========" << std::endl;
checkMargNullspace();
std::cout << "=================================" << std::endl;
}
}
// std::cout << "new marg prior order" << std::endl;
// marg_order.print_order();
}
}
void KeypointVoEstimator::optimize() {
if (config.vio_debug) {
std::cout << "=================================" << std::endl;
}
if (true) {
// Optimize
AbsOrderMap aom;
for (const auto& kv : frame_poses) {
aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE);
// Check that we have the same order as marginalization
if (marg_order.abs_order_map.count(kv.first) > 0)
BASALT_ASSERT(marg_order.abs_order_map.at(kv.first) ==
aom.abs_order_map.at(kv.first));
aom.total_size += POSE_SIZE;
aom.items++;
}
BASALT_ASSERT(frame_states.empty());
// std::cout << "opt order" << std::endl;
// aom.print_order();
// std::cout << "marg prior order" << std::endl;
// marg_order.print_order();
for (int iter = 0; iter < config.vio_max_iterations; iter++) {
auto t1 = std::chrono::high_resolution_clock::now();
double rld_error;
Eigen::vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt(
aom);
tbb::blocked_range<Eigen::vector<RelLinData>::iterator> range(
rld_vec.begin(), rld_vec.end());
tbb::parallel_reduce(range, lopt);
double marg_prior_error = 0;
linearizeMargPrior(marg_order, marg_H, marg_b, aom, lopt.accum.getH(),
lopt.accum.getB(), marg_prior_error);
double error_total = rld_error + marg_prior_error;
if (config.vio_debug)
std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl;
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool converged = false;
if (config.vio_use_lm) { // Use Levenberg–Marquardt
bool step = false;
int max_iter = 10;
while (!step && max_iter > 0 && !converged) {
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
for (int i = 0; i < Hdiag_lambda.size(); i++)
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
double max_inc = inc.array().abs().maxCoeff();
if (max_inc < 1e-4) converged = true;
backup();
// apply increment to poses
for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first;
kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
}
BASALT_ASSERT(frame_states.empty());
// Update points
tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
const auto& rld = rld_vec[i];
updatePoints(aom, rld, inc);
}
};
tbb::parallel_for(keys_range, update_points_func);
double after_update_marg_prior_error = 0;
double after_update_vision_error = 0;
computeError(after_update_vision_error);
computeMargPriorError(marg_order, marg_H, marg_b,
after_update_marg_prior_error);
double after_error_total =
after_update_vision_error + after_update_marg_prior_error;
double f_diff = (error_total - after_error_total);
if (f_diff < 0) {
if (config.vio_debug)
std::cout << "\t[REJECTED] lambda:" << lambda
<< " f_diff: " << f_diff << " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl;
lambda = std::min(max_lambda, lambda_vee * lambda);
lambda_vee *= 2;
restore();
} else {
if (config.vio_debug)
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " f_diff: " << f_diff << " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl;
lambda = std::max(min_lambda, lambda / 3);
lambda_vee = 2;
step = true;
}
max_iter--;
}
if (config.vio_debug && converged) {
std::cout << "[CONVERGED]" << std::endl;
}
} else { // Use Gauss-Newton
Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda;
for (int i = 0; i < Hdiag_lambda.size(); i++)
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
double max_inc = inc.array().abs().maxCoeff();
if (max_inc < 1e-4) converged = true;
// apply increment to poses
for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first;
kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
}
// apply increment to states
for (auto& kv : frame_states) {
int idx = aom.abs_order_map.at(kv.first).first;
kv.second.applyInc(-inc.segment<POSE_VEL_BIAS_SIZE>(idx));
}
// Update points
tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
const auto& rld = rld_vec[i];
updatePoints(aom, rld, inc);
}
};
tbb::parallel_for(keys_range, update_points_func);
}
if (config.vio_debug) {
double after_update_marg_prior_error = 0;
double after_update_vision_error = 0;
computeError(after_update_vision_error);
computeMargPriorError(marg_order, marg_H, marg_b,
after_update_marg_prior_error);
double after_error_total =
after_update_vision_error + after_update_marg_prior_error;
double error_diff = error_total - after_error_total;
auto t2 = std::chrono::high_resolution_clock::now();
auto elapsed =
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
std::cout << "iter " << iter
<< " before_update_error: vision: " << rld_error
<< " marg_prior: " << marg_prior_error
<< " total: " << error_total << std::endl;
std::cout << "iter " << iter << " after_update_error: vision: "
<< after_update_vision_error
<< " marg prior: " << after_update_marg_prior_error
<< " total: " << after_error_total << " error_diff "
<< error_diff << " time : " << elapsed.count()
<< "(us), num_states " << frame_states.size()
<< " num_poses " << frame_poses.size() << std::endl;
if (after_error_total > error_total) {
std::cout << "increased error after update!!!" << std::endl;
}
}
if (iter == config.vio_filter_iteration) {
filterOutliers(config.vio_outlier_threshold, 4);
}
if (converged) break;
// std::cerr << "LT\n" << LT << std::endl;
// std::cerr << "z_p\n" << z_p.transpose() << std::endl;
// std::cerr << "inc\n" << inc.transpose() << std::endl;
}
}
if (config.vio_debug) {
std::cout << "=================================" << std::endl;
}
} // namespace basalt
void KeypointVoEstimator::computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {
for (const auto& kv : lmdb.getObservations()) {
const TimeCamId& tcid_h = kv.first;
for (const auto& obs_kv : kv.second) {
const TimeCamId& tcid_t = obs_kv.first;
if (tcid_t.frame_id != last_state_t_ns) continue;
if (tcid_h != tcid_t) {
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
Sophus::SE3d T_t_h_sophus =
computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
FrameRelLinData rld;
std::visit(
[&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[i];
const KeypointPosition& kpt_pos =
lmdb.getLandmark(kpt_obs.kpt_id);
Eigen::Vector2d res;
Eigen::Vector4d proj;
linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res, nullptr,
nullptr, &proj);
proj[3] = kpt_obs.kpt_id;
data[tcid_t.cam_id].emplace_back(proj);
}
},
calib.intrinsics[tcid_t.cam_id].variant);
} else {
// target and host are the same
// residual does not depend on the pose
// it just depends on the point
std::visit(
[&](const auto& cam) {
for (size_t i = 0; i < obs_kv.second.size(); i++) {
const KeypointObservation& kpt_obs = obs_kv.second[i];
const KeypointPosition& kpt_pos =
lmdb.getLandmark(kpt_obs.kpt_id);
Eigen::Vector2d res;
Eigen::Vector4d proj;
linearizePoint(kpt_obs, kpt_pos, Eigen::Matrix4d::Identity(),
cam, res, nullptr, nullptr, &proj);
proj[3] = kpt_obs.kpt_id;
data[tcid_t.cam_id].emplace_back(proj);
}
},
calib.intrinsics[tcid_t.cam_id].variant);
}
}
}
}
} // namespace basalt

14
src/vi_estimator/landmark_database.cpp
View File

@ -42,6 +42,20 @@ void LandmarkDatabase::addLandmark(int lm_id, const KeypointPosition &pos) {
host_to_kpts[pos.kf_id].emplace(lm_id);
}
void LandmarkDatabase::removeFrame(const FrameId &frame) {
for (auto it = obs.begin(); it != obs.end(); ++it) {
for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) {
if (it2->first.frame_id == frame) {
for (const auto &v : it2->second) kpts_num_obs.at(v.kpt_id)--;
it2 = it->second.erase(it2);
} else {
++it2;
}
}
}
}
void LandmarkDatabase::removeKeyframes(
const std::set<FrameId> &kfs_to_marg,
const std::set<FrameId> &poses_to_marg,

16
src/vi_estimator/vio_estimator.cpp
View File

@ -36,18 +36,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <basalt/vi_estimator/vio_estimator.h>
#include <basalt/vi_estimator/keypoint_vio.h>
#include <basalt/vi_estimator/keypoint_vo.h>
namespace basalt {
VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator(
const VioConfig& config, const Calibration<double>& cam, double int_std_dev,
const Eigen::Vector3d& g) {
KeypointVioEstimator::Ptr res;
const Eigen::Vector3d& g, bool use_imu) {
VioEstimatorBase::Ptr res;
res.reset(new KeypointVioEstimator(int_std_dev, g, cam, config));
res->setMaxKfs(config.vio_max_kfs);
res->setMaxStates(config.vio_max_states);
if (use_imu) {
res.reset(new KeypointVioEstimator(int_std_dev, g, cam, config));
} else {
res.reset(new KeypointVoEstimator(int_std_dev, cam, config));
}
return res;
}
@ -79,7 +81,7 @@ double alignSVD(const std::vector<int64_t>& filter_t_ns,
BASALT_ASSERT_STREAM(int_t_ns > 0, "int_t_ns " << int_t_ns);
// Skip if the interval between gt larger than 100ms
if (int_t_ns > 1e8) continue;
if (int_t_ns > 1.1e8) continue;
double ratio = dt_ns / int_t_ns;

124
src/vio.cpp
View File

@ -84,6 +84,7 @@ pangolin::Plotter* plotter;
pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1500);
pangolin::Var<bool> show_flow("ui.show_flow", false, false, true);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
@ -105,6 +106,7 @@ Button align_se3_btn("ui.align_se3", &alignButton);
pangolin::Var<bool> euroc_fmt("ui.euroc_fmt", true, false, true);
pangolin::Var<bool> tum_rgbd_fmt("ui.tum_rgbd_fmt", false, false, true);
pangolin::Var<bool> kitti_fmt("ui.kitti_fmt", false, false, true);
Button save_traj_btn("ui.save_traj", &saveTrajectoryButton);
pangolin::Var<bool> follow("ui.follow", true, false, true);
@ -187,7 +189,9 @@ int main(int argc, char** argv) {
std::string dataset_type;
std::string config_path;
std::string result_path;
std::string trajectory_fmt;
int num_threads = 0;
bool use_imu = true;
CLI::App app{"App description"};
@ -211,6 +215,9 @@ int main(int argc, char** argv) {
"Path to result file where the system will write RMSE ATE.");
app.add_option("--num-threads", num_threads, "Number of threads.");
app.add_option("--step-by-step", step_by_step, "Path to config file.");
app.add_option("--save-trajectory", trajectory_fmt,
"Save trajectory. Supported formats <tum, euroc, kitti>");
app.add_option("--use-imu", use_imu, "Use IMU.");
if (num_threads > 0) {
tbb::task_scheduler_init init(num_threads);
@ -246,8 +253,6 @@ int main(int argc, char** argv) {
dataset_io->read(dataset_path);
vio_dataset = dataset_io->get_data();
vio_dataset->get_image_timestamps().erase(
vio_dataset->get_image_timestamps().begin());
show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1;
show_frame.Meta().gui_changed = true;
@ -264,7 +269,7 @@ int main(int argc, char** argv) {
const int64_t start_t_ns = vio_dataset->get_image_timestamps().front();
{
vio = basalt::VioEstimatorFactory::getVioEstimator(
vio_config, calib, 0.0001, basalt::constants::g);
vio_config, calib, 0.0001, basalt::constants::g, use_imu);
vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
opt_flow_ptr->output_queue = &vio->vision_data_queue;
@ -424,7 +429,12 @@ int main(int argc, char** argv) {
auto it = vis_map.find(t_ns);
if (it != vis_map.end()) {
auto T_w_i = it->second->states.back();
Sophus::SE3d T_w_i;
if (!it->second->states.empty()) {
T_w_i = it->second->states.back();
} else if (!it->second->frames.empty()) {
T_w_i = it->second->frames.back();
}
T_w_i.so3() = Sophus::SO3d();
camera.Follow(T_w_i.matrix());
@ -451,9 +461,10 @@ int main(int argc, char** argv) {
fmt.gltype = GL_UNSIGNED_SHORT;
fmt.scalable_internal_format = GL_LUMINANCE16;
img_view[cam_id]->SetImage(
img_vec[cam_id].img->ptr, img_vec[cam_id].img->w,
img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);
if (img_vec[cam_id].img.get())
img_view[cam_id]->SetImage(
img_vec[cam_id].img->ptr, img_vec[cam_id].img->w,
img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);
}
draw_plots();
@ -465,11 +476,21 @@ int main(int argc, char** argv) {
}
if (euroc_fmt.GuiChanged()) {
tum_rgbd_fmt = !euroc_fmt;
euroc_fmt = true;
tum_rgbd_fmt = false;
kitti_fmt = false;
}
if (tum_rgbd_fmt.GuiChanged()) {
euroc_fmt = !tum_rgbd_fmt;
tum_rgbd_fmt = true;
euroc_fmt = false;
kitti_fmt = false;
}
if (kitti_fmt.GuiChanged()) {
kitti_fmt = true;
euroc_fmt = false;
tum_rgbd_fmt = false;
}
pangolin::FinishFrame();
@ -505,6 +526,28 @@ int main(int argc, char** argv) {
auto time_end = std::chrono::high_resolution_clock::now();
if (!trajectory_fmt.empty()) {
std::cout << "Saving trajectory..." << std::endl;
if (trajectory_fmt == "kitti") {
kitti_fmt = true;
euroc_fmt = false;
tum_rgbd_fmt = false;
}
if (trajectory_fmt == "euroc") {
euroc_fmt = true;
kitti_fmt = false;
tum_rgbd_fmt = false;
}
if (trajectory_fmt == "tum") {
tum_rgbd_fmt = true;
euroc_fmt = false;
kitti_fmt = false;
}
saveTrajectoryButton();
}
if (!result_path.empty()) {
double error = basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i);
@ -574,6 +617,38 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) {
.Draw(5, 20);
}
}
if (show_flow) {
glLineWidth(1.0);
glColor3f(1.0, 0.0, 0.0);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
if (it != vis_map.end()) {
const Eigen::map<basalt::KeypointId, Eigen::AffineCompact2f>& kp_map =
it->second->opt_flow_res->observations[cam_id];
for (const auto& kv : kp_map) {
Eigen::MatrixXf transformed_patch =
kv.second.linear() * opt_flow_ptr->patch_coord;
transformed_patch.colwise() += kv.second.translation();
for (int i = 0; i < transformed_patch.cols(); i++) {
const Eigen::Vector2f c = transformed_patch.col(i);
pangolin::glDrawCirclePerimeter(c[0], c[1], 0.5f);
}
const Eigen::Vector2f c = kv.second.translation();
if (show_ids)
pangolin::GlFont::I().Text("%d", kv.first).Draw(5 + c[0], 5 + c[1]);
}
pangolin::GlFont::I()
.Text("%d opt_flow patches", kp_map.size())
.Draw(5, 20);
}
}
}
void draw_scene() {
@ -597,6 +672,15 @@ void draw_scene() {
auto it = vis_map.find(t_ns);
if (it != vis_map.end()) {
for (size_t i = 0; i < calib.T_i_c.size(); i++)
if (!it->second->states.empty()) {
render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(),
2.0f, cam_color, 0.1f);
} else if (!it->second->frames.empty()) {
render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(),
2.0f, cam_color, 0.1f);
}
for (const auto& p : it->second->states)
for (size_t i = 0; i < calib.T_i_c.size(); i++)
render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f);
@ -605,10 +689,6 @@ void draw_scene() {
for (size_t i = 0; i < calib.T_i_c.size(); i++)
render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);
for (size_t i = 0; i < calib.T_i_c.size(); i++)
render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(), 2.0f,
cam_color, 0.1f);
glColor3ubv(pose_color);
pangolin::glDrawPoints(it->second->points);
}
@ -722,7 +802,7 @@ void saveTrajectoryButton() {
std::cout
<< "Saved trajectory in TUM RGB-D Dataset format in trajectory.txt"
<< std::endl;
} else {
} else if (euroc_fmt) {
std::ofstream os("trajectory.csv");
os << "#timestamp [ns],p_RS_R_x [m],p_RS_R_y [m],p_RS_R_z [m],q_RS_w "
@ -738,9 +818,21 @@ void saveTrajectoryButton() {
<< "," << pose.unit_quaternion().z() << std::endl;
}
os.close();
std::cout << "Saved trajectory in Euroc Dataset format in trajectory.csv"
<< std::endl;
} else {
std::ofstream os("trajectory_kitti.txt");
for (size_t i = 0; i < vio_t_ns.size(); i++) {
Eigen::Matrix<double, 3, 4> mat = vio_T_w_i[i].matrix3x4();
os << std::scientific << std::setprecision(12) << mat.row(0) << " "
<< mat.row(1) << " " << mat.row(2) << " " << std::endl;
}
os.close();
std::cout
<< "Saved trajectory in KITTI Dataset format in trajectory_kitti.txt"
<< std::endl;
}
}

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit df6b2ca48658a3261370cbf9799ed69f7692525a
Subproject commit 5ff8c21a7867fc9210d08656f9d7fec6da99baeb