v01

include/basalt/utils/ba_utils.h

@@ -0,0 +1,146 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <basalt/vi_estimator/landmark_database.h>
+
+namespace basalt {
+
+template <class Scalar>
+Sophus::SE3<Scalar> computeRelPose(
+    const Sophus::SE3<Scalar>& T_w_i_h, const Sophus::SE3<Scalar>& T_i_c_h,
+    const Sophus::SE3<Scalar>& T_w_i_t, const Sophus::SE3<Scalar>& T_i_c_t,
+    Sophus::Matrix6<Scalar>* d_rel_d_h = nullptr,
+    Sophus::Matrix6<Scalar>* d_rel_d_t = nullptr) {
+  Sophus::SE3<Scalar> tmp2 = (T_i_c_t).inverse();
+
+  Sophus::SE3<Scalar> T_t_i_h_i;
+  T_t_i_h_i.so3() = T_w_i_t.so3().inverse() * T_w_i_h.so3();
+  T_t_i_h_i.translation() =
+      T_w_i_t.so3().inverse() * (T_w_i_h.translation() - T_w_i_t.translation());
+
+  Sophus::SE3<Scalar> tmp = tmp2 * T_t_i_h_i;
+  Sophus::SE3<Scalar> res = tmp * T_i_c_h;
+
+  if (d_rel_d_h) {
+    Sophus::Matrix3<Scalar> R = T_w_i_h.so3().inverse().matrix();
+
+    Sophus::Matrix6<Scalar> RR;
+    RR.setZero();
+    RR.template topLeftCorner<3, 3>() = R;
+    RR.template bottomRightCorner<3, 3>() = R;
+
+    *d_rel_d_h = tmp.Adj() * RR;
+  }
+
+  if (d_rel_d_t) {
+    Sophus::Matrix3<Scalar> R = T_w_i_t.so3().inverse().matrix();
+
+    Sophus::Matrix6<Scalar> RR;
+    RR.setZero();
+    RR.template topLeftCorner<3, 3>() = R;
+    RR.template bottomRightCorner<3, 3>() = R;
+
+    *d_rel_d_t = -tmp2.Adj() * RR;
+  }
+
+  return res;
+}
+
+template <class Scalar, class CamT>
+inline bool linearizePoint(
+    const Eigen::Matrix<Scalar, 2, 1>& kpt_obs, const Keypoint<Scalar>& kpt_pos,
+    const Eigen::Matrix<Scalar, 4, 4>& T_t_h, const CamT& cam,
+    Eigen::Matrix<Scalar, 2, 1>& res,
+    Eigen::Matrix<Scalar, 2, POSE_SIZE>* d_res_d_xi = nullptr,
+    Eigen::Matrix<Scalar, 2, 3>* d_res_d_p = nullptr,
+    Eigen::Matrix<Scalar, 4, 1>* proj = nullptr) {
+  static_assert(std::is_same_v<typename CamT::Scalar, Scalar>);
+
+  // Todo implement without jacobians
+  Eigen::Matrix<Scalar, 4, 2> Jup;
+  Eigen::Matrix<Scalar, 4, 1> p_h_3d;
+  p_h_3d = StereographicParam<Scalar>::unproject(kpt_pos.direction, &Jup);
+  p_h_3d[3] = kpt_pos.inv_dist;
+
+  const Eigen::Matrix<Scalar, 4, 1> p_t_3d = T_t_h * p_h_3d;
+
+  Eigen::Matrix<Scalar, 2, 4> Jp;
+  bool valid = cam.project(p_t_3d, res, &Jp);
+  valid &= res.array().isFinite().all();
+
+  if (!valid) {
+    //      std::cerr << " Invalid projection! kpt_pos.dir "
+    //                << kpt_pos.dir.transpose() << " kpt_pos.id " <<
+    //                kpt_pos.id
+    //                << " idx " << kpt_obs.kpt_id << std::endl;
+
+    //      std::cerr << "T_t_h\n" << T_t_h << std::endl;
+    //      std::cerr << "p_h_3d\n" << p_h_3d.transpose() << std::endl;
+    //      std::cerr << "p_t_3d\n" << p_t_3d.transpose() << std::endl;
+
+    return false;
+  }
+
+  if (proj) {
+    proj->template head<2>() = res;
+    (*proj)[2] = p_t_3d[3] / p_t_3d.template head<3>().norm();
+  }
+  res -= kpt_obs;
+
+  if (d_res_d_xi) {
+    Eigen::Matrix<Scalar, 4, POSE_SIZE> d_point_d_xi;
+    d_point_d_xi.template topLeftCorner<3, 3>() =
+        Eigen::Matrix<Scalar, 3, 3>::Identity() * kpt_pos.inv_dist;
+    d_point_d_xi.template topRightCorner<3, 3>() =
+        -Sophus::SO3<Scalar>::hat(p_t_3d.template head<3>());
+    d_point_d_xi.row(3).setZero();
+
+    *d_res_d_xi = Jp * d_point_d_xi;
+  }
+
+  if (d_res_d_p) {
+    Eigen::Matrix<Scalar, 4, 3> Jpp;
+    Jpp.setZero();
+    Jpp.template block<3, 2>(0, 0) = T_t_h.template topLeftCorner<3, 4>() * Jup;
+    Jpp.col(2) = T_t_h.col(3);
+
+    *d_res_d_p = Jp * Jpp;
+  }
+
+  return true;
+}
+
+}  // namespace basalt

include/basalt/utils/cast_utils.hpp

@@ -0,0 +1,64 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2021, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <type_traits>
+
+namespace basalt {
+
+template <class I>
+typename std::make_signed_t<I> signed_cast(I v) {
+  static_assert(std::is_unsigned_v<I>, "no unsigned");
+  return static_cast<typename std::make_signed_t<I>>(v);
+}
+
+template <class I>
+typename std::make_unsigned_t<I> unsigned_cast(I v) {
+  static_assert(std::is_signed_v<I>, "no signed");
+  return static_cast<typename std::make_unsigned_t<I>>(v);
+}
+
+// copy-assign map while casting Scalar type of values
+template <class M1, class M2>
+void assign_cast_map_values(M1& a, const M2& b) {
+  using Scalar = typename M1::mapped_type::Scalar;
+  a.clear();
+  for (const auto& [k, v] : b) {
+    a.try_emplace(k, v.template cast<Scalar>());
+  }
+}
+
+}  // namespace basalt

include/basalt/utils/common_types.h

