v01

src/calibrate.cpp

@@ -0,0 +1,81 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/calibration/cam_calib.h>
+
+#include <CLI/CLI.hpp>
+
+int main(int argc, char **argv) {
+  std::string dataset_path;
+  std::string dataset_type;
+  std::string aprilgrid_path;
+  std::string result_path;
+  std::vector<std::string> cam_types;
+  std::string cache_dataset_name = "calib-cam";
+  int skip_images = 1;
+
+  CLI::App app{"Calibrate IMU"};
+
+  app.add_option("--dataset-path", dataset_path, "Path to dataset")->required();
+  app.add_option("--result-path", result_path, "Path to result folder")
+      ->required();
+  app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)")
+      ->required();
+
+  app.add_option("--aprilgrid", aprilgrid_path,
+                 "Path to Aprilgrid config file)")
+      ->required();
+
+  app.add_option("--cache-name", cache_dataset_name,
+                 "Name to save cached files");
+
+  app.add_option("--skip-images", skip_images, "Number of images to skip");
+  app.add_option("--cam-types", cam_types,
+                 "Type of cameras (eucm, ds, kb4, pinhole)")
+      ->required();
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError &e) {
+    return app.exit(e);
+  }
+
+  basalt::CamCalib cv(dataset_path, dataset_type, aprilgrid_path, result_path,
+                      cache_dataset_name, skip_images, cam_types);
+
+  cv.renderingLoop();
+
+  return 0;
+}

src/calibrate_imu.cpp

@@ -0,0 +1,95 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/optimization/spline_optimize.h>
+
+#include <basalt/calibration/cam_imu_calib.h>
+
+#include <CLI/CLI.hpp>
+
+int main(int argc, char **argv) {
+  std::string dataset_path;
+  std::string dataset_type;
+  std::string aprilgrid_path;
+  std::string result_path;
+  std::string cache_dataset_name = "calib-cam-imu";
+  int skip_images = 1;
+
+  double accel_noise_std = 0.016;
+  double gyro_noise_std = 0.000282;
+  double accel_bias_std = 0.001;
+  double gyro_bias_std = 0.0001;
+
+  CLI::App app{"Calibrate IMU"};
+
+  app.add_option("--dataset-path", dataset_path, "Path to dataset")->required();
+  app.add_option("--result-path", result_path, "Path to result folder")
+      ->required();
+  app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)")
+      ->required();
+
+  app.add_option("--aprilgrid", aprilgrid_path,
+                 "Path to Aprilgrid config file)")
+      ->required();
+
+  app.add_option("--gyro-noise-std", gyro_noise_std, "Gyroscope noise std");
+  app.add_option("--accel-noise-std", accel_noise_std,
+                 "Accelerometer noise std");
+
+  app.add_option("--gyro-bias-std", gyro_bias_std,
+                 "Gyroscope bias random walk std");
+  app.add_option("--accel-bias-std", accel_bias_std,
+                 "Accelerometer bias random walk std");
+
+  app.add_option("--cache-name", cache_dataset_name,
+                 "Name to save cached files");
+
+  app.add_option("--skip-images", skip_images, "Number of images to skip");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError &e) {
+    return app.exit(e);
+  }
+
+  basalt::CamImuCalib cv(
+      dataset_path, dataset_type, aprilgrid_path, result_path,
+      cache_dataset_name, skip_images,
+      {accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std});
+
+  cv.renderingLoop();
+
+  return 0;
+}

src/calibration/aprilgrid.cpp

@@ -0,0 +1,144 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <fstream>
+
+#include <basalt/calibration/aprilgrid.h>
+
+#include <cereal/archives/json.hpp>
+
+namespace basalt {
+
+AprilGrid::AprilGrid(const std::string &config_path) {
+  std::ifstream is(config_path);
+  if (is.is_open()) {
+    cereal::JSONInputArchive ar(is);
+    ar(cereal::make_nvp("tagCols", tagCols));
+    ar(cereal::make_nvp("tagRows", tagRows));
+    ar(cereal::make_nvp("tagSize", tagSize));
+    ar(cereal::make_nvp("tagSpacing", tagSpacing));
+  } else {
+    std::cerr << "Could not open aprilgrid configuration: " << config_path
+              << std::endl;
+    std::abort();
+  }
+
+  double x_corner_offsets[4] = {0, tagSize, tagSize, 0};
+  double y_corner_offsets[4] = {0, 0, tagSize, tagSize};
+
+  aprilgrid_corner_pos_3d.resize(tagCols * tagRows * 4);
+
+  for (int y = 0; y < tagRows; y++) {
+    for (int x = 0; x < tagCols; x++) {
+      int tag_id = tagCols * y + x;
+      double x_offset = x * tagSize * (1 + tagSpacing);
+      double y_offset = y * tagSize * (1 + tagSpacing);
+
+      for (int i = 0; i < 4; i++) {
+        int corner_id = (tag_id << 2) + i;
+
+        Eigen::Vector4d &pos_3d = aprilgrid_corner_pos_3d[corner_id];
+
+        pos_3d[0] = x_offset + x_corner_offsets[i];
+        pos_3d[1] = y_offset + y_corner_offsets[i];
+        pos_3d[2] = 0;
+        pos_3d[3] = 1;
+      }
+    }
+  }
+
+  int num_vign_points = 5;
+  int num_blocks = tagCols * tagRows * 2;
+
+  aprilgrid_vignette_pos_3d.resize((num_blocks + tagCols + tagRows) *
+                                   num_vign_points);
+
+  for (int k = 0; k < num_vign_points; k++) {
+    for (int i = 0; i < tagCols * tagRows; i++) {
+      // const Eigen::Vector3d p0 = aprilgrid_corner_pos_3d[4 * i + 0];
+      const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1];
+      const Eigen::Vector4d p2 = aprilgrid_corner_pos_3d[4 * i + 2];
+      const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i + 3];
+
+      double coeff = double(k + 1) / double(num_vign_points + 1);
+
+      aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0] =
+          (p1 + coeff * (p2 - p1));
+      aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1] =
+          (p2 + coeff * (p3 - p2));
+
+      aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0][0] +=
+          tagSize * tagSpacing / 2;
+      aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1][1] +=
+          tagSize * tagSpacing / 2;
+    }
+  }
+
+  size_t curr_idx = num_blocks * num_vign_points;
+
+  for (int k = 0; k < num_vign_points; k++) {
+    for (int i = 0; i < tagCols; i++) {
+      const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i + 0];
+      const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1];
+
+      double coeff = double(k + 1) / double(num_vign_points + 1);
+
+      aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] =
+          (p0 + coeff * (p1 - p0));
+
+      aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][1] -=
+          tagSize * tagSpacing / 2;
+    }
+  }
+
+  curr_idx += tagCols * num_vign_points;
+
+  for (int k = 0; k < num_vign_points; k++) {
+    for (int i = 0; i < tagRows; i++) {
+      const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i * tagCols + 0];
+      const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i * tagCols + 3];
+
+      double coeff = double(k + 1) / double(num_vign_points + 1);
+
+      aprilgrid_vignette_pos_3d[curr_idx + k * tagRows + i] =
+          (p0 + coeff * (p3 - p0));
+
+      aprilgrid_vignette_pos_3d[curr_idx + k * tagRows + i][0] -=
+          tagSize * tagSpacing / 2;
+    }
+  }
+}
+
+}  // namespace basalt

src/calibration/calibraiton_helper.cpp

@@ -0,0 +1,466 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/calibration/calibration_helper.h>
+
+#include <basalt/utils/apriltag.h>
+
+#include <tbb/parallel_for.h>
+
+#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
+#include <opengv/absolute_pose/methods.hpp>
+
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <opengv/relative_pose/methods.hpp>
+
+#include <opengv/sac/Ransac.hpp>
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
+#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+#pragma GCC diagnostic pop
+
+#include <opencv2/calib3d/calib3d.hpp>
+
+namespace basalt {
+
+template <class CamT>
+bool estimateTransformation(
+    const CamT &cam_calib,
+    const Eigen::aligned_vector<Eigen::Vector2d> &corners,
+    const std::vector<int> &corner_ids,
+    const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
+    Sophus::SE3d &T_target_camera, size_t &num_inliers) {
+  opengv::bearingVectors_t bearingVectors;
+  opengv::points_t points;
+
+  for (size_t i = 0; i < corners.size(); i++) {
+    Eigen::Vector4d tmp;
+    if (!cam_calib.unproject(corners[i], tmp)) {
+      continue;
+    }
+    Eigen::Vector3d bearing = tmp.head<3>();
+    Eigen::Vector3d point = aprilgrid_corner_pos_3d[corner_ids[i]].head<3>();
+    bearing.normalize();
+
+    bearingVectors.push_back(bearing);
+    points.push_back(point);
+  }
+
+  opengv::absolute_pose::CentralAbsoluteAdapter adapter(bearingVectors, points);
+
+  opengv::sac::Ransac<
+      opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem>
+      ransac;
+  std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem>
+      absposeproblem_ptr(
+          new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(
+              adapter, opengv::sac_problems::absolute_pose::
+                           AbsolutePoseSacProblem::KNEIP));
+  ransac.sac_model_ = absposeproblem_ptr;
+  ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0) * 1 / cam_calib.getParam()[0]));
+  ransac.max_iterations_ = 50;
+
+  ransac.computeModel();
+
+  T_target_camera =
+      Sophus::SE3d(ransac.model_coefficients_.topLeftCorner<3, 3>(),
+                   ransac.model_coefficients_.topRightCorner<3, 1>());
+
+  num_inliers = ransac.inliers_.size();
+
+  return ransac.inliers_.size() > 8;
+}
+
+void CalibHelper::detectCorners(const VioDatasetPtr &vio_data,
+                                CalibCornerMap &calib_corners,
+                                CalibCornerMap &calib_corners_rejected) {
+  calib_corners.clear();
+  calib_corners_rejected.clear();
+
+  tbb::parallel_for(
+      tbb::blocked_range<size_t>(0, vio_data->get_image_timestamps().size()),
+      [&](const tbb::blocked_range<size_t> &r) {
+        ApriltagDetector ad;
+
+        for (size_t j = r.begin(); j != r.end(); ++j) {
+          int64_t timestamp_ns = vio_data->get_image_timestamps()[j];
+          const std::vector<ImageData> &img_vec =
+              vio_data->get_image_data(timestamp_ns);
+
+          for (size_t i = 0; i < img_vec.size(); i++) {
+            if (img_vec[i].img.get()) {
+              CalibCornerData ccd_good;
+              CalibCornerData ccd_bad;
+              ad.detectTags(*img_vec[i].img, ccd_good.corners,
+                            ccd_good.corner_ids, ccd_good.radii,
+                            ccd_bad.corners, ccd_bad.corner_ids, ccd_bad.radii);
+
+              //                std::cout << "image (" << timestamp_ns << ","
+              //                << i
+              //                          << ")  detected " <<
+              //                          ccd_good.corners.size()
+              //                          << "corners (" <<
+              //                          ccd_bad.corners.size()
+              //                          << " rejected)" << std::endl;
+
+              TimeCamId tcid(timestamp_ns, i);
+
+              calib_corners.emplace(tcid, ccd_good);
+              calib_corners_rejected.emplace(tcid, ccd_bad);
+            }
+          }
+        }
+      });
+}
+
+void CalibHelper::initCamPoses(
+    const Calibration<double>::Ptr &calib,
+    const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
+    CalibCornerMap &calib_corners, CalibInitPoseMap &calib_init_poses) {
+  calib_init_poses.clear();
+
+  std::vector<TimeCamId> corners;
+  corners.reserve(calib_corners.size());
+  for (const auto &kv : calib_corners) {
+    corners.emplace_back(kv.first);
+  }
+
+  tbb::parallel_for(tbb::blocked_range<size_t>(0, corners.size()),
+                    [&](const tbb::blocked_range<size_t> &r) {
+                      for (size_t j = r.begin(); j != r.end(); ++j) {
+                        TimeCamId tcid = corners[j];
+                        const CalibCornerData &ccd = calib_corners.at(tcid);
+
+                        CalibInitPoseData cp;
+
+                        computeInitialPose(calib, tcid.cam_id,
+                                           aprilgrid_corner_pos_3d, ccd, cp);
+
+                        calib_init_poses.emplace(tcid, cp);
+                      }
+                    });
+}
+
+bool CalibHelper::initializeIntrinsics(
+    const Eigen::aligned_vector<Eigen::Vector2d> &corners,
+    const std::vector<int> &corner_ids, const AprilGrid &aprilgrid, int cols,
+    int rows, Eigen::Vector4d &init_intr) {
+  // First, initialize the image center at the center of the image.
+
+  Eigen::aligned_map<int, Eigen::Vector2d> id_to_corner;
+  for (size_t i = 0; i < corner_ids.size(); i++) {
+    id_to_corner[corner_ids[i]] = corners[i];
+  }
+
+  const double _xi = 1.0;
+  const double _cu = cols / 2.0 - 0.5;
+  const double _cv = rows / 2.0 - 0.5;
+
+  /// Initialize some temporaries needed.
+  double gamma0 = 0.0;
+  double minReprojErr = std::numeric_limits<double>::max();
+
+  // Now we try to find a non-radial line to initialize the focal length
+  const size_t target_cols = aprilgrid.getTagCols();
+  const size_t target_rows = aprilgrid.getTagRows();
+
+  bool success = false;
+  for (int tag_corner_offset = 0; tag_corner_offset < 2; tag_corner_offset++)
+    for (size_t r = 0; r < target_rows; ++r) {
+      // cv::Mat P(target.cols(); 4, CV_64F);
+
+      Eigen::aligned_vector<Eigen::Vector4d> P;
+
+      for (size_t c = 0; c < target_cols; ++c) {
+        int tag_offset = (r * target_cols + c) << 2;
+
+        for (int i = 0; i < 2; i++) {
+          int corner_id = tag_offset + i + tag_corner_offset * 2;
+
+          // std::cerr << corner_id << " ";
+
+          if (id_to_corner.find(corner_id) != id_to_corner.end()) {
+            const Eigen::Vector2d imagePoint = id_to_corner[corner_id];
+
+            double u = imagePoint[0] - _cu;
+            double v = imagePoint[1] - _cv;
+
+            P.emplace_back(u, v, 0.5, -0.5 * (square(u) + square(v)));
+          }
+        }
+      }
+
+      // std::cerr << std::endl;
+
+      const int MIN_CORNERS = 8;
+      // MIN_CORNERS is an arbitrary threshold for the number of corners
+      if (P.size() > MIN_CORNERS) {
+        // Resize P to fit with the count of valid points.
+
+        Eigen::Map<Eigen::Matrix4Xd> P_mat((double *)P.data(), 4, P.size());
+
+        // std::cerr << "P_mat\n" << P_mat.transpose() << std::endl;
+
+        Eigen::MatrixXd P_mat_t = P_mat.transpose();
+
+        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+            P_mat_t, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+        // std::cerr << "U\n" << svd.matrixU() << std::endl;
+        // std::cerr << "V\n" << svd.matrixV() << std::endl;
+        // std::cerr << "singularValues\n" << svd.singularValues() <<
+        // std::endl;
+
+        Eigen::Vector4d C = svd.matrixV().col(3);
+        // std::cerr << "C\n" << C.transpose() << std::endl;
+        // std::cerr << "P*res\n" << P_mat.transpose() * C << std::endl;
+
+        double t = square(C(0)) + square(C(1)) + C(2) * C(3);
+        if (t < 0) {
+          continue;
+        }
+
+        // check that line image is not radial
+        double d = sqrt(1.0 / t);
+        double nx = C(0) * d;
+        double ny = C(1) * d;
+        if (hypot(nx, ny) > 0.95) {
+          // std::cerr << "hypot(nx, ny) " << hypot(nx, ny) << std::endl;
+          continue;
+        }
+
+        double nz = sqrt(1.0 - square(nx) - square(ny));
+        double gamma = fabs(C(2) * d / nz);
+
+        Eigen::Matrix<double, 5, 1> calib;
+        calib << 0.5 * gamma, 0.5 * gamma, _cu, _cv, 0.5 * _xi;
+        // std::cerr << "gamma " << gamma << std::endl;
+
+        UnifiedCamera<double> cam_calib(calib);
+
+        size_t num_inliers;
+        Sophus::SE3d T_target_camera;
+        if (!estimateTransformation(cam_calib, corners, corner_ids,
+                                    aprilgrid.aprilgrid_corner_pos_3d,
+                                    T_target_camera, num_inliers)) {
+          continue;
+        }
+
+        double reprojErr = 0.0;
+        size_t numReprojected = computeReprojectionError(
+            cam_calib, corners, corner_ids, aprilgrid.aprilgrid_corner_pos_3d,
+            T_target_camera, reprojErr);
+
+        // std::cerr << "numReprojected " << numReprojected << " reprojErr "
+        //          << reprojErr / numReprojected << std::endl;
+
+        if (numReprojected > MIN_CORNERS) {
+          double avgReprojErr = reprojErr / numReprojected;
+
+          if (avgReprojErr < minReprojErr) {
+            minReprojErr = avgReprojErr;
+            gamma0 = gamma;
+            success = true;
+          }
+        }
+
+      }  // If this observation has enough valid corners
+    }    // For each row in the image.
+
+  if (success) init_intr << 0.5 * gamma0, 0.5 * gamma0, _cu, _cv;
+
+  return success;
+}
+
+bool CalibHelper::initializeIntrinsicsPinhole(
+    const std::vector<CalibCornerData *> pinhole_corners,
+    const AprilGrid &aprilgrid, int cols, int rows,
+    Eigen::Vector4d &init_intr) {
+  // First, initialize the image center at the center of the image.
+
+  const double _cu = cols / 2.0 - 0.5;
+  const double _cv = rows / 2.0 - 0.5;
+
+  // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000
+
+  size_t nImages = pinhole_corners.size();
+
+  //  Eigen::MatrixXd A(2 * nImages, 2);
+  //  Eigen::VectorXd b(2 * nImages);
+
+  Eigen::MatrixXd A(nImages * 2, 2);
+  Eigen::VectorXd b(nImages * 2, 1);
+
+  int i = 0;
+
+  for (const CalibCornerData *ccd : pinhole_corners) {
+    const auto &corners = ccd->corners;
+    const auto &corner_ids = ccd->corner_ids;
+
+    std::vector<cv::Point2f> M(corners.size()), imagePoints(corners.size());
+    for (size_t j = 0; j < corners.size(); ++j) {
+      M.at(j) =
+          cv::Point2f(aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]][0],
+                      aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]][1]);
+
+      //    std::cout << "corner "
+      //              <<
+      //              aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]].transpose()
+      //              << std::endl;
+
+      imagePoints.at(j) = cv::Point2f(corners[j][0], corners[j][1]);
+    }
+
+    cv::Mat H = cv::findHomography(M, imagePoints);
+
+    if (H.empty()) return false;
+
+    // std::cout << H << std::endl;
+
+    H.at<double>(0, 0) -= H.at<double>(2, 0) * _cu;
+    H.at<double>(0, 1) -= H.at<double>(2, 1) * _cu;
+    H.at<double>(0, 2) -= H.at<double>(2, 2) * _cu;
+    H.at<double>(1, 0) -= H.at<double>(2, 0) * _cv;
+    H.at<double>(1, 1) -= H.at<double>(2, 1) * _cv;
+    H.at<double>(1, 2) -= H.at<double>(2, 2) * _cv;
+
+    double h[3], v[3], d1[3], d2[3];
+    double n[4] = {0, 0, 0, 0};
+
+    for (int j = 0; j < 3; ++j) {
+      double t0 = H.at<double>(j, 0);
+      double t1 = H.at<double>(j, 1);
+      h[j] = t0;
+      v[j] = t1;
+      d1[j] = (t0 + t1) * 0.5;
+      d2[j] = (t0 - t1) * 0.5;
+      n[0] += t0 * t0;
+      n[1] += t1 * t1;
+      n[2] += d1[j] * d1[j];
+      n[3] += d2[j] * d2[j];
+    }
+
+    for (int j = 0; j < 4; ++j) {
+      n[j] = 1.0 / sqrt(n[j]);
+    }
+
+    for (int j = 0; j < 3; ++j) {
+      h[j] *= n[0];
+      v[j] *= n[1];
+      d1[j] *= n[2];
+      d2[j] *= n[3];
+    }
+
+    A(i * 2, 0) = h[0] * v[0];
+    A(i * 2, 1) = h[1] * v[1];
+    A(i * 2 + 1, 0) = d1[0] * d2[0];
+    A(i * 2 + 1, 1) = d1[1] * d2[1];
+    b(i * 2, 0) = -h[2] * v[2];
+    b(i * 2 + 1, 0) = -d1[2] * d2[2];
+
+    i++;
+  }
+
+  Eigen::Vector2d f = (A.transpose() * A).ldlt().solve(A.transpose() * b);
+
+  double fx = sqrt(fabs(1.0 / f(0)));
+  double fy = sqrt(fabs(1.0 / f(1)));
+
+  init_intr << fx, fy, _cu, _cv;
+
+  return true;
+}
+
+void CalibHelper::computeInitialPose(
+    const Calibration<double>::Ptr &calib, size_t cam_id,
+    const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
+    const CalibCornerData &cd, CalibInitPoseData &cp) {
+  if (cd.corners.size() < 8) {
+    cp.num_inliers = 0;
+    return;
+  }
+
+  bool success;
+  size_t num_inliers;
+
+  std::visit(
+      [&](const auto &cam) {
+        Sophus::SE3d T_target_camera;
+        success = estimateTransformation(cam, cd.corners, cd.corner_ids,
+                                         aprilgrid_corner_pos_3d, cp.T_a_c,
+                                         num_inliers);
+      },
+      calib->intrinsics[cam_id].variant);
+
+  if (success) {
+    Eigen::Matrix4d T_c_a_init = cp.T_a_c.inverse().matrix();
+
+    std::vector<bool> proj_success;
+    calib->intrinsics[cam_id].project(aprilgrid_corner_pos_3d, T_c_a_init,
+                                      cp.reprojected_corners, proj_success);
+
+    cp.num_inliers = num_inliers;
+  } else {
+    cp.num_inliers = 0;
+  }
+}
+
+size_t CalibHelper::computeReprojectionError(
+    const UnifiedCamera<double> &cam_calib,
+    const Eigen::aligned_vector<Eigen::Vector2d> &corners,
+    const std::vector<int> &corner_ids,
+    const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
+    const Sophus::SE3d &T_target_camera, double &error) {
+  size_t num_projected = 0;
+  error = 0;
+
+  Eigen::Matrix4d T_camera_target = T_target_camera.inverse().matrix();
+
+  for (size_t i = 0; i < corners.size(); i++) {
+    Eigen::Vector4d p_cam =
+        T_camera_target * aprilgrid_corner_pos_3d[corner_ids[i]];
+    Eigen::Vector2d res;
+    cam_calib.project(p_cam, res);
+    res -= corners[i];
+
+    num_projected++;
+    error += res.norm();
+  }
+
+  return num_projected;
+}
+}  // namespace basalt

src/calibration/vignette.cpp

@@ -0,0 +1,312 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/calibration/vignette.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+VignetteEstimator::VignetteEstimator(
+    const VioDatasetPtr &vio_dataset,
+    const Eigen::aligned_vector<Eigen::Vector2d> &optical_centers,
+    const Eigen::aligned_vector<Eigen::Vector2i> &resolutions,
+    const std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
+        &reprojected_vignette,
+    const AprilGrid &april_grid)
+    : vio_dataset(vio_dataset),
+      optical_centers(optical_centers),
+      resolutions(resolutions),
+      reprojected_vignette(reprojected_vignette),
+      april_grid(april_grid),
+      vign_param(vio_dataset->get_num_cams(),
+                 RdSpline<1, SPLINE_N>(knot_spacing)) {
+  vign_size = 0;
+
+  for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) {
+    Eigen::Vector2d oc = optical_centers[i];
+
+    size_t new_size = oc.norm() * 1.1;
+    vign_size = std::max(vign_size, new_size);
+  }
+
+  // std::cerr << vign_size << std::endl;
+
+  for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) {
+    while (vign_param[i].maxTimeNs() <
+           int64_t(vign_size) * int64_t(1e9 * 0.7)) {
+      vign_param[i].knotsPushBack(Eigen::Matrix<double, 1, 1>(1));
+    }
+
+    while (vign_param[i].maxTimeNs() < int64_t(vign_size) * int64_t(1e9)) {
+      vign_param[i].knotsPushBack(Eigen::Matrix<double, 1, 1>(0.01));
+    }
+  }
+
+  irradiance.resize(april_grid.aprilgrid_vignette_pos_3d.size());
+  std::fill(irradiance.begin(), irradiance.end(), 1.0);
+}
+
+void VignetteEstimator::compute_error(
+    std::map<TimeCamId, std::vector<double>> *reprojected_vignette_error) {
+  //  double error = 0;
+  //  double mean_residual = 0;
+  double max_residual = 0;
+  int num_residuals = 0;
+
+  TimeCamId tcid_max;
+  // int point_id = 0;
+
+  if (reprojected_vignette_error) reprojected_vignette_error->clear();
+
+  for (const auto &kv : reprojected_vignette) {
+    const TimeCamId &tcid = kv.first;
+    const auto &points_2d_val = kv.second;
+
+    Eigen::Vector2d oc = optical_centers[tcid.cam_id];
+
+    BASALT_ASSERT(points_2d_val.size() ==
+                  april_grid.aprilgrid_vignette_pos_3d.size());
+
+    std::vector<double> ve(april_grid.aprilgrid_vignette_pos_3d.size());
+
+    for (size_t i = 0; i < points_2d_val.size(); i++) {
+      if (points_2d_val[i][2] >= 0) {
+        double val = points_2d_val[i][2];
+        int64_t loc =
+            (points_2d_val[i].head<2>() - oc).norm() * 1e9;  // in pixels * 1e9
+        double e =
+            irradiance[i] * vign_param[tcid.cam_id].evaluate(loc)[0] - val;
+        ve[i] = e;
+        //        error += e * e;
+        //        mean_residual += std::abs(e);
+        max_residual = std::max(max_residual, std::abs(e));
+        if (max_residual == std::abs(e)) {
+          tcid_max = tcid;
+          // point_id = i;
+        }
+        num_residuals++;
+      }
+    }
+
+    if (reprojected_vignette_error)
+      reprojected_vignette_error->emplace(tcid, ve);
+  }
+
+  //  std::cerr << "error " << error << std::endl;
+  //  std::cerr << "mean_residual " << mean_residual / num_residuals <<
+  //  std::endl;
+  //  std::cerr << "max_residual " << max_residual << std::endl;
+
+  // int frame_id = 0;
+  //  for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
+  //    if (tcid_max.first == vio_dataset->get_image_timestamps()[i]) {
+  //      frame_id = i;
+  //    }
+  //  }
+
+  //  std::cerr << "tcid_max " << frame_id << " " << tcid_max.second << " point
+  //  id "
+  //            << point_id << std::endl
+  //            << std::endl;
+}
+
+void VignetteEstimator::opt_irradience() {
+  std::vector<double> new_irradiance(irradiance.size(), 0);
+  std::vector<int> new_irradiance_count(irradiance.size(), 0);
+
+  for (const auto &kv : reprojected_vignette) {
+    const TimeCamId &tcid = kv.first;
+    const auto &points_2d_val = kv.second;
+
+    Eigen::Vector2d oc = optical_centers[tcid.cam_id];
+
+    BASALT_ASSERT(points_2d_val.size() ==
+                  april_grid.aprilgrid_vignette_pos_3d.size());
+
+    for (size_t i = 0; i < points_2d_val.size(); i++) {
+      if (points_2d_val[i][2] >= 0) {
+        double val = points_2d_val[i][2];
+        int64_t loc =
+            (points_2d_val[i].head<2>() - oc).norm() * 1e9;  // in pixels * 1e9
+
+        new_irradiance[i] += val / vign_param[tcid.cam_id].evaluate(loc)[0];
+        new_irradiance_count[i] += 1;
+      }
+    }
+  }
+
+  for (size_t i = 0; i < irradiance.size(); i++) {
+    if (new_irradiance_count[i] > 0)
+      irradiance[i] = new_irradiance[i] / new_irradiance_count[i];
+  }
+}
+
+void VignetteEstimator::opt_vign() {
+  size_t num_knots = vign_param[0].getKnots().size();
+
+  std::vector<std::vector<double>> new_vign_param(
+      vio_dataset->get_num_cams(), std::vector<double>(num_knots, 0));
+  std::vector<std::vector<double>> new_vign_param_count(
+      vio_dataset->get_num_cams(), std::vector<double>(num_knots, 0));
+
+  for (const auto &kv : reprojected_vignette) {
+    const TimeCamId &tcid = kv.first;
+    const auto &points_2d_val = kv.second;
+
+    //      Sophus::SE3d T_w_cam =
+    //          calib_opt->getT_w_i(tcid.first) *
+    //          calib_opt->getCamT_i_c(tcid.second);
+    //      Eigen::Vector3d opt_axis_w = T_w_cam.so3() *
+    //      Eigen::Vector3d::UnitZ();
+    //      if (-opt_axis_w[2] < angle_threshold) continue;
+
+    Eigen::Vector2d oc = optical_centers[tcid.cam_id];
+
+    BASALT_ASSERT(points_2d_val.size() ==
+                  april_grid.aprilgrid_vignette_pos_3d.size());
+
+    for (size_t i = 0; i < points_2d_val.size(); i++) {
+      if (points_2d_val[i][2] >= 0) {
+        double val = points_2d_val[i][2];
+        int64_t loc = (points_2d_val[i].head<2>() - oc).norm() * 1e9;
+
+        RdSpline<1, SPLINE_N>::JacobianStruct J;
+        vign_param[tcid.cam_id].evaluate(loc, &J);
+
+        for (size_t k = 0; k < J.d_val_d_knot.size(); k++) {
+          new_vign_param[tcid.cam_id][J.start_idx + k] +=
+              J.d_val_d_knot[k] * val / irradiance[i];
+          new_vign_param_count[tcid.cam_id][J.start_idx + k] +=
+              J.d_val_d_knot[k];
+        }
+      }
+    }
+  }
+
+  double max_val = 0;
+  for (size_t j = 0; j < vio_dataset->get_num_cams(); j++)
+    for (size_t i = 0; i < num_knots; i++) {
+      if (new_vign_param_count[j][i] > 0) {
+        // std::cerr << "update " << i << " " << j << std::endl;
+        double val = new_vign_param[j][i] / new_vign_param_count[j][i];
+        max_val = std::max(max_val, val);
+        vign_param[j].getKnot(i)[0] = val;
+      }
+    }
+
+  // normalize vignette
+  double max_val_inv = 1.0 / max_val;
+  for (size_t j = 0; j < vio_dataset->get_num_cams(); j++)
+    for (size_t i = 0; i < num_knots; i++) {
+      if (new_vign_param_count[j][i] > 0) {
+        vign_param[j].getKnot(i)[0] *= max_val_inv;
+      }
+    }
+}
+
+void VignetteEstimator::optimize() {
+  compute_error();
+  for (int i = 0; i < 10; i++) {
+    opt_irradience();
+    compute_error();
+    opt_vign();
+    compute_error();
+  }
+}
+
+void VignetteEstimator::compute_data_log(
+    std::vector<std::vector<float>> &vign_data_log) {
+  std::vector<std::vector<double>> num_proj_points(
+      vio_dataset->get_num_cams(), std::vector<double>(vign_size, 0));
+
+  for (const auto &kv : reprojected_vignette) {
+    const TimeCamId &tcid = kv.first;
+    const auto &points_2d = kv.second;
+
+    Eigen::Vector2d oc = optical_centers[tcid.cam_id];
+
+    BASALT_ASSERT(points_2d.size() ==
+                  april_grid.aprilgrid_vignette_pos_3d.size());
+
+    for (size_t i = 0; i < points_2d.size(); i++) {
+      if (points_2d[i][2] >= 0) {
+        size_t loc = (points_2d[i].head<2>() - oc).norm();
+
+        if (loc < vign_size) num_proj_points[tcid.cam_id][loc] += 1.;
+      }
+    }
+  }
+
+  vign_data_log.clear();
+  for (size_t i = 0; i < vign_size; i++) {
+    std::vector<float> log_data;
+    for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) {
+      int64_t loc = i * 1e9;
+      log_data.push_back(vign_param[j].evaluate(loc)[0]);
+      log_data.push_back(num_proj_points[j][i]);
+    }
+    vign_data_log.push_back(log_data);
+  }
+}
+
+void VignetteEstimator::save_vign_png(const std::string &path) {
+  for (size_t k = 0; k < vio_dataset->get_num_cams(); k++) {
+    ManagedImage<uint16_t> vign_img(resolutions[k][0], resolutions[k][1]);
+    vign_img.Fill(0);
+
+    Eigen::Vector2d oc = optical_centers[k];
+
+    for (size_t x = 0; x < vign_img.w; x++) {
+      for (size_t y = 0; y < vign_img.h; y++) {
+        int64_t loc = (Eigen::Vector2d(x, y) - oc).norm() * 1e9;
+        double val = vign_param[k].evaluate(loc)[0];
+        if (val < 0.5) continue;
+        uint16_t val_int =
+            val >= 1.0 ? std::numeric_limits<uint16_t>::max()
+                       : uint16_t(val * std::numeric_limits<uint16_t>::max());
+        vign_img(x, y) = val_int;
+      }
+    }
+
+    //    pangolin::SaveImage(vign_img.UnsafeReinterpret<uint8_t>(),
+    //                        pangolin::PixelFormatFromString("GRAY16LE"),
+    //                        path + "/vingette_" + std::to_string(k) + ".png");
+
+    cv::Mat img(vign_img.h, vign_img.w, CV_16U, vign_img.ptr);
+    cv::imwrite(path + "/vingette_" + std::to_string(k) + ".png", img);
+  }
+}
+}  // namespace basalt

src/device/rs_t265.cpp

@@ -0,0 +1,371 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/device/rs_t265.h>
+
+std::string get_date();
+
+namespace basalt {
+
+RsT265Device::RsT265Device(bool manual_exposure, int skip_frames,
+                           int webp_quality, double exposure_value)
+    : manual_exposure(manual_exposure),
+      skip_frames(skip_frames),
+      webp_quality(webp_quality) {
+  rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
+  pipe = rs2::pipeline(context);
+
+  config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
+  config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
+  config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
+  config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
+  if (!manual_exposure) {
+    config.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
+  }
+
+  if (context.query_devices().size() == 0) {
+    std::cout << "Waiting for device to be connected" << std::endl;
+    rs2::device_hub hub(context);
+    hub.wait_for_device();
+  }
+
+  for (auto& s : context.query_devices()[0].query_sensors()) {
+    std::cout << "Sensor " << s.get_info(RS2_CAMERA_INFO_NAME)
+              << ". Supported options:" << std::endl;
+
+    for (const auto& o : s.get_supported_options()) {
+      std::cout << "\t" << rs2_option_to_string(o) << std::endl;
+    }
+  }
+
+  auto device = context.query_devices()[0];
+  device.hardware_reset();
+
+  std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME)
+            << " connected" << std::endl;
+  sensor = device.query_sensors()[0];
+
+  if (manual_exposure) {
+    std::cout << "Enabling manual exposure control" << std::endl;
+    sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
+    sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000);
+  }
+}
+
+void RsT265Device::start() {
+  auto callback = [&](const rs2::frame& frame) {
+    exportCalibration();
+
+    if (auto fp = frame.as<rs2::motion_frame>()) {
+      auto motion = frame.as<rs2::motion_frame>();
+
+      if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO &&
+          motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) {
+        RsIMUData d;
+        d.timestamp = motion.get_timestamp();
+        d.data << motion.get_motion_data().x, motion.get_motion_data().y,
+            motion.get_motion_data().z;
+
+        gyro_data_queue.emplace_back(d);
+      } else if (motion &&
+                 motion.get_profile().stream_type() == RS2_STREAM_ACCEL &&
+                 motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) {
+        RsIMUData d;
+        d.timestamp = motion.get_timestamp();
+        d.data << motion.get_motion_data().x, motion.get_motion_data().y,
+            motion.get_motion_data().z;
+
+        if (!prev_accel_data.get()) {
+          prev_accel_data.reset(new RsIMUData(d));
+        } else {
+          BASALT_ASSERT(d.timestamp > prev_accel_data->timestamp);
+
+          while (!gyro_data_queue.empty() && gyro_data_queue.front().timestamp <
+                                                 prev_accel_data->timestamp) {
+            std::cout << "Skipping gyro data. Timestamp before the first accel "
+                         "measurement.";
+            gyro_data_queue.pop_front();
+          }
+
+          while (!gyro_data_queue.empty() &&
+                 gyro_data_queue.front().timestamp < d.timestamp) {
+            RsIMUData gyro_data = gyro_data_queue.front();
+            gyro_data_queue.pop_front();
+
+            double w0 = (d.timestamp - gyro_data.timestamp) /
+                        (d.timestamp - prev_accel_data->timestamp);
+
+            double w1 = (gyro_data.timestamp - prev_accel_data->timestamp) /
+                        (d.timestamp - prev_accel_data->timestamp);
+
+            Eigen::Vector3d accel_interpolated =
+                w0 * prev_accel_data->data + w1 * d.data;
+
+            basalt::ImuData<double>::Ptr data;
+            data.reset(new basalt::ImuData<double>);
+            data->t_ns = gyro_data.timestamp * 1e6;
+            data->accel = accel_interpolated;
+            data->gyro = gyro_data.data;
+
+            if (imu_data_queue) imu_data_queue->push(data);
+          }
+
+          prev_accel_data.reset(new RsIMUData(d));
+        }
+      }
+    } else if (auto fs = frame.as<rs2::frameset>()) {
+      BASALT_ASSERT(fs.size() == NUM_CAMS);
+
+      std::vector<rs2::video_frame> vfs;
+      for (int i = 0; i < NUM_CAMS; ++i) {
+        rs2::video_frame vf = fs[i].as<rs2::video_frame>();
+        if (!vf) {
+          std::cout << "Weird Frame, skipping" << std::endl;
+          return;
+        }
+        vfs.push_back(vf);
+      }
+
+      // Callback is called for every new image, so in every other call, the
+      // left frame is updated but the right frame is still from the previous
+      // timestamp. So we only process framesets where both images are valid and
+      // have the same timestamp.
+      for (int i = 1; i < NUM_CAMS; ++i) {
+        if (vfs[0].get_timestamp() != vfs[i].get_timestamp()) {
+          return;
+        }
+      }
+
+      // skip frames if configured
+      if (frame_counter++ % skip_frames != 0) {
+        return;
+      }
+
+      OpticalFlowInput::Ptr data(new OpticalFlowInput);
+      data->img_data.resize(NUM_CAMS);
+
+      //      std::cout << "Reading frame " << frame_counter << std::endl;
+
+      for (int i = 0; i < NUM_CAMS; i++) {
+        const auto& vf = vfs[i];
+
+        int64_t t_ns = vf.get_timestamp() * 1e6;
+
+        // at this stage both image timestamps are expected to be equal
+        BASALT_ASSERT(i == 0 || t_ns == data->t_ns);
+
+        data->t_ns = t_ns;
+
+        data->img_data[i].exposure =
+            vf.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) * 1e-6;
+
+        data->img_data[i].img.reset(new basalt::ManagedImage<uint16_t>(
+            vf.get_width(), vf.get_height()));
+
+        const uint8_t* data_in = (const uint8_t*)vf.get_data();
+        uint16_t* data_out = data->img_data[i].img->ptr;
+
+        size_t full_size = vf.get_width() * vf.get_height();
+        for (size_t j = 0; j < full_size; j++) {
+          int val = data_in[j];
+          val = val << 8;
+          data_out[j] = val;
+        }
+
+        //        std::cout << "Timestamp / exposure " << i << ": " <<
+        //        data->t_ns << " / "
+        //                  << int(data->img_data[i].exposure * 1e3) << "ms" <<
+        //                  std::endl;
+      }
+
+      last_img_data = data;
+      if (image_data_queue) image_data_queue->push(data);
+
+    } else if (auto pf = frame.as<rs2::pose_frame>()) {
+      auto data = pf.get_pose_data();
+
+      RsPoseData pdata;
+      pdata.t_ns = pf.get_timestamp() * 1e6;
+
+      Eigen::Vector3d trans(data.translation.x, data.translation.y,
+                            data.translation.z);
+      Eigen::Quaterniond quat(data.rotation.w, data.rotation.x, data.rotation.y,
+                              data.rotation.z);
+
+      pdata.data = Sophus::SE3d(quat, trans);
+
+      if (pose_data_queue) pose_data_queue->push(pdata);
+    }
+  };
+
+  profile = pipe.start(config, callback);
+}
+
+void RsT265Device::stop() {
+  if (image_data_queue) image_data_queue->push(nullptr);
+  if (imu_data_queue) imu_data_queue->push(nullptr);
+}
+
+bool RsT265Device::setExposure(double exposure) {
+  if (!manual_exposure) return false;
+
+  sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000);
+  return true;
+}
+
+void RsT265Device::setSkipFrames(int skip) { skip_frames = skip; }
+
+void RsT265Device::setWebpQuality(int quality) { webp_quality = quality; }
+
+std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
+  using Scalar = double;
+
+  if (calib.get()) return calib;
+
+  calib.reset(new basalt::Calibration<Scalar>);
+  calib->imu_update_rate = IMU_RATE;
+
+  auto accel_stream = profile.get_stream(RS2_STREAM_ACCEL);
+  auto gyro_stream = profile.get_stream(RS2_STREAM_GYRO);
+  auto cam0_stream = profile.get_stream(RS2_STREAM_FISHEYE, 1);
+  auto cam1_stream = profile.get_stream(RS2_STREAM_FISHEYE, 2);
+
+  // get gyro extrinsics
+  if (auto gyro = gyro_stream.as<rs2::motion_stream_profile>()) {
+    rs2_motion_device_intrinsic intrinsics = gyro.get_motion_intrinsics();
+
+    Eigen::Matrix<Scalar, 12, 1> gyro_bias_full;
+    gyro_bias_full << intrinsics.data[0][3], intrinsics.data[1][3],
+        intrinsics.data[2][3], intrinsics.data[0][0] - 1.0,
+        intrinsics.data[1][0], intrinsics.data[2][0], intrinsics.data[0][1],
+        intrinsics.data[1][1] - 1.0, intrinsics.data[2][1],
+        intrinsics.data[0][2], intrinsics.data[1][2],
+        intrinsics.data[2][2] - 1.0;
+    basalt::CalibGyroBias<Scalar> gyro_bias;
+    gyro_bias.getParam() = gyro_bias_full;
+    calib->calib_gyro_bias = gyro_bias;
+
+    // std::cout << "Gyro Bias\n" << gyro_bias_full << std::endl;
+
+    calib->gyro_noise_std = Eigen::Vector3d(intrinsics.noise_variances[0],
+                                            intrinsics.noise_variances[1],
+                                            intrinsics.noise_variances[2])
+                                .cwiseSqrt();
+
+    calib->gyro_bias_std = Eigen::Vector3d(intrinsics.bias_variances[0],
+                                           intrinsics.bias_variances[1],
+                                           intrinsics.bias_variances[2])
+                               .cwiseSqrt();
+
+    // std::cout << "Gyro noise var: " << intrinsics.noise_variances[0]
+    //          << " bias var: " << intrinsics.bias_variances[0] << std::endl;
+  } else {
+    std::abort();
+  }
+
+  // get accel extrinsics
+  if (auto accel = accel_stream.as<rs2::motion_stream_profile>()) {
+    rs2_motion_device_intrinsic intrinsics = accel.get_motion_intrinsics();
+    Eigen::Matrix<Scalar, 9, 1> accel_bias_full;
+    accel_bias_full << intrinsics.data[0][3], intrinsics.data[1][3],
+        intrinsics.data[2][3], intrinsics.data[0][0] - 1.0,
+        intrinsics.data[1][0], intrinsics.data[2][0],
+        intrinsics.data[1][1] - 1.0, intrinsics.data[2][1],
+        intrinsics.data[2][2] - 1.0;
+    basalt::CalibAccelBias<Scalar> accel_bias;
+    accel_bias.getParam() = accel_bias_full;
+    calib->calib_accel_bias = accel_bias;
+
+    // std::cout << "Gyro Bias\n" << accel_bias_full << std::endl;
+
+    calib->accel_noise_std = Eigen::Vector3d(intrinsics.noise_variances[0],
+                                             intrinsics.noise_variances[1],
+                                             intrinsics.noise_variances[2])
+                                 .cwiseSqrt();
+
+    calib->accel_bias_std = Eigen::Vector3d(intrinsics.bias_variances[0],
+                                            intrinsics.bias_variances[1],
+                                            intrinsics.bias_variances[2])
+                                .cwiseSqrt();
+
+    // std::cout << "Accel noise var: " << intrinsics.noise_variances[0]
+    //          << " bias var: " << intrinsics.bias_variances[0] << std::endl;
+  } else {
+    std::abort();
+  }
+
+  // get camera ex-/intrinsics
+  for (const auto& cam_stream : {cam0_stream, cam1_stream}) {
+    if (auto cam = cam_stream.as<rs2::video_stream_profile>()) {
+      // extrinsics
+      rs2_extrinsics ex = cam.get_extrinsics_to(gyro_stream);
+      Eigen::Matrix3f rot = Eigen::Map<Eigen::Matrix3f>(ex.rotation);
+      Eigen::Vector3f trans = Eigen::Map<Eigen::Vector3f>(ex.translation);
+
+      Eigen::Quaterniond q(rot.cast<double>());
+      basalt::Calibration<Scalar>::SE3 T_i_c(q, trans.cast<double>());
+
+      // std::cout << "T_i_c\n" << T_i_c.matrix() << std::endl;
+
+      calib->T_i_c.push_back(T_i_c);
+
+      // get resolution
+      Eigen::Vector2i resolution;
+      resolution << cam.width(), cam.height();
+      calib->resolution.push_back(resolution);
+
+      // intrinsics
+      rs2_intrinsics intrinsics = cam.get_intrinsics();
+      basalt::KannalaBrandtCamera4<Scalar>::VecN params;
+      params << intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy,
+          intrinsics.coeffs[0], intrinsics.coeffs[1], intrinsics.coeffs[2],
+          intrinsics.coeffs[3];
+
+      // std::cout << "Camera intrinsics: " << params.transpose() << std::endl;
+
+      basalt::GenericCamera<Scalar> camera;
+      basalt::KannalaBrandtCamera4 kannala_brandt(params);
+      camera.variant = kannala_brandt;
+
+      calib->intrinsics.push_back(camera);
+    } else {
+      std::abort();
+    }
+  }
+
+  return calib;
+}
+
+}  // namespace basalt

src/io/dataset_io.cpp

@@ -0,0 +1,62 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/io/dataset_io_euroc.h>
+#include <basalt/io/dataset_io_kitti.h>
+#include <basalt/io/dataset_io_rosbag.h>
+#include <basalt/io/dataset_io_uzh.h>
+
+namespace basalt {
+
+DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo(
+    const std::string &dataset_type, bool load_mocap_as_gt) {
+  if (dataset_type == "euroc") {
+    // return DatasetIoInterfacePtr();
+    return DatasetIoInterfacePtr(new EurocIO(load_mocap_as_gt));
+  } else if (dataset_type == "bag") {
+    return DatasetIoInterfacePtr(new RosbagIO);
+  } else if (dataset_type == "uzh") {
+    return DatasetIoInterfacePtr(new UzhIO);
+  } else if (dataset_type == "kitti") {
+    return DatasetIoInterfacePtr(new KittiIO);
+  } else {
+    std::cerr << "Dataset type " << dataset_type << " is not supported"
+              << std::endl;
+    std::abort();
+  }
+}
+
+}  // namespace basalt

src/io/marg_data_io.cpp

@@ -0,0 +1,226 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/io/marg_data_io.h>
+
+#include <basalt/serialization/headers_serialization.h>
+#include <basalt/utils/filesystem.h>
+
+namespace basalt {
+
+MargDataSaver::MargDataSaver(const std::string& path) {
+  fs::remove_all(path);
+  fs::create_directory(path);
+
+  save_image_queue.set_capacity(300);
+
+  std::string img_path = path + "/images/";
+  fs::create_directory(img_path);
+
+  in_marg_queue.set_capacity(1000);
+
+  auto save_func = [&, path]() {
+    basalt::MargData::Ptr data;
+
+    std::unordered_set<int64_t> processed_opt_flow;
+
+    while (true) {
+      in_marg_queue.pop(data);
+
+      if (data.get()) {
+        int64_t kf_id = *data->kfs_to_marg.begin();
+
+        std::string p = path + "/" + std::to_string(kf_id) + ".cereal";
+        std::ofstream os(p, std::ios::binary);
+
+        {
+          cereal::BinaryOutputArchive archive(os);
+          archive(*data);
+        }
+        os.close();
+
+        for (const auto& d : data->opt_flow_res) {
+          if (processed_opt_flow.count(d->t_ns) == 0) {
+            save_image_queue.push(d);
+            processed_opt_flow.emplace(d->t_ns);
+          }
+        }
+
+      } else {
+        save_image_queue.push(nullptr);
+        break;
+      }
+    }
+
+    std::cout << "Finished MargDataSaver" << std::endl;
+  };
+
+  auto save_image_func = [&, img_path]() {
+    basalt::OpticalFlowResult::Ptr data;
+
+    while (true) {
+      save_image_queue.pop(data);
+
+      if (data.get()) {
+        std::string p = img_path + "/" + std::to_string(data->t_ns) + ".cereal";
+        std::ofstream os(p, std::ios::binary);
+
+        {
+          cereal::BinaryOutputArchive archive(os);
+          archive(data);
+        }
+        os.close();
+      } else {
+        break;
+      }
+    }
+
+    std::cout << "Finished image MargDataSaver" << std::endl;
+  };
+
+  saving_thread.reset(new std::thread(save_func));
+  saving_img_thread.reset(new std::thread(save_image_func));
+}  // namespace basalt
+
+MargDataLoader::MargDataLoader() : out_marg_queue(nullptr) {}
+
+void MargDataLoader::start(const std::string& path) {
+  if (!fs::exists(path))
+    std::cerr << "No marg. data found in " << path << std::endl;
+
+  auto func = [&, path]() {
+    std::string img_path = path + "/images/";
+
+    std::unordered_set<uint64_t> saved_images;
+
+    std::map<int64_t, OpticalFlowResult::Ptr> opt_flow_res;
+
+    for (const auto& entry : fs::directory_iterator(img_path)) {
+      OpticalFlowResult::Ptr data;
+      // std::cout << entry.path() << std::endl;
+      std::ifstream is(entry.path(), std::ios::binary);
+      {
+        cereal::BinaryInputArchive archive(is);
+        archive(data);
+      }
+      is.close();
+      opt_flow_res[data->t_ns] = data;
+    }
+
+    std::map<int64_t, std::string> filenames;
+
+    for (auto& p : fs::directory_iterator(path)) {
+      std::string filename = p.path().filename();
+      if (!std::isdigit(filename[0])) continue;
+
+      size_t lastindex = filename.find_last_of(".");
+      std::string rawname = filename.substr(0, lastindex);
+
+      int64_t t_ns = std::stol(rawname);
+
+      filenames.emplace(t_ns, filename);
+    }
+
+    for (const auto& kv : filenames) {
+      basalt::MargData::Ptr data(new basalt::MargData);
+
+      std::string p = path + "/" + kv.second;
+      std::ifstream is(p, std::ios::binary);
+
+      {
+        cereal::BinaryInputArchive archive(is);
+        archive(*data);
+      }
+      is.close();
+
+      for (const auto& d : data->kfs_all) {
+        data->opt_flow_res.emplace_back(opt_flow_res.at(d));
+      }
+
+      out_marg_queue->push(data);
+    }
+
+    out_marg_queue->push(nullptr);
+
+    std::cout << "Finished MargDataLoader" << std::endl;
+  };
+
+  processing_thread.reset(new std::thread(func));
+}
+}  // namespace basalt
+
+namespace cereal {
+
+template <class Archive, class T>
+void save(Archive& ar, const basalt::ManagedImage<T>& m) {
+  ar(m.w);
+  ar(m.h);
+  ar(cereal::binary_data(m.ptr, sizeof(T) * m.w * m.h));
+}
+
+template <class Archive, class T>
+void load(Archive& ar, basalt::ManagedImage<T>& m) {
+  size_t w;
+  size_t h;
+  ar(w);
+  ar(h);
+  m.Reinitialise(w, h);
+  ar(cereal::binary_data(m.ptr, sizeof(T) * m.w * m.h));
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::OpticalFlowResult& m) {
+  ar(m.t_ns);
+  ar(m.observations);
+  ar(m.input_images);
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::OpticalFlowInput& m) {
+  ar(m.t_ns);
+  ar(m.img_data);
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::ImageData& m) {
+  ar(m.exposure);
+  ar(m.img);
+}
+
+template <class Archive>
+static void serialize(Archive& ar, Eigen::AffineCompact2f& m) {
+  ar(m.matrix());
+}
+}  // namespace cereal

src/kitti_eval.cpp

@@ -0,0 +1,215 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/utils/sophus_utils.hpp>
+
+#include <cereal/archives/json.hpp>
+#include <cereal/types/string.hpp>
+
+namespace cereal {
+
+template <class Archive, class T, class C, class A>
+inline void save(Archive& ar, std::map<std::string, T, C, A> const& map) {
+  for (const auto& i : map) ar(cereal::make_nvp(i.first, i.second));
+}
+
+template <class Archive, class T, class C, class A>
+inline void load(Archive& ar, std::map<std::string, T, C, A>& map) {
+  map.clear();
+
+  auto hint = map.begin();
+  while (true) {
+    const auto namePtr = ar.getNodeName();
+
+    if (!namePtr) break;
+
+    std::string key = namePtr;
+    T value;
+    ar(value);
+    hint = map.emplace_hint(hint, std::move(key), std::move(value));
+  }
+}
+
+}  // namespace cereal
+
+Eigen::aligned_vector<Sophus::SE3d> load_poses(const std::string& path) {
+  Eigen::aligned_vector<Sophus::SE3d> res;
+
+  std::ifstream f(path);
+  std::string line;
+  while (std::getline(f, line)) {
+    if (line[0] == '#') continue;
+
+    std::stringstream ss(line);
+
+    Eigen::Matrix3d rot;
+    Eigen::Vector3d pos;
+
+    ss >> rot(0, 0) >> rot(0, 1) >> rot(0, 2) >> pos[0] >> rot(1, 0) >>
+        rot(1, 1) >> rot(1, 2) >> pos[1] >> rot(2, 0) >> rot(2, 1) >>
+        rot(2, 2) >> pos[2];
+
+    res.emplace_back(Eigen::Quaterniond(rot), pos);
+  }
+
+  return res;
+}
+
+void eval_kitti(const std::vector<double>& lengths,
+                const Eigen::aligned_vector<Sophus::SE3d>& poses_gt,
+                const Eigen::aligned_vector<Sophus::SE3d>& poses_result,
+                std::map<std::string, std::map<std::string, double>>& res) {
+  auto lastFrameFromSegmentLength = [](std::vector<float>& dist,
+                                       int first_frame, float len) {
+    for (int i = first_frame; i < (int)dist.size(); i++)
+      if (dist[i] > dist[first_frame] + len) return i;
+    return -1;
+  };
+
+  std::cout << "poses_gt.size() " << poses_gt.size() << std::endl;
+  std::cout << "poses_result.size() " << poses_result.size() << std::endl;
+
+  // pre-compute distances (from ground truth as reference)
+  std::vector<float> dist_gt;
+  dist_gt.emplace_back(0);
+  for (size_t i = 1; i < poses_gt.size(); i++) {
+    const auto& p1 = poses_gt[i - 1];
+    const auto& p2 = poses_gt[i];
+
+    dist_gt.emplace_back(dist_gt.back() +
+                         (p2.translation() - p1.translation()).norm());
+  }
+
+  const size_t step_size = 10;
+
+  for (size_t i = 0; i < lengths.size(); i++) {
+    // current length
+    float len = lengths[i];
+
+    double t_error_sum = 0;
+    double r_error_sum = 0;
+    int num_meas = 0;
+
+    for (size_t first_frame = 0; first_frame < poses_gt.size();
+         first_frame += step_size) {
+      // for all segment lengths do
+
+      // compute last frame
+      int32_t last_frame =
+          lastFrameFromSegmentLength(dist_gt, first_frame, len);
+
+      // continue, if sequence not long enough
+      if (last_frame == -1) continue;
+
+      // compute rotational and translational errors
+      Sophus::SE3d pose_delta_gt =
+          poses_gt[first_frame].inverse() * poses_gt[last_frame];
+      Sophus::SE3d pose_delta_result =
+          poses_result[first_frame].inverse() * poses_result[last_frame];
+      // Sophus::SE3d pose_error = pose_delta_result.inverse() * pose_delta_gt;
+      double r_err = pose_delta_result.unit_quaternion().angularDistance(
+                         pose_delta_gt.unit_quaternion()) *
+                     180.0 / M_PI;
+      double t_err =
+          (pose_delta_result.translation() - pose_delta_gt.translation())
+              .norm();
+
+      t_error_sum += t_err / len;
+      r_error_sum += r_err / len;
+      num_meas++;
+    }
+
+    std::string len_str = std::to_string((int)len);
+    res[len_str]["trans_error"] = 100.0 * t_error_sum / num_meas;
+    res[len_str]["rot_error"] = r_error_sum / num_meas;
+    res[len_str]["num_meas"] = num_meas;
+  }
+}
+
+int main(int argc, char** argv) {
+  std::vector<double> lengths = {100, 200, 300, 400, 500, 600, 700, 800};
+  std::string result_path;
+  std::string traj_path;
+  std::string gt_path;
+
+  CLI::App app{"KITTI evaluation"};
+
+  app.add_option("--traj-path", traj_path,
+                 "Path to the file with computed trajectory.")
+      ->required();
+  app.add_option("--gt-path", gt_path,
+                 "Path to the file with ground truth trajectory.")
+      ->required();
+  app.add_option("--result-path", result_path, "Path to store the result file.")
+      ->required();
+
+  app.add_option("--eval-lengths", lengths, "Trajectory length to evaluate.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  const Eigen::aligned_vector<Sophus::SE3d> poses_gt = load_poses(gt_path);
+  const Eigen::aligned_vector<Sophus::SE3d> poses_result =
+      load_poses(traj_path);
+
+  if (poses_gt.empty() || poses_gt.size() != poses_result.size()) {
+    std::cerr << "Wrong number of poses: poses_gt " << poses_gt.size()
+              << " poses_result " << poses_result.size() << std::endl;
+    std::abort();
+  }
+
+  std::map<std::string, std::map<std::string, double>> res_map;
+  eval_kitti(lengths, poses_gt, poses_result, res_map);
+
+  {
+    cereal::JSONOutputArchive ar(std::cout);
+    ar(cereal::make_nvp("results", res_map));
+    std::cout << std::endl;
+  }
+
+  if (!result_path.empty()) {
+    std::ofstream os(result_path);
+    {
+      cereal::JSONOutputArchive ar(os);
+      ar(cereal::make_nvp("results", res_map));
+    }
+    os.close();
+  }
+}

src/linearization/landmark_block.cpp

@@ -0,0 +1,63 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/landmark_block.hpp>
+#include <basalt/linearization/landmark_block_abs_dynamic.hpp>
+
+namespace basalt {
+
+template <typename Scalar>
+template <int POSE_SIZE>
+std::unique_ptr<LandmarkBlock<Scalar>>
+LandmarkBlock<Scalar>::createLandmarkBlock() {
+  return std::make_unique<LandmarkBlockAbsDynamic<Scalar, POSE_SIZE>>();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate factory templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template std::unique_ptr<LandmarkBlock<double>>
+LandmarkBlock<double>::createLandmarkBlock<6>();
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template std::unique_ptr<LandmarkBlock<float>>
+LandmarkBlock<float>::createLandmarkBlock<6>();
+#endif
+
+}  // namespace basalt

src/linearization/linearization_abs_qr.cpp

@@ -0,0 +1,711 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_abs_qr.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+#include <basalt/linearization/imu_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsQR<Scalar, POSE_SIZE>::LinearizationAbsQR(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg)
+    : options_(options),
+      estimator(estimator),
+      lmdb_(estimator->lmdb),
+      frame_poses(estimator->frame_poses),
+      calib(estimator->calib),
+      aom(aom),
+      used_frames(used_frames),
+      marg_lin_data(marg_lin_data),
+      imu_lin_data(imu_lin_data),
+      pose_damping_diagonal(0),
+      pose_damping_diagonal_sqrt(0) {
+  UNUSED(last_state_to_marg);
+
+  BASALT_ASSERT_STREAM(
+      options.lb_options.huber_parameter == estimator->huber_thresh,
+      "Huber threshold should be set to the same value");
+
+  BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
+                       "obs_std_dev should be set to the same value");
+
+  // Allocate memory for relative pose linearization
+  for (const auto& [tcid_h, target_map] : lmdb_.getObservations()) {
+    // if (used_frames && used_frames->count(tcid_h.frame_id) == 0) continue;
+    const size_t host_idx = host_to_idx_.size();
+    host_to_idx_.try_emplace(tcid_h, host_idx);
+    host_to_landmark_block.try_emplace(tcid_h);
+
+    // assumption: every host frame has at least target frame with
+    // observations
+    // NOTE: in case a host frame loses all of its landmarks due
+    // to outlier removal or marginalization of other frames, it becomes quite
+    // useless and is expected to be removed before optimization.
+    BASALT_ASSERT(!target_map.empty());
+
+    for (const auto& [tcid_t, obs] : target_map) {
+      // assumption: every target frame has at least one observation
+      BASALT_ASSERT(!obs.empty());
+
+      std::pair<TimeCamId, TimeCamId> key(tcid_h, tcid_t);
+      relative_pose_lin.emplace(key, RelPoseLin<Scalar>());
+    }
+  }
+
+  // Populate lookup for relative poses grouped by host-frame
+  for (const auto& [tcid_h, target_map] : lmdb_.getObservations()) {
+    // if (used_frames && used_frames->count(tcid_h.frame_id) == 0) continue;
+    relative_pose_per_host.emplace_back();
+
+    for (const auto& [tcid_t, _] : target_map) {
+      std::pair<TimeCamId, TimeCamId> key(tcid_h, tcid_t);
+      auto it = relative_pose_lin.find(key);
+
+      BASALT_ASSERT(it != relative_pose_lin.end());
+
+      relative_pose_per_host.back().emplace_back(it);
+    }
+  }
+
+  num_cameras = frame_poses.size();
+
+  landmark_ids.clear();
+  for (const auto& [k, v] : lmdb_.getLandmarks()) {
+    if (used_frames || lost_landmarks) {
+      if (used_frames && used_frames->count(v.host_kf_id.frame_id)) {
+        landmark_ids.emplace_back(k);
+      } else if (lost_landmarks && lost_landmarks->count(k)) {
+        landmark_ids.emplace_back(k);
+      }
+    } else {
+      landmark_ids.emplace_back(k);
+    }
+  }
+  size_t num_landmakrs = landmark_ids.size();
+
+  // std::cout << "num_landmakrs " << num_landmakrs << std::endl;
+
+  landmark_blocks.resize(num_landmakrs);
+
+  {
+    auto body = [&](const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        KeypointId lm_id = landmark_ids[r];
+        auto& lb = landmark_blocks[r];
+        auto& landmark = lmdb_.getLandmark(lm_id);
+
+        lb = LandmarkBlock<Scalar>::template createLandmarkBlock<POSE_SIZE>();
+
+        lb->allocateLandmark(landmark, relative_pose_lin, calib, aom,
+                             options.lb_options);
+      }
+    };
+
+    tbb::blocked_range<size_t> range(0, num_landmakrs);
+    tbb::parallel_for(range, body);
+  }
+
+  landmark_block_idx.reserve(num_landmakrs);
+
+  num_rows_Q2r = 0;
+  for (size_t i = 0; i < num_landmakrs; i++) {
+    landmark_block_idx.emplace_back(num_rows_Q2r);
+    const auto& lb = landmark_blocks[i];
+    num_rows_Q2r += lb->numQ2rows();
+
+    host_to_landmark_block.at(lb->getHostKf()).emplace_back(lb.get());
+  }
+
+  if (imu_lin_data) {
+    for (const auto& kv : imu_lin_data->imu_meas) {
+      imu_blocks.emplace_back(
+          new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
+    }
+  }
+
+  //    std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
+  //    num_cameras
+  //              << std::endl;
+}
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsQR<Scalar, POSE_SIZE>::~LinearizationAbsQR() = default;
+
+template <typename Scalar_, int POSE_SIZE_>
+void LinearizationAbsQR<Scalar_, POSE_SIZE_>::log_problem_stats(
+    ExecutionStats& stats) const {
+  UNUSED(stats);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsQR<Scalar, POSE_SIZE>::linearizeProblem(
+    bool* numerically_valid) {
+  // reset damping and scaling (might be set from previous iteration)
+  pose_damping_diagonal = 0;
+  pose_damping_diagonal_sqrt = 0;
+  marg_scaling = VecX();
+
+  // Linearize relative poses
+  for (const auto& [tcid_h, target_map] : lmdb_.getObservations()) {
+    // if (used_frames && used_frames->count(tcid_h.frame_id) == 0) continue;
+
+    for (const auto& [tcid_t, _] : target_map) {
+      std::pair<TimeCamId, TimeCamId> key(tcid_h, tcid_t);
+      RelPoseLin<Scalar>& rpl = relative_pose_lin.at(key);
+
+      if (tcid_h != tcid_t) {
+        const PoseStateWithLin<Scalar>& state_h =
+            estimator->getPoseStateWithLin(tcid_h.frame_id);
+        const PoseStateWithLin<Scalar>& state_t =
+            estimator->getPoseStateWithLin(tcid_t.frame_id);
+
+        // compute relative pose & Jacobians at linearization point
+        Sophus::SE3<Scalar> T_t_h_sophus =
+            computeRelPose(state_h.getPoseLin(), calib.T_i_c[tcid_h.cam_id],
+                           state_t.getPoseLin(), calib.T_i_c[tcid_t.cam_id],
+                           &rpl.d_rel_d_h, &rpl.d_rel_d_t);
+
+        // if either state is already linearized, then the current state
+        // estimate is different from the linearization point, so recompute
+        // the value (not Jacobian) again based on the current state.
+        if (state_h.isLinearized() || state_t.isLinearized()) {
+          T_t_h_sophus =
+              computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
+                             state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
+        }
+
+        rpl.T_t_h = T_t_h_sophus.matrix();
+      } else {
+        rpl.T_t_h.setIdentity();
+        rpl.d_rel_d_h.setZero();
+        rpl.d_rel_d_t.setZero();
+      }
+    }
+  }
+
+  // Linearize landmarks
+  size_t num_landmarks = landmark_blocks.size();
+
+  auto body = [&](const tbb::blocked_range<size_t>& range,
+                  std::pair<Scalar, bool> error_valid) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      error_valid.first += landmark_blocks[r]->linearizeLandmark();
+      error_valid.second =
+          error_valid.second && !landmark_blocks[r]->isNumericalFailure();
+    }
+    return error_valid;
+  };
+
+  std::pair<Scalar, bool> initial_value = {0.0, true};
+  auto join = [](auto p1, auto p2) {
+    p1.first += p2.first;
+    p1.second = p1.second && p2.second;
+    return p1;
+  };
+
+  tbb::blocked_range<size_t> range(0, num_landmarks);
+  auto reduction_res = tbb::parallel_reduce(range, initial_value, body, join);
+
+  if (numerically_valid) *numerically_valid = reduction_res.second;
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      reduction_res.first += imu_block->linearizeImu(estimator->frame_states);
+    }
+  }
+
+  if (marg_lin_data) {
+    Scalar marg_prior_error;
+    estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
+    reduction_res.first += marg_prior_error;
+  }
+
+  return reduction_res.first;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::performQR() {
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->performQR();
+    }
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::setPoseDamping(
+    const Scalar lambda) {
+  BASALT_ASSERT(lambda >= 0);
+
+  pose_damping_diagonal = lambda;
+  pose_damping_diagonal_sqrt = std::sqrt(lambda);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsQR<Scalar, POSE_SIZE>::backSubstitute(
+    const VecX& pose_inc) {
+  BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
+
+  auto body = [&](const tbb::blocked_range<size_t>& range, Scalar l_diff) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->backSubstitute(pose_inc, l_diff);
+    }
+    return l_diff;
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  Scalar l_diff =
+      tbb::parallel_reduce(range, Scalar(0), body, std::plus<Scalar>());
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->backSubstitute(pose_inc, l_diff);
+    }
+  }
+
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    VecX pose_inc_marg = pose_inc.head(marg_size);
+
+    l_diff += estimator->computeMargPriorModelCostChange(
+        *marg_lin_data, marg_scaling, pose_inc_marg);
+  }
+
+  return l_diff;
+}
+
+template <typename Scalar, int POSE_SIZE>
+typename LinearizationAbsQR<Scalar, POSE_SIZE>::VecX
+LinearizationAbsQR<Scalar, POSE_SIZE>::getJp_diag2() const {
+  // TODO: group relative by host frame
+
+  struct Reductor {
+    Reductor(size_t num_rows,
+             const std::vector<LandmarkBlockPtr>& landmark_blocks)
+        : num_rows_(num_rows), landmark_blocks_(landmark_blocks) {
+      res_.setZero(num_rows);
+    }
+
+    void operator()(const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        const auto& lb = landmark_blocks_[r];
+        lb->addJp_diag2(res_);
+      }
+    }
+
+    Reductor(Reductor& a, tbb::split)
+        : num_rows_(a.num_rows_), landmark_blocks_(a.landmark_blocks_) {
+      res_.setZero(num_rows_);
+    };
+
+    inline void join(const Reductor& b) { res_ += b.res_; }
+
+    size_t num_rows_;
+    const std::vector<LandmarkBlockPtr>& landmark_blocks_;
+    VecX res_;
+  };
+
+  Reductor r(aom.total_size, landmark_blocks);
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_reduce(range, r);
+  // r(range);
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->addJp_diag2(r.res_);
+    }
+  }
+
+  // TODO: We don't include pose damping here, b/c we use this to compute
+  // jacobian scale. Make sure this is clear in the the usage, possibly rename
+  // to reflect this, or add assert such that it fails when pose damping is
+  // set.
+
+  // Note: ignore damping here
+
+  // Add marginalization prior
+  // Asumes marginalization part is in the head. Check for this is located
+  // outside
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    if (marg_scaling.rows() > 0) {
+      r.res_.head(marg_size) += (marg_lin_data->H * marg_scaling.asDiagonal())
+                                    .colwise()
+                                    .squaredNorm();
+    } else {
+      r.res_.head(marg_size) += marg_lin_data->H.colwise().squaredNorm();
+    }
+  }
+
+  return r.res_;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::scaleJl_cols() {
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->scaleJl_cols();
+    }
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::scaleJp_cols(
+    const VecX& jacobian_scaling) {
+  //    auto body = [&](const tbb::blocked_range<size_t>& range) {
+  //      for (size_t r = range.begin(); r != range.end(); ++r) {
+  //        landmark_blocks[r]->scaleJp_cols(jacobian_scaling);
+  //      }
+  //    };
+
+  //    tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  //    tbb::parallel_for(range, body);
+
+  if (true) {
+    // In case of absolute poses, we scale Jp in the LMB.
+
+    auto body = [&](const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        landmark_blocks[r]->scaleJp_cols(jacobian_scaling);
+      }
+    };
+
+    tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+    tbb::parallel_for(range, body);
+  } else {
+    // In case LMB use relative poses we cannot directly scale the relative pose
+    // Jacobians. We have
+    //
+    //     Jp * diag(S) = Jp_rel * J_rel_to_abs * diag(S)
+    //
+    // so instead we scale the rel-to-abs jacobians.
+    //
+    // Note that since we do perform operations like J^T * J on the relative
+    // pose Jacobians, we should maybe consider additional scaling like
+    //
+    //     (Jp_rel * diag(S_rel)) * (diag(S_rel)^-1 * J_rel_to_abs * diag(S)),
+    //
+    // but this might be only relevant if we do something more sensitive like
+    // also include camera intrinsics in the optimization.
+
+    for (auto& [k, v] : relative_pose_lin) {
+      size_t h_idx = aom.abs_order_map.at(k.first.frame_id).first;
+      size_t t_idx = aom.abs_order_map.at(k.second.frame_id).first;
+
+      v.d_rel_d_h *=
+          jacobian_scaling.template segment<POSE_SIZE>(h_idx).asDiagonal();
+
+      v.d_rel_d_t *=
+          jacobian_scaling.template segment<POSE_SIZE>(t_idx).asDiagonal();
+    }
+  }
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->scaleJp_cols(jacobian_scaling);
+    }
+  }
+
+  // Add marginalization scaling
+  if (marg_lin_data) {
+    // We are only supposed to apply the scaling once.
+    BASALT_ASSERT(marg_scaling.size() == 0);
+
+    size_t marg_size = marg_lin_data->H.cols();
+    marg_scaling = jacobian_scaling.head(marg_size);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      landmark_blocks[r]->setLandmarkDamping(lambda);
+    }
+  };
+
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
+    MatX& Q2Jp, VecX& Q2r) const {
+  size_t total_size = num_rows_Q2r;
+  size_t poses_size = aom.total_size;
+
+  size_t lm_start_idx = 0;
+
+  // Space for IMU data if present
+  size_t imu_start_idx = total_size;
+  if (imu_lin_data) {
+    total_size += imu_lin_data->imu_meas.size() * POSE_VEL_BIAS_SIZE;
+  }
+
+  // Space for damping if present
+  size_t damping_start_idx = total_size;
+  if (hasPoseDamping()) {
+    total_size += poses_size;
+  }
+
+  // Space for marg-prior if present
+  size_t marg_start_idx = total_size;
+  if (marg_lin_data) total_size += marg_lin_data->H.rows();
+
+  Q2Jp.setZero(total_size, poses_size);
+  Q2r.setZero(total_size);
+
+  auto body = [&](const tbb::blocked_range<size_t>& range) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      const auto& lb = landmark_blocks[r];
+      lb->get_dense_Q2Jp_Q2r(Q2Jp, Q2r, lm_start_idx + landmark_block_idx[r]);
+    }
+  };
+
+  // go over all host frames
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_for(range, body);
+  // body(range);
+
+  if (imu_lin_data) {
+    size_t start_idx = imu_start_idx;
+    for (const auto& imu_block : imu_blocks) {
+      imu_block->add_dense_Q2Jp_Q2r(Q2Jp, Q2r, start_idx);
+      start_idx += POSE_VEL_BIAS_SIZE;
+    }
+  }
+
+  // Add damping
+  get_dense_Q2Jp_Q2r_pose_damping(Q2Jp, damping_start_idx);
+
+  // Add marginalization
+  get_dense_Q2Jp_Q2r_marg_prior(Q2Jp, Q2r, marg_start_idx);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
+                                                          VecX& b) const {
+  struct Reductor {
+    Reductor(size_t opt_size,
+             const std::vector<LandmarkBlockPtr>& landmark_blocks)
+        : opt_size_(opt_size), landmark_blocks_(landmark_blocks) {
+      H_.setZero(opt_size_, opt_size_);
+      b_.setZero(opt_size_);
+    }
+
+    void operator()(const tbb::blocked_range<size_t>& range) {
+      for (size_t r = range.begin(); r != range.end(); ++r) {
+        auto& lb = landmark_blocks_[r];
+        lb->add_dense_H_b(H_, b_);
+      }
+    }
+
+    Reductor(Reductor& a, tbb::split)
+        : opt_size_(a.opt_size_), landmark_blocks_(a.landmark_blocks_) {
+      H_.setZero(opt_size_, opt_size_);
+      b_.setZero(opt_size_);
+    };
+
+    inline void join(Reductor& b) {
+      H_ += b.H_;
+      b_ += b.b_;
+    }
+
+    size_t opt_size_;
+    const std::vector<LandmarkBlockPtr>& landmark_blocks_;
+
+    MatX H_;
+    VecX b_;
+  };
+
+  size_t opt_size = aom.total_size;
+
+  Reductor r(opt_size, landmark_blocks);
+
+  // go over all host frames
+  tbb::blocked_range<size_t> range(0, landmark_block_idx.size());
+  tbb::parallel_reduce(range, r);
+
+  // Add imu
+  add_dense_H_b_imu(r.H_, r.b_);
+
+  // Add damping
+  add_dense_H_b_pose_damping(r.H_);
+
+  // Add marginalization
+  add_dense_H_b_marg_prior(r.H_, r.b_);
+
+  H = std::move(r.H_);
+  b = std::move(r.b_);
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
+    MatX& Q2Jp, size_t start_idx) const {
+  size_t poses_size = num_cameras * POSE_SIZE;
+  if (hasPoseDamping()) {
+    Q2Jp.template block(start_idx, 0, poses_size, poses_size)
+        .diagonal()
+        .array() = pose_damping_diagonal_sqrt;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
+    MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
+  if (!marg_lin_data) return;
+
+  BASALT_ASSERT(marg_lin_data->is_sqrt);
+
+  size_t marg_rows = marg_lin_data->H.rows();
+  size_t marg_cols = marg_lin_data->H.cols();
+
+  VecX delta;
+  estimator->computeDelta(marg_lin_data->order, delta);
+
+  if (marg_scaling.rows() > 0) {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
+        marg_lin_data->H * marg_scaling.asDiagonal();
+  } else {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
+  }
+
+  Q2r.template segment(start_idx, marg_rows) =
+      marg_lin_data->H * delta + marg_lin_data->b;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
+    MatX& H) const {
+  if (hasPoseDamping()) {
+    H.diagonal().array() += pose_damping_diagonal;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
+    MatX& H, VecX& b) const {
+  if (!marg_lin_data) return;
+
+  // Scaling not supported ATM
+  BASALT_ASSERT(marg_scaling.rows() == 0);
+
+  Scalar marg_prior_error;
+  estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
+
+  //  size_t marg_size = marg_lin_data->H.cols();
+
+  //  VecX delta;
+  //  estimator->computeDelta(marg_lin_data->order, delta);
+
+  //  if (marg_scaling.rows() > 0) {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
+  //        marg_lin_data->H * marg_scaling.asDiagonal();
+
+  //    b.head(marg_size) += marg_scaling.asDiagonal() *
+  //                         marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+
+  //  } else {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_lin_data->H.transpose() * marg_lin_data->H;
+
+  //    b.head(marg_size) += marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+  //  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_imu(
+    DenseAccumulator<Scalar>& accum) const {
+  if (!imu_lin_data) return;
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsQR<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
+                                                              VecX& b) const {
+  if (!imu_lin_data) return;
+
+  // workaround: create an accumulator here, to avoid implementing the
+  // add_dense_H_b(H, b) overload in ImuBlock
+  DenseAccumulator<Scalar> accum;
+  accum.reset(b.size());
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+
+  H += accum.getH();
+  b += accum.getB();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template class LinearizationAbsQR<double, 6>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template class LinearizationAbsQR<float, 6>;
+#endif
+
+}  // namespace basalt

src/linearization/linearization_abs_sc.cpp

@@ -0,0 +1,432 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_abs_sc.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+#include <basalt/vi_estimator/sc_ba_base.h>
+#include <basalt/linearization/imu_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsSC<Scalar, POSE_SIZE>::LinearizationAbsSC(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg)
+    : options_(options),
+      estimator(estimator),
+      lmdb_(estimator->lmdb),
+      calib(estimator->calib),
+      aom(aom),
+      used_frames(used_frames),
+      marg_lin_data(marg_lin_data),
+      imu_lin_data(imu_lin_data),
+      lost_landmarks(lost_landmarks),
+      last_state_to_marg(last_state_to_marg),
+      pose_damping_diagonal(0),
+      pose_damping_diagonal_sqrt(0) {
+  BASALT_ASSERT_STREAM(
+      options.lb_options.huber_parameter == estimator->huber_thresh,
+      "Huber threshold should be set to the same value");
+
+  BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
+                       "obs_std_dev should be set to the same value");
+
+  if (imu_lin_data) {
+    for (const auto& kv : imu_lin_data->imu_meas) {
+      imu_blocks.emplace_back(
+          new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
+    }
+  }
+
+  //    std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
+  //    num_cameras
+  //              << std::endl;
+}
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationAbsSC<Scalar, POSE_SIZE>::~LinearizationAbsSC() = default;
+
+template <typename Scalar_, int POSE_SIZE_>
+void LinearizationAbsSC<Scalar_, POSE_SIZE_>::log_problem_stats(
+    ExecutionStats& stats) const {
+  UNUSED(stats);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsSC<Scalar, POSE_SIZE>::linearizeProblem(
+    bool* numerically_valid) {
+  // reset damping and scaling (might be set from previous iteration)
+  pose_damping_diagonal = 0;
+  pose_damping_diagonal_sqrt = 0;
+  marg_scaling = VecX();
+
+  std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
+      obs_to_lin;
+
+  if (used_frames) {
+    const auto& obs = lmdb_.getObservations();
+
+    // select all observations where the host frame is about to be
+    // marginalized
+
+    if (lost_landmarks) {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        } else {
+          std::map<TimeCamId, std::set<KeypointId>> lost_obs_map;
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            for (const auto& lm_id : it2->second) {
+              if (lost_landmarks->count(lm_id)) {
+                if (it2->first.frame_id <= last_state_to_marg)
+                  lost_obs_map[it2->first].emplace(lm_id);
+              }
+            }
+          }
+          if (!lost_obs_map.empty()) {
+            obs_to_lin[it->first] = lost_obs_map;
+          }
+        }
+      }
+
+    } else {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        }
+      }
+    }
+  } else {
+    obs_to_lin = lmdb_.getObservations();
+  }
+
+  Scalar error;
+
+  ScBundleAdjustmentBase<Scalar>::linearizeHelperAbsStatic(ald_vec, obs_to_lin,
+                                                           estimator, error);
+
+  // TODO: Fix the computation of numarically valid points
+  if (numerically_valid) *numerically_valid = true;
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      error += imu_block->linearizeImu(estimator->frame_states);
+    }
+  }
+
+  if (marg_lin_data) {
+    Scalar marg_prior_error;
+    estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
+    error += marg_prior_error;
+  }
+
+  return error;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::performQR() {}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::setPoseDamping(
+    const Scalar lambda) {
+  BASALT_ASSERT(lambda >= 0);
+
+  pose_damping_diagonal = lambda;
+  pose_damping_diagonal_sqrt = std::sqrt(lambda);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationAbsSC<Scalar, POSE_SIZE>::backSubstitute(
+    const VecX& pose_inc) {
+  BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
+
+  // Update points
+  tbb::blocked_range<size_t> keys_range(0, ald_vec.size());
+  auto update_points_func = [&](const tbb::blocked_range<size_t>& r,
+                                Scalar l_diff) {
+    for (size_t i = r.begin(); i != r.end(); ++i) {
+      const auto& ald = ald_vec[i];
+      ScBundleAdjustmentBase<Scalar>::updatePointsAbs(aom, ald, -pose_inc,
+                                                      lmdb_, &l_diff);
+    }
+
+    return l_diff;
+  };
+  Scalar l_diff = tbb::parallel_reduce(keys_range, Scalar(0),
+                                       update_points_func, std::plus<Scalar>());
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->backSubstitute(pose_inc, l_diff);
+    }
+  }
+
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    VecX pose_inc_marg = pose_inc.head(marg_size);
+
+    l_diff += estimator->computeMargPriorModelCostChange(
+        *marg_lin_data, marg_scaling, pose_inc_marg);
+  }
+
+  return l_diff;
+}
+
+template <typename Scalar, int POSE_SIZE>
+typename LinearizationAbsSC<Scalar, POSE_SIZE>::VecX
+LinearizationAbsSC<Scalar, POSE_SIZE>::getJp_diag2() const {
+  // TODO: group relative by host frame
+
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::scaleJl_cols() {
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::scaleJp_cols(
+    const VecX& jacobian_scaling) {
+  UNUSED(jacobian_scaling);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
+  UNUSED(lambda);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
+    MatX& Q2Jp, VecX& Q2r) const {
+  MatX H;
+  VecX b;
+  get_dense_H_b(H, b);
+
+  Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+
+  VecX D_sqrt = ldlt.vectorD().array().max(0).sqrt().matrix();
+
+  // After LDLT, we have
+  //     marg_H == P^T*L*D*L^T*P,
+  // so we compute the square root as
+  //     marg_sqrt_H = sqrt(D)*L^T*P,
+  // such that
+  //     marg_sqrt_H^T marg_sqrt_H == marg_H.
+  Q2Jp.setIdentity(H.rows(), H.cols());
+  Q2Jp = ldlt.transpositionsP() * Q2Jp;
+  Q2Jp = ldlt.matrixU() * Q2Jp;  // U == L^T
+  Q2Jp = D_sqrt.asDiagonal() * Q2Jp;
+
+  // For the right hand side, we want
+  //     marg_b == marg_sqrt_H^T * marg_sqrt_b
+  // so we compute
+  //     marg_sqrt_b = (marg_sqrt_H^T)^-1 * marg_b
+  //                 = (P^T*L*sqrt(D))^-1 * marg_b
+  //                 = sqrt(D)^-1 * L^-1 * P * marg_b
+  Q2r = ldlt.transpositionsP() * b;
+  ldlt.matrixL().solveInPlace(Q2r);
+
+  // We already clamped negative values in D_sqrt to 0 above, but for values
+  // close to 0 we set b to 0.
+  for (int i = 0; i < Q2r.size(); ++i) {
+    if (D_sqrt(i) > std::sqrt(std::numeric_limits<Scalar>::min()))
+      Q2r(i) /= D_sqrt(i);
+    else
+      Q2r(i) = 0;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
+                                                          VecX& b) const {
+  typename ScBundleAdjustmentBase<Scalar>::template LinearizeAbsReduce2<
+      DenseAccumulator<Scalar>>
+      lopt_abs(aom);
+
+  tbb::blocked_range<typename Eigen::aligned_vector<AbsLinData>::const_iterator>
+      range(ald_vec.cbegin(), ald_vec.cend());
+  tbb::parallel_reduce(range, lopt_abs);
+  // lopt_abs(range);
+
+  // Add imu
+  add_dense_H_b_imu(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  // Add damping
+  add_dense_H_b_pose_damping(lopt_abs.accum.getH());
+
+  // Add marginalization
+  add_dense_H_b_marg_prior(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  H = std::move(lopt_abs.accum.getH());
+  b = std::move(lopt_abs.accum.getB());
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
+    MatX& Q2Jp, size_t start_idx) const {
+  UNUSED(Q2Jp);
+  UNUSED(start_idx);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
+    MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
+  if (!marg_lin_data) return;
+
+  size_t marg_rows = marg_lin_data->H.rows();
+  size_t marg_cols = marg_lin_data->H.cols();
+
+  VecX delta;
+  estimator->computeDelta(marg_lin_data->order, delta);
+
+  if (marg_scaling.rows() > 0) {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
+        marg_lin_data->H * marg_scaling.asDiagonal();
+  } else {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
+  }
+
+  Q2r.template segment(start_idx, marg_rows) =
+      marg_lin_data->H * delta + marg_lin_data->b;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
+    MatX& H) const {
+  if (hasPoseDamping()) {
+    H.diagonal().array() += pose_damping_diagonal;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
+    MatX& H, VecX& b) const {
+  if (!marg_lin_data) return;
+
+  BASALT_ASSERT(marg_scaling.rows() == 0);
+
+  Scalar marg_prior_error;
+  estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
+
+  //  size_t marg_size = marg_lin_data->H.cols();
+
+  //  VecX delta;
+  //  estimator->computeDelta(marg_lin_data->order, delta);
+
+  // Scaling not supported ATM
+
+  //  if (marg_scaling.rows() > 0) {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
+  //        marg_lin_data->H * marg_scaling.asDiagonal();
+
+  //    b.head(marg_size) += marg_scaling.asDiagonal() *
+  //                         marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+
+  //  } else {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_lin_data->H.transpose() * marg_lin_data->H;
+
+  //    b.head(marg_size) += marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+  //  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(
+    DenseAccumulator<Scalar>& accum) const {
+  if (!imu_lin_data) return;
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
+                                                              VecX& b) const {
+  if (!imu_lin_data) return;
+
+  // workaround: create an accumulator here, to avoid implementing the
+  // add_dense_H_b(H, b) overload in ImuBlock
+  DenseAccumulator<Scalar> accum;
+  accum.reset(b.size());
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+
+  H += accum.getH();
+  b += accum.getB();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template class LinearizationAbsSC<double, 6>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template class LinearizationAbsSC<float, 6>;
+#endif
+
+}  // namespace basalt

src/linearization/linearization_base.cpp

@@ -0,0 +1,119 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_abs_qr.hpp>
+#include <basalt/linearization/linearization_abs_sc.hpp>
+#include <basalt/linearization/linearization_base.hpp>
+#include <basalt/linearization/linearization_rel_sc.hpp>
+
+#include <magic_enum.hpp>
+
+namespace basalt {
+
+bool isLinearizationSqrt(const LinearizationType& type) {
+  switch (type) {
+    case LinearizationType::ABS_QR:
+      return true;
+    case LinearizationType::ABS_SC:
+    case LinearizationType::REL_SC:
+      return false;
+    default:
+      BASALT_ASSERT_STREAM(false, "Linearization type is not supported.");
+      return false;
+  }
+}
+
+template <typename Scalar_, int POSE_SIZE_>
+std::unique_ptr<LinearizationBase<Scalar_, POSE_SIZE_>>
+LinearizationBase<Scalar_, POSE_SIZE_>::create(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg) {
+  //  std::cout << "Creaing Linearization of type "
+  //            << magic_enum::enum_name(options.linearization_type) <<
+  //            std::endl;
+
+  switch (options.linearization_type) {
+    case LinearizationType::ABS_QR:
+      return std::make_unique<LinearizationAbsQR<Scalar, POSE_SIZE>>(
+          estimator, aom, options, marg_lin_data, imu_lin_data, used_frames,
+          lost_landmarks, last_state_to_marg);
+
+    case LinearizationType::ABS_SC:
+      return std::make_unique<LinearizationAbsSC<Scalar, POSE_SIZE>>(
+          estimator, aom, options, marg_lin_data, imu_lin_data, used_frames,
+          lost_landmarks, last_state_to_marg);
+
+    case LinearizationType::REL_SC:
+      return std::make_unique<LinearizationRelSC<Scalar, POSE_SIZE>>(
+          estimator, aom, options, marg_lin_data, imu_lin_data, used_frames,
+          lost_landmarks, last_state_to_marg);
+
+    default:
+      std::cerr << "Could not select a valid linearization." << std::endl;
+      std::abort();
+  }
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate factory templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template std::unique_ptr<LinearizationBase<double, 6>>
+LinearizationBase<double, 6>::create(
+    BundleAdjustmentBase<double>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<double>* marg_lin_data,
+    const ImuLinData<double>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg);
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template std::unique_ptr<LinearizationBase<float, 6>>
+LinearizationBase<float, 6>::create(
+    BundleAdjustmentBase<float>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<float>* marg_lin_data,
+    const ImuLinData<float>* imu_lin_data, const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg);
+#endif
+
+}  // namespace basalt

src/linearization/linearization_rel_sc.cpp

@@ -0,0 +1,431 @@
+/**
+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.
+*/
+
+#include <basalt/linearization/linearization_rel_sc.hpp>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+#include <basalt/vi_estimator/sc_ba_base.h>
+#include <basalt/linearization/imu_block.hpp>
+#include <basalt/utils/cast_utils.hpp>
+
+namespace basalt {
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationRelSC<Scalar, POSE_SIZE>::LinearizationRelSC(
+    BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
+    const Options& options, const MargLinData<Scalar>* marg_lin_data,
+    const ImuLinData<Scalar>* imu_lin_data,
+    const std::set<FrameId>* used_frames,
+    const std::unordered_set<KeypointId>* lost_landmarks,
+    int64_t last_state_to_marg)
+    : options_(options),
+      estimator(estimator),
+      lmdb_(estimator->lmdb),
+      calib(estimator->calib),
+      aom(aom),
+      used_frames(used_frames),
+      marg_lin_data(marg_lin_data),
+      imu_lin_data(imu_lin_data),
+      lost_landmarks(lost_landmarks),
+      last_state_to_marg(last_state_to_marg),
+      pose_damping_diagonal(0),
+      pose_damping_diagonal_sqrt(0) {
+  BASALT_ASSERT_STREAM(
+      options.lb_options.huber_parameter == estimator->huber_thresh,
+      "Huber threshold should be set to the same value");
+
+  BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
+                       "obs_std_dev should be set to the same value");
+
+  if (imu_lin_data) {
+    for (const auto& kv : imu_lin_data->imu_meas) {
+      imu_blocks.emplace_back(
+          new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
+    }
+  }
+
+  //    std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
+  //    num_cameras
+  //              << std::endl;
+}
+
+template <typename Scalar, int POSE_SIZE>
+LinearizationRelSC<Scalar, POSE_SIZE>::~LinearizationRelSC() = default;
+
+template <typename Scalar_, int POSE_SIZE_>
+void LinearizationRelSC<Scalar_, POSE_SIZE_>::log_problem_stats(
+    ExecutionStats& stats) const {
+  UNUSED(stats);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationRelSC<Scalar, POSE_SIZE>::linearizeProblem(
+    bool* numerically_valid) {
+  // reset damping and scaling (might be set from previous iteration)
+  pose_damping_diagonal = 0;
+  pose_damping_diagonal_sqrt = 0;
+  marg_scaling = VecX();
+
+  std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
+      obs_to_lin;
+
+  if (used_frames) {
+    const auto& obs = lmdb_.getObservations();
+
+    // select all observations where the host frame is about to be
+    // marginalized
+
+    if (lost_landmarks) {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        } else {
+          std::map<TimeCamId, std::set<KeypointId>> lost_obs_map;
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            for (const auto& lm_id : it2->second) {
+              if (lost_landmarks->count(lm_id)) {
+                if (it2->first.frame_id <= last_state_to_marg)
+                  lost_obs_map[it2->first].emplace(lm_id);
+              }
+            }
+          }
+          if (!lost_obs_map.empty()) {
+            obs_to_lin[it->first] = lost_obs_map;
+          }
+        }
+      }
+
+    } else {
+      for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
+        if (used_frames->count(it->first.frame_id) > 0) {
+          for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
+               ++it2) {
+            // TODO: Check how ABS_QR works without last_state_to_marg
+            if (it2->first.frame_id <= last_state_to_marg)
+              obs_to_lin[it->first].emplace(*it2);
+          }
+        }
+      }
+    }
+  } else {
+    obs_to_lin = lmdb_.getObservations();
+  }
+
+  Scalar error;
+
+  ScBundleAdjustmentBase<Scalar>::linearizeHelperStatic(rld_vec, obs_to_lin,
+                                                        estimator, error);
+
+  // TODO: Fix the computation of numerically valid points
+  if (numerically_valid) *numerically_valid = true;
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      error += imu_block->linearizeImu(estimator->frame_states);
+    }
+  }
+
+  if (marg_lin_data) {
+    Scalar marg_prior_error;
+    estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
+    error += marg_prior_error;
+  }
+
+  return error;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::performQR() {}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::setPoseDamping(
+    const Scalar lambda) {
+  BASALT_ASSERT(lambda >= 0);
+
+  pose_damping_diagonal = lambda;
+  pose_damping_diagonal_sqrt = std::sqrt(lambda);
+}
+
+template <typename Scalar, int POSE_SIZE>
+Scalar LinearizationRelSC<Scalar, POSE_SIZE>::backSubstitute(
+    const VecX& pose_inc) {
+  BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
+
+  // Update points
+  tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+  auto update_points_func = [&](const tbb::blocked_range<size_t>& r,
+                                Scalar l_diff) {
+    for (size_t i = r.begin(); i != r.end(); ++i) {
+      const auto& rld = rld_vec[i];
+      ScBundleAdjustmentBase<Scalar>::updatePoints(aom, rld, -pose_inc, lmdb_,
+                                                   &l_diff);
+    }
+
+    return l_diff;
+  };
+  Scalar l_diff = tbb::parallel_reduce(keys_range, Scalar(0),
+                                       update_points_func, std::plus<Scalar>());
+
+  if (imu_lin_data) {
+    for (auto& imu_block : imu_blocks) {
+      imu_block->backSubstitute(pose_inc, l_diff);
+    }
+  }
+
+  if (marg_lin_data) {
+    size_t marg_size = marg_lin_data->H.cols();
+    VecX pose_inc_marg = pose_inc.head(marg_size);
+
+    l_diff += estimator->computeMargPriorModelCostChange(
+        *marg_lin_data, marg_scaling, pose_inc_marg);
+  }
+
+  return l_diff;
+}
+
+template <typename Scalar, int POSE_SIZE>
+typename LinearizationRelSC<Scalar, POSE_SIZE>::VecX
+LinearizationRelSC<Scalar, POSE_SIZE>::getJp_diag2() const {
+  // TODO: group relative by host frame
+
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::scaleJl_cols() {
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::scaleJp_cols(
+    const VecX& jacobian_scaling) {
+  UNUSED(jacobian_scaling);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
+  UNUSED(lambda);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
+    MatX& Q2Jp, VecX& Q2r) const {
+  MatX H;
+  VecX b;
+  get_dense_H_b(H, b);
+
+  Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+
+  VecX D_sqrt = ldlt.vectorD().array().max(0).sqrt().matrix();
+
+  // After LDLT, we have
+  //     marg_H == P^T*L*D*L^T*P,
+  // so we compute the square root as
+  //     marg_sqrt_H = sqrt(D)*L^T*P,
+  // such that
+  //     marg_sqrt_H^T marg_sqrt_H == marg_H.
+  Q2Jp.setIdentity(H.rows(), H.cols());
+  Q2Jp = ldlt.transpositionsP() * Q2Jp;
+  Q2Jp = ldlt.matrixU() * Q2Jp;  // U == L^T
+  Q2Jp = D_sqrt.asDiagonal() * Q2Jp;
+
+  // For the right hand side, we want
+  //     marg_b == marg_sqrt_H^T * marg_sqrt_b
+  // so we compute
+  //     marg_sqrt_b = (marg_sqrt_H^T)^-1 * marg_b
+  //                 = (P^T*L*sqrt(D))^-1 * marg_b
+  //                 = sqrt(D)^-1 * L^-1 * P * marg_b
+  Q2r = ldlt.transpositionsP() * b;
+  ldlt.matrixL().solveInPlace(Q2r);
+
+  // We already clamped negative values in D_sqrt to 0 above, but for values
+  // close to 0 we set b to 0.
+  for (int i = 0; i < Q2r.size(); ++i) {
+    if (D_sqrt(i) > std::sqrt(std::numeric_limits<Scalar>::min()))
+      Q2r(i) /= D_sqrt(i);
+    else
+      Q2r(i) = 0;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
+                                                          VecX& b) const {
+  typename ScBundleAdjustmentBase<Scalar>::template LinearizeAbsReduce<
+      DenseAccumulator<Scalar>>
+      lopt_abs(aom);
+
+  tbb::blocked_range<typename Eigen::aligned_vector<RelLinData>::const_iterator>
+      range(rld_vec.cbegin(), rld_vec.cend());
+  tbb::parallel_reduce(range, lopt_abs);
+
+  // Add imu
+  add_dense_H_b_imu(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  // Add damping
+  add_dense_H_b_pose_damping(lopt_abs.accum.getH());
+
+  // Add marginalization
+  add_dense_H_b_marg_prior(lopt_abs.accum.getH(), lopt_abs.accum.getB());
+
+  H = std::move(lopt_abs.accum.getH());
+  b = std::move(lopt_abs.accum.getB());
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
+    MatX& Q2Jp, size_t start_idx) const {
+  UNUSED(Q2Jp);
+  UNUSED(start_idx);
+  BASALT_ASSERT_STREAM(false, "Not implemented");
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
+    MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
+  if (!marg_lin_data) return;
+
+  size_t marg_rows = marg_lin_data->H.rows();
+  size_t marg_cols = marg_lin_data->H.cols();
+
+  VecX delta;
+  estimator->computeDelta(marg_lin_data->order, delta);
+
+  if (marg_scaling.rows() > 0) {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
+        marg_lin_data->H * marg_scaling.asDiagonal();
+  } else {
+    Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
+  }
+
+  Q2r.template segment(start_idx, marg_rows) =
+      marg_lin_data->H * delta + marg_lin_data->b;
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
+    MatX& H) const {
+  if (hasPoseDamping()) {
+    H.diagonal().array() += pose_damping_diagonal;
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
+    MatX& H, VecX& b) const {
+  if (!marg_lin_data) return;
+
+  // Scaling not supported ATM
+  BASALT_ASSERT(marg_scaling.rows() == 0);
+
+  Scalar marg_prior_error;
+  estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
+
+  //  size_t marg_size = marg_lin_data->H.cols();
+
+  //  VecX delta;
+  //  estimator->computeDelta(marg_lin_data->order, delta);
+
+  //  if (marg_scaling.rows() > 0) {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
+  //        marg_lin_data->H * marg_scaling.asDiagonal();
+
+  //    b.head(marg_size) += marg_scaling.asDiagonal() *
+  //                         marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+
+  //  } else {
+  //    H.topLeftCorner(marg_size, marg_size) +=
+  //        marg_lin_data->H.transpose() * marg_lin_data->H;
+
+  //    b.head(marg_size) += marg_lin_data->H.transpose() *
+  //                         (marg_lin_data->H * delta + marg_lin_data->b);
+  //  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(
+    DenseAccumulator<Scalar>& accum) const {
+  if (!imu_lin_data) return;
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+}
+
+template <typename Scalar, int POSE_SIZE>
+void LinearizationRelSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
+                                                              VecX& b) const {
+  if (!imu_lin_data) return;
+
+  // workaround: create an accumulator here, to avoid implementing the
+  // add_dense_H_b(H, b) overload in ImuBlock
+  DenseAccumulator<Scalar> accum;
+  accum.reset(b.size());
+
+  for (const auto& imu_block : imu_blocks) {
+    imu_block->add_dense_H_b(accum);
+  }
+
+  H += accum.getH();
+  b += accum.getB();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+// Scalar=double, POSE_SIZE=6
+template class LinearizationRelSC<double, 6>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+// Scalar=float, POSE_SIZE=6
+template class LinearizationRelSC<float, 6>;
+#endif
+
+}  // namespace basalt

src/mapper.cpp

@@ -0,0 +1,722 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <sophus/se3.hpp>
+
+#include <tbb/concurrent_unordered_map.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/io/marg_data_io.h>
+#include <basalt/optimization/accumulator.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/utils/filesystem.h>
+#include <basalt/utils/imu_types.h>
+#include <basalt/utils/nfr.h>
+#include <basalt/utils/vis_utils.h>
+#include <basalt/vi_estimator/nfr_mapper.h>
+#include <basalt/vi_estimator/vio_estimator.h>
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/serialization/headers_serialization.h>
+
+using basalt::POSE_SIZE;
+using basalt::POSE_VEL_BIAS_SIZE;
+
+Eigen::Vector3d g(0, 0, -9.81);
+
+const Eigen::aligned_vector<Eigen::Vector2i> image_resolutions = {{752, 480},
+                                                                  {752, 480}};
+
+basalt::VioConfig vio_config;
+basalt::NfrMapper::Ptr nrf_mapper;
+
+Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i;
+std::vector<int64_t> gt_frame_t_ns, image_t_ns;
+
+Eigen::aligned_vector<Eigen::Vector3d> mapper_points;
+std::vector<int> mapper_point_ids;
+
+std::map<int64_t, basalt::MargData::Ptr> marg_data;
+
+Eigen::aligned_vector<Eigen::Vector3d> edges_vis;
+Eigen::aligned_vector<Eigen::Vector3d> roll_pitch_vis;
+Eigen::aligned_vector<Eigen::Vector3d> rel_edges_vis;
+
+void draw_image_overlay(pangolin::View& v, size_t cam_id);
+void draw_scene();
+void load_data(const std::string& calib_path,
+               const std::string& marg_data_path);
+void processMargData(basalt::MargData& m);
+void extractNonlinearFactors(basalt::MargData& m);
+void computeEdgeVis();
+void optimize();
+void randomInc();
+void randomYawInc();
+void compute_error();
+double alignButton();
+void detect();
+void match();
+void tracks();
+void optimize();
+void filter();
+void saveTrajectoryButton();
+
+constexpr int UI_WIDTH = 200;
+
+basalt::Calibration<double> calib;
+
+pangolin::Var<int> show_frame1("ui.show_frame1", 0, 0, 1);
+pangolin::Var<int> show_cam1("ui.show_cam1", 0, 0, 0);
+pangolin::Var<int> show_frame2("ui.show_frame2", 0, 0, 1);
+pangolin::Var<int> show_cam2("ui.show_cam2", 0, 0, 0);
+pangolin::Var<bool> lock_frames("ui.lock_frames", true, false, true);
+pangolin::Var<bool> show_detected("ui.show_detected", true, false, true);
+pangolin::Var<bool> show_matches("ui.show_matches", true, false, true);
+pangolin::Var<bool> show_inliers("ui.show_inliers", true, false, true);
+pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
+
+pangolin::Var<int> num_opt_iter("ui.num_opt_iter", 10, 0, 20);
+
+pangolin::Var<bool> show_gt("ui.show_gt", true, false, true);
+pangolin::Var<bool> show_edges("ui.show_edges", true, false, true);
+pangolin::Var<bool> show_points("ui.show_points", true, false, true);
+
+using Button = pangolin::Var<std::function<void(void)>>;
+
+Button detect_btn("ui.detect", &detect);
+Button match_btn("ui.match", &match);
+Button tracks_btn("ui.tracks", &tracks);
+Button optimize_btn("ui.optimize", &optimize);
+
+pangolin::Var<double> outlier_threshold("ui.outlier_threshold", 3.0, 0.01, 10);
+
+Button filter_btn("ui.filter", &filter);
+Button align_btn("ui.aling_se3", &alignButton);
+
+pangolin::Var<bool> euroc_fmt("ui.euroc_fmt", true, false, true);
+pangolin::Var<bool> tum_rgbd_fmt("ui.tum_rgbd_fmt", false, false, true);
+Button save_traj_btn("ui.save_traj", &saveTrajectoryButton);
+
+pangolin::OpenGlRenderState camera;
+
+std::string marg_data_path;
+
+int main(int argc, char** argv) {
+  bool show_gui = true;
+  std::string cam_calib_path;
+  std::string result_path;
+  std::string config_path;
+
+  CLI::App app{"App description"};
+
+  app.add_option("--show-gui", show_gui, "Show GUI");
+  app.add_option("--cam-calib", cam_calib_path,
+                 "Ground-truth camera calibration used for simulation.")
+      ->required();
+
+  app.add_option("--marg-data", marg_data_path, "Path to cache folder.")
+      ->required();
+
+  app.add_option("--config-path", config_path, "Path to config file.");
+
+  app.add_option("--result-path", result_path, "Path to config file.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  if (!config_path.empty()) {
+    vio_config.load(config_path);
+  }
+
+  load_data(cam_calib_path, marg_data_path);
+
+  for (auto& kv : marg_data) {
+    nrf_mapper->addMargData(kv.second);
+  }
+
+  computeEdgeVis();
+
+  {
+    std::cout << "Loaded " << nrf_mapper->img_data.size() << " images."
+              << std::endl;
+
+    show_frame1.Meta().range[1] = nrf_mapper->img_data.size() - 1;
+    show_frame2.Meta().range[1] = nrf_mapper->img_data.size() - 1;
+    show_frame1.Meta().gui_changed = true;
+    show_frame2.Meta().gui_changed = true;
+
+    show_cam1.Meta().range[1] = calib.intrinsics.size() - 1;
+    show_cam2.Meta().range[1] = calib.intrinsics.size() - 1;
+    if (calib.intrinsics.size() > 1) show_cam2 = 1;
+
+    for (const auto& kv : nrf_mapper->img_data) {
+      image_t_ns.emplace_back(kv.first);
+    }
+
+    std::sort(image_t_ns.begin(), image_t_ns.end());
+  }
+
+  if (show_gui) {
+    pangolin::CreateWindowAndBind("Main", 1800, 1000);
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::View& img_view_display =
+        pangolin::CreateDisplay()
+            .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.4)
+            .SetLayout(pangolin::LayoutEqual);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+    while (img_view.size() < calib.intrinsics.size()) {
+      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
+
+      size_t idx = img_view.size();
+      img_view.push_back(iv);
+
+      img_view_display.AddDisplay(*iv);
+      iv->extern_draw_function =
+          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
+    }
+
+    camera = pangolin::OpenGlRenderState(
+        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
+        pangolin::ModelViewLookAt(-3.4, -3.7, -8.3, 2.1, 0.6, 0.2,
+                                  pangolin::AxisNegY));
+
+    //    pangolin::OpenGlRenderState camera(
+    //        pangolin::ProjectionMatrixOrthographic(-30, 30, -30, 30, -30, 30),
+    //        pangolin::ModelViewLookAt(-3.4, -3.7, -8.3, 2.1, 0.6, 0.2,
+    //                                  pangolin::AxisNegY));
+
+    pangolin::View& display3D =
+        pangolin::CreateDisplay()
+            .SetAspect(-640 / 480.0)
+            .SetBounds(0.0, 1.0, 0.4, 1.0)
+            .SetHandler(new pangolin::Handler3D(camera));
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      if (lock_frames) {
+        // in case of locking frames, chaning one should change the other
+        if (show_frame1.GuiChanged()) {
+          show_frame2 = show_frame1;
+          show_frame2.Meta().gui_changed = true;
+          show_frame1.Meta().gui_changed = true;
+        } else if (show_frame2.GuiChanged()) {
+          show_frame1 = show_frame2;
+          show_frame1.Meta().gui_changed = true;
+          show_frame2.Meta().gui_changed = true;
+        }
+      }
+
+      display3D.Activate(camera);
+      glClearColor(1.f, 1.f, 1.f, 1.0f);
+
+      draw_scene();
+
+      if (show_frame1.GuiChanged() || show_cam1.GuiChanged()) {
+        size_t frame_id = static_cast<size_t>(show_frame1);
+        int64_t timestamp = image_t_ns[frame_id];
+        size_t cam_id = show_cam1;
+
+        if (nrf_mapper->img_data.count(timestamp) > 0 &&
+            nrf_mapper->img_data.at(timestamp).get()) {
+          const std::vector<basalt::ImageData>& img_vec =
+              nrf_mapper->img_data.at(timestamp)->img_data;
+
+          pangolin::GlPixFormat fmt;
+          fmt.glformat = GL_LUMINANCE;
+          fmt.gltype = GL_UNSIGNED_SHORT;
+          fmt.scalable_internal_format = GL_LUMINANCE16;
+
+          if (img_vec[cam_id].img.get()) {
+            img_view[0]->SetImage(
+                img_vec[cam_id].img->ptr, img_vec[cam_id].img->w,
+                img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);
+          } else {
+            img_view[0]->Clear();
+          }
+        } else {
+          img_view[0]->Clear();
+        }
+      }
+
+      if (euroc_fmt.GuiChanged()) {
+        tum_rgbd_fmt = !euroc_fmt;
+      }
+
+      if (tum_rgbd_fmt.GuiChanged()) {
+        euroc_fmt = !tum_rgbd_fmt;
+      }
+
+      if (show_frame2.GuiChanged() || show_cam2.GuiChanged()) {
+        size_t frame_id = static_cast<size_t>(show_frame2);
+        int64_t timestamp = image_t_ns[frame_id];
+        size_t cam_id = show_cam2;
+
+        if (nrf_mapper->img_data.count(timestamp) > 0 &&
+            nrf_mapper->img_data.at(timestamp).get()) {
+          const std::vector<basalt::ImageData>& img_vec =
+              nrf_mapper->img_data.at(timestamp)->img_data;
+
+          pangolin::GlPixFormat fmt;
+          fmt.glformat = GL_LUMINANCE;
+          fmt.gltype = GL_UNSIGNED_SHORT;
+          fmt.scalable_internal_format = GL_LUMINANCE16;
+
+          if (img_vec[cam_id].img.get()) {
+            img_view[1]->SetImage(
+                img_vec[cam_id].img->ptr, img_vec[cam_id].img->w,
+                img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);
+          } else {
+            img_view[1]->Clear();
+          }
+        } else {
+          img_view[1]->Clear();
+        }
+      }
+
+      pangolin::FinishFrame();
+
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+  } else {
+    auto time_start = std::chrono::high_resolution_clock::now();
+    // optimize();
+    detect();
+    match();
+    tracks();
+    optimize();
+    filter();
+    optimize();
+
+    auto time_end = std::chrono::high_resolution_clock::now();
+
+    if (!result_path.empty()) {
+      double error = alignButton();
+
+      auto exec_time_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+          time_end - time_start);
+
+      std::ofstream os(result_path);
+      {
+        cereal::JSONOutputArchive ar(os);
+        ar(cereal::make_nvp("rms_ate", error));
+        ar(cereal::make_nvp("num_frames", nrf_mapper->getFramePoses().size()));
+        ar(cereal::make_nvp("exec_time_ns", exec_time_ns.count()));
+      }
+      os.close();
+    }
+  }
+
+  return 0;
+}
+
+void draw_image_overlay(pangolin::View& v, size_t view_id) {
+  UNUSED(v);
+
+  size_t frame_id = (view_id == 0) ? show_frame1 : show_frame2;
+  size_t cam_id = (view_id == 0) ? show_cam1 : show_cam2;
+
+  basalt::TimeCamId tcid(image_t_ns[frame_id], cam_id);
+
+  float text_row = 20;
+
+  if (show_detected) {
+    glLineWidth(1.0);
+    glColor3f(1.0, 0.0, 0.0);  // red
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (nrf_mapper->feature_corners.find(tcid) !=
+        nrf_mapper->feature_corners.end()) {
+      const basalt::KeypointsData& cr = nrf_mapper->feature_corners.at(tcid);
+
+      for (size_t i = 0; i < cr.corners.size(); i++) {
+        Eigen::Vector2d c = cr.corners[i];
+        double angle = cr.corner_angles[i];
+        pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0);
+
+        Eigen::Vector2d r(3, 0);
+        Eigen::Rotation2Dd rot(angle);
+        r = rot * r;
+
+        pangolin::glDrawLine(c, c + r);
+      }
+
+      pangolin::GlFont::I()
+          .Text("Detected %d corners", cr.corners.size())
+          .Draw(5, 20);
+
+    } else {
+      glLineWidth(1.0);
+
+      pangolin::GlFont::I().Text("Corners not processed").Draw(5, text_row);
+    }
+    text_row += 20;
+  }
+
+  if (show_matches || show_inliers) {
+    glLineWidth(1.0);
+    glColor3f(0.0, 0.0, 1.0);  // blue
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    size_t o_frame_id = (view_id == 0 ? show_frame2 : show_frame1);
+    size_t o_cam_id = (view_id == 0 ? show_cam2 : show_cam1);
+
+    basalt::TimeCamId o_tcid(image_t_ns[o_frame_id], o_cam_id);
+
+    int idx = -1;
+
+    auto it = nrf_mapper->feature_matches.find(std::make_pair(tcid, o_tcid));
+
+    if (it != nrf_mapper->feature_matches.end()) {
+      idx = 0;
+    } else {
+      it = nrf_mapper->feature_matches.find(std::make_pair(o_tcid, tcid));
+      if (it != nrf_mapper->feature_matches.end()) {
+        idx = 1;
+      }
+    }
+
+    if (idx >= 0 && show_matches) {
+      if (nrf_mapper->feature_corners.find(tcid) !=
+          nrf_mapper->feature_corners.end()) {
+        const basalt::KeypointsData& cr = nrf_mapper->feature_corners.at(tcid);
+
+        for (size_t i = 0; i < it->second.matches.size(); i++) {
+          size_t c_idx = idx == 0 ? it->second.matches[i].first
+                                  : it->second.matches[i].second;
+
+          Eigen::Vector2d c = cr.corners[c_idx];
+          double angle = cr.corner_angles[c_idx];
+          pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0);
+
+          Eigen::Vector2d r(3, 0);
+          Eigen::Rotation2Dd rot(angle);
+          r = rot * r;
+
+          pangolin::glDrawLine(c, c + r);
+
+          if (show_ids) {
+            pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]);
+          }
+        }
+
+        pangolin::GlFont::I()
+            .Text("Detected %d matches", it->second.matches.size())
+            .Draw(5, text_row);
+        text_row += 20;
+      }
+    }
+
+    glColor3f(0.0, 1.0, 0.0);  // green
+
+    if (idx >= 0 && show_inliers) {
+      if (nrf_mapper->feature_corners.find(tcid) !=
+          nrf_mapper->feature_corners.end()) {
+        const basalt::KeypointsData& cr = nrf_mapper->feature_corners.at(tcid);
+
+        for (size_t i = 0; i < it->second.inliers.size(); i++) {
+          size_t c_idx = idx == 0 ? it->second.inliers[i].first
+                                  : it->second.inliers[i].second;
+
+          Eigen::Vector2d c = cr.corners[c_idx];
+          double angle = cr.corner_angles[c_idx];
+          pangolin::glDrawCirclePerimeter(c[0], c[1], 3.0);
+
+          Eigen::Vector2d r(3, 0);
+          Eigen::Rotation2Dd rot(angle);
+          r = rot * r;
+
+          pangolin::glDrawLine(c, c + r);
+
+          if (show_ids) {
+            pangolin::GlFont::I().Text("%d", i).Draw(c[0], c[1]);
+          }
+        }
+
+        pangolin::GlFont::I()
+            .Text("Detected %d inliers", it->second.inliers.size())
+            .Draw(5, text_row);
+        text_row += 20;
+      }
+    }
+  }
+}
+
+void draw_scene() {
+  glPointSize(3);
+  glColor3f(1.0, 0.0, 0.0);
+  glEnable(GL_BLEND);
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+  glColor3ubv(pose_color);
+  if (show_points) pangolin::glDrawPoints(mapper_points);
+
+  glColor3ubv(gt_color);
+  if (show_gt) pangolin::glDrawLineStrip(gt_frame_t_w_i);
+
+  glColor3f(0.0, 1.0, 0.0);
+  if (show_edges) pangolin::glDrawLines(edges_vis);
+
+  glLineWidth(2);
+  glColor3f(1.0, 0.0, 1.0);
+  if (show_edges) pangolin::glDrawLines(roll_pitch_vis);
+  glLineWidth(1);
+
+  glColor3f(1.0, 0.0, 0.0);
+  if (show_edges) pangolin::glDrawLines(rel_edges_vis);
+
+  for (const auto& kv : nrf_mapper->getFramePoses()) {
+    pangolin::glDrawAxis(kv.second.getPose().matrix(), 0.1);
+  }
+
+  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
+}
+
+void load_data(const std::string& calib_path, const std::string& cache_path) {
+  {
+    std::ifstream os(calib_path, std::ios::binary);
+
+    if (os.is_open()) {
+      cereal::JSONInputArchive archive(os);
+      archive(calib);
+      std::cout << "Loaded camera with " << calib.intrinsics.size()
+                << " cameras" << std::endl;
+
+    } else {
+      std::cerr << "could not load camera calibration " << calib_path
+                << std::endl;
+      std::abort();
+    }
+  }
+
+  {
+    // Load gt.
+    {
+      std::string p = cache_path + "/gt.cereal";
+      std::ifstream is(p, std::ios::binary);
+
+      {
+        cereal::BinaryInputArchive archive(is);
+        archive(gt_frame_t_ns);
+        archive(gt_frame_t_w_i);
+      }
+      is.close();
+      std::cout << "Loaded " << gt_frame_t_ns.size() << " timestamps and "
+                << gt_frame_t_w_i.size() << " poses" << std::endl;
+    }
+  }
+
+  nrf_mapper.reset(new basalt::NfrMapper(calib, vio_config));
+
+  basalt::MargDataLoader mdl;
+  tbb::concurrent_bounded_queue<basalt::MargData::Ptr> marg_queue;
+  mdl.out_marg_queue = &marg_queue;
+
+  mdl.start(cache_path);
+
+  while (true) {
+    basalt::MargData::Ptr data;
+    marg_queue.pop(data);
+
+    if (data.get()) {
+      int64_t t_ns = *data->kfs_to_marg.begin();
+      marg_data[t_ns] = data;
+
+    } else {
+      break;
+    }
+  }
+
+  std::cout << "Loaded " << marg_data.size() << " marg data." << std::endl;
+}
+
+void computeEdgeVis() {
+  edges_vis.clear();
+  for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) {
+    for (const auto& kv2 : kv1.second) {
+      Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
+                               .at(kv1.first.frame_id)
+                               .getPose()
+                               .translation();
+      Eigen::Vector3d p2 = nrf_mapper->getFramePoses()
+                               .at(kv2.first.frame_id)
+                               .getPose()
+                               .translation();
+
+      edges_vis.emplace_back(p1);
+      edges_vis.emplace_back(p2);
+    }
+  }
+
+  roll_pitch_vis.clear();
+  for (const auto& v : nrf_mapper->roll_pitch_factors) {
+    const Sophus::SE3d& T_w_i =
+        nrf_mapper->getFramePoses().at(v.t_ns).getPose();
+
+    Eigen::Vector3d p = T_w_i.translation();
+    Eigen::Vector3d d =
+        v.R_w_i_meas * T_w_i.so3().inverse() * (-Eigen::Vector3d::UnitZ());
+
+    roll_pitch_vis.emplace_back(p);
+    roll_pitch_vis.emplace_back(p + 0.1 * d);
+  }
+
+  rel_edges_vis.clear();
+  for (const auto& v : nrf_mapper->rel_pose_factors) {
+    Eigen::Vector3d p1 =
+        nrf_mapper->getFramePoses().at(v.t_i_ns).getPose().translation();
+    Eigen::Vector3d p2 =
+        nrf_mapper->getFramePoses().at(v.t_j_ns).getPose().translation();
+
+    rel_edges_vis.emplace_back(p1);
+    rel_edges_vis.emplace_back(p2);
+  }
+}
+
+void optimize() {
+  nrf_mapper->optimize(num_opt_iter);
+  nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
+
+  computeEdgeVis();
+}
+
+double alignButton() {
+  Eigen::aligned_vector<Eigen::Vector3d> filter_t_w_i;
+  std::vector<int64_t> filter_t_ns;
+
+  for (const auto& kv : nrf_mapper->getFramePoses()) {
+    filter_t_ns.emplace_back(kv.first);
+    filter_t_w_i.emplace_back(kv.second.getPose().translation());
+  }
+
+  return basalt::alignSVD(filter_t_ns, filter_t_w_i, gt_frame_t_ns,
+                          gt_frame_t_w_i);
+}
+
+void detect() {
+  nrf_mapper->feature_corners.clear();
+  nrf_mapper->feature_matches.clear();
+  nrf_mapper->detect_keypoints();
+}
+
+void match() {
+  nrf_mapper->feature_matches.clear();
+  nrf_mapper->match_stereo();
+  nrf_mapper->match_all();
+}
+
+void tracks() {
+  nrf_mapper->build_tracks();
+  nrf_mapper->setup_opt();
+  nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
+  //  nrf_mapper->get_current_points_with_color(mapper_points,
+  //  mapper_points_color,
+  //                                            mapper_point_ids);
+  computeEdgeVis();
+}
+
+void filter() {
+  nrf_mapper->filterOutliers(outlier_threshold, 4);
+  nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
+}
+
+void saveTrajectoryButton() {
+  if (tum_rgbd_fmt) {
+    std::ofstream os("keyframeTrajectory.txt");
+
+    os << "# timestamp tx ty tz qx qy qz qw" << std::endl;
+
+    for (const auto& kv : nrf_mapper->getFramePoses()) {
+      const Sophus::SE3d pose = kv.second.getPose();
+      os << std::scientific << std::setprecision(18) << kv.first * 1e-9 << " "
+         << pose.translation().x() << " " << pose.translation().y() << " "
+         << pose.translation().z() << " " << pose.unit_quaternion().x() << " "
+         << pose.unit_quaternion().y() << " " << pose.unit_quaternion().z()
+         << " " << pose.unit_quaternion().w() << std::endl;
+    }
+
+    os.close();
+
+    std::cout << "Saved trajectory in TUM RGB-D Dataset format in "
+                 "keyframeTrajectory.txt"
+              << std::endl;
+  } else {
+    std::ofstream os("keyframeTrajectory.csv");
+
+    os << "#timestamp [ns],p_RS_R_x [m],p_RS_R_y [m],p_RS_R_z [m],q_RS_w "
+          "[],q_RS_x [],q_RS_y [],q_RS_z []"
+       << std::endl;
+
+    for (const auto& kv : nrf_mapper->getFramePoses()) {
+      const Sophus::SE3d pose = kv.second.getPose();
+      os << std::scientific << std::setprecision(18) << kv.first << ","
+         << pose.translation().x() << "," << pose.translation().y() << ","
+         << pose.translation().z() << "," << pose.unit_quaternion().w() << ","
+         << pose.unit_quaternion().x() << "," << pose.unit_quaternion().y()
+         << "," << pose.unit_quaternion().z() << std::endl;
+    }
+
+    os.close();
+
+    std::cout
+        << "Saved trajectory in Euroc Dataset format in keyframeTrajectory.csv"
+        << std::endl;
+  }
+}

src/mapper_sim.cpp

@@ -0,0 +1,605 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <sophus/se3.hpp>
+
+#include <tbb/concurrent_unordered_map.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/io/marg_data_io.h>
+#include <basalt/optimization/accumulator.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/utils/filesystem.h>
+#include <basalt/utils/imu_types.h>
+#include <basalt/utils/nfr.h>
+#include <basalt/utils/sim_utils.h>
+#include <basalt/utils/vis_utils.h>
+#include <basalt/vi_estimator/marg_helper.h>
+#include <basalt/vi_estimator/nfr_mapper.h>
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/serialization/headers_serialization.h>
+
+using basalt::POSE_SIZE;
+using basalt::POSE_VEL_BIAS_SIZE;
+
+Eigen::Vector3d g(0, 0, -9.81);
+
+std::shared_ptr<basalt::Se3Spline<5>> gt_spline;
+
+Eigen::aligned_vector<Sophus::SE3d> gt_frame_T_w_i;
+Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
+std::vector<int64_t> gt_frame_t_ns;
+
+Eigen::aligned_vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias,
+    gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel;
+
+std::vector<int64_t> gt_imu_t_ns;
+
+Eigen::aligned_vector<Eigen::Vector3d> filter_points;
+std::vector<int> filter_point_ids;
+
+std::map<int64_t, basalt::MargData::Ptr> marg_data;
+
+Eigen::aligned_vector<basalt::RollPitchFactor> roll_pitch_factors;
+Eigen::aligned_vector<basalt::RelPoseFactor> rel_pose_factors;
+
+Eigen::aligned_vector<Eigen::Vector3d> edges_vis;
+Eigen::aligned_vector<Eigen::Vector3d> roll_pitch_vis;
+Eigen::aligned_vector<Eigen::Vector3d> rel_edges_vis;
+
+Eigen::aligned_vector<Eigen::Vector3d> mapper_points;
+std::vector<int> mapper_point_ids;
+
+basalt::NfrMapper::Ptr nrf_mapper;
+
+std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
+std::map<basalt::TimeCamId, basalt::SimObservations> noisy_observations;
+
+void draw_scene();
+void load_data(const std::string& calib_path,
+               const std::string& marg_data_path);
+void processMargData(basalt::MargData& m);
+void extractNonlinearFactors(basalt::MargData& m);
+void computeEdgeVis();
+void optimize();
+void randomInc();
+void randomYawInc();
+double alignButton();
+void setup_points();
+
+constexpr int UI_WIDTH = 200;
+// constexpr int NUM_FRAMES = 500;
+
+basalt::Calibration<double> calib;
+
+pangolin::Var<bool> show_edges("ui.show_edges", true, false, true);
+pangolin::Var<bool> show_points("ui.show_points", true, false, true);
+
+using Button = pangolin::Var<std::function<void(void)>>;
+
+Button optimize_btn("ui.optimize", &optimize);
+Button rand_inc_btn("ui.rand_inc", &randomInc);
+Button rand_yaw_inc_btn("ui.rand_yaw", &randomYawInc);
+Button setup_points_btn("ui.setup_points", &setup_points);
+Button align_se3_btn("ui.align_se3", &alignButton);
+
+std::string marg_data_path;
+
+int main(int argc, char** argv) {
+  bool show_gui = true;
+  std::string cam_calib_path;
+  std::string result_path;
+
+  CLI::App app{"App description"};
+
+  app.add_option("--show-gui", show_gui, "Show GUI");
+  app.add_option("--cam-calib", cam_calib_path,
+                 "Ground-truth camera calibration used for simulation.")
+      ->required();
+
+  app.add_option("--marg-data", marg_data_path, "Path to cache folder.")
+      ->required();
+
+  app.add_option("--result-path", result_path,
+                 "Path to result file where the system will write RMSE ATE.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  load_data(cam_calib_path, marg_data_path);
+
+  basalt::VioConfig config;
+
+  nrf_mapper.reset(new basalt::NfrMapper(calib, config));
+
+  for (auto& kv : marg_data) {
+    nrf_mapper->addMargData(kv.second);
+  }
+
+  computeEdgeVis();
+
+  std::cout << "roll_pitch_factors.size() " << roll_pitch_factors.size()
+            << std::endl;
+  std::cout << "rel_pose_factors.size() " << rel_pose_factors.size()
+            << std::endl;
+
+  if (show_gui) {
+    pangolin::CreateWindowAndBind("Main", 1800, 1000);
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    pangolin::OpenGlRenderState camera(
+        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
+        pangolin::ModelViewLookAt(-8.4, -8.7, -8.3, 2.1, 0.6, 0.2,
+                                  pangolin::AxisNegY));
+
+    pangolin::View& display3D =
+        pangolin::CreateDisplay()
+            .SetAspect(-640 / 480.0)
+            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0)
+            .SetHandler(new pangolin::Handler3D(camera));
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      display3D.Activate(camera);
+      glClearColor(1.f, 1.f, 1.f, 1.0f);
+
+      draw_scene();
+
+      pangolin::FinishFrame();
+
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+  } else {
+    setup_points();
+    optimize();
+
+    if (!result_path.empty()) {
+      double error = alignButton();
+
+      std::ofstream os(result_path);
+      os << error << std::endl;
+      os.close();
+    }
+  }
+
+  return 0;
+}
+
+void draw_scene() {
+  glPointSize(3);
+  glColor3f(1.0, 0.0, 0.0);
+  glEnable(GL_BLEND);
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+  glColor3ubv(pose_color);
+  if (show_points) pangolin::glDrawPoints(mapper_points);
+
+  glColor3ubv(gt_color);
+  pangolin::glDrawLineStrip(gt_frame_t_w_i);
+
+  glColor3ubv(cam_color);
+  pangolin::glDrawLineStrip(vio_t_w_i);
+
+  glColor3f(0.0, 1.0, 0.0);
+  if (show_edges) pangolin::glDrawLines(edges_vis);
+
+  glColor3f(1.0, 0.0, 1.0);
+  if (show_edges) pangolin::glDrawLines(roll_pitch_vis);
+
+  glColor3f(1.0, 0.0, 0.0);
+  if (show_edges) pangolin::glDrawLines(rel_edges_vis);
+
+  for (const auto& kv : nrf_mapper->getFramePoses()) {
+    pangolin::glDrawAxis(kv.second.getPose().matrix(), 0.1);
+  }
+
+  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
+}
+
+void load_data(const std::string& calib_path, const std::string& cache_path) {
+  {
+    std::ifstream os(calib_path, std::ios::binary);
+
+    if (os.is_open()) {
+      cereal::JSONInputArchive archive(os);
+      archive(calib);
+      std::cout << "Loaded camera with " << calib.intrinsics.size()
+                << " cameras" << std::endl;
+
+    } else {
+      std::cerr << "could not load camera calibration " << calib_path
+                << std::endl;
+      std::abort();
+    }
+  }
+
+  {
+    std::string path = cache_path + "/gt_spline.cereal";
+    std::cout << "path " << path << std::endl;
+
+    std::ifstream is(path, std::ios::binary);
+
+    if (is.is_open()) {
+      cereal::JSONInputArchive archive(is);
+
+      int64_t t_ns;
+      Eigen::aligned_vector<Sophus::SE3d> knots;
+
+      archive(cereal::make_nvp("t_ns", t_ns));
+      archive(cereal::make_nvp("knots", knots));
+
+      archive(cereal::make_nvp("gt_observations", gt_observations));
+      archive(cereal::make_nvp("noisy_observations", noisy_observations));
+
+      std::cout << "path " << path << std::endl;
+      std::cout << "t_ns " << t_ns << std::endl;
+      std::cout << "knots " << knots.size() << std::endl;
+
+      gt_spline.reset(new basalt::Se3Spline<5>(t_ns));
+
+      for (size_t i = 0; i < knots.size(); i++) {
+        gt_spline->knotsPushBack(knots[i]);
+      }
+
+      is.close();
+    } else {
+      std::cerr << "could not open " << path << std::endl;
+      std::abort();
+    }
+  }
+
+  {
+    int64_t dt_ns = int64_t(1e9) / 50;
+
+    for (int64_t t_ns = 0; t_ns < gt_spline->maxTimeNs(); t_ns += dt_ns) {
+      gt_frame_t_w_i.emplace_back(gt_spline->pose(t_ns).translation());
+      gt_frame_t_ns.emplace_back(t_ns);
+    }
+  }
+
+  basalt::MargDataLoader mdl;
+  tbb::concurrent_bounded_queue<basalt::MargData::Ptr> marg_queue;
+  mdl.out_marg_queue = &marg_queue;
+
+  mdl.start(cache_path);
+
+  while (true) {
+    basalt::MargData::Ptr data;
+    marg_queue.pop(data);
+
+    if (data.get()) {
+      int64_t t_ns = *data->kfs_to_marg.begin();
+      marg_data[t_ns] = data;
+    } else {
+      break;
+    }
+  }
+
+  std::cout << "Loaded " << marg_data.size() << " marg data." << std::endl;
+}
+
+void processMargData(basalt::MargData& m) {
+  BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
+
+  std::cout << "rank " << m.abs_H.fullPivLu().rank() << " size "
+            << m.abs_H.cols() << std::endl;
+
+  basalt::AbsOrderMap aom_new;
+  std::set<int> idx_to_keep;
+  std::set<int> idx_to_marg;
+
+  for (const auto& kv : m.aom.abs_order_map) {
+    if (kv.second.second == POSE_SIZE) {
+      for (size_t i = 0; i < POSE_SIZE; i++)
+        idx_to_keep.emplace(kv.second.first + i);
+      aom_new.abs_order_map.emplace(kv);
+      aom_new.total_size += POSE_SIZE;
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      if (m.kfs_all.count(kv.first) > 0) {
+        for (size_t i = 0; i < POSE_SIZE; i++)
+          idx_to_keep.emplace(kv.second.first + i);
+        for (size_t i = POSE_SIZE; i < POSE_VEL_BIAS_SIZE; i++)
+          idx_to_marg.emplace(kv.second.first + i);
+
+        aom_new.abs_order_map[kv.first] =
+            std::make_pair(aom_new.total_size, POSE_SIZE);
+        aom_new.total_size += POSE_SIZE;
+
+        basalt::PoseStateWithLin<double> p(m.frame_states.at(kv.first));
+        m.frame_poses[kv.first] = p;
+        m.frame_states.erase(kv.first);
+      } else {
+        for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++)
+          idx_to_marg.emplace(kv.second.first + i);
+        m.frame_states.erase(kv.first);
+      }
+    } else {
+      std::cerr << "Unknown size" << std::endl;
+      std::abort();
+    }
+
+    std::cout << kv.first << " " << kv.second.first << " " << kv.second.second
+              << std::endl;
+  }
+
+  Eigen::MatrixXd marg_H_new;
+  Eigen::VectorXd marg_b_new;
+  basalt::MargHelper<double>::marginalizeHelperSqToSq(
+      m.abs_H, m.abs_b, idx_to_keep, idx_to_marg, marg_H_new, marg_b_new);
+
+  std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
+            << marg_H_new.cols() << std::endl;
+
+  m.abs_H = marg_H_new;
+  m.abs_b = marg_b_new;
+  m.aom = aom_new;
+
+  BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
+}
+
+void extractNonlinearFactors(basalt::MargData& m) {
+  size_t asize = m.aom.total_size;
+  std::cout << "asize " << asize << std::endl;
+
+  Eigen::MatrixXd cov_old;
+  cov_old.setIdentity(asize, asize);
+  m.abs_H.ldlt().solveInPlace(cov_old);
+
+  int64_t kf_id = *m.kfs_to_marg.cbegin();
+  int kf_start_idx = m.aom.abs_order_map.at(kf_id).first;
+
+  auto state_kf = m.frame_poses.at(kf_id);
+
+  Sophus::SE3d T_w_i_kf = state_kf.getPose();
+
+  Eigen::Vector3d pos = T_w_i_kf.translation();
+  Eigen::Vector3d yaw_dir_body =
+      T_w_i_kf.so3().inverse() * Eigen::Vector3d::UnitX();
+
+  Sophus::Matrix<double, 3, POSE_SIZE> d_pos_d_T_w_i;
+  Sophus::Matrix<double, 1, POSE_SIZE> d_yaw_d_T_w_i;
+  Sophus::Matrix<double, 2, POSE_SIZE> d_rp_d_T_w_i;
+
+  basalt::absPositionError(T_w_i_kf, pos, &d_pos_d_T_w_i);
+  basalt::yawError(T_w_i_kf, yaw_dir_body, &d_yaw_d_T_w_i);
+  basalt::rollPitchError(T_w_i_kf, T_w_i_kf.so3(), &d_rp_d_T_w_i);
+
+  {
+    Eigen::MatrixXd J;
+    J.setZero(POSE_SIZE, asize);
+    J.block<3, POSE_SIZE>(0, kf_start_idx) = d_pos_d_T_w_i;
+    J.block<1, POSE_SIZE>(3, kf_start_idx) = d_yaw_d_T_w_i;
+    J.block<2, POSE_SIZE>(4, kf_start_idx) = d_rp_d_T_w_i;
+
+    Sophus::Matrix6d cov_new = J * cov_old * J.transpose();
+
+    // std::cout << "cov_new\n" << cov_new << std::endl;
+
+    basalt::RollPitchFactor rpf;
+    rpf.t_ns = kf_id;
+    rpf.R_w_i_meas = T_w_i_kf.so3();
+    rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse();
+
+    roll_pitch_factors.emplace_back(rpf);
+  }
+
+  for (int64_t other_id : m.kfs_all) {
+    if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) {
+      continue;
+    }
+
+    auto state_o = m.frame_poses.at(other_id);
+
+    Sophus::SE3d T_w_i_o = state_o.getPose();
+    Sophus::SE3d T_kf_o = T_w_i_kf.inverse() * T_w_i_o;
+
+    int o_start_idx = m.aom.abs_order_map.at(other_id).first;
+
+    Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j;
+    basalt::relPoseError(T_kf_o, T_w_i_kf, T_w_i_o, &d_res_d_T_w_i,
+                         &d_res_d_T_w_j);
+
+    Eigen::MatrixXd J;
+    J.setZero(POSE_SIZE, asize);
+    J.block<POSE_SIZE, POSE_SIZE>(0, kf_start_idx) = d_res_d_T_w_i;
+    J.block<POSE_SIZE, POSE_SIZE>(0, o_start_idx) = d_res_d_T_w_j;
+
+    Sophus::Matrix6d cov_new = J * cov_old * J.transpose();
+    basalt::RelPoseFactor rpf;
+    rpf.t_i_ns = kf_id;
+    rpf.t_j_ns = other_id;
+    rpf.T_i_j = T_kf_o;
+    rpf.cov_inv.setIdentity();
+    cov_new.ldlt().solveInPlace(rpf.cov_inv);
+
+    // std::cout << "rpf.cov_inv\n" << rpf.cov_inv << std::endl;
+
+    rel_pose_factors.emplace_back(rpf);
+  }
+}
+
+void computeEdgeVis() {
+  edges_vis.clear();
+  for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) {
+    for (const auto& kv2 : kv1.second) {
+      Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
+                               .at(kv1.first.frame_id)
+                               .getPose()
+                               .translation();
+      Eigen::Vector3d p2 = nrf_mapper->getFramePoses()
+                               .at(kv2.first.frame_id)
+                               .getPose()
+                               .translation();
+
+      edges_vis.emplace_back(p1);
+      edges_vis.emplace_back(p2);
+    }
+  }
+
+  roll_pitch_vis.clear();
+  for (const auto& v : nrf_mapper->roll_pitch_factors) {
+    const Sophus::SE3d& T_w_i =
+        nrf_mapper->getFramePoses().at(v.t_ns).getPose();
+
+    Eigen::Vector3d p = T_w_i.translation();
+    Eigen::Vector3d d =
+        v.R_w_i_meas * T_w_i.so3().inverse() * (-Eigen::Vector3d::UnitZ());
+
+    roll_pitch_vis.emplace_back(p);
+    roll_pitch_vis.emplace_back(p + 0.1 * d);
+  }
+
+  rel_edges_vis.clear();
+  for (const auto& v : nrf_mapper->rel_pose_factors) {
+    Eigen::Vector3d p1 =
+        nrf_mapper->getFramePoses().at(v.t_i_ns).getPose().translation();
+    Eigen::Vector3d p2 =
+        nrf_mapper->getFramePoses().at(v.t_j_ns).getPose().translation();
+
+    rel_edges_vis.emplace_back(p1);
+    rel_edges_vis.emplace_back(p2);
+  }
+}
+
+void optimize() {
+  nrf_mapper->optimize();
+  nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
+  //  nrf_mapper->get_current_points_with_color(mapper_points,
+  //  mapper_points_color,
+  //                                            mapper_point_ids);
+
+  computeEdgeVis();
+}
+
+void randomInc() {
+  Sophus::Vector6d rnd = Sophus::Vector6d::Random().array().abs();
+  Sophus::SE3d random_inc = Sophus::se3_expd(rnd / 10);
+
+  for (auto& kv : nrf_mapper->getFramePoses()) {
+    Sophus::SE3d pose = random_inc * kv.second.getPose();
+    basalt::PoseStateWithLin<double> p(kv.first, pose);
+    kv.second = p;
+  }
+
+  computeEdgeVis();
+}
+
+void randomYawInc() {
+  Sophus::Vector6d rnd;
+  rnd.setZero();
+  rnd[5] = std::abs(Eigen::Vector2d::Random()[0]);
+
+  Sophus::SE3d random_inc = Sophus::se3_expd(rnd);
+
+  std::cout << "random_inc\n" << random_inc.matrix() << std::endl;
+
+  for (auto& kv : nrf_mapper->getFramePoses()) {
+    Sophus::SE3d pose = random_inc * kv.second.getPose();
+    basalt::PoseStateWithLin<double> p(kv.first, pose);
+    kv.second = p;
+  }
+
+  computeEdgeVis();
+}
+
+double alignButton() {
+  Eigen::aligned_vector<Eigen::Vector3d> filter_t_w_i;
+  std::vector<int64_t> filter_t_ns;
+
+  for (const auto& kv : nrf_mapper->getFramePoses()) {
+    filter_t_ns.emplace_back(kv.first);
+    filter_t_w_i.emplace_back(kv.second.getPose().translation());
+  }
+
+  return basalt::alignSVD(filter_t_ns, filter_t_w_i, gt_frame_t_ns,
+                          gt_frame_t_w_i);
+}
+
+void setup_points() {
+  for (auto& kv : nrf_mapper->getFramePoses()) {
+    for (size_t i = 0; i < calib.intrinsics.size(); i++) {
+      basalt::TimeCamId tcid(kv.first, i);
+      auto obs = noisy_observations.at(tcid);
+
+      basalt::KeypointsData kd;
+      kd.corners = obs.pos;
+
+      nrf_mapper->feature_corners[tcid] = kd;
+
+      for (size_t j = 0; j < kd.corners.size(); j++) {
+        nrf_mapper->feature_tracks[obs.id[j]][tcid] = j;
+      }
+    }
+  }
+
+  for (auto it = nrf_mapper->feature_tracks.cbegin();
+       it != nrf_mapper->feature_tracks.cend();) {
+    if (it->second.size() < 5) {
+      it = nrf_mapper->feature_tracks.erase(it);
+    } else {
+      ++it;
+    }
+  }
+
+  std::cerr << "nrf_mapper->feature_tracks.size() "
+            << nrf_mapper->feature_tracks.size() << std::endl;
+
+  nrf_mapper->setup_opt();
+
+  nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
+}

src/mapper_sim_naive.cpp

@@ -0,0 +1,766 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <sophus/se3.hpp>
+
+#include <tbb/concurrent_unordered_map.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/io/marg_data_io.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/utils/sim_utils.h>
+#include <basalt/vi_estimator/vio_estimator.h>
+
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/serialization/headers_serialization.h>
+
+#include <basalt/utils/vis_utils.h>
+
+// GUI functions
+void draw_image_overlay(pangolin::View& v, size_t cam_id);
+void draw_scene();
+void load_data(const std::string& calib_path);
+void gen_data();
+void compute_projections();
+void setup_vio();
+void draw_plots();
+bool next_step();
+void alignButton();
+
+static const int knot_time = 3;
+// static const double obs_std_dev = 0.5;
+
+Eigen::Vector3d g(0, 0, -9.81);
+
+// std::random_device rd{};
+// std::mt19937 gen{rd()};
+std::mt19937 gen{1};
+
+// Simulated data
+
+basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9));
+
+Eigen::aligned_vector<Eigen::Vector3d> gt_points;
+Eigen::aligned_vector<Sophus::SE3d> gt_frame_T_w_i;
+Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
+std::vector<int64_t> gt_frame_t_ns, kf_t_ns;
+Eigen::aligned_vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias,
+    gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel;
+std::vector<int64_t> gt_imu_t_ns;
+
+std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
+std::map<basalt::TimeCamId, basalt::SimObservations> noisy_observations;
+
+std::string marg_data_path;
+
+// VIO vars
+basalt::Calibration<double> calib;
+basalt::VioEstimatorBase::Ptr vio;
+
+// Visualization vars
+std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
+tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
+tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
+    out_state_queue;
+
+std::vector<pangolin::TypedImage> images;
+
+// Pangolin vars
+constexpr int UI_WIDTH = 200;
+pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
+pangolin::Plotter* plotter;
+
+pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1000);
+
+pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
+pangolin::Var<bool> show_obs_noisy("ui.show_obs_noisy", true, false, true);
+pangolin::Var<bool> show_obs_vio("ui.show_obs_vio", true, false, true);
+
+pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
+
+pangolin::Var<bool> show_accel("ui.show_accel", false, false, true);
+pangolin::Var<bool> show_gyro("ui.show_gyro", false, false, true);
+pangolin::Var<bool> show_gt_vel("ui.show_gt_vel", false, false, true);
+pangolin::Var<bool> show_gt_pos("ui.show_gt_pos", true, false, true);
+pangolin::Var<bool> show_gt_bg("ui.show_gt_bg", false, false, true);
+pangolin::Var<bool> show_gt_ba("ui.show_gt_ba", false, false, true);
+
+pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
+pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
+pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
+pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);
+
+using Button = pangolin::Var<std::function<void(void)>>;
+
+Button next_step_btn("ui.next_step", &next_step);
+
+pangolin::Var<bool> continue_btn("ui.continue", true, false, true);
+
+Button align_step_btn("ui.align_se3", &alignButton);
+
+int main(int argc, char** argv) {
+  srand(1);
+
+  bool show_gui = true;
+  std::string cam_calib_path;
+  std::string result_path;
+
+  CLI::App app{"App description"};
+
+  app.add_option("--show-gui", show_gui, "Show GUI");
+  app.add_option("--cam-calib", cam_calib_path,
+                 "Ground-truth camera calibration used for simulation.")
+      ->required();
+
+  app.add_option("--marg-data", marg_data_path,
+                 "Folder to store marginalization data.")
+      ->required();
+
+  app.add_option("--result-path", result_path,
+                 "Path to result file where the system will write RMSE ATE.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  load_data(cam_calib_path);
+
+  gen_data();
+
+  setup_vio();
+
+  vio->out_vis_queue = &out_vis_queue;
+  vio->out_state_queue = &out_state_queue;
+
+  std::thread t0([&]() {
+    for (size_t i = 0; i < gt_imu_t_ns.size(); i++) {
+      basalt::ImuData<double>::Ptr data(new basalt::ImuData<double>);
+      data->t_ns = gt_imu_t_ns[i];
+
+      data->accel = noisy_accel[i];
+      data->gyro = noisy_gyro[i];
+
+      vio->addIMUToQueue(data);
+    }
+
+    vio->addIMUToQueue(nullptr);
+
+    std::cout << "Finished t0" << std::endl;
+  });
+
+  std::thread t1([&]() {
+    for (const auto& t_ns : kf_t_ns) {
+      basalt::OpticalFlowResult::Ptr data(new basalt::OpticalFlowResult);
+      data->t_ns = t_ns;
+
+      for (size_t j = 0; j < calib.T_i_c.size(); j++) {
+        data->observations.emplace_back();
+        basalt::TimeCamId tcid(data->t_ns, j);
+        const basalt::SimObservations& obs = noisy_observations.at(tcid);
+        for (size_t k = 0; k < obs.pos.size(); k++) {
+          Eigen::AffineCompact2f t;
+          t.setIdentity();
+          t.translation() = obs.pos[k].cast<float>();
+          data->observations.back()[obs.id[k]] = t;
+        }
+      }
+
+      vio->addVisionToQueue(data);
+    }
+
+    vio->addVisionToQueue(nullptr);
+
+    std::cout << "Finished t1" << std::endl;
+  });
+
+  std::thread t2([&]() {
+    basalt::VioVisualizationData::Ptr data;
+
+    while (true) {
+      out_vis_queue.pop(data);
+
+      if (data.get()) {
+        vis_map[data->t_ns] = data;
+      } else {
+        break;
+      }
+    }
+
+    std::cout << "Finished t2" << std::endl;
+  });
+
+  std::thread t3([&]() {
+    basalt::PoseVelBiasState<double>::Ptr data;
+
+    while (true) {
+      out_state_queue.pop(data);
+
+      if (!data.get()) break;
+
+      int64_t t_ns = data->t_ns;
+
+      // std::cerr << "t_ns " << t_ns << std::endl;
+      Sophus::SE3d T_w_i = data->T_w_i;
+      Eigen::Vector3d vel_w_i = data->vel_w_i;
+      Eigen::Vector3d bg = data->bias_gyro;
+      Eigen::Vector3d ba = data->bias_accel;
+
+      vio_t_w_i.emplace_back(T_w_i.translation());
+
+      {
+        std::vector<float> vals;
+        vals.push_back(t_ns * 1e-9);
+
+        for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(bg[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(ba[i]);
+
+        vio_data_log.Log(vals);
+      }
+    }
+
+    std::cout << "Finished t3" << std::endl;
+  });
+
+  if (show_gui) {
+    pangolin::CreateWindowAndBind("Main", 1800, 1000);
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::View& img_view_display =
+        pangolin::CreateDisplay()
+            .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.5)
+            .SetLayout(pangolin::LayoutEqual);
+
+    pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
+        0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0);
+
+    plotter = new pangolin::Plotter(&imu_data_log, 0.0, kf_t_ns.back() * 1e-9,
+                                    -10.0, 10.0, 0.01f, 0.01f);
+    plot_display.AddDisplay(*plotter);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+    while (img_view.size() < calib.intrinsics.size()) {
+      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
+
+      size_t idx = img_view.size();
+      img_view.push_back(iv);
+
+      img_view_display.AddDisplay(*iv);
+      iv->extern_draw_function =
+          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
+    }
+
+    pangolin::OpenGlRenderState camera(
+        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
+        pangolin::ModelViewLookAt(15, 3, 15, 0, 0, 0, pangolin::AxisZ));
+
+    pangolin::View& display3D =
+        pangolin::CreateDisplay()
+            .SetAspect(-640 / 480.0)
+            .SetBounds(0.4, 1.0, 0.5, 1.0)
+            .SetHandler(new pangolin::Handler3D(camera));
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      display3D.Activate(camera);
+      glClearColor(0.95f, 0.95f, 0.95f, 1.0f);
+
+      draw_scene();
+
+      img_view_display.Activate();
+
+      if (show_frame.GuiChanged()) {
+        for (size_t i = 0; i < calib.intrinsics.size(); i++) {
+          // img_view[i]->SetImage(images[i]);
+        }
+        draw_plots();
+      }
+
+      if (show_accel.GuiChanged() || show_gyro.GuiChanged() ||
+          show_gt_vel.GuiChanged() || show_gt_pos.GuiChanged() ||
+          show_gt_ba.GuiChanged() || show_gt_bg.GuiChanged() ||
+          show_est_vel.GuiChanged() || show_est_pos.GuiChanged() ||
+          show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) {
+        draw_plots();
+      }
+
+      pangolin::FinishFrame();
+
+      if (continue_btn) {
+        if (!next_step()) continue_btn = false;
+      } else {
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      }
+    }
+  }
+
+  t0.join();
+  t1.join();
+  t2.join();
+  t3.join();
+  // t4.join();
+
+  if (!result_path.empty()) {
+    Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
+
+    auto it = vis_map.find(kf_t_ns.back());
+
+    if (it != vis_map.end()) {
+      for (const auto& t : it->second->states)
+        vio_t_w_i.emplace_back(t.translation());
+
+    } else {
+      std::cerr << "Could not find results!!" << std::endl;
+    }
+
+    BASALT_ASSERT(kf_t_ns.size() == vio_t_w_i.size());
+
+    double error =
+        basalt::alignSVD(kf_t_ns, vio_t_w_i, gt_frame_t_ns, gt_frame_t_w_i);
+
+    std::ofstream os(result_path);
+    os << error << std::endl;
+    os.close();
+  }
+
+  return 0;
+}
+
+void draw_image_overlay(pangolin::View& v, size_t cam_id) {
+  UNUSED(v);
+
+  size_t frame_id = show_frame;
+  basalt::TimeCamId tcid(kf_t_ns[frame_id], cam_id);
+
+  if (show_obs) {
+    glLineWidth(1.0);
+    glColor3f(1.0, 0.0, 0.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (gt_observations.find(tcid) != gt_observations.end()) {
+      const basalt::SimObservations& cr = gt_observations.at(tcid);
+
+      for (size_t i = 0; i < cr.pos.size(); i++) {
+        const float radius = 2;
+        const Eigen::Vector2f c = cr.pos[i].cast<float>();
+        pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+        if (show_ids)
+          pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]);
+      }
+
+      pangolin::GlFont::I().Text("%d gt points", cr.pos.size()).Draw(5, 20);
+    }
+  }
+
+  if (show_obs_noisy) {
+    glLineWidth(1.0);
+    glColor3f(1.0, 1.0, 0.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (noisy_observations.find(tcid) != noisy_observations.end()) {
+      const basalt::SimObservations& cr = noisy_observations.at(tcid);
+
+      for (size_t i = 0; i < cr.pos.size(); i++) {
+        const float radius = 2;
+        const Eigen::Vector2f c = cr.pos[i].cast<float>();
+        pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+        if (show_ids)
+          pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]);
+      }
+
+      pangolin::GlFont::I().Text("%d noisy points", cr.pos.size()).Draw(5, 40);
+    }
+  }
+
+  if (show_obs_vio) {
+    glLineWidth(1.0);
+    glColor3f(0.0, 0.0, 1.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    auto it = vis_map.find(gt_frame_t_ns[frame_id]);
+
+    if (it != vis_map.end() && cam_id < it->second->projections.size()) {
+      const auto& points = it->second->projections[cam_id];
+
+      if (points.size() > 0) {
+        double min_id = points[0][2], max_id = points[0][2];
+        for (size_t i = 0; i < points.size(); i++) {
+          min_id = std::min(min_id, points[i][2]);
+          max_id = std::max(max_id, points[i][2]);
+        }
+
+        for (size_t i = 0; i < points.size(); i++) {
+          const float radius = 2;
+          const Eigen::Vector4d c = points[i];
+          pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+          if (show_ids)
+            pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]);
+        }
+      }
+
+      glColor3f(0.0, 0.0, 1.0);
+      pangolin::GlFont::I().Text("%d vio points", points.size()).Draw(5, 60);
+    }
+  }
+}
+
+void draw_scene() {
+  glPointSize(3);
+  glColor3f(1.0, 0.0, 0.0);
+  glEnable(GL_BLEND);
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+  glColor3ubv(gt_color);
+  pangolin::glDrawPoints(gt_points);
+  pangolin::glDrawLineStrip(gt_frame_t_w_i);
+
+  glColor3ubv(cam_color);
+  pangolin::glDrawLineStrip(vio_t_w_i);
+
+  size_t frame_id = show_frame;
+
+  auto it = vis_map.find(kf_t_ns[frame_id]);
+
+  if (it != vis_map.end()) {
+    for (const auto& p : it->second->states)
+      for (size_t i = 0; i < calib.T_i_c.size(); i++)
+        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, cam_color, 0.1f);
+
+    for (const auto& p : it->second->frames)
+      for (size_t i = 0; i < calib.T_i_c.size(); i++)
+        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);
+
+    glColor3ubv(pose_color);
+    pangolin::glDrawPoints(it->second->points);
+  }
+
+  // pangolin::glDrawAxis(gt_frame_T_w_i[frame_id].matrix(), 0.1);
+
+  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
+}
+
+void load_data(const std::string& calib_path) {
+  std::ifstream os(calib_path, std::ios::binary);
+
+  if (os.is_open()) {
+    cereal::JSONInputArchive archive(os);
+    archive(calib);
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
+              << std::endl;
+
+  } else {
+    std::cerr << "could not load camera calibration " << calib_path
+              << std::endl;
+    std::abort();
+  }
+}
+
+void gen_data() {
+  // Save spline data
+  {
+    std::string path = marg_data_path + "/gt_spline.cereal";
+
+    std::cout << "Loading gt_spline " << path << std::endl;
+
+    std::ifstream is(path, std::ios::binary);
+    {
+      cereal::JSONInputArchive archive(is);
+
+      int64_t t_ns;
+      Eigen::aligned_vector<Sophus::SE3d> knots;
+
+      archive(cereal::make_nvp("t_ns", t_ns));
+      archive(cereal::make_nvp("knots", knots));
+
+      gt_spline = basalt::Se3Spline<5>(t_ns);
+
+      for (size_t i = 0; i < knots.size(); i++) {
+        gt_spline.knotsPushBack(knots[i]);
+      }
+
+      archive(cereal::make_nvp("noisy_accel", noisy_accel));
+      archive(cereal::make_nvp("noisy_gyro", noisy_gyro));
+      archive(cereal::make_nvp("noisy_accel", gt_accel));
+      archive(cereal::make_nvp("gt_gyro", gt_gyro));
+      archive(cereal::make_nvp("gt_accel_bias", gt_accel_bias));
+      archive(cereal::make_nvp("gt_gyro_bias", gt_gyro_bias));
+
+      archive(cereal::make_nvp("gt_points", gt_points));
+
+      archive(cereal::make_nvp("gt_observations", gt_observations));
+      archive(cereal::make_nvp("noisy_observations", noisy_observations));
+
+      archive(cereal::make_nvp("gt_points", gt_points));
+
+      archive(cereal::make_nvp("gt_frame_t_ns", gt_frame_t_ns));
+      archive(cereal::make_nvp("gt_imu_t_ns", gt_imu_t_ns));
+    }
+
+    gt_frame_t_w_i.clear();
+    for (int64_t t_ns : gt_frame_t_ns) {
+      gt_frame_t_w_i.emplace_back(gt_spline.pose(t_ns).translation());
+    }
+
+    is.close();
+  }
+
+  basalt::MargDataLoader mdl;
+  tbb::concurrent_bounded_queue<basalt::MargData::Ptr> marg_queue;
+  mdl.out_marg_queue = &marg_queue;
+
+  mdl.start(marg_data_path);
+
+  Eigen::aligned_map<int64_t, Sophus::SE3d> tmp_poses;
+
+  while (true) {
+    basalt::MargData::Ptr data;
+    marg_queue.pop(data);
+
+    if (data.get()) {
+      for (const auto& kv : data->frame_poses) {
+        tmp_poses[kv.first] = kv.second.getPose();
+      }
+
+      for (const auto& kv : data->frame_states) {
+        if (data->kfs_all.count(kv.first) > 0) {
+          tmp_poses[kv.first] = kv.second.getState().T_w_i;
+        }
+      }
+
+    } else {
+      break;
+    }
+  }
+
+  for (const auto& kv : tmp_poses) {
+    kf_t_ns.emplace_back(kv.first);
+  }
+
+  show_frame.Meta().range[1] = kf_t_ns.size() - 1;
+}
+
+void draw_plots() {
+  plotter->ClearSeries();
+  plotter->ClearMarkers();
+
+  if (show_accel) {
+    plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "accel measurements x");
+    plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "accel measurements y");
+    plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "accel measurements z");
+  }
+
+  if (show_gyro) {
+    plotter->AddSeries("$0", "$4", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "gyro measurements x");
+    plotter->AddSeries("$0", "$5", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "gyro measurements y");
+    plotter->AddSeries("$0", "$6", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "gyro measurements z");
+  }
+
+  if (show_gt_vel) {
+    plotter->AddSeries("$0", "$7", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth velocity x");
+    plotter->AddSeries("$0", "$8", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth velocity y");
+    plotter->AddSeries("$0", "$9", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth velocity z");
+  }
+
+  if (show_gt_pos) {
+    plotter->AddSeries("$0", "$10", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth position x");
+    plotter->AddSeries("$0", "$11", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth position y");
+    plotter->AddSeries("$0", "$12", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth position z");
+  }
+
+  if (show_gt_bg) {
+    plotter->AddSeries("$0", "$13", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth gyro bias x");
+    plotter->AddSeries("$0", "$14", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth gyro bias y");
+    plotter->AddSeries("$0", "$15", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth gyro bias z");
+  }
+
+  if (show_gt_ba) {
+    plotter->AddSeries("$0", "$16", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth accel bias x");
+    plotter->AddSeries("$0", "$17", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth accel bias y");
+    plotter->AddSeries("$0", "$18", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth accel bias z");
+  }
+
+  if (show_est_vel) {
+    plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated velocity x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated velocity y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated velocity z",
+                       &vio_data_log);
+  }
+
+  if (show_est_pos) {
+    plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated position x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated position y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated position z",
+                       &vio_data_log);
+  }
+
+  if (show_est_bg) {
+    plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated gyro bias x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated gyro bias y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated gyro bias z",
+                       &vio_data_log);
+  }
+
+  if (show_est_ba) {
+    plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated accel bias x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated accel bias y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated accel bias z",
+                       &vio_data_log);
+  }
+
+  double t = kf_t_ns[show_frame] * 1e-9;
+  plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal,
+                     pangolin::Colour::White());
+}
+
+void setup_vio() {
+  int64_t t_init_ns = kf_t_ns.front();
+  Sophus::SE3d T_w_i_init = gt_spline.pose(t_init_ns);
+  Eigen::Vector3d vel_w_i_init = gt_spline.transVelWorld(t_init_ns);
+
+  std::cout << "Setting up filter: t_ns " << t_init_ns << std::endl;
+  std::cout << "T_w_i\n" << T_w_i_init.matrix() << std::endl;
+  std::cout << "vel_w_i " << vel_w_i_init.transpose() << std::endl;
+
+  basalt::VioConfig config;
+  config.vio_debug = true;
+
+  vio = basalt::VioEstimatorFactory::getVioEstimator(config, calib, g, true,
+                                                     true);
+  vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
+                  gt_accel_bias.front());
+
+  vio->setMaxStates(10000);
+  vio->setMaxKfs(10000);
+
+  // int iteration = 0;
+  vio_data_log.Clear();
+  error_data_log.Clear();
+  vio_t_w_i.clear();
+}
+
+bool next_step() {
+  if (show_frame < int(kf_t_ns.size()) - 1) {
+    show_frame = show_frame + 1;
+    show_frame.Meta().gui_changed = true;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void alignButton() {
+  Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
+
+  auto it = vis_map.find(kf_t_ns.back());
+
+  if (it != vis_map.end()) {
+    for (const auto& t : it->second->states)
+      vio_t_w_i.emplace_back(t.translation());
+
+  } else {
+    std::cerr << "Could not find results!!" << std::endl;
+  }
+
+  BASALT_ASSERT(kf_t_ns.size() == vio_t_w_i.size());
+
+  basalt::alignSVD(kf_t_ns, vio_t_w_i, gt_frame_t_ns, gt_frame_t_w_i);
+}

src/opt_flow.cpp

@@ -0,0 +1,356 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <sophus/se3.hpp>
+
+#include <tbb/concurrent_unordered_map.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/spline/se3_spline.h>
+
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/optical_flow/optical_flow.h>
+
+#include <basalt/serialization/headers_serialization.h>
+
+constexpr int UI_WIDTH = 200;
+
+void draw_image_overlay(pangolin::View& v, size_t cam_id);
+void load_data(const std::string& calib_path);
+bool next_step();
+bool prev_step();
+
+pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1500);
+pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
+pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
+
+using Button = pangolin::Var<std::function<void(void)>>;
+Button next_step_btn("ui.next_step", &next_step);
+Button prev_step_btn("ui.prev_step", &prev_step);
+pangolin::Var<bool> continue_btn("ui.continue", true, false, true);
+
+// Opt flow variables
+basalt::VioDatasetPtr vio_dataset;
+
+basalt::VioConfig vio_config;
+basalt::OpticalFlowBase::Ptr opt_flow_ptr;
+
+tbb::concurrent_unordered_map<int64_t, basalt::OpticalFlowResult::Ptr,
+                              std::hash<int64_t>>
+    observations;
+tbb::concurrent_bounded_queue<basalt::OpticalFlowResult::Ptr>
+    observations_queue;
+
+basalt::Calibration<double> calib;
+
+std::unordered_map<basalt::KeypointId, int> keypoint_stats;
+
+void feed_images() {
+  std::cout << "Started input_data thread " << std::endl;
+
+  for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
+    basalt::OpticalFlowInput::Ptr data(new basalt::OpticalFlowInput);
+
+    data->t_ns = vio_dataset->get_image_timestamps()[i];
+    data->img_data = vio_dataset->get_image_data(data->t_ns);
+
+    opt_flow_ptr->input_queue.push(data);
+  }
+
+  // Indicate the end of the sequence
+  basalt::OpticalFlowInput::Ptr data;
+  opt_flow_ptr->input_queue.push(data);
+
+  std::cout << "Finished input_data thread " << std::endl;
+}
+
+void read_result() {
+  std::cout << "Started read_result thread " << std::endl;
+
+  basalt::OpticalFlowResult::Ptr res;
+
+  while (true) {
+    observations_queue.pop(res);
+    if (!res.get()) break;
+
+    res->input_images.reset();
+
+    observations.emplace(res->t_ns, res);
+
+    for (size_t i = 0; i < res->observations.size(); i++)
+      for (const auto& kv : res->observations.at(i)) {
+        if (keypoint_stats.count(kv.first) == 0) {
+          keypoint_stats[kv.first] = 1;
+        } else {
+          keypoint_stats[kv.first]++;
+        }
+      }
+  }
+
+  std::cout << "Finished read_result thread " << std::endl;
+
+  double sum = 0;
+
+  for (const auto& kv : keypoint_stats) {
+    sum += kv.second;
+  }
+
+  std::cout << "Mean track length: " << sum / keypoint_stats.size()
+            << " num_points: " << keypoint_stats.size() << std::endl;
+}
+
+int main(int argc, char** argv) {
+  bool show_gui = true;
+  std::string cam_calib_path;
+  std::string dataset_path;
+  std::string dataset_type;
+  std::string config_path;
+
+  CLI::App app{"App description"};
+
+  app.add_option("--show-gui", show_gui, "Show GUI");
+  app.add_option("--cam-calib", cam_calib_path,
+                 "Ground-truth camera calibration used for simulation.")
+      ->required();
+
+  app.add_option("--dataset-path", dataset_path, "Path to dataset.")
+      ->required();
+
+  app.add_option("--dataset-type", dataset_type, "Type of dataset.")
+      ->required();
+
+  app.add_option("--config-path", config_path, "Path to config file.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  if (!config_path.empty()) {
+    vio_config.load(config_path);
+  }
+
+  load_data(cam_calib_path);
+
+  {
+    basalt::DatasetIoInterfacePtr dataset_io =
+        basalt::DatasetIoFactory::getDatasetIo(dataset_type);
+
+    dataset_io->read(dataset_path);
+
+    vio_dataset = dataset_io->get_data();
+    vio_dataset->get_image_timestamps().erase(
+        vio_dataset->get_image_timestamps().begin());
+
+    show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1;
+    show_frame.Meta().gui_changed = true;
+
+    opt_flow_ptr =
+        basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib);
+    if (show_gui) opt_flow_ptr->output_queue = &observations_queue;
+    observations_queue.set_capacity(100);
+
+    keypoint_stats.reserve(50000);
+  }
+
+  std::thread t1(&feed_images);
+
+  if (show_gui) {
+    std::thread t2(&read_result);
+
+    pangolin::CreateWindowAndBind("Main", 1800, 1000);
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::View& img_view_display =
+        pangolin::CreateDisplay()
+            .SetBounds(0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0)
+            .SetLayout(pangolin::LayoutEqual);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+    while (img_view.size() < calib.intrinsics.size()) {
+      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
+
+      size_t idx = img_view.size();
+      img_view.push_back(iv);
+
+      img_view_display.AddDisplay(*iv);
+      iv->extern_draw_function =
+          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
+    }
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      glClearColor(0.95f, 0.95f, 0.95f, 1.0f);
+
+      img_view_display.Activate();
+
+      if (show_frame.GuiChanged()) {
+        size_t frame_id = static_cast<size_t>(show_frame);
+        int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id];
+
+        const std::vector<basalt::ImageData>& img_vec =
+            vio_dataset->get_image_data(timestamp);
+
+        for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) {
+          if (img_vec[cam_id].img.get()) {
+            auto img = img_vec[cam_id].img;
+
+            pangolin::GlPixFormat fmt;
+            fmt.glformat = GL_LUMINANCE;
+            fmt.gltype = GL_UNSIGNED_SHORT;
+            fmt.scalable_internal_format = GL_LUMINANCE16;
+
+            img_view[cam_id]->SetImage(img->ptr, img->w, img->h, img->pitch,
+                                       fmt);
+          } else {
+            img_view[cam_id]->Clear();
+          }
+        }
+      }
+
+      pangolin::FinishFrame();
+
+      if (continue_btn) {
+        if (!next_step()) {
+          continue_btn = false;
+        }
+      } else {
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      }
+    }
+
+    t2.join();
+  }
+
+  t1.join();
+
+  return 0;
+}
+
+void draw_image_overlay(pangolin::View& v, size_t cam_id) {
+  UNUSED(v);
+
+  size_t frame_id = static_cast<size_t>(show_frame);
+  int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
+
+  if (show_obs) {
+    glLineWidth(1.0);
+    glColor3f(1.0, 0.0, 0.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (observations.count(t_ns) > 0) {
+      const Eigen::aligned_map<basalt::KeypointId, Eigen::AffineCompact2f>&
+          kp_map = observations.at(t_ns)->observations[cam_id];
+
+      for (const auto& kv : kp_map) {
+        Eigen::MatrixXf transformed_patch =
+            kv.second.linear() * opt_flow_ptr->patch_coord;
+        transformed_patch.colwise() += kv.second.translation();
+
+        for (int i = 0; i < transformed_patch.cols(); i++) {
+          const Eigen::Vector2f c = transformed_patch.col(i);
+          pangolin::glDrawCirclePerimeter(c[0], c[1], 0.5f);
+        }
+
+        const Eigen::Vector2f c = kv.second.translation();
+
+        if (show_ids)
+          pangolin::GlFont::I().Text("%d", kv.first).Draw(5 + c[0], 5 + c[1]);
+      }
+
+      pangolin::GlFont::I()
+          .Text("Tracked %d keypoints", kp_map.size())
+          .Draw(5, 20);
+    }
+  }
+}
+
+void load_data(const std::string& calib_path) {
+  std::ifstream os(calib_path, std::ios::binary);
+
+  if (os.is_open()) {
+    cereal::JSONInputArchive archive(os);
+    archive(calib);
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
+              << std::endl;
+
+  } else {
+    std::cerr << "could not load camera calibration " << calib_path
+              << std::endl;
+    std::abort();
+  }
+}
+
+bool next_step() {
+  if (show_frame < int(vio_dataset->get_image_timestamps().size()) - 1) {
+    show_frame = show_frame + 1;
+    show_frame.Meta().gui_changed = true;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool prev_step() {
+  if (show_frame > 0) {
+    show_frame = show_frame - 1;
+    show_frame.Meta().gui_changed = true;
+    return true;
+  } else {
+    return false;
+  }
+}

src/optical_flow/optical_flow.cpp

@@ -0,0 +1,131 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/optical_flow/optical_flow.h>
+
+#include <basalt/optical_flow/frame_to_frame_optical_flow.h>
+#include <basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h>
+#include <basalt/optical_flow/patch_optical_flow.h>
+
+namespace basalt {
+
+OpticalFlowBase::Ptr OpticalFlowFactory::getOpticalFlow(
+    const VioConfig& config, const Calibration<double>& cam) {
+  OpticalFlowBase::Ptr res;
+
+  if (config.optical_flow_type == "patch") {
+    switch (config.optical_flow_pattern) {
+      case 24:
+        res.reset(new PatchOpticalFlow<float, Pattern24>(config, cam));
+        break;
+
+      case 52:
+        res.reset(new PatchOpticalFlow<float, Pattern52>(config, cam));
+        break;
+
+      case 51:
+        res.reset(new PatchOpticalFlow<float, Pattern51>(config, cam));
+        break;
+
+      case 50:
+        res.reset(new PatchOpticalFlow<float, Pattern50>(config, cam));
+        break;
+
+      default:
+        std::cerr << "config.optical_flow_pattern "
+                  << config.optical_flow_pattern << " is not supported."
+                  << std::endl;
+        std::abort();
+    }
+  }
+
+  if (config.optical_flow_type == "frame_to_frame") {
+    switch (config.optical_flow_pattern) {
+      case 24:
+        res.reset(new FrameToFrameOpticalFlow<float, Pattern24>(config, cam));
+        break;
+
+      case 52:
+        res.reset(new FrameToFrameOpticalFlow<float, Pattern52>(config, cam));
+        break;
+
+      case 51:
+        res.reset(new FrameToFrameOpticalFlow<float, Pattern51>(config, cam));
+        break;
+
+      case 50:
+        res.reset(new FrameToFrameOpticalFlow<float, Pattern50>(config, cam));
+        break;
+
+      default:
+        std::cerr << "config.optical_flow_pattern "
+                  << config.optical_flow_pattern << " is not supported."
+                  << std::endl;
+        std::abort();
+    }
+  }
+
+  if (config.optical_flow_type == "multiscale_frame_to_frame") {
+    switch (config.optical_flow_pattern) {
+      case 24:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern24>(
+            config, cam));
+        break;
+
+      case 52:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern52>(
+            config, cam));
+        break;
+
+      case 51:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern51>(
+            config, cam));
+        break;
+
+      case 50:
+        res.reset(new MultiscaleFrameToFrameOpticalFlow<float, Pattern50>(
+            config, cam));
+        break;
+
+      default:
+        std::cerr << "config.optical_flow_pattern "
+                  << config.optical_flow_pattern << " is not supported."
+                  << std::endl;
+        std::abort();
+    }
+  }
+  return res;
+}
+}  // namespace basalt

src/rs_t265_record.cpp

@@ -0,0 +1,503 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <atomic>
+#include <cmath>
+#include <cstring>
+#include <fstream>
+#include <iomanip>
+#include <thread>
+
+#include <librealsense2/rs.hpp>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <tbb/concurrent_queue.h>
+
+#include <basalt/device/rs_t265.h>
+#include <basalt/serialization/headers_serialization.h>
+#include <basalt/utils/filesystem.h>
+#include <CLI/CLI.hpp>
+#include <cereal/archives/json.hpp>
+
+constexpr int UI_WIDTH = 200;
+
+basalt::RsT265Device::Ptr t265_device;
+
+std::shared_ptr<pangolin::DataLog> imu_log;
+
+pangolin::Var<int> webp_quality("ui.webp_quality", 90, 0, 101);
+pangolin::Var<int> skip_frames("ui.skip_frames", 1, 1, 10);
+pangolin::Var<float> exposure("ui.exposure", 5.0, 1, 20);
+
+tbb::concurrent_bounded_queue<basalt::OpticalFlowInput::Ptr> image_data_queue,
+    image_data_queue2;
+tbb::concurrent_bounded_queue<basalt::ImuData<double>::Ptr> imu_data_queue;
+tbb::concurrent_bounded_queue<basalt::RsPoseData> pose_data_queue;
+
+std::atomic<bool> stop_workers;
+std::atomic<bool> recording;
+
+std::string dataset_dir;
+
+static constexpr int NUM_CAMS = basalt::RsT265Device::NUM_CAMS;
+static constexpr int NUM_WORKERS = 8;
+
+std::ofstream cam_data[NUM_CAMS], exposure_data[NUM_CAMS], imu0_data, pose_data;
+
+std::vector<std::thread> worker_threads;
+std::thread imu_worker_thread, pose_worker_thread, exposure_save_thread,
+    stop_recording_thread;
+
+#if CV_MAJOR_VERSION >= 3
+std::string file_extension = ".webp";
+#else
+std::string file_extension = ".jpg";
+#endif
+
+// manual exposure mode, if not enabled will also record pose data
+bool manual_exposure;
+
+void exposure_save_worker() {
+  basalt::OpticalFlowInput::Ptr img;
+  while (!stop_workers) {
+    if (image_data_queue.try_pop(img)) {
+      for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
+        cam_data[cam_id] << img->t_ns << "," << img->t_ns << file_extension
+                         << std::endl;
+
+        exposure_data[cam_id] << img->t_ns << ","
+                              << int64_t(img->img_data[cam_id].exposure * 1e9)
+                              << std::endl;
+      }
+
+      image_data_queue2.push(img);
+    } else {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+}
+
+void image_save_worker() {
+  basalt::OpticalFlowInput::Ptr img;
+
+  while (!stop_workers) {
+    if (image_data_queue2.try_pop(img)) {
+      for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
+        basalt::ManagedImage<uint16_t>::Ptr image_raw =
+            img->img_data[cam_id].img;
+
+        if (!image_raw.get()) continue;
+
+        cv::Mat image(image_raw->h, image_raw->w, CV_8U);
+
+        uint8_t *dst = image.ptr();
+        const uint16_t *src = image_raw->ptr;
+
+        for (size_t i = 0; i < image_raw->size(); i++) {
+          dst[i] = (src[i] >> 8);
+        }
+
+#if CV_MAJOR_VERSION >= 3
+        std::string filename = dataset_dir + "mav0/cam" +
+                               std::to_string(cam_id) + "/data/" +
+                               std::to_string(img->t_ns) + ".webp";
+
+        std::vector<int> compression_params = {cv::IMWRITE_WEBP_QUALITY,
+                                               webp_quality};
+        cv::imwrite(filename, image, compression_params);
+#else
+        std::string filename = dataset_dir + "mav0/cam" +
+                               std::to_string(cam_id) + "/data/" +
+                               std::to_string(img->t_ns) + ".jpg";
+
+        std::vector<int> compression_params = {cv::IMWRITE_JPEG_QUALITY,
+                                               webp_quality};
+        cv::imwrite(filename, image, compression_params);
+#endif
+      }
+
+    } else {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+}
+
+void imu_save_worker() {
+  basalt::ImuData<double>::Ptr data;
+
+  while (!stop_workers) {
+    if (imu_data_queue.try_pop(data)) {
+      if (imu_log.get())
+        imu_log->Log(data->accel[0], data->accel[1], data->accel[2]);
+
+      if (recording) {
+        imu0_data << data->t_ns << "," << data->gyro[0] << "," << data->gyro[1]
+                  << "," << data->gyro[2] << "," << data->accel[0] << ","
+                  << data->accel[1] << "," << data->accel[2] << "\n";
+      }
+
+    } else {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+}
+
+void pose_save_worker() {
+  basalt::RsPoseData data;
+
+  while (!stop_workers) {
+    if (pose_data_queue.try_pop(data)) {
+      if (recording) {
+        pose_data << data.t_ns << "," << data.data.translation().x() << ","
+                  << data.data.translation().y() << ","
+                  << data.data.translation().z() << ","
+                  << data.data.unit_quaternion().w() << ","
+                  << data.data.unit_quaternion().x() << ","
+                  << data.data.unit_quaternion().y() << ","
+                  << data.data.unit_quaternion().z() << std::endl;
+      }
+
+    } else {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+}
+
+void save_calibration(const basalt::RsT265Device::Ptr &device) {
+  auto calib = device->exportCalibration();
+
+  if (calib) {
+    std::ofstream os(dataset_dir + "/calibration.json");
+    cereal::JSONOutputArchive archive(os);
+
+    archive(*calib);
+  }
+}
+
+inline std::string get_date() {
+  constexpr int MAX_DATE = 64;
+  time_t now;
+  char the_date[MAX_DATE];
+
+  the_date[0] = '\0';
+
+  now = time(nullptr);
+
+  if (now != -1) {
+    strftime(the_date, MAX_DATE, "%Y_%m_%d_%H_%M_%S", gmtime(&now));
+  }
+
+  return std::string(the_date);
+}
+
+void startRecording(const std::string &dir_path) {
+  if (!recording) {
+    if (stop_recording_thread.joinable()) stop_recording_thread.join();
+
+    dataset_dir = dir_path + "dataset_" + get_date() + "/";
+
+    basalt::fs::create_directory(dataset_dir);
+    basalt::fs::create_directory(dataset_dir + "mav0/");
+    basalt::fs::create_directory(dataset_dir + "mav0/cam0/");
+    basalt::fs::create_directory(dataset_dir + "mav0/cam0/data/");
+    basalt::fs::create_directory(dataset_dir + "mav0/cam1/");
+    basalt::fs::create_directory(dataset_dir + "mav0/cam1/data/");
+    basalt::fs::create_directory(dataset_dir + "mav0/imu0/");
+
+    cam_data[0].open(dataset_dir + "mav0/cam0/data.csv");
+    cam_data[1].open(dataset_dir + "mav0/cam1/data.csv");
+    exposure_data[0].open(dataset_dir + "mav0/cam0/exposure.csv");
+    exposure_data[1].open(dataset_dir + "mav0/cam1/exposure.csv");
+    imu0_data.open(dataset_dir + "mav0/imu0/data.csv");
+
+    if (!manual_exposure) {
+      basalt::fs::create_directory(dataset_dir + "mav0/realsense0/");
+      pose_data.open(dataset_dir + "mav0/realsense0/data.csv");
+      pose_data << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], "
+                   "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n";
+    }
+
+    cam_data[0] << "#timestamp [ns], filename\n";
+    cam_data[1] << "#timestamp [ns], filename\n";
+    exposure_data[0] << "#timestamp [ns], exposure time[ns]\n";
+    exposure_data[1] << "#timestamp [ns], exposure time[ns]\n";
+    imu0_data << "#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad "
+                 "s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y "
+                 "[m s^-2],a_RS_S_z [m s^-2]\n";
+
+    save_calibration(t265_device);
+    t265_device->image_data_queue = &image_data_queue;
+    t265_device->imu_data_queue = &imu_data_queue;
+    t265_device->pose_data_queue = &pose_data_queue;
+
+    std::cout << "Started recording dataset in " << dataset_dir << std::endl;
+
+    recording = true;
+  } else {
+    std::cout << "Already recording" << std::endl;
+  }
+}
+
+void stopRecording() {
+  if (recording) {
+    auto stop_recording_func = [&]() {
+      t265_device->imu_data_queue = nullptr;
+      t265_device->pose_data_queue = nullptr;
+      t265_device->image_data_queue = nullptr;
+
+      while (!image_data_queue.empty() || !image_data_queue2.empty() ||
+             !imu_data_queue.empty() || !pose_data_queue.empty()) {
+        std::cout << "Waiting until the data from the queues is written to the "
+                     "hard drive."
+                  << std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+      }
+
+      recording = false;
+      cam_data[0].close();
+      cam_data[1].close();
+      exposure_data[0].close();
+      exposure_data[1].close();
+      imu0_data.close();
+      pose_data.close();
+
+      std::cout << "Stopped recording dataset in " << dataset_dir << std::endl;
+    };
+
+    stop_recording_thread = std::thread(stop_recording_func);
+  }
+}
+
+void toggleRecording(const std::string &dir_path) {
+  if (recording) {
+    stopRecording();
+  } else {
+    startRecording(dir_path);
+  }
+}
+
+int main(int argc, char *argv[]) {
+  CLI::App app{"Record RealSense T265 Data"};
+
+  std::string dataset_path;
+
+  app.add_option("--dataset-path", dataset_path, "Path to dataset");
+  app.add_flag("--manual-exposure", manual_exposure,
+               "If set will enable manual exposure.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError &e) {
+    return app.exit(e);
+  }
+
+  if (!dataset_path.empty() && dataset_path[dataset_path.length() - 1] != '/') {
+    dataset_path += '/';
+  }
+
+  bool show_gui = true;
+
+  stop_workers = false;
+  if (worker_threads.empty()) {
+    for (int i = 0; i < NUM_WORKERS; i++) {
+      worker_threads.emplace_back(image_save_worker);
+    }
+  }
+
+  exposure_save_thread = std::thread(exposure_save_worker);
+  imu_worker_thread = std::thread(imu_save_worker);
+  pose_worker_thread = std::thread(pose_save_worker);
+
+  image_data_queue.set_capacity(1000);
+  image_data_queue2.set_capacity(1000);
+  imu_data_queue.set_capacity(10000);
+  pose_data_queue.set_capacity(10000);
+
+  // realsense
+  t265_device.reset(new basalt::RsT265Device(manual_exposure, skip_frames,
+                                             webp_quality, exposure));
+
+  t265_device->start();
+  imu_log.reset(new pangolin::DataLog);
+
+  if (show_gui) {
+    pangolin::CreateWindowAndBind("Record RealSense T265", 1200, 800);
+
+    pangolin::Var<std::function<void(void)>> record_btn(
+        "ui.record", [&] { return toggleRecording(dataset_path); });
+    pangolin::Var<std::function<void(void)>> export_calibration(
+        "ui.export_calib", [&] { return save_calibration(t265_device); });
+
+    std::atomic<int64_t> record_t_ns;
+    record_t_ns = 0;
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::View &img_view_display =
+        pangolin::CreateDisplay()
+            .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0)
+            .SetLayout(pangolin::LayoutEqual);
+
+    pangolin::View &plot_display = pangolin::CreateDisplay().SetBounds(
+        0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+    while (img_view.size() < basalt::RsT265Device::NUM_CAMS) {
+      int idx = img_view.size();
+      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
+
+      iv->extern_draw_function = [&, idx](pangolin::View &v) {
+        UNUSED(v);
+
+        glLineWidth(1.0);
+        glColor3f(1.0, 0.0, 0.0);  // red
+        glEnable(GL_BLEND);
+        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+        if (t265_device->last_img_data.get())
+          pangolin::GlFont::I()
+              .Text("Exposure: %.3f ms.",
+                    t265_device->last_img_data->img_data[idx].exposure * 1000.0)
+              .Draw(30, 30);
+
+        if (idx == 0) {
+          pangolin::GlFont::I()
+              .Text("Queue: %d.", image_data_queue2.size())
+              .Draw(30, 60);
+        }
+
+        if (idx == 0 && recording) {
+          pangolin::GlFont::I().Text("Recording").Draw(30, 90);
+        }
+      };
+
+      iv->OnSelectionCallback =
+          [&](pangolin::ImageView::OnSelectionEventData o) {
+            UNUSED(o);
+
+            int64_t curr_t_ns = std::chrono::high_resolution_clock::now()
+                                    .time_since_epoch()
+                                    .count();
+            if (std::abs(record_t_ns - curr_t_ns) > int64_t(2e9)) {
+              toggleRecording(dataset_path);
+              record_t_ns = curr_t_ns;
+            }
+          };
+
+      img_view.push_back(iv);
+      img_view_display.AddDisplay(*iv);
+    }
+
+    imu_log->Clear();
+
+    std::vector<std::string> labels;
+    labels.push_back(std::string("accel x"));
+    labels.push_back(std::string("accel y"));
+    labels.push_back(std::string("accel z"));
+    imu_log->SetLabels(labels);
+
+    pangolin::Plotter plotter(imu_log.get(), 0.0f, 2000.0f, -15.0f, 15.0f, 0.1f,
+                              0.1f);
+    plotter.SetBounds(0.0, 1.0, 0.0, 1.0);
+    plotter.Track("$i");
+
+    plot_display.AddDisplay(plotter);
+
+    plotter.ClearSeries();
+    plotter.AddSeries("$i", "$0", pangolin::DrawingModeLine,
+                      pangolin::Colour::Red(), "accel x");
+    plotter.AddSeries("$i", "$1", pangolin::DrawingModeLine,
+                      pangolin::Colour::Green(), "accel y");
+    plotter.AddSeries("$i", "$2", pangolin::DrawingModeLine,
+                      pangolin::Colour::Blue(), "accel z");
+
+    while (!pangolin::ShouldQuit()) {
+      {
+        pangolin::GlPixFormat fmt;
+        fmt.glformat = GL_LUMINANCE;
+        fmt.gltype = GL_UNSIGNED_SHORT;
+        fmt.scalable_internal_format = GL_LUMINANCE16;
+
+        if (t265_device->last_img_data.get())
+          for (size_t cam_id = 0; cam_id < basalt::RsT265Device::NUM_CAMS;
+               cam_id++) {
+            if (t265_device->last_img_data->img_data[cam_id].img.get())
+              img_view[cam_id]->SetImage(
+                  t265_device->last_img_data->img_data[cam_id].img->ptr,
+                  t265_device->last_img_data->img_data[cam_id].img->w,
+                  t265_device->last_img_data->img_data[cam_id].img->h,
+                  t265_device->last_img_data->img_data[cam_id].img->pitch, fmt);
+          }
+      }
+
+      if (manual_exposure && exposure.GuiChanged()) {
+        t265_device->setExposure(exposure);
+      }
+
+      if (webp_quality.GuiChanged()) {
+        t265_device->setWebpQuality(webp_quality);
+      }
+
+      if (skip_frames.GuiChanged()) {
+        t265_device->setSkipFrames(skip_frames);
+      }
+
+      pangolin::FinishFrame();
+
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+
+  if (recording) stopRecording();
+  stop_workers = true;
+
+  for (auto &t : worker_threads) t.join();
+  imu_worker_thread.join();
+  pose_worker_thread.join();
+
+  return EXIT_SUCCESS;
+}

src/rs_t265_vio.cpp

@@ -0,0 +1,503 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <condition_variable>
+#include <iostream>
+#include <memory>
+#include <thread>
+
+#include <sophus/se3.hpp>
+
+#include <tbb/concurrent_queue.h>
+#include <tbb/global_control.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/device/rs_t265.h>
+#include <basalt/io/dataset_io.h>
+#include <basalt/io/marg_data_io.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/vi_estimator/vio_estimator.h>
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/serialization/headers_serialization.h>
+
+#include <basalt/utils/vis_utils.h>
+
+// GUI functions
+void draw_image_overlay(pangolin::View& v, size_t cam_id);
+void draw_scene();
+void load_data(const std::string& calib_path);
+void draw_plots();
+
+// Pangolin variables
+constexpr int UI_WIDTH = 200;
+
+basalt::RsT265Device::Ptr t265_device;
+
+using Button = pangolin::Var<std::function<void(void)>>;
+
+pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
+pangolin::Plotter* plotter;
+
+pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
+pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
+
+pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
+pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
+pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
+pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);
+
+pangolin::Var<bool> show_gt("ui.show_gt", true, false, true);
+
+pangolin::Var<bool> follow("ui.follow", true, false, true);
+
+// Visualization variables
+basalt::VioVisualizationData::Ptr curr_vis_data;
+
+tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
+tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
+    out_state_queue;
+
+std::vector<int64_t> vio_t_ns;
+Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
+
+std::string marg_data_path;
+
+std::mutex m;
+bool step_by_step = false;
+int64_t curr_t_ns = -1;
+
+// VIO variables
+basalt::Calibration<double> calib;
+
+basalt::VioConfig vio_config;
+basalt::OpticalFlowBase::Ptr opt_flow_ptr;
+basalt::VioEstimatorBase::Ptr vio;
+
+int main(int argc, char** argv) {
+  bool terminate = false;
+  bool show_gui = true;
+  bool print_queue = false;
+  std::string cam_calib_path;
+  std::string config_path;
+  int num_threads = 0;
+  bool use_double = false;
+
+  CLI::App app{"RealSense T265 Live Vio"};
+
+  app.add_option("--show-gui", show_gui, "Show GUI");
+  app.add_option("--cam-calib", cam_calib_path,
+                 "Ground-truth camera calibration used for simulation.");
+
+  app.add_option("--marg-data", marg_data_path,
+                 "Path to folder where marginalization data will be stored.");
+
+  app.add_option("--print-queue", print_queue, "Print queue.");
+  app.add_option("--config-path", config_path, "Path to config file.");
+  app.add_option("--num-threads", num_threads, "Number of threads.");
+  app.add_option("--step-by-step", step_by_step, "Path to config file.");
+  app.add_option("--use-double", use_double, "Use double not float.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  // global thread limit is in effect until global_control object is destroyed
+  std::unique_ptr<tbb::global_control> tbb_global_control;
+  if (num_threads > 0) {
+    tbb_global_control = std::make_unique<tbb::global_control>(
+        tbb::global_control::max_allowed_parallelism, num_threads);
+  }
+
+  if (!config_path.empty()) {
+    vio_config.load(config_path);
+  } else {
+    vio_config.optical_flow_skip_frames = 2;
+  }
+
+  // realsense
+  t265_device.reset(
+      new basalt::RsT265Device(false, 1, 90, 10.0));  // TODO: add options?
+
+  // startup device and load calibration
+  t265_device->start();
+  if (cam_calib_path.empty()) {
+    calib = *t265_device->exportCalibration();
+  } else {
+    load_data(cam_calib_path);
+  }
+
+  opt_flow_ptr = basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib);
+  t265_device->image_data_queue = &opt_flow_ptr->input_queue;
+
+  vio = basalt::VioEstimatorFactory::getVioEstimator(
+      vio_config, calib, basalt::constants::g, true, use_double);
+  vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+  t265_device->imu_data_queue = &vio->imu_data_queue;
+
+  opt_flow_ptr->output_queue = &vio->vision_data_queue;
+  if (show_gui) vio->out_vis_queue = &out_vis_queue;
+  vio->out_state_queue = &out_state_queue;
+
+  basalt::MargDataSaver::Ptr marg_data_saver;
+
+  if (!marg_data_path.empty()) {
+    marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path));
+    vio->out_marg_queue = &marg_data_saver->in_marg_queue;
+  }
+
+  vio_data_log.Clear();
+
+  std::shared_ptr<std::thread> t3;
+
+  if (show_gui)
+    t3.reset(new std::thread([&]() {
+      while (true) {
+        out_vis_queue.pop(curr_vis_data);
+
+        if (!curr_vis_data.get()) break;
+      }
+
+      std::cout << "Finished t3" << std::endl;
+    }));
+
+  std::thread t4([&]() {
+    basalt::PoseVelBiasState<double>::Ptr data;
+
+    while (true) {
+      out_state_queue.pop(data);
+
+      if (!data.get()) break;
+
+      int64_t t_ns = data->t_ns;
+
+      if (curr_t_ns < 0) curr_t_ns = t_ns;
+
+      // std::cerr << "t_ns " << t_ns << std::endl;
+      Sophus::SE3d T_w_i = data->T_w_i;
+      Eigen::Vector3d vel_w_i = data->vel_w_i;
+      Eigen::Vector3d bg = data->bias_gyro;
+      Eigen::Vector3d ba = data->bias_accel;
+
+      vio_t_ns.emplace_back(data->t_ns);
+      vio_t_w_i.emplace_back(T_w_i.translation());
+
+      if (show_gui) {
+        std::vector<float> vals;
+        vals.push_back((t_ns - curr_t_ns) * 1e-9);
+
+        for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(bg[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(ba[i]);
+
+        vio_data_log.Log(vals);
+      }
+    }
+
+    std::cout << "Finished t4" << std::endl;
+  });
+
+  std::shared_ptr<std::thread> t5;
+
+  if (print_queue) {
+    t5.reset(new std::thread([&]() {
+      while (!terminate) {
+        std::cout << "opt_flow_ptr->input_queue "
+                  << opt_flow_ptr->input_queue.size()
+                  << " opt_flow_ptr->output_queue "
+                  << opt_flow_ptr->output_queue->size() << " out_state_queue "
+                  << out_state_queue.size() << std::endl;
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+      }
+    }));
+  }
+
+  if (show_gui) {
+    pangolin::CreateWindowAndBind("RS T265 Vio", 1800, 1000);
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::View& img_view_display =
+        pangolin::CreateDisplay()
+            .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.4)
+            .SetLayout(pangolin::LayoutEqual);
+
+    pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
+        0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0);
+
+    plotter =
+        new pangolin::Plotter(&imu_data_log, 0.0, 100, -3.0, 3.0, 0.01f, 0.01f);
+    plot_display.AddDisplay(*plotter);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+    while (img_view.size() < calib.intrinsics.size()) {
+      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
+
+      size_t idx = img_view.size();
+      img_view.push_back(iv);
+
+      img_view_display.AddDisplay(*iv);
+      iv->extern_draw_function =
+          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
+    }
+
+    Eigen::Vector3d cam_p(0.5, -2, -2);
+    cam_p = vio->getT_w_i_init().so3() * calib.T_i_c[0].so3() * cam_p;
+    cam_p[2] = 1;
+
+    pangolin::OpenGlRenderState camera(
+        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
+        pangolin::ModelViewLookAt(cam_p[0], cam_p[1], cam_p[2], 0, 0, 0,
+                                  pangolin::AxisZ));
+
+    pangolin::View& display3D =
+        pangolin::CreateDisplay()
+            .SetAspect(-640 / 480.0)
+            .SetBounds(0.4, 1.0, 0.4, 1.0)
+            .SetHandler(new pangolin::Handler3D(camera));
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      if (follow) {
+        if (curr_vis_data.get()) {
+          auto T_w_i = curr_vis_data->states.back();
+          T_w_i.so3() = Sophus::SO3d();
+
+          camera.Follow(T_w_i.matrix());
+        }
+      }
+
+      display3D.Activate(camera);
+      glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
+
+      draw_scene();
+
+      img_view_display.Activate();
+
+      {
+        pangolin::GlPixFormat fmt;
+        fmt.glformat = GL_LUMINANCE;
+        fmt.gltype = GL_UNSIGNED_SHORT;
+        fmt.scalable_internal_format = GL_LUMINANCE16;
+
+        if (curr_vis_data.get() && curr_vis_data->opt_flow_res.get() &&
+            curr_vis_data->opt_flow_res->input_images.get()) {
+          auto& img_data = curr_vis_data->opt_flow_res->input_images->img_data;
+
+          for (size_t cam_id = 0; cam_id < basalt::RsT265Device::NUM_CAMS;
+               cam_id++) {
+            if (img_data[cam_id].img.get())
+              img_view[cam_id]->SetImage(
+                  img_data[cam_id].img->ptr, img_data[cam_id].img->w,
+                  img_data[cam_id].img->h, img_data[cam_id].img->pitch, fmt);
+          }
+        }
+
+        draw_plots();
+      }
+
+      if (show_est_vel.GuiChanged() || show_est_pos.GuiChanged() ||
+          show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) {
+        draw_plots();
+      }
+
+      pangolin::FinishFrame();
+    }
+  }
+
+  t265_device->stop();
+  terminate = true;
+
+  if (t3.get()) t3->join();
+  t4.join();
+  if (t5.get()) t5->join();
+
+  return 0;
+}
+
+void draw_image_overlay(pangolin::View& v, size_t cam_id) {
+  UNUSED(v);
+
+  if (show_obs) {
+    glLineWidth(1.0);
+    glColor3f(1.0, 0.0, 0.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (curr_vis_data.get() && cam_id < curr_vis_data->projections.size()) {
+      const auto& points = curr_vis_data->projections[cam_id];
+
+      if (!points.empty()) {
+        double min_id = points[0][2], max_id = points[0][2];
+
+        for (const auto& points2 : curr_vis_data->projections)
+          for (const auto& p : points2) {
+            min_id = std::min(min_id, p[2]);
+            max_id = std::max(max_id, p[2]);
+          }
+
+        for (const auto& c : points) {
+          const float radius = 6.5;
+
+          float r, g, b;
+          getcolor(c[2] - min_id, max_id - min_id, b, g, r);
+          glColor3f(r, g, b);
+
+          pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+          if (show_ids)
+            pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]);
+        }
+      }
+
+      glColor3f(1.0, 0.0, 0.0);
+      pangolin::GlFont::I()
+          .Text("Tracked %d points", points.size())
+          .Draw(5, 20);
+    }
+  }
+}
+
+void draw_scene() {
+  glPointSize(3);
+  glColor3f(1.0, 0.0, 0.0);
+  glEnable(GL_BLEND);
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+  glColor3ubv(cam_color);
+  Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
+                                                vio_t_w_i.end());
+  pangolin::glDrawLineStrip(sub_gt);
+
+  if (curr_vis_data.get()) {
+    for (const auto& p : curr_vis_data->states)
+      for (const auto& t_i_c : calib.T_i_c)
+        render_camera((p * t_i_c).matrix(), 2.0f, state_color, 0.1f);
+
+    for (const auto& p : curr_vis_data->frames)
+      for (const auto& t_i_c : calib.T_i_c)
+        render_camera((p * t_i_c).matrix(), 2.0f, pose_color, 0.1f);
+
+    for (const auto& t_i_c : calib.T_i_c)
+      render_camera((curr_vis_data->states.back() * t_i_c).matrix(), 2.0f,
+                    cam_color, 0.1f);
+
+    glColor3ubv(pose_color);
+    pangolin::glDrawPoints(curr_vis_data->points);
+  }
+
+  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
+}
+
+void load_data(const std::string& calib_path) {
+  std::ifstream os(calib_path, std::ios::binary);
+
+  if (os.is_open()) {
+    cereal::JSONInputArchive archive(os);
+    archive(calib);
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
+              << std::endl;
+
+  } else {
+    std::cerr << "could not load camera calibration " << calib_path
+              << std::endl;
+    std::abort();
+  }
+}
+
+void draw_plots() {
+  plotter->ClearSeries();
+  plotter->ClearMarkers();
+
+  if (show_est_pos) {
+    plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "position x", &vio_data_log);
+    plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "position y", &vio_data_log);
+    plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "position z", &vio_data_log);
+  }
+
+  if (show_est_vel) {
+    plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "velocity x", &vio_data_log);
+    plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "velocity y", &vio_data_log);
+    plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "velocity z", &vio_data_log);
+  }
+
+  if (show_est_bg) {
+    plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "gyro bias x", &vio_data_log);
+    plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "gyro bias y", &vio_data_log);
+    plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "gyro bias z", &vio_data_log);
+  }
+
+  if (show_est_ba) {
+    plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "accel bias x", &vio_data_log);
+    plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "accel bias y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "accel bias z", &vio_data_log);
+  }
+
+  if (t265_device->last_img_data.get()) {
+    double t = t265_device->last_img_data->t_ns * 1e-9;
+    plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal,
+                       pangolin::Colour::White());
+  }
+}

src/time_alignment.cpp

@@ -0,0 +1,522 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/serialization/headers_serialization.h>
+#include <basalt/utils/filesystem.h>
+#include <basalt/calibration/calibration.hpp>
+
+#include <CLI/CLI.hpp>
+
+basalt::Calibration<double> calib;
+basalt::MocapCalibration<double> mocap_calib;
+
+// Linear time version
+double compute_error(
+    int64_t offset, const std::vector<int64_t> &gyro_timestamps,
+    const Eigen::aligned_vector<Eigen::Vector3d> &gyro_data,
+    const std::vector<int64_t> &mocap_rot_vel_timestamps,
+    const Eigen::aligned_vector<Eigen::Vector3d> &mocap_rot_vel_data) {
+  double error = 0;
+  int num_points = 0;
+
+  size_t j = 0;
+
+  for (size_t i = 0; i < mocap_rot_vel_timestamps.size(); i++) {
+    int64_t corrected_time = mocap_rot_vel_timestamps[i] + offset;
+
+    while (gyro_timestamps[j] < corrected_time) j++;
+    if (j >= gyro_timestamps.size()) break;
+
+    int64_t dist_j = gyro_timestamps[j] - corrected_time;
+    int64_t dist_j_m1 = corrected_time - gyro_timestamps[j - 1];
+
+    BASALT_ASSERT(dist_j >= 0);
+    BASALT_ASSERT(dist_j_m1 >= 0);
+
+    int idx = dist_j < dist_j_m1 ? j : j - 1;
+
+    if (std::min(dist_j, dist_j_m1) > 1e9 / 120) continue;
+
+    error += (gyro_data[idx] - mocap_rot_vel_data[i]).norm();
+    num_points++;
+  }
+  return error / num_points;
+}
+
+int main(int argc, char **argv) {
+  std::string dataset_path;
+  std::string calibration_path;
+  std::string mocap_calibration_path;
+  std::string dataset_type;
+  std::string output_path;
+  std::string output_error_path;
+  std::string output_gyro_path;
+  std::string output_mocap_path;
+
+  double max_offset_s = 10.0;
+
+  bool show_gui = true;
+
+  CLI::App app{"Calibrate time offset"};
+
+  app.add_option("-d,--dataset-path", dataset_path, "Path to dataset")
+      ->required();
+  app.add_option("--calibration", calibration_path, "Path to calibration file");
+  app.add_option("--mocap-calibration", mocap_calibration_path,
+                 "Path to mocap calibration file");
+  app.add_option("--dataset-type", dataset_type, "Dataset type <euroc, bag>.")
+      ->required();
+
+  app.add_option("--output", output_path,
+                 "Path to output file with time-offset result");
+  app.add_option("--output-error", output_error_path,
+                 "Path to output file with error time-series for plotting");
+  app.add_option(
+      "--output-gyro", output_gyro_path,
+      "Path to output file with gyro rotational velocities for plotting");
+  app.add_option(
+      "--output-mocap", output_mocap_path,
+      "Path to output file with mocap rotational velocities for plotting");
+
+  app.add_option("--max-offset", max_offset_s,
+                 "Maximum offset for a grid search in seconds.");
+
+  app.add_flag("--show-gui", show_gui, "Show GUI for debugging");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError &e) {
+    return app.exit(e);
+  }
+
+  if (!dataset_path.empty() && dataset_path[dataset_path.length() - 1] != '/') {
+    dataset_path += '/';
+  }
+
+  basalt::VioDatasetPtr vio_dataset;
+
+  const bool use_calib =
+      !(calibration_path.empty() || mocap_calibration_path.empty());
+
+  if (use_calib) {
+    std::ifstream is(calibration_path);
+
+    if (is.good()) {
+      cereal::JSONInputArchive archive(is);
+      archive(calib);
+      std::cout << "Loaded calibration from: " << calibration_path << std::endl;
+    } else {
+      std::cerr << "No calibration found" << std::endl;
+      std::abort();
+    }
+
+    std::ifstream mocap_is(mocap_calibration_path);
+
+    if (mocap_is.good()) {
+      cereal::JSONInputArchive archive(mocap_is);
+      archive(mocap_calib);
+      std::cout << "Loaded mocap calibration from: " << mocap_calibration_path
+                << std::endl;
+    } else {
+      std::cerr << "No mocap calibration found" << std::endl;
+      std::abort();
+    }
+  }
+
+  basalt::DatasetIoInterfacePtr dataset_io =
+      basalt::DatasetIoFactory::getDatasetIo(dataset_type, true);
+
+  dataset_io->read(dataset_path);
+  vio_dataset = dataset_io->get_data();
+
+  std::vector<int64_t> gyro_timestamps;
+  Eigen::aligned_vector<Eigen::Vector3d> gyro_data;
+
+  std::vector<int64_t> mocap_rot_vel_timestamps;
+  Eigen::aligned_vector<Eigen::Vector3d> mocap_rot_vel_data;
+
+  // Apply calibration to gyro
+  {
+    int saturation_count = 0;
+    for (size_t i = 0; i < vio_dataset->get_gyro_data().size(); i++) {
+      if (vio_dataset->get_gyro_data()[i].data.array().abs().maxCoeff() >
+          499.0 * M_PI / 180) {
+        ++saturation_count;
+        continue;
+      }
+      gyro_timestamps.push_back(vio_dataset->get_gyro_data()[i].timestamp_ns);
+
+      Eigen::Vector3d measurement = vio_dataset->get_gyro_data()[i].data;
+      if (use_calib) {
+        gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement));
+      } else {
+        gyro_data.push_back(measurement);
+      }
+    }
+    std::cout << "saturated gyro measurement count: " << saturation_count
+              << std::endl;
+  }
+
+  // compute rotational velocity from mocap data
+  {
+    Sophus::SE3d T_mark_i;
+    if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse();
+
+    int saturation_count = 0;
+    for (size_t i = 1; i < vio_dataset->get_gt_timestamps().size() - 1; i++) {
+      Sophus::SE3d p0, p1;
+
+      // compute central differences, to have no timestamp bias
+      p0 = vio_dataset->get_gt_pose_data()[i - 1] * T_mark_i;
+      p1 = vio_dataset->get_gt_pose_data()[i + 1] * T_mark_i;
+
+      double dt = (vio_dataset->get_gt_timestamps()[i + 1] -
+                   vio_dataset->get_gt_timestamps()[i - 1]) *
+                  1e-9;
+
+      // only compute difference, if measurements are really 2 consecutive
+      // measurements apart (assuming 120 Hz data)
+      if (dt > 2.5 / 120) continue;
+
+      Eigen::Vector3d rot_vel = (p0.so3().inverse() * p1.so3()).log() / dt;
+
+      // Filter outliers
+      if (rot_vel.array().abs().maxCoeff() > 500 * M_PI / 180) {
+        ++saturation_count;
+        continue;
+      }
+
+      mocap_rot_vel_timestamps.push_back(vio_dataset->get_gt_timestamps()[i]);
+      mocap_rot_vel_data.push_back(rot_vel);
+    }
+    std::cout << "outlier mocap rotation velocity count: " << saturation_count
+              << std::endl;
+  }
+
+  std::cout << "gyro_data.size() " << gyro_data.size() << std::endl;
+  std::cout << "mocap_rot_vel_data.size() " << mocap_rot_vel_data.size()
+            << std::endl;
+
+  std::vector<double> offsets_vec;
+  std::vector<double> errors_vec;
+
+  int best_offset_ns = 0;
+  double best_error = std::numeric_limits<double>::max();
+  int best_error_idx = -1;
+
+  int64_t max_offset_ns = max_offset_s * 1e9;
+  int64_t offset_inc_ns = 100000;
+
+  for (int64_t offset_ns = -max_offset_ns; offset_ns <= max_offset_ns;
+       offset_ns += offset_inc_ns) {
+    double error = compute_error(offset_ns, gyro_timestamps, gyro_data,
+                                 mocap_rot_vel_timestamps, mocap_rot_vel_data);
+
+    offsets_vec.push_back(offset_ns * 1e-6);
+    errors_vec.push_back(error);
+
+    if (error < best_error) {
+      best_error = error;
+      best_offset_ns = offset_ns;
+      best_error_idx = errors_vec.size() - 1;
+    }
+  }
+
+  std::cout << "Best error: " << best_error << std::endl;
+  std::cout << "Best error idx : " << best_error_idx << std::endl;
+  std::cout << "Best offset: " << best_offset_ns << std::endl;
+
+  pangolin::DataLog error_log;
+
+  int best_offset_refined_ns = best_offset_ns;
+
+  // Subpixel accuracy
+  Eigen::Vector3d coeff(0, 0, 0);
+  {
+    const static int SAMPLE_INTERVAL = 10;
+
+    if (best_error_idx - SAMPLE_INTERVAL >= 0 &&
+        best_error_idx + SAMPLE_INTERVAL < int(errors_vec.size())) {
+      Eigen::MatrixXd pol(2 * SAMPLE_INTERVAL + 1, 3);
+      Eigen::VectorXd err(2 * SAMPLE_INTERVAL + 1);
+
+      for (int i = 0; i < 2 * SAMPLE_INTERVAL + 1; i++) {
+        int idx = i - SAMPLE_INTERVAL;
+        pol(i, 0) = idx * idx;
+        pol(i, 1) = idx;
+        pol(i, 2) = 1;
+
+        err(i) = errors_vec[best_error_idx + idx];
+      }
+
+      coeff =
+          pol.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(err);
+
+      double a = coeff[0];
+      double b = coeff[1];
+
+      if (a > 1e-9) {
+        best_offset_refined_ns -= offset_inc_ns * b / (2 * a);
+      }
+    }
+
+    for (size_t i = 0; i < errors_vec.size(); i++) {
+      const double idx =
+          static_cast<double>(static_cast<int>(i) - best_error_idx);
+
+      const Eigen::Vector3d pol(idx * idx, idx, 1);
+
+      error_log.Log(offsets_vec[i], errors_vec[i], pol.transpose() * coeff);
+    }
+  }
+
+  std::cout << "Best error refined: "
+            << compute_error(best_offset_refined_ns, gyro_timestamps, gyro_data,
+                             mocap_rot_vel_timestamps, mocap_rot_vel_data)
+            << std::endl;
+  std::cout << "Best offset refined: " << best_offset_refined_ns << std::endl;
+
+  std::cout << "Total mocap offset: "
+            << vio_dataset->get_mocap_to_imu_offset_ns() +
+                   best_offset_refined_ns
+            << std::endl;
+
+  if (output_path != "") {
+    std::ofstream os(output_path);
+    cereal::JSONOutputArchive archive(os);
+    archive(cereal::make_nvp("mocap_to_imu_initial_offset_ns",
+                             vio_dataset->get_mocap_to_imu_offset_ns()));
+    archive(cereal::make_nvp("mocap_to_imu_additional_offset_refined_ns",
+                             best_offset_refined_ns));
+    archive(cereal::make_nvp(
+        "mocap_to_imu_total_offset_ns",
+        vio_dataset->get_mocap_to_imu_offset_ns() + best_offset_refined_ns));
+  }
+
+  if (output_error_path != "") {
+    std::cout << "Writing error time series to '" << output_error_path << "'"
+              << std::endl;
+
+    std::ofstream os(output_error_path);
+    os << "#TIME_MS,ERROR,ERROR_FITTED" << std::endl;
+    os << "# best_offset_ms: " << best_offset_ns * 1e-6
+       << ", best_offset_refined_ms: " << best_offset_refined_ns * 1e-6
+       << std::endl;
+
+    for (size_t i = 0; i < errors_vec.size(); ++i) {
+      const double idx =
+          static_cast<double>(static_cast<int>(i) - best_error_idx);
+      const Eigen::Vector3d pol(idx * idx, idx, 1);
+      const double fitted = pol.transpose() * coeff;
+      os << offsets_vec[i] << "," << errors_vec[i] << "," << fitted
+         << std::endl;
+    }
+  }
+
+  const int64_t min_time = vio_dataset->get_gyro_data().front().timestamp_ns;
+  const int64_t max_time = vio_dataset->get_gyro_data().back().timestamp_ns;
+
+  if (output_gyro_path != "") {
+    std::cout << "Writing gyro values to '" << output_gyro_path << "'"
+              << std::endl;
+
+    std::ofstream os(output_gyro_path);
+    os << "#TIME_M, GX, GY, GZ" << std::endl;
+
+    for (size_t i = 0; i < gyro_timestamps.size(); ++i) {
+      os << (gyro_timestamps[i] - min_time) * 1e-9 << " "
+         << gyro_data[i].transpose() << std::endl;
+    }
+  }
+
+  if (output_mocap_path != "") {
+    std::cout << "Writing mocap rotational velocity values to '"
+              << output_mocap_path << "'" << std::endl;
+
+    std::ofstream os(output_mocap_path);
+    os << "#TIME_M, GX, GY, GZ" << std::endl;
+
+    for (size_t i = 0; i < gyro_timestamps.size(); ++i) {
+      os << (mocap_rot_vel_timestamps[i] + best_offset_ns - min_time) * 1e-9
+         << " " << mocap_rot_vel_data[i].transpose() << std::endl;
+    }
+  }
+
+  if (show_gui) {
+    static constexpr int UI_WIDTH = 280;
+
+    pangolin::CreateWindowAndBind("Main", 1280, 800);
+
+    pangolin::Plotter *plotter;
+
+    pangolin::DataLog data_log, mocap_log;
+
+    pangolin::View &plot_display = pangolin::CreateDisplay().SetBounds(
+        0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    plotter = new pangolin::Plotter(&data_log, 0, (max_time - min_time) * 1e-9,
+                                    -10.0, 10.0, 0.01, 0.01);
+
+    plot_display.AddDisplay(*plotter);
+    pangolin::Var<bool> show_gyro("ui.show_gyro", true, false, true);
+    pangolin::Var<bool> show_mocap_rot_vel("ui.show_mocap_rot_vel", true, false,
+                                           true);
+
+    pangolin::Var<bool> show_error("ui.show_error", false, false, true);
+
+    std::string save_button_name = "ui.save_aligned_dataset";
+    // Disable save_aligned_dataset button if GT data already exists
+    if (basalt::fs::exists(
+            basalt::fs::path(dataset_path + "mav0/gt/data.csv"))) {
+      save_button_name += "(disabled)";
+    }
+
+    pangolin::Var<std::function<void(void)>> save_aligned_dataset(
+        save_button_name, [&]() {
+          if (basalt::fs::exists(
+                  basalt::fs::path(dataset_path + "mav0/gt/data.csv"))) {
+            std::cout << "Aligned ground-truth data already exists, skipping. "
+                         "If you want to run the calibration again delete "
+                      << dataset_path << "mav0/gt/ folder." << std::endl;
+            return;
+          }
+          std::cout << "Saving aligned dataset in "
+                    << dataset_path + "mav0/gt/data.csv" << std::endl;
+          // output corrected mocap data
+          Sophus::SE3d T_mark_i;
+          if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse();
+          basalt::fs::create_directory(dataset_path + "mav0/gt/");
+          std::ofstream gt_out_stream;
+          gt_out_stream.open(dataset_path + "mav0/gt/data.csv");
+          gt_out_stream
+              << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], "
+                 "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n";
+
+          for (size_t i = 0; i < vio_dataset->get_gt_timestamps().size(); i++) {
+            gt_out_stream << vio_dataset->get_gt_timestamps()[i] +
+                                 best_offset_refined_ns
+                          << ",";
+            Sophus::SE3d pose_corrected =
+                vio_dataset->get_gt_pose_data()[i] * T_mark_i;
+            gt_out_stream << pose_corrected.translation().x() << ","
+                          << pose_corrected.translation().y() << ","
+                          << pose_corrected.translation().z() << ","
+                          << pose_corrected.unit_quaternion().w() << ","
+                          << pose_corrected.unit_quaternion().x() << ","
+                          << pose_corrected.unit_quaternion().y() << ","
+                          << pose_corrected.unit_quaternion().z() << std::endl;
+          }
+          gt_out_stream.close();
+        });
+
+    auto recompute_logs = [&]() {
+      data_log.Clear();
+      mocap_log.Clear();
+
+      for (size_t i = 0; i < gyro_timestamps.size(); i++) {
+        data_log.Log((gyro_timestamps[i] - min_time) * 1e-9, gyro_data[i][0],
+                     gyro_data[i][1], gyro_data[i][2]);
+      }
+
+      for (size_t i = 0; i < mocap_rot_vel_timestamps.size(); i++) {
+        mocap_log.Log(
+            (mocap_rot_vel_timestamps[i] + best_offset_ns - min_time) * 1e-9,
+            mocap_rot_vel_data[i][0], mocap_rot_vel_data[i][1],
+            mocap_rot_vel_data[i][2]);
+      }
+    };
+
+    auto drawPlots = [&]() {
+      plotter->ClearSeries();
+      plotter->ClearMarkers();
+
+      if (show_gyro) {
+        plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
+                           pangolin::Colour::Red(), "g x");
+        plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
+                           pangolin::Colour::Green(), "g y");
+        plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine,
+                           pangolin::Colour::Blue(), "g z");
+      }
+
+      if (show_mocap_rot_vel) {
+        plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
+                           pangolin::Colour(1, 1, 0), "pv x", &mocap_log);
+        plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
+                           pangolin::Colour(1, 0, 1), "pv y", &mocap_log);
+        plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine,
+                           pangolin::Colour(0, 1, 1), "pv z", &mocap_log);
+      }
+
+      if (show_error) {
+        plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
+                           pangolin::Colour(1, 1, 1), "error", &error_log);
+        plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
+                           pangolin::Colour(0.3, 1, 0.8), "fitted error",
+                           &error_log);
+        plotter->AddMarker(pangolin::Marker::Vertical,
+                           best_offset_refined_ns * 1e-6,
+                           pangolin::Marker::Equal, pangolin::Colour(1, 0, 0));
+      }
+    };
+
+    recompute_logs();
+    drawPlots();
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      if (show_gyro.GuiChanged() || show_mocap_rot_vel.GuiChanged() ||
+          show_error.GuiChanged()) {
+        drawPlots();
+      }
+
+      pangolin::FinishFrame();
+    }
+  }
+
+  return 0;
+}

src/utils/keypoints.cpp

@@ -0,0 +1,428 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <unordered_set>
+
+#include <basalt/utils/keypoints.h>
+
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/sac/Ransac.hpp>
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+#pragma GCC diagnostic pop
+
+namespace basalt {
+
+// const int PATCH_SIZE = 31;
+const int HALF_PATCH_SIZE = 15;
+const int EDGE_THRESHOLD = 19;
+
+const static char pattern_31_x_a[256] = {
+    8,   4,   -11, 7,   2,   1,   -2,  -13, -13, 10,  -13, -11, 7,   -4,  -13,
+    -9,  12,  -3,  -6,  11,  4,   5,   3,   -8,  -2,  -13, -7,  -4,  -10, 5,
+    5,   1,   9,   4,   2,   -4,  -8,  4,   0,   -13, -3,  -6,  8,   0,   7,
+    -13, 10,  -6,  10,  -13, -13, 3,   5,   -1,  3,   2,   -13, -13, -13, -7,
+    6,   -9,  -2,  -12, 3,   -7,  -3,  2,   -11, -1,  5,   -4,  -9,  -12, 10,
+    7,   -7,  -4,  7,   -7,  -13, -3,  7,   -13, 1,   2,   -4,  -1,  7,   1,
+    9,   -1,  -13, 7,   12,  6,   5,   2,   3,   2,   9,   -8,  -11, 1,   6,
+    2,   6,   3,   7,   -11, -10, -5,  -10, 8,   4,   -10, 4,   -2,  -5,  7,
+    -9,  -5,  8,   -9,  1,   7,   -2,  11,  -12, 3,   5,   0,   -9,  0,   -1,
+    5,   3,   -13, -5,  -4,  6,   -7,  -13, 1,   4,   -2,  2,   -2,  4,   -6,
+    -3,  7,   4,   -13, 7,   7,   -7,  -8,  -13, 2,   10,  -6,  8,   2,   -11,
+    -12, -11, 5,   -2,  -1,  -13, -10, -3,  2,   -9,  -4,  -4,  -6,  6,   -13,
+    11,  7,   -1,  -4,  -7,  -13, -7,  -8,  -5,  -13, 1,   1,   9,   5,   -1,
+    -9,  -1,  -13, 8,   2,   7,   -10, -10, 4,   3,   -4,  5,   4,   -9,  0,
+    -12, 3,   -10, 8,   -8,  2,   10,  6,   -7,  -3,  -1,  -3,  -8,  4,   2,
+    6,   3,   11,  -3,  4,   2,   -10, -13, -13, 6,   0,   -13, -9,  -13, 5,
+    2,   -1,  9,   11,  3,   -1,  3,   -13, 5,   8,   7,   -10, 7,   9,   7,
+    -1};
+
+const static char pattern_31_y_a[256] = {
+    -3,  2,   9,   -12, -13, -7,  -10, -13, -3,  4,   -8,  7,   7,   -5,  2,
+    0,   -6,  6,   -13, -13, 7,   -3,  -7,  -7,  11,  12,  3,   2,   -12, -12,
+    -6,  0,   11,  7,   -1,  -12, -5,  11,  -8,  -2,  -2,  9,   12,  9,   -5,
+    -6,  7,   -3,  -9,  8,   0,   3,   7,   7,   -10, -4,  0,   -7,  3,   12,
+    -10, -1,  -5,  5,   -10, -7,  -2,  9,   -13, 6,   -3,  -13, -6,  -10, 2,
+    12,  -13, 9,   -1,  6,   11,  7,   -8,  -7,  -3,  -6,  3,   -13, 1,   -1,
+    1,   -9,  -13, 7,   -5,  3,   -13, -12, 8,   6,   -12, 4,   12,  12,  -9,
+    3,   3,   -3,  8,   -5,  11,  -8,  5,   -1,  -6,  12,  -2,  0,   -8,  -6,
+    -13, -13, -8,  -11, -8,  -4,  1,   -6,  -9,  7,   5,   -4,  12,  7,   2,
+    11,  5,   -4,  9,   -7,  5,   6,   6,   -10, 1,   -2,  -12, -13, 1,   -10,
+    -13, 5,   -2,  9,   1,   -8,  -4,  11,  6,   4,   -5,  -5,  -3,  -12, -2,
+    -13, 0,   -3,  -13, -8,  -11, -2,  9,   -3,  -13, 6,   12,  -11, -3,  11,
+    11,  -5,  12,  -8,  1,   -12, -2,  5,   -1,  7,   5,   0,   12,  -8,  11,
+    -3,  -10, 1,   -11, -13, -13, -10, -8,  -6,  12,  2,   -13, -13, 9,   3,
+    1,   2,   -10, -13, -12, 2,   6,   8,   10,  -9,  -13, -7,  -2,  2,   -5,
+    -9,  -1,  -1,  0,   -11, -4,  -6,  7,   12,  0,   -1,  3,   8,   -6,  -9,
+    7,   -6,  5,   -3,  0,   4,   -6,  0,   8,   9,   -4,  4,   3,   -7,  0,
+    -6};
+
+const static char pattern_31_x_b[256] = {
+    9,   7,  -8, 12,  2,   1,  -2,  -11, -12, 11,  -8,  -9,  12,  -3,  -12, -7,
+    12,  -2, -4, 12,  5,   10, 6,   -6,  -1,  -8,  -5,  -3,  -6,  6,   7,   4,
+    11,  4,  4,  -2,  -7,  9,  1,   -8,  -2,  -4,  10,  1,   11,  -11, 12,  -6,
+    12,  -8, -8, 7,   10,  1,  5,   3,   -13, -12, -11, -4,  12,  -7,  0,   -7,
+    8,   -4, -1, 5,   -5,  0,  5,   -4,  -9,  -8,  12,  12,  -6,  -3,  12,  -5,
+    -12, -2, 12, -11, 12,  3,  -2,  1,   8,   3,   12,  -1,  -10, 10,  12,  7,
+    6,   2,  4,  12,  10,  -7, -4,  2,   7,   3,   11,  8,   9,   -6,  -5,  -3,
+    -9,  12, 6,  -8,  6,   -2, -5,  10,  -8,  -5,  9,   -9,  1,   9,   -1,  12,
+    -6,  7,  10, 2,   -5,  2,  1,   7,   6,   -8,  -3,  -3,  8,   -6,  -5,  3,
+    8,   2,  12, 0,   9,   -3, -1,  12,  5,   -9,  8,   7,   -7,  -7,  -12, 3,
+    12,  -6, 9,  2,   -10, -7, -10, 11,  -1,  0,   -12, -10, -2,  3,   -4,  -3,
+    -2,  -4, 6,  -5,  12,  12, 0,   -3,  -6,  -8,  -6,  -6,  -4,  -8,  5,   10,
+    10,  10, 1,  -6,  1,   -8, 10,  3,   12,  -5,  -8,  8,   8,   -3,  10,  5,
+    -4,  3,  -6, 4,   -10, 12, -6,  3,   11,  8,   -6,  -3,  -1,  -3,  -8,  12,
+    3,   11, 7,  12,  -3,  4,  2,   -8,  -11, -11, 11,  1,   -9,  -6,  -8,  8,
+    3,   -1, 11, 12,  3,   0,  4,   -10, 12,  9,   8,   -10, 12,  10,  12,  0};
+
+const static char pattern_31_y_b[256] = {
+    5,   -12, 2,   -13, 12,  6,   -4,  -8,  -9,  9,   -9,  12,  6,   0,  -3,
+    5,   -1,  12,  -8,  -8,  1,   -3,  12,  -2,  -10, 10,  -3,  7,   11, -7,
+    -1,  -5,  -13, 12,  4,   7,   -10, 12,  -13, 2,   3,   -9,  7,   3,  -10,
+    0,   1,   12,  -4,  -12, -4,  8,   -7,  -12, 6,   -10, 5,   12,  8,  7,
+    8,   -6,  12,  5,   -13, 5,   -7,  -11, -13, -1,  2,   12,  6,   -4, -3,
+    12,  5,   4,   2,   1,   5,   -6,  -7,  -12, 12,  0,   -13, 9,   -6, 12,
+    6,   3,   5,   12,  9,   11,  10,  3,   -6,  -13, 3,   9,   -6,  -8, -4,
+    -2,  0,   -8,  3,   -4,  10,  12,  0,   -6,  -11, 7,   7,   12,  2,  12,
+    -8,  -2,  -13, 0,   -2,  1,   -4,  -11, 4,   12,  8,   8,   -13, 12, 7,
+    -9,  -8,  9,   -3,  -12, 0,   12,  -2,  10,  -4,  -13, 12,  -6,  3,  -5,
+    1,   -11, -7,  -5,  6,   6,   1,   -8,  -8,  9,   3,   7,   -8,  8,  3,
+    -9,  -5,  8,   12,  9,   -5,  11,  -13, 2,   0,   -10, -7,  9,   11, 5,
+    6,   -2,  7,   -2,  7,   -13, -8,  -9,  5,   10,  -13, -13, -1,  -9, -13,
+    2,   12,  -10, -6,  -6,  -9,  -7,  -13, 5,   -13, -3,  -12, -1,  3,  -9,
+    1,   -8,  9,   12,  -5,  7,   -8,  -12, 5,   9,   5,   4,   3,   12, 11,
+    -13, 12,  4,   6,   12,  1,   1,   1,   -13, -13, 4,   -2,  -3,  -2, 10,
+    -9,  -1,  -2,  -8,  5,   10,  5,   5,   11,  -6,  -12, 9,   4,   -2, -2,
+    -11};
+
+void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
+                            KeypointsData& kd, int num_features) {
+  cv::Mat image(img_raw.h, img_raw.w, CV_8U);
+
+  uint8_t* dst = image.ptr();
+  const uint16_t* src = img_raw.ptr;
+
+  for (size_t i = 0; i < img_raw.size(); i++) {
+    dst[i] = (src[i] >> 8);
+  }
+
+  std::vector<cv::Point2f> points;
+  goodFeaturesToTrack(image, points, num_features, 0.01, 8);
+
+  kd.corners.clear();
+  kd.corner_angles.clear();
+  kd.corner_descriptors.clear();
+
+  for (size_t i = 0; i < points.size(); i++) {
+    if (img_raw.InBounds(points[i].x, points[i].y, EDGE_THRESHOLD)) {
+      kd.corners.emplace_back(points[i].x, points[i].y);
+    }
+  }
+}
+
+void detectKeypoints(
+    const basalt::Image<const uint16_t>& img_raw, KeypointsData& kd,
+    int PATCH_SIZE, int num_points_cell,
+    const Eigen::aligned_vector<Eigen::Vector2d>& current_points) {
+  kd.corners.clear();
+  kd.corner_angles.clear();
+  kd.corner_descriptors.clear();
+
+  const size_t x_start = (img_raw.w % PATCH_SIZE) / 2;
+  const size_t x_stop = x_start + PATCH_SIZE * (img_raw.w / PATCH_SIZE - 1);
+
+  const size_t y_start = (img_raw.h % PATCH_SIZE) / 2;
+  const size_t y_stop = y_start + PATCH_SIZE * (img_raw.h / PATCH_SIZE - 1);
+
+  //  std::cerr << "x_start " << x_start << " x_stop " << x_stop << std::endl;
+  //  std::cerr << "y_start " << y_start << " y_stop " << y_stop << std::endl;
+
+  Eigen::MatrixXi cells;
+  cells.setZero(img_raw.h / PATCH_SIZE + 1, img_raw.w / PATCH_SIZE + 1);
+
+  for (const Eigen::Vector2d& p : current_points) {
+    if (p[0] >= x_start && p[1] >= y_start && p[0] < x_stop + PATCH_SIZE &&
+        p[1] < y_stop + PATCH_SIZE) {
+      int x = (p[0] - x_start) / PATCH_SIZE;
+      int y = (p[1] - y_start) / PATCH_SIZE;
+
+      cells(y, x) += 1;
+    }
+  }
+
+  for (size_t x = x_start; x <= x_stop; x += PATCH_SIZE) {
+    for (size_t y = y_start; y <= y_stop; y += PATCH_SIZE) {
+      if (cells((y - y_start) / PATCH_SIZE, (x - x_start) / PATCH_SIZE) > 0)
+        continue;
+
+      const basalt::Image<const uint16_t> sub_img_raw =
+          img_raw.SubImage(x, y, PATCH_SIZE, PATCH_SIZE);
+
+      cv::Mat subImg(PATCH_SIZE, PATCH_SIZE, CV_8U);
+
+      for (int y = 0; y < PATCH_SIZE; y++) {
+        uchar* sub_ptr = subImg.ptr(y);
+        for (int x = 0; x < PATCH_SIZE; x++) {
+          sub_ptr[x] = (sub_img_raw(x, y) >> 8);
+        }
+      }
+
+      int points_added = 0;
+      int threshold = 40;
+
+      while (points_added < num_points_cell && threshold >= 5) {
+        std::vector<cv::KeyPoint> points;
+        cv::FAST(subImg, points, threshold);
+
+        std::sort(points.begin(), points.end(),
+                  [](const cv::KeyPoint& a, const cv::KeyPoint& b) -> bool {
+                    return a.response > b.response;
+                  });
+
+        //        std::cout << "Detected " << points.size() << " points.
+        //        Threshold "
+        //                  << threshold << std::endl;
+
+        for (size_t i = 0; i < points.size() && points_added < num_points_cell;
+             i++)
+          if (img_raw.InBounds(x + points[i].pt.x, y + points[i].pt.y,
+                               EDGE_THRESHOLD)) {
+            kd.corners.emplace_back(x + points[i].pt.x, y + points[i].pt.y);
+            points_added++;
+          }
+
+        threshold /= 2;
+      }
+    }
+  }
+
+  // std::cout << "Total points: " << kd.corners.size() << std::endl;
+
+  //  cv::TermCriteria criteria =
+  //      cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001);
+  //  cv::Size winSize = cv::Size(5, 5);
+  //  cv::Size zeroZone = cv::Size(-1, -1);
+  //  cv::cornerSubPix(image, points, winSize, zeroZone, criteria);
+
+  //  for (size_t i = 0; i < points.size(); i++) {
+  //    if (img_raw.InBounds(points[i].pt.x, points[i].pt.y, EDGE_THRESHOLD)) {
+  //      kd.corners.emplace_back(points[i].pt.x, points[i].pt.y);
+  //    }
+  //  }
+}
+
+void computeAngles(const basalt::Image<const uint16_t>& img_raw,
+                   KeypointsData& kd, bool rotate_features) {
+  kd.corner_angles.resize(kd.corners.size());
+
+  for (size_t i = 0; i < kd.corners.size(); i++) {
+    const Eigen::Vector2d& p = kd.corners[i];
+
+    const int cx = p[0];
+    const int cy = p[1];
+
+    double angle = 0;
+
+    if (rotate_features) {
+      double m01 = 0, m10 = 0;
+      for (int x = -HALF_PATCH_SIZE; x <= HALF_PATCH_SIZE; x++) {
+        for (int y = -HALF_PATCH_SIZE; y <= HALF_PATCH_SIZE; y++) {
+          if (x * x + y * y <= HALF_PATCH_SIZE * HALF_PATCH_SIZE) {
+            double val = img_raw(cx + x, cy + y);
+            m01 += y * val;
+            m10 += x * val;
+          }
+        }
+      }
+
+      angle = atan2(m01, m10);
+    }
+
+    kd.corner_angles[i] = angle;
+  }
+}
+
+void computeDescriptors(const basalt::Image<const uint16_t>& img_raw,
+                        KeypointsData& kd) {
+  kd.corner_descriptors.resize(kd.corners.size());
+
+  for (size_t i = 0; i < kd.corners.size(); i++) {
+    std::bitset<256> descriptor;
+
+    const Eigen::Vector2d& p = kd.corners[i];
+    double angle = kd.corner_angles[i];
+
+    int cx = p[0];
+    int cy = p[1];
+
+    Eigen::Rotation2Dd rot(angle);
+    Eigen::Matrix2d mat_rot = rot.matrix();
+
+    for (int i = 0; i < 256; i++) {
+      Eigen::Vector2d va(pattern_31_x_a[i], pattern_31_y_a[i]),
+          vb(pattern_31_x_b[i], pattern_31_y_b[i]);
+
+      Eigen::Vector2i vva = (mat_rot * va).array().round().cast<int>();
+      Eigen::Vector2i vvb = (mat_rot * vb).array().round().cast<int>();
+
+      descriptor[i] =
+          img_raw(cx + vva[0], cy + vva[1]) < img_raw(cx + vvb[0], cy + vvb[1]);
+    }
+
+    kd.corner_descriptors[i] = descriptor;
+  }
+}
+
+void matchFastHelper(const std::vector<std::bitset<256>>& corner_descriptors_1,
+                     const std::vector<std::bitset<256>>& corner_descriptors_2,
+                     std::unordered_map<int, int>& matches, int threshold,
+                     double test_dist) {
+  matches.clear();
+
+  for (size_t i = 0; i < corner_descriptors_1.size(); i++) {
+    int best_idx = -1, best_dist = 500;
+    int best2_dist = 500;
+
+    for (size_t j = 0; j < corner_descriptors_2.size(); j++) {
+      int dist = (corner_descriptors_1[i] ^ corner_descriptors_2[j]).count();
+
+      if (dist <= best_dist) {
+        best2_dist = best_dist;
+
+        best_dist = dist;
+        best_idx = j;
+      } else if (dist < best2_dist) {
+        best2_dist = dist;
+      }
+    }
+
+    if (best_dist < threshold && best_dist * test_dist <= best2_dist) {
+      matches.emplace(i, best_idx);
+    }
+  }
+}
+
+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) {
+  matches.clear();
+
+  std::unordered_map<int, int> matches_1_2, matches_2_1;
+  matchFastHelper(corner_descriptors_1, corner_descriptors_2, matches_1_2,
+                  threshold, dist_2_best);
+  matchFastHelper(corner_descriptors_2, corner_descriptors_1, matches_2_1,
+                  threshold, dist_2_best);
+
+  for (const auto& kv : matches_1_2) {
+    if (matches_2_1[kv.second] == kv.first) {
+      matches.emplace_back(kv.first, kv.second);
+    }
+  }
+}
+
+void findInliersRansac(const KeypointsData& kd1, const KeypointsData& kd2,
+                       const double ransac_thresh, const int ransac_min_inliers,
+                       MatchData& md) {
+  md.inliers.clear();
+
+  opengv::bearingVectors_t bearingVectors1, bearingVectors2;
+
+  for (size_t i = 0; i < md.matches.size(); i++) {
+    bearingVectors1.push_back(kd1.corners_3d[md.matches[i].first].head<3>());
+    bearingVectors2.push_back(kd2.corners_3d[md.matches[i].second].head<3>());
+  }
+
+  // create the central relative adapter
+  opengv::relative_pose::CentralRelativeAdapter adapter(bearingVectors1,
+                                                        bearingVectors2);
+  // create a RANSAC object
+  opengv::sac::Ransac<
+      opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem>
+      ransac;
+  // create a CentralRelativePoseSacProblem
+  // (set algorithm to STEWENIUS, NISTER, SEVENPT, or EIGHTPT)
+  std::shared_ptr<
+      opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem>
+      relposeproblem_ptr(
+          new opengv::sac_problems::relative_pose::
+              CentralRelativePoseSacProblem(
+                  adapter, opengv::sac_problems::relative_pose::
+                               CentralRelativePoseSacProblem::STEWENIUS));
+  // run ransac
+  ransac.sac_model_ = relposeproblem_ptr;
+  ransac.threshold_ = ransac_thresh;
+  ransac.max_iterations_ = 100;
+  ransac.computeModel();
+
+  // do non-linear refinement and add more inliers
+  const size_t num_inliers_ransac = ransac.inliers_.size();
+
+  adapter.sett12(ransac.model_coefficients_.topRightCorner<3, 1>());
+  adapter.setR12(ransac.model_coefficients_.topLeftCorner<3, 3>());
+
+  const opengv::transformation_t nonlinear_transformation =
+      opengv::relative_pose::optimize_nonlinear(adapter, ransac.inliers_);
+
+  ransac.sac_model_->selectWithinDistance(nonlinear_transformation,
+                                          ransac.threshold_, ransac.inliers_);
+
+  // Sanity check if the number of inliers decreased, but only warn if it is
+  // by 3 or more, since some small fluctuation is expected.
+  if (ransac.inliers_.size() + 2 < num_inliers_ransac) {
+    std::cout << "Warning: non-linear refinement reduced the relative pose "
+                 "ransac inlier count from "
+              << num_inliers_ransac << " to " << ransac.inliers_.size() << "."
+              << std::endl;
+  }
+
+  // get the result (normalize translation)
+  md.T_i_j = Sophus::SE3d(
+      nonlinear_transformation.topLeftCorner<3, 3>(),
+      nonlinear_transformation.topRightCorner<3, 1>().normalized());
+
+  if ((long)ransac.inliers_.size() >= ransac_min_inliers) {
+    for (size_t i = 0; i < ransac.inliers_.size(); i++)
+      md.inliers.emplace_back(md.matches[ransac.inliers_[i]]);
+  }
+}
+
+}  // namespace basalt

src/utils/system_utils.cpp

@@ -0,0 +1,96 @@
+/**
+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.
+*/
+
+#include <basalt/utils/system_utils.h>
+
+#include <fstream>
+
+#if __APPLE__
+#include <mach/mach.h>
+#include <mach/task.h>
+#elif __linux__
+#include <unistd.h>
+
+#include <sys/resource.h>
+#endif
+
+namespace basalt {
+
+bool get_memory_info(MemoryInfo& info) {
+#if __APPLE__
+  mach_task_basic_info_data_t t_info;
+  mach_msg_type_number_t t_info_count = MACH_TASK_BASIC_INFO_COUNT;
+
+  if (KERN_SUCCESS != task_info(mach_task_self(), MACH_TASK_BASIC_INFO,
+                                (task_info_t)&t_info, &t_info_count)) {
+    return false;
+  }
+  info.resident_memory = t_info.resident_size;
+  info.resident_memory_peak = t_info.resident_size_max;
+
+  /*
+  struct rusage resource_usage;
+  getrusage(RUSAGE_SELF, &resource_usage);
+  info.resident_memory_peak = resource_usage.ru_maxrss;
+  */
+
+  return true;
+#elif __linux__
+
+  // get current memory first
+  std::size_t program_size = 0;
+  std::size_t resident_size = 0;
+
+  std::ifstream fs("/proc/self/statm");
+  if (fs.fail()) {
+    return false;
+  }
+  fs >> program_size;
+  fs >> resident_size;
+
+  info.resident_memory = resident_size * sysconf(_SC_PAGESIZE);
+
+  // get peak memory after that
+  struct rusage resource_usage;
+  getrusage(RUSAGE_SELF, &resource_usage);
+  info.resident_memory_peak = resource_usage.ru_maxrss * 1024;
+
+  return true;
+#else
+  return false;
+#endif
+}
+
+}  // namespace basalt

src/utils/time_utils.cpp

@@ -0,0 +1,212 @@
+#include <basalt/utils/assert.h>
+#include <basalt/utils/time_utils.hpp>
+
+#include <fstream>
+#include <iomanip>
+
+#include <fmt/format.h>
+#include <Eigen/Dense>
+#include <nlohmann/json.hpp>
+
+namespace basalt {
+
+using namespace fmt::literals;
+
+// compute the median of an eigen vector
+// Note: Changes the order of elements in the vector!
+// Note: For even sized vectors we don't return the mean of the middle two, but
+// simply the second one as is.
+template <class Scalar, int Rows>
+Scalar median_non_const(Eigen::Matrix<Scalar, Rows, 1>& vec) {
+  static_assert(Rows != 0);
+  if constexpr (Rows < 0) {
+    BASALT_ASSERT(vec.size() >= 1);
+  }
+  int n = vec.size() / 2;
+  std::nth_element(vec.begin(), vec.begin() + n, vec.end());
+  return vec(n);
+}
+
+template <class Scalar, int N>
+Scalar variance(const Eigen::Matrix<Scalar, N, 1>& vec) {
+  static_assert(N != 0);
+  const Eigen::Matrix<Scalar, N, 1> centered = vec.array() - vec.mean();
+  return centered.squaredNorm() / Scalar(vec.size());
+}
+
+ExecutionStats::Meta& ExecutionStats::add(const std::string& name,
+                                          double value) {
+  auto [it, new_item] = stats_.try_emplace(name);
+  if (new_item) {
+    order_.push_back(name);
+    it->second.data_ = std::vector<double>();
+  }
+  std::get<std::vector<double>>(it->second.data_).push_back(value);
+  return it->second;
+}
+
+ExecutionStats::Meta& ExecutionStats::add(const std::string& name,
+                                          const Eigen::VectorXd& value) {
+  auto [it, new_item] = stats_.try_emplace(name);
+  if (new_item) {
+    order_.push_back(name);
+    it->second.data_ = std::vector<Eigen::VectorXd>();
+  }
+  std::get<std::vector<Eigen::VectorXd>>(it->second.data_).push_back(value);
+  return it->second;
+}
+
+ExecutionStats::Meta& ExecutionStats::add(const std::string& name,
+                                          const Eigen::VectorXf& value) {
+  Eigen::VectorXd x = value.cast<double>();
+  return add(name, x);
+}
+
+void ExecutionStats::merge_all(const ExecutionStats& other) {
+  for (const auto& name : other.order_) {
+    const auto& meta = other.stats_.at(name);
+    std::visit(
+        [&](auto& data) {
+          for (auto v : data) {
+            add(name, v);
+          }
+        },
+        meta.data_);
+    stats_.at(name).set_meta(meta);
+  }
+}
+
+namespace {  // helper
+// ////////////////////////////////////////////////////////////////////////////
+// overloads for generic lambdas
+// See also: https://stackoverflow.com/q/55087826/1813258
+template <class... Ts>
+struct overload : Ts... {
+  using Ts::operator()...;
+};
+template <class... Ts>
+overload(Ts...) -> overload<Ts...>;
+}  // namespace
+
+void ExecutionStats::merge_sums(const ExecutionStats& other) {
+  for (const auto& name : other.order_) {
+    const auto& meta = other.stats_.at(name);
+    std::visit(overload{[&](const std::vector<double>& data) {
+                          Eigen::Map<const Eigen::VectorXd> map(data.data(),
+                                                                data.size());
+                          add(name, map.sum());
+                        },
+                        [&](const std::vector<Eigen::VectorXd>& data) {
+                          UNUSED(data);
+                          // TODO: for now no-op
+                        }},
+               meta.data_);
+    stats_.at(name).set_meta(meta);
+  }
+}
+
+void ExecutionStats::print() const {
+  for (const auto& name : order_) {
+    const auto& meta = stats_.at(name);
+
+    std::visit(
+        overload{
+            [&](const std::vector<double>& data) {
+              Eigen::Map<const Eigen::VectorXd> map(data.data(), data.size());
+
+              // create a copy for median computation
+              Eigen::VectorXd vec = map;
+
+              if (meta.format_ == "ms") {
+                // convert seconds to milliseconds
+                vec *= 1000;
+              }
+
+              int count = vec.size();
+              // double sum = vec.sum();
+              double mean = vec.mean();
+              double stddev = std::sqrt(variance(vec));
+              double max = vec.maxCoeff();
+              double min = vec.minCoeff();
+
+              // double median = median_non_const(vec);
+
+              if (meta.format_ == "count") {
+                std::cout << "{:20} ({:>4}):{: 8.1f}+-{:.1f} [{}, {}]\n"_format(
+                    name, count, mean, stddev, min, max);
+              } else if (meta.format_ != "none") {
+                std::cout
+                    << "{:20} ({:>4}):{: 8.2f}+-{:.2f} [{:.2f}, {:.2f}]\n"_format(
+                           name, count, mean, stddev, min, max);
+              }
+            },
+            [&](const std::vector<Eigen::VectorXd>& data) {
+              int count = data.size();
+              std::cout << "{:20} ({:>4})\n"_format(name, count);
+            }},
+        meta.data_);
+  }
+}
+
+bool ExecutionStats::save_json(const std::string& path) const {
+  using json = nlohmann::json;
+  json result;
+
+  for (const auto& name : order_) {
+    const auto& meta = stats_.at(name);
+
+    std::visit(
+        overload{[&](const std::vector<double>& data) { result[name] = data; },
+                 [&](const std::vector<Eigen::VectorXd>& data) {
+                   std::vector<int> indices;
+                   std::vector<double> values;
+                   for (const auto& v : data) {
+                     indices.push_back(int(values.size()));
+                     values.insert(values.end(), v.begin(), v.end());
+                   }
+                   std::string name_values = std::string(name) + "__values";
+                   std::string name_indices = std::string(name) + "__index";
+                   result[name_indices] = indices;
+                   result[name_values] = values;
+                 }},
+        meta.data_);
+  }
+
+  constexpr bool save_as_json = false;
+  constexpr bool save_as_ubjson = true;
+
+  // save json
+  if (save_as_json) {
+    std::ofstream ofs(path);
+
+    if (!ofs.is_open()) {
+      std::cerr << "Could not save ExecutionStats to {}.\n"_format(path);
+      return false;
+    }
+
+    ofs << std::setw(4) << result;  //!< pretty printing
+    // ofs << result;  //!< no pretty printing
+
+    std::cout << "Saved ExecutionStats to {}.\n"_format(path);
+  }
+
+  // save ubjson
+  if (save_as_ubjson) {
+    std::string ubjson_path =
+        path.substr(0, path.find_last_of('.')) + ".ubjson";
+    std::ofstream ofs(ubjson_path, std::ios_base::binary);
+
+    if (!ofs.is_open()) {
+      std::cerr << "Could not save ExecutionStats to {}.\n"_format(ubjson_path);
+      return false;
+    }
+
+    json::to_ubjson(result, ofs);
+
+    std::cout << "Saved ExecutionStats to {}.\n"_format(ubjson_path);
+  }
+
+  return true;
+}
+
+}  // namespace basalt

src/utils/vio_config.cpp

@@ -0,0 +1,222 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/utils/assert.h>
+#include <basalt/utils/vio_config.h>
+
+#include <fstream>
+
+#include <cereal/archives/json.hpp>
+#include <cereal/cereal.hpp>
+#include <magic_enum.hpp>
+
+namespace basalt {
+
+VioConfig::VioConfig() {
+  // optical_flow_type = "patch";
+  optical_flow_type = "frame_to_frame";
+  optical_flow_detection_grid_size = 50;
+  optical_flow_max_recovered_dist2 = 0.09f;
+  optical_flow_pattern = 51;
+  optical_flow_max_iterations = 5;
+  optical_flow_levels = 3;
+  optical_flow_epipolar_error = 0.005;
+  optical_flow_skip_frames = 1;
+
+  vio_linearization_type = LinearizationType::ABS_QR;
+  vio_sqrt_marg = true;
+
+  vio_max_states = 3;
+  vio_max_kfs = 7;
+  vio_min_frames_after_kf = 5;
+  vio_new_kf_keypoints_thresh = 0.7;
+
+  vio_debug = false;
+  vio_extended_logging = false;
+  vio_obs_std_dev = 0.5;
+  vio_obs_huber_thresh = 1.0;
+  vio_min_triangulation_dist = 0.05;
+  //  vio_outlier_threshold = 3.0;
+  //  vio_filter_iteration = 4;
+  vio_max_iterations = 7;
+
+  vio_enforce_realtime = false;
+
+  vio_use_lm = false;
+  vio_lm_lambda_initial = 1e-4;
+  vio_lm_lambda_min = 1e-6;
+  vio_lm_lambda_max = 1e2;
+
+  vio_scale_jacobian = true;
+
+  vio_init_pose_weight = 1e8;
+  vio_init_ba_weight = 1e1;
+  vio_init_bg_weight = 1e2;
+
+  vio_marg_lost_landmarks = true;
+
+  vio_kf_marg_feature_ratio = 0.1;
+
+  mapper_obs_std_dev = 0.25;
+  mapper_obs_huber_thresh = 1.5;
+  mapper_detection_num_points = 800;
+  mapper_num_frames_to_match = 30;
+  mapper_frames_to_match_threshold = 0.04;
+  mapper_min_matches = 20;
+  mapper_ransac_threshold = 5e-5;
+  mapper_min_track_length = 5;
+  mapper_max_hamming_distance = 70;
+  mapper_second_best_test_ratio = 1.2;
+  mapper_bow_num_bits = 16;
+  mapper_min_triangulation_dist = 0.07;
+  mapper_no_factor_weights = false;
+  mapper_use_factors = true;
+
+  mapper_use_lm = false;
+  mapper_lm_lambda_min = 1e-32;
+  mapper_lm_lambda_max = 1e2;
+}
+
+void VioConfig::save(const std::string& filename) {
+  std::ofstream os(filename);
+
+  {
+    cereal::JSONOutputArchive archive(os);
+    archive(*this);
+  }
+  os.close();
+}
+
+void VioConfig::load(const std::string& filename) {
+  std::ifstream is(filename);
+
+  {
+    cereal::JSONInputArchive archive(is);
+    archive(*this);
+  }
+  is.close();
+}
+}  // namespace basalt
+
+namespace cereal {
+
+template <class Archive>
+std::string save_minimal(const Archive& ar,
+                         const basalt::LinearizationType& linearization_type) {
+  UNUSED(ar);
+  auto name = magic_enum::enum_name(linearization_type);
+  return std::string(name);
+}
+
+template <class Archive>
+void load_minimal(const Archive& ar,
+                  basalt::LinearizationType& linearization_type,
+                  const std::string& name) {
+  UNUSED(ar);
+
+  auto lin_enum = magic_enum::enum_cast<basalt::LinearizationType>(name);
+
+  if (lin_enum.has_value()) {
+    linearization_type = lin_enum.value();
+  } else {
+    std::cerr << "Could not find the LinearizationType for " << name
+              << std::endl;
+    std::abort();
+  }
+}
+
+template <class Archive>
+void serialize(Archive& ar, basalt::VioConfig& config) {
+  ar(CEREAL_NVP(config.optical_flow_type));
+  ar(CEREAL_NVP(config.optical_flow_detection_grid_size));
+  ar(CEREAL_NVP(config.optical_flow_max_recovered_dist2));
+  ar(CEREAL_NVP(config.optical_flow_pattern));
+  ar(CEREAL_NVP(config.optical_flow_max_iterations));
+  ar(CEREAL_NVP(config.optical_flow_epipolar_error));
+  ar(CEREAL_NVP(config.optical_flow_levels));
+  ar(CEREAL_NVP(config.optical_flow_skip_frames));
+
+  ar(CEREAL_NVP(config.vio_linearization_type));
+  ar(CEREAL_NVP(config.vio_sqrt_marg));
+  ar(CEREAL_NVP(config.vio_max_states));
+  ar(CEREAL_NVP(config.vio_max_kfs));
+  ar(CEREAL_NVP(config.vio_min_frames_after_kf));
+  ar(CEREAL_NVP(config.vio_new_kf_keypoints_thresh));
+  ar(CEREAL_NVP(config.vio_debug));
+  ar(CEREAL_NVP(config.vio_extended_logging));
+  ar(CEREAL_NVP(config.vio_max_iterations));
+  //  ar(CEREAL_NVP(config.vio_outlier_threshold));
+  //  ar(CEREAL_NVP(config.vio_filter_iteration));
+
+  ar(CEREAL_NVP(config.vio_obs_std_dev));
+  ar(CEREAL_NVP(config.vio_obs_huber_thresh));
+  ar(CEREAL_NVP(config.vio_min_triangulation_dist));
+
+  ar(CEREAL_NVP(config.vio_enforce_realtime));
+
+  ar(CEREAL_NVP(config.vio_use_lm));
+  ar(CEREAL_NVP(config.vio_lm_lambda_initial));
+  ar(CEREAL_NVP(config.vio_lm_lambda_min));
+  ar(CEREAL_NVP(config.vio_lm_lambda_max));
+
+  ar(CEREAL_NVP(config.vio_scale_jacobian));
+
+  ar(CEREAL_NVP(config.vio_init_pose_weight));
+  ar(CEREAL_NVP(config.vio_init_ba_weight));
+  ar(CEREAL_NVP(config.vio_init_bg_weight));
+
+  ar(CEREAL_NVP(config.vio_marg_lost_landmarks));
+  ar(CEREAL_NVP(config.vio_kf_marg_feature_ratio));
+
+  ar(CEREAL_NVP(config.mapper_obs_std_dev));
+  ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
+  ar(CEREAL_NVP(config.mapper_detection_num_points));
+  ar(CEREAL_NVP(config.mapper_num_frames_to_match));
+  ar(CEREAL_NVP(config.mapper_frames_to_match_threshold));
+  ar(CEREAL_NVP(config.mapper_min_matches));
+  ar(CEREAL_NVP(config.mapper_ransac_threshold));
+  ar(CEREAL_NVP(config.mapper_min_track_length));
+  ar(CEREAL_NVP(config.mapper_max_hamming_distance));
+  ar(CEREAL_NVP(config.mapper_second_best_test_ratio));
+  ar(CEREAL_NVP(config.mapper_bow_num_bits));
+  ar(CEREAL_NVP(config.mapper_min_triangulation_dist));
+  ar(CEREAL_NVP(config.mapper_no_factor_weights));
+  ar(CEREAL_NVP(config.mapper_use_factors));
+
+  ar(CEREAL_NVP(config.mapper_use_lm));
+  ar(CEREAL_NVP(config.mapper_lm_lambda_min));
+  ar(CEREAL_NVP(config.mapper_lm_lambda_max));
+}
+}  // namespace cereal

src/vi_estimator/ba_base.cpp

@@ -0,0 +1,604 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/vi_estimator/ba_base.h>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+
+#include <basalt/utils/ba_utils.h>
+
+namespace basalt {
+
+template <class Scalar>
+void BundleAdjustmentBase<Scalar>::optimize_single_frame_pose(
+    PoseStateWithLin<Scalar>& state_t,
+    const std::vector<std::vector<int>>& connected_obs) const {
+  const int num_iter = 2;
+
+  struct AbsLinData {
+    Mat4 T_t_h;
+    Mat6 d_rel_d_t;
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+  };
+
+  for (int iter = 0; iter < num_iter; iter++) {
+    Scalar error = 0;
+    Mat6 Ht;
+    Vec6 bt;
+
+    Ht.setZero();
+    bt.setZero();
+
+    std::unordered_map<std::pair<TimeCamId, TimeCamId>, AbsLinData>
+        abs_lin_data;
+
+    for (size_t cam_id = 0; cam_id < connected_obs.size(); cam_id++) {
+      TimeCamId tcid_t(state_t.getT_ns(), cam_id);
+      for (const auto& lm_id : connected_obs[cam_id]) {
+        const Keypoint<Scalar>& kpt_pos = lmdb.getLandmark(lm_id);
+        std::pair<TimeCamId, TimeCamId> map_key(kpt_pos.host_kf_id, tcid_t);
+
+        if (abs_lin_data.count(map_key) == 0) {
+          const PoseStateWithLin<Scalar>& state_h =
+              frame_poses.at(kpt_pos.host_kf_id.frame_id);
+
+          BASALT_ASSERT(kpt_pos.host_kf_id.frame_id != state_t.getT_ns());
+
+          AbsLinData& ald = abs_lin_data[map_key];
+
+          SE3 T_t_h_sophus = computeRelPose<Scalar>(
+              state_h.getPose(), calib.T_i_c[kpt_pos.host_kf_id.cam_id],
+              state_t.getPose(), calib.T_i_c[cam_id], nullptr, &ald.d_rel_d_t);
+          ald.T_t_h = T_t_h_sophus.matrix();
+        }
+      }
+    }
+
+    for (size_t cam_id = 0; cam_id < connected_obs.size(); cam_id++) {
+      std::visit(
+          [&](const auto& cam) {
+            for (const auto& lm_id : connected_obs[cam_id]) {
+              TimeCamId tcid_t(state_t.getT_ns(), cam_id);
+
+              const Keypoint<Scalar>& kpt_pos = lmdb.getLandmark(lm_id);
+              const Vec2& kpt_obs = kpt_pos.obs.at(tcid_t);
+              const AbsLinData& ald =
+                  abs_lin_data.at(std::make_pair(kpt_pos.host_kf_id, tcid_t));
+
+              Vec2 res;
+              Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
+              bool valid = linearizePoint(kpt_obs, kpt_pos, ald.T_t_h, cam, res,
+                                          &d_res_d_xi);
+
+              if (valid) {
+                Scalar e = res.norm();
+                Scalar huber_weight =
+                    e < huber_thresh ? Scalar(1.0) : huber_thresh / e;
+                Scalar obs_weight = huber_weight / (obs_std_dev * obs_std_dev);
+
+                error += Scalar(0.5) * (2 - huber_weight) * obs_weight *
+                         res.transpose() * res;
+
+                d_res_d_xi *= ald.d_rel_d_t;
+
+                Ht.noalias() += d_res_d_xi.transpose() * d_res_d_xi;
+                bt.noalias() += d_res_d_xi.transpose() * res;
+              }
+            }
+          },
+          calib.intrinsics[cam_id].variant);
+    }
+
+    // Add small damping for GN
+    constexpr Scalar lambda = 1e-6;
+    Vec6 diag = Ht.diagonal();
+    diag *= lambda;
+    Ht.diagonal().array() += diag.array().max(lambda);
+
+    // std::cout << "pose opt error " << error << std::endl;
+    Vec6 inc = -Ht.ldlt().solve(bt);
+    state_t.applyInc(inc);
+  }
+  // std::cout << "=============================" << std::endl;
+}
+
+template <class Scalar_>
+void BundleAdjustmentBase<Scalar_>::computeError(
+    Scalar& error,
+    std::map<int, std::vector<std::pair<TimeCamId, Scalar>>>* outliers,
+    Scalar outlier_threshold) const {
+  std::vector<TimeCamId> host_frames;
+  for (const auto& [tcid, _] : lmdb.getObservations()) {
+    host_frames.push_back(tcid);
+  }
+
+  tbb::concurrent_unordered_map<int, std::vector<std::pair<TimeCamId, Scalar>>>
+      outliers_concurrent;
+
+  auto body = [&](const tbb::blocked_range<size_t>& range, Scalar local_error) {
+    for (size_t r = range.begin(); r != range.end(); ++r) {
+      const TimeCamId& tcid_h = host_frames[r];
+
+      for (const auto& obs_kv : lmdb.getObservations().at(tcid_h)) {
+        const TimeCamId& tcid_t = obs_kv.first;
+
+        Mat4 T_t_h;
+
+        if (tcid_h != tcid_t) {
+          PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
+          PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
+
+          Sophus::SE3<Scalar> T_t_h_sophus =
+              computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
+                             state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
+
+          T_t_h = T_t_h_sophus.matrix();
+        } else {
+          T_t_h.setIdentity();
+        }
+
+        std::visit(
+            [&](const auto& cam) {
+              for (KeypointId kpt_id : obs_kv.second) {
+                const Keypoint<Scalar>& kpt_pos = lmdb.getLandmark(kpt_id);
+                const Vec2& kpt_obs = kpt_pos.obs.at(tcid_t);
+
+                Vec2 res;
+
+                bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res);
+
+                if (valid) {
+                  Scalar e = res.norm();
+
+                  if (outliers && e > outlier_threshold) {
+                    outliers_concurrent[kpt_id].emplace_back(
+                        tcid_t, tcid_h != tcid_t ? e : -2);
+                  }
+
+                  Scalar huber_weight =
+                      e < huber_thresh ? Scalar(1.0) : huber_thresh / e;
+                  Scalar obs_weight =
+                      huber_weight / (obs_std_dev * obs_std_dev);
+
+                  local_error += Scalar(0.5) * (2 - huber_weight) * obs_weight *
+                                 res.transpose() * res;
+                } else {
+                  if (outliers) {
+                    outliers_concurrent[kpt_id].emplace_back(
+                        tcid_t, tcid_h != tcid_t ? -1 : -2);
+                  }
+                }
+              }
+            },
+            calib.intrinsics[tcid_t.cam_id].variant);
+      }
+    }
+
+    return local_error;
+  };
+
+  tbb::blocked_range<size_t> range(0, host_frames.size());
+  Scalar init = 0;
+  auto join = std::plus<Scalar>();
+  error = tbb::parallel_reduce(range, init, body, join);
+
+  if (outliers) {
+    outliers->clear();
+    for (auto& [k, v] : outliers_concurrent) {
+      outliers->emplace(k, std::move(v));
+    }
+  }
+}
+
+template <class Scalar_>
+template <class Scalar2>
+void BundleAdjustmentBase<Scalar_>::get_current_points(
+    Eigen::aligned_vector<Eigen::Matrix<Scalar2, 3, 1>>& points,
+    std::vector<int>& ids) const {
+  points.clear();
+  ids.clear();
+
+  for (const auto& tcid_host : lmdb.getHostKfs()) {
+    Sophus::SE3<Scalar> T_w_i;
+
+    int64_t id = tcid_host.frame_id;
+    if (frame_states.count(id) > 0) {
+      PoseVelBiasStateWithLin<Scalar> state = frame_states.at(id);
+      T_w_i = state.getState().T_w_i;
+    } else if (frame_poses.count(id) > 0) {
+      PoseStateWithLin<Scalar> state = frame_poses.at(id);
+
+      T_w_i = state.getPose();
+    } else {
+      std::cout << "Unknown frame id: " << id << std::endl;
+      std::abort();
+    }
+
+    const Sophus::SE3<Scalar>& T_i_c = calib.T_i_c[tcid_host.cam_id];
+    Mat4 T_w_c = (T_w_i * T_i_c).matrix();
+
+    for (const Keypoint<Scalar>* kpt_pos :
+         lmdb.getLandmarksForHost(tcid_host)) {
+      Vec4 pt_cam = StereographicParam<Scalar>::unproject(kpt_pos->direction);
+      pt_cam[3] = kpt_pos->inv_dist;
+
+      Vec4 pt_w = T_w_c * pt_cam;
+
+      points.emplace_back(
+          (pt_w.template head<3>() / pt_w[3]).template cast<Scalar2>());
+      ids.emplace_back(1);
+    }
+  }
+}
+
+template <class Scalar_>
+void BundleAdjustmentBase<Scalar_>::filterOutliers(Scalar outlier_threshold,
+                                                   int min_num_obs) {
+  Scalar error;
+  std::map<int, std::vector<std::pair<TimeCamId, Scalar>>> outliers;
+  computeError(error, &outliers, outlier_threshold);
+
+  //  std::cout << "============================================" <<
+  //  std::endl; std::cout << "Num landmarks: " << lmdb.numLandmarks() << "
+  //  with outliners
+  //  "
+  //            << outliers.size() << std::endl;
+
+  for (const auto& kv : outliers) {
+    int num_obs = lmdb.numObservations(kv.first);
+    int num_outliers = kv.second.size();
+
+    bool remove = false;
+
+    if (num_obs - num_outliers < min_num_obs) remove = true;
+
+    //    std::cout << "\tlm_id: " << kv.first << " num_obs: " << num_obs
+    //              << " outliers: " << num_outliers << " [";
+
+    for (const auto& kv2 : kv.second) {
+      if (kv2.second == -2) remove = true;
+
+      //      std::cout << kv2.second << ", ";
+    }
+
+    //    std::cout << "] " << std::endl;
+
+    if (remove) {
+      lmdb.removeLandmark(kv.first);
+    } else {
+      std::set<TimeCamId> outliers;
+      for (const auto& kv2 : kv.second) outliers.emplace(kv2.first);
+      lmdb.removeObservations(kv.first, outliers);
+    }
+  }
+
+  // std::cout << "============================================" <<
+  // std::endl;
+}
+
+template <class Scalar_>
+void BundleAdjustmentBase<Scalar_>::computeDelta(const AbsOrderMap& marg_order,
+                                                 VecX& delta) const {
+  size_t marg_size = marg_order.total_size;
+  delta.setZero(marg_size);
+  for (const auto& kv : marg_order.abs_order_map) {
+    if (kv.second.second == POSE_SIZE) {
+      BASALT_ASSERT(frame_poses.at(kv.first).isLinearized());
+      delta.template segment<POSE_SIZE>(kv.second.first) =
+          frame_poses.at(kv.first).getDelta();
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      BASALT_ASSERT(frame_states.at(kv.first).isLinearized());
+      delta.template segment<POSE_VEL_BIAS_SIZE>(kv.second.first) =
+          frame_states.at(kv.first).getDelta();
+    } else {
+      BASALT_ASSERT(false);
+    }
+  }
+}
+
+template <class Scalar_>
+Scalar_ BundleAdjustmentBase<Scalar_>::computeModelCostChange(
+    const MatX& H, const VecX& b, const VecX& inc) const {
+  // Linearized model cost
+  //
+  //    L(x) = 0.5 || J*x + r ||^2
+  //         = 0.5 x^T J^T J x + x^T J r + 0.5 r^T r
+  //         = 0.5 x^T H x + x^T b + 0.5 r^T r,
+  //
+  // given in normal equation form as
+  //
+  //    H = J^T J,
+  //    b = J^T r.
+  //
+  // The expected decrease in cost for the computed increment is
+  //
+  //     l_diff = L(0) - L(inc)
+  //            = - 0.5 inc^T H inc - inc^T b
+  //            = - inc^T (0.5 H inc + b)
+
+  Scalar l_diff = -inc.dot(Scalar(0.5) * (H * inc) + b);
+
+  return l_diff;
+}
+
+template <class Scalar_>
+template <class Scalar2>
+void BundleAdjustmentBase<Scalar_>::computeProjections(
+    std::vector<Eigen::aligned_vector<Eigen::Matrix<Scalar2, 4, 1>>>& data,
+    FrameId last_state_t_ns) const {
+  for (const auto& kv : lmdb.getObservations()) {
+    const TimeCamId& tcid_h = kv.first;
+
+    for (const auto& obs_kv : kv.second) {
+      const TimeCamId& tcid_t = obs_kv.first;
+
+      if (tcid_t.frame_id != last_state_t_ns) continue;
+
+      Mat4 T_t_h;
+      if (tcid_h != tcid_t) {
+        PoseStateWithLin<Scalar> state_h = getPoseStateWithLin(tcid_h.frame_id);
+        PoseStateWithLin<Scalar> state_t = getPoseStateWithLin(tcid_t.frame_id);
+
+        Sophus::SE3<Scalar> T_t_h_sophus =
+            computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
+                           state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
+
+        T_t_h = T_t_h_sophus.matrix();
+      } else {
+        T_t_h.setIdentity();
+      }
+
+      std::visit(
+          [&](const auto& cam) {
+            for (KeypointId kpt_id : obs_kv.second) {
+              const Keypoint<Scalar>& kpt_pos = lmdb.getLandmark(kpt_id);
+
+              Vec2 res;
+              Vec4 proj;
+
+              using CamT = std::decay_t<decltype(cam)>;
+              linearizePoint<Scalar, CamT>(Vec2::Zero(), kpt_pos, T_t_h, cam,
+                                           res, nullptr, nullptr, &proj);
+
+              proj[3] = kpt_id;
+              data[tcid_t.cam_id].emplace_back(proj.template cast<Scalar2>());
+            }
+          },
+          calib.intrinsics[tcid_t.cam_id].variant);
+    }
+  }
+}
+
+template <class Scalar_>
+void BundleAdjustmentBase<Scalar_>::linearizeMargPrior(
+    const MargLinData<Scalar>& mld, const AbsOrderMap& aom, MatX& abs_H,
+    VecX& abs_b, Scalar& marg_prior_error) const {
+  // Prior is ordered to be in the top left corner of Hessian
+
+  BASALT_ASSERT(size_t(mld.H.cols()) == mld.order.total_size);
+
+  // Check if the order of variables is the same, and it's indeed top-left
+  // corner
+  for (const auto& kv : mld.order.abs_order_map) {
+    UNUSED(aom);
+    UNUSED(kv);
+    BASALT_ASSERT(aom.abs_order_map.at(kv.first) == kv.second);
+    BASALT_ASSERT(kv.second.first < int(mld.order.total_size));
+  }
+
+  // Quadratic prior and "delta" of the current state to the original
+  // linearization point give cost function
+  //
+  //    P(x) = 0.5 || J*(delta+x) + r ||^2,
+  //
+  // alternatively stored in quadratic form as
+  //
+  //    Hmarg = J^T J,
+  //    bmarg = J^T r.
+  //
+  // This is now to be linearized at x=0, so we get linearization
+  //
+  //    P(x) = 0.5 || J*x + (J*delta + r) ||^2,
+  //
+  // with Jacobian J and residual J*delta + r. The normal equations are
+  //
+  //    H*x + b = 0,
+  //    H = J^T J = Hmarg,
+  //    b = J^T (J*delta + r) = Hmarg*delta + bmarg.
+  //
+  // The current cost is
+  //
+  //    P(0) = 0.5 || J*delta + r ||^2
+  //         = 0.5 delta^T J^T J delta + delta^T J^T r + 0.5 r^T r.
+  //         = 0.5 delta^T Hmarg delta + delta^T bmarg + 0.5 r^T r.
+  //
+  // Note: Since the r^T r term does not change with delta, we drop it from the
+  // error computation. The main need for the error value is for comparing
+  // the cost before and after an update to delta in the optimization loop. This
+  // also means the computed error can be negative.
+
+  const size_t marg_size = mld.order.total_size;
+
+  VecX delta;
+  computeDelta(mld.order, delta);
+
+  if (mld.is_sqrt) {
+    abs_H.topLeftCorner(marg_size, marg_size) += mld.H.transpose() * mld.H;
+
+    abs_b.head(marg_size) += mld.H.transpose() * (mld.b + mld.H * delta);
+
+    marg_prior_error = delta.transpose() * mld.H.transpose() *
+                       (Scalar(0.5) * mld.H * delta + mld.b);
+  } else {
+    abs_H.topLeftCorner(marg_size, marg_size) += mld.H;
+
+    abs_b.head(marg_size) += mld.H * delta + mld.b;
+
+    marg_prior_error =
+        delta.transpose() * (Scalar(0.5) * mld.H * delta + mld.b);
+  }
+}
+
+template <class Scalar_>
+void BundleAdjustmentBase<Scalar_>::computeMargPriorError(
+    const MargLinData<Scalar>& mld, Scalar& marg_prior_error) const {
+  BASALT_ASSERT(size_t(mld.H.cols()) == mld.order.total_size);
+
+  // The current cost is (see above in linearizeMargPrior())
+  //
+  //    P(0) = 0.5 || J*delta + r ||^2
+  //         = 0.5 delta^T J^T J delta + delta^T J^T r + 0.5 r^T r
+  //         = 0.5 delta^T Hmarg delta + delta^T bmarg + 0.5 r^T r.
+  //
+  // Note: Since the r^T r term does not change with delta, we drop it from the
+  // error computation. The main need for the error value is for comparing
+  // the cost before and after an update to delta in the optimization loop. This
+  // also means the computed error can be negative.
+
+  VecX delta;
+  computeDelta(mld.order, delta);
+
+  if (mld.is_sqrt) {
+    marg_prior_error = delta.transpose() * mld.H.transpose() *
+                       (Scalar(0.5) * mld.H * delta + mld.b);
+  } else {
+    marg_prior_error =
+        delta.transpose() * (Scalar(0.5) * mld.H * delta + mld.b);
+  }
+}
+
+template <class Scalar_>
+Scalar_ BundleAdjustmentBase<Scalar_>::computeMargPriorModelCostChange(
+    const MargLinData<Scalar>& mld, const VecX& marg_scaling,
+    const VecX& marg_pose_inc) const {
+  // Quadratic prior and "delta" of the current state to the original
+  // linearization point give cost function
+  //
+  //    P(x) = 0.5 || J*(delta+x) + r ||^2,
+  //
+  // alternatively stored in quadratic form as
+  //
+  //    Hmarg = J^T J,
+  //    bmarg = J^T r.
+  //
+  // We want to compute the model cost change. The model function is
+  //
+  //     L(inc) = P(inc) = 0.5 || J*inc + (J*delta + r) ||^2
+  //
+  // By setting rlin = J*delta + r we get
+  //
+  //     L(inc) = 0.5 || J*inc + rlin ||^2
+  //            = P(0) + inc^T J^T rlin + 0.5 inc^T J^T J inc
+  //
+  // and thus the expected decrease in cost for the computed increment is
+  //
+  //     l_diff = L(0) - L(inc)
+  //            = - inc^T J^T rlin - 0.5 inc^T J^T J inc
+  //            = - inc^T J^T (rlin + 0.5 J inc)
+  //            = - (J inc)^T (rlin + 0.5 (J inc))
+  //            = - (J inc)^T (J*delta + r + 0.5 (J inc)).
+  //
+  // Alternatively, for squared prior storage, we get
+  //
+  //     l_diff = - inc^T (Hmarg delta + bmarg + 0.5 Hmarg inc)
+  //            = - inc^T (Hmarg (delta + 0.5 inc) + bmarg)
+  //
+  // For Jacobian scaling we need to take extra care. Note that we store the
+  // scale separately and don't actually update marg_sqrt_H and marg_sqrt_b
+  // in place with the scale. So in the computation above, we need to scale
+  // marg_sqrt_H whenever it is multiplied with inc, but NOT when it is
+  // multiplied with delta, since delta is also WITHOUT scaling.
+
+  VecX delta;
+  computeDelta(mld.order, delta);
+
+  VecX J_inc = marg_pose_inc;
+  if (marg_scaling.rows() > 0) J_inc = marg_scaling.asDiagonal() * J_inc;
+
+  Scalar l_diff;
+
+  if (mld.is_sqrt) {
+    // No scaling here. This is b part not Jacobian
+    const VecX b_Jdelta = mld.H * delta + mld.b;
+
+    J_inc = mld.H * J_inc;
+    l_diff = -J_inc.transpose() * (b_Jdelta + Scalar(0.5) * J_inc);
+  } else {
+    l_diff = -J_inc.dot(mld.H * (delta + Scalar(0.5) * J_inc) + mld.b);
+  }
+
+  return l_diff;
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+// Note: double specialization is unconditional, b/c NfrMapper depends on it.
+//#ifdef BASALT_INSTANTIATIONS_DOUBLE
+template class BundleAdjustmentBase<double>;
+
+template void BundleAdjustmentBase<double>::get_current_points<double>(
+    Eigen::aligned_vector<Eigen::Matrix<double, 3, 1>>& points,
+    std::vector<int>& ids) const;
+
+template void BundleAdjustmentBase<double>::computeProjections<double>(
+    std::vector<Eigen::aligned_vector<Eigen::Matrix<double, 4, 1>>>& data,
+    FrameId last_state_t_ns) const;
+//#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+template class BundleAdjustmentBase<float>;
+
+// template void BundleAdjustmentBase<float>::get_current_points<float>(
+//    Eigen::aligned_vector<Eigen::Matrix<float, 3, 1>>& points,
+//    std::vector<int>& ids) const;
+
+template void BundleAdjustmentBase<float>::get_current_points<double>(
+    Eigen::aligned_vector<Eigen::Matrix<double, 3, 1>>& points,
+    std::vector<int>& ids) const;
+
+// template void BundleAdjustmentBase<float>::computeProjections<float>(
+//    std::vector<Eigen::aligned_vector<Eigen::Matrix<float, 4, 1>>>& data,
+//    FrameId last_state_t_ns) const;
+
+template void BundleAdjustmentBase<float>::computeProjections<double>(
+    std::vector<Eigen::aligned_vector<Eigen::Matrix<double, 4, 1>>>& data,
+    FrameId last_state_t_ns) const;
+#endif
+
+}  // namespace basalt

src/vi_estimator/landmark_database.cpp

@@ -0,0 +1,247 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+
+#include <basalt/vi_estimator/landmark_database.h>
+
+namespace basalt {
+
+template <class Scalar_>
+void LandmarkDatabase<Scalar_>::addLandmark(KeypointId lm_id,
+                                            const Keypoint<Scalar> &pos) {
+  auto &kpt = kpts[lm_id];
+  kpt.direction = pos.direction;
+  kpt.inv_dist = pos.inv_dist;
+  kpt.host_kf_id = pos.host_kf_id;
+}
+
+template <class Scalar_>
+void LandmarkDatabase<Scalar_>::removeFrame(const FrameId &frame) {
+  for (auto it = kpts.begin(); it != kpts.end();) {
+    for (auto it2 = it->second.obs.begin(); it2 != it->second.obs.end();) {
+      if (it2->first.frame_id == frame)
+        it2 = removeLandmarkObservationHelper(it, it2);
+      else
+        it2++;
+    }
+
+    if (it->second.obs.size() < min_num_obs) {
+      it = removeLandmarkHelper(it);
+    } else {
+      ++it;
+    }
+  }
+}
+
+template <class Scalar_>
+void LandmarkDatabase<Scalar_>::removeKeyframes(
+    const std::set<FrameId> &kfs_to_marg,
+    const std::set<FrameId> &poses_to_marg,
+    const std::set<FrameId> &states_to_marg_all) {
+  for (auto it = kpts.begin(); it != kpts.end();) {
+    if (kfs_to_marg.count(it->second.host_kf_id.frame_id) > 0) {
+      it = removeLandmarkHelper(it);
+    } else {
+      for (auto it2 = it->second.obs.begin(); it2 != it->second.obs.end();) {
+        FrameId fid = it2->first.frame_id;
+        if (poses_to_marg.count(fid) > 0 || states_to_marg_all.count(fid) > 0 ||
+            kfs_to_marg.count(fid) > 0)
+          it2 = removeLandmarkObservationHelper(it, it2);
+        else
+          it2++;
+      }
+
+      if (it->second.obs.size() < min_num_obs) {
+        it = removeLandmarkHelper(it);
+      } else {
+        ++it;
+      }
+    }
+  }
+}
+
+template <class Scalar_>
+std::vector<TimeCamId> LandmarkDatabase<Scalar_>::getHostKfs() const {
+  std::vector<TimeCamId> res;
+
+  for (const auto &kv : observations) res.emplace_back(kv.first);
+
+  return res;
+}
+
+template <class Scalar_>
+std::vector<const Keypoint<Scalar_> *>
+LandmarkDatabase<Scalar_>::getLandmarksForHost(const TimeCamId &tcid) const {
+  std::vector<const Keypoint<Scalar> *> res;
+
+  for (const auto &[k, obs] : observations.at(tcid))
+    for (const auto &v : obs) res.emplace_back(&kpts.at(v));
+
+  return res;
+}
+
+template <class Scalar_>
+void LandmarkDatabase<Scalar_>::addObservation(
+    const TimeCamId &tcid_target, const KeypointObservation<Scalar> &o) {
+  auto it = kpts.find(o.kpt_id);
+  BASALT_ASSERT(it != kpts.end());
+
+  it->second.obs[tcid_target] = o.pos;
+
+  observations[it->second.host_kf_id][tcid_target].insert(it->first);
+}
+
+template <class Scalar_>
+Keypoint<Scalar_> &LandmarkDatabase<Scalar_>::getLandmark(KeypointId lm_id) {
+  return kpts.at(lm_id);
+}
+
+template <class Scalar_>
+const Keypoint<Scalar_> &LandmarkDatabase<Scalar_>::getLandmark(
+    KeypointId lm_id) const {
+  return kpts.at(lm_id);
+}
+
+template <class Scalar_>
+const std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
+    &LandmarkDatabase<Scalar_>::getObservations() const {
+  return observations;
+}
+
+template <class Scalar_>
+const Eigen::aligned_unordered_map<KeypointId, Keypoint<Scalar_>>
+    &LandmarkDatabase<Scalar_>::getLandmarks() const {
+  return kpts;
+}
+
+template <class Scalar_>
+bool LandmarkDatabase<Scalar_>::landmarkExists(int lm_id) const {
+  return kpts.count(lm_id) > 0;
+}
+
+template <class Scalar_>
+size_t LandmarkDatabase<Scalar_>::numLandmarks() const {
+  return kpts.size();
+}
+
+template <class Scalar_>
+int LandmarkDatabase<Scalar_>::numObservations() const {
+  int num_observations = 0;
+
+  for (const auto &[_, val_map] : observations) {
+    for (const auto &[_, val] : val_map) {
+      num_observations += val.size();
+    }
+  }
+
+  return num_observations;
+}
+
+template <class Scalar_>
+int LandmarkDatabase<Scalar_>::numObservations(KeypointId lm_id) const {
+  return kpts.at(lm_id).obs.size();
+}
+
+template <class Scalar_>
+typename LandmarkDatabase<Scalar_>::MapIter
+LandmarkDatabase<Scalar_>::removeLandmarkHelper(
+    LandmarkDatabase<Scalar>::MapIter it) {
+  auto host_it = observations.find(it->second.host_kf_id);
+
+  for (const auto &[k, v] : it->second.obs) {
+    auto target_it = host_it->second.find(k);
+    target_it->second.erase(it->first);
+
+    if (target_it->second.empty()) host_it->second.erase(target_it);
+  }
+
+  if (host_it->second.empty()) observations.erase(host_it);
+
+  return kpts.erase(it);
+}
+
+template <class Scalar_>
+typename Keypoint<Scalar_>::MapIter
+LandmarkDatabase<Scalar_>::removeLandmarkObservationHelper(
+    LandmarkDatabase<Scalar>::MapIter it,
+    typename Keypoint<Scalar>::MapIter it2) {
+  auto host_it = observations.find(it->second.host_kf_id);
+  auto target_it = host_it->second.find(it2->first);
+  target_it->second.erase(it->first);
+
+  if (target_it->second.empty()) host_it->second.erase(target_it);
+  if (host_it->second.empty()) observations.erase(host_it);
+
+  return it->second.obs.erase(it2);
+}
+
+template <class Scalar_>
+void LandmarkDatabase<Scalar_>::removeLandmark(KeypointId lm_id) {
+  auto it = kpts.find(lm_id);
+  if (it != kpts.end()) removeLandmarkHelper(it);
+}
+
+template <class Scalar_>
+void LandmarkDatabase<Scalar_>::removeObservations(
+    KeypointId lm_id, const std::set<TimeCamId> &obs) {
+  auto it = kpts.find(lm_id);
+  BASALT_ASSERT(it != kpts.end());
+
+  for (auto it2 = it->second.obs.begin(); it2 != it->second.obs.end();) {
+    if (obs.count(it2->first) > 0) {
+      it2 = removeLandmarkObservationHelper(it, it2);
+    } else
+      it2++;
+  }
+
+  if (it->second.obs.size() < min_num_obs) {
+    removeLandmarkHelper(it);
+  }
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+// Note: double specialization is unconditional, b/c NfrMapper depends on it.
+//#ifdef BASALT_INSTANTIATIONS_DOUBLE
+template class LandmarkDatabase<double>;
+//#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+template class LandmarkDatabase<float>;
+#endif
+
+}  // namespace basalt

src/vi_estimator/marg_helper.cpp

@@ -0,0 +1,361 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/utils/assert.h>
+#include <basalt/vi_estimator/marg_helper.h>
+
+namespace basalt {
+
+template <class Scalar_>
+void MargHelper<Scalar_>::marginalizeHelperSqToSq(
+    MatX& abs_H, VecX& abs_b, const std::set<int>& idx_to_keep,
+    const std::set<int>& idx_to_marg, MatX& marg_H, VecX& marg_b) {
+  int keep_size = idx_to_keep.size();
+  int marg_size = idx_to_marg.size();
+
+  BASALT_ASSERT(keep_size + marg_size == abs_H.cols());
+
+  // Fill permutation matrix
+  Eigen::Matrix<int, Eigen::Dynamic, 1> indices(idx_to_keep.size() +
+                                                idx_to_marg.size());
+
+  {
+    auto it = idx_to_keep.begin();
+    for (size_t i = 0; i < idx_to_keep.size(); i++) {
+      indices[i] = *it;
+      it++;
+    }
+  }
+
+  {
+    auto it = idx_to_marg.begin();
+    for (size_t i = 0; i < idx_to_marg.size(); i++) {
+      indices[idx_to_keep.size() + i] = *it;
+      it++;
+    }
+  }
+
+  const Eigen::PermutationWrapper<Eigen::Matrix<int, Eigen::Dynamic, 1>> p(
+      indices);
+
+  const Eigen::PermutationMatrix<Eigen::Dynamic> pt = p.transpose();
+
+  abs_b.applyOnTheLeft(pt);
+  abs_H.applyOnTheLeft(pt);
+  abs_H.applyOnTheRight(p);
+
+  // Use of fullPivLu compared to alternatives was determined experimentally. It
+  // is more stable than ldlt when the matrix is numerically close ot
+  // indefinite, but it is faster than the even more stable
+  // fullPivHouseholderQr.
+
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size).ldlt();
+
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size).fullPivLu();
+  //  MatX H_mm_inv = H_mm_decomposition.inverse();
+
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size).colPivHouseholderQr();
+
+  // DO NOT USE!!!
+  // Pseudoinverse with SVD. Uses iterative method and results in severe low
+  // accuracy. Use the version below (COD) for pseudoinverse
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size)
+  //          .jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+  // Eigen version of pseudoinverse
+  auto H_mm_decomposition = abs_H.bottomRightCorner(marg_size, marg_size)
+                                .completeOrthogonalDecomposition();
+  MatX H_mm_inv = H_mm_decomposition.pseudoInverse();
+
+  // Should be more numerially stable version of:
+  abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv;
+  //  abs_H.topRightCorner(keep_size, marg_size) =
+  //      H_mm_decomposition
+  //          .solve(abs_H.topRightCorner(keep_size, marg_size).transpose())
+  //          .transpose();
+
+  marg_H = abs_H.topLeftCorner(keep_size, keep_size);
+  marg_b = abs_b.head(keep_size);
+
+  marg_H -= abs_H.topRightCorner(keep_size, marg_size) *
+            abs_H.bottomLeftCorner(marg_size, keep_size);
+  marg_b -= abs_H.topRightCorner(keep_size, marg_size) * abs_b.tail(marg_size);
+
+  abs_H.resize(0, 0);
+  abs_b.resize(0);
+}
+
+template <class Scalar_>
+void MargHelper<Scalar_>::marginalizeHelperSqToSqrt(
+    MatX& abs_H, VecX& abs_b, const std::set<int>& idx_to_keep,
+    const std::set<int>& idx_to_marg, MatX& marg_sqrt_H, VecX& marg_sqrt_b) {
+  // TODO: We might lose the strong initial pose prior if there are no obs
+  // during marginalization (e.g. during first marginalization). Unclear when
+  // this can happen. Keeping keyframes w/o any points in the optimization
+  // window might not make sense. --> Double check if and when this currently
+  // occurs. If no, add asserts. If yes, somehow deal with it differently.
+
+  int keep_size = idx_to_keep.size();
+  int marg_size = idx_to_marg.size();
+
+  BASALT_ASSERT(keep_size + marg_size == abs_H.cols());
+
+  // Fill permutation matrix
+  Eigen::Matrix<int, Eigen::Dynamic, 1> indices(idx_to_keep.size() +
+                                                idx_to_marg.size());
+
+  {
+    auto it = idx_to_keep.begin();
+    for (size_t i = 0; i < idx_to_keep.size(); i++) {
+      indices[i] = *it;
+      it++;
+    }
+  }
+
+  {
+    auto it = idx_to_marg.begin();
+    for (size_t i = 0; i < idx_to_marg.size(); i++) {
+      indices[idx_to_keep.size() + i] = *it;
+      it++;
+    }
+  }
+
+  const Eigen::PermutationWrapper<Eigen::Matrix<int, Eigen::Dynamic, 1>> p(
+      indices);
+
+  const Eigen::PermutationMatrix<Eigen::Dynamic> pt = p.transpose();
+
+  abs_b.applyOnTheLeft(pt);
+  abs_H.applyOnTheLeft(pt);
+  abs_H.applyOnTheRight(p);
+
+  // Use of fullPivLu compared to alternatives was determined experimentally. It
+  // is more stable than ldlt when the matrix is numerically close ot
+  // indefinite, but it is faster than the even more stable
+  // fullPivHouseholderQr.
+
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size).ldlt();
+
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size).fullPivLu();
+  //  MatX H_mm_inv = H_mm_decomposition.inverse();
+
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size).colPivHouseholderQr();
+
+  // DO NOT USE!!!
+  // Pseudoinverse with SVD. Uses iterative method and results in severe low
+  // accuracy. Use the version below (COD) for pseudoinverse
+  //  auto H_mm_decomposition =
+  //      abs_H.bottomRightCorner(marg_size, marg_size)
+  //          .jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+  // Eigen version of pseudoinverse
+  auto H_mm_decomposition = abs_H.bottomRightCorner(marg_size, marg_size)
+                                .completeOrthogonalDecomposition();
+  MatX H_mm_inv = H_mm_decomposition.pseudoInverse();
+
+  // Should be more numerially stable version of:
+  abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv;
+  //  abs_H.topRightCorner(keep_size, marg_size).noalias() =
+  //      H_mm_decomposition.solve(abs_H.bottomLeftCorner(marg_size, keep_size))
+  //          .transpose();
+
+  MatX marg_H;
+  VecX marg_b;
+
+  marg_H = abs_H.topLeftCorner(keep_size, keep_size);
+  marg_b = abs_b.head(keep_size);
+
+  marg_H -= abs_H.topRightCorner(keep_size, marg_size) *
+            abs_H.bottomLeftCorner(marg_size, keep_size);
+  marg_b -= abs_H.topRightCorner(keep_size, marg_size) * abs_b.tail(marg_size);
+
+  Eigen::LDLT<Eigen::Ref<MatX>> ldlt(marg_H);
+
+  VecX D_sqrt = ldlt.vectorD().array().max(0).sqrt().matrix();
+
+  // After LDLT, we have
+  //     marg_H == P^T*L*D*L^T*P,
+  // so we compute the square root as
+  //     marg_sqrt_H = sqrt(D)*L^T*P,
+  // such that
+  //     marg_sqrt_H^T marg_sqrt_H == marg_H.
+  marg_sqrt_H.setIdentity(keep_size, keep_size);
+  marg_sqrt_H = ldlt.transpositionsP() * marg_sqrt_H;
+  marg_sqrt_H = ldlt.matrixU() * marg_sqrt_H;  // U == L^T
+  marg_sqrt_H = D_sqrt.asDiagonal() * marg_sqrt_H;
+
+  // For the right hand side, we want
+  //     marg_b == marg_sqrt_H^T * marg_sqrt_b
+  // so we compute
+  //     marg_sqrt_b = (marg_sqrt_H^T)^-1 * marg_b
+  //                 = (P^T*L*sqrt(D))^-1 * marg_b
+  //                 = sqrt(D)^-1 * L^-1 * P * marg_b
+  marg_sqrt_b = ldlt.transpositionsP() * marg_b;
+  ldlt.matrixL().solveInPlace(marg_sqrt_b);
+
+  // We already clamped negative values in D_sqrt to 0 above, but for values
+  // close to 0 we set b to 0.
+  for (int i = 0; i < marg_sqrt_b.size(); ++i) {
+    if (D_sqrt(i) > std::sqrt(std::numeric_limits<Scalar>::min()))
+      marg_sqrt_b(i) /= D_sqrt(i);
+    else
+      marg_sqrt_b(i) = 0;
+  }
+
+  //  std::cout << "marg_sqrt_H diff: "
+  //            << (marg_sqrt_H.transpose() * marg_sqrt_H - marg_H).norm() << "
+  //            "
+  //            << marg_H.norm() << std::endl;
+
+  //  std::cout << "marg_sqrt_b diff: "
+  //            << (marg_sqrt_H.transpose() * marg_sqrt_b - marg_b).norm()
+  //            << std::endl;
+
+  abs_H.resize(0, 0);
+  abs_b.resize(0);
+}
+
+template <class Scalar_>
+void MargHelper<Scalar_>::marginalizeHelperSqrtToSqrt(
+    MatX& Q2Jp, VecX& Q2r, const std::set<int>& idx_to_keep,
+    const std::set<int>& idx_to_marg, MatX& marg_sqrt_H, VecX& marg_sqrt_b) {
+  Eigen::Index keep_size = idx_to_keep.size();
+  Eigen::Index marg_size = idx_to_marg.size();
+
+  BASALT_ASSERT(keep_size + marg_size == Q2Jp.cols());
+  BASALT_ASSERT(Q2Jp.rows() == Q2r.rows());
+
+  // Fill permutation matrix
+  Eigen::Matrix<int, Eigen::Dynamic, 1> indices(idx_to_keep.size() +
+                                                idx_to_marg.size());
+
+  {
+    auto it = idx_to_marg.begin();
+    for (size_t i = 0; i < idx_to_marg.size(); i++) {
+      indices[i] = *it;
+      it++;
+    }
+  }
+
+  {
+    auto it = idx_to_keep.begin();
+    for (size_t i = 0; i < idx_to_keep.size(); i++) {
+      indices[idx_to_marg.size() + i] = *it;
+      it++;
+    }
+  }
+
+  // TODO: check if using TranspositionMatrix is faster
+  const Eigen::PermutationWrapper<Eigen::Matrix<int, Eigen::Dynamic, 1>> p(
+      indices);
+
+  Q2Jp.applyOnTheRight(p);
+
+  Eigen::Index marg_rank = 0;
+  Eigen::Index total_rank = 0;
+
+  {
+    const Scalar rank_threshold =
+        std::sqrt(std::numeric_limits<Scalar>::epsilon());
+
+    const Eigen::Index rows = Q2Jp.rows();
+    const Eigen::Index cols = Q2Jp.cols();
+
+    VecX tempVector;
+    tempVector.resize(cols + 1);
+    Scalar* tempData = tempVector.data();
+
+    for (Eigen::Index k = 0; k < cols && total_rank < rows; ++k) {
+      Eigen::Index remainingRows = rows - total_rank;
+      Eigen::Index remainingCols = cols - k - 1;
+
+      Scalar beta;
+      Scalar hCoeff;
+      Q2Jp.col(k).tail(remainingRows).makeHouseholderInPlace(hCoeff, beta);
+
+      if (std::abs(beta) > rank_threshold) {
+        Q2Jp.coeffRef(total_rank, k) = beta;
+
+        Q2Jp.bottomRightCorner(remainingRows, remainingCols)
+            .applyHouseholderOnTheLeft(Q2Jp.col(k).tail(remainingRows - 1),
+                                       hCoeff, tempData + k + 1);
+        Q2r.tail(remainingRows)
+            .applyHouseholderOnTheLeft(Q2Jp.col(k).tail(remainingRows - 1),
+                                       hCoeff, tempData + cols);
+        total_rank++;
+      } else {
+        Q2Jp.coeffRef(total_rank, k) = 0;
+      }
+
+      // Overwrite householder vectors with 0
+      Q2Jp.col(k).tail(remainingRows - 1).setZero();
+
+      // Save the rank of marginalize-out part
+      if (k == marg_size - 1) {
+        marg_rank = total_rank;
+      }
+    }
+  }
+
+  Eigen::Index keep_valid_rows =
+      std::max(total_rank - marg_rank, Eigen::Index(1));
+
+  marg_sqrt_H = Q2Jp.block(marg_rank, marg_size, keep_valid_rows, keep_size);
+  marg_sqrt_b = Q2r.segment(marg_rank, keep_valid_rows);
+
+  Q2Jp.resize(0, 0);
+  Q2r.resize(0);
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+// Note: double specialization is unconditional, b/c NfrMapper depends on it.
+//#ifdef BASALT_INSTANTIATIONS_DOUBLE
+template class MargHelper<double>;
+//#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+template class MargHelper<float>;
+#endif
+
+}  // namespace basalt

src/vi_estimator/nfr_mapper.cpp

@@ -0,0 +1,758 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/utils/keypoints.h>
+#include <basalt/utils/nfr.h>
+#include <basalt/utils/tracks.h>
+#include <basalt/vi_estimator/marg_helper.h>
+#include <basalt/vi_estimator/nfr_mapper.h>
+
+#include <basalt/hash_bow/hash_bow.h>
+
+namespace basalt {
+
+NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
+    : config(config),
+      lambda(config.mapper_lm_lambda_min),
+      min_lambda(config.mapper_lm_lambda_min),
+      max_lambda(config.mapper_lm_lambda_max),
+      lambda_vee(2) {
+  this->calib = calib;
+  this->obs_std_dev = config.mapper_obs_std_dev;
+  this->huber_thresh = config.mapper_obs_huber_thresh;
+
+  hash_bow_database.reset(new HashBow<256>(config.mapper_bow_num_bits));
+}
+
+void NfrMapper::addMargData(MargData::Ptr& data) {
+  processMargData(*data);
+  bool valid = extractNonlinearFactors(*data);
+
+  if (valid) {
+    for (const auto& kv : data->frame_poses) {
+      PoseStateWithLin<double> p(kv.second.getT_ns(), kv.second.getPose());
+
+      frame_poses[kv.first] = p;
+    }
+
+    for (const auto& kv : data->frame_states) {
+      if (data->kfs_all.count(kv.first) > 0) {
+        auto state = kv.second;
+        PoseStateWithLin<double> p(state.getState().t_ns,
+                                   state.getState().T_w_i);
+        frame_poses[kv.first] = p;
+      }
+    }
+  }
+}
+
+void NfrMapper::processMargData(MargData& m) {
+  BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
+
+  //    std::cout << "rank " << m.abs_H.fullPivLu().rank() << " size "
+  //              << m.abs_H.cols() << std::endl;
+
+  AbsOrderMap aom_new;
+  std::set<int> idx_to_keep;
+  std::set<int> idx_to_marg;
+
+  for (const auto& kv : m.aom.abs_order_map) {
+    if (kv.second.second == POSE_SIZE) {
+      for (size_t i = 0; i < POSE_SIZE; i++)
+        idx_to_keep.emplace(kv.second.first + i);
+      aom_new.abs_order_map.emplace(kv);
+      aom_new.total_size += POSE_SIZE;
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      if (m.kfs_all.count(kv.first) > 0) {
+        for (size_t i = 0; i < POSE_SIZE; i++)
+          idx_to_keep.emplace(kv.second.first + i);
+        for (size_t i = POSE_SIZE; i < POSE_VEL_BIAS_SIZE; i++)
+          idx_to_marg.emplace(kv.second.first + i);
+
+        aom_new.abs_order_map[kv.first] =
+            std::make_pair(aom_new.total_size, POSE_SIZE);
+        aom_new.total_size += POSE_SIZE;
+
+        PoseStateWithLin<double> p(m.frame_states.at(kv.first));
+        m.frame_poses[kv.first] = p;
+        m.frame_states.erase(kv.first);
+      } else {
+        for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++)
+          idx_to_marg.emplace(kv.second.first + i);
+        m.frame_states.erase(kv.first);
+      }
+    } else {
+      std::cerr << "Unknown size" << std::endl;
+      std::abort();
+    }
+
+    //      std::cout << kv.first << " " << kv.second.first << " " <<
+    //      kv.second.second
+    //                << std::endl;
+  }
+
+  if (!idx_to_marg.empty()) {
+    Eigen::MatrixXd marg_H_new;
+    Eigen::VectorXd marg_b_new;
+    MargHelper<Scalar>::marginalizeHelperSqToSq(
+        m.abs_H, m.abs_b, idx_to_keep, idx_to_marg, marg_H_new, marg_b_new);
+
+    //    std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
+    //              << marg_H_new.cols() << std::endl;
+
+    m.abs_H = marg_H_new;
+    m.abs_b = marg_b_new;
+    m.aom = aom_new;
+  }
+
+  BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
+
+  // save image data
+  {
+    for (const auto& v : m.opt_flow_res) {
+      img_data[v->t_ns] = v->input_images;
+    }
+  }
+}
+
+bool NfrMapper::extractNonlinearFactors(MargData& m) {
+  size_t asize = m.aom.total_size;
+  // std::cout << "asize " << asize << std::endl;
+
+  Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(m.abs_H);
+  if (qr.rank() != m.abs_H.cols()) return false;
+
+  Eigen::MatrixXd cov_old = qr.solve(Eigen::MatrixXd::Identity(asize, asize));
+
+  int64_t kf_id = *m.kfs_to_marg.cbegin();
+  int kf_start_idx = m.aom.abs_order_map.at(kf_id).first;
+
+  auto state_kf = m.frame_poses.at(kf_id);
+
+  Sophus::SE3d T_w_i_kf = state_kf.getPose();
+
+  Eigen::Vector3d pos = T_w_i_kf.translation();
+  Eigen::Vector3d yaw_dir_body =
+      T_w_i_kf.so3().inverse() * Eigen::Vector3d::UnitX();
+
+  Sophus::Matrix<double, 3, POSE_SIZE> d_pos_d_T_w_i;
+  Sophus::Matrix<double, 1, POSE_SIZE> d_yaw_d_T_w_i;
+  Sophus::Matrix<double, 2, POSE_SIZE> d_rp_d_T_w_i;
+
+  absPositionError(T_w_i_kf, pos, &d_pos_d_T_w_i);
+  yawError(T_w_i_kf, yaw_dir_body, &d_yaw_d_T_w_i);
+  rollPitchError(T_w_i_kf, T_w_i_kf.so3(), &d_rp_d_T_w_i);
+
+  {
+    Eigen::MatrixXd J;
+    J.setZero(POSE_SIZE, asize);
+    J.block<3, POSE_SIZE>(0, kf_start_idx) = d_pos_d_T_w_i;
+    J.block<1, POSE_SIZE>(3, kf_start_idx) = d_yaw_d_T_w_i;
+    J.block<2, POSE_SIZE>(4, kf_start_idx) = d_rp_d_T_w_i;
+
+    Sophus::Matrix6d cov_new = J * cov_old * J.transpose();
+
+    // std::cout << "cov_new\n" << cov_new << std::endl;
+
+    RollPitchFactor rpf;
+    rpf.t_ns = kf_id;
+    rpf.R_w_i_meas = T_w_i_kf.so3();
+
+    if (!config.mapper_no_factor_weights) {
+      rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse();
+    } else {
+      rpf.cov_inv.setIdentity();
+    }
+
+    if (m.use_imu) {
+      roll_pitch_factors.emplace_back(rpf);
+    }
+  }
+
+  for (int64_t other_id : m.kfs_all) {
+    if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) {
+      continue;
+    }
+
+    auto state_o = m.frame_poses.at(other_id);
+
+    Sophus::SE3d T_w_i_o = state_o.getPose();
+    Sophus::SE3d T_kf_o = T_w_i_kf.inverse() * T_w_i_o;
+
+    int o_start_idx = m.aom.abs_order_map.at(other_id).first;
+
+    Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j;
+    relPoseError(T_kf_o, T_w_i_kf, T_w_i_o, &d_res_d_T_w_i, &d_res_d_T_w_j);
+
+    Eigen::MatrixXd J;
+    J.setZero(POSE_SIZE, asize);
+    J.block<POSE_SIZE, POSE_SIZE>(0, kf_start_idx) = d_res_d_T_w_i;
+    J.block<POSE_SIZE, POSE_SIZE>(0, o_start_idx) = d_res_d_T_w_j;
+
+    Sophus::Matrix6d cov_new = J * cov_old * J.transpose();
+    RelPoseFactor rpf;
+    rpf.t_i_ns = kf_id;
+    rpf.t_j_ns = other_id;
+    rpf.T_i_j = T_kf_o;
+    rpf.cov_inv.setIdentity();
+
+    if (!config.mapper_no_factor_weights) {
+      cov_new.ldlt().solveInPlace(rpf.cov_inv);
+    }
+
+    // std::cout << "rpf.cov_inv\n" << rpf.cov_inv << std::endl;
+
+    rel_pose_factors.emplace_back(rpf);
+  }
+
+  return true;
+}
+
+void NfrMapper::optimize(int num_iterations) {
+  AbsOrderMap aom;
+
+  for (const auto& kv : frame_poses) {
+    aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE);
+    aom.total_size += POSE_SIZE;
+  }
+
+  for (int iter = 0; iter < num_iterations; iter++) {
+    auto t1 = std::chrono::high_resolution_clock::now();
+
+    double rld_error;
+    Eigen::aligned_vector<RelLinData> rld_vec;
+    linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
+
+    //      SparseHashAccumulator<double> accum;
+    //      accum.reset(aom.total_size);
+
+    //      for (auto& rld : rld_vec) {
+    //        rld.invert_keypoint_hessians();
+
+    //        Eigen::MatrixXd rel_H;
+    //        Eigen::VectorXd rel_b;
+    //        linearizeRel(rld, rel_H, rel_b);
+
+    //        linearizeAbs(rel_H, rel_b, rld, aom, accum);
+    //      }
+
+    MapperLinearizeAbsReduce<SparseHashAccumulator<double>> lopt(aom,
+                                                                 &frame_poses);
+    tbb::blocked_range<Eigen::aligned_vector<RelLinData>::const_iterator> range(
+        rld_vec.begin(), rld_vec.end());
+    tbb::blocked_range<Eigen::aligned_vector<RollPitchFactor>::const_iterator>
+        range1(roll_pitch_factors.begin(), roll_pitch_factors.end());
+    tbb::blocked_range<Eigen::aligned_vector<RelPoseFactor>::const_iterator>
+        range2(rel_pose_factors.begin(), rel_pose_factors.end());
+
+    tbb::parallel_reduce(range, lopt);
+
+    if (config.mapper_use_factors) {
+      tbb::parallel_reduce(range1, lopt);
+      tbb::parallel_reduce(range2, lopt);
+    }
+
+    double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error;
+
+    std::cout << "[LINEARIZE] iter " << iter
+              << " before_update_error: vision: " << rld_error
+              << " rel_error: " << lopt.rel_error
+              << " roll_pitch_error: " << lopt.roll_pitch_error
+              << " total: " << error_total << std::endl;
+
+    lopt.accum.iterative_solver = true;
+    lopt.accum.print_info = true;
+
+    lopt.accum.setup_solver();
+    const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
+    bool converged = false;
+
+    if (config.mapper_use_lm) {  // Use Levenberg–Marquardt
+      bool step = false;
+      int max_iter = 10;
+
+      while (!step && max_iter > 0 && !converged) {
+        Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
+        for (int i = 0; i < Hdiag_lambda.size(); i++)
+          Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
+
+        const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+        double max_inc = inc.array().abs().maxCoeff();
+        if (max_inc < 1e-5) converged = true;
+
+        backup();
+
+        // apply increment to poses
+        for (auto& kv : frame_poses) {
+          int idx = aom.abs_order_map.at(kv.first).first;
+          BASALT_ASSERT(!kv.second.isLinearized());
+          kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
+        }
+
+        // Update points
+        tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+        auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
+          for (size_t i = r.begin(); i != r.end(); ++i) {
+            const auto& rld = rld_vec[i];
+            updatePoints(aom, rld, inc, lmdb);
+          }
+        };
+        tbb::parallel_for(keys_range, update_points_func);
+
+        double after_update_vision_error = 0;
+        double after_rel_error = 0;
+        double after_roll_pitch_error = 0;
+
+        computeError(after_update_vision_error);
+        if (config.mapper_use_factors) {
+          computeRelPose(after_rel_error);
+          computeRollPitch(after_roll_pitch_error);
+        }
+
+        double after_error_total = after_update_vision_error + after_rel_error +
+                                   after_roll_pitch_error;
+
+        double f_diff = (error_total - after_error_total);
+
+        if (f_diff < 0) {
+          std::cout << "\t[REJECTED] lambda:" << lambda << " f_diff: " << f_diff
+                    << " max_inc: " << max_inc
+                    << " vision_error: " << after_update_vision_error
+                    << " rel_error: " << after_rel_error
+                    << " roll_pitch_error: " << after_roll_pitch_error
+                    << " total: " << after_error_total << std::endl;
+          lambda = std::min(max_lambda, lambda_vee * lambda);
+          lambda_vee *= 2;
+
+          restore();
+        } else {
+          std::cout << "\t[ACCEPTED] lambda:" << lambda << " f_diff: " << f_diff
+                    << " max_inc: " << max_inc
+                    << " vision_error: " << after_update_vision_error
+                    << " rel_error: " << after_rel_error
+                    << " roll_pitch_error: " << after_roll_pitch_error
+                    << " total: " << after_error_total << std::endl;
+
+          lambda = std::max(min_lambda, lambda / 3);
+          lambda_vee = 2;
+
+          step = true;
+        }
+
+        max_iter--;
+
+        if (after_error_total > error_total) {
+          std::cout << "increased error after update!!!" << std::endl;
+        }
+      }
+    } else {  // Use Gauss-Newton
+      Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda;
+      for (int i = 0; i < Hdiag_lambda.size(); i++)
+        Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
+
+      const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc.array().abs().maxCoeff();
+      if (max_inc < 1e-5) converged = true;
+
+      // apply increment to poses
+      for (auto& kv : frame_poses) {
+        int idx = aom.abs_order_map.at(kv.first).first;
+        BASALT_ASSERT(!kv.second.isLinearized());
+        kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
+      }
+
+      // Update points
+      tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+      auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
+        for (size_t i = r.begin(); i != r.end(); ++i) {
+          const auto& rld = rld_vec[i];
+          updatePoints(aom, rld, inc, lmdb);
+        }
+      };
+      tbb::parallel_for(keys_range, update_points_func);
+    }
+
+    auto t2 = std::chrono::high_resolution_clock::now();
+    auto elapsed =
+        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+    std::cout << "iter " << iter << " time : " << elapsed.count()
+              << "(us),  num_states " << frame_states.size() << " num_poses "
+              << frame_poses.size() << std::endl;
+
+    if (converged) break;
+
+    // std::cerr << "LT\n" << LT << std::endl;
+    // std::cerr << "z_p\n" << z_p.transpose() << std::endl;
+    // std::cerr << "inc\n" << inc.transpose() << std::endl;
+  }
+}
+
+Eigen::aligned_map<int64_t, PoseStateWithLin<double>>&
+NfrMapper::getFramePoses() {
+  return frame_poses;
+}
+
+void NfrMapper::computeRelPose(double& rel_error) {
+  rel_error = 0;
+
+  for (const RelPoseFactor& rpf : rel_pose_factors) {
+    const Sophus::SE3d& pose_i = frame_poses.at(rpf.t_i_ns).getPose();
+    const Sophus::SE3d& pose_j = frame_poses.at(rpf.t_j_ns).getPose();
+
+    Sophus::Vector6d res = relPoseError(rpf.T_i_j, pose_i, pose_j);
+
+    rel_error += res.transpose() * rpf.cov_inv * res;
+  }
+}
+
+void NfrMapper::computeRollPitch(double& roll_pitch_error) {
+  roll_pitch_error = 0;
+
+  for (const RollPitchFactor& rpf : roll_pitch_factors) {
+    const Sophus::SE3d& pose = frame_poses.at(rpf.t_ns).getPose();
+
+    Sophus::Vector2d res = rollPitchError(pose, rpf.R_w_i_meas);
+
+    roll_pitch_error += res.transpose() * rpf.cov_inv * res;
+  }
+}
+
+void NfrMapper::detect_keypoints() {
+  std::vector<int64_t> keys;
+  for (const auto& kv : img_data) {
+    if (frame_poses.count(kv.first) > 0) {
+      keys.emplace_back(kv.first);
+    }
+  }
+
+  auto t1 = std::chrono::high_resolution_clock::now();
+
+  tbb::parallel_for(
+      tbb::blocked_range<size_t>(0, keys.size()),
+      [&](const tbb::blocked_range<size_t>& r) {
+        for (size_t j = r.begin(); j != r.end(); ++j) {
+          auto kv = img_data.find(keys[j]);
+          if (kv->second.get()) {
+            for (size_t i = 0; i < kv->second->img_data.size(); i++) {
+              TimeCamId tcid(kv->first, i);
+              KeypointsData& kd = feature_corners[tcid];
+
+              if (!kv->second->img_data[i].img.get()) continue;
+
+              const Image<const uint16_t> img =
+                  kv->second->img_data[i].img->Reinterpret<const uint16_t>();
+
+              detectKeypointsMapping(img, kd,
+                                     config.mapper_detection_num_points);
+              computeAngles(img, kd, true);
+              computeDescriptors(img, kd);
+
+              std::vector<bool> success;
+              calib.intrinsics[tcid.cam_id].unproject(kd.corners, kd.corners_3d,
+                                                      success);
+
+              hash_bow_database->compute_bow(kd.corner_descriptors, kd.hashes,
+                                             kd.bow_vector);
+
+              hash_bow_database->add_to_database(tcid, kd.bow_vector);
+
+              // std::cout << "bow " << kd.bow_vector.size() << " desc "
+              //          << kd.corner_descriptors.size() << std::endl;
+            }
+          }
+        }
+      });
+
+  auto t2 = std::chrono::high_resolution_clock::now();
+
+  auto elapsed1 =
+      std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+  std::cout << "Processed " << feature_corners.size() << " frames."
+            << std::endl;
+
+  std::cout << "Detection time: " << elapsed1.count() * 1e-6 << "s."
+            << std::endl;
+}
+
+void NfrMapper::match_stereo() {
+  // Pose of camera 1 (right) w.r.t camera 0 (left)
+  const Sophus::SE3d T_0_1 = calib.T_i_c[0].inverse() * calib.T_i_c[1];
+
+  // Essential matrix
+  Eigen::Matrix4d E;
+  computeEssential(T_0_1, E);
+
+  std::cout << "Matching " << img_data.size() << " stereo pairs..."
+            << std::endl;
+
+  int num_matches = 0;
+  int num_inliers = 0;
+
+  for (const auto& kv : img_data) {
+    const TimeCamId tcid1(kv.first, 0), tcid2(kv.first, 1);
+
+    MatchData md;
+    md.T_i_j = T_0_1;
+
+    const KeypointsData& kd1 = feature_corners[tcid1];
+    const KeypointsData& kd2 = feature_corners[tcid2];
+
+    matchDescriptors(kd1.corner_descriptors, kd2.corner_descriptors, md.matches,
+                     config.mapper_max_hamming_distance,
+                     config.mapper_second_best_test_ratio);
+
+    num_matches += md.matches.size();
+
+    findInliersEssential(kd1, kd2, E, 1e-3, md);
+
+    if (md.inliers.size() > 16) {
+      num_inliers += md.inliers.size();
+      feature_matches[std::make_pair(tcid1, tcid2)] = md;
+    }
+  }
+
+  std::cout << "Matched " << img_data.size() << " stereo pairs with "
+            << num_inliers << " inlier matches (" << num_matches << " total)."
+            << std::endl;
+}
+
+void NfrMapper::match_all() {
+  std::vector<TimeCamId> keys;
+  std::unordered_map<TimeCamId, size_t> id_to_key_idx;
+
+  for (const auto& kv : feature_corners) {
+    id_to_key_idx[kv.first] = keys.size();
+    keys.push_back(kv.first);
+  }
+
+  auto t1 = std::chrono::high_resolution_clock::now();
+
+  struct match_pair {
+    size_t i;
+    size_t j;
+    double score;
+  };
+
+  tbb::concurrent_vector<match_pair> ids_to_match;
+
+  tbb::blocked_range<size_t> keys_range(0, keys.size());
+  auto compute_pairs = [&](const tbb::blocked_range<size_t>& r) {
+    for (size_t i = r.begin(); i != r.end(); ++i) {
+      const TimeCamId& tcid = keys[i];
+      const KeypointsData& kd = feature_corners.at(tcid);
+
+      std::vector<std::pair<TimeCamId, double>> results;
+
+      hash_bow_database->querry_database(kd.bow_vector,
+                                         config.mapper_num_frames_to_match,
+                                         results, &tcid.frame_id);
+
+      // std::cout << "Closest frames for " << tcid << ": ";
+      for (const auto& otcid_score : results) {
+        // std::cout << otcid_score.first << "(" << otcid_score.second << ")
+        // ";
+        if (otcid_score.first.frame_id != tcid.frame_id &&
+            otcid_score.second > config.mapper_frames_to_match_threshold) {
+          match_pair m;
+          m.i = i;
+          m.j = id_to_key_idx.at(otcid_score.first);
+          m.score = otcid_score.second;
+
+          ids_to_match.emplace_back(m);
+        }
+      }
+      // std::cout << std::endl;
+    }
+  };
+
+  tbb::parallel_for(keys_range, compute_pairs);
+  // compute_pairs(keys_range);
+
+  auto t2 = std::chrono::high_resolution_clock::now();
+
+  std::cout << "Matching " << ids_to_match.size() << " image pairs..."
+            << std::endl;
+
+  std::atomic<int> total_matched = 0;
+
+  tbb::blocked_range<size_t> range(0, ids_to_match.size());
+  auto match_func = [&](const tbb::blocked_range<size_t>& r) {
+    int matched = 0;
+
+    for (size_t j = r.begin(); j != r.end(); ++j) {
+      const TimeCamId& id1 = keys[ids_to_match[j].i];
+      const TimeCamId& id2 = keys[ids_to_match[j].j];
+
+      const KeypointsData& f1 = feature_corners[id1];
+      const KeypointsData& f2 = feature_corners[id2];
+
+      MatchData md;
+
+      matchDescriptors(f1.corner_descriptors, f2.corner_descriptors, md.matches,
+                       70, 1.2);
+
+      if (int(md.matches.size()) > config.mapper_min_matches) {
+        matched++;
+
+        findInliersRansac(f1, f2, config.mapper_ransac_threshold,
+                          config.mapper_min_matches, md);
+      }
+
+      if (!md.inliers.empty()) feature_matches[std::make_pair(id1, id2)] = md;
+    }
+    total_matched += matched;
+  };
+
+  tbb::parallel_for(range, match_func);
+  // match_func(range);
+
+  auto t3 = std::chrono::high_resolution_clock::now();
+
+  auto elapsed1 =
+      std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+  auto elapsed2 =
+      std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
+
+  //
+  int num_matches = 0;
+  int num_inliers = 0;
+
+  for (const auto& kv : feature_matches) {
+    num_matches += kv.second.matches.size();
+    num_inliers += kv.second.inliers.size();
+  }
+
+  std::cout << "Matched " << ids_to_match.size() << " image pairs with "
+            << num_inliers << " inlier matches (" << num_matches << " total)."
+            << std::endl;
+
+  std::cout << "DB query " << elapsed1.count() * 1e-6 << "s. matching "
+            << elapsed2.count() * 1e-6
+            << "s. Geometric verification attemts: " << total_matched << "."
+            << std::endl;
+}
+
+void NfrMapper::build_tracks() {
+  TrackBuilder trackBuilder;
+  // Build: Efficient fusion of correspondences
+  trackBuilder.Build(feature_matches);
+  // Filter: Remove tracks that have conflict
+  trackBuilder.Filter(config.mapper_min_track_length);
+  // Export tree to usable data structure
+  trackBuilder.Export(feature_tracks);
+
+  // info
+  size_t inlier_match_count = 0;
+  for (const auto& it : feature_matches) {
+    inlier_match_count += it.second.inliers.size();
+  }
+
+  size_t total_track_obs_count = 0;
+  for (const auto& it : feature_tracks) {
+    total_track_obs_count += it.second.size();
+  }
+
+  std::cout << "Built " << feature_tracks.size() << " feature tracks from "
+            << inlier_match_count << " matches. Average track length is "
+            << total_track_obs_count / (double)feature_tracks.size() << "."
+            << std::endl;
+}
+
+void NfrMapper::setup_opt() {
+  const double min_triang_distance2 = config.mapper_min_triangulation_dist *
+                                      config.mapper_min_triangulation_dist;
+
+  for (const auto& kv : feature_tracks) {
+    if (kv.second.size() < 2) continue;
+
+    // Take first observation as host
+    auto it = kv.second.begin();
+    TimeCamId tcid_h = it->first;
+
+    FeatureId feat_id_h = it->second;
+    Eigen::Vector2d pos_2d_h = feature_corners.at(tcid_h).corners[feat_id_h];
+    Eigen::Vector4d pos_3d_h;
+    calib.intrinsics[tcid_h.cam_id].unproject(pos_2d_h, pos_3d_h);
+
+    it++;
+
+    for (; it != kv.second.end(); it++) {
+      TimeCamId tcid_o = it->first;
+
+      FeatureId feat_id_o = it->second;
+      Eigen::Vector2d pos_2d_o = feature_corners.at(tcid_o).corners[feat_id_o];
+      Eigen::Vector4d pos_3d_o;
+      calib.intrinsics[tcid_o.cam_id].unproject(pos_2d_o, pos_3d_o);
+
+      Sophus::SE3d T_w_h = frame_poses.at(tcid_h.frame_id).getPose() *
+                           calib.T_i_c[tcid_h.cam_id];
+      Sophus::SE3d T_w_o = frame_poses.at(tcid_o.frame_id).getPose() *
+                           calib.T_i_c[tcid_o.cam_id];
+
+      Sophus::SE3d T_h_o = T_w_h.inverse() * T_w_o;
+
+      if (T_h_o.translation().squaredNorm() < min_triang_distance2) continue;
+
+      Eigen::Vector4d pos_3d =
+          triangulate(pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_h_o);
+
+      if (!pos_3d.array().isFinite().all() || pos_3d[3] <= 0 || pos_3d[3] > 2.0)
+        continue;
+
+      Keypoint<Scalar> pos;
+      pos.host_kf_id = tcid_h;
+      pos.direction = StereographicParam<double>::project(pos_3d);
+      pos.inv_dist = pos_3d[3];
+
+      lmdb.addLandmark(kv.first, pos);
+
+      for (const auto& obs_kv : kv.second) {
+        KeypointObservation<Scalar> ko;
+        ko.kpt_id = kv.first;
+        ko.pos = feature_corners.at(obs_kv.first).corners[obs_kv.second];
+
+        lmdb.addObservation(obs_kv.first, ko);
+        // obs[tcid_h][obs_kv.first].emplace_back(ko);
+      }
+      break;
+    }
+  }
+}
+
+}  // namespace basalt

src/vi_estimator/sc_ba_base.cpp

@@ -0,0 +1,808 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/vi_estimator/sc_ba_base.h>
+
+#include <tbb/parallel_for.h>
+
+#include <basalt/imu/preintegration.h>
+#include <basalt/utils/ba_utils.h>
+
+namespace basalt {
+
+template <class Scalar_>
+void ScBundleAdjustmentBase<Scalar_>::updatePoints(
+    const AbsOrderMap& aom, const RelLinData& rld, const VecX& inc,
+    LandmarkDatabase<Scalar>& lmdb, Scalar* l_diff) {
+  // We want to compute the model cost change. The model fuction is
+  //
+  //    L(inc) = 0.5 r^T r + inc^T b + 0.5 inc^T H inc,
+  //
+  // and thus the expected decrease in cost for the computed increment is
+  //
+  //    l_diff = L(0) - L(inc)
+  //           = - inc^T b - 0.5 inc^T H inc.
+  //
+  // Here we have
+  //
+  //          | incp |       | bp |       | Hpp  Hpl |
+  //    inc = |      |   b = |    |   H = |          |
+  //          | incl |,      | bl |,      | Hlp  Hll |,
+  //
+  // and thus
+  //
+  //    l_diff = - incp^T bp - incl^T bl -
+  //               0.5 incp^T Hpp incp - incp^T Hpl incl - 0.5 incl^T Hll incl
+  //           = - sum_{p} (incp^T (bp + 0.5 Hpp incp)) -
+  //               sum_{l} (incl^T (bl + 0.5 Hll incl +
+  //                                sum_{obs} (Hpl^T incp))),
+  //
+  // where sum_{p} sums over all (relative) poses, sum_{l} over all landmarks,
+  // and sum_{obs} over all observations of each landmark.
+  //
+  // Note: l_diff acts like an accumulator; we just add w/o initializing to 0.
+
+  VecX rel_inc;
+  rel_inc.setZero(rld.order.size() * POSE_SIZE);
+  for (size_t i = 0; i < rld.order.size(); i++) {
+    const TimeCamId& tcid_h = rld.order[i].first;
+    const TimeCamId& tcid_t = rld.order[i].second;
+
+    if (tcid_h.frame_id != tcid_t.frame_id) {
+      int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
+      int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
+
+      Eigen::Matrix<Scalar, POSE_SIZE, 1> inc_p =
+          rld.d_rel_d_h[i] * inc.template segment<POSE_SIZE>(abs_h_idx) +
+          rld.d_rel_d_t[i] * inc.template segment<POSE_SIZE>(abs_t_idx);
+
+      rel_inc.template segment<POSE_SIZE>(i * POSE_SIZE) = inc_p;
+
+      if (l_diff) {
+        // Note: inc_p is still negated b/c we solve H(-x) = b
+        const FrameRelLinData& frld = rld.Hpppl[i];
+        *l_diff -= -inc_p.dot((frld.bp - Scalar(0.5) * (frld.Hpp * inc_p)));
+      }
+    }
+  }
+
+  for (const auto& kv : rld.lm_to_obs) {
+    int lm_idx = kv.first;
+    const auto& lm_obs = kv.second;
+
+    Vec3 H_l_p_x;
+    H_l_p_x.setZero();
+
+    for (size_t k = 0; k < lm_obs.size(); k++) {
+      int rel_idx = lm_obs[k].first;
+      const FrameRelLinData& frld = rld.Hpppl.at(rel_idx);
+      const Eigen::Matrix<Scalar, POSE_SIZE, 3>& H_p_l_other =
+          frld.Hpl.at(lm_obs[k].second);
+
+      // Note: pose increment is still negated b/c we solve "H (-inc) = b"
+      H_l_p_x += H_p_l_other.transpose() *
+                 rel_inc.template segment<POSE_SIZE>(rel_idx * POSE_SIZE);
+    }
+
+    // final negation of inc_l b/c we solve "H (-inc) = b"
+    Vec3 inc_l = -(rld.Hllinv.at(lm_idx) * (rld.bl.at(lm_idx) - H_l_p_x));
+
+    Keypoint<Scalar>& kpt = lmdb.getLandmark(lm_idx);
+    kpt.direction += inc_l.template head<2>();
+    kpt.inv_dist += inc_l[2];
+
+    kpt.inv_dist = std::max(Scalar(0), kpt.inv_dist);
+
+    if (l_diff) {
+      // Note: rel_inc and thus H_l_p_x are still negated b/c we solve H(-x) = b
+      *l_diff -= inc_l.transpose() *
+                 (rld.bl.at(lm_idx) +
+                  Scalar(0.5) * (rld.Hll.at(lm_idx) * inc_l) - H_l_p_x);
+    }
+  }
+}
+
+template <class Scalar_>
+void ScBundleAdjustmentBase<Scalar_>::updatePointsAbs(
+    const AbsOrderMap& aom, const AbsLinData& ald, const VecX& inc,
+    LandmarkDatabase<Scalar>& lmdb, Scalar* l_diff) {
+  // We want to compute the model cost change. The model fuction is
+  //
+  //    L(inc) = 0.5 r^T r + inc^T b + 0.5 inc^T H inc,
+  //
+  // and thus the expected decrease in cost for the computed increment is
+  //
+  //    l_diff = L(0) - L(inc)
+  //           = - inc^T b - 0.5 inc^T H inc.
+  //
+  // Here we have
+  //
+  //          | incp |       | bp |       | Hpp  Hpl |
+  //    inc = |      |   b = |    |   H = |          |
+  //          | incl |,      | bl |,      | Hlp  Hll |,
+  //
+  // and thus
+  //
+  //    l_diff = - incp^T bp - incl^T bl -
+  //               0.5 incp^T Hpp incp - incp^T Hpl incl - 0.5 incl^T Hll incl
+  //           = - sum_{p} (incp^T (bp + 0.5 Hpp incp)) -
+  //               sum_{l} (incl^T (bl + 0.5 Hll incl +
+  //                                sum_{obs} (Hpl^T incp))),
+  //
+  // where sum_{p} sums over all (relative) poses, sum_{l} over all landmarks,
+  // and sum_{obs} over all observations of each landmark.
+  //
+  // Note: l_diff acts like an accumulator; we just add w/o initializing to 0.
+
+  if (l_diff) {
+    for (size_t i = 0; i < ald.order.size(); i++) {
+      const TimeCamId& tcid_h = ald.order[i].first;
+      const TimeCamId& tcid_t = ald.order[i].second;
+
+      int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
+      int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
+
+      auto inc_h = inc.template segment<POSE_SIZE>(abs_h_idx);
+      auto inc_t = inc.template segment<POSE_SIZE>(abs_t_idx);
+
+      // Note: inc_p is still negated b/c we solve H(-x) = b
+      const FrameAbsLinData& fald = ald.Hpppl[i];
+      *l_diff -= -inc_h.dot((fald.bph - Scalar(0.5) * (fald.Hphph * inc_h)) -
+                            fald.Hphpt * inc_t) -
+                 inc_t.dot((fald.bpt - Scalar(0.5) * (fald.Hptpt * inc_t)));
+    }
+  }
+
+  for (const auto& kv : ald.lm_to_obs) {
+    int lm_idx = kv.first;
+    const auto& lm_obs = kv.second;
+
+    Vec3 H_l_p_x;
+    H_l_p_x.setZero();
+
+    for (size_t k = 0; k < lm_obs.size(); k++) {
+      int rel_idx = lm_obs[k].first;
+      const TimeCamId& tcid_h = ald.order.at(rel_idx).first;
+      const TimeCamId& tcid_t = ald.order.at(rel_idx).second;
+
+      int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
+      int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
+
+      auto inc_h = inc.template segment<POSE_SIZE>(abs_h_idx);
+      auto inc_t = inc.template segment<POSE_SIZE>(abs_t_idx);
+
+      const FrameAbsLinData& fald = ald.Hpppl.at(rel_idx);
+      const Eigen::Matrix<Scalar, POSE_SIZE, 3>& H_ph_l_other =
+          fald.Hphl.at(lm_obs[k].second);
+      const Eigen::Matrix<Scalar, POSE_SIZE, 3>& H_pt_l_other =
+          fald.Hptl.at(lm_obs[k].second);
+
+      // Note: pose increment is still negated b/c we solve "H (-inc) = b"
+      H_l_p_x += H_ph_l_other.transpose() * inc_h;
+      H_l_p_x += H_pt_l_other.transpose() * inc_t;
+    }
+
+    // final negation of inc_l b/c we solve "H (-inc) = b"
+    Vec3 inc_l = -(ald.Hllinv.at(lm_idx) * (ald.bl.at(lm_idx) - H_l_p_x));
+
+    Keypoint<Scalar>& kpt = lmdb.getLandmark(lm_idx);
+    kpt.direction += inc_l.template head<2>();
+    kpt.inv_dist += inc_l[2];
+
+    kpt.inv_dist = std::max(Scalar(0), kpt.inv_dist);
+
+    if (l_diff) {
+      // Note: rel_inc and thus H_l_p_x are still negated b/c we solve H(-x) = b
+      *l_diff -= inc_l.transpose() *
+                 (ald.bl.at(lm_idx) +
+                  Scalar(0.5) * (ald.Hll.at(lm_idx) * inc_l) - H_l_p_x);
+    }
+  }
+}
+
+template <class Scalar_>
+void ScBundleAdjustmentBase<Scalar_>::linearizeHelperStatic(
+    Eigen::aligned_vector<RelLinData>& rld_vec,
+    const std::unordered_map<
+        TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+    const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error) {
+  error = 0;
+
+  rld_vec.clear();
+
+  std::vector<TimeCamId> obs_tcid_vec;
+  for (const auto& kv : obs_to_lin) {
+    obs_tcid_vec.emplace_back(kv.first);
+    rld_vec.emplace_back(ba_base->lmdb.numLandmarks(), kv.second.size());
+  }
+
+  tbb::parallel_for(
+      tbb::blocked_range<size_t>(0, obs_tcid_vec.size()),
+      [&](const tbb::blocked_range<size_t>& range) {
+        for (size_t r = range.begin(); r != range.end(); ++r) {
+          auto kv = obs_to_lin.find(obs_tcid_vec[r]);
+
+          RelLinData& rld = rld_vec[r];
+
+          rld.error = Scalar(0);
+
+          const TimeCamId& tcid_h = kv->first;
+
+          for (const auto& obs_kv : kv->second) {
+            const TimeCamId& tcid_t = obs_kv.first;
+            rld.order.emplace_back(std::make_pair(tcid_h, tcid_t));
+
+            Mat4 T_t_h;
+
+            if (tcid_h != tcid_t) {
+              // target and host are not the same
+              PoseStateWithLin state_h =
+                  ba_base->getPoseStateWithLin(tcid_h.frame_id);
+              PoseStateWithLin state_t =
+                  ba_base->getPoseStateWithLin(tcid_t.frame_id);
+
+              Mat6 d_rel_d_h, d_rel_d_t;
+
+              SE3 T_t_h_sophus = computeRelPose(
+                  state_h.getPoseLin(), ba_base->calib.T_i_c[tcid_h.cam_id],
+                  state_t.getPoseLin(), ba_base->calib.T_i_c[tcid_t.cam_id],
+                  &d_rel_d_h, &d_rel_d_t);
+
+              rld.d_rel_d_h.emplace_back(d_rel_d_h);
+              rld.d_rel_d_t.emplace_back(d_rel_d_t);
+
+              if (state_h.isLinearized() || state_t.isLinearized()) {
+                T_t_h_sophus = computeRelPose(
+                    state_h.getPose(), ba_base->calib.T_i_c[tcid_h.cam_id],
+                    state_t.getPose(), ba_base->calib.T_i_c[tcid_t.cam_id]);
+              }
+
+              T_t_h = T_t_h_sophus.matrix();
+            } else {
+              T_t_h.setIdentity();
+              rld.d_rel_d_h.emplace_back(Mat6::Zero());
+              rld.d_rel_d_t.emplace_back(Mat6::Zero());
+            }
+
+            FrameRelLinData frld;
+
+            std::visit(
+                [&](const auto& cam) {
+                  for (KeypointId kpt_id : obs_kv.second) {
+                    const Keypoint<Scalar>& kpt_pos =
+                        ba_base->lmdb.getLandmark(kpt_id);
+                    const Vec2& kpt_obs = kpt_pos.obs.at(tcid_t);
+
+                    Vec2 res;
+                    Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
+                    Eigen::Matrix<Scalar, 2, 3> d_res_d_p;
+
+                    bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam,
+                                                res, &d_res_d_xi, &d_res_d_p);
+
+                    if (valid) {
+                      Scalar e = res.norm();
+                      Scalar huber_weight = e < ba_base->huber_thresh
+                                                ? Scalar(1.0)
+                                                : ba_base->huber_thresh / e;
+                      Scalar obs_weight = huber_weight / (ba_base->obs_std_dev *
+                                                          ba_base->obs_std_dev);
+
+                      rld.error += Scalar(0.5) * (2 - huber_weight) *
+                                   obs_weight * res.transpose() * res;
+
+                      if (rld.Hll.count(kpt_id) == 0) {
+                        rld.Hll[kpt_id].setZero();
+                        rld.bl[kpt_id].setZero();
+                      }
+
+                      rld.Hll[kpt_id] +=
+                          obs_weight * d_res_d_p.transpose() * d_res_d_p;
+                      rld.bl[kpt_id] +=
+                          obs_weight * d_res_d_p.transpose() * res;
+
+                      frld.Hpp +=
+                          obs_weight * d_res_d_xi.transpose() * d_res_d_xi;
+                      frld.bp += obs_weight * d_res_d_xi.transpose() * res;
+                      frld.Hpl.emplace_back(obs_weight *
+                                            d_res_d_xi.transpose() * d_res_d_p);
+                      frld.lm_id.emplace_back(kpt_id);
+
+                      rld.lm_to_obs[kpt_id].emplace_back(rld.Hpppl.size(),
+                                                         frld.lm_id.size() - 1);
+                    }
+                  }
+                },
+                ba_base->calib.intrinsics[tcid_t.cam_id].variant);
+
+            rld.Hpppl.emplace_back(frld);
+          }
+
+          rld.invert_keypoint_hessians();
+        }
+      });
+
+  for (const auto& rld : rld_vec) error += rld.error;
+}
+
+template <class Scalar_>
+void ScBundleAdjustmentBase<Scalar_>::linearizeHelperAbsStatic(
+    Eigen::aligned_vector<AbsLinData>& ald_vec,
+    const std::unordered_map<
+        TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
+    const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error) {
+  error = 0;
+
+  ald_vec.clear();
+
+  std::vector<TimeCamId> obs_tcid_vec;
+  for (const auto& kv : obs_to_lin) {
+    obs_tcid_vec.emplace_back(kv.first);
+    ald_vec.emplace_back(ba_base->lmdb.numLandmarks(), kv.second.size());
+  }
+
+  tbb::parallel_for(
+      tbb::blocked_range<size_t>(0, obs_tcid_vec.size()),
+      [&](const tbb::blocked_range<size_t>& range) {
+        for (size_t r = range.begin(); r != range.end(); ++r) {
+          auto kv = obs_to_lin.find(obs_tcid_vec[r]);
+
+          AbsLinData& ald = ald_vec[r];
+
+          ald.error = Scalar(0);
+
+          const TimeCamId& tcid_h = kv->first;
+
+          for (const auto& obs_kv : kv->second) {
+            const TimeCamId& tcid_t = obs_kv.first;
+            ald.order.emplace_back(std::make_pair(tcid_h, tcid_t));
+
+            Mat4 T_t_h;
+            Mat6 d_rel_d_h, d_rel_d_t;
+
+            if (tcid_h != tcid_t) {
+              // target and host are not the same
+              PoseStateWithLin state_h =
+                  ba_base->getPoseStateWithLin(tcid_h.frame_id);
+              PoseStateWithLin state_t =
+                  ba_base->getPoseStateWithLin(tcid_t.frame_id);
+
+              SE3 T_t_h_sophus = computeRelPose(
+                  state_h.getPoseLin(), ba_base->calib.T_i_c[tcid_h.cam_id],
+                  state_t.getPoseLin(), ba_base->calib.T_i_c[tcid_t.cam_id],
+                  &d_rel_d_h, &d_rel_d_t);
+
+              if (state_h.isLinearized() || state_t.isLinearized()) {
+                T_t_h_sophus = computeRelPose(
+                    state_h.getPose(), ba_base->calib.T_i_c[tcid_h.cam_id],
+                    state_t.getPose(), ba_base->calib.T_i_c[tcid_t.cam_id]);
+              }
+
+              T_t_h = T_t_h_sophus.matrix();
+            } else {
+              T_t_h.setIdentity();
+              d_rel_d_h.setZero();
+              d_rel_d_t.setZero();
+            }
+
+            FrameAbsLinData fald;
+
+            std::visit(
+                [&](const auto& cam) {
+                  for (KeypointId kpt_id : obs_kv.second) {
+                    const Keypoint<Scalar>& kpt_pos =
+                        ba_base->lmdb.getLandmark(kpt_id);
+                    const Vec2& kpt_obs = kpt_pos.obs.at(tcid_t);
+
+                    Vec2 res;
+                    Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi_h,
+                        d_res_d_xi_t;
+                    Eigen::Matrix<Scalar, 2, 3> d_res_d_p;
+
+                    bool valid;
+                    {
+                      Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
+
+                      valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res,
+                                             &d_res_d_xi, &d_res_d_p);
+
+                      d_res_d_xi_h = d_res_d_xi * d_rel_d_h;
+                      d_res_d_xi_t = d_res_d_xi * d_rel_d_t;
+                    }
+
+                    if (valid) {
+                      Scalar e = res.norm();
+                      Scalar huber_weight = e < ba_base->huber_thresh
+                                                ? Scalar(1.0)
+                                                : ba_base->huber_thresh / e;
+                      Scalar obs_weight = huber_weight / (ba_base->obs_std_dev *
+                                                          ba_base->obs_std_dev);
+
+                      ald.error += Scalar(0.5) * (2 - huber_weight) *
+                                   obs_weight * res.transpose() * res;
+
+                      if (ald.Hll.count(kpt_id) == 0) {
+                        ald.Hll[kpt_id].setZero();
+                        ald.bl[kpt_id].setZero();
+                      }
+
+                      ald.Hll[kpt_id] +=
+                          obs_weight * d_res_d_p.transpose() * d_res_d_p;
+                      ald.bl[kpt_id] +=
+                          obs_weight * d_res_d_p.transpose() * res;
+
+                      fald.Hphph +=
+                          obs_weight * d_res_d_xi_h.transpose() * d_res_d_xi_h;
+                      fald.Hptpt +=
+                          obs_weight * d_res_d_xi_t.transpose() * d_res_d_xi_t;
+                      fald.Hphpt +=
+                          obs_weight * d_res_d_xi_h.transpose() * d_res_d_xi_t;
+
+                      fald.bph += obs_weight * d_res_d_xi_h.transpose() * res;
+                      fald.bpt += obs_weight * d_res_d_xi_t.transpose() * res;
+
+                      fald.Hphl.emplace_back(
+                          obs_weight * d_res_d_xi_h.transpose() * d_res_d_p);
+                      fald.Hptl.emplace_back(
+                          obs_weight * d_res_d_xi_t.transpose() * d_res_d_p);
+
+                      fald.lm_id.emplace_back(kpt_id);
+
+                      ald.lm_to_obs[kpt_id].emplace_back(ald.Hpppl.size(),
+                                                         fald.lm_id.size() - 1);
+                    }
+                  }
+                },
+                ba_base->calib.intrinsics[tcid_t.cam_id].variant);
+
+            ald.Hpppl.emplace_back(fald);
+          }
+
+          ald.invert_keypoint_hessians();
+        }
+      });
+
+  for (const auto& rld : ald_vec) error += rld.error;
+}
+
+template <class Scalar_>
+void ScBundleAdjustmentBase<Scalar_>::linearizeRel(const RelLinData& rld,
+                                                   MatX& H, VecX& b) {
+  //  std::cout << "linearizeRel: KF " << frame_states.size() << " obs "
+  //            << obs.size() << std::endl;
+
+  // Do schur complement
+  size_t msize = rld.order.size();
+  H.setZero(POSE_SIZE * msize, POSE_SIZE * msize);
+  b.setZero(POSE_SIZE * msize);
+
+  for (size_t i = 0; i < rld.order.size(); i++) {
+    const FrameRelLinData& frld = rld.Hpppl.at(i);
+
+    H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i, POSE_SIZE * i) +=
+        frld.Hpp;
+    b.template segment<POSE_SIZE>(POSE_SIZE * i) += frld.bp;
+
+    for (size_t j = 0; j < frld.lm_id.size(); j++) {
+      Eigen::Matrix<Scalar, POSE_SIZE, 3> H_pl_H_ll_inv;
+      int lm_id = frld.lm_id[j];
+
+      H_pl_H_ll_inv = frld.Hpl[j] * rld.Hllinv.at(lm_id);
+      b.template segment<POSE_SIZE>(POSE_SIZE * i) -=
+          H_pl_H_ll_inv * rld.bl.at(lm_id);
+
+      const auto& other_obs = rld.lm_to_obs.at(lm_id);
+      for (size_t k = 0; k < other_obs.size(); k++) {
+        const FrameRelLinData& frld_other = rld.Hpppl.at(other_obs[k].first);
+        int other_i = other_obs[k].first;
+
+        Eigen::Matrix<Scalar, 3, POSE_SIZE> H_l_p_other =
+            frld_other.Hpl[other_obs[k].second].transpose();
+
+        H.template block<POSE_SIZE, POSE_SIZE>(
+            POSE_SIZE * i, POSE_SIZE * other_i) -= H_pl_H_ll_inv * H_l_p_other;
+      }
+    }
+  }
+}
+
+template <class Scalar_>
+Eigen::VectorXd ScBundleAdjustmentBase<Scalar_>::checkNullspace(
+    const MatX& H, const VecX& b, const AbsOrderMap& order,
+    const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
+        frame_states,
+    const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
+    bool verbose) {
+  using Vec3d = Eigen::Vector3d;
+  using VecXd = Eigen::VectorXd;
+  using Mat3d = Eigen::Matrix3d;
+  using MatXd = Eigen::MatrixXd;
+  using SO3d = Sophus::SO3d;
+
+  BASALT_ASSERT(size_t(H.cols()) == order.total_size);
+  size_t marg_size = order.total_size;
+
+  // Idea: We construct increments that we know should lie in the null-space of
+  // the prior from marginalized observations (except for the initial pose prior
+  // we set at initialization) --> shift global translations (x,y,z separately),
+  // or global rotations (r,p,y separately); for VIO only yaw rotation shift is
+  // in nullspace. Compared to a random increment, we expect them to stay small
+  // (at initial level of the initial pose prior). If they increase over time,
+  // we accumulate spurious information on unobservable degrees of freedom.
+  //
+  // Poses are cam-to-world and we use left-increment in optimization, so
+  // perturbations are in world frame. Hence translational increments are
+  // independent. For rotational increments we also need to rotate translations
+  // and velocities, both of which are expressed in world frame. For
+  // translations, we move the center of rotation to the camera center centroid
+  // for better numerics.
+
+  VecXd inc_x, inc_y, inc_z, inc_roll, inc_pitch, inc_yaw;
+  inc_x.setZero(marg_size);
+  inc_y.setZero(marg_size);
+  inc_z.setZero(marg_size);
+  inc_roll.setZero(marg_size);
+  inc_pitch.setZero(marg_size);
+  inc_yaw.setZero(marg_size);
+
+  int num_trans = 0;
+  Vec3d mean_trans;
+  mean_trans.setZero();
+
+  // Compute mean translation
+  for (const auto& kv : order.abs_order_map) {
+    Vec3d trans;
+    if (kv.second.second == POSE_SIZE) {
+      mean_trans += frame_poses.at(kv.first)
+                        .getPoseLin()
+                        .translation()
+                        .template cast<double>();
+      num_trans++;
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      mean_trans += frame_states.at(kv.first)
+                        .getStateLin()
+                        .T_w_i.translation()
+                        .template cast<double>();
+      num_trans++;
+    } else {
+      std::cerr << "Unknown size of the state: " << kv.second.second
+                << std::endl;
+      std::abort();
+    }
+  }
+  mean_trans /= num_trans;
+
+  double eps = 0.01;
+
+  // Compute nullspace increments
+  for (const auto& kv : order.abs_order_map) {
+    inc_x(kv.second.first + 0) = eps;
+    inc_y(kv.second.first + 1) = eps;
+    inc_z(kv.second.first + 2) = eps;
+    inc_roll(kv.second.first + 3) = eps;
+    inc_pitch(kv.second.first + 4) = eps;
+    inc_yaw(kv.second.first + 5) = eps;
+
+    Vec3d trans;
+    if (kv.second.second == POSE_SIZE) {
+      trans = frame_poses.at(kv.first)
+                  .getPoseLin()
+                  .translation()
+                  .template cast<double>();
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      trans = frame_states.at(kv.first)
+                  .getStateLin()
+                  .T_w_i.translation()
+                  .template cast<double>();
+    } else {
+      BASALT_ASSERT(false);
+    }
+
+    // combine rotation with global translation to make it rotation around
+    // translation centroid, not around origin (better numerics). Note that
+    // velocities are not affected by global translation.
+    trans -= mean_trans;
+
+    // Jacobian of translation w.r.t. the rotation increment (one column each
+    // for the 3 different increments)
+    Mat3d J = -SO3d::hat(trans);
+    J *= eps;
+
+    inc_roll.template segment<3>(kv.second.first) = J.col(0);
+    inc_pitch.template segment<3>(kv.second.first) = J.col(1);
+    inc_yaw.template segment<3>(kv.second.first) = J.col(2);
+
+    if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      Vec3d vel = frame_states.at(kv.first)
+                      .getStateLin()
+                      .vel_w_i.template cast<double>();
+
+      // Jacobian of velocity w.r.t. the rotation increment (one column each
+      // for the 3 different increments)
+      Mat3d J_vel = -SO3d::hat(vel);
+      J_vel *= eps;
+
+      inc_roll.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(0);
+      inc_pitch.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(1);
+      inc_yaw.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(2);
+    }
+  }
+
+  inc_x.normalize();
+  inc_y.normalize();
+  inc_z.normalize();
+  inc_roll.normalize();
+  inc_pitch.normalize();
+  inc_yaw.normalize();
+
+  //  std::cout << "inc_x   " << inc_x.transpose() << std::endl;
+  //  std::cout << "inc_y   " << inc_y.transpose() << std::endl;
+  //  std::cout << "inc_z   " << inc_z.transpose() << std::endl;
+  //  std::cout << "inc_yaw " << inc_yaw.transpose() << std::endl;
+
+  VecXd inc_random;
+  inc_random.setRandom(marg_size);
+  inc_random.normalize();
+
+  MatXd H_d = H.template cast<double>();
+  VecXd b_d = b.template cast<double>();
+
+  VecXd xHx(7);
+  VecXd xb(7);
+
+  xHx[0] = inc_x.transpose() * H_d * inc_x;
+  xHx[1] = inc_y.transpose() * H_d * inc_y;
+  xHx[2] = inc_z.transpose() * H_d * inc_z;
+  xHx[3] = inc_roll.transpose() * H_d * inc_roll;
+  xHx[4] = inc_pitch.transpose() * H_d * inc_pitch;
+  xHx[5] = inc_yaw.transpose() * H_d * inc_yaw;
+  xHx[6] = inc_random.transpose() * H_d * inc_random;
+
+  // b == J^T r, so the increments should also lie in left-nullspace of b
+  xb[0] = inc_x.transpose() * b_d;
+  xb[1] = inc_y.transpose() * b_d;
+  xb[2] = inc_z.transpose() * b_d;
+  xb[3] = inc_roll.transpose() * b_d;
+  xb[4] = inc_pitch.transpose() * b_d;
+  xb[5] = inc_yaw.transpose() * b_d;
+  xb[6] = inc_random.transpose() * b_d;
+
+  if (verbose) {
+    std::cout << "nullspace x_trans: " << xHx[0] << " + " << xb[0] << std::endl;
+    std::cout << "nullspace y_trans: " << xHx[1] << " + " << xb[1] << std::endl;
+    std::cout << "nullspace z_trans: " << xHx[2] << " + " << xb[2] << std::endl;
+    std::cout << "nullspace roll   : " << xHx[3] << " + " << xb[3] << std::endl;
+    std::cout << "nullspace pitch  : " << xHx[4] << " + " << xb[4] << std::endl;
+    std::cout << "nullspace yaw    : " << xHx[5] << " + " << xb[5] << std::endl;
+    std::cout << "nullspace random : " << xHx[6] << " + " << xb[6] << std::endl;
+  }
+
+  return xHx + xb;
+}
+
+template <class Scalar_>
+Eigen::VectorXd ScBundleAdjustmentBase<Scalar_>::checkEigenvalues(
+    const MatX& H, bool verbose) {
+  // For EV, we use SelfAdjointEigenSolver to avoid getting (numerically)
+  // complex eigenvalues.
+
+  // We do this computation in double precision to avoid any inaccuracies that
+  // come from the squaring.
+
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(
+      H.template cast<double>());
+  if (eigensolver.info() != Eigen::Success) {
+    BASALT_LOG_FATAL("eigen solver failed");
+  }
+
+  if (verbose) {
+    std::cout << "EV:\n" << eigensolver.eigenvalues() << std::endl;
+  }
+
+  return eigensolver.eigenvalues();
+}
+
+template <class Scalar_>
+void ScBundleAdjustmentBase<Scalar_>::computeImuError(
+    const AbsOrderMap& aom, Scalar& imu_error, Scalar& bg_error,
+    Scalar& ba_error,
+    const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>& states,
+    const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<Scalar>>&
+        imu_meas,
+    const Vec3& gyro_bias_weight, const Vec3& accel_bias_weight,
+    const Vec3& g) {
+  imu_error = 0;
+  bg_error = 0;
+  ba_error = 0;
+  for (const auto& kv : imu_meas) {
+    if (kv.second.get_dt_ns() != 0) {
+      int64_t start_t = kv.second.get_start_t_ns();
+      int64_t end_t = kv.second.get_start_t_ns() + kv.second.get_dt_ns();
+
+      if (aom.abs_order_map.count(start_t) == 0 ||
+          aom.abs_order_map.count(end_t) == 0)
+        continue;
+
+      PoseVelBiasStateWithLin<Scalar> start_state = states.at(start_t);
+      PoseVelBiasStateWithLin<Scalar> end_state = states.at(end_t);
+
+      const typename PoseVelState<Scalar>::VecN res = kv.second.residual(
+          start_state.getState(), g, end_state.getState(),
+          start_state.getState().bias_gyro, start_state.getState().bias_accel);
+
+      //      std::cout << "res: (" << start_t << "," << end_t << ") "
+      //                << res.transpose() << std::endl;
+
+      //      std::cerr << "cov_inv:\n" << kv.second.get_cov_inv() <<
+      //      std::endl;
+
+      imu_error +=
+          Scalar(0.5) * res.transpose() * kv.second.get_cov_inv() * res;
+
+      Scalar dt = kv.second.get_dt_ns() * Scalar(1e-9);
+      {
+        Vec3 gyro_bias_weight_dt = gyro_bias_weight / dt;
+        Vec3 res_bg =
+            start_state.getState().bias_gyro - end_state.getState().bias_gyro;
+
+        bg_error += Scalar(0.5) * res_bg.transpose() *
+                    gyro_bias_weight_dt.asDiagonal() * res_bg;
+      }
+
+      {
+        Vec3 accel_bias_weight_dt = accel_bias_weight / dt;
+        Vec3 res_ba =
+            start_state.getState().bias_accel - end_state.getState().bias_accel;
+
+        ba_error += Scalar(0.5) * res_ba.transpose() *
+                    accel_bias_weight_dt.asDiagonal() * res_ba;
+      }
+    }
+  }
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+// Note: double specialization is unconditional, b/c NfrMapper depends on it.
+//#ifdef BASALT_INSTANTIATIONS_DOUBLE
+template class ScBundleAdjustmentBase<double>;
+//#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+template class ScBundleAdjustmentBase<float>;
+#endif
+
+}  // namespace basalt

src/vi_estimator/sqrt_ba_base.cpp

@@ -0,0 +1,270 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/vi_estimator/sqrt_ba_base.h>
+
+#include <tbb/parallel_for.h>
+
+namespace basalt {
+
+template <class Scalar_>
+Eigen::VectorXd SqrtBundleAdjustmentBase<Scalar_>::checkNullspace(
+    const MargLinData<Scalar_>& mld,
+    const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
+        frame_states,
+    const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
+    bool verbose) {
+  using Vec3d = Eigen::Vector3d;
+  using VecXd = Eigen::VectorXd;
+  using Mat3d = Eigen::Matrix3d;
+  using MatXd = Eigen::MatrixXd;
+  using SO3d = Sophus::SO3d;
+
+  BASALT_ASSERT_STREAM(size_t(mld.H.cols()) == mld.order.total_size,
+                       mld.H.cols() << " " << mld.order.total_size);
+  size_t marg_size = mld.order.total_size;
+
+  // Idea: We construct increments that we know should lie in the null-space of
+  // the prior from marginalized observations (except for the initial pose prior
+  // we set at initialization) --> shift global translations (x,y,z separately),
+  // or global rotations (r,p,y separately); for VIO only yaw rotation shift is
+  // in nullspace. Compared to a random increment, we expect them to stay small
+  // (at initial level of the initial pose prior). If they increase over time,
+  // we accumulate spurious information on unobservable degrees of freedom.
+  //
+  // Poses are cam-to-world and we use left-increment in optimization, so
+  // perturbations are in world frame. Hence translational increments are
+  // independent. For rotational increments we also need to rotate translations
+  // and velocities, both of which are expressed in world frame. For
+  // translations, we move the center of rotation to the camera center centroid
+  // for better numerics.
+
+  VecXd inc_x, inc_y, inc_z, inc_roll, inc_pitch, inc_yaw;
+  inc_x.setZero(marg_size);
+  inc_y.setZero(marg_size);
+  inc_z.setZero(marg_size);
+  inc_roll.setZero(marg_size);
+  inc_pitch.setZero(marg_size);
+  inc_yaw.setZero(marg_size);
+
+  int num_trans = 0;
+  Vec3d mean_trans;
+  mean_trans.setZero();
+
+  // Compute mean translation
+  for (const auto& kv : mld.order.abs_order_map) {
+    Vec3d trans;
+    if (kv.second.second == POSE_SIZE) {
+      mean_trans += frame_poses.at(kv.first)
+                        .getPoseLin()
+                        .translation()
+                        .template cast<double>();
+      num_trans++;
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      mean_trans += frame_states.at(kv.first)
+                        .getStateLin()
+                        .T_w_i.translation()
+                        .template cast<double>();
+      num_trans++;
+    } else {
+      std::cerr << "Unknown size of the state: " << kv.second.second
+                << std::endl;
+      std::abort();
+    }
+  }
+  mean_trans /= num_trans;
+
+  double eps = 0.01;
+
+  // Compute nullspace increments
+  for (const auto& kv : mld.order.abs_order_map) {
+    inc_x(kv.second.first + 0) = eps;
+    inc_y(kv.second.first + 1) = eps;
+    inc_z(kv.second.first + 2) = eps;
+    inc_roll(kv.second.first + 3) = eps;
+    inc_pitch(kv.second.first + 4) = eps;
+    inc_yaw(kv.second.first + 5) = eps;
+
+    Vec3d trans;
+    if (kv.second.second == POSE_SIZE) {
+      trans = frame_poses.at(kv.first)
+                  .getPoseLin()
+                  .translation()
+                  .template cast<double>();
+    } else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      trans = frame_states.at(kv.first)
+                  .getStateLin()
+                  .T_w_i.translation()
+                  .template cast<double>();
+    } else {
+      BASALT_ASSERT(false);
+    }
+
+    // combine rotation with global translation to make it rotation around
+    // translation centroid, not around origin (better numerics). Note that
+    // velocities are not affected by global translation.
+    trans -= mean_trans;
+
+    // Jacobian of translation w.r.t. the rotation increment (one column each
+    // for the 3 different increments)
+    Mat3d J = -SO3d::hat(trans);
+    J *= eps;
+
+    inc_roll.template segment<3>(kv.second.first) = J.col(0);
+    inc_pitch.template segment<3>(kv.second.first) = J.col(1);
+    inc_yaw.template segment<3>(kv.second.first) = J.col(2);
+
+    if (kv.second.second == POSE_VEL_BIAS_SIZE) {
+      Vec3d vel = frame_states.at(kv.first)
+                      .getStateLin()
+                      .vel_w_i.template cast<double>();
+
+      // Jacobian of velocity w.r.t. the rotation increment (one column each
+      // for the 3 different increments)
+      Mat3d J_vel = -SO3d::hat(vel);
+      J_vel *= eps;
+
+      inc_roll.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(0);
+      inc_pitch.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(1);
+      inc_yaw.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(2);
+    }
+  }
+
+  inc_x.normalize();
+  inc_y.normalize();
+  inc_z.normalize();
+  inc_roll.normalize();
+  inc_pitch.normalize();
+  inc_yaw.normalize();
+
+  //  std::cout << "inc_x   " << inc_x.transpose() << std::endl;
+  //  std::cout << "inc_y   " << inc_y.transpose() << std::endl;
+  //  std::cout << "inc_z   " << inc_z.transpose() << std::endl;
+  //  std::cout << "inc_yaw " << inc_yaw.transpose() << std::endl;
+
+  VecXd inc_random;
+  inc_random.setRandom(marg_size);
+  inc_random.normalize();
+
+  MatXd H_d;
+  VecXd b_d;
+
+  if (mld.is_sqrt) {
+    MatXd H_sqrt_d = mld.H.template cast<double>();
+    VecXd b_sqrt_d = mld.b.template cast<double>();
+
+    H_d = H_sqrt_d.transpose() * H_sqrt_d;
+    b_d = H_sqrt_d.transpose() * b_sqrt_d;
+
+  } else {
+    H_d = mld.H.template cast<double>();
+    b_d = mld.b.template cast<double>();
+  }
+
+  VecXd xHx(7);
+  VecXd xb(7);
+
+  xHx[0] = inc_x.transpose() * H_d * inc_x;
+  xHx[1] = inc_y.transpose() * H_d * inc_y;
+  xHx[2] = inc_z.transpose() * H_d * inc_z;
+  xHx[3] = inc_roll.transpose() * H_d * inc_roll;
+  xHx[4] = inc_pitch.transpose() * H_d * inc_pitch;
+  xHx[5] = inc_yaw.transpose() * H_d * inc_yaw;
+  xHx[6] = inc_random.transpose() * H_d * inc_random;
+
+  // b == J^T r, so the increments should also lie in left-nullspace of b
+  xb[0] = inc_x.transpose() * b_d;
+  xb[1] = inc_y.transpose() * b_d;
+  xb[2] = inc_z.transpose() * b_d;
+  xb[3] = inc_roll.transpose() * b_d;
+  xb[4] = inc_pitch.transpose() * b_d;
+  xb[5] = inc_yaw.transpose() * b_d;
+  xb[6] = inc_random.transpose() * b_d;
+
+  if (verbose) {
+    std::cout << "nullspace x_trans: " << xHx[0] << " + " << xb[0] << std::endl;
+    std::cout << "nullspace y_trans: " << xHx[1] << " + " << xb[1] << std::endl;
+    std::cout << "nullspace z_trans: " << xHx[2] << " + " << xb[2] << std::endl;
+    std::cout << "nullspace roll   : " << xHx[3] << " + " << xb[3] << std::endl;
+    std::cout << "nullspace pitch  : " << xHx[4] << " + " << xb[4] << std::endl;
+    std::cout << "nullspace yaw    : " << xHx[5] << " + " << xb[5] << std::endl;
+    std::cout << "nullspace random : " << xHx[6] << " + " << xb[6] << std::endl;
+  }
+
+  return xHx + xb;
+}
+
+template <class Scalar_>
+Eigen::VectorXd SqrtBundleAdjustmentBase<Scalar_>::checkEigenvalues(
+    const MargLinData<Scalar_>& mld, bool verbose) {
+  // Check EV of J^T J explicitly instead of doing SVD on J to easily notice if
+  // we have negative EVs (numerically). We do this computation in double
+  // precision to avoid any inaccuracies that come from the squaring.
+
+  // For EV, we use SelfAdjointEigenSolver to avoid getting (numerically)
+  // complex eigenvalues.
+  Eigen::MatrixXd H;
+
+  if (mld.is_sqrt) {
+    Eigen::MatrixXd sqrt_H_double = mld.H.template cast<double>();
+    H = sqrt_H_double.transpose() * sqrt_H_double;
+  } else {
+    H = mld.H.template cast<double>();
+  }
+
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(H);
+  if (eigensolver.info() != Eigen::Success) {
+    BASALT_LOG_FATAL("eigen solver failed");
+  }
+
+  if (verbose) {
+    std::cout << "EV:\n" << eigensolver.eigenvalues() << std::endl;
+  }
+
+  return eigensolver.eigenvalues();
+}
+
+// //////////////////////////////////////////////////////////////////
+// instatiate templates
+
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+template class SqrtBundleAdjustmentBase<double>;
+#endif
+
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+template class SqrtBundleAdjustmentBase<float>;
+#endif
+
+}  // namespace basalt

src/vi_estimator/vio_estimator.cpp

@@ -0,0 +1,176 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/vi_estimator/vio_estimator.h>
+
+#include <basalt/vi_estimator/sqrt_keypoint_vio.h>
+#include <basalt/vi_estimator/sqrt_keypoint_vo.h>
+
+namespace basalt {
+
+namespace {
+
+template <class Scalar>
+VioEstimatorBase::Ptr factory_helper(const VioConfig& config,
+                                     const Calibration<double>& cam,
+                                     const Eigen::Vector3d& g, bool use_imu) {
+  VioEstimatorBase::Ptr res;
+
+  if (use_imu) {
+    res.reset(new SqrtKeypointVioEstimator<Scalar>(g, cam, config));
+
+  } else {
+    res.reset(new SqrtKeypointVoEstimator<Scalar>(cam, config));
+  }
+
+  return res;
+}
+
+}  // namespace
+
+VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator(
+    const VioConfig& config, const Calibration<double>& cam,
+    const Eigen::Vector3d& g, bool use_imu, bool use_double) {
+  if (use_double) {
+#ifdef BASALT_INSTANTIATIONS_DOUBLE
+    return factory_helper<double>(config, cam, g, use_imu);
+#else
+    BASALT_LOG_FATAL("Compiled without double support.");
+#endif
+  } else {
+#ifdef BASALT_INSTANTIATIONS_FLOAT
+    return factory_helper<float>(config, cam, g, use_imu);
+#else
+    BASALT_LOG_FATAL("Compiled without float support.");
+#endif
+  }
+}
+
+double alignSVD(const std::vector<int64_t>& filter_t_ns,
+                const Eigen::aligned_vector<Eigen::Vector3d>& filter_t_w_i,
+                const std::vector<int64_t>& gt_t_ns,
+                Eigen::aligned_vector<Eigen::Vector3d>& gt_t_w_i) {
+  Eigen::aligned_vector<Eigen::Vector3d> est_associations;
+  Eigen::aligned_vector<Eigen::Vector3d> gt_associations;
+
+  for (size_t i = 0; i < filter_t_w_i.size(); i++) {
+    int64_t t_ns = filter_t_ns[i];
+
+    size_t j;
+    for (j = 0; j < gt_t_ns.size(); j++) {
+      if (gt_t_ns.at(j) > t_ns) break;
+    }
+    j--;
+
+    if (j >= gt_t_ns.size() - 1) {
+      continue;
+    }
+
+    double dt_ns = t_ns - gt_t_ns.at(j);
+    double int_t_ns = gt_t_ns.at(j + 1) - gt_t_ns.at(j);
+
+    BASALT_ASSERT_STREAM(dt_ns >= 0, "dt_ns " << dt_ns);
+    BASALT_ASSERT_STREAM(int_t_ns > 0, "int_t_ns " << int_t_ns);
+
+    // Skip if the interval between gt larger than 100ms
+    if (int_t_ns > 1.1e8) continue;
+
+    double ratio = dt_ns / int_t_ns;
+
+    BASALT_ASSERT(ratio >= 0);
+    BASALT_ASSERT(ratio < 1);
+
+    Eigen::Vector3d gt = (1 - ratio) * gt_t_w_i[j] + ratio * gt_t_w_i[j + 1];
+
+    gt_associations.emplace_back(gt);
+    est_associations.emplace_back(filter_t_w_i[i]);
+  }
+
+  int num_kfs = est_associations.size();
+
+  Eigen::Matrix<double, 3, Eigen::Dynamic> gt, est;
+  gt.setZero(3, num_kfs);
+  est.setZero(3, num_kfs);
+
+  for (size_t i = 0; i < est_associations.size(); i++) {
+    gt.col(i) = gt_associations[i];
+    est.col(i) = est_associations[i];
+  }
+
+  Eigen::Vector3d mean_gt = gt.rowwise().mean();
+  Eigen::Vector3d mean_est = est.rowwise().mean();
+
+  gt.colwise() -= mean_gt;
+  est.colwise() -= mean_est;
+
+  Eigen::Matrix3d cov = gt * est.transpose();
+
+  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
+      cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Eigen::Matrix3d S;
+  S.setIdentity();
+
+  if (svd.matrixU().determinant() * svd.matrixV().determinant() < 0)
+    S(2, 2) = -1;
+
+  Eigen::Matrix3d rot_gt_est = svd.matrixU() * S * svd.matrixV().transpose();
+  Eigen::Vector3d trans = mean_gt - rot_gt_est * mean_est;
+
+  Sophus::SE3d T_gt_est(rot_gt_est, trans);
+  Sophus::SE3d T_est_gt = T_gt_est.inverse();
+
+  for (size_t i = 0; i < gt_t_w_i.size(); i++) {
+    gt_t_w_i[i] = T_est_gt * gt_t_w_i[i];
+  }
+
+  double error = 0;
+  for (size_t i = 0; i < est_associations.size(); i++) {
+    est_associations[i] = T_gt_est * est_associations[i];
+    Eigen::Vector3d res = est_associations[i] - gt_associations[i];
+
+    error += res.transpose() * res;
+  }
+
+  error /= est_associations.size();
+  error = std::sqrt(error);
+
+  std::cout << "T_align\n" << T_gt_est.matrix() << std::endl;
+  std::cout << "error " << error << std::endl;
+  std::cout << "number of associations " << num_kfs << std::endl;
+
+  return error;
+}
+}  // namespace basalt

src/vio_sim.cpp

@@ -0,0 +1,911 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <sophus/se3.hpp>
+
+#include <tbb/concurrent_unordered_map.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/image/image.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/pangolin.h>
+
+#include <CLI/CLI.hpp>
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/io/marg_data_io.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/utils/sim_utils.h>
+#include <basalt/utils/vis_utils.h>
+#include <basalt/vi_estimator/vio_estimator.h>
+
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/serialization/headers_serialization.h>
+
+#include <basalt/utils/vis_utils.h>
+
+// GUI functions
+void draw_image_overlay(pangolin::View& v, size_t cam_id);
+void draw_scene();
+void load_data(const std::string& calib_path);
+void gen_data();
+void compute_projections();
+void setup_vio(const std::string& config_path);
+void draw_plots();
+bool next_step();
+void alignButton();
+
+// Parameters for simulations
+int NUM_POINTS = 1000;
+double POINT_DIST = 10.0;
+constexpr int NUM_FRAMES = 1000;
+constexpr int CAM_FREQ = 20;
+constexpr int IMU_FREQ = 200;
+
+static const int knot_time = 3;
+static const double obs_std_dev = 0.5;
+
+Eigen::Vector3d g(0, 0, -9.81);
+
+// std::random_device rd{};
+// std::mt19937 gen{rd()};
+std::mt19937 gen{1};
+std::normal_distribution<> obs_noise_dist{0, obs_std_dev};
+
+// Simulated data
+
+basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9));
+
+Eigen::aligned_vector<Eigen::Vector3d> gt_points;
+Eigen::aligned_vector<Sophus::SE3d> gt_frame_T_w_i;
+Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
+std::vector<int64_t> gt_frame_t_ns;
+Eigen::aligned_vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias,
+    gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel;
+std::vector<int64_t> gt_imu_t_ns;
+
+std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
+std::map<basalt::TimeCamId, basalt::SimObservations> noisy_observations;
+
+std::string marg_data_path;
+
+// VIO vars
+basalt::Calibration<double> calib;
+basalt::VioEstimatorBase::Ptr vio;
+
+// Visualization vars
+std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
+tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
+tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
+    out_state_queue;
+
+std::vector<pangolin::TypedImage> images;
+
+// Pangolin vars
+constexpr int UI_WIDTH = 200;
+pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
+pangolin::Plotter* plotter;
+
+pangolin::Var<int> show_frame("ui.show_frame", 0, 0, NUM_FRAMES - 1);
+
+pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
+pangolin::Var<bool> show_obs_noisy("ui.show_obs_noisy", true, false, true);
+pangolin::Var<bool> show_obs_vio("ui.show_obs_vio", true, false, true);
+
+pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);
+
+pangolin::Var<bool> show_accel("ui.show_accel", false, false, true);
+pangolin::Var<bool> show_gyro("ui.show_gyro", false, false, true);
+pangolin::Var<bool> show_gt_vel("ui.show_gt_vel", false, false, true);
+pangolin::Var<bool> show_gt_pos("ui.show_gt_pos", true, false, true);
+pangolin::Var<bool> show_gt_bg("ui.show_gt_bg", false, false, true);
+pangolin::Var<bool> show_gt_ba("ui.show_gt_ba", false, false, true);
+
+pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
+pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
+pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
+pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);
+
+using Button = pangolin::Var<std::function<void(void)>>;
+
+Button next_step_btn("ui.next_step", &next_step);
+
+pangolin::Var<bool> continue_btn("ui.continue", true, false, true);
+
+Button align_step_btn("ui.align_se3", &alignButton);
+
+bool use_imu = true;
+bool use_double = true;
+
+int main(int argc, char** argv) {
+  srand(1);
+
+  bool show_gui = true;
+  std::string cam_calib_path;
+  std::string result_path;
+  std::string config_path;
+
+  CLI::App app{"App description"};
+
+  app.add_option("--show-gui", show_gui, "Show GUI");
+  app.add_option("--cam-calib", cam_calib_path,
+                 "Ground-truth camera calibration used for simulation.")
+      ->required();
+
+  app.add_option("--marg-data", marg_data_path,
+                 "Folder to store marginalization data.")
+      ->required();
+
+  app.add_option("--result-path", result_path,
+                 "Path to result file where the system will write RMSE ATE.");
+
+  app.add_option("--config-path", config_path, "Path to config file.");
+
+  app.add_option("--num-points", NUM_POINTS, "Number of points in simulation.");
+  app.add_option("--use-imu", use_imu, "Use IMU.");
+
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  basalt::MargDataSaver::Ptr mds;
+  if (!marg_data_path.empty()) {
+    mds.reset(new basalt::MargDataSaver(marg_data_path));
+  }
+
+  load_data(cam_calib_path);
+
+  gen_data();
+
+  setup_vio(config_path);
+
+  vio->out_vis_queue = &out_vis_queue;
+  vio->out_state_queue = &out_state_queue;
+
+  if (mds.get()) {
+    vio->out_marg_queue = &mds->in_marg_queue;
+  }
+
+  std::thread t0([&]() {
+    for (size_t i = 0; i < gt_imu_t_ns.size(); i++) {
+      basalt::ImuData<double>::Ptr data(new basalt::ImuData<double>);
+      data->t_ns = gt_imu_t_ns[i];
+
+      data->accel = noisy_accel[i];
+      data->gyro = noisy_gyro[i];
+
+      vio->imu_data_queue.push(data);
+    }
+
+    vio->imu_data_queue.push(nullptr);
+
+    std::cout << "Finished t0" << std::endl;
+  });
+
+  std::thread t1([&]() {
+    for (size_t i = 0; i < gt_frame_t_ns.size(); i++) {
+      basalt::OpticalFlowResult::Ptr data(new basalt::OpticalFlowResult);
+      data->t_ns = gt_frame_t_ns[i];
+
+      for (size_t j = 0; j < calib.T_i_c.size(); j++) {
+        data->observations.emplace_back();
+        basalt::TimeCamId tcid(data->t_ns, j);
+        const basalt::SimObservations& obs = noisy_observations.at(tcid);
+        for (size_t k = 0; k < obs.pos.size(); k++) {
+          Eigen::AffineCompact2f t;
+          t.setIdentity();
+          t.translation() = obs.pos[k].cast<float>();
+          data->observations.back()[obs.id[k]] = t;
+        }
+      }
+
+      vio->vision_data_queue.push(data);
+    }
+
+    vio->vision_data_queue.push(nullptr);
+
+    std::cout << "Finished t1" << std::endl;
+  });
+
+  std::thread t2([&]() {
+    basalt::VioVisualizationData::Ptr data;
+
+    while (true) {
+      out_vis_queue.pop(data);
+
+      if (data.get()) {
+        vis_map[data->t_ns] = data;
+      } else {
+        break;
+      }
+    }
+
+    std::cout << "Finished t2" << std::endl;
+  });
+
+  std::thread t3([&]() {
+    basalt::PoseVelBiasState<double>::Ptr data;
+
+    while (true) {
+      out_state_queue.pop(data);
+
+      if (!data.get()) break;
+
+      int64_t t_ns = data->t_ns;
+
+      // std::cerr << "t_ns " << t_ns << std::endl;
+      Sophus::SE3d T_w_i = data->T_w_i;
+      Eigen::Vector3d vel_w_i = data->vel_w_i;
+      Eigen::Vector3d bg = data->bias_gyro;
+      Eigen::Vector3d ba = data->bias_accel;
+
+      vio_t_w_i.emplace_back(T_w_i.translation());
+
+      {
+        std::vector<float> vals;
+        vals.push_back(t_ns * 1e-9);
+
+        for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(bg[i]);
+        for (int i = 0; i < 3; i++) vals.push_back(ba[i]);
+
+        vio_data_log.Log(vals);
+      }
+    }
+
+    std::cout << "Finished t3" << std::endl;
+  });
+
+  //  std::thread t4([&]() {
+
+  //    basalt::MargData::Ptr data;
+
+  //    while (true) {
+  //      out_marg_queue.pop(data);
+
+  //      if (data.get()) {
+  //        int64_t kf_id = *data->kfs_to_marg.begin();
+
+  //        std::string path = cache_path + "/" + std::to_string(kf_id) +
+  //        ".cereal";
+  //        std::ofstream os(path, std::ios::binary);
+
+  //        {
+  //          cereal::BinaryOutputArchive archive(os);
+  //          archive(*data);
+  //        }
+  //        os.close();
+
+  //      } else {
+  //        break;
+  //      }
+  //    }
+
+  //    std::cout << "Finished t4" << std::endl;
+
+  //  });
+
+  if (show_gui) {
+    pangolin::CreateWindowAndBind("Main", 1800, 1000);
+
+    glEnable(GL_DEPTH_TEST);
+
+    pangolin::View& img_view_display =
+        pangolin::CreateDisplay()
+            .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.5)
+            .SetLayout(pangolin::LayoutEqual);
+
+    pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
+        0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0);
+
+    plotter = new pangolin::Plotter(&imu_data_log, 0.0,
+                                    gt_frame_t_ns[NUM_FRAMES - 1] * 1e-9, -10.0,
+                                    10.0, 0.01f, 0.01f);
+    plot_display.AddDisplay(*plotter);
+
+    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
+                                          pangolin::Attach::Pix(UI_WIDTH));
+
+    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
+    while (img_view.size() < calib.intrinsics.size()) {
+      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
+
+      size_t idx = img_view.size();
+      img_view.push_back(iv);
+
+      img_view_display.AddDisplay(*iv);
+      iv->extern_draw_function =
+          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
+    }
+
+    pangolin::OpenGlRenderState camera(
+        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
+        pangolin::ModelViewLookAt(15, 3, 15, 0, 0, 0, pangolin::AxisZ));
+
+    pangolin::View& display3D =
+        pangolin::CreateDisplay()
+            .SetAspect(-640 / 480.0)
+            .SetBounds(0.4, 1.0, 0.5, 1.0)
+            .SetHandler(new pangolin::Handler3D(camera));
+
+    while (!pangolin::ShouldQuit()) {
+      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+      display3D.Activate(camera);
+      glClearColor(0.95f, 0.95f, 0.95f, 1.0f);
+
+      draw_scene();
+
+      img_view_display.Activate();
+
+      if (show_frame.GuiChanged()) {
+        for (size_t i = 0; i < calib.intrinsics.size(); i++) {
+          img_view[i]->SetImage(images[i]);
+        }
+        draw_plots();
+      }
+
+      if (show_accel.GuiChanged() || show_gyro.GuiChanged() ||
+          show_gt_vel.GuiChanged() || show_gt_pos.GuiChanged() ||
+          show_gt_ba.GuiChanged() || show_gt_bg.GuiChanged() ||
+          show_est_vel.GuiChanged() || show_est_pos.GuiChanged() ||
+          show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) {
+        draw_plots();
+      }
+
+      pangolin::FinishFrame();
+
+      if (continue_btn) {
+        if (!next_step()) continue_btn = false;
+      } else {
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      }
+    }
+  }
+
+  t0.join();
+  t1.join();
+  t2.join();
+  t3.join();
+  // t4.join();
+
+  if (!result_path.empty()) {
+    double error = basalt::alignSVD(gt_frame_t_ns, vio_t_w_i, gt_frame_t_ns,
+                                    gt_frame_t_w_i);
+
+    std::ofstream os(result_path);
+    os << error << std::endl;
+    os.close();
+  }
+
+  return 0;
+}
+
+void draw_image_overlay(pangolin::View& v, size_t cam_id) {
+  UNUSED(v);
+
+  size_t frame_id = show_frame;
+  basalt::TimeCamId tcid(gt_frame_t_ns[frame_id], cam_id);
+
+  if (show_obs) {
+    glLineWidth(1.0);
+    glColor3ubv(gt_color);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (gt_observations.find(tcid) != gt_observations.end()) {
+      const basalt::SimObservations& cr = gt_observations.at(tcid);
+
+      for (size_t i = 0; i < cr.pos.size(); i++) {
+        const float radius = 2;
+        const Eigen::Vector2f c = cr.pos[i].cast<float>();
+        pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+        if (show_ids)
+          pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]);
+      }
+
+      pangolin::GlFont::I().Text("%d gt points", cr.pos.size()).Draw(5, 20);
+    }
+  }
+
+  if (show_obs_noisy) {
+    glLineWidth(1.0);
+    glColor3f(1.0, 1.0, 0.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    if (noisy_observations.find(tcid) != noisy_observations.end()) {
+      const basalt::SimObservations& cr = noisy_observations.at(tcid);
+
+      for (size_t i = 0; i < cr.pos.size(); i++) {
+        const float radius = 2;
+        const Eigen::Vector2f c = cr.pos[i].cast<float>();
+        pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+        if (show_ids)
+          pangolin::GlFont::I().Text("%d", cr.id[i]).Draw(c[0], c[1]);
+      }
+
+      pangolin::GlFont::I().Text("%d noisy points", cr.pos.size()).Draw(5, 40);
+    }
+  }
+
+  if (show_obs_vio) {
+    glLineWidth(1.0);
+    glColor3f(0.0, 0.0, 1.0);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    auto it = vis_map.find(gt_frame_t_ns[frame_id]);
+
+    if (it != vis_map.end() && cam_id < it->second->projections.size()) {
+      const auto& points = it->second->projections[cam_id];
+
+      if (points.size() > 0) {
+        double min_id = points[0][2], max_id = points[0][2];
+        for (size_t i = 0; i < points.size(); i++) {
+          min_id = std::min(min_id, points[i][2]);
+          max_id = std::max(max_id, points[i][2]);
+        }
+
+        for (size_t i = 0; i < points.size(); i++) {
+          const float radius = 2;
+          const Eigen::Vector4d c = points[i];
+          pangolin::glDrawCirclePerimeter(c[0], c[1], radius);
+
+          if (show_ids)
+            pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]);
+        }
+      }
+
+      glColor3f(0.0, 0.0, 1.0);
+      pangolin::GlFont::I().Text("%d vio points", points.size()).Draw(5, 60);
+    }
+  }
+}
+
+void draw_scene() {
+  glPointSize(3);
+  glColor3f(1.0, 0.0, 0.0);
+  glEnable(GL_BLEND);
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+  glColor3ubv(gt_color);
+  pangolin::glDrawPoints(gt_points);
+  pangolin::glDrawLineStrip(gt_frame_t_w_i);
+
+  glColor3ubv(cam_color);
+  pangolin::glDrawLineStrip(vio_t_w_i);
+
+  size_t frame_id = show_frame;
+
+  auto it = vis_map.find(gt_frame_t_ns[frame_id]);
+
+  if (it != vis_map.end()) {
+    for (const auto& p : it->second->states)
+      for (size_t i = 0; i < calib.T_i_c.size(); i++)
+        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, cam_color, 0.1f);
+
+    for (const auto& p : it->second->frames)
+      for (size_t i = 0; i < calib.T_i_c.size(); i++)
+        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);
+
+    glColor3ubv(pose_color);
+    pangolin::glDrawPoints(it->second->points);
+  }
+
+  pangolin::glDrawAxis(gt_frame_T_w_i[frame_id].matrix(), 0.1);
+
+  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
+}
+
+void load_data(const std::string& calib_path) {
+  std::ifstream os(calib_path, std::ios::binary);
+
+  if (os.is_open()) {
+    cereal::JSONInputArchive archive(os);
+    archive(calib);
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
+              << std::endl;
+
+  } else {
+    std::cerr << "could not load camera calibration " << calib_path
+              << std::endl;
+    std::abort();
+  }
+}
+
+void compute_projections() {
+  for (size_t i = 0; i < gt_frame_T_w_i.size(); i++) {
+    for (size_t j = 0; j < calib.T_i_c.size(); j++) {
+      basalt::TimeCamId tcid(gt_frame_t_ns[i], j);
+      basalt::SimObservations obs, obs_noisy;
+
+      for (size_t k = 0; k < gt_points.size(); k++) {
+        Eigen::Vector4d p_cam;
+        p_cam.head<3>() =
+            (gt_frame_T_w_i[i] * calib.T_i_c[j]).inverse() * gt_points[k];
+
+        std::visit(
+            [&](const auto& cam) {
+              if (p_cam[2] > 0.1) {
+                Eigen::Vector2d p_2d;
+                cam.project(p_cam, p_2d);
+
+                const int border = 5;
+                if (p_2d[0] >= border && p_2d[1] >= border &&
+                    p_2d[0] < calib.resolution[j][0] - border - 1 &&
+                    p_2d[1] < calib.resolution[j][1] - border - 1) {
+                  obs.pos.emplace_back(p_2d);
+                  obs.id.emplace_back(k);
+
+                  p_2d[0] += obs_noise_dist(gen);
+                  p_2d[1] += obs_noise_dist(gen);
+
+                  obs_noisy.pos.emplace_back(p_2d);
+                  obs_noisy.id.emplace_back(k);
+                }
+              }
+            },
+            calib.intrinsics[j].variant);
+      }
+
+      gt_observations[tcid] = obs;
+      noisy_observations[tcid] = obs_noisy;
+    }
+  }
+}
+
+void gen_data() {
+  std::normal_distribution<> gyro_noise_dist{
+      0, calib.dicrete_time_gyro_noise_std()[0]};
+  std::normal_distribution<> accel_noise_dist{
+      0, calib.dicrete_time_accel_noise_std()[0]};
+
+  std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std[0]};
+  std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std[0]};
+
+  for (size_t i = 0; i < calib.intrinsics.size(); i++) {
+    images.emplace_back();
+    images.back() =
+        pangolin::TypedImage(calib.resolution[i][0], calib.resolution[i][1],
+                             pangolin::PixelFormatFromString("GRAY8"));
+
+    images.back().Fill(200);
+  }
+  show_frame.Meta().gui_changed = true;
+
+  double seconds = NUM_FRAMES / CAM_FREQ;
+  int num_knots = seconds / knot_time + 5;
+  // for (int i = 0; i < 2; i++) gt_spline.knots_push_back(Sophus::SE3d());
+  gt_spline.genRandomTrajectory(num_knots);
+
+  int64_t t_ns = 0;
+  int64_t dt_ns = int64_t(1e9) / CAM_FREQ;
+
+  for (int i = 0; i < NUM_FRAMES; i++) {
+    gt_frame_T_w_i.emplace_back(gt_spline.pose(t_ns));
+    gt_frame_t_w_i.emplace_back(gt_frame_T_w_i.back().translation());
+    gt_frame_t_ns.emplace_back(t_ns);
+
+    t_ns += dt_ns;
+  }
+
+  dt_ns = int64_t(1e9) / IMU_FREQ;
+
+  int64_t offset =
+      dt_ns / 2;  // Offset to make IMU in the center of the interval
+  t_ns = offset;
+
+  imu_data_log.Clear();
+
+  gt_accel_bias.clear();
+  gt_gyro_bias.clear();
+
+  gt_accel_bias.emplace_back(Eigen::Vector3d::Random() / 10);
+  gt_gyro_bias.emplace_back(Eigen::Vector3d::Random() / 100);
+
+  // gt_accel_bias.emplace_back(Eigen::Vector3d::Zero());
+  // gt_gyro_bias.emplace_back(Eigen::Vector3d::Zero());
+
+  while (t_ns < gt_frame_t_ns.back()) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() * (gt_spline.transAccelWorld(t_ns) - g);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    gt_accel.emplace_back(accel_body);
+    gt_gyro.emplace_back(rot_vel_body);
+    gt_vel.emplace_back(gt_spline.transVelWorld(t_ns));
+
+    accel_body[0] += accel_noise_dist(gen);
+    accel_body[1] += accel_noise_dist(gen);
+    accel_body[2] += accel_noise_dist(gen);
+
+    accel_body += gt_accel_bias.back();
+
+    rot_vel_body[0] += gyro_noise_dist(gen);
+    rot_vel_body[1] += gyro_noise_dist(gen);
+    rot_vel_body[2] += gyro_noise_dist(gen);
+
+    rot_vel_body += gt_gyro_bias.back();
+
+    noisy_accel.emplace_back(accel_body);
+    noisy_gyro.emplace_back(rot_vel_body);
+
+    gt_imu_t_ns.emplace_back(t_ns + offset);
+
+    std::vector<float> vals;
+    vals.push_back(t_ns * 1e-9);
+
+    for (int i = 0; i < 3; i++) vals.push_back(gt_accel.back()[i]);
+    for (int i = 0; i < 3; i++) vals.push_back(gt_gyro.back()[i]);
+    for (int i = 0; i < 3; i++) vals.push_back(gt_vel.back()[i]);
+    for (int i = 0; i < 3; i++) vals.push_back(pose.translation()[i]);
+    for (int i = 0; i < 3; i++) vals.push_back(gt_gyro_bias.back()[i]);
+    for (int i = 0; i < 3; i++) vals.push_back(gt_accel_bias.back()[i]);
+
+    imu_data_log.Log(vals);
+
+    double dt_sqrt = std::sqrt(dt_ns * 1e-9);
+    Eigen::Vector3d gt_accel_bias_next = gt_accel_bias.back();
+    gt_accel_bias_next[0] += accel_bias_dist(gen) * dt_sqrt;
+    gt_accel_bias_next[1] += accel_bias_dist(gen) * dt_sqrt;
+    gt_accel_bias_next[2] += accel_bias_dist(gen) * dt_sqrt;
+    gt_accel_bias.emplace_back(gt_accel_bias_next);
+
+    Eigen::Vector3d gt_gyro_bias_next = gt_gyro_bias.back();
+    gt_gyro_bias_next[0] += gyro_bias_dist(gen) * dt_sqrt;
+    gt_gyro_bias_next[1] += gyro_bias_dist(gen) * dt_sqrt;
+    gt_gyro_bias_next[2] += gyro_bias_dist(gen) * dt_sqrt;
+    gt_gyro_bias.emplace_back(gt_gyro_bias_next);
+
+    t_ns += dt_ns;
+  }
+
+  show_accel.Meta().gui_changed = true;
+
+  for (int i = 0; i < NUM_POINTS; i++) {
+    Eigen::Vector3d point;
+
+    point = Eigen::Vector3d::Random().normalized() * POINT_DIST;
+
+    gt_points.push_back(point);
+  }
+
+  compute_projections();
+
+  // Save spline data
+  {
+    std::string path = marg_data_path + "/gt_spline.cereal";
+
+    std::cout << "Saving gt_spline " << path << std::endl;
+
+    std::ofstream os(path, std::ios::binary);
+    {
+      cereal::JSONOutputArchive archive(os);
+
+      int64_t t_ns = gt_spline.getDtNs();
+
+      Eigen::aligned_vector<Sophus::SE3d> knots;
+      for (size_t i = 0; i < gt_spline.numKnots(); i++) {
+        knots.push_back(gt_spline.getKnot(i));
+      }
+
+      archive(cereal::make_nvp("t_ns", t_ns));
+      archive(cereal::make_nvp("knots", knots));
+
+      archive(cereal::make_nvp("noisy_accel", noisy_accel));
+      archive(cereal::make_nvp("noisy_gyro", noisy_gyro));
+      archive(cereal::make_nvp("noisy_accel", gt_accel));
+      archive(cereal::make_nvp("gt_gyro", gt_gyro));
+      archive(cereal::make_nvp("gt_points", gt_points));
+      archive(cereal::make_nvp("gt_accel_bias", gt_accel_bias));
+      archive(cereal::make_nvp("gt_gyro_bias", gt_gyro_bias));
+
+      archive(cereal::make_nvp("gt_observations", gt_observations));
+      archive(cereal::make_nvp("noisy_observations", noisy_observations));
+
+      archive(cereal::make_nvp("gt_points", gt_points));
+
+      archive(cereal::make_nvp("gt_frame_t_ns", gt_frame_t_ns));
+      archive(cereal::make_nvp("gt_imu_t_ns", gt_imu_t_ns));
+    }
+
+    os.close();
+  }
+}
+
+void draw_plots() {
+  plotter->ClearSeries();
+  plotter->ClearMarkers();
+
+  if (show_accel) {
+    plotter->AddSeries("$0", "$1", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "accel measurements x");
+    plotter->AddSeries("$0", "$2", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "accel measurements y");
+    plotter->AddSeries("$0", "$3", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "accel measurements z");
+  }
+
+  if (show_gyro) {
+    plotter->AddSeries("$0", "$4", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "gyro measurements x");
+    plotter->AddSeries("$0", "$5", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "gyro measurements y");
+    plotter->AddSeries("$0", "$6", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "gyro measurements z");
+  }
+
+  if (show_gt_vel) {
+    plotter->AddSeries("$0", "$7", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth velocity x");
+    plotter->AddSeries("$0", "$8", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth velocity y");
+    plotter->AddSeries("$0", "$9", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth velocity z");
+  }
+
+  if (show_gt_pos) {
+    plotter->AddSeries("$0", "$10", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth position x");
+    plotter->AddSeries("$0", "$11", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth position y");
+    plotter->AddSeries("$0", "$12", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth position z");
+  }
+
+  if (show_gt_bg) {
+    plotter->AddSeries("$0", "$13", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth gyro bias x");
+    plotter->AddSeries("$0", "$14", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth gyro bias y");
+    plotter->AddSeries("$0", "$15", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth gyro bias z");
+  }
+
+  if (show_gt_ba) {
+    plotter->AddSeries("$0", "$16", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Red(), "ground-truth accel bias x");
+    plotter->AddSeries("$0", "$17", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Green(), "ground-truth accel bias y");
+    plotter->AddSeries("$0", "$18", pangolin::DrawingModeDashed,
+                       pangolin::Colour::Blue(), "ground-truth accel bias z");
+  }
+
+  if (show_est_vel) {
+    plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated velocity x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated velocity y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated velocity z",
+                       &vio_data_log);
+  }
+
+  if (show_est_pos) {
+    plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated position x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated position y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated position z",
+                       &vio_data_log);
+  }
+
+  if (show_est_bg) {
+    plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated gyro bias x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated gyro bias y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated gyro bias z",
+                       &vio_data_log);
+  }
+
+  if (show_est_ba) {
+    plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine,
+                       pangolin::Colour::Red(), "estimated accel bias x",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine,
+                       pangolin::Colour::Green(), "estimated accel bias y",
+                       &vio_data_log);
+    plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine,
+                       pangolin::Colour::Blue(), "estimated accel bias z",
+                       &vio_data_log);
+  }
+
+  double t = gt_frame_t_ns[show_frame] * 1e-9;
+  plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal,
+                     pangolin::Colour::White());
+}
+
+void setup_vio(const std::string& config_path) {
+  int64_t t_init_ns = gt_frame_t_ns[0];
+  Sophus::SE3d T_w_i_init = gt_frame_T_w_i[0];
+  Eigen::Vector3d vel_w_i_init = gt_spline.transVelWorld(t_init_ns);
+
+  std::cout << "Setting up filter: t_ns " << t_init_ns << std::endl;
+  std::cout << "T_w_i\n" << T_w_i_init.matrix() << std::endl;
+  std::cout << "vel_w_i " << vel_w_i_init.transpose() << std::endl;
+
+  basalt::VioConfig config;
+  if (!config_path.empty()) {
+    config.load(config_path);
+  }
+
+  vio = basalt::VioEstimatorFactory::getVioEstimator(
+      config, calib, basalt::constants::g, use_imu, use_double);
+  vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
+                  gt_accel_bias.front());
+
+  // int iteration = 0;
+  vio_data_log.Clear();
+  error_data_log.Clear();
+  vio_t_w_i.clear();
+}
+
+bool next_step() {
+  if (show_frame < NUM_FRAMES - 1) {
+    show_frame = show_frame + 1;
+    show_frame.Meta().gui_changed = true;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void alignButton() {
+  basalt::alignSVD(gt_frame_t_ns, vio_t_w_i, gt_frame_t_ns, gt_frame_t_w_i);
+}