selected-additions.patch

diff --git a/src/rootba/bal/bal_bundle_adjustment_helper.cpp b/src/rootba/bal/bal_bundle_adjustment_helper.cpp
index 204fb16..0c20fc2 100644
--- a/src/rootba/bal/bal_bundle_adjustment_helper.cpp
+++ b/src/rootba/bal/bal_bundle_adjustment_helper.cpp
@@ -35,6 +35,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #include "rootba/bal/bal_bundle_adjustment_helper.hpp"
 
+#include <variant>
+
 #include <tbb/blocked_range.h>
 #include <tbb/parallel_reduce.h>
 
@@ -71,6 +73,9 @@ void BalBundleAdjustmentHelper<Scalar>::compute_error(
     ResidualInfo& error) {
   const bool ignore_validity_check = !options.use_projection_validity_check();
 
+  const auto& keyframes = bal_problem.keyframes();
+  const auto& calib = bal_problem.calib();
+
   // body for parallel reduce
   auto body = [&](const tbb::blocked_range<int>& range,
                   ResidualInfoAccu error_accu) {
@@ -78,13 +83,22 @@ void BalBundleAdjustmentHelper<Scalar>::compute_error(
       const int lm_id = r;
       const auto& lm = bal_problem.landmarks().at(lm_id);
 
-      for (const auto& [frame_id, obs] : lm.obs) {
-        const auto& cam = bal_problem.cameras().at(frame_id);
+      for (const auto& [tcid, obs] : lm.obs) {
+        FrameIdx frame_idx = tcid.frame_id;
+        CamId cam_id = tcid.cam_id;
+
+        const auto& keyframe = keyframes.at(frame_idx);
+        const auto& T_w_i = keyframe.T_w_i;
+
+        const auto& T_i_c = calib.T_i_c[cam_id];
+        const auto& cam_model = calib.intrinsics[cam_id];
+
+        // Compute transformation from world to camera frame
+        typename BalProblem<Scalar>::SE3 T_c_w = (T_w_i * T_i_c).inverse();
 
         VecR res;
-        const bool projection_valid =
-            linearize_point(obs.pos, lm.p_w, cam.T_c_w, cam.intrinsics,
-                            ignore_validity_check, res);
+        const bool projection_valid = linearize_point(
+            obs.pos, lm.p_w, T_c_w, cam_model, ignore_validity_check, res);
 
         const bool numerically_valid = res.array().isFinite().all();
 
@@ -111,7 +125,7 @@ void BalBundleAdjustmentHelper<Scalar>::compute_error(
 template <typename Scalar>
 bool BalBundleAdjustmentHelper<Scalar>::linearize_point(
     const Vec2& obs, const Vec3& lm_p_w, const SE3& T_c_w,
-    const basalt::BalCamera<Scalar>& intr, const bool ignore_validity_check,
+    const basalt::GenericCamera<Scalar>& intr, const bool ignore_validity_check,
     VecR& res, MatRP* d_res_d_xi, MatRI* d_res_d_i, MatRL* d_res_d_l) {
   Mat4 T_c_w_mat = T_c_w.matrix();
 
@@ -120,7 +134,7 @@ bool BalBundleAdjustmentHelper<Scalar>::linearize_point(
   Mat24 d_res_d_p;
   bool projection_valid;
   if (d_res_d_xi || d_res_d_i || d_res_d_l) {
-    projection_valid = intr.project(p_c_3d, res, &d_res_d_p, d_res_d_i);
+    projection_valid = intr.project(p_c_3d, res, &d_res_d_p, nullptr);
   } else {
     projection_valid = intr.project(p_c_3d, res, nullptr, nullptr);
   }
diff --git a/src/rootba/bal/bal_dataset_options.hpp b/src/rootba/bal/bal_dataset_options.hpp
index 9daffe0..38269d1 100644
--- a/src/rootba/bal/bal_dataset_options.hpp
+++ b/src/rootba/bal/bal_dataset_options.hpp
@@ -50,6 +50,9 @@ struct BalDatasetOptions : public VisitableOptions<BalDatasetOptions> {
   VISITABLE_META(DatasetType, input_type,
                  init(DatasetType::AUTO).help("type of dataset to load"));
 
+  VISITABLE_META(std::string, calibration_file,
+                 init("").help("calibration file to use"));
+
   VISITABLE_META(bool, save_output,
                  init(false).help("save optimization result"));
   VISITABLE_META(std::string, save_bal,
diff --git a/src/rootba/bal/bal_problem.hpp b/src/rootba/bal/bal_problem.hpp
index 61d560c..26da589 100644
--- a/src/rootba/bal/bal_problem.hpp
+++ b/src/rootba/bal/bal_problem.hpp
@@ -38,7 +38,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cstdint>
 #include <map>
 
-#include <basalt/camera/bal_camera.hpp>
+#include <basalt/calibration/calibration.hpp>
+#include <basalt/camera/generic_camera.hpp>
 #include <glog/logging.h>
 
 #include "rootba/bal/common_types.hpp"
@@ -70,9 +71,12 @@ class BalProblem {
   using SE3 = Sophus::SE3<Scalar>;
   using SO3 = Sophus::SO3<Scalar>;
 
+  // Only used when the map is loaded using load_bal or load_bundler, which are
+  // hardcoded for bal cameras.
   static constexpr int CAM_STATE_SIZE = 10;
 
-  using CameraModel = basalt::BalCamera<Scalar>;
+  // Generic camera wrapper to support multiple intrinsic models.
+  using CameraModel = basalt::GenericCamera<Scalar>;
 
   struct Observation {
     Vec2 pos = Vec2::Zero();
@@ -83,16 +87,22 @@ class BalProblem {
     CameraModel intrinsics;  // per-camera intrinsics
 
     VecX params() const {
-      VecX p(CAM_STATE_SIZE);
+      const int intrinsics_size = intrinsics.getN();
+      VecX p(7 + intrinsics_size);
       p.template head<7>() = T_c_w.params();
-      p.template tail<3>() = intrinsics.getParam();
+      p.tail(intrinsics_size) = intrinsics.getParam();
       return p;
     }
 
     void from_params(const VecX& p) {
-      CHECK_EQ(p.size(), CAM_STATE_SIZE);
+      CHECK_GE(p.size(), 7);
+      const int intrinsics_size = p.size() - 7;
       T_c_w = Eigen::Map<const SE3>(p.data());
-      intrinsics = CameraModel(p.template tail<3>());
+
+      // Ensure the stored model matches the provided parameter size.
+      CHECK_EQ(intrinsics.getN(), intrinsics_size)
+          << "Camera intrinsics size mismatch; model needs reinitialization";
+      intrinsics.setParams(p.tail(intrinsics_size));
     }
 
     void apply_inc_pose(const Vec6& inc) { inc_pose(inc, T_c_w); }
@@ -101,12 +111,9 @@ class BalProblem {
       T_c_w = Sophus::se3_expd(inc) * T_c_w;
     }
 
-    void apply_inc_intrinsics(const Vec3& inc) {
-      inc_intrinsics(inc, intrinsics);
-    }
-
-    inline static void inc_intrinsics(const Vec3& inc, CameraModel& intr) {
-      intr += inc;
+    template <class Derived>
+    void apply_inc_intrinsics(const Eigen::MatrixBase<Derived>& inc) {
+      intrinsics.applyInc(inc);
     }
 
     void backup() {
@@ -134,9 +141,9 @@ class BalProblem {
   };
 
   struct Landmark {
-    Vec3 p_w;                             // point position in world coordinates
-    Eigen::Vector3<uint8_t> color;        // RGB color
-    std::map<FrameIdx, Observation> obs;  // list of frame indices
+    Vec3 p_w;                       // point position in world coordinates
+    Eigen::Vector3<uint8_t> color;  // RGB color
+    std::map<TimeCamId, Observation> obs;  // list of frame indices
 
     void backup() { p_w_backup_ = p_w; }
 
@@ -158,19 +165,45 @@ class BalProblem {
   };
 
   using Cameras = std::vector<Camera>;
+  // Keyframe represents a rig (set of cameras with fixed relative transforms)
+  struct Keyframe {
+    // TODO@tsantucci: rename to something like Rig or Pose
+    SE3 T_w_i;  // IMU pose (world-to-IMU transformation)
+    size_t t_ns;
+
+    void backup() { T_w_i_backup_ = T_w_i; }
+
+    void restore() { T_w_i = T_w_i_backup_; }
+
+    void apply_inc_pose(const Vec6& inc) {
+      T_w_i = Sophus::se3_expd(inc) * T_w_i;
+    }
+
+    template <typename Scalar2>
+    typename BalProblem<Scalar2>::Keyframe cast() const {
+      typename BalProblem<Scalar2>::Keyframe res;
+      res.T_w_i = T_w_i.template cast<Scalar2>();
+      return res;
+    }
+
+   private:
+    SE3 T_w_i_backup_;
+  };
+
   using Landmarks = std::vector<Landmark>;
+  using Keyframes = std::vector<Keyframe>;
+  using Calibration = basalt::Calibration<Scalar>;
 
   BalProblem() = default;
   explicit BalProblem(const std::string& path);
 
-  void load_bal(const std::string& path);
-  void load_bundler(const std::string& path);
-  void load_colmap(const std::string& path);
+  void load_basalt(const std::string& path);
+
+  bool save_basalt(const std::string& path);
 
-  bool load_rootba(const std::string& path);
-  bool save_rootba(const std::string& path);
-  bool save_bal(const std::string& path);
+  bool save_euroc(const std::string& path) const;
 
+  void add_noise(const double obs_noise_sigma);
 
   void normalize(double new_scale);
 
@@ -182,20 +215,23 @@ class BalProblem {
   void postprocress(const BalDatasetOptions& options,
                     PipelineTimingSummary* timing_summary = nullptr);
 
-  void copy_to_camera_state(VecX& camera_state) const;
-  void copy_from_camera_state(const VecX& camera_state);
-
   void backup();
   void restore();
 
   inline const Cameras& cameras() const { return cameras_; }
+  void set_calibration(Calibration& calib) { calib_ = calib; };
+
   inline const Landmarks& landmarks() const { return landmarks_; }
+  inline const Keyframes& keyframes() const { return keyframes_; }
+  inline const Calibration& calib() const { return calib_; }
 
   inline Cameras& cameras() { return cameras_; }
   inline Landmarks& landmarks() { return landmarks_; }
+  inline Keyframes& keyframes() { return keyframes_; }
 
   inline int num_cameras() const { return signed_cast(cameras_.size()); }
   inline int num_landmarks() const { return signed_cast(landmarks_.size()); }
+  inline int num_keyframes() const { return signed_cast(keyframes_.size()); }
   int num_observations() const;
   int max_num_observations_per_lm() const;
   double compute_rcs_sparsity() const;
@@ -207,9 +243,14 @@ class BalProblem {
   BalProblem<Scalar2> copy_cast() const {
     BalProblem<Scalar2> res;
 
+    res.keyframes_.resize(this->keyframes_.size());
     res.cameras_.resize(this->cameras_.size());
     res.landmarks_.resize(this->landmarks_.size());
 
+    for (size_t i = 0; i < this->keyframes_.size(); i++) {
+      res.keyframes_[i] = this->keyframes_[i].template cast<Scalar2>();
+    }
+
     for (size_t i = 0; i < this->cameras_.size(); i++) {
       res.cameras_[i] = this->cameras_[i].template cast<Scalar2>();
     }
@@ -233,6 +274,12 @@ class BalProblem {
 
   Cameras cameras_;
   Landmarks landmarks_;
+  Keyframes keyframes_;
+
+  Calibration calib_;
+
+  std::unordered_map<size_t, size_t> sequential_colmap_id_to_basalt_id;
+  std::unordered_map<size_t, size_t> image_id_to_cam_id;
 
   /// quiet means no INFO level log output
   bool quiet_ = false;
diff --git a/src/rootba/bal/common_types.hpp b/src/rootba/bal/common_types.hpp
index f8723d2..dd9c237 100644
--- a/src/rootba/bal/common_types.hpp
+++ b/src/rootba/bal/common_types.hpp
@@ -43,6 +43,30 @@ namespace rootba {
 
 using FrameIdx = int;
 using LandmarkIdx = int;
+using CamId = std::size_t;
+
+struct TimeCamId {
+  // TODO@tsantucci: rename to something like PoseCamId
+  TimeCamId() : frame_id(0), cam_id(0) {}
+
+  TimeCamId(const FrameIdx& frame_id, const CamId& cam_id)
+      : frame_id(frame_id), cam_id(cam_id) {}
+
+  FrameIdx frame_id;
+  CamId cam_id;
+
+  // Comparison operators for use in std::map
+  bool operator<(const TimeCamId& other) const {
+    if (frame_id != other.frame_id) return frame_id < other.frame_id;
+    return cam_id < other.cam_id;
+  }
+
+  bool operator==(const TimeCamId& other) const {
+    return frame_id == other.frame_id && cam_id == other.cam_id;
+  }
+
+  bool operator!=(const TimeCamId& other) const { return !(*this == other); }
+};
 
 static constexpr FrameIdx INVALID_FRAME_IDX = -1;
 static constexpr LandmarkIdx INVALID_LANDMARK_IDX = -1;
diff --git a/src/rootba/pangolin/bal_map_display.hpp b/src/rootba/pangolin/bal_map_display.hpp
index 0f4106c..71a1d0b 100644
--- a/src/rootba/pangolin/bal_map_display.hpp
+++ b/src/rootba/pangolin/bal_map_display.hpp
@@ -101,7 +101,7 @@ class BalFrameDisplay {
 
   const FrameIdx frame_id_;
 
-  SE3d T_w_c_;
+  SE3d T_w_i_;
 };
 
 }  // namespace rootba
diff --git a/src/rootba/qr/impl/landmark_block_base.ipp b/src/rootba/qr/impl/landmark_block_base.ipp
index 3a89284..a47dc7b 100644
--- a/src/rootba/qr/impl/landmark_block_base.ipp
+++ b/src/rootba/qr/impl/landmark_block_base.ipp
@@ -87,7 +87,7 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::allocate_landmark(
 
 template <class T, typename Scalar, int POSE_SIZE>
 void LandmarkBlockBase<T, Scalar, POSE_SIZE>::linearize_landmark(
-    const LandmarkBlockBase::Cameras& cameras) {
+    const Keyframes& keyframes, const Calibration& calib) {
   ROOTBA_ASSERT(state_ == State::ALLOCATED ||
                 state_ == State::NUMERICAL_FAILURE ||
                 state_ == State::LINEARIZED || state_ == State::MARGINALIZED);
@@ -95,6 +95,7 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::linearize_landmark(
   const size_t lm_idx = static_cast<const T*>(this)->get_lm_idx();
   const size_t res_idx = static_cast<const T*>(this)->get_res_idx();
   const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& obs_to_kf_idx = static_cast<const T*>(this)->get_obs_to_kf_idx();
   auto& storage = static_cast<T*>(this)->get_storage();
   storage.setZero();
 
@@ -103,27 +104,38 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::linearize_landmark(
 
   bool numerically_valid = true;
 
-  for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
-    size_t obs_idx = i * 2;
-    size_t pose_idx = i * POSE_SIZE;
+  size_t obs_i = 0;
+  for (const auto& [tcid, obs] : lm_ptr_->obs) {
+    FrameIdx frame_idx = tcid.frame_id;
+    CamId cam_id = tcid.cam_id;
+
+    size_t obs_idx = obs_i * 2;  // Row index for this observation's residual
+    size_t kf_col_idx = obs_to_kf_idx[obs_i];  // Column index in Jacobian
+    size_t pose_col_idx = kf_col_idx * POSE_SIZE;
+
+    const auto& keyframe = keyframes.at(frame_idx);
+    const auto& T_w_i = keyframe.T_w_i;  // IMU-to-world
+
+    // Get camera intrinsics and extrinsics from calibration
+    ROOTBA_ASSERT(cam_id < calib.T_i_c.size());
+    const auto& T_i_c = calib.T_i_c[cam_id];  // camera-to-IMU
+    const auto& cam_model = calib.intrinsics[cam_id];
 
-    const auto& obs = lm_ptr_->obs.at(cam_idx);
-    const auto& cam = cameras.at(cam_idx);
+    // Compute transformation from world to camera frame
+    typename BalProblem<Scalar>::SE3 T_c_w = T_i_c.inverse() * T_w_i.inverse();
 
-    typename BalBundleAdjustmentHelper<Scalar>::MatRP Jp;
-    typename BalBundleAdjustmentHelper<Scalar>::MatRI Ji;
+    // Compute Jacobian w.r.t. camera pose and landmark
+    typename BalBundleAdjustmentHelper<Scalar>::MatRP Jp_cam;
     typename BalBundleAdjustmentHelper<Scalar>::MatRL Jl;
 
     Vec2 res;
     const bool valid = BalBundleAdjustmentHelper<Scalar>::linearize_point(
-        obs.pos, lm_ptr_->p_w, cam.T_c_w, cam.intrinsics, true, res, &Jp, &Ji,
+        obs.pos, lm_ptr_->p_w, T_c_w, cam_model, true, res, &Jp_cam, nullptr,
         &Jl);
 
     if (!options_.use_valid_projections_only || valid) {
       numerically_valid = numerically_valid && Jl.array().isFinite().all() &&
-                          Jp.array().isFinite().all() &&
-                          Ji.array().isFinite().all() &&
+                          Jp_cam.array().isFinite().all() &&
                           res.array().isFinite().all();
 
       const Scalar res_squared = res.squaredNorm();
@@ -132,11 +144,43 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::linearize_landmark(
               options_.residual_options, res_squared);
       const Scalar sqrt_weight = std::sqrt(weight);
 
-      storage.template block<2, 6>(obs_idx, pose_idx) = sqrt_weight * Jp;
-      storage.template block<2, 3>(obs_idx, pose_idx + 6) = sqrt_weight * Ji;
+      /*
+        Chain rule to get Jacobian w.r.t. IMU pose
+        Perturbation model: T_w_i_new = T_w_i * exp(ξ_imu)
+        This affects camera pose: T_w_c_new = T_w_i * exp(ξ_imu) * T_i_c
+
+        Using adjoint property: T * exp(ξ) = exp(Ad(T) * ξ) * T
+        We get:
+        (1) T_w_i * exp(ξ_imu) * T_i_c =
+          T_w_i * T_i_c * exp(Ad(T_i_c^{-1}) * ξ_imu)
+        by eq. C:
+        (2) T_w_i * exp(ξ_imu) * T_i_c =
+          T_w_i * T_i_c * exp(Ad(T_i_c^{-1}) * ξ_imu)
+
+        T_w_c(e_cam) = T_w_c * exp(Ad(T_i_c^{-1}) * ξ_imu)
+
+        A. exp(Ad(T_i_c^{-1}) * ξ_imu) * T_i_c^{-1} = T_i_c^{-1} * exp(ξ_imu)
+        B. T_i_c * exp(Ad(T_i_c^{-1}) * ξ_imu) * T_i_c^{-1} = exp(ξ_imu)
+        C. exp(ξ_imu) = T_i_c * exp(Ad(T_i_c^{-1}) * ξ_imu) * T_i_c^{-1}
+
+        Therefore:
+          ξ_cam = Ad(T_i_c^{-1}) * ξ_imu
+          And: ∂r/∂ξ_imu = ∂r/∂ξ_cam * Ad(T_i_c^{-1}) = Jp_cam * Ad(T_i_c^{-1})
+      */
+
+      // If we use a left perturbation model, d_cam_d_imu would be id.
+      Mat66 Ad_T_i_c_inv = T_i_c.inverse().Adj();
+      Mat66 d_cam_d_imu = Ad_T_i_c_inv;
+      typename BalBundleAdjustmentHelper<Scalar>::MatRP Jp_imu =
+          Jp_cam * d_cam_d_imu;
+
+      storage.template block<2, 6>(obs_idx, pose_col_idx) =
+          sqrt_weight * Jp_imu;
       storage.template block<2, 3>(obs_idx, lm_idx) = sqrt_weight * Jl;
       storage.template block<2, 1>(obs_idx, res_idx) = sqrt_weight * res;
     }
+
+    obs_i++;
   }
 
   if (numerically_valid) {
@@ -219,14 +263,15 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::back_substitute(
   const size_t padding_idx = static_cast<T*>(this)->get_padding_idx();
   const size_t padding_size = static_cast<T*>(this)->get_padding_size();
   const size_t num_rows = static_cast<T*>(this)->get_num_rows();
-  const auto& pose_idx = static_cast<T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<T*>(this)->get_pose_idx();  // Keyframe indices
   const auto& storage = static_cast<T*>(this)->get_storage();
 
   VecX pose_inc_reduced(padding_idx + padding_size);
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
+    size_t kf_idx = pose_idx[i];  // This is now a keyframe index (FrameIdx)
     pose_inc_reduced.template segment<POSE_SIZE>(POSE_SIZE * i) =
-        pose_inc.template segment<POSE_SIZE>(POSE_SIZE * cam_idx);
+        pose_inc.template segment<POSE_SIZE>(POSE_SIZE * kf_idx);
   }
   pose_inc_reduced.tail(padding_size).setConstant(0);
 
@@ -309,12 +354,14 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_triplets_Q2TJp(
     size_t row_offset, std::vector<Eigen::Triplet<Scalar>>& triplets) const {
   ROOTBA_ASSERT(state_ == State::MARGINALIZED);
 
-  const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<const T*>(this)->get_pose_idx();  // Keyframe indices
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
   // TODO: check if it makes a difference if we order the triples by rows
   for (size_t i = 0; i < pose_idx.size(); ++i) {
-    size_t col_offset = pose_idx[i] * POSE_SIZE;
+    size_t kf_idx = pose_idx[i];  // Keyframe index
+    size_t col_offset = kf_idx * POSE_SIZE;
     add_triplets_dense(
         storage.block(3, i * POSE_SIZE, num_Q2T_rows(), POSE_SIZE), row_offset,
         col_offset, triplets);
@@ -329,15 +376,16 @@ LandmarkBlockBase<T, Scalar, POSE_SIZE>::get_Q1TJp_postmult_x(
 
   const size_t padding_idx = static_cast<const T*>(this)->get_padding_idx();
   const size_t padding_size = static_cast<const T*>(this)->get_padding_size();
-  const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<const T*>(this)->get_pose_idx();  // Keyframe indices
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
   VecX x_pose_reduced(padding_idx + padding_size);
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
+    size_t kf_idx = pose_idx[i];  // Keyframe index
     x_pose_reduced.template segment<POSE_SIZE>(POSE_SIZE * i) =
-        x_pose.template segment<POSE_SIZE>(POSE_SIZE * cam_idx);
+        x_pose.template segment<POSE_SIZE>(POSE_SIZE * kf_idx);
   }
   x_pose_reduced.tail(padding_size).setConstant(0);
 
@@ -356,15 +404,16 @@ LandmarkBlockBase<T, Scalar, POSE_SIZE>::get_Q2TJp_postmult_x(
   const size_t padding_idx = static_cast<const T*>(this)->get_padding_idx();
   const size_t padding_size = static_cast<const T*>(this)->get_padding_size();
   const size_t num_rows = static_cast<const T*>(this)->get_num_rows();
-  const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<const T*>(this)->get_pose_idx();  // Keyframe indices
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
   VecX x_pose_reduced(padding_idx + padding_size);
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
+    size_t kf_idx = pose_idx[i];  // Keyframe index
     x_pose_reduced.template segment<POSE_SIZE>(POSE_SIZE * i) =
-        x_pose.template segment<POSE_SIZE>(POSE_SIZE * cam_idx);
+        x_pose.template segment<POSE_SIZE>(POSE_SIZE * kf_idx);
   }
   x_pose_reduced.tail(padding_size).setConstant(0);
 
@@ -383,7 +432,8 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_premult_x(
   const size_t padding_idx = static_cast<const T*>(this)->get_padding_idx();
   const size_t padding_size = static_cast<const T*>(this)->get_padding_size();
   const size_t num_rows = static_cast<const T*>(this)->get_num_rows();
-  const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<const T*>(this)->get_pose_idx();  // Keyframe indices
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
   VecX res_reduced =
@@ -391,8 +441,8 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_premult_x(
       storage.bottomLeftCorner(num_rows - 3, padding_idx + padding_size);
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
-    res.template segment<POSE_SIZE>(POSE_SIZE * cam_idx) +=
+    size_t kf_idx = pose_idx[i];  // Keyframe index
+    res.template segment<POSE_SIZE>(POSE_SIZE * kf_idx) +=
         res_reduced.template segment<POSE_SIZE>(POSE_SIZE * i);
   }
 }
@@ -407,15 +457,16 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_T_Q2TJp_mult_x(
   const size_t padding_idx = static_cast<const T*>(this)->get_padding_idx();
   const size_t padding_size = static_cast<const T*>(this)->get_padding_size();
   const size_t num_rows = static_cast<const T*>(this)->get_num_rows();
-  const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<const T*>(this)->get_pose_idx();  // Keyframe indices
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
   VecX x_pose_reduced(padding_idx + padding_size);
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
+    size_t kf_idx = pose_idx[i];  // Keyframe index
     x_pose_reduced.template segment<POSE_SIZE>(POSE_SIZE * i) =
-        x_pose.template segment<POSE_SIZE>(POSE_SIZE * cam_idx);
+        x_pose.template segment<POSE_SIZE>(POSE_SIZE * kf_idx);
   }
   x_pose_reduced.tail(padding_size).setConstant(0);
 
@@ -426,15 +477,15 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_T_Q2TJp_mult_x(
   x_pose_reduced.noalias() = block.adjoint() * tmp;
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
+    size_t kf_idx = pose_idx[i];  // Keyframe index
 
     if (pose_mutex == nullptr) {
-      res.template segment<POSE_SIZE>(POSE_SIZE * cam_idx) +=
+      res.template segment<POSE_SIZE>(POSE_SIZE * kf_idx) +=
           x_pose_reduced.template segment<POSE_SIZE>(POSE_SIZE * i);
     } else {
-      std::scoped_lock lock(pose_mutex->at(cam_idx));
+      std::scoped_lock lock(pose_mutex->at(kf_idx));
 
-      res.template segment<POSE_SIZE>(POSE_SIZE * cam_idx) +=
+      res.template segment<POSE_SIZE>(POSE_SIZE * kf_idx) +=
           x_pose_reduced.template segment<POSE_SIZE>(POSE_SIZE * i);
     }
   }
@@ -459,8 +510,8 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_T_Q2Tr(
   // (Q2^T * Jp)^T * Q2^Tr
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
-    res.template segment<POSE_SIZE>(POSE_SIZE * cam_idx) +=
+    size_t kf_idx = pose_idx[i];  // Keyframe index
+    res.template segment<POSE_SIZE>(POSE_SIZE * kf_idx) +=
         x_pose_reduced.template segment<POSE_SIZE>(POSE_SIZE * i);
   }
 }
@@ -484,8 +535,8 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_diag2(
           .squaredNorm();
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
-    res.template segment<POSE_SIZE>(POSE_SIZE * cam_idx) +=
+    size_t kf_idx = pose_idx[i];  // Keyframe index
+    res.template segment<POSE_SIZE>(POSE_SIZE * kf_idx) +=
         res_reduced.template segment<POSE_SIZE>(POSE_SIZE * i);
   }
 }
@@ -501,9 +552,6 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Jp_diag2(
   const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
-  // TODO: we know that every column has only 2 non-zero rows, so maybe we
-  // should limit computation to those...
-
   // exclude landmark damping rows (but they'd be 0 anyway)
   VecX res_reduced =
       storage.topLeftCorner(num_rows - 3, padding_idx + padding_size)
@@ -511,8 +559,8 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Jp_diag2(
           .squaredNorm();
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
-    res.template segment<POSE_SIZE>(POSE_SIZE * cam_idx) +=
+    size_t kf_idx = pose_idx[i];  // Keyframe index
+    res.template segment<POSE_SIZE>(POSE_SIZE * kf_idx) +=
         res_reduced.template segment<POSE_SIZE>(POSE_SIZE * i);
   }
 }
@@ -534,19 +582,19 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Q2TJp_T_Q2TJp_blockdiag(
       const auto Q2T_Jp =
           storage.block(3, POSE_SIZE * i, num_rows - 3, POSE_SIZE);
 
-      const size_t cam_idx = pose_idx[i];
+      const size_t kf_idx = pose_idx[i];  // Keyframe index
       {
         MatX tmp = Q2T_Jp.transpose() * Q2T_Jp;
-        std::scoped_lock lock(pose_mutex->at(cam_idx));
-        accu.add(cam_idx, std::move(tmp));
+        std::scoped_lock lock(pose_mutex->at(kf_idx));
+        accu.add(kf_idx, std::move(tmp));
       }
     }
   } else {
     for (size_t i = 0; i < pose_idx.size(); i++) {
       // using auto gives us a "reference" to the block
       auto Q2T_Jp = storage.block(3, POSE_SIZE * i, num_rows - 3, POSE_SIZE);
-      const size_t cam_idx = pose_idx[i];
-      accu.add(cam_idx, Q2T_Jp.transpose() * Q2T_Jp);
+      const size_t kf_idx = pose_idx[i];  // Keyframe index
+      accu.add(kf_idx, Q2T_Jp.transpose() * Q2T_Jp);
     }
   }
 }
@@ -556,15 +604,19 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::add_Jp_T_Jp_blockdiag(
     BlockDiagonalAccumulator<Scalar>& accu) const {
   ROOTBA_ASSERT(state_ == State::LINEARIZED);
 
-  const auto& pose_idx = static_cast<const T*>(this)->get_pose_idx();
+  const auto& pose_idx =
+      static_cast<const T*>(this)->get_pose_idx();  // Keyframe indices
+  const auto& obs_to_kf_idx = static_cast<const T*>(this)->get_obs_to_kf_idx();
   const auto& storage = static_cast<const T*>(this)->get_storage();
 
-  for (size_t i = 0; i < pose_idx.size(); i++) {
-    // using auto gives us a "reference" to the block
-    auto Jp = storage.block(2 * i, POSE_SIZE * i, 2, POSE_SIZE);
+  // Iterate over observations and accumulate to corresponding keyframes
+  for (size_t obs_i = 0; obs_i < obs_to_kf_idx.size(); obs_i++) {
+    size_t kf_col_idx = obs_to_kf_idx[obs_i];  // Column index in Jacobian
+    size_t kf_idx = pose_idx[kf_col_idx];      // Actual keyframe index
 
-    size_t cam_idx = pose_idx[i];
-    accu.add(cam_idx, Jp.transpose() * Jp);
+    // Extract Jacobian for this observation (2 rows)
+    auto Jp = storage.block(2 * obs_i, POSE_SIZE * kf_col_idx, 2, POSE_SIZE);
+    accu.add(kf_idx, Jp.transpose() * Jp);
   }
 }
 
@@ -603,9 +655,9 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::scale_Jp_cols(
   VecX jacobian_scaling_reduced(padding_idx + padding_size);
 
   for (size_t i = 0; i < pose_idx.size(); i++) {
-    size_t cam_idx = pose_idx[i];
+    size_t kf_idx = pose_idx[i];  // Keyframe index
     jacobian_scaling_reduced.template segment<POSE_SIZE>(POSE_SIZE * i) =
-        jacobian_scaling.template segment<POSE_SIZE>(POSE_SIZE * cam_idx);
+        jacobian_scaling.template segment<POSE_SIZE>(POSE_SIZE * kf_idx);
   }
   jacobian_scaling_reduced.tail(padding_size).setConstant(0);
 
diff --git a/src/rootba/qr/landmark_block_dynamic.hpp b/src/rootba/qr/landmark_block_dynamic.hpp
index 081da39..a9e476b 100644
--- a/src/rootba/qr/landmark_block_dynamic.hpp
+++ b/src/rootba/qr/landmark_block_dynamic.hpp
@@ -48,13 +48,38 @@ class LandmarkBlockDynamic
 
   inline void allocate_landmark_impl(Landmark& lm) {
     pose_idx_.clear();
-    pose_idx_.reserve(lm.obs.size());
-    for (const auto& [cam_idx, obs] : lm.obs) {
-      pose_idx_.push_back(cam_idx);
+    obs_to_kf_idx_.clear();
+
+    // Build a map from keyframe index to column index in Jacobian
+    std::map<FrameIdx, size_t> kf_to_col_idx;
+
+    pose_idx_.reserve(lm.obs.size());  // Upper bound
+    obs_to_kf_idx_.reserve(lm.obs.size());
+
+    for (const auto& [tcid, obs] : lm.obs) {
+      // TODO@tsantucci: check that the observations are accessed in order?
+      FrameIdx frame_idx = tcid.frame_id;
+
+      // Check if this keyframe is already in pose_idx_
+      auto it = kf_to_col_idx.find(frame_idx);
+      if (it == kf_to_col_idx.end()) {
+        // New keyframe, add it
+        size_t col_idx = pose_idx_.size();
+        pose_idx_.push_back(frame_idx);
+        kf_to_col_idx[frame_idx] = col_idx;
+        obs_to_kf_idx_.push_back(col_idx);
+      } else {
+        // Existing keyframe
+        obs_to_kf_idx_.push_back(it->second);
+      }
     }
 
-    padding_idx_ = pose_idx_.size() * POSE_SIZE;
-    num_rows_ = pose_idx_.size() * 2 + 3;  // residuals and lm damping
+    // Number of unique keyframes observing this landmark
+    size_t num_kfs = pose_idx_.size();
+
+    padding_idx_ = num_kfs * POSE_SIZE;
+    num_rows_ =
+        lm.obs.size() * 2 + 3;  // residuals (2 per obs) and lm damping (3)
 
     size_t pad = padding_idx_ % 4;
     if (pad != 0) {
@@ -74,6 +99,9 @@ class LandmarkBlockDynamic
   inline const std::vector<size_t>& get_pose_idx() const override {
     return pose_idx_;
   }
+  inline const std::vector<size_t>& get_obs_to_kf_idx() const {
+    return obs_to_kf_idx_;
+  }
   inline size_t get_padding_idx() const { return padding_idx_; }
   inline size_t get_padding_size() const { return padding_size_; }
   inline size_t get_lm_idx() const { return lm_idx_; }
@@ -88,7 +116,9 @@ class LandmarkBlockDynamic
   Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
       storage_;
 
-  std::vector<size_t> pose_idx_;
+  std::vector<size_t> pose_idx_;  // Keyframe indices (FrameIdx)
+  std::vector<size_t>
+      obs_to_kf_idx_;  // Maps observation index to column index in J_p
   size_t padding_idx_ = 0;
   size_t padding_size_ = 0;
   size_t lm_idx_ = 0;
diff --git a/src/rootba/qr/linearization_qr.hpp b/src/rootba/qr/linearization_qr.hpp
index 3097014..a38b07f 100644
--- a/src/rootba/qr/linearization_qr.hpp
+++ b/src/rootba/qr/linearization_qr.hpp
@@ -106,8 +106,8 @@ class LinearizationQR : public LinearOperator<Scalar_> {
       num_rows_Q2Tr_ += landmark_blocks_[i]->num_Q2T_rows();
     }
 
-    num_cameras_ = bal_problem_.cameras().size();
-    std::vector<std::mutex>(num_cameras_).swap(pose_mutex_);
+    num_keyframes_ = bal_problem_.keyframes().size();
+    std::vector<std::mutex>(num_keyframes_).swap(pose_mutex_);
   }
 
   // return value `false` indicates numerical failure --> linearization at this
@@ -122,7 +122,8 @@ class LinearizationQR : public LinearOperator<Scalar_> {
     auto body = [&](const tbb::blocked_range<size_t>& range,
                     bool numerically_valid) {
       for (size_t r = range.begin(); r != range.end(); ++r) {
-        landmark_blocks_[r]->linearize_landmark(bal_problem_.cameras());
+        landmark_blocks_[r]->linearize_landmark(bal_problem_.keyframes(),
+                                                bal_problem_.calib());
         numerically_valid &= !landmark_blocks_[r]->is_numerical_failure();
       }
       return numerically_valid;
@@ -145,11 +146,13 @@ class LinearizationQR : public LinearOperator<Scalar_> {
   bool has_pose_damping() const { return pose_damping_diagonal_ > 0; }
 
   size_t num_rows_reduced() const {
-    return has_pose_damping() ? num_rows_Q2Tr_ + num_cameras_ * POSE_SIZE
+    // TODO@tsantucci: isn't this actually num_cameras or num_obs, not
+    // num_keyframes?
+    return has_pose_damping() ? num_rows_Q2Tr_ + num_keyframes_ * POSE_SIZE
                               : num_rows_Q2Tr_;
   }
 
-  size_t num_cols_reduced() const { return num_cameras_ * POSE_SIZE; }
+  size_t num_cols_reduced() const { return num_keyframes_ * POSE_SIZE; }
 
   void perform_qr() {
     auto body = [&](const tbb::blocked_range<size_t>& range) {
@@ -163,7 +166,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
   }
 
   Scalar back_substitute(const VecX& pose_inc) {
-    ROOTBA_ASSERT(pose_inc.size() == signed_cast(num_cameras_ * POSE_SIZE));
+    ROOTBA_ASSERT(pose_inc.size() == signed_cast(num_keyframes_ * POSE_SIZE));
 
     auto body = [&](const tbb::blocked_range<size_t>& range, Scalar l_diff) {
       for (size_t r = range.begin(); r != range.end(); ++r) {
@@ -190,7 +193,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
       num_triplets += lb->num_reduced_cams() * POSE_SIZE * lb->num_Q2T_rows();
     }
     if (has_pose_damping()) {
-      num_triplets += num_cameras_ * POSE_SIZE;
+      num_triplets += num_keyframes_ * POSE_SIZE;
     }
     std::vector<Eigen::Triplet<Scalar>> triplets;
     triplets.reserve(num_triplets);
@@ -203,15 +206,15 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
     // add damping entries
     if (has_pose_damping()) {
-      for (size_t i = 0; i < num_cameras_ * POSE_SIZE; ++i) {
+      for (size_t i = 0; i < num_keyframes_ * POSE_SIZE; ++i) {
         triplets.emplace_back(num_rows_Q2Tr_ + i, i,
                               pose_damping_diagonal_sqrt_);
       }
     }
 
     // build sparse matrix
-    Eigen::SparseMatrix<Scalar, Eigen::RowMajor> res(num_rows_reduced(),
-                                                     POSE_SIZE * num_cameras_);
+    Eigen::SparseMatrix<Scalar, Eigen::RowMajor> res(
+        num_rows_reduced(), POSE_SIZE * num_keyframes_);
     if (!triplets.empty()) {
       res.setFromTriplets(triplets.begin(), triplets.end());
     }
@@ -220,7 +223,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
   }
 
   VecX get_Q2TJp_postmult_x(const VecX& x_pose) const {
-    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));
+    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_keyframes_ * POSE_SIZE));
 
     VecX res(num_rows_reduced());
 
@@ -236,7 +239,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
     tbb::parallel_for(range, body);
 
     if (has_pose_damping()) {
-      res.tail(POSE_SIZE * num_cameras_) =
+      res.tail(POSE_SIZE * num_keyframes_) =
           x_pose.array() * pose_damping_diagonal_sqrt_;
     }
 
@@ -259,14 +262,14 @@ class LinearizationQR : public LinearOperator<Scalar_> {
     auto join = [](const auto& x, const auto& y) { return x + y; };
 
     VecX init;
-    init.setZero(POSE_SIZE * num_cameras_);
+    init.setZero(POSE_SIZE * num_keyframes_);
 
     // go over all host frames
     tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
     VecX res = tbb::parallel_reduce(range, init, body, join);
 
     if (has_pose_damping()) {
-      res += (x_r.tail(POSE_SIZE * num_cameras_).array() *
+      res += (x_r.tail(POSE_SIZE * num_keyframes_).array() *
               pose_damping_diagonal_sqrt_)
                  .matrix();
     }
@@ -292,7 +295,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
   }
 
   VecX get_Q2TJp_T_Q2TJp_mult_x_v0(const VecX& x_pose) const {
-    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));
+    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_keyframes_ * POSE_SIZE));
 
     struct Reductor {
       Reductor(const VecX& x_pose,
@@ -335,14 +338,14 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
   // Version with unordered_map and mutex guarded writes
   VecX get_Q2TJp_T_Q2TJp_mult_x_v1(const VecX& x_pose) const {
-    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));
+    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_keyframes_ * POSE_SIZE));
 
     VecX res;
     res.setZero(x_pose.rows());
 
     auto body = [&](const tbb::blocked_range<size_t>& range) {
       std::unordered_map<int, VecP> partial_sum_map;
-      partial_sum_map.reserve(num_cameras_ / 8);
+      partial_sum_map.reserve(num_keyframes_ / 8);
 
       for (size_t r = range.begin(); r != range.end(); ++r) {
         const auto& lb = landmark_blocks_[r];
@@ -370,7 +373,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
   // Verstion with map and mutex guarded writes
   VecX get_Q2TJp_T_Q2TJp_mult_x_v2(const VecX& x_pose) const {
-    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));
+    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_keyframes_ * POSE_SIZE));
 
     VecX res;
     res.setZero(x_pose.rows());
@@ -404,7 +407,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
   // Verstion with direct mutex guarded writes
   VecX get_Q2TJp_T_Q2TJp_mult_x_v3(const VecX& x_pose) const {
-    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));
+    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_keyframes_ * POSE_SIZE));
 
     VecX res;
     res.setZero(x_pose.rows());
@@ -440,7 +443,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
     auto join = [](const auto& x, const auto& y) { return x + y; };
 
     VecX init;
-    init.setZero(POSE_SIZE * num_cameras_);
+    init.setZero(POSE_SIZE * num_keyframes_);
 
     tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
     VecX res = tbb::parallel_reduce(range, init, body, join);
@@ -477,7 +480,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
       VecX res;
     };
 
-    Reductor r(num_cameras_ * POSE_SIZE, landmark_blocks_);
+    Reductor r(num_keyframes_ * POSE_SIZE, landmark_blocks_);
 
     tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
     tbb::parallel_reduce(range, r);
@@ -516,7 +519,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
       VecX res;
     };
 
-    Reductor r(num_cameras_ * POSE_SIZE, landmark_blocks_);
+    Reductor r(num_keyframes_ * POSE_SIZE, landmark_blocks_);
 
     tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
     tbb::parallel_reduce(range, r);
@@ -554,8 +557,8 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
     if (has_pose_damping()) {
       r.accum.add_diag(
-          num_cameras_, POSE_SIZE,
-          VecX::Constant(num_cameras_ * POSE_SIZE, pose_damping_diagonal_));
+          num_keyframes_, POSE_SIZE,
+          VecX::Constant(num_keyframes_ * POSE_SIZE, pose_damping_diagonal_));
     }
 
     return r.accum.block_diagonal;
@@ -589,8 +592,8 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
     if (has_pose_damping()) {
       r.accum.add_diag(
-          num_cameras_, POSE_SIZE,
-          VecX::Constant(num_cameras_ * POSE_SIZE, pose_damping_diagonal_));
+          num_keyframes_, POSE_SIZE,
+          VecX::Constant(num_keyframes_ * POSE_SIZE, pose_damping_diagonal_));
     }
 
     return r.accum.block_diagonal;
@@ -649,7 +652,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
       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->linearize_landmark(bal_problem.cameras());
+          lb->linearize_landmark(bal_problem.keyframes(), bal_problem.calib());
           if (!lb->is_numerical_failure()) {
             if (precond_block_diagonal_accum) {
               lb->add_Jp_T_Jp_blockdiag(*precond_block_diagonal_accum);
@@ -693,7 +696,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
     const bool compute_precond_block_diagonal = precond_block_diagonal;
 
-    Reductor r(num_cameras_ * POSE_SIZE, compute_precond_block_diagonal,
+    Reductor r(num_keyframes_ * POSE_SIZE, compute_precond_block_diagonal,
                landmark_blocks_, bal_problem_);
 
     tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
@@ -786,7 +789,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
 
     const bool compute_precond_block_diagonal = precond_block_diagonal;
 
-    Reductor r(num_cameras_ * POSE_SIZE, lambda, jacobian_scaling,
+    Reductor r(num_keyframes_ * POSE_SIZE, lambda, jacobian_scaling,
                compute_precond_block_diagonal, landmark_blocks_);
 
     tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
@@ -796,8 +799,8 @@ class LinearizationQR : public LinearOperator<Scalar_> {
     if (has_pose_damping()) {
       if (compute_precond_block_diagonal) {
         r.precond_block_diagonal_accum->add_diag(
-            num_cameras_, POSE_SIZE,
-            VecX::Constant(num_cameras_ * POSE_SIZE, pose_damping_diagonal_));
+            num_keyframes_, POSE_SIZE,
+            VecX::Constant(num_keyframes_ * POSE_SIZE, pose_damping_diagonal_));
       }
     }
 
@@ -836,7 +839,7 @@ class LinearizationQR : public LinearOperator<Scalar_> {
   Scalar pose_damping_diagonal_ = 0;
   Scalar pose_damping_diagonal_sqrt_ = 0;
 
-  size_t num_cameras_ = 0;
+  size_t num_keyframes_ = 0;
   size_t num_rows_Q2Tr_ = 0;
 };
 
diff --git a/src/rootba/solver/linearizor_qr.cpp b/src/rootba/solver/linearizor_qr.cpp
index 7a0d533..487480b 100644
--- a/src/rootba/solver/linearizor_qr.cpp
+++ b/src/rootba/solver/linearizor_qr.cpp
@@ -55,7 +55,7 @@ LinearizorQR<Scalar_>::LinearizorQR(BalProblem<Scalar>& bal_problem,
                                     SolverSummary* summary)
     : LinearizorBase<Scalar>(bal_problem, options, summary) {
   // set options
-  typename LinearizationQR<Scalar, 9>::Options lqr_options;
+  typename LinearizationQR<Scalar, 6>::Options lqr_options;
 
   lqr_options.lb_options.use_householder =
       options_.use_householder_marginalization;
@@ -68,7 +68,7 @@ LinearizorQR<Scalar_>::LinearizorQR(BalProblem<Scalar>& bal_problem,
   lqr_options.lb_options.residual_options = options_.residual;
 
   // create linearization object
-  lqr_ = std::make_unique<LinearizationQR<Scalar, 9>>(bal_problem, lqr_options);
+  lqr_ = std::make_unique<LinearizationQR<Scalar, 6>>(bal_problem, lqr_options);
 }
 
 template <class Scalar_>
@@ -203,7 +203,7 @@ typename LinearizorQR<Scalar_>::VecX LinearizorQR<Scalar_>::solve(
   std::unique_ptr<Preconditioner<Scalar>> precond;
   {
     Timer timer;
-    const int num_cams = bal_problem_.num_cameras();
+    const int num_cams = bal_problem_.num_keyframes();
     const int pose_size = lqr_->POSE_SIZE;
     if (options_.preconditioner_type ==
         SolverOptions::PreconditionerType::JACOBI) {
@@ -280,10 +280,8 @@ Scalar_ LinearizorQR<Scalar_>::apply(VecX&& inc) {
   inc.array() *= pose_jacobian_scaling_.array();
 
   // update cameras
-  for (size_t i = 0; i < bal_problem_.cameras().size(); i++) {
-    bal_problem_.cameras()[i].apply_inc_pose(inc.template segment<6>(i * 9));
-    bal_problem_.cameras()[i].apply_inc_intrinsics(
-        inc.template segment<3>(i * 9 + 6));
+  for (size_t i = 0; i < bal_problem_.keyframes().size(); i++) {
+    bal_problem_.keyframes()[i].apply_inc_pose(inc.template segment<6>(i * 6));
   }
   IF_SET(it_summary_)->update_cameras_time_in_seconds = timer.elapsed();
 
diff --git a/src/rootba/solver/linearizor_qr.hpp b/src/rootba/solver/linearizor_qr.hpp
index 4402a18..0d80d35 100644
--- a/src/rootba/solver/linearizor_qr.hpp
+++ b/src/rootba/solver/linearizor_qr.hpp
@@ -48,7 +48,7 @@ class LinearizorQR : public LinearizorBase<Scalar_> {
  public:
   using Scalar = Scalar_;
   using Base = LinearizorBase<Scalar>;
-  constexpr static int POSE_SIZE = 9;
+  constexpr static int POSE_SIZE = 6;
 
   using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
 
diff --git a/stereo_optimization.md b/stereo_optimization.md
new file mode 100644
index 0000000..216671b
--- /dev/null
+++ b/stereo_optimization.md
@@ -0,0 +1,48 @@
+## Intro
+
+This project has been adapted to preserve the stereo constraints of a multicamera setup.
+
+Only square root optimization is implemented.
+
+To support this, the Keyframe structure was added to represent the set of frames corresponding to the same timestamp. It contains the IMU pose (T_w_i). Using the Calibration object, attribute of `BalProblem`, one can access the transformation from the IMU to the corresponding cameras (T_c_i). Since the problem state contains only keyframe poses, we must compute errors, residuals, and Jacobians with respect to the IMU pose.
+
+Additionally, intrinsics are no longer optimized and must be provided using a calibration file with the `--calibration-file` argument.
+
+Because we now optimize IMU poses, the structure of a landmark block has been modified. Previously, each camera could have only one observation for a landmark, meaning that in a given landmark block, the column of a camera had only two non-zero rows. Now, since the columns of J_p represent IMU poses, each column can have up to 2*n non-zero rows, where n is the number of cameras in the setup.
+
+Another conceptual difference is that Jacobians were previously computed with respect to the camera pose (d_res_d_cam), but are now computed with respect to the IMU pose (d_res_d_imu). The (possibly incorrect) derivation of this Jacobian can be found in the `linearize_landmark` method of `landmark_block_base.ipp`.
+
+Note that since we no longer optimize intrinsics and use a fixed T_c_i, we cannot normalize the problem, which requires us to use the `--no-normalize` argument.
+
+## How to run
+
+Compile with:
+
+```bash
+./scripts/build-rootba.sh
+```
+
+A list of pre-exported maps is available in `data/stereo`, along with the calibration files used to generate them in `data/calib`. You can run it with:
+
+```bash
+./bin/bal_gui --input data/stereo/MOO01.json --solver-type SQUARE_ROOT --calibration-file data/calib/msdmo_calib.json --no-normalize
+```
+
+## Problems to check
+
+### 1
+
+The linear model does not appear to correctly represent the real model. This can be observed in nearly every dataset. The l_diff indicates that the cost should decrease, but f_diff shows the opposite. When both decrease, l_diff is not sufficiently similar to f_diff. This suggests a potential issue with the linearization. This leads to many rejected iterations and causes lambda to increase to very large values. For example:
+
+```bash
+	[EVAL] f_diff -4.8196e+05 l_diff 5.5251e+05 step_quality -8.7231e-01 ri1 5.6967e+05 ri2 1.0516e+06
+```
+
+### 2
+
+In some cases, such as with `MOO13.json`, the sum of the weighted error decreases while the mean residual error increases. This should not occur, especially when the number of observations remains constant before and after optimization.
+
+```bash
+Iteration 0, error: 5.6967e+05 (mean res: 1.82, num: 33353), error valid: 5.6967e+05 (mean res: 1.82, num: 33353)
+Final Cost: error: 3.9156e+05 (mean res: 2.36, num: 33353), error valid: 3.9156e+05 (mean res: 2.36, num: 33353)
+```