diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 79bc72d..0b08a4f 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -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: diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e9688d..f072266 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/data/kitti_config.json b/data/kitti_config.json new file mode 100644 index 0000000..daf90e6 --- /dev/null +++ b/data/kitti_config.json @@ -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 + } +} diff --git a/include/basalt/io/dataset_io_kitti.h b/include/basalt/io/dataset_io_kitti.h new file mode 100644 index 0000000..00ebeda --- /dev/null +++ b/include/basalt/io/dataset_io_kitti.h @@ -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 + +#include +namespace fs = std::experimental::filesystem; + +#include + +namespace basalt { + +class KittiVioDataset : public VioDataset { + size_t num_cams; + + std::string path; + + std::vector image_timestamps; + std::unordered_map 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> image_data; + + Eigen::vector accel_data; + Eigen::vector gyro_data; + + std::vector gt_timestamps; // ordered gt timestamps + Eigen::vector 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 &get_image_timestamps() { return image_timestamps; } + + const Eigen::vector &get_accel_data() const { return accel_data; } + const Eigen::vector &get_gyro_data() const { return gyro_data; } + const std::vector &get_gt_timestamps() const { + return gt_timestamps; + } + const Eigen::vector &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 get_image_data(int64_t t_ns) { + std::vector res(num_cams); + + const std::vector 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(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 data; +}; + +} // namespace basalt diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index 386cbb0..dd5ef4f 100644 --- a/include/basalt/utils/imu_types.h +++ b/include/basalt/utils/imu_types.h @@ -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; diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h index 1c98ea6..8db8e1e 100644 --- a/include/basalt/vi_estimator/ba_base.h +++ b/include/basalt/vi_estimator/ba_base.h @@ -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& frame_states, + const Eigen::map& frame_poses); + /// Triangulates the point and returns homogenous representation. First 3 /// components - unit-length direction vector. Last component inverse /// distance. diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h index 5f7d555..8f47e99 100644 --- a/include/basalt/vi_estimator/keypoint_vio.h +++ b/include/basalt/vi_estimator/keypoint_vio.h @@ -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& frame_states, - const Eigen::map& frame_poses); int64_t get_t_ns() const { return frame_states.at(last_state_t_ns).getState().t_ns; diff --git a/include/basalt/vi_estimator/keypoint_vo.h b/include/basalt/vi_estimator/keypoint_vo.h new file mode 100644 index 0000000..ffc37bf --- /dev/null +++ b/include/basalt/vi_estimator/keypoint_vo.h @@ -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 +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace basalt { + +class KeypointVoEstimator : public VioEstimatorBase, + public BundleAdjustmentBase { + public: + typedef std::shared_ptr Ptr; + + static const int N = 9; + typedef Eigen::Matrix VecN; + typedef Eigen::Matrix MatNN; + typedef Eigen::Matrix MatN3; + + KeypointVoEstimator(double int_std_dev, + const basalt::Calibration& 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& 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(); } + + void computeProjections( + std::vector>& res) const; + + inline void setMaxStates(size_t val) { max_states = val; } + inline void setMaxKfs(size_t val) { max_kfs = val; } + + Eigen::vector getFrameStates() const { + Eigen::vector res; + + for (const auto& kv : frame_states) { + res.push_back(kv.second.getState().T_w_i); + } + + return res; + } + + Eigen::vector getFramePoses() const { + Eigen::vector res; + + for (const auto& kv : frame_poses) { + res.push_back(kv.second.getPose()); + } + + return res; + } + + Eigen::map getAllPosesMap() const { + Eigen::map 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 kf_ids; + + int64_t last_state_t_ns; + + // Input + + Eigen::map prev_opt_flow_res; + + std::map 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 processing_thread; +}; +} // namespace basalt diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h index 4ffe783..69cd42b 100644 --- a/include/basalt/vi_estimator/landmark_database.h +++ b/include/basalt/vi_estimator/landmark_database.h @@ -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& kfs_to_marg, const std::set& poses_to_marg, const std::set& states_to_marg_all); diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h index 8195249..bf35bbf 100644 --- a/include/basalt/vi_estimator/vio_estimator.h +++ b/include/basalt/vi_estimator/vio_estimator.h @@ -101,7 +101,8 @@ class VioEstimatorFactory { static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config, const Calibration& cam, double int_std_dev, - const Eigen::Vector3d& g); + const Eigen::Vector3d& g, + bool use_imu); }; double alignSVD(const std::vector& filter_t_ns, diff --git a/scripts/convert_kitti_calib.py b/scripts/convert_kitti_calib.py new file mode 100755 index 0000000..0d7a77d --- /dev/null +++ b/scripts/convert_kitti_calib.py @@ -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) \ No newline at end of file diff --git a/scripts/eval_full/gen_results_kitti.py b/scripts/eval_full/gen_results_kitti.py new file mode 100755 index 0000000..a51017d --- /dev/null +++ b/scripts/eval_full/gen_results_kitti.py @@ -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']) \ No newline at end of file diff --git a/scripts/eval_full/run_evaluations_kitti.sh b/scripts/eval_full/run_evaluations_kitti.sh new file mode 100755 index 0000000..fd7478f --- /dev/null +++ b/scripts/eval_full/run_evaluations_kitti.sh @@ -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 + diff --git a/src/io/dataset_io.cpp b/src/io/dataset_io.cpp index 9656a02..2c5d789 100644 --- a/src/io/dataset_io.cpp +++ b/src/io/dataset_io.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -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; diff --git a/src/kitti_eval.cpp b/src/kitti_eval.cpp new file mode 100644 index 0000000..a90845f --- /dev/null +++ b/src/kitti_eval.cpp @@ -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 + +#include + +#include +#include + +namespace cereal { + +template +inline void save(Archive& ar, std::map const& map) { + for (const auto& i : map) ar(cereal::make_nvp(i.first, i.second)); +} + +template +inline void load(Archive& ar, std::map& 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 load_poses(const std::string& path) { + Eigen::vector 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& lengths, + const Eigen::vector& poses_gt, + const Eigen::vector& poses_result, + std::map>& res) { + auto lastFrameFromSegmentLength = [](std::vector& 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 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 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 poses_gt = load_poses(gt_path); + const Eigen::vector 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> 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(); + } +} diff --git a/src/opt_flow.cpp b/src/opt_flow.cpp index 66a0102..29c05c5 100644 --- a/src/opt_flow.cpp +++ b/src/opt_flow.cpp @@ -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 show_frame("ui.show_frame", 0, 0, 1500); pangolin::Var show_obs("ui.show_obs", true, false, true); @@ -73,6 +74,7 @@ pangolin::Var show_ids("ui.show_ids", false, false, true); using Button = pangolin::Var>; Button next_step_btn("ui.next_step", &next_step); +Button prev_step_btn("ui.prev_step", &prev_step); pangolin::Var 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; + } +} diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index 24c10b1..14b886b 100644 --- a/src/rs_t265_vio.cpp +++ b/src/rs_t265_vio.cpp @@ -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; diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index a4edc77..06940aa 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -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(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(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& frame_states, + const Eigen::map& 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 diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 6d88dc9..5eb5439 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -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::max(); - max_kfs = std::numeric_limits::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& frame_states, - const Eigen::map& 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(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(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 + diff --git a/src/vi_estimator/keypoint_vio_linearize.cpp b/src/vi_estimator/keypoint_vio_linearize.cpp index b77c5ba..52ccdb0 100644 --- a/src/vi_estimator/keypoint_vio_linearize.cpp +++ b/src/vi_estimator/keypoint_vio_linearize.cpp @@ -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, diff --git a/src/vi_estimator/keypoint_vo.cpp b/src/vi_estimator/keypoint_vo.cpp new file mode 100644 index 0000000..6a2401f --- /dev/null +++ b/src/vi_estimator/keypoint_vo.cpp @@ -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 +#include + +#include + +#include +#include +#include + +#include + +namespace basalt { + +KeypointVoEstimator::KeypointVoEstimator( + double int_std_dev, const basalt::Calibration& 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 num_points_connected; + std::unordered_set 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(); + + 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 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(); + + // 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(); + const Eigen::Vector2d p1 = prev_opt_flow_res[tcido.frame_id] + ->observations[tcido.cam_id] + .at(lm_id) + .translation() + .cast(); + + 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::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& 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 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 kfs_to_marg; + while (kf_ids.size() > max_kfs) { + int64_t id_to_marg = -1; + + { + std::vector 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 ids; + for (int64_t id : kf_ids) { + ids.push_back(id); + } + + int64_t last_kf = *kf_ids.crbegin(); + double min_score = std::numeric_limits::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>> + 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 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 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 rld_vec; + linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); + + BundleAdjustmentBase::LinearizeAbsReduce> lopt( + aom); + + tbb::blocked_range::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(idx)); + } + + BASALT_ASSERT(frame_states.empty()); + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& 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(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(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& 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(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>& 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 diff --git a/src/vi_estimator/landmark_database.cpp b/src/vi_estimator/landmark_database.cpp index b06b931..f1f42ce 100644 --- a/src/vi_estimator/landmark_database.cpp +++ b/src/vi_estimator/landmark_database.cpp @@ -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 &kfs_to_marg, const std::set &poses_to_marg, diff --git a/src/vi_estimator/vio_estimator.cpp b/src/vi_estimator/vio_estimator.cpp index 248920d..068f129 100644 --- a/src/vi_estimator/vio_estimator.cpp +++ b/src/vi_estimator/vio_estimator.cpp @@ -36,18 +36,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace basalt { VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator( const VioConfig& config, const Calibration& 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& 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; diff --git a/src/vio.cpp b/src/vio.cpp index 3bf9ec6..82a625b 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -84,6 +84,7 @@ pangolin::Plotter* plotter; pangolin::Var show_frame("ui.show_frame", 0, 0, 1500); +pangolin::Var show_flow("ui.show_flow", false, false, true); pangolin::Var show_obs("ui.show_obs", true, false, true); pangolin::Var show_ids("ui.show_ids", false, false, true); @@ -105,6 +106,7 @@ Button align_se3_btn("ui.align_se3", &alignButton); pangolin::Var euroc_fmt("ui.euroc_fmt", true, false, true); pangolin::Var tum_rgbd_fmt("ui.tum_rgbd_fmt", false, false, true); +pangolin::Var kitti_fmt("ui.kitti_fmt", false, false, true); Button save_traj_btn("ui.save_traj", &saveTrajectoryButton); pangolin::Var 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 "); + 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& 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 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; } } diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index df6b2ca..5ff8c21 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit df6b2ca48658a3261370cbf9799ed69f7692525a +Subproject commit 5ff8c21a7867fc9210d08656f9d7fec6da99baeb