├── CMakeLists.txt ├── LICENSE ├── include └── slamtools │ ├── bundle_adjustor.h │ ├── common.h │ ├── configurator.h │ ├── essential.h │ ├── factor.h │ ├── frame.h │ ├── homography.h │ ├── initializer.h │ ├── lie_algebra.h │ ├── pnp.h │ ├── preintegrator.h │ ├── random.h │ ├── ransac.h │ ├── sliding_window.h │ ├── state.h │ ├── stereo.h │ └── track.h └── src └── slamtools ├── bundle_adjustor.cpp ├── ceres ├── lie_algebra_eigen_quaternion_parameterization.h ├── preintegration_error_cost.h └── reprojection_error_cost.h ├── configurator.cpp ├── essential.cpp ├── factor.cpp ├── frame.cpp ├── homography.cpp ├── initializer.cpp ├── lie_algebra.cpp ├── pnp.cpp ├── preintegrator.cpp ├── sliding_window.cpp ├── stereo.cpp └── track.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR) 2 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules") 3 | project(slamtools VERSION 0.1.0 LANGUAGES CXX) 4 | 5 | find_package(Eigen3 REQUIRED) 6 | find_package(Ceres REQUIRED COMPONENTS LAPACK SuiteSparse EigenSparse SchurSpecializations) 7 | 8 | set(SLAMTOOLS_LIB_SOURCES 9 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/homography.cpp 10 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/essential.cpp 11 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/stereo.cpp 12 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/lie_algebra.cpp 13 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/frame.cpp 14 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/initializer.cpp 15 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/track.cpp 16 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/sliding_window.cpp 17 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/factor.cpp 18 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/preintegrator.cpp 19 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/pnp.cpp 20 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/bundle_adjustor.cpp 21 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slamtools/configurator.cpp 22 | ) 23 | 24 | add_library(slamtools 25 | ${SLAMTOOLS_LIB_SOURCES} 26 | ) 27 | 28 | target_include_directories(slamtools 29 | PRIVATE 30 | ${CERES_INCLUDE_DIRS} 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src 32 | PUBLIC 33 | ${CMAKE_CURRENT_SOURCE_DIR}/include 34 | ) 35 | 36 | target_compile_features(slamtools 37 | PUBLIC 38 | cxx_std_17 39 | ) 40 | 41 | target_compile_options(slamtools 42 | PRIVATE 43 | -pedantic 44 | $<$:-Wextra> 45 | ) 46 | 47 | target_link_libraries(slamtools 48 | PUBLIC 49 | Eigen3::Eigen 50 | ${CERES_LIBRARIES} 51 | ) 52 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Jinyu Li 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /include/slamtools/bundle_adjustor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class SlidingWindow; 6 | class Frame; 7 | 8 | class BundleAdjustor { 9 | struct BundleAdjustorSolver; // pimpl 10 | 11 | public: 12 | BundleAdjustor(); 13 | virtual ~BundleAdjustor(); 14 | 15 | bool solve(SlidingWindow *map, bool use_inertial = true, size_t max_iter = 50, const double &max_time = 1.0e6); 16 | 17 | private: 18 | std::unique_ptr solver; 19 | }; 20 | -------------------------------------------------------------------------------- /include/slamtools/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define GRAVITY_NOMINAL 9.80665 21 | 22 | template 23 | struct Comparator; /* 24 | constexpr bool operator()(const T &a, const T &b) const; 25 | */ 26 | 27 | template 28 | struct Comparator { 29 | constexpr bool operator()(const T *a, const T *b) const { 30 | return Comparator()(*a, *b); 31 | } 32 | }; 33 | 34 | inline constexpr size_t nil() { 35 | return size_t(-1); 36 | } 37 | 38 | struct IMUData { 39 | double t; // timestamp 40 | Eigen::Vector3d w; // gyro measurement 41 | Eigen::Vector3d a; // accelerometer measurement 42 | }; 43 | 44 | struct Image { 45 | double t; 46 | Eigen::Vector3d g; // an external gravity measured by the device, it is only used for comparison or algorithms that uses external gravity. 47 | virtual ~Image() = default; 48 | virtual void detect_keypoints(std::vector &keypoints, size_t max_points = 0) const = 0; 49 | virtual void track_keypoints(const Image *next_image, const std::vector &curr_keypoints, std::vector &next_keypoints, std::vector &result_status) const = 0; 50 | virtual void detect_segments(std::vector> &segments, size_t max_segments = 0) const = 0; 51 | }; 52 | 53 | template <> 54 | struct Comparator { 55 | constexpr bool operator()(const Image &a, const Image &b) const { 56 | return a.t < b.t; 57 | } 58 | }; 59 | -------------------------------------------------------------------------------- /include/slamtools/configurator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class Configurator { 6 | public: 7 | static std::shared_ptr default_config(); 8 | virtual ~Configurator(); 9 | virtual Eigen::Matrix3d camera_intrinsic() const; 10 | virtual double keypoint_pixel_error() const; 11 | virtual Eigen::Quaterniond camera_to_center_rotation() const; 12 | virtual Eigen::Vector3d camera_to_center_translation() const; 13 | virtual Eigen::Quaterniond imu_to_center_rotation() const; 14 | virtual Eigen::Vector3d imu_to_center_translation() const; 15 | virtual Eigen::Matrix3d imu_gyro_white_noise() const; 16 | virtual Eigen::Matrix3d imu_accel_white_noise() const; 17 | virtual Eigen::Matrix3d imu_gyro_random_walk() const; 18 | virtual Eigen::Matrix3d imu_accel_random_walk() const; 19 | virtual int random() const; 20 | virtual size_t max_keypoint_detection() const; 21 | virtual size_t max_init_raw_frames() const; 22 | virtual size_t min_init_raw_frames() const; 23 | virtual size_t min_raw_matches() const; 24 | virtual double min_raw_parallax() const; 25 | virtual size_t min_raw_triangulation() const; 26 | virtual size_t init_map_frames() const; 27 | virtual size_t min_init_map_landmarks() const; 28 | virtual bool init_refine_imu() const; 29 | virtual size_t solver_iteration_limit() const; 30 | virtual double solver_time_limit() const; 31 | virtual size_t tracking_window_size() const; 32 | virtual bool predict_keypoints() const; 33 | }; 34 | -------------------------------------------------------------------------------- /include/slamtools/essential.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void decompose_essential(const Eigen::Matrix3d &E, Eigen::Matrix3d &R1, Eigen::Matrix3d &R2, Eigen::Vector3d &T); 6 | 7 | // p2^T * E * p1 = 0 8 | std::vector solve_essential_5pt(const std::array &points1, const std::array &points2); 9 | 10 | // d(p2, E * p1) 11 | inline double essential_geometric_error(const Eigen::Matrix3d &E, const Eigen::Vector2d &p1, const Eigen::Vector2d &p2) { 12 | Eigen::Vector3d Ep1 = E * p1.homogeneous(); 13 | double r = p2.homogeneous().transpose() * Ep1; 14 | return r * r / Ep1.segment<2>(0).squaredNorm(); 15 | } 16 | -------------------------------------------------------------------------------- /include/slamtools/factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class Frame; 6 | 7 | class Factor { 8 | Factor() = delete; 9 | struct factor_construct_t {}; 10 | 11 | public: 12 | struct FactorCostFunction { 13 | virtual ~FactorCostFunction() = default; 14 | }; 15 | 16 | static std::unique_ptr create_reprojection_error(Frame *frame, size_t keypoint_id); 17 | static std::unique_ptr create_preintegration_error(Frame *frame_i, Frame *frame_j); 18 | 19 | template 20 | T *get_cost_function() { 21 | return static_cast(cost_function.get()); 22 | } 23 | 24 | Factor(std::unique_ptr cost_function, const factor_construct_t &); 25 | virtual ~Factor(); 26 | 27 | private: 28 | std::unique_ptr cost_function; 29 | }; 30 | -------------------------------------------------------------------------------- /include/slamtools/frame.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class Track; 8 | class SlidingWindow; 9 | class Factor; 10 | class Configurator; 11 | 12 | struct create_if_empty_t {}; 13 | extern create_if_empty_t create_if_empty; 14 | 15 | class Frame { 16 | friend class Track; 17 | friend class SlidingWindow; 18 | SlidingWindow *map; 19 | 20 | public: 21 | Frame(); 22 | virtual ~Frame(); 23 | 24 | std::unique_ptr clone() const; 25 | 26 | size_t keypoint_num() const { 27 | return keypoints.size(); 28 | } 29 | 30 | const Eigen::Vector2d &get_keypoint(size_t keypoint_id) const { 31 | return keypoints[keypoint_id]; 32 | } 33 | 34 | Track *get_track(size_t keypoint_id) const { 35 | return tracks[keypoint_id]; 36 | } 37 | 38 | Track *get_track(size_t keypoint_id, const create_if_empty_t &); 39 | 40 | Factor *get_reprojection_factor(size_t keypoint_id) { 41 | return reprojection_factors[keypoint_id].get(); 42 | } 43 | 44 | Factor *get_preintegration_factor() { 45 | return preintegration_factor.get(); 46 | } 47 | 48 | void detect_keypoints(Configurator *config); 49 | void track_keypoints(Frame *next_frame, Configurator *config); 50 | void detect_segments(size_t max_segments = 0); 51 | 52 | PoseState get_pose(const ExtrinsicParams &sensor) const; 53 | void set_pose(const ExtrinsicParams &sensor, const PoseState &pose); 54 | 55 | Eigen::Matrix3d K; 56 | Eigen::Matrix2d sqrt_inv_cov; 57 | std::shared_ptr image; 58 | 59 | PoseState pose; 60 | MotionState motion; 61 | ExtrinsicParams camera; 62 | ExtrinsicParams imu; 63 | 64 | PreIntegrator preintegration; 65 | 66 | Eigen::Vector3d external_gravity; 67 | std::vector> segments; 68 | 69 | private: 70 | std::vector keypoints; 71 | std::vector tracks; 72 | std::vector> reprojection_factors; 73 | std::unique_ptr preintegration_factor; 74 | }; 75 | 76 | template <> 77 | struct Comparator { 78 | constexpr bool operator()(const Frame &frame_i, const Frame &frame_j) const { 79 | return Comparator()(frame_i.image.get(), frame_j.image.get()); 80 | } 81 | }; 82 | -------------------------------------------------------------------------------- /include/slamtools/homography.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | bool decompose_homography(const Eigen::Matrix3d &H, Eigen::Matrix3d &R1, Eigen::Matrix3d &R2, Eigen::Vector3d &T1, Eigen::Vector3d &T2, Eigen::Vector3d &n1, Eigen::Vector3d &n2); 6 | 7 | // p2 = H * p1 8 | Eigen::Matrix3d solve_homography_4pt(const std::array &points1, const std::array &points2); 9 | 10 | // p2 = H * p1 11 | Eigen::Matrix3d solve_homography(const std::vector &points1, const std::vector &points2); 12 | 13 | // d(p2, H * p1) 14 | inline double homography_geometric_error(const Eigen::Matrix3d &H, const Eigen::Vector2d &p1, const Eigen::Vector2d &p2) { 15 | return (p2 - (H * p1.homogeneous()).hnormalized()).squaredNorm(); 16 | } 17 | -------------------------------------------------------------------------------- /include/slamtools/initializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class Frame; 6 | class SlidingWindow; 7 | class Configurator; 8 | 9 | class Initializer { 10 | public: 11 | Initializer(std::shared_ptr config); 12 | virtual ~Initializer(); 13 | 14 | virtual void append_frame(std::unique_ptr frame); 15 | virtual std::unique_ptr init() const = 0; 16 | 17 | virtual std::unique_ptr init_sfm() const; 18 | 19 | protected: 20 | std::unique_ptr raw; 21 | std::shared_ptr config; 22 | }; 23 | -------------------------------------------------------------------------------- /include/slamtools/lie_algebra.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | inline Eigen::Matrix3d hat(const Eigen::Vector3d &w) { 6 | return (Eigen::Matrix3d() << 0, -w.z(), w.y(), 7 | w.z(), 0, -w.x(), 8 | -w.y(), w.x(), 0) 9 | .finished(); 10 | } 11 | 12 | inline Eigen::Quaterniond expmap(const Eigen::Vector3d &w) { 13 | Eigen::AngleAxisd aa(w.norm(), w.stableNormalized()); 14 | Eigen::Quaterniond q; 15 | q = aa; 16 | return q; 17 | } 18 | 19 | inline Eigen::Vector3d logmap(const Eigen::Quaterniond &q) { 20 | Eigen::AngleAxisd aa(q); 21 | return aa.angle() * aa.axis(); 22 | } 23 | 24 | Eigen::Matrix3d right_jacobian(const Eigen::Vector3d &w); 25 | Eigen::Matrix s2_tangential_basis(const Eigen::Vector3d &x); 26 | -------------------------------------------------------------------------------- /include/slamtools/pnp.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class Frame; 6 | class SlidingWindow; 7 | 8 | void visual_inertial_pnp(SlidingWindow *map, Frame *frame, bool use_inertial = true, size_t max_iter = 50, const double &max_time = 1.0e6); 9 | -------------------------------------------------------------------------------- /include/slamtools/preintegrator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | struct PreIntegrator { 7 | struct Delta { 8 | double t; 9 | Eigen::Quaterniond q; 10 | Eigen::Vector3d p; 11 | Eigen::Vector3d v; 12 | Eigen::Matrix cov; // ordered in q, p, v, bg, ba 13 | Eigen::Matrix sqrt_inv_cov; 14 | }; 15 | 16 | struct Jacobian { 17 | Eigen::Matrix3d dq_dbg; 18 | Eigen::Matrix3d dp_dbg; 19 | Eigen::Matrix3d dp_dba; 20 | Eigen::Matrix3d dv_dbg; 21 | Eigen::Matrix3d dv_dba; 22 | }; 23 | 24 | void reset(); 25 | void increment(double dt, const IMUData &data, const Eigen::Vector3d &bg, const Eigen::Vector3d &ba, bool compute_jacobian, bool compute_covariance); 26 | bool integrate(double t, const Eigen::Vector3d &bg, const Eigen::Vector3d &ba, bool compute_jacobian, bool compute_covariance); 27 | void compute_sqrt_inv_cov(); 28 | 29 | Eigen::Matrix3d cov_w; // continuous noise covariance 30 | Eigen::Matrix3d cov_a; 31 | Eigen::Matrix3d cov_bg; // continuous random walk noise covariance 32 | Eigen::Matrix3d cov_ba; 33 | 34 | Delta delta; 35 | Jacobian jacobian; 36 | 37 | std::vector data; 38 | }; 39 | -------------------------------------------------------------------------------- /include/slamtools/random.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | inline unsigned int get_random_seed() { 6 | static std::random_device rd; 7 | return rd(); 8 | } 9 | 10 | class _RandomBase { 11 | public: 12 | _RandomBase() { 13 | seed(); 14 | } 15 | 16 | void seed() { 17 | engine.seed(get_random_seed()); 18 | } 19 | 20 | void seed(unsigned int value) { 21 | engine.seed(value); 22 | } 23 | 24 | protected: 25 | std::default_random_engine engine; 26 | }; 27 | 28 | template 29 | class UniformNoise : public _RandomBase { 30 | public: 31 | typedef T value_type; 32 | 33 | UniformNoise(value_type left = value_type(0.0), value_type right = value_type(1.0)) : 34 | distribution(left, right) { 35 | } 36 | 37 | value_type next() { 38 | return distribution(engine); 39 | } 40 | 41 | private: 42 | std::uniform_real_distribution distribution; 43 | }; 44 | 45 | template 46 | class GaussianNoise : public _RandomBase { 47 | public: 48 | typedef T value_type; 49 | 50 | GaussianNoise(value_type sigma = value_type(1.0), value_type mean = value_type(0.0)) : 51 | distribution(mean, sigma) { 52 | } 53 | 54 | value_type next() { 55 | return distribution(engine); 56 | } 57 | 58 | private: 59 | std::normal_distribution distribution; 60 | }; 61 | 62 | template 63 | class UniformInteger : public _RandomBase { 64 | public: 65 | typedef T value_type; 66 | 67 | // [left, right] 68 | UniformInteger(value_type left = value_type(0), value_type right = std::numeric_limits::max()) : 69 | distribution(left, right) { 70 | } 71 | 72 | void param(value_type left, value_type right) { 73 | distribution.param(std::uniform_int_distribution::param_type(left, right)); 74 | } 75 | 76 | value_type next() { 77 | return distribution(engine); 78 | } 79 | 80 | value_type next(value_type left, value_type right) { 81 | return distribution(engine, typename std::uniform_int_distribution::param_type(left, right)); 82 | } 83 | 84 | private: 85 | std::uniform_int_distribution distribution; 86 | }; 87 | 88 | class LotBox { 89 | public: 90 | LotBox(size_t size) : 91 | cap(0), lots(size) { 92 | std::iota(lots.begin(), lots.end(), 0); 93 | } 94 | 95 | size_t draw_with_replacement() { 96 | size_t result = draw_without_replacement(); 97 | refill_last(); 98 | return result; 99 | } 100 | 101 | size_t draw_without_replacement() { 102 | if (remaining() > 1) { 103 | std::swap(lots[cap], lots[dice.next(cap, lots.size() - 1)]); 104 | size_t result = lots[cap]; 105 | cap++; 106 | return result; 107 | } else if (remaining() == 1) { 108 | cap++; 109 | return lots.back(); 110 | } else { 111 | return size_t(-1); // Hey we have nothing left! 112 | } 113 | } 114 | 115 | void refill_last(size_t n = 1) { 116 | if (cap > n) { 117 | cap -= n; 118 | } else { 119 | cap = 0; 120 | } 121 | } 122 | 123 | void refill_all() { 124 | cap = 0; 125 | } 126 | 127 | size_t remaining() const { 128 | return lots.size() - cap; 129 | } 130 | 131 | void seed() { 132 | dice.seed(get_random_seed()); 133 | } 134 | 135 | void seed(unsigned int value) { 136 | dice.seed(value); 137 | } 138 | 139 | private: 140 | size_t cap; 141 | std::vector lots; 142 | UniformInteger dice; 143 | }; 144 | 145 | template 146 | class WhiteNoise { 147 | public: 148 | typedef T value_type; 149 | WhiteNoise(value_type sigma, value_type freq = 1, value_type mean = 0) : 150 | n(sigma, mean), isdt(sqrt(freq)) { 151 | } 152 | 153 | void seed() { 154 | n.seed(); 155 | } 156 | 157 | void seed(unsigned int value) { 158 | n.seed(value); 159 | } 160 | 161 | value_type next() { 162 | return n.next() * isdt; 163 | } 164 | 165 | private: 166 | GaussianNoise n; 167 | double isdt; 168 | }; 169 | 170 | template 171 | class RandomWalk { 172 | public: 173 | typedef T value_type; 174 | 175 | RandomWalk(value_type sigma, value_type freq = 1, value_type init = 0, value_type bias = 0) : 176 | v(init), n(sigma, bias), sdt(sqrt(1 / freq)) { 177 | } 178 | 179 | void seed() { 180 | n.seed(); 181 | } 182 | 183 | void seed(unsigned int value) { 184 | n.seed(value); 185 | } 186 | 187 | void init(const value_type &v) { 188 | this->v = v; 189 | } 190 | 191 | value_type next() { 192 | step(); 193 | return v; 194 | } 195 | 196 | private: 197 | void step() { 198 | v += n.next() * sdt; 199 | } 200 | 201 | double v; 202 | GaussianNoise n; 203 | double sdt; 204 | }; 205 | -------------------------------------------------------------------------------- /include/slamtools/ransac.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | template 7 | struct Ransac { 8 | double threshold; 9 | double confidence; 10 | size_t max_iteration; 11 | int seed; 12 | Ransac(double threshold, double confidence = 0.999, size_t max_iteration = 1000, int seed = 0) : 13 | threshold(threshold), confidence(confidence), max_iteration(max_iteration), seed(seed) { 14 | } 15 | 16 | template 17 | ModelType solve(const std::vector &... data) { 18 | std::vector _; 19 | return solve(_, data...); 20 | } 21 | 22 | template 23 | ModelType solve(std::vector &inlier_mask, const std::vector &... data) { 24 | std::tuple &...> tdata = std::make_tuple(std::cref(data)...); 25 | size_t size = std::get<0>(tdata).size(); 26 | 27 | LotBox lotbox(size); 28 | lotbox.seed(seed); 29 | 30 | double K = log(std::max(1 - confidence, 1.0e-5)); 31 | 32 | ModelType best_model; 33 | size_t best_inliers = 0; 34 | 35 | size_t iter_max = max_iteration; 36 | for (size_t iter = 0; iter < iter_max; ++iter) { 37 | std::tuple...> tsample; 38 | 39 | lotbox.refill_all(); 40 | for (size_t si = 0; si < ModelDoF; ++si) { 41 | size_t sample_id = lotbox.draw_without_replacement(); 42 | make_sample(tdata, tsample, sample_id, si); 43 | } 44 | 45 | std::vector models{apply(ModelSolver(), tsample)}; 46 | for (const ModelType ¤t_model : models) { 47 | size_t current_inliers = 0; 48 | std::vector current_inlier_mask(size, 0); 49 | ModelEvaluator eval(current_model); 50 | for (size_t i = 0; i < size; ++i) { 51 | double error = eval(data[i]...); 52 | if (error <= threshold) { 53 | current_inliers++; 54 | current_inlier_mask[i] = 1; 55 | } 56 | } 57 | 58 | if (current_inliers > best_inliers) { 59 | best_model = current_model; 60 | best_inliers = current_inliers; 61 | inlier_mask.swap(current_inlier_mask); 62 | 63 | double inlier_ratio = best_inliers / (double)size; 64 | double N = K / log(1 - pow(inlier_ratio, 5)); 65 | if (N < (double)iter_max) { 66 | iter_max = (size_t)ceil(N); 67 | } 68 | } 69 | } 70 | } 71 | 72 | return best_model; 73 | } 74 | 75 | private: 76 | template 77 | static void make_sample_impl(Data &&data, Sample &&sample, size_t idata, size_t isample, std::index_sequence) { 78 | auto ret = {(std::get(sample)[isample] = std::get(data)[idata])...}; 79 | } 80 | 81 | template 82 | static void make_sample(Data &&data, Sample &&sample, size_t idata, size_t isample) { 83 | make_sample_impl(std::forward(data), std::forward(sample), idata, isample, 84 | std::make_index_sequence::type>::value>{}); 85 | } 86 | }; 87 | -------------------------------------------------------------------------------- /include/slamtools/sliding_window.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class Frame; 6 | class Track; 7 | class Factor; 8 | 9 | class SlidingWindow { 10 | friend class Track; 11 | struct construct_by_map_t; 12 | 13 | public: 14 | SlidingWindow(); 15 | virtual ~SlidingWindow(); 16 | 17 | void clear(); 18 | 19 | size_t frame_num() const { 20 | return frames.size(); 21 | } 22 | 23 | Frame *get_frame(size_t id) const { 24 | return frames[id].get(); 25 | } 26 | 27 | void put_frame(std::unique_ptr frame, size_t pos = nil()); 28 | 29 | void erase_frame(size_t id); 30 | 31 | size_t track_num() const { 32 | return tracks.size(); 33 | } 34 | 35 | Track *get_track(size_t id) const { 36 | return tracks[id].get(); 37 | } 38 | 39 | Track *create_track(); 40 | void erase_track(Track *track); 41 | 42 | void prune_tracks(const std::function &condition); 43 | 44 | private: 45 | void recycle_track(Track *track); 46 | 47 | std::deque> frames; 48 | std::vector> tracks; 49 | }; 50 | -------------------------------------------------------------------------------- /include/slamtools/state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | enum ErrorStateLocation { 6 | ES_Q = 0, 7 | ES_P = 3, 8 | ES_V = 6, 9 | ES_BG = 9, 10 | ES_BA = 12, 11 | ES_SIZE = 15 12 | }; 13 | 14 | enum PoseFlag { 15 | PF_FIXED = 0, 16 | PF_SIZE 17 | }; 18 | 19 | enum LandmarkFlag { 20 | LF_VALID = 0, 21 | LF_SIZE 22 | }; 23 | 24 | struct ExtrinsicParams { 25 | Eigen::Quaterniond q_cs; 26 | Eigen::Vector3d p_cs; 27 | }; 28 | 29 | struct PoseState { 30 | PoseState() { 31 | q.setIdentity(); 32 | p.setZero(); 33 | flags.reset(); 34 | } 35 | 36 | bool flag(PoseFlag f) const { 37 | return flags[f]; 38 | } 39 | 40 | std::bitset::reference flag(PoseFlag f) { 41 | return flags[f]; 42 | } 43 | 44 | Eigen::Quaterniond q; 45 | Eigen::Vector3d p; 46 | 47 | private: 48 | std::bitset flags; 49 | }; 50 | 51 | struct MotionState { 52 | MotionState() { 53 | v.setZero(); 54 | bg.setZero(); 55 | ba.setZero(); 56 | } 57 | 58 | Eigen::Vector3d v; 59 | Eigen::Vector3d bg; 60 | Eigen::Vector3d ba; 61 | }; 62 | 63 | struct LandmarkState { 64 | LandmarkState() { 65 | x.setZero(); 66 | quality = 0; 67 | flags.reset(); 68 | } 69 | 70 | Eigen::Vector3d x; 71 | double quality; 72 | 73 | bool flag(LandmarkFlag f) const { 74 | return flags[f]; 75 | } 76 | 77 | std::bitset::reference flag(LandmarkFlag f) { 78 | return flags[f]; 79 | } 80 | 81 | private: 82 | std::bitset flags; 83 | }; 84 | -------------------------------------------------------------------------------- /include/slamtools/stereo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | inline Eigen::Vector2d apply_k(const Eigen::Vector2d &p, const Eigen::Matrix3d &K) { 6 | return {p(0) * K(0, 0) + K(0, 2), p(1) * K(1, 1) + K(1, 2)}; 7 | } 8 | 9 | inline Eigen::Vector2d remove_k(const Eigen::Vector2d &p, const Eigen::Matrix3d &K) { 10 | return {(p(0) - K(0, 2)) / K(0, 0), (p(1) - K(1, 2)) / K(1, 1)}; 11 | } 12 | 13 | Eigen::Matrix3d find_essential_matrix(const std::vector &points1, const std::vector &points2, double threshold = 1.0, double confidence = 0.999, size_t max_iteration = 1000, int seed = 0); 14 | Eigen::Matrix3d find_homography_matrix(const std::vector &points1, const std::vector &points2, double threshold = 1.0, double confidence = 0.999, size_t max_iteration = 1000, int seed = 0); 15 | Eigen::Matrix3d find_essential_matrix(const std::vector &points1, const std::vector &points2, std::vector &inlier_mask, double threshold = 1.0, double confidence = 0.999, size_t max_iteration = 1000, int seed = 0); 16 | Eigen::Matrix3d find_homography_matrix(const std::vector &points1, const std::vector &points2, std::vector &inlier_mask, double threshold = 1.0, double confidence = 0.999, size_t max_iteration = 1000, int seed = 0); 17 | 18 | Eigen::Vector4d triangulate_point(const Eigen::Matrix &P1, const Eigen::Matrix &P2, const Eigen::Vector2d &point1, const Eigen::Vector2d &point2); 19 | Eigen::Vector4d triangulate_point(const std::vector> &Ps, const std::vector &points); 20 | 21 | bool triangulate_point_checked(const Eigen::Matrix &P1, const Eigen::Matrix &P2, const Eigen::Vector2d &point1, const Eigen::Vector2d &point2, Eigen::Vector3d &p); 22 | bool triangulate_point_checked(const std::vector> &Ps, const std::vector &points, Eigen::Vector3d &p); 23 | bool triangulate_point_scored(const Eigen::Matrix &P1, const Eigen::Matrix &P2, const Eigen::Vector2d &point1, const Eigen::Vector2d &point2, Eigen::Vector3d &p, double &score); 24 | bool triangulate_point_scored(const std::vector> &Ps, const std::vector &points, Eigen::Vector3d &p, double &score); 25 | 26 | size_t triangulate_from_rt(const std::vector &points1, const std::vector &points2, const Eigen::Matrix3d &R, const Eigen::Vector3d &T, std::vector &result_points, std::vector &result_status); 27 | size_t triangulate_from_rt(const std::vector &points1, const std::vector &points2, const std::vector &Rs, const std::vector &Ts, std::vector &result_points, Eigen::Matrix3d &result_R, Eigen::Vector3d &result_T, std::vector &result_status); 28 | 29 | size_t triangulate_from_rt_scored(const std::vector &points1, const std::vector &points2, const Eigen::Matrix3d &R, const Eigen::Vector3d &T, std::vector &result_points, std::vector &result_status, double &score); 30 | size_t triangulate_from_rt_scored(const std::vector &points1, const std::vector &points2, const std::vector &Rs, const std::vector &Ts, size_t count_threshold, std::vector &result_points, Eigen::Matrix3d &result_R, Eigen::Vector3d &result_T, std::vector &result_status); 31 | 32 | size_t triangulate_from_essential(const std::vector &points1, const std::vector &points2, const Eigen::Matrix3d &E, std::vector &result_points, std::vector &result_status, Eigen::Matrix3d &R, Eigen::Vector3d &T); 33 | size_t triangulate_from_homography(const std::vector &points1, const std::vector &points2, const Eigen::Matrix3d &H, std::vector &result_points, std::vector &result_status, Eigen::Matrix3d &R, Eigen::Vector3d &T); 34 | 35 | inline Eigen::Vector4d triangulate_point(const Eigen::Matrix &P1, const Eigen::Matrix &P2, const Eigen::Vector2d &point1, const Eigen::Vector2d &point2) { 36 | Eigen::Matrix4d A; 37 | A.row(0) = point1(0) * P1.row(2) - P1.row(0); 38 | A.row(1) = point1(1) * P1.row(2) - P1.row(1); 39 | A.row(2) = point2(0) * P2.row(2) - P2.row(0); 40 | A.row(3) = point2(1) * P2.row(2) - P2.row(1); 41 | return A.jacobiSvd(Eigen::ComputeFullV).matrixV().col(3); 42 | } 43 | 44 | inline Eigen::Vector4d triangulate_point(const std::vector> &Ps, const std::vector &points) { 45 | Eigen::Matrix A(points.size() * 2, 4); 46 | for (size_t i = 0; i < points.size(); ++i) { 47 | A.row(i * 2 + 0) = points[i](0) * Ps[i].row(2) - Ps[i].row(0); 48 | A.row(i * 2 + 1) = points[i](1) * Ps[i].row(2) - Ps[i].row(1); 49 | } 50 | return A.jacobiSvd(Eigen::ComputeFullV).matrixV().col(3); 51 | } 52 | 53 | inline bool triangulate_point_checked(const Eigen::Matrix &P1, const Eigen::Matrix &P2, const Eigen::Vector2d &point1, const Eigen::Vector2d &point2, Eigen::Vector3d &p) { 54 | double score; 55 | return triangulate_point_scored(P1, P2, point1, point2, p, score); 56 | } 57 | 58 | inline bool triangulate_point_checked(const std::vector> &Ps, const std::vector &points, Eigen::Vector3d &p) { 59 | double score; 60 | return triangulate_point_scored(Ps, points, p, score); 61 | } 62 | 63 | inline bool triangulate_point_scored(const Eigen::Matrix &P1, const Eigen::Matrix &P2, const Eigen::Vector2d &point1, const Eigen::Vector2d &point2, Eigen::Vector3d &p, double &score) { 64 | Eigen::Vector4d q = triangulate_point(P1, P2, point1, point2); 65 | score = 0; 66 | 67 | Eigen::Vector3d q1 = P1 * q; 68 | Eigen::Vector3d q2 = P2 * q; 69 | 70 | if (q1[2] * q[3] > 0 && q2[2] * q[3] > 0) { 71 | if (q1[2] / q[3] < 100 && q2[2] / q[3] < 100) { 72 | p = q.hnormalized(); 73 | score = 0.5 * ((q1.hnormalized() - point1).squaredNorm() + (q2.hnormalized() - point2).squaredNorm()); 74 | return true; 75 | } 76 | } 77 | 78 | return false; 79 | } 80 | 81 | inline bool triangulate_point_scored(const std::vector> &Ps, const std::vector &points, Eigen::Vector3d &p, double &score) { 82 | if (Ps.size() < 2) return false; 83 | Eigen::Vector4d q = triangulate_point(Ps, points); 84 | score = 0; 85 | 86 | for (size_t i = 0; i < points.size(); ++i) { 87 | Eigen::Vector3d qi = Ps[i] * q; 88 | if (!(qi[2] * q[3] > 0)) { 89 | return false; 90 | } 91 | if (!(qi[2] / q[3] < 100)) { 92 | return false; 93 | } 94 | score += (qi.hnormalized() - points[i]).squaredNorm(); 95 | } 96 | score /= points.size(); 97 | p = q.hnormalized(); 98 | return true; 99 | } 100 | -------------------------------------------------------------------------------- /include/slamtools/track.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Frame; 9 | class SlidingWindow; 10 | 11 | class Track { 12 | friend class SlidingWindow; 13 | size_t id_in_map; 14 | SlidingWindow *map; 15 | Track(); 16 | 17 | public: 18 | Track(const SlidingWindow::construct_by_map_t &) : 19 | Track() { 20 | } 21 | 22 | virtual ~Track(); 23 | 24 | const std::map> &keypoint_map() const { 25 | return keypoint_refs; 26 | } 27 | 28 | bool has_keypoint(Frame *frame) const { 29 | return keypoint_refs.count(frame) > 0; 30 | } 31 | 32 | size_t get_keypoint_id(Frame *frame) const { 33 | if (has_keypoint(frame)) { 34 | return keypoint_refs.at(frame); 35 | } else { 36 | return nil(); 37 | } 38 | } 39 | 40 | const Eigen::Vector2d &get_keypoint(Frame *frame) const; 41 | 42 | void add_keypoint(Frame *frame, size_t keypoint_id); 43 | 44 | void remove_keypoint(Frame *frame, bool suicide_if_empty = true); 45 | 46 | bool triangulate(); 47 | 48 | LandmarkState landmark; 49 | 50 | private: 51 | std::map> keypoint_refs; 52 | }; 53 | 54 | // This comparator is incorrect in theory, consider if a and b are modified... 55 | // Hence, don't use this in production. 56 | template <> 57 | struct Comparator { 58 | bool operator()(const Track &a, const Track &b) const { 59 | auto ia = a.keypoint_map().cbegin(); 60 | auto ib = b.keypoint_map().cbegin(); 61 | while (ia != a.keypoint_map().cend() && ib != b.keypoint_map().cend()) { 62 | bool eab = Comparator()(ia->first, ib->first); 63 | bool eba = Comparator()(ib->first, ia->first); 64 | if (eab || eba) { 65 | return eab; 66 | } else if (ia->second != ib->second) { 67 | return ia->second < ib->second; 68 | } else { 69 | ia++; 70 | ib++; 71 | } 72 | } 73 | return ia != a.keypoint_map().cend(); 74 | } 75 | }; 76 | -------------------------------------------------------------------------------- /src/slamtools/bundle_adjustor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace Eigen; 13 | using namespace ceres; 14 | 15 | struct BundleAdjustor::BundleAdjustorSolver { 16 | BundleAdjustorSolver() { 17 | cauchy_loss = std::make_unique(1.0); 18 | quaternion_parameterization = std::make_unique(); 19 | } 20 | std::unique_ptr cauchy_loss; 21 | std::unique_ptr quaternion_parameterization; 22 | }; 23 | 24 | BundleAdjustor::BundleAdjustor() { 25 | solver = std::make_unique(); 26 | } 27 | 28 | BundleAdjustor::~BundleAdjustor() = default; 29 | 30 | bool BundleAdjustor::solve(SlidingWindow *map, bool use_inertial, size_t max_iter, const double &max_time) { 31 | Problem::Options problem_options; 32 | problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; 33 | problem_options.loss_function_ownership = DO_NOT_TAKE_OWNERSHIP; 34 | problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP; 35 | 36 | Problem problem(problem_options); 37 | 38 | for (size_t i = 0; i < map->frame_num(); ++i) { 39 | Frame *frame = map->get_frame(i); 40 | problem.AddParameterBlock(frame->pose.q.coeffs().data(), 4, solver->quaternion_parameterization.get()); 41 | problem.AddParameterBlock(frame->pose.p.data(), 3); 42 | if (frame->pose.flag(PF_FIXED)) { 43 | problem.SetParameterBlockConstant(frame->pose.q.coeffs().data()); 44 | problem.SetParameterBlockConstant(frame->pose.p.data()); 45 | } 46 | if (use_inertial) { 47 | problem.AddParameterBlock(frame->motion.v.data(), 3); 48 | problem.AddParameterBlock(frame->motion.bg.data(), 3); 49 | problem.AddParameterBlock(frame->motion.ba.data(), 3); 50 | } 51 | } 52 | 53 | std::unordered_set visited_tracks; 54 | for (size_t i = 0; i < map->frame_num(); ++i) { 55 | Frame *frame = map->get_frame(i); 56 | for (size_t j = 0; j < frame->keypoint_num(); ++j) { 57 | Track *track = frame->get_track(j); 58 | if (!track) continue; 59 | if (!track->landmark.flag(LF_VALID)) continue; 60 | if (visited_tracks.count(track) > 0) continue; 61 | visited_tracks.insert(track); 62 | problem.AddParameterBlock(track->landmark.x.data(), 3); 63 | } 64 | } 65 | 66 | for (size_t i = 0; i < map->frame_num(); ++i) { 67 | Frame *frame = map->get_frame(i); 68 | for (size_t j = 0; j < frame->keypoint_num(); ++j) { 69 | Track *track = frame->get_track(j); 70 | if (!track) continue; 71 | if (!track->landmark.flag(LF_VALID)) continue; 72 | problem.AddResidualBlock(frame->get_reprojection_factor(j)->get_cost_function(), 73 | solver->cauchy_loss.get(), 74 | frame->pose.q.coeffs().data(), 75 | frame->pose.p.data(), 76 | track->landmark.x.data()); 77 | } 78 | } 79 | 80 | if (use_inertial) { 81 | for (size_t j = 1; j < map->frame_num(); ++j) { 82 | Frame *frame_i = map->get_frame(j - 1); 83 | Frame *frame_j = map->get_frame(j); 84 | if (frame_j->preintegration.integrate(frame_j->image->t, frame_i->motion.bg, frame_i->motion.ba, true, true)) { 85 | problem.AddResidualBlock(frame_j->get_preintegration_factor()->get_cost_function(), 86 | nullptr, 87 | frame_i->pose.q.coeffs().data(), 88 | frame_i->pose.p.data(), 89 | frame_i->motion.v.data(), 90 | frame_i->motion.bg.data(), 91 | frame_i->motion.ba.data(), 92 | frame_j->pose.q.coeffs().data(), 93 | frame_j->pose.p.data(), 94 | frame_j->motion.v.data(), 95 | frame_j->motion.bg.data(), 96 | frame_j->motion.ba.data()); 97 | } 98 | } 99 | } 100 | 101 | Solver::Options solver_options; 102 | solver_options.linear_solver_type = SPARSE_SCHUR; 103 | solver_options.trust_region_strategy_type = DOGLEG; 104 | solver_options.use_explicit_schur_complement = true; 105 | solver_options.minimizer_progress_to_stdout = false; 106 | solver_options.logging_type = SILENT; 107 | solver_options.max_num_iterations = (int)max_iter; 108 | solver_options.max_solver_time_in_seconds = max_time; 109 | solver_options.num_threads = 1; 110 | solver_options.num_linear_solver_threads = 1; 111 | 112 | Solver::Summary solver_summary; 113 | ceres::Solve(solver_options, &problem, &solver_summary); 114 | 115 | for (size_t i = 0; i < map->track_num(); ++i) { 116 | Track *track = map->get_track(i); 117 | if (!track->landmark.flag(LF_VALID)) continue; 118 | const Vector3d &x = track->landmark.x; 119 | double quality = 0.0; 120 | double quality_num = 0.0; 121 | for (const auto &k : track->keypoint_map()) { 122 | Frame *frame = k.first; 123 | size_t keypoint_id = k.second; 124 | PoseState pose = frame->get_pose(frame->camera); 125 | Vector3d y = pose.q.conjugate() * (x - pose.p); 126 | if (y.z() <= 1.0e-3 || y.z() > 50) { 127 | track->landmark.flag(LF_VALID) = false; 128 | break; 129 | } 130 | quality += (apply_k(y.hnormalized(), frame->K) - apply_k(frame->get_keypoint(keypoint_id), frame->K)).norm(); 131 | quality_num += 1.0; 132 | } 133 | if (!track->landmark.flag(LF_VALID)) continue; 134 | track->landmark.quality = quality / std::max(quality_num, 1.0); 135 | } 136 | 137 | return solver_summary.IsSolutionUsable(); 138 | } 139 | 140 | struct LandmarkInfo { 141 | LandmarkInfo() { 142 | mat.setZero(); 143 | vec.setZero(); 144 | } 145 | Matrix3d mat; 146 | Vector3d vec; 147 | std::unordered_map> h; 148 | }; 149 | -------------------------------------------------------------------------------- /src/slamtools/ceres/lie_algebra_eigen_quaternion_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | struct LieAlgebraEigenQuaternionParamatrization : public ceres::LocalParameterization { 8 | virtual bool Plus(const double *q, const double *dq, double *q_plus_dq) const override { 9 | Eigen::Map result(q_plus_dq); 10 | result = (Eigen::Map(q) * expmap(Eigen::Map(dq))).normalized(); 11 | return true; 12 | } 13 | virtual bool ComputeJacobian(const double *, double *jacobian) const override { 14 | Eigen::Map> J(jacobian); 15 | J.setIdentity(); // the composited jacobian is computed in PreIntegrationError::Evaluate(), we simply forward it. 16 | return true; 17 | } 18 | virtual int GlobalSize() const override { 19 | return 4; 20 | } 21 | virtual int LocalSize() const override { 22 | return 3; 23 | } 24 | }; 25 | -------------------------------------------------------------------------------- /src/slamtools/ceres/preintegration_error_cost.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class PreIntegrationErrorCost : public Factor::FactorCostFunction, public ceres::SizedCostFunction<15, 4, 3, 3, 3, 3, 4, 3, 3, 3, 3> { 12 | public: 13 | PreIntegrationErrorCost(const Frame *frame_i, const Frame *frame_j) : 14 | frame_i(frame_i), frame_j(frame_j) { 15 | } 16 | 17 | bool Evaluate(const double *const *parameters, double *residuals, double **jacobians) const override { 18 | static const Eigen::Vector3d gravity = {0.0, 0.0, -GRAVITY_NOMINAL}; 19 | Eigen::Map q_center_i(parameters[0]); 20 | Eigen::Map p_center_i(parameters[1]); 21 | Eigen::Map v_i(parameters[2]); 22 | Eigen::Map bg_i(parameters[3]); 23 | Eigen::Map ba_i(parameters[4]); 24 | 25 | Eigen::Map q_center_j(parameters[5]); 26 | Eigen::Map p_center_j(parameters[6]); 27 | Eigen::Map v_j(parameters[7]); 28 | Eigen::Map bg_j(parameters[8]); 29 | Eigen::Map ba_j(parameters[9]); 30 | 31 | const PreIntegrator &pre = frame_j->preintegration; 32 | const ExtrinsicParams &imu_i = frame_i->imu; 33 | const ExtrinsicParams &imu_j = frame_j->imu; 34 | const Eigen::Vector3d &ba_i_0 = frame_i->motion.ba; 35 | const Eigen::Vector3d &bg_i_0 = frame_i->motion.bg; 36 | 37 | const Eigen::Quaterniond q_i = q_center_i * imu_i.q_cs; 38 | const Eigen::Vector3d p_i = p_center_i + q_center_i * imu_i.p_cs; 39 | const Eigen::Quaterniond q_j = q_center_j * imu_j.q_cs; 40 | const Eigen::Vector3d p_j = p_center_j + q_center_j * imu_j.p_cs; 41 | 42 | const double &dt = pre.delta.t; 43 | const Eigen::Quaterniond &dq = pre.delta.q; 44 | const Eigen::Vector3d &dp = pre.delta.p; 45 | const Eigen::Vector3d &dv = pre.delta.v; 46 | const Eigen::Vector3d dbg = bg_i - bg_i_0; 47 | const Eigen::Vector3d dba = ba_i - ba_i_0; 48 | 49 | const Eigen::Matrix3d &dq_dbg = pre.jacobian.dq_dbg; 50 | const Eigen::Matrix3d &dp_dbg = pre.jacobian.dp_dbg; 51 | const Eigen::Matrix3d &dp_dba = pre.jacobian.dp_dba; 52 | const Eigen::Matrix3d &dv_dbg = pre.jacobian.dv_dbg; 53 | const Eigen::Matrix3d &dv_dba = pre.jacobian.dv_dba; 54 | 55 | Eigen::Map> r(residuals); 56 | r.segment<3>(ES_Q) = logmap((dq * expmap(dq_dbg * dbg)).conjugate() * q_i.conjugate() * q_j); 57 | r.segment<3>(ES_P) = q_i.conjugate() * (p_j - p_i - dt * v_i - 0.5 * dt * dt * gravity) - (dp + dp_dbg * dbg + dp_dba * dba); 58 | r.segment<3>(ES_V) = q_i.conjugate() * (v_j - v_i - dt * gravity) - (dv + dv_dbg * dbg + dv_dba * dba); 59 | r.segment<3>(ES_BG) = bg_j - bg_i; 60 | r.segment<3>(ES_BA) = ba_j - ba_i; 61 | 62 | if (jacobians) { 63 | if (jacobians[0]) { 64 | Eigen::Map> dr_dq_i(jacobians[0]); 65 | dr_dq_i.setZero(); 66 | dr_dq_i.block<3, 3>(ES_Q, 0) = -right_jacobian(r.segment<3>(ES_Q)).inverse() * q_j.conjugate().matrix() * q_center_i.matrix(); 67 | dr_dq_i.block<3, 3>(ES_P, 0) = imu_i.q_cs.conjugate().matrix() * hat(q_center_i.conjugate() * (p_j - p_center_i - dt * v_i - 0.5 * dt * dt * gravity)); 68 | dr_dq_i.block<3, 3>(ES_V, 0) = imu_i.q_cs.conjugate().matrix() * hat(q_center_i.conjugate() * (v_j - v_i - dt * gravity)); 69 | dr_dq_i = pre.delta.sqrt_inv_cov * dr_dq_i; 70 | } 71 | if (jacobians[1]) { 72 | Eigen::Map> dr_dp_i(jacobians[1]); 73 | dr_dp_i.setZero(); 74 | dr_dp_i.block<3, 3>(ES_P, 0) = -q_i.conjugate().matrix(); 75 | dr_dp_i = pre.delta.sqrt_inv_cov * dr_dp_i; 76 | } 77 | if (jacobians[2]) { 78 | Eigen::Map> dr_dv_i(jacobians[2]); 79 | dr_dv_i.setZero(); 80 | dr_dv_i.block<3, 3>(ES_P, 0) = -dt * q_i.conjugate().matrix(); 81 | dr_dv_i.block<3, 3>(ES_V, 0) = -q_i.conjugate().matrix(); 82 | dr_dv_i = pre.delta.sqrt_inv_cov * dr_dv_i; 83 | } 84 | if (jacobians[3]) { 85 | Eigen::Map> dr_dbg_i(jacobians[3]); 86 | dr_dbg_i.setZero(); 87 | dr_dbg_i.block<3, 3>(ES_Q, 0) = -right_jacobian(r.segment<3>(ES_Q)).inverse() * expmap(r.segment<3>(ES_Q)).conjugate().matrix() * right_jacobian(dq_dbg * dbg) * dq_dbg; 88 | dr_dbg_i.block<3, 3>(ES_P, 0) = -dp_dbg; 89 | dr_dbg_i.block<3, 3>(ES_V, 0) = -dv_dbg; 90 | dr_dbg_i.block<3, 3>(ES_BG, 0) = -Eigen::Matrix3d::Identity(); 91 | dr_dbg_i = pre.delta.sqrt_inv_cov * dr_dbg_i; 92 | } 93 | if (jacobians[4]) { 94 | Eigen::Map> dr_dba_i(jacobians[4]); 95 | dr_dba_i.setZero(); 96 | dr_dba_i.block<3, 3>(ES_P, 0) = -dp_dba; 97 | dr_dba_i.block<3, 3>(ES_V, 0) = -dv_dba; 98 | dr_dba_i.block<3, 3>(ES_BA, 0) = -Eigen::Matrix3d::Identity(); 99 | dr_dba_i = pre.delta.sqrt_inv_cov * dr_dba_i; 100 | } 101 | if (jacobians[5]) { 102 | Eigen::Map> dr_dq_j(jacobians[5]); 103 | dr_dq_j.setZero(); 104 | dr_dq_j.block<3, 3>(ES_Q, 0) = right_jacobian(r.segment<3>(ES_Q)).inverse() * imu_j.q_cs.conjugate().matrix(); 105 | dr_dq_j.block<3, 3>(ES_P, 0) = -q_i.conjugate().matrix() * q_center_j.matrix() * hat(imu_j.p_cs); 106 | dr_dq_j = pre.delta.sqrt_inv_cov * dr_dq_j; 107 | } 108 | if (jacobians[6]) { 109 | Eigen::Map> dr_dp_j(jacobians[6]); 110 | dr_dp_j.setZero(); 111 | dr_dp_j.block<3, 3>(ES_P, 0) = q_i.conjugate().matrix(); 112 | dr_dp_j = pre.delta.sqrt_inv_cov * dr_dp_j; 113 | } 114 | if (jacobians[7]) { 115 | Eigen::Map> dr_dv_j(jacobians[7]); 116 | dr_dv_j.setZero(); 117 | dr_dv_j.block<3, 3>(ES_V, 0) = q_i.conjugate().matrix(); 118 | dr_dv_j = pre.delta.sqrt_inv_cov * dr_dv_j; 119 | } 120 | if (jacobians[8]) { 121 | Eigen::Map> dr_dbg_j(jacobians[8]); 122 | dr_dbg_j.setZero(); 123 | dr_dbg_j.block<3, 3>(ES_BG, 0).setIdentity(); 124 | dr_dbg_j = pre.delta.sqrt_inv_cov * dr_dbg_j; 125 | } 126 | if (jacobians[9]) { 127 | Eigen::Map> dr_dba_j(jacobians[9]); 128 | dr_dba_j.setZero(); 129 | dr_dba_j.block<3, 3>(ES_BA, 0).setIdentity(); 130 | dr_dba_j = pre.delta.sqrt_inv_cov * dr_dba_j; 131 | } 132 | } 133 | 134 | r = pre.delta.sqrt_inv_cov * r; 135 | 136 | return true; 137 | } 138 | 139 | private: 140 | const Frame *frame_i; 141 | const Frame *frame_j; 142 | }; 143 | 144 | class PreIntegrationPriorCost : public ceres::SizedCostFunction<15, 4, 3, 3, 3, 3> { 145 | public: 146 | PreIntegrationPriorCost(const Frame *frame_i, const Frame *frame_j) : 147 | error(frame_i, frame_j), frame_i(frame_i) { 148 | } 149 | 150 | bool Evaluate(const double *const *parameters, double *residuals, double **jacobians) const override { 151 | std::array params = { 152 | frame_i->pose.q.coeffs().data(), 153 | frame_i->pose.p.data(), 154 | frame_i->motion.v.data(), 155 | frame_i->motion.bg.data(), 156 | frame_i->motion.ba.data(), 157 | parameters[0], 158 | parameters[1], 159 | parameters[2], 160 | parameters[3], 161 | parameters[4]}; 162 | if (jacobians) { 163 | std::array jacobs = { 164 | nullptr, 165 | nullptr, 166 | nullptr, 167 | nullptr, 168 | nullptr, 169 | jacobians[0], 170 | jacobians[1], 171 | jacobians[2], 172 | jacobians[3], 173 | jacobians[4]}; 174 | return error.Evaluate(params.data(), residuals, jacobs.data()); 175 | } else { 176 | return error.Evaluate(params.data(), residuals, nullptr); 177 | } 178 | } 179 | 180 | private: 181 | PreIntegrationErrorCost error; 182 | const Frame *frame_i; 183 | }; 184 | -------------------------------------------------------------------------------- /src/slamtools/ceres/reprojection_error_cost.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class ReprojectionErrorCost : public Factor::FactorCostFunction, public ceres::SizedCostFunction<2, 4, 3, 3> { 12 | public: 13 | ReprojectionErrorCost(const Frame *frame, size_t keypoint_id) : 14 | frame(frame), keypoint_id(keypoint_id) { 15 | } 16 | 17 | bool Evaluate(const double *const *parameters, double *residuals, double **jacobians) const override { 18 | Eigen::Map q_center(parameters[0]); 19 | Eigen::Map p_center(parameters[1]); 20 | Eigen::Map x(parameters[2]); 21 | 22 | const Eigen::Vector2d &z = frame->get_keypoint(keypoint_id); 23 | const ExtrinsicParams &camera = frame->camera; 24 | const Eigen::Matrix2d &sqrt_inv_cov = frame->sqrt_inv_cov; 25 | 26 | //Eigen::Vector3d y = camera.q_cs.conjugate() * q_center.conjugate() * (x - p_center) - camera.q_cs.conjugate() * camera.p_cs; 27 | Eigen::Vector3d y_center = q_center.conjugate() * (x - p_center); 28 | Eigen::Vector3d y = camera.q_cs.conjugate() * y_center - camera.q_cs.conjugate() * camera.p_cs; 29 | Eigen::Map r(residuals); 30 | r = y.hnormalized() - z; 31 | 32 | if (jacobians) { 33 | Eigen::Matrix dr_dy; 34 | dr_dy << 1.0 / y.z(), 0.0, -y.x() / (y.z() * y.z()), 35 | 0.0, 1.0 / y.z(), -y.y() / (y.z() * y.z()); 36 | if (jacobians[0]) { 37 | Eigen::Map> dr_dq(jacobians[0]); 38 | dr_dq.block<2, 3>(0, 0) = dr_dy * camera.q_cs.conjugate().matrix() * hat(y_center); 39 | dr_dq.col(3).setZero(); 40 | dr_dq = sqrt_inv_cov * dr_dq; 41 | } 42 | if (jacobians[1]) { 43 | Eigen::Map> dr_dp(jacobians[1]); 44 | dr_dp = -dr_dy * (q_center * camera.q_cs).conjugate().matrix(); 45 | dr_dp = sqrt_inv_cov * dr_dp; 46 | } 47 | if (jacobians[2]) { 48 | Eigen::Map> dr_dx(jacobians[2]); 49 | dr_dx = dr_dy * (q_center * camera.q_cs).conjugate().matrix(); 50 | dr_dx = sqrt_inv_cov * dr_dx; 51 | } 52 | } 53 | 54 | r = sqrt_inv_cov * r; 55 | 56 | return true; 57 | }; 58 | 59 | private: 60 | const Frame *frame; 61 | const size_t keypoint_id; 62 | }; 63 | 64 | class PoseOnlyReprojectionErrorCost : public ceres::SizedCostFunction<2, 4, 3> { 65 | public: 66 | PoseOnlyReprojectionErrorCost(const Frame *frame, size_t keypoint_id) : 67 | error(frame, keypoint_id), x(frame->get_track(keypoint_id)->landmark.x) { 68 | } 69 | 70 | bool Evaluate(const double *const *parameters, double *residuals, double **jacobians) const override { 71 | std::array params = { 72 | parameters[0], 73 | parameters[1], 74 | x.data()}; 75 | if (jacobians) { 76 | std::array jacobs = { 77 | jacobians[0], 78 | jacobians[1], 79 | nullptr}; 80 | return error.Evaluate(params.data(), residuals, jacobs.data()); 81 | } else { 82 | return error.Evaluate(params.data(), residuals, nullptr); 83 | } 84 | } 85 | 86 | private: 87 | ReprojectionErrorCost error; 88 | const Eigen::Vector3d &x; 89 | }; 90 | -------------------------------------------------------------------------------- /src/slamtools/configurator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace Eigen; 4 | 5 | std::shared_ptr Configurator::default_config() { 6 | static std::shared_ptr s_default_config = std::make_shared(); 7 | return s_default_config; 8 | } 9 | 10 | Configurator::~Configurator() = default; 11 | 12 | Matrix3d Configurator::camera_intrinsic() const { 13 | return (Matrix3d() << 549.0, 0.0, 320.0, 0.0, 549.0, 240.0, 0.0, 0.0, 1.0).finished(); 14 | } 15 | 16 | double Configurator::keypoint_pixel_error() const { 17 | return 0.7; 18 | } 19 | 20 | Quaterniond Configurator::camera_to_center_rotation() const { 21 | Quaterniond q; 22 | q = (Matrix3d() << -Vector3d::UnitY(), -Vector3d::UnitX(), -Vector3d::UnitZ()).finished(); 23 | return q; 24 | } 25 | 26 | Vector3d Configurator::camera_to_center_translation() const { 27 | return {0.0, 0.065, 0.0}; 28 | } 29 | 30 | Quaterniond Configurator::imu_to_center_rotation() const { 31 | return Quaterniond::Identity(); 32 | } 33 | 34 | Vector3d Configurator::imu_to_center_translation() const { 35 | return Vector3d::Zero(); 36 | } 37 | 38 | Matrix3d Configurator::imu_gyro_white_noise() const { 39 | return 4e-4 * Matrix3d::Identity(); 40 | } 41 | 42 | Matrix3d Configurator::imu_accel_white_noise() const { 43 | return 2.5e-3 * Matrix3d::Identity(); 44 | } 45 | 46 | Matrix3d Configurator::imu_gyro_random_walk() const { 47 | return 1.6e-9 * Matrix3d::Identity(); 48 | } 49 | 50 | Matrix3d Configurator::imu_accel_random_walk() const { 51 | return 4e-6 * Matrix3d::Identity(); 52 | } 53 | 54 | int Configurator::random() const { 55 | return 648; // <-- should be a random seed. 56 | } 57 | 58 | size_t Configurator::max_keypoint_detection() const { 59 | return 150; 60 | } 61 | 62 | size_t Configurator::max_init_raw_frames() const { 63 | return (init_map_frames() - 1) * 3 + 1; 64 | } 65 | 66 | size_t Configurator::min_init_raw_frames() const { 67 | return (init_map_frames() - 1) * 2 + 1; 68 | } 69 | 70 | size_t Configurator::min_raw_matches() const { 71 | return 50; 72 | } 73 | 74 | double Configurator::min_raw_parallax() const { 75 | return 10; 76 | } 77 | 78 | size_t Configurator::min_raw_triangulation() const { 79 | return 20; 80 | } 81 | 82 | size_t Configurator::init_map_frames() const { 83 | return 8; 84 | } 85 | 86 | size_t Configurator::min_init_map_landmarks() const { 87 | return 30; 88 | } 89 | 90 | bool Configurator::init_refine_imu() const { 91 | return true; 92 | } 93 | 94 | size_t Configurator::solver_iteration_limit() const { 95 | return 10; 96 | } 97 | 98 | double Configurator::solver_time_limit() const { 99 | return 1.0e6; 100 | } 101 | 102 | size_t Configurator::tracking_window_size() const { 103 | return init_map_frames(); 104 | } 105 | 106 | bool Configurator::predict_keypoints() const { 107 | return true; 108 | } 109 | -------------------------------------------------------------------------------- /src/slamtools/essential.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace Eigen; 4 | 5 | class Polynomial { 6 | public: 7 | // clang-format off 8 | enum GRevLexMonomials { 9 | XXX = 0, XXY = 1, XYY = 2, YYY = 3, XXZ = 4, XYZ = 5, YYZ = 6, XZZ = 7, YZZ = 8, ZZZ = 9, 10 | XX = 10, XY = 11, YY = 12, XZ = 13, YZ = 14, ZZ = 15, X = 16, Y = 17, Z = 18, I = 19 11 | }; 12 | // clang-format on 13 | 14 | Matrix v; 15 | 16 | Polynomial(const Matrix &coeffcients) : 17 | v(coeffcients) { 18 | } 19 | 20 | public: 21 | Polynomial() : 22 | Polynomial(Matrix::Zero()) { 23 | } 24 | 25 | Polynomial(double w) { 26 | v.setZero(); 27 | v[I] = w; 28 | } 29 | 30 | void set_xyzw(double x, double y, double z, double w) { 31 | v.setZero(); 32 | v[X] = x; 33 | v[Y] = y; 34 | v[Z] = z; 35 | v[I] = w; 36 | } 37 | 38 | Polynomial operator-() const { 39 | return Polynomial(-v); 40 | } 41 | 42 | Polynomial operator+(const Polynomial &b) const { 43 | return Polynomial(v + b.v); 44 | } 45 | 46 | Polynomial operator-(const Polynomial &b) const { 47 | return Polynomial(v - b.v); 48 | } 49 | 50 | Polynomial operator*(const Polynomial &b) const { 51 | Polynomial r; 52 | 53 | r.v[I] = v[I] * b.v[I]; 54 | 55 | r.v[Z] = v[I] * b.v[Z] + v[Z] * b.v[I]; 56 | r.v[Y] = v[I] * b.v[Y] + v[Y] * b.v[I]; 57 | r.v[X] = v[I] * b.v[X] + v[X] * b.v[I]; 58 | 59 | r.v[ZZ] = v[I] * b.v[ZZ] + v[Z] * b.v[Z] + v[ZZ] * b.v[I]; 60 | r.v[YZ] = v[I] * b.v[YZ] + v[Z] * b.v[Y] + v[Y] * b.v[Z] + v[YZ] * b.v[I]; 61 | r.v[XZ] = v[I] * b.v[XZ] + v[Z] * b.v[X] + v[X] * b.v[Z] + v[XZ] * b.v[I]; 62 | r.v[YY] = v[I] * b.v[YY] + v[Y] * b.v[Y] + v[YY] * b.v[I]; 63 | r.v[XY] = v[I] * b.v[XY] + v[Y] * b.v[X] + v[X] * b.v[Y] + v[XY] * b.v[I]; 64 | r.v[XX] = v[I] * b.v[XX] + v[X] * b.v[X] + v[XX] * b.v[I]; 65 | 66 | r.v[ZZZ] = v[I] * b.v[ZZZ] + v[Z] * b.v[ZZ] + v[ZZ] * b.v[Z] + v[ZZZ] * b.v[I]; 67 | r.v[YZZ] = v[I] * b.v[YZZ] + v[Z] * b.v[YZ] + v[Y] * b.v[ZZ] + v[ZZ] * b.v[Y] + v[YZ] * b.v[Z] + v[YZZ] * b.v[I]; 68 | r.v[XZZ] = v[I] * b.v[XZZ] + v[Z] * b.v[XZ] + v[X] * b.v[ZZ] + v[ZZ] * b.v[X] + v[XZ] * b.v[Z] + v[XZZ] * b.v[I]; 69 | r.v[YYZ] = v[I] * b.v[YYZ] + v[Z] * b.v[YY] + v[Y] * b.v[YZ] + v[YZ] * b.v[Y] + v[YY] * b.v[Z] + v[YYZ] * b.v[I]; 70 | r.v[XYZ] = v[I] * b.v[XYZ] + v[Z] * b.v[XY] + v[Y] * b.v[XZ] + v[X] * b.v[YZ] + v[YZ] * b.v[X] + v[XZ] * b.v[Y] + v[XY] * b.v[Z] + v[XYZ] * b.v[I]; 71 | r.v[XXZ] = v[I] * b.v[XXZ] + v[Z] * b.v[XX] + v[X] * b.v[XZ] + v[XZ] * b.v[X] + v[XX] * b.v[Z] + v[XXZ] * b.v[I]; 72 | r.v[YYY] = v[I] * b.v[YYY] + v[Y] * b.v[YY] + v[YY] * b.v[Y] + v[YYY] * b.v[I]; 73 | r.v[XYY] = v[I] * b.v[XYY] + v[Y] * b.v[XY] + v[X] * b.v[YY] + v[YY] * b.v[X] + v[XY] * b.v[Y] + v[XYY] * b.v[I]; 74 | r.v[XXY] = v[I] * b.v[XXY] + v[Y] * b.v[XX] + v[X] * b.v[XY] + v[XY] * b.v[X] + v[XX] * b.v[Y] + v[XXY] * b.v[I]; 75 | r.v[XXX] = v[I] * b.v[XXX] + v[X] * b.v[XX] + v[XX] * b.v[X] + v[XXX] * b.v[I]; 76 | 77 | return r; 78 | } 79 | 80 | const Matrix &coeffcients() const { 81 | return v; 82 | } 83 | }; 84 | 85 | Polynomial operator*(const double &scale, const Polynomial &poly) { 86 | return Polynomial(scale * poly.coeffcients()); 87 | } 88 | 89 | inline Matrix3d to_matrix(const Matrix &vec) { 90 | return (Matrix3d() << vec.segment<3>(0), vec.segment<3>(3), vec.segment<3>(6)).finished(); 91 | } 92 | 93 | inline Matrix generate_nullspace_basis(const std::array &points1, const std::array &points2) { 94 | Matrix A; 95 | for (size_t i = 0; i < 5; ++i) { 96 | Matrix3d h = Vector3d(points1[i].homogeneous()) * points2[i].homogeneous().transpose(); 97 | for (size_t j = 0; j < 3; ++j) { 98 | A.block<1, 3>(i, j * 3) = h.row(j); 99 | } 100 | } 101 | return A.jacobiSvd(ComputeFullV).matrixV().block<9, 4>(0, 5); 102 | } 103 | 104 | inline Matrix generate_polynomials(const Matrix &basis) { 105 | typedef Matrix matrix_poly; 106 | Matrix3d Ex = to_matrix(basis.col(0)); 107 | Matrix3d Ey = to_matrix(basis.col(1)); 108 | Matrix3d Ez = to_matrix(basis.col(2)); 109 | Matrix3d Ew = to_matrix(basis.col(3)); 110 | 111 | matrix_poly Epoly; 112 | for (size_t i = 0; i < 3; ++i) { 113 | for (size_t j = 0; j < 3; ++j) { 114 | Epoly(i, j).set_xyzw(Ex(i, j), Ey(i, j), Ez(i, j), Ew(i, j)); 115 | } 116 | } 117 | 118 | Matrix polynomials; 119 | 120 | matrix_poly EEt = Epoly * Epoly.transpose(); 121 | matrix_poly singular_value_constraints = (EEt * Epoly) - (0.5 * EEt.trace()) * Epoly; 122 | for (size_t i = 0; i < 3; ++i) { 123 | for (size_t j = 0; j < 3; ++j) { 124 | polynomials.row(i * 3 + j) = singular_value_constraints(i, j).coeffcients(); 125 | } 126 | } 127 | 128 | Polynomial detE = Epoly.determinant(); 129 | polynomials.row(9) = detE.coeffcients(); 130 | 131 | return polynomials; 132 | } 133 | 134 | inline Matrix generate_action_matrix(Matrix &polynomials) { 135 | std::array perm; 136 | for (size_t i = 0; i < 10; ++i) { 137 | perm[i] = i; 138 | } 139 | for (size_t i = 0; i < 10; ++i) { 140 | for (size_t j = i + 1; j < 10; ++j) { 141 | if (abs(polynomials(perm[i], i)) < abs(polynomials(perm[j], i))) { 142 | std::swap(perm[i], perm[j]); 143 | } 144 | } 145 | if (polynomials(perm[i], i) == 0) continue; 146 | polynomials.row(perm[i]) /= polynomials(perm[i], i); 147 | for (size_t j = i + 1; j < 10; ++j) { 148 | polynomials.row(perm[j]) -= polynomials.row(perm[i]) * polynomials(perm[j], i); 149 | } 150 | } 151 | for (size_t i = 9; i > 0; --i) { 152 | for (size_t j = 0; j < i; ++j) { 153 | polynomials.row(perm[j]) -= polynomials.row(perm[i]) * polynomials(perm[j], i); 154 | } 155 | } 156 | 157 | Matrix action; 158 | action.row(0) = -polynomials.block<1, 10>(perm[Polynomial::XXX], Polynomial::XX); 159 | action.row(1) = -polynomials.block<1, 10>(perm[Polynomial::XXY], Polynomial::XX); 160 | action.row(2) = -polynomials.block<1, 10>(perm[Polynomial::XYY], Polynomial::XX); 161 | action.row(3) = -polynomials.block<1, 10>(perm[Polynomial::XXZ], Polynomial::XX); 162 | action.row(4) = -polynomials.block<1, 10>(perm[Polynomial::XYZ], Polynomial::XX); 163 | action.row(5) = -polynomials.block<1, 10>(perm[Polynomial::XZZ], Polynomial::XX); 164 | action.row(6) = Matrix::Unit(Polynomial::XX - Polynomial::XX).transpose(); 165 | action.row(7) = Matrix::Unit(Polynomial::XY - Polynomial::XX).transpose(); 166 | action.row(8) = Matrix::Unit(Polynomial::XZ - Polynomial::XX).transpose(); 167 | action.row(9) = Matrix::Unit(Polynomial::X - Polynomial::XX).transpose(); 168 | 169 | return action; 170 | } 171 | 172 | inline std::vector solve_grobner_system(const Matrix &action) { 173 | EigenSolver> eigen(action, true); 174 | Matrix, 10, 1> xs = eigen.eigenvalues(); 175 | 176 | std::vector results; 177 | for (size_t i = 0; i < 10; ++i) { 178 | if (abs(xs[i].imag()) < 1.0e-10) { 179 | Matrix h = eigen.eigenvectors().col(i).real(); 180 | double xw = h(Polynomial::X - Polynomial::XX); 181 | double yw = h(Polynomial::Y - Polynomial::XX); 182 | double zw = h(Polynomial::Z - Polynomial::XX); 183 | double w = h(Polynomial::I - Polynomial::XX); 184 | results.emplace_back(xw / w, yw / w, zw / w); 185 | } 186 | } 187 | return results; 188 | } 189 | 190 | void decompose_essential(const Matrix3d &E, Matrix3d &R1, Matrix3d &R2, Vector3d &T) { 191 | #ifdef ESSENTIAL_DECOMPOSE_HORN 192 | Matrix3d EET = E * E.transpose(); 193 | double halfTrace = 0.5 * EET.trace(); 194 | Vector3d b; 195 | 196 | Vector3d e0e1 = E.col(0).cross(E.col(1)); 197 | Vector3d e1e2 = E.col(1).cross(E.col(2)); 198 | Vector3d e2e0 = E.col(2).cross(E.col(0)); 199 | 200 | # if ESSENTIAL_DECOMPOSE_HORN == 1 201 | if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) { 202 | b = e0e1.normalized() * sqrt(halfTrace); 203 | } else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) { 204 | b = e1e2.normalized() * sqrt(halfTrace); 205 | } else { 206 | b = e2e0.normalized() * sqrt(halfTrace); 207 | } 208 | # else 209 | Matrix3d bbT = halfTrace * Matrix3d::Identity() - EET; 210 | Vector3d bbT_diag = bbT.diagonal(); 211 | if (bbT_diag(0) > bbt_diag(1) && bbT_diag(0) > bbT_diag(2)) { 212 | b = bbT.row(0) / sqrt(bbT_diag(0)); 213 | } else if (bbT_diag(1) > bbT_diag(0) && bbT_diag(1) > bbT_diag(2)) { 214 | b = bbT.row(1) / sqrt(bbT_diag(1)); 215 | } else { 216 | b = bbT.row(2) / sqrt(bbT_diag(2)); 217 | } 218 | # endif 219 | 220 | Matrix3d cofactorsT; 221 | cofactorsT.col(0) = e1e2; 222 | cofactorsT.col(1) = e2e0; 223 | cofactorsT.col(2) = e0e1; 224 | 225 | Matrix3d skew_b; 226 | skew_b << 0, -b.z(), b.y(), 227 | b.z(), 0, -b.x(), 228 | -b.y(), b.x(), 0; 229 | Matrix3d bxE = skew_b * E; 230 | 231 | double bTb = b.dot(b); 232 | R1 = (cofactorsT - bxE) / bTb; 233 | R2 = (cofactorsT + bxE) / bTb; 234 | T = b; 235 | #else 236 | JacobiSVD svd(E, ComputeFullU | ComputeFullV); 237 | Matrix3d U = svd.matrixU(); 238 | Matrix3d VT = svd.matrixV().transpose(); 239 | if (U.determinant() < 0) { 240 | U = -U; 241 | } 242 | if (VT.determinant() < 0) { 243 | VT = -VT; 244 | } 245 | Matrix3d W; 246 | W << 0, 1, 0, 247 | -1, 0, 0, 248 | 0, 0, 1; 249 | R1 = U * W * VT; 250 | R2 = U * W.transpose() * VT; 251 | T = U.col(2); 252 | #endif 253 | } 254 | 255 | std::vector solve_essential_5pt(const std::array &points1, const std::array &points2) { 256 | Matrix basis = generate_nullspace_basis(points1, points2); 257 | Matrix polynomials = generate_polynomials(basis); 258 | Matrix action = generate_action_matrix(polynomials); 259 | std::vector solutions = solve_grobner_system(action); 260 | std::vector results(solutions.size()); 261 | for (size_t i = 0; i < solutions.size(); ++i) { 262 | results[i] = to_matrix(basis * solutions[i].homogeneous()); 263 | } 264 | return results; 265 | } 266 | -------------------------------------------------------------------------------- /src/slamtools/factor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | std::unique_ptr Factor::create_reprojection_error(Frame *frame, size_t keypoint_id) { 7 | return std::make_unique( 8 | std::make_unique(frame, keypoint_id), 9 | factor_construct_t()); 10 | } 11 | 12 | std::unique_ptr Factor::create_preintegration_error(Frame *frame_i, Frame *frame_j) { 13 | return std::make_unique( 14 | std::make_unique(frame_i, frame_j), 15 | factor_construct_t()); 16 | } 17 | 18 | Factor::Factor(std::unique_ptr cost_function, const factor_construct_t &) : 19 | cost_function(std::move(cost_function)) { 20 | } 21 | 22 | Factor::~Factor() = default; 23 | -------------------------------------------------------------------------------- /src/slamtools/frame.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace Eigen; 9 | 10 | create_if_empty_t create_if_empty{}; 11 | 12 | Frame::Frame() : 13 | map(nullptr) { 14 | } 15 | 16 | Frame::~Frame() = default; 17 | 18 | std::unique_ptr Frame::clone() const { 19 | std::unique_ptr frame = std::make_unique(); 20 | frame->K = K; 21 | frame->sqrt_inv_cov = sqrt_inv_cov; 22 | frame->image = image; 23 | frame->pose = pose; 24 | frame->motion = motion; 25 | frame->camera = camera; 26 | frame->imu = imu; 27 | frame->preintegration = preintegration; 28 | frame->external_gravity = external_gravity; // maybe inappropriate? 29 | frame->keypoints = keypoints; 30 | frame->segments = segments; 31 | frame->tracks = std::vector(keypoints.size(), nullptr); 32 | frame->reprojection_factors = std::vector>(keypoints.size()); 33 | frame->map = nullptr; 34 | return frame; 35 | } 36 | 37 | Track *Frame::get_track(size_t keypoint_id, const create_if_empty_t &) { 38 | if (tracks[keypoint_id] == nullptr) { 39 | assert(("for get_track(..., create_if_empty) to work, frame->map cannot be nullptr") && (map != nullptr)); 40 | Track *track = map->create_track(); 41 | track->add_keypoint(this, keypoint_id); 42 | } 43 | return tracks[keypoint_id]; 44 | } 45 | 46 | void Frame::detect_keypoints(Configurator *config) { 47 | std::vector pkeypoints(keypoints.size()); 48 | for (size_t i = 0; i < keypoints.size(); ++i) { 49 | pkeypoints[i] = apply_k(keypoints[i], K); 50 | } 51 | image->detect_keypoints(pkeypoints, config->max_keypoint_detection()); 52 | size_t old_keypoint_num = keypoints.size(); 53 | keypoints.resize(pkeypoints.size()); 54 | tracks.resize(pkeypoints.size(), nullptr); 55 | reprojection_factors.resize(pkeypoints.size()); 56 | for (size_t i = old_keypoint_num; i < pkeypoints.size(); ++i) { 57 | keypoints[i] = remove_k(pkeypoints[i], K); 58 | } 59 | } 60 | 61 | void Frame::track_keypoints(Frame *next_frame, Configurator *config) { 62 | std::vector curr_keypoints(keypoints.size()); 63 | std::vector next_keypoints; 64 | 65 | for (size_t i = 0; i < keypoints.size(); ++i) { 66 | curr_keypoints[i] = apply_k(keypoints[i], K); 67 | } 68 | 69 | if (config->predict_keypoints()) { 70 | Quaterniond delta_key_q = (camera.q_cs.conjugate() * imu.q_cs * next_frame->preintegration.delta.q * next_frame->imu.q_cs.conjugate() * next_frame->camera.q_cs).conjugate(); 71 | next_keypoints.resize(curr_keypoints.size()); 72 | for (size_t i = 0; i < keypoints.size(); ++i) { 73 | next_keypoints[i] = apply_k((delta_key_q * keypoints[i].homogeneous()).hnormalized(), next_frame->K); 74 | } 75 | } 76 | 77 | std::vector status; 78 | image->track_keypoints(next_frame->image.get(), curr_keypoints, next_keypoints, status); 79 | 80 | for (size_t curr_keypoint_id = 0; curr_keypoint_id < curr_keypoints.size(); ++curr_keypoint_id) { 81 | if (status[curr_keypoint_id]) { 82 | size_t next_keypoint_id = next_frame->keypoints.size(); 83 | next_frame->keypoints.emplace_back(remove_k(next_keypoints[curr_keypoint_id], next_frame->K)); 84 | next_frame->tracks.emplace_back(nullptr); 85 | next_frame->reprojection_factors.emplace_back(nullptr); 86 | get_track(curr_keypoint_id, create_if_empty)->add_keypoint(next_frame, next_keypoint_id); 87 | } 88 | } 89 | } 90 | 91 | void Frame::detect_segments(size_t max_segments) { 92 | image->detect_segments(segments, max_segments); 93 | for (size_t i = 0; i < segments.size(); ++i) { 94 | std::get<0>(segments[i]) = remove_k(std::get<0>(segments[i]), K); 95 | std::get<1>(segments[i]) = remove_k(std::get<1>(segments[i]), K); 96 | } 97 | } 98 | 99 | PoseState Frame::get_pose(const ExtrinsicParams &sensor) const { 100 | PoseState result; 101 | result.q = pose.q * sensor.q_cs; 102 | result.p = pose.p + pose.q * sensor.p_cs; 103 | return result; 104 | } 105 | 106 | void Frame::set_pose(const ExtrinsicParams &sensor, const PoseState &pose) { 107 | this->pose.q = pose.q * sensor.q_cs.conjugate(); 108 | this->pose.p = pose.p - this->pose.q * sensor.p_cs; 109 | } 110 | -------------------------------------------------------------------------------- /src/slamtools/homography.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace Eigen; 4 | 5 | bool decompose_homography(const Matrix3d &H, Matrix3d &R1, Matrix3d &R2, Vector3d &T1, Vector3d &T2, Vector3d &n1, Vector3d &n2) { 6 | Matrix3d Hn = H / H.jacobiSvd().singularValues()(1); 7 | Matrix3d S = Hn.transpose() * Hn - Matrix3d::Identity(); 8 | 9 | bool is_pure_rotation = true; 10 | for (int i = 0; i < 3 && is_pure_rotation; ++i) { 11 | for (int j = 0; j < 3 && is_pure_rotation; ++j) { 12 | if (abs(S(i, j)) > 1e-3) { 13 | is_pure_rotation = false; 14 | } 15 | } 16 | } 17 | 18 | if (is_pure_rotation) { 19 | // Pure rotation 20 | JacobiSVD svd(H, ComputeFullU | ComputeFullV); 21 | R1 = svd.matrixU() * Matrix3d::Identity() * svd.matrixV().transpose(); 22 | if (R1.determinant() < 0) { 23 | R1 = -R1; 24 | } 25 | R2 = R1; 26 | //R1 = R2 = Hn; 27 | T1 = T2 = Vector3d::Zero(); 28 | n1 = n2 = Vector3d::Zero(); 29 | } else { 30 | double Ms00 = S(1, 2) * S(1, 2) - S(1, 1) * S(2, 2); 31 | double Ms11 = S(0, 2) * S(0, 2) - S(0, 0) * S(2, 2); 32 | double Ms22 = S(0, 1) * S(0, 1) - S(0, 0) * S(1, 1); 33 | 34 | double sqrtMs00 = sqrt(Ms00); 35 | double sqrtMs11 = sqrt(Ms11); 36 | double sqrtMs22 = sqrt(Ms22); 37 | 38 | double nu = 2.0 * sqrt(1 + S.trace() - Ms00 - Ms11 - Ms22); 39 | double tenormsq = 2 + S.trace() - nu; 40 | 41 | Vector3d tstar1, tstar2; 42 | 43 | if (S(0, 0) > S(1, 1) && S(0, 0) > S(2, 2)) { 44 | double epslMs12 = (((S(0, 1) * S(0, 2) - S(0, 0) * S(1, 2)) < 0) ? -1 : 1); 45 | n1 << S(0, 0), S(0, 1) + sqrtMs22, S(0, 2) + epslMs12 * sqrtMs11; 46 | n2 << S(0, 0), S(0, 1) - sqrtMs22, S(0, 2) - epslMs12 * sqrtMs11; 47 | tstar1 = n1.norm() * n2 / S(0, 0); 48 | tstar2 = n2.norm() * n1 / S(0, 0); 49 | } else if (S(1, 1) > S(0, 0) && S(1, 1) > S(2, 2)) { 50 | double epslMs02 = (((S(1, 1) * S(0, 2) - S(0, 1) * S(1, 2)) < 0) ? -1 : 1); 51 | n1 << S(0, 1) + sqrtMs22, S(1, 1), S(1, 2) - epslMs02 * sqrtMs00; 52 | n2 << S(0, 1) - sqrtMs22, S(1, 1), S(1, 2) + epslMs02 * sqrtMs00; 53 | tstar2 = n2.norm() * n1 / S(1, 1); 54 | tstar1 = n1.norm() * n2 / S(1, 1); 55 | } else { 56 | double epslMs01 = (((S(1, 2) * S(0, 2) - S(0, 1) * S(2, 2)) < 0) ? -1 : 1); 57 | n1 << S(0, 2) + epslMs01 * sqrtMs11, S(1, 2) + sqrtMs00, S(2, 2); 58 | n2 << S(0, 2) - epslMs01 * sqrtMs11, S(1, 2) - sqrtMs00, S(2, 2); 59 | tstar1 = n1.norm() * n2 / S(2, 2); 60 | tstar2 = n2.norm() * n1 / S(2, 2); 61 | } 62 | n1.normalize(); 63 | n2.normalize(); 64 | tstar1 -= tenormsq * n1; 65 | tstar2 -= tenormsq * n2; 66 | R1 = Hn * (Matrix3d::Identity() - (tstar1 / nu) * n1.transpose()); 67 | R2 = Hn * (Matrix3d::Identity() - (tstar2 / nu) * n2.transpose()); 68 | tstar1 *= 0.5; 69 | tstar2 *= 0.5; 70 | T1 = R1 * tstar1; 71 | T2 = R2 * tstar2; 72 | } 73 | return !is_pure_rotation; 74 | } 75 | 76 | inline Matrix3d to_matrix(const Matrix &vec) { 77 | return (Matrix3d() << vec.segment<3>(0), vec.segment<3>(3), vec.segment<3>(6)).finished(); 78 | } 79 | 80 | inline Matrix3d solve_homography_normalized(const std::array &pa, const std::array &pb) { 81 | Matrix A = Matrix::Zero(); 82 | 83 | for (size_t i = 0; i < 4; ++i) { 84 | const Vector2d &a = pa[i]; 85 | const Vector2d &b = pb[i]; 86 | A(i * 2, 1) = -a(0); 87 | A(i * 2, 2) = a(0) * b(1); 88 | A(i * 2, 4) = -a(1); 89 | A(i * 2, 5) = a(1) * b(1); 90 | A(i * 2, 7) = -1; 91 | A(i * 2, 8) = b(1); 92 | A(i * 2 + 1, 0) = a(0); 93 | A(i * 2 + 1, 2) = -a(0) * b(0); 94 | A(i * 2 + 1, 3) = a(1); 95 | A(i * 2 + 1, 5) = -a(1) * b(0); 96 | A(i * 2 + 1, 6) = 1; 97 | A(i * 2 + 1, 8) = -b(0); 98 | } 99 | 100 | Matrix h = A.jacobiSvd(ComputeFullV).matrixV().col(8); 101 | return to_matrix(h); 102 | } 103 | 104 | inline Matrix3d solve_homography_normalized(const std::vector &pa, const std::vector &pb) { 105 | Matrix A; 106 | A.resize(pa.size() * 2, 9); 107 | A.setZero(); 108 | 109 | for (size_t i = 0; i < pa.size(); ++i) { 110 | const Vector2d &a = pa[i]; 111 | const Vector2d &b = pb[i]; 112 | A(i * 2, 1) = -a(0); 113 | A(i * 2, 2) = a(0) * b(1); 114 | A(i * 2, 4) = -a(1); 115 | A(i * 2, 5) = a(1) * b(1); 116 | A(i * 2, 7) = -1; 117 | A(i * 2, 8) = b(1); 118 | A(i * 2 + 1, 0) = a(0); 119 | A(i * 2 + 1, 2) = -a(0) * b(0); 120 | A(i * 2 + 1, 3) = a(1); 121 | A(i * 2 + 1, 5) = -a(1) * b(0); 122 | A(i * 2 + 1, 6) = 1; 123 | A(i * 2 + 1, 8) = -b(0); 124 | } 125 | 126 | Matrix h = A.jacobiSvd(ComputeFullV).matrixV().col(8); 127 | return to_matrix(h); 128 | } 129 | 130 | Matrix3d solve_homography_4pt(const std::array &points1, const std::array &points2) { 131 | static const double sqrt2 = sqrt(2.0); 132 | 133 | Vector2d pa_mean = Vector2d::Zero(); 134 | Vector2d pb_mean = Vector2d::Zero(); 135 | for (size_t i = 0; i < 4; ++i) { 136 | pa_mean += points1[i]; 137 | pb_mean += points2[i]; 138 | } 139 | pa_mean /= 4; 140 | pb_mean /= 4; 141 | 142 | double sa = 0; 143 | double sb = 0; 144 | 145 | for (size_t i = 0; i < 4; ++i) { 146 | sa += (points1[i] - pa_mean).norm(); 147 | sb += (points2[i] - pb_mean).norm(); 148 | } 149 | 150 | sa = 1.0 / (sqrt2 * sa); 151 | sb = 1.0 / (sqrt2 * sb); 152 | 153 | std::array na; 154 | std::array nb; 155 | for (size_t i = 0; i < 4; ++i) { 156 | na[i] = (points1[i] - pa_mean) * sa; 157 | nb[i] = (points2[i] - pb_mean) * sb; 158 | } 159 | 160 | Matrix3d NH = solve_homography_normalized(na, nb); 161 | 162 | Matrix3d Na, Nb; 163 | Nb << 1 / sb, 0, pb_mean(0), 164 | 0, 1 / sb, pb_mean(1), 165 | 0, 0, 1; 166 | Na << sa, 0, -sa * pa_mean(0), 167 | 0, sa, -sa * pa_mean(1), 168 | 0, 0, 1; 169 | 170 | return Nb * NH * Na; 171 | } 172 | 173 | Matrix3d solve_homography(const std::vector &points1, const std::vector &points2) { 174 | static const double sqrt2 = sqrt(2.0); 175 | 176 | Vector2d pa_mean = Vector2d::Zero(); 177 | Vector2d pb_mean = Vector2d::Zero(); 178 | for (size_t i = 0; i < points1.size(); ++i) { 179 | pa_mean += points1[i]; 180 | pb_mean += points2[i]; 181 | } 182 | pa_mean /= 4; 183 | pb_mean /= 4; 184 | 185 | double sa = 0; 186 | double sb = 0; 187 | 188 | for (size_t i = 0; i < points1.size(); ++i) { 189 | sa += (points1[i] - pa_mean).norm(); 190 | sb += (points2[i] - pb_mean).norm(); 191 | } 192 | 193 | sa = 1.0 / (sqrt2 * sa); 194 | sb = 1.0 / (sqrt2 * sb); 195 | 196 | std::vector na(points1.size()); 197 | std::vector nb(points1.size()); 198 | for (size_t i = 0; i < points1.size(); ++i) { 199 | na[i] = (points1[i] - pa_mean) * sa; 200 | nb[i] = (points2[i] - pb_mean) * sb; 201 | } 202 | 203 | Matrix3d NH = solve_homography_normalized(na, nb); 204 | 205 | Matrix3d Na, Nb; 206 | Nb << 1 / sb, 0, pb_mean(0), 207 | 0, 1 / sb, pb_mean(1), 208 | 0, 0, 1; 209 | Na << sa, 0, -sa * pa_mean(0), 210 | 0, sa, -sa * pa_mean(1), 211 | 0, 0, 1; 212 | 213 | return Nb * NH * Na; 214 | } 215 | -------------------------------------------------------------------------------- /src/slamtools/initializer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace Eigen; 14 | 15 | Initializer::Initializer(std::shared_ptr config) : 16 | config(config) { 17 | raw = std::make_unique(); 18 | } 19 | 20 | Initializer::~Initializer() = default; 21 | 22 | void Initializer::append_frame(std::unique_ptr frame) { 23 | if (raw->frame_num() > 0) { 24 | Frame *last_frame = raw->get_frame(raw->frame_num() - 1); 25 | frame->preintegration.integrate(frame->image->t, last_frame->motion.bg, last_frame->motion.ba, true, false); 26 | last_frame->track_keypoints(frame.get(), config.get()); 27 | } 28 | frame->detect_keypoints(config.get()); 29 | 30 | raw->put_frame(std::move(frame)); 31 | while (raw->frame_num() > config->max_init_raw_frames()) { 32 | raw->erase_frame(0); 33 | } 34 | } 35 | 36 | 37 | std::unique_ptr Initializer::init_sfm() const { 38 | // [1] find a proper pair for initialization 39 | Frame *init_frame_i = nullptr; 40 | Frame *init_frame_j = raw->get_frame(raw->frame_num() - 1); 41 | size_t init_frame_i_id = nil(); 42 | std::vector init_points; 43 | std::vector> init_matches; 44 | std::vector init_point_status; 45 | Matrix3d init_R; 46 | Vector3d init_T; 47 | 48 | Frame *frame_j = init_frame_j; 49 | std::vector frame_i_keypoints; 50 | std::vector frame_j_keypoints; 51 | for (size_t frame_i_id = 0; frame_i_id + config->min_init_raw_frames() < raw->frame_num(); ++frame_i_id) { 52 | double total_parallax = 0; 53 | int common_track_num = 0; 54 | Frame *frame_i = raw->get_frame(frame_i_id); 55 | frame_i_keypoints.clear(); 56 | frame_j_keypoints.clear(); 57 | init_matches.clear(); 58 | for (size_t ki = 0; ki < frame_i->keypoint_num(); ++ki) { 59 | Track *track = frame_i->get_track(ki); 60 | if (!track) continue; 61 | size_t kj = track->get_keypoint_id(init_frame_j); 62 | if (kj == nil()) continue; 63 | frame_i_keypoints.push_back(frame_i->get_keypoint(ki)); 64 | frame_j_keypoints.push_back(frame_j->get_keypoint(kj)); 65 | init_matches.emplace_back(ki, kj); 66 | total_parallax += (apply_k(frame_i->get_keypoint(ki), frame_i->K) - apply_k(frame_j->get_keypoint(kj), frame_j->K)).norm(); 67 | common_track_num++; 68 | } 69 | 70 | if (common_track_num < (int)config->min_raw_matches()) continue; 71 | total_parallax /= std::max(common_track_num, 1); 72 | if (total_parallax < config->min_raw_parallax()) continue; 73 | 74 | std::vector Rs; 75 | std::vector Ts; 76 | 77 | Matrix3d RH1, RH2; 78 | Vector3d TH1, TH2, nH1, nH2; 79 | Matrix3d H = find_homography_matrix(frame_i_keypoints, frame_j_keypoints, 0.7 / frame_i->K(0, 0), 0.999, 1000, config->random()); 80 | if (!decompose_homography(H, RH1, RH2, TH1, TH2, nH1, nH2)) { 81 | continue; // is pure rotation 82 | } 83 | TH1 = TH1.normalized(); 84 | TH2 = TH2.normalized(); 85 | Rs.insert(Rs.end(), {RH1, RH1, RH2, RH2}); 86 | Ts.insert(Ts.end(), {TH1, -TH1, TH2, -TH2}); 87 | 88 | Matrix3d RE1, RE2; 89 | Vector3d TE; 90 | Matrix3d E = find_essential_matrix(frame_i_keypoints, frame_j_keypoints, 0.7 / frame_i->K(0, 0), 0.999, 1000, config->random()); 91 | decompose_essential(E, RE1, RE2, TE); 92 | TE = TE.normalized(); 93 | Rs.insert(Rs.end(), {RE1, RE1, RE2, RE2}); 94 | Ts.insert(Ts.end(), {TE, -TE, TE, -TE}); 95 | 96 | size_t triangulated_num = triangulate_from_rt_scored(frame_i_keypoints, frame_j_keypoints, Rs, Ts, config->min_raw_triangulation(), init_points, init_R, init_T, init_point_status); 97 | 98 | if (triangulated_num < config->min_raw_triangulation()) { 99 | continue; 100 | } 101 | 102 | init_frame_i = frame_i; 103 | init_frame_i_id = frame_i_id; 104 | break; 105 | } 106 | 107 | if (!init_frame_i) return nullptr; 108 | 109 | // [2] create sfm map 110 | 111 | // [2.1] enumerate keyframe ids 112 | std::vector init_keyframe_ids; 113 | size_t init_map_frames = config->init_map_frames(); 114 | double keyframe_id_gap = (double)(raw->frame_num() - 1 - init_frame_i_id) / (double)(init_map_frames - 1); 115 | for (size_t i = 0; i < init_map_frames; ++i) { 116 | init_keyframe_ids.push_back((size_t)round(init_frame_i_id + keyframe_id_gap * i)); 117 | } 118 | 119 | // [2.2] make a clone of submap using keyframe ids 120 | std::unique_ptr map = std::make_unique(); 121 | for (size_t i = 0; i < init_keyframe_ids.size(); ++i) { 122 | map->put_frame(raw->get_frame(init_keyframe_ids[i])->clone()); 123 | } 124 | for (size_t j = 1; j < init_keyframe_ids.size(); ++j) { 125 | Frame *old_frame_i = raw->get_frame(init_keyframe_ids[j - 1]); 126 | Frame *old_frame_j = raw->get_frame(init_keyframe_ids[j]); 127 | Frame *new_frame_i = map->get_frame(j - 1); 128 | Frame *new_frame_j = map->get_frame(j); 129 | for (size_t ki = 0; ki < old_frame_i->keypoint_num(); ++ki) { 130 | Track *track = old_frame_i->get_track(ki); 131 | if (track == nullptr) continue; 132 | size_t kj = track->get_keypoint_id(old_frame_j); 133 | if (kj == nil()) continue; 134 | new_frame_i->get_track(ki, create_if_empty)->add_keypoint(new_frame_j, kj); 135 | } 136 | new_frame_j->preintegration.data.clear(); 137 | for (size_t f = init_keyframe_ids[j - 1]; f < init_keyframe_ids[j]; ++f) { 138 | Frame *old_frame = raw->get_frame(f + 1); 139 | std::vector &old_data = old_frame->preintegration.data; 140 | std::vector &new_data = new_frame_j->preintegration.data; 141 | new_data.insert(new_data.end(), old_data.begin(), old_data.end()); 142 | } 143 | } 144 | 145 | Frame *new_init_frame_i = map->get_frame(0); 146 | Frame *new_init_frame_j = map->get_frame(map->frame_num() - 1); 147 | 148 | // [2.3] set init states 149 | PoseState pose; 150 | pose.q.setIdentity(); 151 | pose.p.setZero(); 152 | new_init_frame_i->set_pose(new_init_frame_i->camera, pose); 153 | pose.q = init_R.transpose(); 154 | pose.p = -(init_R.transpose() * init_T); 155 | new_init_frame_j->set_pose(new_init_frame_j->camera, pose); 156 | 157 | for (size_t k = 0; k < init_points.size(); ++k) { 158 | if (init_point_status[k] == 0) continue; 159 | Track *track = new_init_frame_i->get_track(init_matches[k].first); 160 | track->landmark.x = init_points[k]; 161 | track->landmark.flag(LF_VALID) = true; 162 | } 163 | 164 | // [2.4] solve other frames via pnp 165 | for (size_t j = 1; j + 1 < map->frame_num(); ++j) { 166 | Frame *frame_i = map->get_frame(j - 1); 167 | Frame *frame_j = map->get_frame(j); 168 | frame_j->set_pose(frame_j->camera, frame_i->get_pose(frame_i->camera)); 169 | visual_inertial_pnp(map.get(), frame_j, false); 170 | } 171 | 172 | // [2.5] triangulate more points 173 | for (size_t i = 0; i < map->track_num(); ++i) { 174 | Track *track = map->get_track(i); 175 | if (track->landmark.flag(LF_VALID)) continue; 176 | track->triangulate(); 177 | } 178 | 179 | // [3] sfm 180 | 181 | // [3.1] bundle adjustment 182 | map->get_frame(0)->pose.flag(PF_FIXED) = true; 183 | if (!BundleAdjustor().solve(map.get(), false, config->solver_iteration_limit() * 5, config->solver_time_limit())) { 184 | return nullptr; 185 | } 186 | 187 | // [3.2] cleanup invalid points 188 | map->prune_tracks([](const Track *track) { 189 | return !track->landmark.flag(LF_VALID) || track->landmark.quality > 1.0; 190 | }); 191 | 192 | return map; 193 | } 194 | -------------------------------------------------------------------------------- /src/slamtools/lie_algebra.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace Eigen; 4 | 5 | Matrix3d right_jacobian(const Vector3d &w) { 6 | static const double root2_eps = sqrt(std::numeric_limits::epsilon()); 7 | static const double root4_eps = sqrt(root2_eps); 8 | static const double twopi = 2.0 * 3.14159265358979323846; 9 | static const double qdrt720 = sqrt(sqrt(720.0)); 10 | static const double qdrt5040 = sqrt(sqrt(5040.0)); 11 | static const double sqrt24 = sqrt(24.0); 12 | static const double sqrt120 = sqrt(120.0); 13 | 14 | double angle = w.norm(); 15 | double cangle = cos(angle); 16 | double sangle = sin(angle); 17 | double angle2 = angle * angle; 18 | 19 | double cos_term; 20 | // compute (1-cos(x))/x^2, its taylor expansion around 0 is 1/2-x^2/24+x^4/720+o(x^6) 21 | if (angle > root4_eps * qdrt720) { 22 | cos_term = (1 - cangle) / angle2; 23 | } else { // use taylor expansion to avoid singularity 24 | cos_term = 0.5; 25 | if (angle > root2_eps * sqrt24) { // we have to include x^2 term 26 | cos_term -= angle2 / 24.0; 27 | } 28 | } 29 | 30 | double sin_term; 31 | // compute (x-sin(x))/x^3, its taylor expansion around 0 is 1/6-x^2/120+x^4/5040+o(x^6) 32 | if (angle > root4_eps * qdrt5040) { 33 | sin_term = (angle - sangle) / (angle * angle2); 34 | } else { 35 | sin_term = 1.0 / 6.0; 36 | if (angle > root2_eps * sqrt120) { // we have to include x^2 term 37 | sin_term -= angle2 / 120.0; 38 | } 39 | } 40 | 41 | Matrix3d hat_w = hat(w); 42 | return Matrix3d::Identity() - cos_term * hat_w + sin_term * hat_w * hat_w; 43 | } 44 | 45 | Matrix s2_tangential_basis(const Vector3d &x) { 46 | int d = 0; 47 | for (int i = 1; i < 3; ++i) { 48 | if (abs(x[i]) > abs(x[d])) d = i; 49 | } 50 | Vector3d b1 = x.cross(Vector3d::Unit((d + 1) % 3)).normalized(); 51 | Vector3d b2 = x.cross(b1).normalized(); 52 | return (Matrix() << b1, b2).finished(); 53 | } 54 | -------------------------------------------------------------------------------- /src/slamtools/pnp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace ceres; 11 | 12 | void visual_inertial_pnp(SlidingWindow *map, Frame *frame, bool use_inertial, size_t max_iter, const double &max_time) { 13 | LocalParameterization *eigen_quaternion = new LieAlgebraEigenQuaternionParamatrization; 14 | LossFunction *cauchy_loss = new CauchyLoss(1.0); 15 | 16 | Problem problem; 17 | 18 | problem.AddParameterBlock(frame->pose.q.coeffs().data(), 4, eigen_quaternion); 19 | problem.AddParameterBlock(frame->pose.p.data(), 3); 20 | 21 | if (use_inertial) { 22 | Frame *last_frame = map->get_frame(map->frame_num() - 1); 23 | problem.AddResidualBlock(new PreIntegrationPriorCost(last_frame, frame), 24 | nullptr, 25 | frame->pose.q.coeffs().data(), 26 | frame->pose.p.data(), 27 | frame->motion.v.data(), 28 | frame->motion.bg.data(), 29 | frame->motion.ba.data()); 30 | } 31 | 32 | for (size_t i = 0; i < frame->keypoint_num(); ++i) { 33 | Track *track = frame->get_track(i); 34 | if (!track) continue; 35 | if (track->landmark.flag(LF_VALID)) { 36 | problem.AddResidualBlock(new PoseOnlyReprojectionErrorCost(frame, i), 37 | cauchy_loss, 38 | frame->pose.q.coeffs().data(), 39 | frame->pose.p.data()); 40 | } 41 | } 42 | 43 | Solver::Options solver_options; 44 | solver_options.linear_solver_type = ceres::DENSE_SCHUR; 45 | solver_options.trust_region_strategy_type = ceres::DOGLEG; 46 | solver_options.use_explicit_schur_complement = true; 47 | solver_options.minimizer_progress_to_stdout = false; 48 | solver_options.logging_type = ceres::SILENT; 49 | solver_options.max_num_iterations = (int)max_iter; 50 | solver_options.max_solver_time_in_seconds = max_time; 51 | solver_options.num_threads = 1; 52 | solver_options.num_linear_solver_threads = 1; 53 | 54 | Solver::Summary solver_summary; 55 | Solve(solver_options, &problem, &solver_summary); 56 | } 57 | -------------------------------------------------------------------------------- /src/slamtools/preintegrator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace Eigen; 5 | 6 | void PreIntegrator::reset() { 7 | delta.t = 0; 8 | delta.q.setIdentity(); 9 | delta.p.setZero(); 10 | delta.v.setZero(); 11 | delta.cov.setZero(); 12 | delta.sqrt_inv_cov.setZero(); 13 | 14 | jacobian.dq_dbg.setZero(); 15 | jacobian.dp_dbg.setZero(); 16 | jacobian.dp_dba.setZero(); 17 | jacobian.dv_dbg.setZero(); 18 | jacobian.dv_dba.setZero(); 19 | } 20 | 21 | void PreIntegrator::increment(double dt, const IMUData &data, const Vector3d &bg, const Vector3d &ba, bool compute_jacobian, bool compute_covariance) { 22 | assert(("dt must > 0") && (dt >= 0)); 23 | 24 | Vector3d w = data.w - bg; 25 | Vector3d a = data.a - ba; 26 | 27 | if (compute_covariance) { 28 | Matrix A; 29 | A.setIdentity(); 30 | A.block<3, 3>(ES_Q, ES_Q) = expmap(w * dt).conjugate().matrix(); 31 | A.block<3, 3>(ES_V, ES_Q) = -dt * delta.q.matrix() * hat(a); 32 | A.block<3, 3>(ES_P, ES_Q) = -0.5 * dt * dt * delta.q.matrix() * hat(a); 33 | A.block<3, 3>(ES_P, ES_V) = dt * Matrix3d::Identity(); 34 | 35 | Matrix B; 36 | B.setZero(); 37 | B.block<3, 3>(ES_Q, ES_BG - ES_BG) = dt * right_jacobian(w * dt); 38 | B.block<3, 3>(ES_V, ES_BA - ES_BG) = dt * delta.q.matrix(); 39 | B.block<3, 3>(ES_P, ES_BA - ES_BG) = 0.5 * dt * dt * delta.q.matrix(); 40 | 41 | Matrix white_noise_cov; 42 | double inv_dt = 1.0 / std::max(dt, 1.0e-7); 43 | white_noise_cov.setZero(); 44 | white_noise_cov.block<3, 3>(ES_BG - ES_BG, ES_BG - ES_BG) = cov_w * inv_dt; 45 | white_noise_cov.block<3, 3>(ES_BA - ES_BG, ES_BA - ES_BG) = cov_a * inv_dt; 46 | 47 | delta.cov.block<9, 9>(ES_Q, ES_Q) = A * delta.cov.block<9, 9>(0, 0) * A.transpose() + B * white_noise_cov * B.transpose(); 48 | delta.cov.block<3, 3>(ES_BG, ES_BG) += cov_bg * dt; 49 | delta.cov.block<3, 3>(ES_BA, ES_BA) += cov_ba * dt; 50 | } 51 | 52 | if (compute_jacobian) { 53 | jacobian.dp_dbg += dt * jacobian.dv_dbg - 0.5 * dt * dt * delta.q.matrix() * hat(a) * jacobian.dq_dbg; 54 | jacobian.dp_dba += dt * jacobian.dv_dba - 0.5 * dt * dt * delta.q.matrix(); 55 | jacobian.dv_dbg -= dt * delta.q.matrix() * hat(a) * jacobian.dq_dbg; 56 | jacobian.dv_dba -= dt * delta.q.matrix(); 57 | jacobian.dq_dbg = expmap(w * dt).conjugate().matrix() * jacobian.dq_dbg - dt * right_jacobian(w * dt); 58 | } 59 | 60 | delta.t = delta.t + dt; 61 | delta.p = delta.p + dt * delta.v + 0.5 * dt * dt * (delta.q * a); 62 | delta.v = delta.v + dt * (delta.q * a); 63 | delta.q = (delta.q * expmap(w * dt)).normalized(); 64 | } 65 | 66 | bool PreIntegrator::integrate(double t, const Vector3d &bg, const Vector3d &ba, bool compute_jacobian, bool compute_covariance) { 67 | if (data.size() == 0) return false; 68 | reset(); 69 | for (size_t i = 0; i + 1 < data.size(); ++i) { 70 | const IMUData &d = data[i]; 71 | increment(data[i + 1].t - d.t, d, bg, ba, compute_jacobian, compute_covariance); 72 | } 73 | assert(("Image time cannot be earlier than last imu.") && (t >= data.back().t)); 74 | increment(t - data.back().t, data.back(), bg, ba, compute_jacobian, compute_covariance); 75 | if (compute_covariance) { 76 | compute_sqrt_inv_cov(); 77 | } 78 | return true; 79 | } 80 | 81 | void PreIntegrator::compute_sqrt_inv_cov() { 82 | delta.sqrt_inv_cov = LLT>(delta.cov.inverse()).matrixL().transpose(); 83 | } 84 | -------------------------------------------------------------------------------- /src/slamtools/sliding_window.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct SlidingWindow::construct_by_map_t {}; 10 | 11 | SlidingWindow::SlidingWindow() = default; 12 | SlidingWindow::~SlidingWindow() = default; 13 | 14 | void SlidingWindow::clear() { 15 | frames.clear(); 16 | tracks.clear(); 17 | } 18 | 19 | void SlidingWindow::put_frame(std::unique_ptr frame, size_t pos) { 20 | frame->map = this; 21 | if (pos == nil()) { 22 | frames.emplace_back(std::move(frame)); 23 | pos = frames.size() - 1; 24 | } else { 25 | frames.emplace(frames.begin() + pos, std::move(frame)); 26 | } 27 | if (pos > 0) { 28 | Frame *frame_i = frames[pos - 1].get(); 29 | Frame *frame_j = frames[pos].get(); 30 | frame_j->preintegration_factor = Factor::create_preintegration_error(frame_i, frame_j); 31 | } 32 | if (pos < frames.size() - 1) { 33 | Frame *frame_i = frames[pos].get(); 34 | Frame *frame_j = frames[pos + 1].get(); 35 | frame_j->preintegration_factor = Factor::create_preintegration_error(frame_i, frame_j); 36 | } 37 | } 38 | 39 | void SlidingWindow::erase_frame(size_t id) { 40 | Frame *frame = frames[id].get(); 41 | for (size_t i = 0; i < frame->keypoint_num(); ++i) { 42 | Track *track = frame->get_track(i); 43 | if (track != nullptr) { 44 | track->remove_keypoint(frame); 45 | } 46 | } 47 | frames.erase(frames.begin() + id); 48 | if (id > 0 && id < frames.size()) { 49 | Frame *frame_i = frames[id - 1].get(); 50 | Frame *frame_j = frames[id].get(); 51 | frame_j->preintegration_factor = Factor::create_preintegration_error(frame_i, frame_j); 52 | } 53 | } 54 | 55 | Track *SlidingWindow::create_track() { 56 | std::unique_ptr track = std::make_unique(construct_by_map_t()); 57 | track->id_in_map = tracks.size(); 58 | track->map = this; 59 | tracks.emplace_back(std::move(track)); 60 | return tracks.back().get(); 61 | } 62 | 63 | void SlidingWindow::erase_track(Track *track) { 64 | while (track->keypoint_map().size() > 0) { 65 | track->remove_keypoint(track->keypoint_map().begin()->first, false); 66 | } 67 | recycle_track(track); 68 | } 69 | 70 | void SlidingWindow::prune_tracks(const std::function &condition) { 71 | std::vector tracks_to_prune; 72 | for (size_t i = 0; i < track_num(); ++i) { 73 | Track *track = get_track(i); 74 | if (condition(track)) { 75 | tracks_to_prune.push_back(track); 76 | } 77 | } 78 | for (Track *t : tracks_to_prune) { 79 | erase_track(t); 80 | } 81 | } 82 | 83 | void SlidingWindow::recycle_track(Track *track) { 84 | if (track->id_in_map != tracks.back()->id_in_map) { 85 | tracks[track->id_in_map].swap(tracks.back()); 86 | tracks[track->id_in_map]->id_in_map = track->id_in_map; 87 | } 88 | tracks.pop_back(); 89 | } 90 | -------------------------------------------------------------------------------- /src/slamtools/stereo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace Eigen; 9 | 10 | Matrix3d find_essential_matrix(const std::vector &points1, const std::vector &points2, double threshold, double confidence, size_t max_iteration, int seed) { 11 | std::vector _; 12 | return find_essential_matrix(points1, points2, _, threshold, confidence, max_iteration, seed); 13 | } 14 | 15 | Matrix3d find_homography_matrix(const std::vector &points1, const std::vector &points2, double threshold, double confidence, size_t max_iteration, int seed) { 16 | std::vector _; 17 | return find_homography_matrix(points1, points2, _, threshold, confidence, max_iteration, seed); 18 | } 19 | 20 | Matrix3d find_essential_matrix(const std::vector &points1, const std::vector &points2, std::vector &inlier_mask, double threshold, double confidence, size_t max_iteration, int seed) { 21 | struct EssentialSolver { 22 | std::vector operator()(const std::array &samples1, const std::array &samples2) const { 23 | return solve_essential_5pt(samples1, samples2); 24 | } 25 | }; 26 | struct EssentialEvaluator { 27 | const Matrix3d &E; 28 | const Matrix3d Et; 29 | EssentialEvaluator(const Matrix3d &E) : 30 | E(E), Et(E.transpose()) { 31 | } 32 | double operator()(const Vector2d &p1, const Vector2d &p2) const { 33 | return essential_geometric_error(E, p1, p2) + essential_geometric_error(Et, p2, p1); 34 | } 35 | }; 36 | static const double t1 = 3.84; 37 | Ransac<5, Matrix3d, EssentialSolver, EssentialEvaluator> ransac(2.0 * t1 * threshold * threshold, confidence, max_iteration, seed); 38 | return ransac.solve(inlier_mask, points1, points2); 39 | } 40 | 41 | Matrix3d find_homography_matrix(const std::vector &points1, const std::vector &points2, std::vector &inlier_mask, double threshold, double confidence, size_t max_iteration, int seed) { 42 | struct HomographySolver { 43 | Matrix3d operator()(const std::array &samples1, const std::array &samples2) const { 44 | return solve_homography_4pt(samples1, samples2); 45 | } 46 | }; 47 | struct HomographyEvaluator { 48 | const Matrix3d &H; 49 | const Matrix3d Hinv; 50 | HomographyEvaluator(const Matrix3d &H) : 51 | H(H), Hinv(H.inverse()) { 52 | } 53 | double operator()(const Vector2d &p1, const Vector2d &p2) const { 54 | return homography_geometric_error(H, p1, p2) + homography_geometric_error(Hinv, p2, p1); 55 | } 56 | }; 57 | static const double t2 = 5.99; 58 | Ransac<4, Matrix3d, HomographySolver, HomographyEvaluator> ransac(2.0 * t2 * threshold * threshold, confidence, max_iteration, seed); 59 | return ransac.solve(inlier_mask, points1, points2); 60 | } 61 | 62 | size_t triangulate_from_rt(const std::vector &points1, const std::vector &points2, const Matrix3d &R, const Vector3d &T, std::vector &result_points, std::vector &result_status) { 63 | size_t count = 0; 64 | result_points.resize(points1.size()); 65 | result_status.resize(points1.size()); 66 | 67 | Matrix P1, P2; 68 | P1.setIdentity(); 69 | P2 << R, T; 70 | 71 | for (size_t i = 0; i < points1.size(); ++i) { 72 | if (triangulate_point_checked(P1, P2, points1[i], points2[i], result_points[i])) { 73 | result_status[i] = 1; 74 | count++; 75 | } else { 76 | result_status[i] = 0; 77 | } 78 | } 79 | return count; 80 | } 81 | 82 | size_t triangulate_from_rt(const std::vector &points1, const std::vector &points2, const std::vector &Rs, const std::vector &Ts, std::vector &result_points, Matrix3d &result_R, Vector3d &result_T, std::vector &result_status) { 83 | std::vector> points(Rs.size()); 84 | std::vector> status(Rs.size()); 85 | std::vector counts(Rs.size()); 86 | 87 | size_t best_i = 0; 88 | for (size_t i = 0; i < Rs.size(); ++i) { 89 | counts[i] = triangulate_from_rt(points1, points2, Rs[i], Ts[i], points[i], status[i]); 90 | if (counts[i] > counts[best_i]) { 91 | best_i = i; 92 | } 93 | } 94 | 95 | result_R = Rs[best_i]; 96 | result_T = Ts[best_i]; 97 | result_points.swap(points[best_i]); 98 | result_status.swap(status[best_i]); 99 | 100 | return counts[best_i]; 101 | } 102 | 103 | size_t triangulate_from_rt_scored(const std::vector &points1, const std::vector &points2, const Matrix3d &R, const Vector3d &T, std::vector &result_points, std::vector &result_status, double &score) { 104 | size_t count = 0; 105 | result_points.resize(points1.size()); 106 | result_status.resize(points1.size()); 107 | 108 | Matrix P1, P2; 109 | P1.setIdentity(); 110 | P2 << R, T; 111 | 112 | score = 0; 113 | 114 | for (size_t i = 0; i < points1.size(); ++i) { 115 | double current_score; 116 | if (triangulate_point_scored(P1, P2, points1[i], points2[i], result_points[i], current_score)) { 117 | result_status[i] = 1; 118 | score += current_score; 119 | count++; 120 | } else { 121 | result_status[i] = 0; 122 | } 123 | } 124 | 125 | score /= (double)std::max(count, size_t(1)); 126 | return count; 127 | } 128 | 129 | size_t triangulate_from_rt_scored(const std::vector &points1, const std::vector &points2, const std::vector &Rs, const std::vector &Ts, size_t count_threshold, std::vector &result_points, Matrix3d &result_R, Vector3d &result_T, std::vector &result_status) { 130 | std::vector> points(Rs.size()); 131 | std::vector> status(Rs.size()); 132 | std::vector counts(Rs.size()); 133 | std::vector scores(Rs.size()); 134 | 135 | size_t best_i = 0; 136 | for (size_t i = 0; i < Rs.size(); ++i) { 137 | counts[i] = triangulate_from_rt_scored(points1, points2, Rs[i], Ts[i], points[i], status[i], scores[i]); 138 | if (counts[i] > count_threshold && scores[i] < scores[best_i]) { 139 | best_i = i; 140 | } else if (counts[i] > counts[best_i]) { 141 | best_i = i; 142 | } 143 | } 144 | 145 | result_R = Rs[best_i]; 146 | result_T = Ts[best_i]; 147 | result_points.swap(points[best_i]); 148 | result_status.swap(status[best_i]); 149 | 150 | return counts[best_i]; 151 | } 152 | 153 | size_t triangulate_from_essential(const std::vector &points1, const std::vector &points2, const Matrix3d &E, std::vector &result_points, std::vector &result_status, Matrix3d &R, Vector3d &T) { 154 | std::array Rs; 155 | std::array Ts; 156 | std::array, 4> results; 157 | std::array, 4> stats; 158 | std::array counts; 159 | 160 | decompose_essential(E, Rs[0], Rs[2], Ts[0]); 161 | Rs[1] = Rs[0]; 162 | Rs[3] = Rs[2]; 163 | Ts[2] = Ts[0]; 164 | Ts[1] = Ts[3] = -Ts[0]; 165 | 166 | size_t best_i = 0; 167 | for (size_t i = 0; i < results.size(); ++i) { 168 | counts[i] = triangulate_from_rt(points1, points2, Rs[i], Ts[i], results[i], stats[i]); 169 | if (counts[i] > counts[best_i]) { 170 | best_i = i; 171 | } 172 | } 173 | 174 | R = Rs[best_i]; 175 | T = Ts[best_i]; 176 | result_points.swap(results[best_i]); 177 | result_status.swap(stats[best_i]); 178 | 179 | return counts[best_i]; 180 | } 181 | 182 | size_t triangulate_from_homography(const std::vector &points1, const std::vector &points2, const Matrix3d &H, std::vector &result_points, std::vector &result_status, Matrix3d &R, Vector3d &T) { 183 | std::array Rs; 184 | std::array Ts; 185 | std::array, 4> results; 186 | std::array, 4> stats; 187 | std::array counts; 188 | 189 | Vector3d n1, n2; 190 | decompose_homography(H, Rs[0], Rs[2], Ts[0], Ts[2], n1, n2); 191 | 192 | Rs[1] = Rs[0]; 193 | Rs[3] = Rs[2]; 194 | Ts[1] = -Ts[0]; 195 | Ts[3] = -Ts[2]; 196 | 197 | size_t best_i = 0; 198 | for (size_t i = 0; i < results.size(); ++i) { 199 | counts[i] = triangulate_from_rt(points1, points2, Rs[i], Ts[i], results[i], stats[i]); 200 | if (counts[i] > counts[best_i]) { 201 | best_i = i; 202 | } 203 | } 204 | 205 | R = Rs[best_i]; 206 | T = Ts[best_i]; 207 | result_points.swap(results[best_i]); 208 | result_status.swap(stats[best_i]); 209 | 210 | return counts[best_i]; 211 | } 212 | -------------------------------------------------------------------------------- /src/slamtools/track.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | 9 | Track::Track() = default; 10 | Track::~Track() = default; 11 | 12 | const Vector2d &Track::get_keypoint(Frame *frame) const { 13 | return frame->get_keypoint(keypoint_refs.at(frame)); 14 | } 15 | 16 | void Track::add_keypoint(Frame *frame, size_t keypoint_id) { 17 | frame->tracks[keypoint_id] = this; 18 | frame->reprojection_factors[keypoint_id] = Factor::create_reprojection_error(frame, keypoint_id); 19 | keypoint_refs[frame] = keypoint_id; 20 | } 21 | 22 | void Track::remove_keypoint(Frame *frame, bool suicide_if_empty) { 23 | size_t keypoint_id = keypoint_refs.at(frame); 24 | frame->tracks[keypoint_id] = nullptr; 25 | frame->reprojection_factors[keypoint_id].reset(); 26 | keypoint_refs.erase(frame); 27 | if (suicide_if_empty && keypoint_refs.size() == 0) { 28 | map->recycle_track(this); 29 | } 30 | } 31 | 32 | bool Track::triangulate() { 33 | std::vector> Ps; 34 | std::vector ps; 35 | for (const auto &t : keypoint_map()) { 36 | Matrix P; 37 | Matrix3d R; 38 | Vector3d T; 39 | PoseState pose = t.first->get_pose(t.first->camera); 40 | R = pose.q.conjugate().matrix(); 41 | T = -(R * pose.p); 42 | P << R, T; 43 | Ps.push_back(P); 44 | ps.push_back(t.first->get_keypoint(t.second)); 45 | } 46 | Vector3d p; 47 | if (triangulate_point_checked(Ps, ps, p)) { 48 | landmark.x = p; 49 | landmark.flag(LF_VALID) = true; 50 | } else { 51 | landmark.flag(LF_VALID) = false; 52 | } 53 | return landmark.flag(LF_VALID); 54 | } 55 | --------------------------------------------------------------------------------