v01

include/basalt/optimization/accumulator.h

@@ -0,0 +1,281 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+
+#include <array>
+#include <chrono>
+#include <unordered_map>
+
+#include <basalt/utils/assert.h>
+#include <basalt/utils/hash.h>
+
+#if defined(BASALT_USE_CHOLMOD)
+
+#include <Eigen/CholmodSupport>
+
+template <class T>
+using SparseLLT = Eigen::CholmodSupernodalLLT<T>;
+
+#else
+
+template <class T>
+using SparseLLT = Eigen::SimplicialLDLT<T>;
+
+#endif
+
+namespace basalt {
+
+template <typename Scalar_ = double>
+class DenseAccumulator {
+ public:
+  using Scalar = Scalar_;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  template <int ROWS, int COLS, typename Derived>
+  inline void addH(int i, int j, const Eigen::MatrixBase<Derived>& data) {
+    BASALT_ASSERT_STREAM(i >= 0, "i " << i);
+    BASALT_ASSERT_STREAM(j >= 0, "j " << j);
+
+    BASALT_ASSERT_STREAM(i + ROWS <= H.cols(), "i " << i << " ROWS " << ROWS
+                                                    << " H.rows() "
+                                                    << H.rows());
+    BASALT_ASSERT_STREAM(j + COLS <= H.rows(), "j " << j << " COLS " << COLS
+                                                    << " H.cols() "
+                                                    << H.cols());
+
+    H.template block<ROWS, COLS>(i, j) += data;
+  }
+
+  template <int ROWS, typename Derived>
+  inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
+    BASALT_ASSERT_STREAM(i >= 0, "i " << i);
+
+    BASALT_ASSERT_STREAM(i + ROWS <= H.cols(), "i " << i << " ROWS " << ROWS
+                                                    << " H.rows() "
+                                                    << H.rows());
+
+    b.template segment<ROWS>(i) += data;
+  }
+
+  // inline VectorX solve() const { return H.ldlt().solve(b); }
+  inline VectorX solve(const VectorX* diagonal) const {
+    if (diagonal == nullptr) {
+      return H.ldlt().solve(b);
+    } else {
+      MatrixX HH = H;
+      HH.diagonal() += *diagonal;
+      return HH.ldlt().solve(b);
+    }
+  }
+
+  inline void reset(int opt_size) {
+    H.setZero(opt_size, opt_size);
+    b.setZero(opt_size);
+  }
+
+  inline void join(const DenseAccumulator<Scalar>& other) {
+    H += other.H;
+    b += other.b;
+  }
+
+  inline void print() {
+    Eigen::IOFormat CleanFmt(2);
+    std::cerr << "H\n" << H.format(CleanFmt) << std::endl;
+    std::cerr << "b\n" << b.transpose().format(CleanFmt) << std::endl;
+  }
+
+  inline void setup_solver(){};
+  inline VectorX Hdiagonal() const { return H.diagonal(); }
+
+  inline const MatrixX& getH() const { return H; }
+  inline const VectorX& getB() const { return b; }
+
+  inline MatrixX& getH() { return H; }
+  inline VectorX& getB() { return b; }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+  MatrixX H;
+  VectorX b;
+};
+
+template <typename Scalar_ = double>
+class SparseHashAccumulator {
+ public:
+  using Scalar = Scalar_;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+  typedef Eigen::Triplet<Scalar> T;
+  typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
+
+  template <int ROWS, int COLS, typename Derived>
+  inline void addH(int si, int sj, const Eigen::MatrixBase<Derived>& data) {
+    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, ROWS, COLS);
+
+    KeyT id;
+    id[0] = si;
+    id[1] = sj;
+    id[2] = ROWS;
+    id[3] = COLS;
+
+    auto it = hash_map.find(id);
+
+    if (it == hash_map.end()) {
+      hash_map.emplace(id, data);
+    } else {
+      it->second += data;
+    }
+  }
+
+  template <int ROWS, typename Derived>
+  inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
+    b.template segment<ROWS>(i) += data;
+  }
+
+  inline void setup_solver() {
+    std::vector<T> triplets;
+    triplets.reserve(hash_map.size() * 36 + b.rows());
+
+    for (const auto& kv : hash_map) {
+      for (int i = 0; i < kv.second.rows(); i++) {
+        for (int j = 0; j < kv.second.cols(); j++) {
+          triplets.emplace_back(kv.first[0] + i, kv.first[1] + j,
+                                kv.second(i, j));
+        }
+      }
+    }
+
+    for (int i = 0; i < b.rows(); i++) {
+      triplets.emplace_back(i, i, std::numeric_limits<double>::min());
+    }
+
+    smm = SparseMatrix(b.rows(), b.rows());
+    smm.setFromTriplets(triplets.begin(), triplets.end());
+  }
+
+  inline VectorX Hdiagonal() const { return smm.diagonal(); }
+
+  inline VectorX& getB() { return b; }
+
+  inline VectorX solve(const VectorX* diagonal) const {
+    auto t2 = std::chrono::high_resolution_clock::now();
+
+    SparseMatrix sm = smm;
+    if (diagonal) sm.diagonal() += *diagonal;
+
+    VectorX res;
+
+    if (iterative_solver) {
+      // NOTE: since we have to disable Eigen's parallelization with OpenMP
+      // (interferes with TBB), the current CG is single-threaded, and we
+      // can expect a substantial speedup by switching to a parallel
+      // implementation of CG.
+      Eigen::ConjugateGradient<Eigen::SparseMatrix<double>,
+                               Eigen::Lower | Eigen::Upper>
+          cg;
+
+      cg.setTolerance(tolerance);
+      cg.compute(sm);
+      res = cg.solve(b);
+    } else {
+      SparseLLT<SparseMatrix> chol(sm);
+      res = chol.solve(b);
+    }
+
+    auto t3 = std::chrono::high_resolution_clock::now();
+
+    auto elapsed2 =
+        std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
+
+    if (print_info) {
+      std::cout << "Solving linear system: " << elapsed2.count() * 1e-6 << "s."
+                << std::endl;
+    }
+
+    return res;
+  }
+
+  inline void reset(int opt_size) {
+    hash_map.clear();
+    b.setZero(opt_size);
+  }
+
+  inline void join(const SparseHashAccumulator<Scalar>& other) {
+    for (const auto& kv : other.hash_map) {
+      auto it = hash_map.find(kv.first);
+
+      if (it == hash_map.end()) {
+        hash_map.emplace(kv.first, kv.second);
+      } else {
+        it->second += kv.second;
+      }
+    }
+
+    b += other.b;
+  }
+
+  double tolerance = 1e-4;
+  bool iterative_solver = false;
+  bool print_info = false;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+  using KeyT = std::array<int, 4>;
+
+  struct KeyHash {
+    inline size_t operator()(const KeyT& c) const {
+      size_t seed = 0;
+      for (int i = 0; i < 4; i++) {
+        hash_combine(seed, c[i]);
+      }
+      return seed;
+    }
+  };
+
+  std::unordered_map<KeyT, MatrixX, KeyHash> hash_map;
+
+  VectorX b;
+
+  SparseMatrix smm;
+};
+
+}  // namespace basalt

include/basalt/optimization/linearize.h