@@ -0,0 +1,319 @@
+/**
+BSD 3-Clause License
+
+Copyright (c) 2018, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <bitset>
+#include <cstdint>
+#include <map>
+#include <unordered_map>
+#include <vector>
+
+#include <tbb/concurrent_unordered_map.h>
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <sophus/se3.hpp>
+
+#include <basalt/utils/hash.h>
+#include <basalt/utils/sophus_utils.hpp>
+
+namespace basalt {
+
+/// ids for 2D features detected in images
+using FeatureId = int;
+
+/// identifies a frame of multiple images (stereo pair)
+using FrameId = int64_t;
+
+/// identifies the camera (left or right)
+using CamId = std::size_t;
+
+/// pair of image timestamp and camera id identifies an image (imageId)
+struct TimeCamId {
+  TimeCamId() : frame_id(0), cam_id(0) {}
+
+  TimeCamId(const FrameId& frame_id, const CamId& cam_id)
+      : frame_id(frame_id), cam_id(cam_id) {}
+
+  FrameId frame_id;
+  CamId cam_id;
+};
+
+inline std::ostream& operator<<(std::ostream& os, const TimeCamId& tcid) {
+  os << tcid.frame_id << "_" << tcid.cam_id;
+  return os;
+}
+
+inline bool operator<(const TimeCamId& o1, const TimeCamId& o2) {
+  if (o1.frame_id == o2.frame_id) return o1.cam_id < o2.cam_id;
+  return o1.frame_id < o2.frame_id;
+}
+
+inline bool operator==(const TimeCamId& o1, const TimeCamId& o2) {
+  return o1.frame_id == o2.frame_id && o1.cam_id == o2.cam_id;
+}
+
+inline bool operator!=(const TimeCamId& o1, const TimeCamId& o2) {
+  return o1.frame_id != o2.frame_id || o1.cam_id != o2.cam_id;
+}
+
+constexpr static const size_t FEATURE_HASH_MAX_SIZE = 32;
+using FeatureHash = std::bitset<FEATURE_HASH_MAX_SIZE>;
+using HashBowVector = std::vector<std::pair<FeatureHash, double>>;
+
+/// keypoint positions and descriptors for an image
+struct KeypointsData {
+  /// collection of 2d corner points (indexed by FeatureId)
+  std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>
+      corners;
+  /// collection of feature orientation (in radian) with same index as `corners`
+  /// (indexed by FeatureId)
+  std::vector<double> corner_angles;
+  /// collection of feature descriptors with same index as `corners` (indexed by
+  /// FeatureId)
+  std::vector<std::bitset<256>> corner_descriptors;
+
+  Eigen::aligned_vector<Eigen::Vector4d> corners_3d;
+
+  std::vector<FeatureHash> hashes;
+  HashBowVector bow_vector;
+};
+
+/// feature corners is a collection of { imageId => KeypointsData }
+using Corners = tbb::concurrent_unordered_map<TimeCamId, KeypointsData,
+                                              std::hash<TimeCamId>>;
+
+/// feature matches for an image pair
+struct MatchData {
+  /// estimated transformation (based on inliers or calibration) from the second
+  /// image's coordinate system to the first image's corrdinate system
+  Sophus::SE3d T_i_j;
+  /// collection of {featureId_i, featureId_j} pairs of all matches
+  std::vector<std::pair<FeatureId, FeatureId>> matches;
+  /// collection of {featureId_i, featureId_j} pairs of inlier matches
+  std::vector<std::pair<FeatureId, FeatureId>> inliers;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// feature matches is a collection of { (imageId, imageId) => MatchData }
+using Matches = tbb::concurrent_unordered_map<
+    std::pair<TimeCamId, TimeCamId>, MatchData,
+    std::hash<std::pair<TimeCamId, TimeCamId>>,
+    std::equal_to<std::pair<TimeCamId, TimeCamId>>,
+    Eigen::aligned_allocator<
+        std::pair<const std::pair<TimeCamId, TimeCamId>, MatchData>>>;
+
+/// pair of image and feature indices
+using ImageFeaturePair = std::pair<TimeCamId, FeatureId>;
+
+/// Feature tracks are collections of {ImageId => FeatureId}.
+/// I.e. a collection of all images that observed this feature and the
+/// corresponding feature index in that image.
+using FeatureTrack = std::map<TimeCamId, FeatureId>;
+
+/// Ids for feature tracks; also used for landmarks created from (some of) the
+/// tracks;
+using TrackId = int64_t;
+
+/// FeatureTracks is a collection {TrackId => FeatureTrack}
+using FeatureTracks = std::unordered_map<TrackId, FeatureTrack>;
+
+/// cameras in the map
+struct Camera {
+  // camera pose (transforms from camera to world)
+  Sophus::SE3d T_w_c;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/// landmarks in the map
+struct Landmark {
+  /// 3d position in world coordinates
+  Eigen::Vector3d p;
+
+  /// Inlier observations in the current map.
+  /// This is a subset of the original feature track.
+  FeatureTrack obs;
+
+  /// Outlier observations in the current map.
+  /// This is a subset of the original feature track.
+  FeatureTrack outlier_obs;
+};
+
+/// collection {imageId => Camera} for all cameras in the map
+using Cameras =
+    std::map<TimeCamId, Camera, std::less<TimeCamId>,
+             Eigen::aligned_allocator<std::pair<const TimeCamId, Camera>>>;
+
+/// collection {trackId => Landmark} for all landmarks in the map.
+/// trackIds correspond to feature_tracks
+using Landmarks = std::unordered_map<TrackId, Landmark>;
+
+/// camera candidate to be added to map
+struct CameraCandidate {
+  TimeCamId tcid;
+  std::vector<TrackId> shared_tracks;
+
+  // keep track of different stages of adding a set of candidate cameras and its
+  // landmarks to the map
+  bool tried = false;            //!< tried to add to map
+  bool camera_added = false;     //!< succeeded to add to map
+  bool landmarks_added = false;  //!< added new landmarks to map
+};
+
+/// list of current candidates and some book keeping for the different stages
+struct CameraCandidates {
+  enum Stage {
+    ComputeCandidates,
+    AddCameras,
+    AddLandmarks,
+    Optimize,
+    RemoveOutliers,
+    Done
+  };
+
+  std::vector<CameraCandidate> cameras;
+  Stage current_stage = ComputeCandidates;
+  int min_localization_inliers = 0;
+  int max_cameras_to_add = 0;
+
+  int num_cameras_added() {
+    int num_added = 0;
+    for (const auto& c : cameras) {
+      if (c.camera_added) {
+        ++num_added;
+      }
+    }
+    return num_added;
+  }
+
+  int num_landmarks_added() {
+    int num_added = 0;
+    for (const auto& c : cameras) {
+      if (c.landmarks_added) {
+        ++num_added;
+      }
+    }
+    return num_added;
+  }
+};
+
+/// Flags for different landmark outlier criteria
+enum OutlierFlags {
+  OutlierNone = 0,
+  // reprojection error much too large
+  OutlierReprojectionErrorHuge = 1 << 0,
+  // reprojection error too large
+  OutlierReprojectionErrorNormal = 1 << 1,
+  // distance to a camera too small
+  OutlierCameraDistance = 1 << 2,
+  // z-coord in some camera frame too small
+  OutlierZCoordinate = 1 << 3
+};
+
+/// info on a single projected landmark
+struct ProjectedLandmark {
+  Eigen::Vector2d point_measured;            //!< detected feature location
+  Eigen::Vector2d point_reprojected;         //!< landmark projected into image
+  Eigen::Vector3d point_3d_c;                //!< 3d point in camera coordinates
+  TrackId track_id = -1;                     //!< corresponding track_id
+  double reprojection_error = 0;             //!< current reprojection error
+  unsigned int outlier_flags = OutlierNone;  //!< flags for outlier
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+using ProjectedLandmarkPtr = std::shared_ptr<ProjectedLandmark>;
+using ProjectedLandmarkConstPtr = std::shared_ptr<const ProjectedLandmark>;
+
+/// all landmark projections for inlier and outlier observations for a single
+/// image
+struct ImageProjection {
+  std::vector<ProjectedLandmarkConstPtr> obs;
+  std::vector<ProjectedLandmarkConstPtr> outlier_obs;
+};
+
+/// projections for all images
+using ImageProjections = std::map<TimeCamId, ImageProjection>;
+
+/// inlier projections indexed per track
+using TrackProjections =
+    std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>;
+
+}  // namespace basalt
+
+namespace cereal {
+
+template <class Archive>
+void serialize(Archive& ar, basalt::TimeCamId& c) {
+  ar(c.frame_id, c.cam_id);
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::KeypointsData& c) {
+  ar(c.corners, c.corner_angles, c.corner_descriptors);
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::MatchData& c) {
+  ar(c.T_i_j, c.matches, c.inliers);
+}
+
+}  // namespace cereal
+
+namespace std {
+
+template <>
+struct hash<basalt::TimeCamId> {
+  size_t operator()(const basalt::TimeCamId& x) const {
+    size_t seed = 0;
+    basalt::hash_combine(seed, x.frame_id);
+    basalt::hash_combine(seed, x.cam_id);
+    return seed;
+  }
+};
+
+template <>
+struct hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>> {
+  size_t operator()(
+      const std::pair<basalt::TimeCamId, basalt::TimeCamId>& x) const {
+    size_t seed = 0;
+    basalt::hash_combine(seed, x.first.frame_id);
+    basalt::hash_combine(seed, x.first.cam_id);
+    basalt::hash_combine(seed, x.second.frame_id);
+    basalt::hash_combine(seed, x.second.cam_id);
+    return seed;
+  }
+};
+
+}  // namespace std

include/basalt/utils/filesystem.h

@@ -0,0 +1,70 @@
+#pragma once
+
+// Check for feature test macro for <filesystem>
+#if defined(__cpp_lib_filesystem)
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0
+
+// Check for feature test macro for <experimental/filesystem>
+#elif defined(__cpp_lib_experimental_filesystem)
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
+
+// We can't check if headers exist...
+// Let's assume experimental to be safe
+#elif !defined(__has_include)
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
+
+// Check if the header "<filesystem>" exists
+#elif __has_include(<filesystem>)
+
+// If we're compiling on Visual Studio and are not compiling with C++17, we need
+// to use experimental
+#ifdef _MSC_VER
+
+// Check and include header that defines "_HAS_CXX17"
+#if __has_include(<yvals_core.h>)
+#include <yvals_core.h>
+
+// Check for enabled C++17 support
+#if defined(_HAS_CXX17) && _HAS_CXX17
+// We're using C++17, so let's use the normal version
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0
+#endif
+#endif
+
+// If the marco isn't defined yet, that means any of the other VS specific
+// checks failed, so we need to use experimental
+#ifndef INCLUDE_STD_FILESYSTEM_EXPERIMENTAL
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
+#endif
+
+// Not on Visual Studio. Let's use the normal version
+#else  // #ifdef _MSC_VER
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0
+#endif
+
+// Check if the header "<filesystem>" exists
+#elif __has_include(<experimental/filesystem>)
+#define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1
+
+// Fail if neither header is available with a nice error message
+#else
+#error Could not find system header "<filesystem>" or "<experimental/filesystem>"
+#endif
+
+// We priously determined that we need the exprimental version
+#if INCLUDE_STD_FILESYSTEM_EXPERIMENTAL
+#include <experimental/filesystem>
+
+namespace basalt {
+namespace fs = std::experimental::filesystem;
+}
+
+// We have a decent compiler and can use the normal version
+#else
+#include <filesystem>
+
+namespace basalt {
+namespace fs = std::filesystem;
+}
+
+#endif

include/basalt/utils/imu_types.h

@@ -0,0 +1,385 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <atomic>
+#include <map>
+#include <memory>
+#include <set>
+
+#include <Eigen/Dense>
+#include <sophus/se3.hpp>
+
+#include <basalt/imu/imu_types.h>
+#include <basalt/optical_flow/optical_flow.h>
+#include <basalt/utils/common_types.h>
+#include <basalt/utils/sophus_utils.hpp>
+
+#include <cereal/cereal.hpp>
+
+namespace basalt {
+
+template <class Scalar_>
+class IntegratedImuMeasurement;
+
+template <class Scalar_>
+struct PoseStateWithLin;
+
+namespace constants {
+static const Eigen::Vector3d g(0, 0, -9.81);
+static const Eigen::Vector3d g_dir(0, 0, -1);
+}  // namespace constants
+
+template <class Scalar_>
+struct PoseVelBiasStateWithLin {
+  using Scalar = Scalar_;
+
+  using VecN = typename PoseVelBiasState<Scalar>::VecN;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using SE3 = Sophus::SE3<Scalar>;
+
+  PoseVelBiasStateWithLin() {
+    linearized = false;
+    delta.setZero();
+  };
+
+  PoseVelBiasStateWithLin(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i,
+                          const Vec3& bias_gyro, const Vec3& bias_accel,
+                          bool linearized)
+      : linearized(linearized),
+        state_linearized(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel) {
+    delta.setZero();
+    state_current = state_linearized;
+  }
+
+  explicit PoseVelBiasStateWithLin(const PoseVelBiasState<Scalar>& other)
+      : linearized(false), state_linearized(other) {
+    delta.setZero();
+    state_current = other;
+  }
+
+  template <class Scalar2>
+  PoseVelBiasStateWithLin<Scalar2> cast() const {
+    PoseVelBiasStateWithLin<Scalar2> a;
+    a.linearized = linearized;
+    a.delta = delta.template cast<Scalar2>();
+    a.state_linearized = state_linearized.template cast<Scalar2>();
+    a.state_current = state_current.template cast<Scalar2>();
+    return a;
+  }
+
+  void setLinFalse() {
+    linearized = false;
+    delta.setZero();
+  }
+
+  void setLinTrue() {
+    linearized = true;
+    BASALT_ASSERT(delta.isApproxToConstant(0));
+    state_current = state_linearized;
+  }
+
+  void applyInc(const VecN& inc) {
+    if (!linearized) {
+      state_linearized.applyInc(inc);
+    } else {
+      delta += inc;
+      state_current = state_linearized;
+      state_current.applyInc(delta);
+    }
+  }
+
+  inline const PoseVelBiasState<Scalar>& getState() const {
+    if (!linearized) {
+      return state_linearized;
+    } else {
+      return state_current;
+    }
+  }
+
+  inline const PoseVelBiasState<Scalar>& getStateLin() const {
+    return state_linearized;
+  }
+
+  inline bool isLinearized() const { return linearized; }
+  inline const VecN& getDelta() const { return delta; }
+  inline int64_t getT_ns() const { return state_linearized.t_ns; }
+
+  inline void backup() {
+    backup_delta = delta;
+    backup_state_linearized = state_linearized;
+    backup_state_current = state_current;
+  }
+
+  inline void restore() {
+    delta = backup_delta;
+    state_linearized = backup_state_linearized;
+    state_current = backup_state_current;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+  bool linearized;
+  VecN delta;
+  PoseVelBiasState<Scalar> state_linearized, state_current;
+
+  VecN backup_delta;
+  PoseVelBiasState<Scalar> backup_state_linearized, backup_state_current;
+
+  // give access to private members for constructor of PoseStateWithLin
+  friend struct PoseStateWithLin<Scalar>;
+
+  // give access to private members for cast() implementation
+  template <class>
+  friend struct PoseVelBiasStateWithLin;
+
+  friend class cereal::access;
+
+  template <class Archive>
+  void serialize(Archive& ar) {
+    ar(state_linearized.T_w_i);
+    ar(state_linearized.vel_w_i);
+    ar(state_linearized.bias_gyro);
+    ar(state_linearized.bias_accel);
+    ar(state_current.T_w_i);
+    ar(state_current.vel_w_i);
+    ar(state_current.bias_gyro);
+    ar(state_current.bias_accel);
+    ar(delta);
+    ar(linearized);
+    ar(state_linearized.t_ns);
+  }
+};
+
+template <class Scalar_>
+struct PoseStateWithLin {
+  using Scalar = Scalar_;
+  using VecN = typename PoseState<Scalar>::VecN;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using SE3 = Sophus::SE3<Scalar>;
+
+  PoseStateWithLin() {
+    linearized = false;
+    delta.setZero();
+  };
+
+  PoseStateWithLin(int64_t t_ns, const SE3& T_w_i, bool linearized = false)
+      : linearized(linearized), pose_linearized(t_ns, T_w_i) {
+    delta.setZero();
+    T_w_i_current = T_w_i;
+  }
+
+  explicit PoseStateWithLin(const PoseVelBiasStateWithLin<Scalar>& other)
+      : linearized(other.linearized),
+        delta(other.delta.template head<6>()),
+        pose_linearized(other.state_linearized.t_ns,
+                        other.state_linearized.T_w_i) {
+    T_w_i_current = pose_linearized.T_w_i;
+    PoseState<Scalar>::incPose(delta, T_w_i_current);
+  }
+
+  template <class Scalar2>
+  PoseStateWithLin<Scalar2> cast() const {
+    PoseStateWithLin<Scalar2> a;
+    a.linearized = linearized;
+    a.delta = delta.template cast<Scalar2>();
+    a.pose_linearized = pose_linearized.template cast<Scalar2>();
+    a.T_w_i_current = T_w_i_current.template cast<Scalar2>();
+    return a;
+  }
+
+  void setLinTrue() {
+    linearized = true;
+    BASALT_ASSERT(delta.isApproxToConstant(0));
+    T_w_i_current = pose_linearized.T_w_i;
+  }
+
+  inline const SE3& getPose() const {
+    if (!linearized) {
+      return pose_linearized.T_w_i;
+    } else {
+      return T_w_i_current;
+    }
+  }
+
+  inline const SE3& getPoseLin() const { return pose_linearized.T_w_i; }
+
+  inline void applyInc(const VecN& inc) {
+    if (!linearized) {
+      PoseState<Scalar>::incPose(inc, pose_linearized.T_w_i);
+    } else {
+      delta += inc;
+      T_w_i_current = pose_linearized.T_w_i;
+      PoseState<Scalar>::incPose(delta, T_w_i_current);
+    }
+  }
+
+  inline bool isLinearized() const { return linearized; }
+  inline const VecN& getDelta() const { return delta; }
+  inline int64_t getT_ns() const { return pose_linearized.t_ns; }
+
+  inline void backup() {
+    backup_delta = delta;
+    backup_pose_linearized = pose_linearized;
+    backup_T_w_i_current = T_w_i_current;
+  }
+
+  inline void restore() {
+    delta = backup_delta;
+    pose_linearized = backup_pose_linearized;
+    T_w_i_current = backup_T_w_i_current;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+  bool linearized;
+  VecN delta;
+  PoseState<Scalar> pose_linearized;
+  SE3 T_w_i_current;
+
+  VecN backup_delta;
+  PoseState<Scalar> backup_pose_linearized;
+  SE3 backup_T_w_i_current;
+
+  // give access to private members for cast() implementation
+  template <class>
+  friend struct PoseStateWithLin;
+
+  friend class cereal::access;
+
+  template <class Archive>
+  void serialize(Archive& ar) {
+    ar(pose_linearized.T_w_i);
+    ar(T_w_i_current);
+    ar(delta);
+    ar(linearized);
+    ar(pose_linearized.t_ns);
+  }
+};
+
+struct AbsOrderMap {
+  std::map<int64_t, std::pair<int, int>> abs_order_map;
+  size_t items = 0;
+  size_t total_size = 0;
+
+  void print_order() {
+    for (const auto& kv : abs_order_map) {
+      std::cout << kv.first << " (" << kv.second.first << ","
+                << kv.second.second << ")" << std::endl;
+    }
+    std::cout << std::endl;
+  }
+};
+
+template <class Scalar_>
+struct ImuLinData {
+  using Scalar = Scalar_;
+
+  const Eigen::Matrix<Scalar, 3, 1>& g;
+  const Eigen::Matrix<Scalar, 3, 1>& gyro_bias_weight_sqrt;
+  const Eigen::Matrix<Scalar, 3, 1>& accel_bias_weight_sqrt;
+
+  std::map<int64_t, const IntegratedImuMeasurement<Scalar>*> imu_meas;
+};
+
+template <class Scalar_>
+struct MargLinData {
+  using Scalar = Scalar_;
+
+  bool is_sqrt = true;
+
+  AbsOrderMap order;
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> H;
+  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> b;
+};
+
+struct MargData {
+  typedef std::shared_ptr<MargData> Ptr;
+
+  AbsOrderMap aom;
+  Eigen::MatrixXd abs_H;
+  Eigen::VectorXd abs_b;
+  Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>> frame_states;
+  Eigen::aligned_map<int64_t, PoseStateWithLin<double>> frame_poses;
+  std::set<int64_t> kfs_all;
+  std::set<int64_t> kfs_to_marg;
+  bool use_imu;
+
+  std::vector<OpticalFlowResult::Ptr> opt_flow_res;
+};
+
+struct RelPoseFactor {
+  int64_t t_i_ns, t_j_ns;
+
+  Sophus::SE3d T_i_j;
+  Sophus::Matrix6d cov_inv;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct RollPitchFactor {
+  int64_t t_ns;
+
+  Sophus::SO3d R_w_i_meas;
+
+  Eigen::Matrix2d cov_inv;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace basalt
+
+namespace cereal {
+
+template <class Archive>
+void serialize(Archive& ar, basalt::AbsOrderMap& a) {
+  ar(a.total_size);
+  ar(a.items);
+  ar(a.abs_order_map);
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::MargData& m) {
+  ar(m.aom);
+  ar(m.abs_H);
+  ar(m.abs_b);
+  ar(m.frame_poses);
+  ar(m.frame_states);
+  ar(m.kfs_all);
+  ar(m.kfs_to_marg);
+  ar(m.use_imu);
+}
+
+}  // namespace cereal

include/basalt/utils/keypoints.h

@@ -0,0 +1,104 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <bitset>
+#include <set>
+
+#include <Eigen/Dense>
+#include <sophus/se3.hpp>
+
+#include <basalt/image/image.h>
+#include <basalt/utils/sophus_utils.hpp>
+
+#include <basalt/utils/common_types.h>
+#include <basalt/camera/generic_camera.hpp>
+
+namespace basalt {
+
+typedef std::bitset<256> Descriptor;
+
+void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
+                            KeypointsData& kd, int num_features);
+
+void detectKeypoints(
+    const basalt::Image<const uint16_t>& img_raw, KeypointsData& kd,
+    int PATCH_SIZE = 32, int num_points_cell = 1,
+    const Eigen::aligned_vector<Eigen::Vector2d>& current_points =
+        Eigen::aligned_vector<Eigen::Vector2d>());
+
+void computeAngles(const basalt::Image<const uint16_t>& img_raw,
+                   KeypointsData& kd, bool rotate_features);
+
+void computeDescriptors(const basalt::Image<const uint16_t>& img_raw,
+                        KeypointsData& kd);
+
+void matchDescriptors(const std::vector<std::bitset<256>>& corner_descriptors_1,
+                      const std::vector<std::bitset<256>>& corner_descriptors_2,
+                      std::vector<std::pair<int, int>>& matches, int threshold,
+                      double dist_2_best);
+
+inline void computeEssential(const Sophus::SE3d& T_0_1, Eigen::Matrix4d& E) {
+  E.setZero();
+  const Eigen::Vector3d t_0_1 = T_0_1.translation();
+  const Eigen::Matrix3d R_0_1 = T_0_1.rotationMatrix();
+
+  E.topLeftCorner<3, 3>() = Sophus::SO3d::hat(t_0_1.normalized()) * R_0_1;
+}
+
+inline void findInliersEssential(const KeypointsData& kd1,
+                                 const KeypointsData& kd2,
+                                 const Eigen::Matrix4d& E,
+                                 double epipolar_error_threshold,
+                                 MatchData& md) {
+  md.inliers.clear();
+
+  for (size_t j = 0; j < md.matches.size(); j++) {
+    const Eigen::Vector4d p0_3d = kd1.corners_3d[md.matches[j].first];
+    const Eigen::Vector4d p1_3d = kd2.corners_3d[md.matches[j].second];
+
+    const double epipolar_error = std::abs(p0_3d.transpose() * E * p1_3d);
+
+    if (epipolar_error < epipolar_error_threshold) {
+      md.inliers.push_back(md.matches[j]);
+    }
+  }
+}
+
+void findInliersRansac(const KeypointsData& kd1, const KeypointsData& kd2,
+                       const double ransac_thresh, const int ransac_min_inliers,
+                       MatchData& md);
+
+}  // namespace basalt

include/basalt/utils/nfr.h

@@ -0,0 +1,120 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <sophus/se3.hpp>
+
+#include <basalt/utils/imu_types.h>
+
+namespace basalt {
+
+inline Sophus::Vector6d relPoseError(
+    const Sophus::SE3d& T_i_j, const Sophus::SE3d& T_w_i,
+    const Sophus::SE3d& T_w_j, Sophus::Matrix6d* d_res_d_T_w_i = nullptr,
+    Sophus::Matrix6d* d_res_d_T_w_j = nullptr) {
+  Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i;
+  Sophus::Vector6d res = Sophus::se3_logd(T_i_j * T_j_i);
+
+  if (d_res_d_T_w_i || d_res_d_T_w_j) {
+    Sophus::Matrix6d J;
+    Sophus::rightJacobianInvSE3Decoupled(res, J);
+
+    Eigen::Matrix3d R = T_w_i.so3().inverse().matrix();
+
+    Sophus::Matrix6d Adj;
+    Adj.setZero();
+    Adj.topLeftCorner<3, 3>() = R;
+    Adj.bottomRightCorner<3, 3>() = R;
+
+    if (d_res_d_T_w_i) {
+      *d_res_d_T_w_i = J * Adj;
+    }
+
+    if (d_res_d_T_w_j) {
+      Adj.topRightCorner<3, 3>() =
+          Sophus::SO3d::hat(T_j_i.inverse().translation()) * R;
+      *d_res_d_T_w_j = -J * Adj;
+    }
+  }
+
+  return res;
+}
+
+inline Sophus::Vector3d absPositionError(
+    const Sophus::SE3d& T_w_i, const Eigen::Vector3d pos,
+    Eigen::Matrix<double, 3, 6>* d_res_d_T_w_i = nullptr) {
+  if (d_res_d_T_w_i) {
+    d_res_d_T_w_i->topLeftCorner<3, 3>().setIdentity();
+    d_res_d_T_w_i->topRightCorner<3, 3>().setZero();
+  }
+
+  return T_w_i.translation() - pos;
+}
+
+inline double yawError(const Sophus::SE3d& T_w_i,
+                       const Eigen::Vector3d yaw_dir_body,
+                       Eigen::Matrix<double, 1, 6>* d_res_d_T_w_i = nullptr) {
+  Eigen::Matrix3d curr_R_w_i = T_w_i.so3().matrix();
+  Eigen::Vector3d tmp = curr_R_w_i * yaw_dir_body;
+  double res_yaw = tmp[1];
+
+  if (d_res_d_T_w_i) {
+    d_res_d_T_w_i->setZero();
+    (*d_res_d_T_w_i)[3] = -tmp[2];
+    (*d_res_d_T_w_i)[5] = tmp[0];
+  }
+
+  return res_yaw;
+}
+
+inline Sophus::Vector2d rollPitchError(
+    const Sophus::SE3d& T_w_i, const Sophus::SO3d& R_w_i_meas,
+    Eigen::Matrix<double, 2, 6>* d_res_d_T_w_i = nullptr) {
+  // Assumes g direction is negative Z in world coordinate frame
+
+  Eigen::Matrix3d R = (R_w_i_meas * T_w_i.so3().inverse()).matrix();
+  Eigen::Vector3d res = R * (-Eigen::Vector3d::UnitZ());
+
+  if (d_res_d_T_w_i) {
+    d_res_d_T_w_i->setZero();
+    (*d_res_d_T_w_i)(0, 3) = -R(0, 1);
+    (*d_res_d_T_w_i)(1, 3) = -R(1, 1);
+    (*d_res_d_T_w_i)(0, 4) = R(0, 0);
+    (*d_res_d_T_w_i)(1, 4) = R(1, 0);
+  }
+
+  return res.head<2>();
+}
+}  // namespace basalt

include/basalt/utils/sim_utils.h

@@ -0,0 +1,53 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <basalt/utils/sophus_utils.hpp>
+
+namespace basalt {
+
+struct SimObservations {
+  Eigen::aligned_vector<Eigen::Vector2d> pos;
+  std::vector<int> id;
+};
+
+}  // namespace basalt
+
+namespace cereal {
+template <class Archive>
+void serialize(Archive& ar, basalt::SimObservations& c) {
+  ar(c.pos, c.id);
+}
+}  // namespace cereal

include/basalt/utils/system_utils.h

@@ -0,0 +1,60 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef BASALT_SYSTEM_UTILS_H
+#define BASALT_SYSTEM_UTILS_H
+
+#include <cstdint>
+#include <string>
+
+namespace basalt {
+
+inline std::string ensure_trailing_slash(const std::string& path) {
+  if (!path.empty() && path[path.size() - 1] != '/') {
+    return path + "/";
+  } else {
+    return path;
+  }
+}
+
+struct MemoryInfo {
+  uint64_t resident_memory = 0;       //!< in bytes
+  uint64_t resident_memory_peak = 0;  //!< in bytes
+};
+
+bool get_memory_info(MemoryInfo& info);
+
+}  // namespace basalt
+
+#endif  // include guard

include/basalt/utils/test_utils.h

@@ -0,0 +1,77 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef BASALT_TEST_UTILS_H
+#define BASALT_TEST_UTILS_H
+
+#include <Eigen/Dense>
+
+template <typename Derived1, typename Derived2, typename F>
+void test_jacobian_code(const std::string& name,
+                        const Eigen::MatrixBase<Derived1>& Ja, F func,
+                        const Eigen::MatrixBase<Derived2>& x0,
+                        double eps = 1e-8, double max_norm = 1e-4) {
+  typedef typename Derived1::Scalar Scalar;
+
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Jn = Ja;
+  Jn.setZero();
+
+  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inc = x0;
+  for (int i = 0; i < Jn.cols(); i++) {
+    inc.setZero();
+    inc[i] += eps;
+
+    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fpe = func(x0 + inc);
+    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fme = func(x0 - inc);
+
+    Jn.col(i) = (fpe - fme) / (2 * eps);
+  }
+
+  Scalar diff = (Ja - Jn).norm();
+
+  if (diff > max_norm || !Ja.allFinite()) {
+    std::cerr << name << std::endl;
+    std::cerr << "Numeric Jacobian is different from analytic. Norm difference "
+              << diff << std::endl;
+    std::cerr << "Ja\n" << Ja << std::endl;
+    std::cerr << "Jn\n" << Jn << std::endl;
+  } else {
+    // std::cout << name << std::endl;
+    //    std::cout << "Success" << std::endl;
+    //    std::cout << "Ja\n" << Ja << std::endl;
+    //    std::cout << "Jn\n" << Jn << std::endl;
+  }
+}
+
+#endif  // BASALT_TEST_UTILS_H

include/basalt/utils/time_utils.hpp

@@ -0,0 +1,141 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2020-2021, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <chrono>
+#include <string>
+#include <unordered_map>
+#include <variant>
+#include <vector>
+
+#include <Eigen/Dense>
+
+namespace basalt {
+
+template <class Clock = std::chrono::high_resolution_clock>
+class Timer {
+ public:
+  /// start timer
+  Timer() : start_(Clock::now()) {}
+
+  /// return elapsed time in seconds
+  double elapsed() const {
+    return std::chrono::duration<double>(Clock::now() - start_).count();
+  }
+
+  /// return elapsed time in seconds and reset timer
+  double reset() {
+    const auto now = Clock::now();
+    const double elapsed = std::chrono::duration<double>(now - start_).count();
+    start_ = now;
+    return elapsed;
+  }
+
+ private:
+  std::chrono::time_point<Clock> start_;
+};
+
+template <class Scalar = double>
+struct assign_op {
+  void operator()(Scalar& lhs, const Scalar& rhs) { lhs = rhs; }
+};
+
+template <class Scalar = double>
+struct plus_assign_op {
+  void operator()(Scalar& lhs, const Scalar& rhs) { lhs += rhs; }
+};
+
+template <class T = Timer<>, class Assign = assign_op<>>
+class ScopedTimer {
+ public:
+  explicit ScopedTimer(double& dest) : dest_(dest) {}
+  ~ScopedTimer() { Assign()(dest_, timer_.elapsed()); }
+
+ private:
+  double& dest_;
+  T timer_;
+};
+
+using ScopedTimerAdd = ScopedTimer<Timer<>, plus_assign_op<>>;
+
+template <class F>
+void log_timing(double& time, F fun) {
+  Timer timer;
+  fun();
+  time = timer.elapsed();
+}
+
+template <class F>
+void log_timing_add(double& time, F fun) {
+  Timer timer;
+  fun();
+  time += timer.elapsed();
+}
+
+class ExecutionStats {
+ public:
+  struct Meta {
+    inline Meta& format(const std::string& s) {
+      format_ = s;
+      return *this;
+    }
+
+    // overwrite the meta data from another object
+    void set_meta(const Meta& other) { format_ = other.format_; }
+
+    std::variant<std::vector<double>, std::vector<Eigen::VectorXd>> data_;
+
+    std::string format_;
+  };
+
+  Meta& add(const std::string& name, double value);
+  Meta& add(const std::string& name, const Eigen::VectorXd& value);
+  Meta& add(const std::string& name, const Eigen::VectorXf& value);
+
+  void merge_all(const ExecutionStats& other);
+
+  void merge_sums(const ExecutionStats& other);
+
+  void print() const;
+
+  bool save_json(const std::string& path) const;
+
+ private:
+  std::unordered_map<std::string, Meta> stats_;
+  std::vector<std::string> order_;
+};
+
+}  // namespace basalt

include/basalt/utils/tracks.h

@@ -0,0 +1,222 @@
+// Adapted from OpenMVG
+
+// Copyright (c) 2012, 2013 Pierre MOULON
+//               2018 Nikolaus DEMMEL
+
+// This file was originally part of OpenMVG, an Open Multiple View Geometry C++
+// library.
+
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+// Implementation of [1] an efficient algorithm to compute track from pairwise
+//  correspondences.
+//
+//  [1] Pierre Moulon and Pascal Monasse,
+//    "Unordered feature tracking made fast and easy" CVMP 2012.
+//
+// It tracks the position of features along the series of image from pairwise
+//  correspondences.
+//
+// From map<[imageI,ImageJ], [indexed matches array] > it builds tracks.
+//
+// Usage :
+//  //---------------------------------------
+//  // Compute tracks from matches
+//  //---------------------------------------
+//  TrackBuilder trackBuilder;
+//  FeatureTracks tracks;
+//  trackBuilder.Build(matches); // Build: Efficient fusion of correspondences
+//  trackBuilder.Filter();       // Filter: Remove tracks that have conflict
+//  trackBuilder.Export(tracks); // Export tree to usable data structure
+
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <functional>
+#include <iterator>
+#include <map>
+#include <memory>
+#include <set>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <basalt/utils/common_types.h>
+#include <basalt/utils/union_find.h>
+
+namespace basalt {
+
+/// TrackBuild class creates feature tracks from matches
+struct TrackBuilder {
+  std::map<ImageFeaturePair, TrackId> map_node_to_index;
+  UnionFind uf_tree;
+
+  /// Build tracks for a given series of pairWise matches
+  void Build(const Matches& map_pair_wise_matches) {
+    // 1. We need to know how much single set we will have.
+    //   i.e each set is made of a tuple : (imageIndex, featureIndex)
+    std::set<ImageFeaturePair> allFeatures;
+    // For each couple of images list the used features
+    for (const auto& iter : map_pair_wise_matches) {
+      const auto I = iter.first.first;
+      const auto J = iter.first.second;
+      const MatchData& matchData = iter.second;
+
+      // Retrieve all shared features and add them to a set
+      for (const auto& match : matchData.inliers) {
+        allFeatures.emplace(I, match.first);
+        allFeatures.emplace(J, match.second);
+      }
+    }
+
+    // 2. Build the 'flat' representation where a tuple (the node)
+    //  is attached to a unique index.
+    TrackId cpt = 0;
+    for (const auto& feat : allFeatures) {
+      map_node_to_index.emplace(feat, cpt);
+      ++cpt;
+    }
+    // Clean some memory
+    allFeatures.clear();
+
+    // 3. Add the node and the pairwise correpondences in the UF tree.
+    uf_tree.InitSets(map_node_to_index.size());
+
+    // 4. Union of the matched features corresponding UF tree sets
+    for (const auto& iter : map_pair_wise_matches) {
+      const auto I = iter.first.first;
+      const auto J = iter.first.second;
+      const MatchData& matchData = iter.second;
+      for (const auto& match : matchData.inliers) {
+        const ImageFeaturePair pairI(I, match.first);
+        const ImageFeaturePair pairJ(J, match.second);
+        // Link feature correspondences to the corresponding containing sets.
+        uf_tree.Union(map_node_to_index[pairI], map_node_to_index[pairJ]);
+      }
+    }
+  }
+
+  /// Remove bad tracks (too short or track with ids collision)
+  bool Filter(size_t minimumTrackLength = 2) {
+    // Remove bad tracks:
+    // - track that are too short,
+    // - track with id conflicts:
+    //    i.e. tracks that have many times the same image index
+
+    // From the UF tree, create tracks of the image indexes.
+    //  If an image index appears twice the track must disappear
+    //  If a track is too short it has to be removed.
+    std::map<TrackId, std::set<TimeCamId>> tracks;
+
+    std::set<TrackId> problematic_track_id;
+    // Build tracks from the UF tree, track problematic ids.
+    for (const auto& iter : map_node_to_index) {
+      const TrackId track_id = uf_tree.Find(iter.second);
+      if (problematic_track_id.count(track_id) != 0) {
+        continue;  // Track already marked
+      }
+
+      const ImageFeaturePair& feat = iter.first;
+
+      if (tracks[track_id].count(feat.first)) {
+        problematic_track_id.insert(track_id);
+      } else {
+        tracks[track_id].insert(feat.first);
+      }
+    }
+
+    // - track that are too short,
+    for (const auto& val : tracks) {
+      if (val.second.size() < minimumTrackLength) {
+        problematic_track_id.insert(val.first);
+      }
+    }
+
+    for (uint32_t& root_index : uf_tree.m_cc_parent) {
+      if (problematic_track_id.count(root_index) > 0) {
+        // reset selected root
+        uf_tree.m_cc_size[root_index] = 1;
+        root_index = UnionFind::InvalidIndex();
+      }
+    }
+    return false;
+  }
+
+  /// Return the number of connected set in the UnionFind structure (tree
+  /// forest)
+  size_t TrackCount() const {
+    std::set<TrackId> parent_id(uf_tree.m_cc_parent.begin(),
+                                uf_tree.m_cc_parent.end());
+    // Erase the "special marker" that depicted rejected tracks
+    parent_id.erase(UnionFind::InvalidIndex());
+    return parent_id.size();
+  }
+
+  /// Export tracks as a map (each entry is a map of imageId and
+  /// featureIndex):
+  ///  {TrackIndex => {imageIndex => featureIndex}}
+  void Export(FeatureTracks& tracks) {
+    tracks.clear();
+    for (const auto& iter : map_node_to_index) {
+      const TrackId track_id = uf_tree.Find(iter.second);
+      const ImageFeaturePair& feat = iter.first;
+      // ensure never add rejected elements (track marked as invalid)
+      if (track_id != UnionFind::InvalidIndex()) {
+        tracks[track_id].emplace(feat);
+      }
+    }
+  }
+};
+
+/// Find common tracks between images.
+bool GetTracksInImages(const std::set<TimeCamId>& image_ids,
+                       const FeatureTracks& all_tracks,
+                       std::vector<TrackId>& shared_track_ids) {
+  shared_track_ids.clear();
+
+  // Go along the tracks
+  for (const auto& kv_track : all_tracks) {
+    // Look if the track contains the provided view index & save the point ids
+    size_t observed_image_count = 0;
+    for (const auto& imageId : image_ids) {
+      if (kv_track.second.count(imageId) > 0) {
+        ++observed_image_count;
+      } else {
+        break;
+      }
+    }
+
+    if (observed_image_count == image_ids.size()) {
+      shared_track_ids.push_back(kv_track.first);
+    }
+  }
+  return !shared_track_ids.empty();
+}
+
+/// Find all tracks in an image.
+bool GetTracksInImage(const TimeCamId& image_id,
+                      const FeatureTracks& all_tracks,
+                      std::vector<TrackId>& track_ids) {
+  std::set<TimeCamId> image_set;
+  image_set.insert(image_id);
+  return GetTracksInImages(image_set, all_tracks, track_ids);
+}
+
+/// Find shared tracks between map and image
+bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks,
+                     const Landmarks& landmarks,
+                     std::vector<TrackId>& track_ids) {
+  track_ids.clear();
+  for (const auto& kv : landmarks) {
+    const TrackId trackId = kv.first;
+    if (all_tracks.at(trackId).count(image_id) > 0) {
+      track_ids.push_back(trackId);
+    }
+  }
+  return !track_ids.empty();
+}
+
+}  // namespace basalt

include/basalt/utils/union_find.h

@@ -0,0 +1,94 @@
+// Adapted from OpenMVG
+
+// Copyright (c) 2016 Pierre MOULON
+//               2018 Nikolaus DEMMEL
+
+// This file was originally part of OpenMVG, an Open Multiple View Geometry C++
+// library.
+
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include <cstdint>
+#include <limits>
+#include <numeric>
+#include <vector>
+
+// Union-Find/Disjoint-Set data structure
+//--
+// A disjoint-set data structure also called a union–find data structure
+// or merge–find set, is a data structure that keeps track of a set of elements
+// partitioned into a number of disjoint (non-overlapping) subsets.
+// It supports two operations:
+// - Find: Determine which subset a particular element is in.
+//   - It returns an item from this set that serves as its "representative";
+// - Union: Join two subsets into a single subset.
+// Sometime a Connected method is implemented:
+// - Connected:
+//   - By comparing the result of two Find operations, one can determine whether
+//      two elements are in the same subset.
+//--
+struct UnionFind {
+  using ValueType = uint32_t;
+
+  // Special Value for invalid parent index
+  static ValueType InvalidIndex() {
+    return std::numeric_limits<ValueType>::max();
+  }
+
+  // Represent the DS/UF forest thanks to two array:
+  // A parent 'pointer tree' where each node holds a reference to its parent
+  // node
+  std::vector<ValueType> m_cc_parent;
+  // A rank array used for union by rank
+  std::vector<ValueType> m_cc_rank;
+  // A 'size array' to know the size of each connected component
+  std::vector<ValueType> m_cc_size;
+
+  // Init the UF structure with num_cc nodes
+  void InitSets(const ValueType num_cc) {
+    // all set size are 1 (independent nodes)
+    m_cc_size.resize(num_cc, 1);
+    // Parents id have their own CC id {0,n}
+    m_cc_parent.resize(num_cc);
+    std::iota(m_cc_parent.begin(), m_cc_parent.end(), 0);
+    // Rank array (0)
+    m_cc_rank.resize(num_cc, 0);
+  }
+
+  // Return the number of nodes that have been initialized in the UF tree
+  std::size_t GetNumNodes() const { return m_cc_size.size(); }
+
+  // Return the representative set id of I nth component
+  ValueType Find(ValueType i) {
+    // Recursively set all branch as children of root (Path compression)
+    if (m_cc_parent[i] != i && m_cc_parent[i] != InvalidIndex())
+      m_cc_parent[i] = Find(m_cc_parent[i]);
+    return m_cc_parent[i];
+  }
+
+  // Replace sets containing I and J with their union
+  void Union(ValueType i, ValueType j) {
+    i = Find(i);
+    j = Find(j);
+    if (i == j) {  // Already in the same set. Nothing to do
+      return;
+    }
+
+    // x and y are not already in same set. Merge them.
+    // Perform an union by rank:
+    //  - always attach the smaller tree to the root of the larger tree
+    if (m_cc_rank[i] < m_cc_rank[j]) {
+      m_cc_parent[i] = j;
+      m_cc_size[j] += m_cc_size[i];
+
+    } else {
+      m_cc_parent[j] = i;
+      m_cc_size[i] += m_cc_size[j];
+      if (m_cc_rank[i] == m_cc_rank[j]) ++m_cc_rank[i];
+    }
+  }
+};

include/basalt/utils/vio_config.h

@@ -0,0 +1,111 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <string>
+
+namespace basalt {
+
+enum class LinearizationType { ABS_QR, ABS_SC, REL_SC };
+
+struct VioConfig {
+  VioConfig();
+  void load(const std::string& filename);
+  void save(const std::string& filename);
+
+  std::string optical_flow_type;
+  int optical_flow_detection_grid_size;
+  float optical_flow_max_recovered_dist2;
+  int optical_flow_pattern;
+  int optical_flow_max_iterations;
+  int optical_flow_levels;
+  float optical_flow_epipolar_error;
+  int optical_flow_skip_frames;
+
+  LinearizationType vio_linearization_type;
+  bool vio_sqrt_marg;
+
+  int vio_max_states;
+  int vio_max_kfs;
+  int vio_min_frames_after_kf;
+  float vio_new_kf_keypoints_thresh;
+  bool vio_debug;
+  bool vio_extended_logging;
+
+  //  double vio_outlier_threshold;
+  //  int vio_filter_iteration;
+  int vio_max_iterations;
+
+  double vio_obs_std_dev;
+  double vio_obs_huber_thresh;
+  double vio_min_triangulation_dist;
+
+  bool vio_enforce_realtime;
+
+  bool vio_use_lm;
+  double vio_lm_lambda_initial;
+  double vio_lm_lambda_min;
+  double vio_lm_lambda_max;
+
+  bool vio_scale_jacobian;
+
+  double vio_init_pose_weight;
+  double vio_init_ba_weight;
+  double vio_init_bg_weight;
+
+  bool vio_marg_lost_landmarks;
+  double vio_kf_marg_feature_ratio;
+
+  double mapper_obs_std_dev;
+  double mapper_obs_huber_thresh;
+  int mapper_detection_num_points;
+  double mapper_num_frames_to_match;
+  double mapper_frames_to_match_threshold;
+  double mapper_min_matches;
+  double mapper_ransac_threshold;
+  double mapper_min_track_length;
+  double mapper_max_hamming_distance;
+  double mapper_second_best_test_ratio;
+  int mapper_bow_num_bits;
+  double mapper_min_triangulation_dist;
+  bool mapper_no_factor_weights;
+  bool mapper_use_factors;
+
+  bool mapper_use_lm;
+  double mapper_lm_lambda_min;
+  double mapper_lm_lambda_max;
+};
+
+}  // namespace basalt

include/basalt/utils/vis_utils.h

@@ -0,0 +1,106 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <Eigen/Dense>
+
+#include <pangolin/gl/gldraw.h>
+
+#include <basalt/utils/sophus_utils.hpp>
+
+const uint8_t cam_color[3]{250, 0, 26};
+const uint8_t state_color[3]{250, 0, 26};
+const uint8_t pose_color[3]{0, 50, 255};
+const uint8_t gt_color[3]{0, 171, 47};
+
+inline void render_camera(const Eigen::Matrix4d& T_w_c, float lineWidth,
+                          const uint8_t* color, float sizeFactor) {
+  const float sz = sizeFactor;
+  const float width = 640, height = 480, fx = 500, fy = 500, cx = 320, cy = 240;
+
+  const Eigen::aligned_vector<Eigen::Vector3f> lines = {
+      {0, 0, 0},
+      {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
+      {0, 0, 0},
+      {sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
+      {0, 0, 0},
+      {sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
+      {0, 0, 0},
+      {sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz},
+      {sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz},
+      {sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
+      {sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
+      {sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
+      {sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz},
+      {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
+      {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
+      {sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz}};
+
+  glPushMatrix();
+  glMultMatrixd(T_w_c.data());
+  glColor3ubv(color);
+  glLineWidth(lineWidth);
+  pangolin::glDrawLines(lines);
+  glPopMatrix();
+}
+
+inline void getcolor(float p, float np, float& r, float& g, float& b) {
+  float inc = 4.0 / np;
+  float x = p * inc;
+  r = 0.0f;
+  g = 0.0f;
+  b = 0.0f;
+
+  if ((0 <= x && x <= 1) || (5 <= x && x <= 6))
+    r = 1.0f;
+  else if (4 <= x && x <= 5)
+    r = x - 4;
+  else if (1 <= x && x <= 2)
+    r = 1.0f - (x - 1);
+
+  if (1 <= x && x <= 3)
+    g = 1.0f;
+  else if (0 <= x && x <= 1)
+    g = x - 0;
+  else if (3 <= x && x <= 4)
+    g = 1.0f - (x - 3);
+
+  if (3 <= x && x <= 5)
+    b = 1.0f;
+  else if (2 <= x && x <= 3)
+    b = x - 2;
+  else if (5 <= x && x <= 6)
+    b = 1.0f - (x - 5);
+}