@@ -0,0 +1,195 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef BASALT_LINEARIZE_H
+#define BASALT_LINEARIZE_H
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/calibration/calibration.hpp>
+#include <basalt/camera/stereographic_param.hpp>
+
+namespace basalt {
+
+template <typename Scalar>
+struct LinearizeBase {
+  static const int POSE_SIZE = 6;
+
+  static const int POS_SIZE = 3;
+  static const int POS_OFFSET = 0;
+  static const int ROT_SIZE = 3;
+  static const int ROT_OFFSET = 3;
+  static const int ACCEL_BIAS_SIZE = 9;
+  static const int GYRO_BIAS_SIZE = 12;
+  static const int G_SIZE = 3;
+
+  static const int TIME_SIZE = 1;
+
+  static const int ACCEL_BIAS_OFFSET = 0;
+  static const int GYRO_BIAS_OFFSET = ACCEL_BIAS_SIZE;
+  static const int G_OFFSET = GYRO_BIAS_OFFSET + GYRO_BIAS_SIZE;
+
+  typedef Sophus::SE3<Scalar> SE3;
+  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef typename Eigen::aligned_vector<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_vector<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_vector<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+
+  template <int INTRINSICS_SIZE>
+  struct PoseCalibH {
+    Sophus::Matrix6d H_pose_accum;
+    Sophus::Vector6d b_pose_accum;
+    Eigen::Matrix<double, INTRINSICS_SIZE, 6> H_intr_pose_accum;
+    Eigen::Matrix<double, INTRINSICS_SIZE, INTRINSICS_SIZE> H_intr_accum;
+    Eigen::Matrix<double, INTRINSICS_SIZE, 1> b_intr_accum;
+
+    PoseCalibH() {
+      H_pose_accum.setZero();
+      b_pose_accum.setZero();
+      H_intr_pose_accum.setZero();
+      H_intr_accum.setZero();
+      b_intr_accum.setZero();
+    }
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  struct CalibCommonData {
+    const Calibration<Scalar>* calibration = nullptr;
+    const MocapCalibration<Scalar>* mocap_calibration = nullptr;
+    const Eigen::aligned_vector<Eigen::Vector4d>* aprilgrid_corner_pos_3d =
+        nullptr;
+
+    // Calib data
+    const std::vector<size_t>* offset_T_i_c = nullptr;
+    const std::vector<size_t>* offset_intrinsics = nullptr;
+
+    // Cam data
+    size_t mocap_block_offset;
+    size_t bias_block_offset;
+    const std::unordered_map<int64_t, size_t>* offset_poses = nullptr;
+
+    // Cam-IMU data
+    const Vector3* g = nullptr;
+
+    bool opt_intrinsics;
+
+    // Cam-IMU options
+    bool opt_cam_time_offset;
+    bool opt_g;
+    bool opt_imu_scale;
+
+    Scalar pose_var_inv;
+    Vector3 gyro_var_inv;
+    Vector3 accel_var_inv;
+    Scalar mocap_var_inv;
+
+    Scalar huber_thresh;
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  template <class CamT>
+  inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id,
+                              const Eigen::Matrix4d& T_c_w, const CamT& cam,
+                              PoseCalibH<CamT::N>* cph, double& error,
+                              int& num_points, double& reproj_err) const {
+    Eigen::Matrix<double, 2, 4> d_r_d_p;
+    Eigen::Matrix<double, 2, CamT::N> d_r_d_param;
+
+    BASALT_ASSERT_STREAM(
+        corner_id < int(common_data.aprilgrid_corner_pos_3d->size()),
+        "corner_id " << corner_id);
+
+    Eigen::Vector4d point3d =
+        T_c_w * common_data.aprilgrid_corner_pos_3d->at(corner_id);
+
+    Eigen::Vector2d proj;
+    bool valid;
+    if (cph) {
+      valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
+    } else {
+      valid = cam.project(point3d, proj);
+    }
+    if (!valid || !proj.array().isFinite().all()) return;
+
+    Eigen::Vector2d residual = proj - corner_pos;
+
+    double e = residual.norm();
+    double huber_weight =
+        e < common_data.huber_thresh ? 1.0 : common_data.huber_thresh / e;
+
+    if (cph) {
+      Eigen::Matrix<double, 4, 6> d_point_d_xi;
+
+      d_point_d_xi.topLeftCorner<3, 3>().setIdentity();
+      d_point_d_xi.topRightCorner<3, 3>() =
+          -Sophus::SO3d::hat(point3d.head<3>());
+      d_point_d_xi.row(3).setZero();
+
+      Eigen::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
+
+      cph->H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi;
+      cph->b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual;
+
+      cph->H_intr_pose_accum +=
+          huber_weight * d_r_d_param.transpose() * d_r_d_xi;
+      cph->H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param;
+      cph->b_intr_accum += huber_weight * d_r_d_param.transpose() * residual;
+    }
+
+    error += huber_weight * e * e * (2 - huber_weight);
+    reproj_err += e;
+    num_points++;
+  }
+
+  CalibCommonData common_data;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace basalt
+
+#endif

include/basalt/optimization/poses_linearize.h

@@ -0,0 +1,264 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef BASALT_POSES_LINEARIZE_H
+#define BASALT_POSES_LINEARIZE_H
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/optimization/linearize.h>
+
+#include <basalt/optimization/accumulator.h>
+
+#include <tbb/blocked_range.h>
+
+namespace basalt {
+
+template <typename Scalar, typename AccumT>
+struct LinearizePosesOpt : public LinearizeBase<Scalar> {
+  static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
+
+  typedef Sophus::SE3<Scalar> SE3;
+  typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
+  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  AccumT accum;
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
+
+  LinearizePosesOpt(
+      size_t opt_size,
+      const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose,
+      const CalibCommonData& common_data)
+      : opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
+    this->common_data = common_data;
+    accum.reset(opt_size);
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+  LinearizePosesOpt(const LinearizePosesOpt& other, tbb::split)
+      : opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
+    this->common_data = other.common_data;
+    accum.reset(opt_size);
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
+    for (const AprilgridCornersData& acd : r) {
+      std::visit(
+          [&](const auto& cam) {
+            constexpr int INTRINSICS_SIZE =
+                std::remove_reference<decltype(cam)>::type::N;
+            typename LinearizeBase<Scalar>::template PoseCalibH<INTRINSICS_SIZE>
+                cph;
+
+            SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns);
+            SE3 T_w_c =
+                T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
+            SE3 T_c_w = T_w_c.inverse();
+            Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
+
+            double err = 0;
+            double reproj_err = 0;
+            int num_inliers = 0;
+
+            for (size_t i = 0; i < acd.corner_pos.size(); i++) {
+              this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
+                                    T_c_w_m, cam, &cph, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+
+            const Matrix6 Adj =
+                -this->common_data.calibration->T_i_c[acd.cam_id]
+                     .inverse()
+                     .Adj();
+
+            const size_t po =
+                this->common_data.offset_poses->at(acd.timestamp_ns);
+            const size_t co = this->common_data.offset_T_i_c->at(acd.cam_id);
+            const size_t io =
+                this->common_data.offset_intrinsics->at(acd.cam_id);
+
+            accum.template addH<POSE_SIZE, POSE_SIZE>(
+                po, po, Adj.transpose() * cph.H_pose_accum * Adj);
+            accum.template addB<POSE_SIZE>(po,
+                                           Adj.transpose() * cph.b_pose_accum);
+
+            if (acd.cam_id > 0) {
+              accum.template addH<POSE_SIZE, POSE_SIZE>(
+                  co, po, -cph.H_pose_accum * Adj);
+              accum.template addH<POSE_SIZE, POSE_SIZE>(co, co,
+                                                        cph.H_pose_accum);
+
+              accum.template addB<POSE_SIZE>(co, -cph.b_pose_accum);
+            }
+
+            if (this->common_data.opt_intrinsics) {
+              accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
+                  io, po, cph.H_intr_pose_accum * Adj);
+
+              if (acd.cam_id > 0)
+                accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
+                    io, co, -cph.H_intr_pose_accum);
+
+              accum.template addH<INTRINSICS_SIZE, INTRINSICS_SIZE>(
+                  io, io, cph.H_intr_accum);
+              accum.template addB<INTRINSICS_SIZE>(io, cph.b_intr_accum);
+            }
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void join(LinearizePosesOpt& rhs) {
+    accum.join(rhs.accum);
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+};
+
+template <typename Scalar>
+struct ComputeErrorPosesOpt : public LinearizeBase<Scalar> {
+  static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
+
+  typedef Sophus::SE3<Scalar> SE3;
+  typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
+  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
+
+  ComputeErrorPosesOpt(
+      size_t opt_size,
+      const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose,
+      const CalibCommonData& common_data)
+      : opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
+    this->common_data = common_data;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  ComputeErrorPosesOpt(const ComputeErrorPosesOpt& other, tbb::split)
+      : opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
+    this->common_data = other.common_data;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
+    for (const AprilgridCornersData& acd : r) {
+      std::visit(
+          [&](const auto& cam) {
+            SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns);
+            SE3 T_w_c =
+                T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
+            SE3 T_c_w = T_w_c.inverse();
+            Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
+
+            double err = 0;
+            double reproj_err = 0;
+            int num_inliers = 0;
+
+            for (size_t i = 0; i < acd.corner_pos.size(); i++) {
+              this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
+                                    T_c_w_m, cam, nullptr, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void join(ComputeErrorPosesOpt& rhs) {
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+};  // namespace basalt
+
+}  // namespace basalt
+
+#endif

include/basalt/optimization/poses_optimize.h

@@ -0,0 +1,315 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <basalt/calibration/aprilgrid.h>
+#include <basalt/calibration/calibration_helper.h>
+#include <basalt/optimization/poses_linearize.h>
+
+#include <tbb/parallel_reduce.h>
+
+namespace basalt {
+
+class PosesOptimization {
+  static constexpr size_t POSE_SIZE = 6;
+
+  using Scalar = double;
+
+  typedef LinearizePosesOpt<Scalar, SparseHashAccumulator<Scalar>> LinearizeT;
+
+  using SE3 = typename LinearizeT::SE3;
+  using Vector2 = typename LinearizeT::Vector2;
+  using Vector3 = typename LinearizeT::Vector3;
+  using Vector4 = typename LinearizeT::Vector4;
+  using VectorX = typename LinearizeT::VectorX;
+
+  using AprilgridCornersDataIter =
+      typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator;
+
+ public:
+  PosesOptimization()
+      : lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {}
+
+  Vector2 getOpticalCenter(size_t i) {
+    return calib->intrinsics[i].getParam().template segment<2>(2);
+  }
+
+  void resetCalib(size_t num_cams, const std::vector<std::string> &cam_types) {
+    BASALT_ASSERT(cam_types.size() == num_cams);
+
+    calib.reset(new Calibration<Scalar>);
+
+    for (size_t i = 0; i < cam_types.size(); i++) {
+      calib->intrinsics.emplace_back(
+          GenericCamera<Scalar>::fromString(cam_types[i]));
+
+      if (calib->intrinsics.back().getName() != cam_types[i]) {
+        std::cerr << "Unknown camera type " << cam_types[i] << " default to "
+                  << calib->intrinsics.back().getName() << std::endl;
+      }
+    }
+    calib->T_i_c.resize(num_cams);
+  }
+
+  void loadCalib(const std::string &p) {
+    std::string path = p + "calibration.json";
+
+    std::ifstream is(path);
+
+    if (is.good()) {
+      cereal::JSONInputArchive archive(is);
+      calib.reset(new Calibration<Scalar>);
+      archive(*calib);
+      std::cout << "Loaded calibration from: " << path << std::endl;
+    } else {
+      std::cout << "No calibration found" << std::endl;
+    }
+  }
+
+  void saveCalib(const std::string &path) const {
+    if (calib) {
+      std::ofstream os(path + "calibration.json");
+      cereal::JSONOutputArchive archive(os);
+
+      archive(*calib);
+    }
+  }
+
+  bool calibInitialized() const { return calib != nullptr; }
+  bool initialized() const { return true; }
+
+  // Returns true when converged
+  bool optimize(bool opt_intrinsics, double huber_thresh, double stop_thresh,
+                double &error, int &num_points, double &reprojection_error) {
+    error = 0;
+    num_points = 0;
+
+    ccd.opt_intrinsics = opt_intrinsics;
+    ccd.huber_thresh = huber_thresh;
+
+    LinearizePosesOpt<double, SparseHashAccumulator<double>> lopt(
+        problem_size, timestam_to_pose, ccd);
+
+    tbb::blocked_range<AprilgridCornersDataIter> april_range(
+        aprilgrid_corners_measurements.begin(),
+        aprilgrid_corners_measurements.end());
+    tbb::parallel_reduce(april_range, lopt);
+
+    error = lopt.error;
+    num_points = lopt.num_points;
+    reprojection_error = lopt.reprojection_error;
+
+    std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
+              << lopt.num_points << std::endl;
+
+    lopt.accum.setup_solver();
+    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
+    bool converged = false;
+    bool step = false;
+    int max_iter = 10;
+
+    while (!step && max_iter > 0 && !converged) {
+      Eigen::aligned_unordered_map<int64_t, Sophus::SE3d>
+          timestam_to_pose_backup = timestam_to_pose;
+      Calibration<Scalar> calib_backup = *calib;
+
+      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);
+
+      Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc.array().abs().maxCoeff();
+      if (max_inc < stop_thresh) converged = true;
+
+      for (auto &kv : timestam_to_pose) {
+        kv.second *=
+            Sophus::se3_expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
+      }
+
+      for (size_t i = 0; i < calib->T_i_c.size(); i++) {
+        calib->T_i_c[i] *=
+            Sophus::se3_expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
+      }
+
+      for (size_t i = 0; i < calib->intrinsics.size(); i++) {
+        auto &c = calib->intrinsics[i];
+        c.applyInc(inc.segment(offset_cam_intrinsics[i], c.getN()));
+      }
+
+      ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
+      tbb::parallel_reduce(april_range, eopt);
+
+      double f_diff = (lopt.error - eopt.error);
+      double l_diff = 0.5 * inc.dot(inc * lambda - lopt.accum.getB());
+
+      // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
+
+      double step_quality = f_diff / l_diff;
+
+      if (step_quality < 0) {
+        std::cout << "\t[REJECTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
+                  << " num points " << eopt.num_points << std::endl;
+        lambda = std::min(max_lambda, lambda_vee * lambda);
+        lambda_vee *= 2;
+
+        timestam_to_pose = timestam_to_pose_backup;
+        *calib = calib_backup;
+      } else {
+        std::cout << "\t[ACCEPTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
+                  << " num points " << eopt.num_points << std::endl;
+
+        lambda = std::max(
+            min_lambda,
+            lambda *
+                std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+        lambda_vee = 2;
+
+        error = eopt.error;
+        num_points = eopt.num_points;
+        reprojection_error = eopt.reprojection_error;
+
+        step = true;
+      }
+      max_iter--;
+    }
+
+    if (converged) {
+      std::cout << "[CONVERGED]" << std::endl;
+    }
+
+    return converged;
+  }
+
+  void recompute_size() {
+    offset_poses.clear();
+    offset_T_i_c.clear();
+    offset_cam_intrinsics.clear();
+
+    size_t curr_offset = 0;
+
+    for (const auto &kv : timestam_to_pose) {
+      offset_poses[kv.first] = curr_offset;
+      curr_offset += POSE_SIZE;
+    }
+
+    offset_T_i_c.emplace_back(curr_offset);
+    for (size_t i = 0; i < calib->T_i_c.size(); i++)
+      offset_T_i_c.emplace_back(offset_T_i_c.back() + POSE_SIZE);
+
+    offset_cam_intrinsics.emplace_back(offset_T_i_c.back());
+    for (size_t i = 0; i < calib->intrinsics.size(); i++)
+      offset_cam_intrinsics.emplace_back(offset_cam_intrinsics.back() +
+                                         calib->intrinsics[i].getN());
+
+    problem_size = offset_cam_intrinsics.back();
+  }
+
+  Sophus::SE3d getT_w_i(int64_t i) {
+    auto it = timestam_to_pose.find(i);
+
+    if (it == timestam_to_pose.end())
+      return Sophus::SE3d();
+    else
+      return it->second;
+  }
+
+  void setAprilgridCorners3d(const Eigen::aligned_vector<Eigen::Vector4d> &v) {
+    aprilgrid_corner_pos_3d = v;
+  }
+
+  void init() {
+    recompute_size();
+
+    ccd.calibration = calib.get();
+    ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d;
+    ccd.offset_poses = &offset_poses;
+    ccd.offset_T_i_c = &offset_T_i_c;
+    ccd.offset_intrinsics = &offset_cam_intrinsics;
+  }
+
+  void addAprilgridMeasurement(
+      int64_t t_ns, int cam_id,
+      const Eigen::aligned_vector<Eigen::Vector2d> &corners_pos,
+      const std::vector<int> &corner_id) {
+    aprilgrid_corners_measurements.emplace_back();
+
+    aprilgrid_corners_measurements.back().timestamp_ns = t_ns;
+    aprilgrid_corners_measurements.back().cam_id = cam_id;
+    aprilgrid_corners_measurements.back().corner_pos = corners_pos;
+    aprilgrid_corners_measurements.back().corner_id = corner_id;
+  }
+
+  void addPoseMeasurement(int64_t t_ns, const Sophus::SE3d &pose) {
+    timestam_to_pose[t_ns] = pose;
+  }
+
+  void setVignette(const std::vector<basalt::RdSpline<1, 4>> &vign) {
+    calib->vignette = vign;
+  }
+
+  void setResolution(const Eigen::aligned_vector<Eigen::Vector2i> &resolution) {
+    calib->resolution = resolution;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  std::shared_ptr<Calibration<Scalar>> calib;
+
+ private:
+  typename LinearizePosesOpt<
+      Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
+
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
+
+  size_t problem_size;
+
+  std::unordered_map<int64_t, size_t> offset_poses;
+  std::vector<size_t> offset_cam_intrinsics;
+  std::vector<size_t> offset_T_i_c;
+
+  // frame poses
+  Eigen::aligned_unordered_map<int64_t, Sophus::SE3d> timestam_to_pose;
+
+  Eigen::aligned_vector<AprilgridCornersData> aprilgrid_corners_measurements;
+
+  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
+};
+
+}  // namespace basalt

include/basalt/optimization/spline_linearize.h

@@ -0,0 +1,846 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef BASALT_SPLINE_LINEARIZE_H
+#define BASALT_SPLINE_LINEARIZE_H
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/spline/se3_spline.h>
+#include <basalt/camera/generic_camera.hpp>
+#include <basalt/camera/stereographic_param.hpp>
+
+#include <basalt/optimization/linearize.h>
+
+#include <basalt/utils/test_utils.h>
+
+#include <tbb/blocked_range.h>
+
+namespace basalt {
+
+template <int N, typename Scalar, typename AccumT>
+struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
+  static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
+  static const int POS_SIZE = LinearizeBase<Scalar>::POS_SIZE;
+  static const int POS_OFFSET = LinearizeBase<Scalar>::POS_OFFSET;
+  static const int ROT_SIZE = LinearizeBase<Scalar>::ROT_SIZE;
+  static const int ROT_OFFSET = LinearizeBase<Scalar>::ROT_OFFSET;
+  static const int ACCEL_BIAS_SIZE = LinearizeBase<Scalar>::ACCEL_BIAS_SIZE;
+  static const int GYRO_BIAS_SIZE = LinearizeBase<Scalar>::GYRO_BIAS_SIZE;
+  static const int G_SIZE = LinearizeBase<Scalar>::G_SIZE;
+  static const int TIME_SIZE = LinearizeBase<Scalar>::TIME_SIZE;
+  static const int ACCEL_BIAS_OFFSET = LinearizeBase<Scalar>::ACCEL_BIAS_OFFSET;
+  static const int GYRO_BIAS_OFFSET = LinearizeBase<Scalar>::GYRO_BIAS_OFFSET;
+  static const int G_OFFSET = LinearizeBase<Scalar>::G_OFFSET;
+
+  typedef Sophus::SE3<Scalar> SE3;
+
+  typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
+  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
+
+  typedef Eigen::Matrix<Scalar, 2, 4> Matrix24;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef Se3Spline<N, Scalar> SplineT;
+
+  typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
+      MocapPoseDataIter;
+
+  // typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  AccumT accum;
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const SplineT* spline;
+
+  LinearizeSplineOpt(size_t opt_size, const SplineT* spl,
+                     const CalibCommonData& common_data)
+      : opt_size(opt_size), spline(spl) {
+    this->common_data = common_data;
+
+    accum.reset(opt_size);
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+
+    BASALT_ASSERT(spline);
+  }
+
+  LinearizeSplineOpt(const LinearizeSplineOpt& other, tbb::split)
+      : opt_size(other.opt_size), spline(other.spline) {
+    this->common_data = other.common_data;
+    accum.reset(opt_size);
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  void operator()(const tbb::blocked_range<PoseDataIter>& r) {
+    for (const PoseData& pm : r) {
+      int64_t start_idx;
+
+      typename SplineT::SO3JacobianStruct J_rot;
+      typename SplineT::PosJacobianStruct J_pos;
+
+      int64_t time_ns = pm.timestamp_ns;
+
+      // std::cout << "time " << time << std::endl;
+      // std::cout << "sline.minTime() " << spline.minTime() << std::endl;
+
+      BASALT_ASSERT_STREAM(
+          time_ns >= spline->minTimeNs(),
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
+
+      // Residual from current value of spline
+      Vector3 residual_pos =
+          spline->positionResidual(time_ns, pm.data.translation(), &J_pos);
+      Vector3 residual_rot =
+          spline->orientationResidual(time_ns, pm.data.so3(), &J_rot);
+
+      // std::cerr << "residual_pos " << residual_pos.transpose() << std::endl;
+      // std::cerr << "residual_rot " << residual_rot.transpose() << std::endl;
+
+      BASALT_ASSERT(J_pos.start_idx == J_rot.start_idx);
+
+      start_idx = J_pos.start_idx;
+
+      // std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl;
+
+      const Scalar& pose_var_inv = this->common_data.pose_var_inv;
+
+      error += pose_var_inv *
+               (residual_pos.squaredNorm() + residual_rot.squaredNorm());
+
+      for (size_t i = 0; i < N; i++) {
+        size_t start_i = (start_idx + i) * POSE_SIZE;
+
+        // std::cout << "start_idx " << start_idx << std::endl;
+        // std::cout << "start_i " << start_i << std::endl;
+
+        BASALT_ASSERT(start_i < opt_size);
+
+        for (size_t j = 0; j <= i; j++) {
+          size_t start_j = (start_idx + j) * POSE_SIZE;
+
+          BASALT_ASSERT(start_j < opt_size);
+
+          accum.template addH<POS_SIZE, POS_SIZE>(
+              start_i + POS_OFFSET, start_j + POS_OFFSET,
+              this->common_data.pose_var_inv * J_pos.d_val_d_knot[i] *
+                  J_pos.d_val_d_knot[j] * Matrix3::Identity());
+
+          accum.template addH<ROT_SIZE, ROT_SIZE>(
+              start_i + ROT_OFFSET, start_j + ROT_OFFSET,
+              this->common_data.pose_var_inv *
+                  J_rot.d_val_d_knot[i].transpose() * J_rot.d_val_d_knot[j]);
+        }
+
+        accum.template addB<POS_SIZE>(
+            start_i + POS_OFFSET,
+            pose_var_inv * J_pos.d_val_d_knot[i] * residual_pos);
+        accum.template addB<ROT_SIZE>(
+            start_i + ROT_OFFSET,
+            pose_var_inv * J_rot.d_val_d_knot[i].transpose() * residual_rot);
+      }
+    }
+  }
+
+  void operator()(const tbb::blocked_range<AccelDataIter>& r) {
+    // size_t num_knots = spline.numKnots();
+    // size_t bias_block_offset = POSE_SIZE * num_knots;
+
+    for (const AccelData& pm : r) {
+      typename SplineT::AccelPosSO3JacobianStruct J;
+      typename SplineT::Mat39 J_bias;
+      Matrix3 J_g;
+
+      int64_t t = pm.timestamp_ns;
+
+      //      std::cout << "time " << t << std::endl;
+      //      std::cout << "sline.minTime() " << spline.minTime() << std::endl;
+
+      BASALT_ASSERT_STREAM(
+          t >= spline->minTimeNs(),
+          "t " << t << " spline.minTime() " << spline->minTimeNs());
+      BASALT_ASSERT_STREAM(
+          t <= spline->maxTimeNs(),
+          "t " << t << " spline.maxTime() " << spline->maxTimeNs());
+
+      Vector3 residual = spline->accelResidual(
+          t, pm.data, this->common_data.calibration->calib_accel_bias,
+          *(this->common_data.g), &J, &J_bias, &J_g);
+
+      if (!this->common_data.opt_imu_scale) {
+        J_bias.template block<3, 6>(0, 3).setZero();
+      }
+
+      //      std::cerr << "================================" << std::endl;
+      //      std::cerr << "accel res: " << residual.transpose() << std::endl;
+      //      std::cerr << "accel_bias.transpose(): "
+      //                << this->common_data.calibration->accel_bias.transpose()
+      //                << std::endl;
+      //      std::cerr << "*(this->common_data.g): "
+      //                << this->common_data.g->transpose() << std::endl;
+      //      std::cerr << "pm.data " << pm.data.transpose() << std::endl;
+
+      //      std::cerr << "time " << t << std::endl;
+      //      std::cerr << "sline.minTime() " << spline.minTime() << std::endl;
+      //      std::cerr << "sline.maxTime() " << spline.maxTime() << std::endl;
+      //      std::cerr << "================================" << std::endl;
+
+      const Vector3& accel_var_inv = this->common_data.accel_var_inv;
+
+      error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
+
+      size_t start_bias =
+          this->common_data.bias_block_offset + ACCEL_BIAS_OFFSET;
+      size_t start_g = this->common_data.bias_block_offset + G_OFFSET;
+
+      for (size_t i = 0; i < N; i++) {
+        size_t start_i = (J.start_idx + i) * POSE_SIZE;
+
+        BASALT_ASSERT(start_i < opt_size);
+
+        for (size_t j = 0; j <= i; j++) {
+          size_t start_j = (J.start_idx + j) * POSE_SIZE;
+
+          BASALT_ASSERT(start_j < opt_size);
+
+          accum.template addH<POSE_SIZE, POSE_SIZE>(
+              start_i, start_j,
+              J.d_val_d_knot[i].transpose() * accel_var_inv.asDiagonal() *
+                  J.d_val_d_knot[j]);
+        }
+        accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
+            start_bias, start_i,
+            J_bias.transpose() * accel_var_inv.asDiagonal() *
+                J.d_val_d_knot[i]);
+
+        if (this->common_data.opt_g) {
+          accum.template addH<G_SIZE, POSE_SIZE>(
+              start_g, start_i,
+              J_g.transpose() * accel_var_inv.asDiagonal() * J.d_val_d_knot[i]);
+        }
+
+        accum.template addB<POSE_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
+                                                    accel_var_inv.asDiagonal() *
+                                                    residual);
+      }
+
+      accum.template addH<ACCEL_BIAS_SIZE, ACCEL_BIAS_SIZE>(
+          start_bias, start_bias,
+          J_bias.transpose() * accel_var_inv.asDiagonal() * J_bias);
+
+      if (this->common_data.opt_g) {
+        accum.template addH<G_SIZE, ACCEL_BIAS_SIZE>(
+            start_g, start_bias,
+            J_g.transpose() * accel_var_inv.asDiagonal() * J_bias);
+        accum.template addH<G_SIZE, G_SIZE>(
+            start_g, start_g,
+            J_g.transpose() * accel_var_inv.asDiagonal() * J_g);
+      }
+
+      accum.template addB<ACCEL_BIAS_SIZE>(
+          start_bias,
+          J_bias.transpose() * accel_var_inv.asDiagonal() * residual);
+
+      if (this->common_data.opt_g) {
+        accum.template addB<G_SIZE>(
+            start_g, J_g.transpose() * accel_var_inv.asDiagonal() * residual);
+      }
+    }
+  }
+
+  void operator()(const tbb::blocked_range<GyroDataIter>& r) {
+    // size_t num_knots = spline.numKnots();
+    // size_t bias_block_offset = POSE_SIZE * num_knots;
+
+    for (const GyroData& pm : r) {
+      typename SplineT::SO3JacobianStruct J;
+      typename SplineT::Mat312 J_bias;
+
+      int64_t t_ns = pm.timestamp_ns;
+
+      BASALT_ASSERT(t_ns >= spline->minTimeNs());
+      BASALT_ASSERT(t_ns <= spline->maxTimeNs());
+
+      const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
+
+      Vector3 residual = spline->gyroResidual(
+          t_ns, pm.data, this->common_data.calibration->calib_gyro_bias, &J,
+          &J_bias);
+
+      if (!this->common_data.opt_imu_scale) {
+        J_bias.template block<3, 9>(0, 3).setZero();
+      }
+
+      //      std::cerr << "==============================" << std::endl;
+      //      std::cerr << "residual " << residual.transpose() << std::endl;
+      //      std::cerr << "gyro_bias "
+      //                << this->common_data.calibration->gyro_bias.transpose()
+      //                << std::endl;
+      //      std::cerr << "pm.data " << pm.data.transpose() << std::endl;
+      //      std::cerr << "t_ns " << t_ns << std::endl;
+
+      error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
+
+      size_t start_bias =
+          this->common_data.bias_block_offset + GYRO_BIAS_OFFSET;
+      for (size_t i = 0; i < N; i++) {
+        size_t start_i = (J.start_idx + i) * POSE_SIZE + ROT_OFFSET;
+
+        // std::cout << "start_idx " << J.start_idx << std::endl;
+        // std::cout << "start_i " << start_i << std::endl;
+
+        BASALT_ASSERT(start_i < opt_size);
+
+        for (size_t j = 0; j <= i; j++) {
+          size_t start_j = (J.start_idx + j) * POSE_SIZE + ROT_OFFSET;
+
+          // std::cout << "start_j " << start_j << std::endl;
+
+          BASALT_ASSERT(start_i < opt_size);
+
+          accum.template addH<ROT_SIZE, ROT_SIZE>(
+              start_i, start_j,
+              J.d_val_d_knot[i].transpose() * gyro_var_inv.asDiagonal() *
+                  J.d_val_d_knot[j]);
+        }
+        accum.template addH<GYRO_BIAS_SIZE, ROT_SIZE>(
+            start_bias, start_i,
+            J_bias.transpose() * gyro_var_inv.asDiagonal() * J.d_val_d_knot[i]);
+        accum.template addB<ROT_SIZE>(start_i, J.d_val_d_knot[i].transpose() *
+                                                   gyro_var_inv.asDiagonal() *
+                                                   residual);
+      }
+
+      accum.template addH<GYRO_BIAS_SIZE, GYRO_BIAS_SIZE>(
+          start_bias, start_bias,
+          J_bias.transpose() * gyro_var_inv.asDiagonal() * J_bias);
+      accum.template addB<GYRO_BIAS_SIZE>(
+          start_bias,
+          J_bias.transpose() * gyro_var_inv.asDiagonal() * residual);
+    }
+  }
+
+  void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
+    for (const AprilgridCornersData& acd : r) {
+      std::visit(
+          [&](const auto& cam) {
+            constexpr int INTRINSICS_SIZE =
+                std::remove_reference<decltype(cam)>::type::N;
+
+            typename SplineT::PosePosSO3JacobianStruct J;
+
+            int64_t time_ns = acd.timestamp_ns +
+                              this->common_data.calibration->cam_time_offset_ns;
+
+            if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
+              return;
+
+            SE3 T_w_i = spline->pose(time_ns, &J);
+
+            Vector6 d_T_wi_d_time;
+            spline->d_pose_d_t(time_ns, d_T_wi_d_time);
+
+            SE3 T_w_c =
+                T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
+            SE3 T_c_w = T_w_c.inverse();
+            Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
+
+            typename LinearizeBase<Scalar>::template PoseCalibH<INTRINSICS_SIZE>
+                cph;
+
+            double err = 0;
+            double reproj_err = 0;
+            int num_inliers = 0;
+
+            for (size_t i = 0; i < acd.corner_pos.size(); i++) {
+              this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
+                                    T_c_w_m, cam, &cph, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+
+            Matrix6 Adj = -this->common_data.calibration->T_i_c[acd.cam_id]
+                               .inverse()
+                               .Adj();
+            Matrix6 A_H_p_A = Adj.transpose() * cph.H_pose_accum * Adj;
+
+            size_t T_i_c_start = this->common_data.offset_T_i_c->at(acd.cam_id);
+            size_t calib_start =
+                this->common_data.offset_intrinsics->at(acd.cam_id);
+            size_t start_cam_time_offset =
+                this->common_data.mocap_block_offset + 2 * POSE_SIZE + 1;
+
+            for (int i = 0; i < N; i++) {
+              int start_i = (J.start_idx + i) * POSE_SIZE;
+
+              Matrix6 Ji_A_H_p_A = J.d_val_d_knot[i].transpose() * A_H_p_A;
+
+              for (int j = 0; j <= i; j++) {
+                int start_j = (J.start_idx + j) * POSE_SIZE;
+
+                accum.template addH<POSE_SIZE, POSE_SIZE>(
+                    start_i, start_j, Ji_A_H_p_A * J.d_val_d_knot[j]);
+              }
+
+              accum.template addH<POSE_SIZE, POSE_SIZE>(
+                  T_i_c_start, start_i,
+                  -cph.H_pose_accum * Adj * J.d_val_d_knot[i]);
+
+              if (this->common_data.opt_intrinsics)
+                accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
+                    calib_start, start_i,
+                    cph.H_intr_pose_accum * Adj * J.d_val_d_knot[i]);
+
+              if (this->common_data.opt_cam_time_offset)
+                accum.template addH<TIME_SIZE, POSE_SIZE>(
+                    start_cam_time_offset, start_i,
+                    d_T_wi_d_time.transpose() * A_H_p_A * J.d_val_d_knot[i]);
+
+              accum.template addB<POSE_SIZE>(
+                  start_i, J.d_val_d_knot[i].transpose() * Adj.transpose() *
+                               cph.b_pose_accum);
+            }
+
+            accum.template addH<POSE_SIZE, POSE_SIZE>(T_i_c_start, T_i_c_start,
+                                                      cph.H_pose_accum);
+            accum.template addB<POSE_SIZE>(T_i_c_start, -cph.b_pose_accum);
+
+            if (this->common_data.opt_intrinsics) {
+              accum.template addH<INTRINSICS_SIZE, POSE_SIZE>(
+                  calib_start, T_i_c_start, -cph.H_intr_pose_accum);
+              accum.template addH<INTRINSICS_SIZE, INTRINSICS_SIZE>(
+                  calib_start, calib_start, cph.H_intr_accum);
+              accum.template addB<INTRINSICS_SIZE>(calib_start,
+                                                   cph.b_intr_accum);
+            }
+
+            if (this->common_data.opt_cam_time_offset) {
+              accum.template addH<TIME_SIZE, POSE_SIZE>(
+                  start_cam_time_offset, T_i_c_start,
+                  -d_T_wi_d_time.transpose() * Adj.transpose() *
+                      cph.H_pose_accum);
+
+              if (this->common_data.opt_intrinsics)
+                accum.template addH<TIME_SIZE, INTRINSICS_SIZE>(
+                    start_cam_time_offset, calib_start,
+                    d_T_wi_d_time.transpose() * Adj.transpose() *
+                        cph.H_intr_pose_accum.transpose());
+
+              accum.template addH<TIME_SIZE, TIME_SIZE>(
+                  start_cam_time_offset, start_cam_time_offset,
+                  d_T_wi_d_time.transpose() * A_H_p_A * d_T_wi_d_time);
+
+              accum.template addB<TIME_SIZE>(start_cam_time_offset,
+                                             d_T_wi_d_time.transpose() *
+                                                 Adj.transpose() *
+                                                 cph.b_pose_accum);
+            }
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void operator()(const tbb::blocked_range<MocapPoseDataIter>& r) {
+    for (const MocapPoseData& pm : r) {
+      typename SplineT::PosePosSO3JacobianStruct J_pose;
+
+      int64_t time_ns =
+          pm.timestamp_ns +
+          this->common_data.mocap_calibration->mocap_time_offset_ns;
+
+      // std::cout << "time " << time << std::endl;
+      // std::cout << "sline.minTime() " << spline.minTime() << std::endl;
+
+      if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
+        continue;
+
+      BASALT_ASSERT_STREAM(
+          time_ns >= spline->minTimeNs(),
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
+
+      const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w;
+      const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
+
+      const SE3 T_w_i = spline->pose(time_ns, &J_pose);
+      const SE3 T_moc_mark = T_moc_w * T_w_i * T_i_mark;
+
+      const SE3 T_mark_moc_meas = pm.data.inverse();
+
+      Vector6 residual = Sophus::se3_logd(T_mark_moc_meas * T_moc_mark);
+
+      // TODO: check derivatives
+      Matrix6 d_res_d_T_i_mark;
+      Sophus::rightJacobianInvSE3Decoupled(residual, d_res_d_T_i_mark);
+      Matrix6 d_res_d_T_w_i = d_res_d_T_i_mark * T_i_mark.inverse().Adj();
+      Matrix6 d_res_d_T_moc_w =
+          d_res_d_T_i_mark * (T_w_i * T_i_mark).inverse().Adj();
+
+      Vector6 d_T_wi_d_time;
+      spline->d_pose_d_t(time_ns, d_T_wi_d_time);
+
+      Vector6 d_res_d_time = d_res_d_T_w_i * d_T_wi_d_time;
+
+      size_t start_idx = J_pose.start_idx;
+
+      size_t start_T_moc_w = this->common_data.mocap_block_offset;
+      size_t start_T_i_mark = this->common_data.mocap_block_offset + POSE_SIZE;
+      size_t start_mocap_time_offset =
+          this->common_data.mocap_block_offset + 2 * POSE_SIZE;
+
+      // std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl;
+
+      const Scalar& mocap_var_inv = this->common_data.mocap_var_inv;
+
+      error += mocap_var_inv * residual.squaredNorm();
+
+      Matrix6 H_T_w_i = d_res_d_T_w_i.transpose() * d_res_d_T_w_i;
+
+      for (size_t i = 0; i < N; i++) {
+        size_t start_i = (start_idx + i) * POSE_SIZE;
+
+        // std::cout << "start_idx " << start_idx << std::endl;
+        // std::cout << "start_i " << start_i << std::endl;
+
+        BASALT_ASSERT(start_i < opt_size);
+
+        Matrix6 Ji_H_T_w_i = J_pose.d_val_d_knot[i].transpose() * H_T_w_i;
+
+        for (size_t j = 0; j <= i; j++) {
+          size_t start_j = (start_idx + j) * POSE_SIZE;
+
+          BASALT_ASSERT(start_j < opt_size);
+
+          accum.template addH<POSE_SIZE, POSE_SIZE>(
+              start_i, start_j,
+              mocap_var_inv * Ji_H_T_w_i * J_pose.d_val_d_knot[j]);
+        }
+
+        accum.template addB<POSE_SIZE>(
+            start_i, mocap_var_inv * J_pose.d_val_d_knot[i].transpose() *
+                         d_res_d_T_w_i.transpose() * residual);
+
+        accum.template addH<POSE_SIZE, POSE_SIZE>(
+            start_T_moc_w, start_i,
+            mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_w_i *
+                J_pose.d_val_d_knot[i]);
+
+        accum.template addH<POSE_SIZE, POSE_SIZE>(
+            start_T_i_mark, start_i,
+            mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_w_i *
+                J_pose.d_val_d_knot[i]);
+
+        accum.template addH<TIME_SIZE, POSE_SIZE>(
+            start_mocap_time_offset, start_i,
+            mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_w_i *
+                J_pose.d_val_d_knot[i]);
+      }
+
+      // start_T_moc_w
+      accum.template addH<POSE_SIZE, POSE_SIZE>(
+          start_T_moc_w, start_T_moc_w,
+          mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_moc_w);
+
+      // start_T_i_mark
+      accum.template addH<POSE_SIZE, POSE_SIZE>(
+          start_T_i_mark, start_T_moc_w,
+          mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_moc_w);
+
+      accum.template addH<POSE_SIZE, POSE_SIZE>(
+          start_T_i_mark, start_T_i_mark,
+          mocap_var_inv * d_res_d_T_i_mark.transpose() * d_res_d_T_i_mark);
+
+      // start_mocap_time_offset
+      accum.template addH<TIME_SIZE, POSE_SIZE>(
+          start_mocap_time_offset, start_T_moc_w,
+          mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_moc_w);
+
+      accum.template addH<TIME_SIZE, POSE_SIZE>(
+          start_mocap_time_offset, start_T_i_mark,
+          mocap_var_inv * d_res_d_time.transpose() * d_res_d_T_i_mark);
+
+      accum.template addH<TIME_SIZE, TIME_SIZE>(
+          start_mocap_time_offset, start_mocap_time_offset,
+          mocap_var_inv * d_res_d_time.transpose() * d_res_d_time);
+
+      // B
+      accum.template addB<POSE_SIZE>(
+          start_T_moc_w,
+          mocap_var_inv * d_res_d_T_moc_w.transpose() * residual);
+
+      accum.template addB<POSE_SIZE>(
+          start_T_i_mark,
+          mocap_var_inv * d_res_d_T_i_mark.transpose() * residual);
+
+      accum.template addB<TIME_SIZE>(
+          start_mocap_time_offset,
+          mocap_var_inv * d_res_d_time.transpose() * residual);
+    }
+  }
+
+  void join(LinearizeSplineOpt& rhs) {
+    accum.join(rhs.accum);
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <int N, typename Scalar>
+struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
+  typedef Sophus::SE3<Scalar> SE3;
+
+  typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
+  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
+
+  typedef Eigen::Matrix<Scalar, 2, 4> Matrix24;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef Se3Spline<N, Scalar> SplineT;
+
+  typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
+      MocapPoseDataIter;
+
+  // typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const SplineT* spline;
+
+  ComputeErrorSplineOpt(size_t opt_size, const SplineT* spl,
+                        const CalibCommonData& common_data)
+      : opt_size(opt_size), spline(spl) {
+    this->common_data = common_data;
+
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+
+    BASALT_ASSERT(spline);
+  }
+
+  ComputeErrorSplineOpt(const ComputeErrorSplineOpt& other, tbb::split)
+      : opt_size(other.opt_size), spline(other.spline) {
+    this->common_data = other.common_data;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  void operator()(const tbb::blocked_range<PoseDataIter>& r) {
+    for (const PoseData& pm : r) {
+      int64_t time_ns = pm.timestamp_ns;
+
+      BASALT_ASSERT_STREAM(
+          time_ns >= spline->minTimeNs(),
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
+
+      // Residual from current value of spline
+      Vector3 residual_pos =
+          spline->positionResidual(time_ns, pm.data.translation());
+      Vector3 residual_rot =
+          spline->orientationResidual(time_ns, pm.data.so3());
+
+      // std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl;
+
+      const Scalar& pose_var_inv = this->common_data.pose_var_inv;
+
+      error += pose_var_inv *
+               (residual_pos.squaredNorm() + residual_rot.squaredNorm());
+    }
+  }
+
+  void operator()(const tbb::blocked_range<AccelDataIter>& r) {
+    // size_t num_knots = spline.numKnots();
+    // size_t bias_block_offset = POSE_SIZE * num_knots;
+
+    for (const AccelData& pm : r) {
+      int64_t t = pm.timestamp_ns;
+
+      //      std::cout << "time " << t << std::endl;
+      //      std::cout << "sline.minTime() " << spline.minTime() << std::endl;
+
+      BASALT_ASSERT_STREAM(
+          t >= spline->minTimeNs(),
+          "t " << t << " spline.minTime() " << spline->minTimeNs());
+      BASALT_ASSERT_STREAM(
+          t <= spline->maxTimeNs(),
+          "t " << t << " spline.maxTime() " << spline->maxTimeNs());
+
+      Vector3 residual = spline->accelResidual(
+          t, pm.data, this->common_data.calibration->calib_accel_bias,
+          *(this->common_data.g));
+
+      const Vector3& accel_var_inv = this->common_data.accel_var_inv;
+
+      error += residual.transpose() * accel_var_inv.asDiagonal() * residual;
+    }
+  }
+
+  void operator()(const tbb::blocked_range<GyroDataIter>& r) {
+    // size_t num_knots = spline.numKnots();
+    // size_t bias_block_offset = POSE_SIZE * num_knots;
+
+    for (const GyroData& pm : r) {
+      int64_t t_ns = pm.timestamp_ns;
+
+      BASALT_ASSERT(t_ns >= spline->minTimeNs());
+      BASALT_ASSERT(t_ns <= spline->maxTimeNs());
+
+      const Vector3& gyro_var_inv = this->common_data.gyro_var_inv;
+
+      Vector3 residual = spline->gyroResidual(
+          t_ns, pm.data, this->common_data.calibration->calib_gyro_bias);
+
+      error += residual.transpose() * gyro_var_inv.asDiagonal() * residual;
+    }
+  }
+
+  void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
+    for (const AprilgridCornersData& acd : r) {
+      std::visit(
+          [&](const auto& cam) {
+            int64_t time_ns = acd.timestamp_ns +
+                              this->common_data.calibration->cam_time_offset_ns;
+
+            if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
+              return;
+
+            SE3 T_w_i = spline->pose(time_ns);
+            SE3 T_w_c =
+                T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
+            SE3 T_c_w = T_w_c.inverse();
+            Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
+
+            double err = 0;
+            double reproj_err = 0;
+            int num_inliers = 0;
+
+            for (size_t i = 0; i < acd.corner_pos.size(); i++) {
+              this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
+                                    T_c_w_m, cam, nullptr, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void operator()(const tbb::blocked_range<MocapPoseDataIter>& r) {
+    for (const MocapPoseData& pm : r) {
+      int64_t time_ns =
+          pm.timestamp_ns +
+          this->common_data.mocap_calibration->mocap_time_offset_ns;
+
+      if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
+        continue;
+
+      BASALT_ASSERT_STREAM(
+          time_ns >= spline->minTimeNs(),
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
+
+      const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w;
+      const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
+
+      const SE3 T_w_i = spline->pose(time_ns);
+      const SE3 T_moc_mark = T_moc_w * T_w_i * T_i_mark;
+
+      const SE3 T_mark_moc_meas = pm.data.inverse();
+
+      Vector6 residual = Sophus::se3_logd(T_mark_moc_meas * T_moc_mark);
+
+      const Scalar& mocap_var_inv = this->common_data.mocap_var_inv;
+
+      error += mocap_var_inv * residual.squaredNorm();
+    }
+  }
+
+  void join(ComputeErrorSplineOpt& rhs) {
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace basalt
+
+#endif

include/basalt/optimization/spline_optimize.h

@@ -0,0 +1,612 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/optimization/spline_linearize.h>
+
+#include <basalt/calibration/calibration.hpp>
+
+#include <basalt/camera/unified_camera.hpp>
+
+#include <basalt/image/image.h>
+
+#include <basalt/serialization/headers_serialization.h>
+
+#include <tbb/parallel_reduce.h>
+
+#include <chrono>
+
+namespace basalt {
+
+template <int N, typename Scalar>
+class SplineOptimization {
+ public:
+  typedef LinearizeSplineOpt<N, Scalar, SparseHashAccumulator<Scalar>>
+      LinearizeT;
+
+  typedef typename LinearizeT::SE3 SE3;
+  typedef typename LinearizeT::Vector2 Vector2;
+  typedef typename LinearizeT::Vector3 Vector3;
+  typedef typename LinearizeT::Vector4 Vector4;
+  typedef typename LinearizeT::VectorX VectorX;
+  typedef typename LinearizeT::Matrix24 Matrix24;
+
+  static const int POSE_SIZE = LinearizeT::POSE_SIZE;
+  static const int POS_SIZE = LinearizeT::POS_SIZE;
+  static const int POS_OFFSET = LinearizeT::POS_OFFSET;
+  static const int ROT_SIZE = LinearizeT::ROT_SIZE;
+  static const int ROT_OFFSET = LinearizeT::ROT_OFFSET;
+  static const int ACCEL_BIAS_SIZE = LinearizeT::ACCEL_BIAS_SIZE;
+  static const int GYRO_BIAS_SIZE = LinearizeT::GYRO_BIAS_SIZE;
+  static const int G_SIZE = LinearizeT::G_SIZE;
+
+  static const int ACCEL_BIAS_OFFSET = LinearizeT::ACCEL_BIAS_OFFSET;
+  static const int GYRO_BIAS_OFFSET = LinearizeT::GYRO_BIAS_OFFSET;
+  static const int G_OFFSET = LinearizeT::G_OFFSET;
+
+  const Scalar pose_var;
+
+  Scalar pose_var_inv;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef Se3Spline<N, Scalar> SplineT;
+
+  SplineOptimization(int64_t dt_ns = 1e7, double init_lambda = 1e-12)
+      : pose_var(1e-4),
+        mocap_initialized(false),
+        lambda(init_lambda),
+        min_lambda(1e-18),
+        max_lambda(100),
+        lambda_vee(2),
+        spline(dt_ns),
+        dt_ns(dt_ns) {
+    pose_var_inv = 1.0 / pose_var;
+
+    // reasonable default values
+    // calib.accelerometer_noise_var = 2e-2;
+    // calib.gyroscope_noise_var = 1e-4;
+
+    g.setZero();
+
+    min_time_us = std::numeric_limits<int64_t>::max();
+    max_time_us = std::numeric_limits<int64_t>::min();
+  }
+
+  int64_t getCamTimeOffsetNs() const { return calib->cam_time_offset_ns; }
+  int64_t getMocapTimeOffsetNs() const {
+    return mocap_calib->mocap_time_offset_ns;
+  }
+
+  const SE3& getCamT_i_c(size_t i) const { return calib->T_i_c[i]; }
+  SE3& getCamT_i_c(size_t i) { return calib->T_i_c[i]; }
+
+  VectorX getIntrinsics(size_t i) const {
+    return calib->intrinsics[i].getParam();
+  }
+
+  const CalibAccelBias<Scalar>& getAccelBias() const {
+    return calib->calib_accel_bias;
+  }
+  const CalibGyroBias<Scalar>& getGyroBias() const {
+    return calib->calib_gyro_bias;
+  }
+
+  void resetCalib(size_t num_cams, const std::vector<std::string>& cam_types) {
+    BASALT_ASSERT(cam_types.size() == num_cams);
+
+    calib.reset(new Calibration<Scalar>);
+
+    calib->intrinsics.resize(num_cams);
+    calib->T_i_c.resize(num_cams);
+
+    mocap_calib.reset(new MocapCalibration<Scalar>);
+  }
+
+  void resetMocapCalib() { mocap_calib.reset(new MocapCalibration<Scalar>); }
+
+  void loadCalib(const std::string& p) {
+    std::string path = p + "calibration.json";
+
+    std::ifstream is(path);
+
+    if (is.good()) {
+      cereal::JSONInputArchive archive(is);
+      calib.reset(new Calibration<Scalar>);
+      archive(*calib);
+      std::cout << "Loaded calibration from: " << path << std::endl;
+    } else {
+      std::cerr << "No calibration found. Run camera calibration first!!!"
+                << std::endl;
+    }
+  }
+
+  void saveCalib(const std::string& path) const {
+    if (calib) {
+      std::ofstream os(path + "calibration.json");
+      cereal::JSONOutputArchive archive(os);
+
+      archive(*calib);
+    }
+  }
+
+  void saveMocapCalib(const std::string& path,
+                      int64_t mocap_to_imu_offset_ns = 0) const {
+    if (calib) {
+      std::ofstream os(path + "mocap_calibration.json");
+      cereal::JSONOutputArchive archive(os);
+
+      mocap_calib->mocap_to_imu_offset_ns = mocap_to_imu_offset_ns;
+
+      archive(*mocap_calib);
+    }
+  }
+
+  bool calibInitialized() const { return calib != nullptr; }
+
+  bool initialized() const { return spline.numKnots() > 0; }
+
+  void initSpline(const SE3& pose, int num_knots) {
+    spline.setKnots(pose, num_knots);
+  }
+
+  void initSpline(const SplineT& other) {
+    spline.setKnots(other);
+
+    // std::cerr << "spline.numKnots() " << spline.numKnots() << std::endl;
+    // std::cerr << "other.numKnots() " << other.numKnots() << std::endl;
+
+    size_t num_knots = spline.numKnots();
+
+    for (size_t i = 0; i < num_knots; i++) {
+      Eigen::Vector3d rot_rand_inc = Eigen::Vector3d::Random() / 20;
+      Eigen::Vector3d trans_rand_inc = Eigen::Vector3d::Random();
+
+      // std::cerr << "rot_rand_inc " << rot_rand_inc.transpose() << std::endl;
+      // std::cerr << "trans_rand_inc " << trans_rand_inc.transpose() <<
+      // std::endl;
+
+      spline.getKnotSO3(i) *= Sophus::SO3d::exp(rot_rand_inc);
+      spline.getKnotPos(i) += trans_rand_inc;
+    }
+  }
+
+  const SplineT& getSpline() const { return spline; }
+
+  Vector3 getG() const { return g; }
+
+  void setG(const Vector3& g_a) { g = g_a; }
+
+  // const Calibration& getCalib() const { return calib; }
+  // void setCalib(const Calibration& c) { calib = c; }
+
+  SE3 getT_w_moc() const { return mocap_calib->T_moc_w.inverse(); }
+  void setT_w_moc(const SE3& val) { mocap_calib->T_moc_w = val.inverse(); }
+
+  SE3 getT_mark_i() const { return mocap_calib->T_i_mark.inverse(); }
+  void setT_mark_i(const SE3& val) { mocap_calib->T_i_mark = val.inverse(); }
+
+  Eigen::Vector3d getTransAccelWorld(int64_t t_ns) const {
+    return spline.transAccelWorld(t_ns);
+  }
+
+  Eigen::Vector3d getRotVelBody(int64_t t_ns) const {
+    return spline.rotVelBody(t_ns);
+  }
+
+  SE3 getT_w_i(int64_t t_ns) { return spline.pose(t_ns); }
+
+  void setAprilgridCorners3d(const Eigen::aligned_vector<Eigen::Vector4d>& v) {
+    aprilgrid_corner_pos_3d = v;
+  }
+
+  void addPoseMeasurement(int64_t t_ns, const SE3& pose) {
+    min_time_us = std::min(min_time_us, t_ns);
+    max_time_us = std::max(max_time_us, t_ns);
+
+    pose_measurements.emplace_back();
+    pose_measurements.back().timestamp_ns = t_ns;
+    pose_measurements.back().data = pose;
+  }
+
+  void addMocapMeasurement(int64_t t_ns, const SE3& pose) {
+    mocap_measurements.emplace_back();
+    mocap_measurements.back().timestamp_ns = t_ns;
+    mocap_measurements.back().data = pose;
+  }
+
+  void addAccelMeasurement(int64_t t_ns, const Vector3& meas) {
+    min_time_us = std::min(min_time_us, t_ns);
+    max_time_us = std::max(max_time_us, t_ns);
+
+    accel_measurements.emplace_back();
+    accel_measurements.back().timestamp_ns = t_ns;
+    accel_measurements.back().data = meas;
+  }
+
+  void addGyroMeasurement(int64_t t_ns, const Vector3& meas) {
+    min_time_us = std::min(min_time_us, t_ns);
+    max_time_us = std::max(max_time_us, t_ns);
+
+    gyro_measurements.emplace_back();
+    gyro_measurements.back().timestamp_ns = t_ns;
+    gyro_measurements.back().data = meas;
+  }
+
+  void addAprilgridMeasurement(
+      int64_t t_ns, int cam_id,
+      const Eigen::aligned_vector<Eigen::Vector2d>& corners_pos,
+      const std::vector<int>& corner_id) {
+    min_time_us = std::min(min_time_us, t_ns);
+    max_time_us = std::max(max_time_us, t_ns);
+
+    aprilgrid_corners_measurements.emplace_back();
+    aprilgrid_corners_measurements.back().timestamp_ns = t_ns;
+    aprilgrid_corners_measurements.back().cam_id = cam_id;
+    aprilgrid_corners_measurements.back().corner_pos = corners_pos;
+    aprilgrid_corners_measurements.back().corner_id = corner_id;
+  }
+
+  Scalar getMinTime() const { return min_time_us * 1e-9; }
+  Scalar getMaxTime() const { return max_time_us * 1e-9; }
+
+  int64_t getMinTimeNs() const { return min_time_us; }
+  int64_t getMaxTimeNs() const { return max_time_us; }
+
+  void init() {
+    int64_t time_interval_us = max_time_us - min_time_us;
+
+    if (spline.numKnots() == 0) {
+      spline.setStartTimeNs(min_time_us);
+      spline.setKnots(pose_measurements.front().data,
+                      time_interval_us / dt_ns + N + 1);
+    }
+
+    recompute_size();
+
+    //    std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl;
+    //    std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl;
+
+    while (!mocap_measurements.empty() &&
+           mocap_measurements.front().timestamp_ns <=
+               spline.minTimeNs() + spline.getDtNs())
+      mocap_measurements.pop_front();
+
+    while (!mocap_measurements.empty() &&
+           mocap_measurements.back().timestamp_ns >=
+               spline.maxTimeNs() - spline.getDtNs())
+      mocap_measurements.pop_back();
+
+    ccd.calibration = calib.get();
+    ccd.mocap_calibration = mocap_calib.get();
+    ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d;
+    ccd.g = &g;
+    ccd.offset_intrinsics = &offset_cam_intrinsics;
+    ccd.offset_T_i_c = &offset_T_i_c;
+    ccd.bias_block_offset = bias_block_offset;
+    ccd.mocap_block_offset = mocap_block_offset;
+
+    ccd.opt_g = true;
+
+    ccd.pose_var_inv = pose_var_inv;
+    ccd.gyro_var_inv =
+        calib->dicrete_time_gyro_noise_std().array().square().inverse();
+    ccd.accel_var_inv =
+        calib->dicrete_time_accel_noise_std().array().square().inverse();
+    ccd.mocap_var_inv = pose_var_inv;
+  }
+
+  void recompute_size() {
+    offset_cam_intrinsics.clear();
+
+    size_t num_knots = spline.numKnots();
+
+    bias_block_offset = POSE_SIZE * num_knots;
+
+    size_t T_i_c_block_offset =
+        bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE;
+
+    offset_T_i_c.emplace_back(T_i_c_block_offset);
+    for (size_t i = 0; i < calib->T_i_c.size(); i++)
+      offset_T_i_c.emplace_back(offset_T_i_c.back() + POSE_SIZE);
+
+    offset_cam_intrinsics.emplace_back(offset_T_i_c.back());
+    for (size_t i = 0; i < calib->intrinsics.size(); i++)
+      offset_cam_intrinsics.emplace_back(offset_cam_intrinsics.back() +
+                                         calib->intrinsics[i].getN());
+
+    mocap_block_offset = offset_cam_intrinsics.back();
+
+    opt_size = mocap_block_offset + 2 * POSE_SIZE + 2;
+
+    //    std::cerr << "bias_block_offset " << bias_block_offset << std::endl;
+    //    std::cerr << "mocap_block_offset " << mocap_block_offset << std::endl;
+    //    std::cerr << "opt_size " << opt_size << std::endl;
+    //    std::cerr << "offset_T_i_c.back() " << offset_T_i_c.back() <<
+    //    std::endl; std::cerr << "offset_cam_intrinsics.back() " <<
+    //    offset_cam_intrinsics.back()
+    //              << std::endl;
+  }
+
+  // Returns true when converged
+  bool optimize(bool use_intr, bool use_poses, bool use_april_corners,
+                bool opt_cam_time_offset, bool opt_imu_scale, bool use_mocap,
+                double huber_thresh, double stop_thresh, double& error,
+                int& num_points, double& reprojection_error,
+                bool print_info = true) {
+    // std::cerr << "optimize num_knots " << num_knots << std::endl;
+
+    ccd.opt_intrinsics = use_intr;
+    ccd.opt_cam_time_offset = opt_cam_time_offset;
+    ccd.opt_imu_scale = opt_imu_scale;
+    ccd.huber_thresh = huber_thresh;
+
+    LinearizeT lopt(opt_size, &spline, ccd);
+
+    // auto t1 = std::chrono::high_resolution_clock::now();
+
+    tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(),
+                                                pose_measurements.end());
+    tbb::blocked_range<AprilgridCornersDataIter> april_range(
+        aprilgrid_corners_measurements.begin(),
+        aprilgrid_corners_measurements.end());
+
+    tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
+        mocap_measurements.begin(), mocap_measurements.end());
+
+    tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
+                                                  accel_measurements.end());
+
+    tbb::blocked_range<GyroDataIter> gyro_range(gyro_measurements.begin(),
+                                                gyro_measurements.end());
+
+    if (use_poses) {
+      tbb::parallel_reduce(pose_range, lopt);
+      // lopt(pose_range);
+    }
+
+    if (use_april_corners) {
+      tbb::parallel_reduce(april_range, lopt);
+      // lopt(april_range);
+    }
+
+    if (use_mocap && mocap_initialized) {
+      tbb::parallel_reduce(mocap_pose_range, lopt);
+      // lopt(mocap_pose_range);
+    } else if (use_mocap && !mocap_initialized) {
+      std::cout << "Mocap residuals are not used. Initialize Mocap first!"
+                << std::endl;
+    }
+
+    tbb::parallel_reduce(accel_range, lopt);
+    tbb::parallel_reduce(gyro_range, lopt);
+
+    error = lopt.error;
+    num_points = lopt.num_points;
+    reprojection_error = lopt.reprojection_error;
+
+    if (print_info)
+      std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
+                << lopt.num_points << std::endl;
+
+    lopt.accum.setup_solver();
+    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
+    bool converged = false;
+    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);
+
+      VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc_full.array().abs().maxCoeff();
+
+      if (max_inc < stop_thresh) converged = true;
+
+      Calibration<Scalar> calib_backup = *calib;
+      MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;
+      SplineT spline_backup = spline;
+      Vector3 g_backup = g;
+
+      applyInc(inc_full, offset_cam_intrinsics);
+
+      ComputeErrorSplineOpt eopt(opt_size, &spline, ccd);
+      if (use_poses) {
+        tbb::parallel_reduce(pose_range, eopt);
+      }
+
+      if (use_april_corners) {
+        tbb::parallel_reduce(april_range, eopt);
+      }
+
+      if (use_mocap && mocap_initialized) {
+        tbb::parallel_reduce(mocap_pose_range, eopt);
+      } else if (use_mocap && !mocap_initialized) {
+        std::cout << "Mocap residuals are not used. Initialize Mocap first!"
+                  << std::endl;
+      }
+
+      tbb::parallel_reduce(accel_range, eopt);
+      tbb::parallel_reduce(gyro_range, eopt);
+
+      double f_diff = (lopt.error - eopt.error);
+      double l_diff = 0.5 * inc_full.dot(inc_full * lambda - lopt.accum.getB());
+
+      // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
+
+      double step_quality = f_diff / l_diff;
+
+      if (step_quality < 0) {
+        if (print_info)
+          std::cout << "\t[REJECTED] lambda:" << lambda
+                    << " step_quality: " << step_quality
+                    << " max_inc: " << max_inc << " Error: " << eopt.error
+                    << " num points " << eopt.num_points << std::endl;
+        lambda = std::min(max_lambda, lambda_vee * lambda);
+        lambda_vee *= 2;
+
+        spline = spline_backup;
+        *calib = calib_backup;
+        *mocap_calib = mocap_calib_backup;
+        g = g_backup;
+
+      } else {
+        if (print_info)
+          std::cout << "\t[ACCEPTED] lambda:" << lambda
+                    << " step_quality: " << step_quality
+                    << " max_inc: " << max_inc << " Error: " << eopt.error
+                    << " num points " << eopt.num_points << std::endl;
+
+        lambda = std::max(
+            min_lambda,
+            lambda *
+                std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+        lambda_vee = 2;
+
+        error = eopt.error;
+        num_points = eopt.num_points;
+        reprojection_error = eopt.reprojection_error;
+
+        step = true;
+      }
+      max_iter--;
+    }
+
+    if (converged && print_info) {
+      std::cout << "[CONVERGED]" << std::endl;
+    }
+
+    return converged;
+  }
+
+  typename Calibration<Scalar>::Ptr calib;
+  typename MocapCalibration<Scalar>::Ptr mocap_calib;
+  bool mocap_initialized;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+  typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
+  typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
+  typedef
+      typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
+  typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
+      MocapPoseDataIter;
+
+  void applyInc(VectorX& inc_full,
+                const std::vector<size_t>& offset_cam_intrinsics) {
+    size_t num_knots = spline.numKnots();
+
+    for (size_t i = 0; i < num_knots; i++) {
+      Vector6 inc = inc_full.template segment<POSE_SIZE>(POSE_SIZE * i);
+
+      // std::cerr << "i: " << i << " inc: " << inc.transpose() << std::endl;
+
+      spline.applyInc(i, inc);
+    }
+
+    size_t bias_block_offset = POSE_SIZE * num_knots;
+    calib->calib_accel_bias += inc_full.template segment<ACCEL_BIAS_SIZE>(
+        bias_block_offset + ACCEL_BIAS_OFFSET);
+
+    calib->calib_gyro_bias += inc_full.template segment<GYRO_BIAS_SIZE>(
+        bias_block_offset + GYRO_BIAS_OFFSET);
+    g += inc_full.template segment<G_SIZE>(bias_block_offset + G_OFFSET);
+
+    size_t T_i_c_block_offset =
+        bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE;
+    for (size_t i = 0; i < calib->T_i_c.size(); i++) {
+      calib->T_i_c[i] *= Sophus::se3_expd(inc_full.template segment<POSE_SIZE>(
+          T_i_c_block_offset + i * POSE_SIZE));
+    }
+
+    for (size_t i = 0; i < calib->intrinsics.size(); i++) {
+      calib->intrinsics[i].applyInc(inc_full.segment(
+          offset_cam_intrinsics[i], calib->intrinsics[i].getN()));
+    }
+
+    size_t mocap_block_offset = offset_cam_intrinsics.back();
+
+    mocap_calib->T_moc_w *= Sophus::se3_expd(
+        inc_full.template segment<POSE_SIZE>(mocap_block_offset));
+    mocap_calib->T_i_mark *= Sophus::se3_expd(
+        inc_full.template segment<POSE_SIZE>(mocap_block_offset + POSE_SIZE));
+
+    mocap_calib->mocap_time_offset_ns +=
+        1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE];
+
+    calib->cam_time_offset_ns +=
+        1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1];
+
+    //    std::cout << "bias_block_offset " << bias_block_offset << std::endl;
+    //    std::cout << "mocap_block_offset " << mocap_block_offset << std::endl;
+  }
+
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
+
+  int64_t min_time_us, max_time_us;
+
+  Eigen::aligned_deque<PoseData> pose_measurements;
+  Eigen::aligned_deque<GyroData> gyro_measurements;
+  Eigen::aligned_deque<AccelData> accel_measurements;
+  Eigen::aligned_deque<AprilgridCornersData> aprilgrid_corners_measurements;
+  Eigen::aligned_deque<MocapPoseData> mocap_measurements;
+
+  typename LinearizeT::CalibCommonData ccd;
+
+  std::vector<size_t> offset_cam_intrinsics;
+  std::vector<size_t> offset_T_i_c;
+  size_t mocap_block_offset;
+  size_t bias_block_offset;
+  size_t opt_size;
+
+  SplineT spline;
+  Vector3 g;
+
+  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
+
+  int64_t dt_ns;
+};  // namespace basalt
+
+}  // namespace basalt