From 2d8cb75000a8bb48c9762677c6dbce206523edb4 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Fri, 9 Aug 2024 18:15:29 +0200
Subject: [PATCH 01/36] frame impl.

---
 src/colmap/scene/frame.h  | 150 ++++++++++++++++++++++++++++++++++++++
 src/colmap/scene/image.cc |   4 +-
 2 files changed, 152 insertions(+), 2 deletions(-)
 create mode 100644 src/colmap/scene/frame.h

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
new file mode 100644
index 0000000000..2229f5f67e
--- /dev/null
+++ b/src/colmap/scene/frame.h
@@ -0,0 +1,150 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS 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 "colmap/geometry/pose.h"
+#include "colmap/scene/camera.h"
+#include "colmap/scene/reconstruction.h"
+#include "colmap/util/types.h"
+
+#include <unordered_map>
+#include <vector>
+
+namespace colmap {
+
+typedef uint64_t frame_t;
+typedef uint32_t rig_t;
+
+enum class SensorType {
+  Camera = 0,
+  IMU = 1,
+  Location = 2, // include GNSS, radios, compass, etc.
+};
+
+typedef data_t std::pair<SensorType, uint64_t>;
+
+class RigCalibration {
+ public:
+  inline rig_t RigId() const {
+    return rig_id_;
+  }
+
+  inline data_t RefDataId() const {
+    return ref_data_id_;
+  }
+
+  inline bool HasData(data_t data_id) const {
+    if (data_id == ref_data_id_)
+      return true;
+    else
+      return sensor_from_rig_.find(data_id) != sensor_from_rig_.end();
+  }
+
+  inline bool IsReference(data_t data_id) const {
+    return data_id == ref_data_id_;
+  }
+
+  inline size_t CountData() const {
+    return sensor_from_rig_.size() + 1;
+  }
+
+  inline Rigi3d& GetSensorFromRig(data_t data_id) {
+    THROW_CHECK(!IsReference(data_id));
+    auto it = sensor_from_rig_.find(data_id);
+    if (it == sensor_from_rig_.end())
+      LOG(FATAL_THROW) << StringPrintf("Data id (%d) not found in the rig calibration", data_id);
+    return sensor_from_rig_.at(data_id);
+  }
+
+  inline const Rigid3d GetSensorFromRig(data_t data_id) const {
+    if (IsReference(data_id))
+      return Rigid3d(); // return identity
+    else {
+      auto it = sensor_from_rig_.find(data_id);
+      if (it == sensor_from_rig_.end())
+        LOG(FATAL_THROW) << StringPrintf("Data id (%d) not found in the rig calibration", data_id);
+      return sensor_from_rig_.at(data_id);
+    }
+  }
+
+ private:
+  rig_t rig_id_;
+  data_t ref_data_id_;
+  std::unordered_map<data_t, Rigid3d> sensor_from_rig_;
+};
+
+struct Frame {
+ public:
+  inline frame_t FrameId() const { return frame_id_; }
+  inline std::unordered_set<data_t>& DataIds() { return data_ids_; }
+  inline const std::unordered_set<data_t>& DataIds() const { return data_ids_; }
+  inline std::shared_ptr<RigCalibration> RigCalibration() const {
+    return rig_calibration_;
+  }
+
+  inline bool HasData(data_t data_id) const {
+    return data_ids_.find(data_id) != data_ids_.end();
+  }
+
+  inline bool HasRigCalibration() const {
+    if (rig_calibration_ == nullptr)
+      return false;
+    else
+      return rig_calibration_.CountData() > 1;
+  }
+
+  inline const Rigid3d& FrameFromWorld() const { return frame_from_world_; }
+  inline Rigid3d& FrameFromWorld() { return frame_from_world_; }
+
+  inline const Rigid3d& SensorFromWorld() const {
+    THROW_CHECK(!HasRigCalibration());
+    return FrameFromWorld();
+  }
+  inline Rigid3d SensorFromWorld() const {
+    THROW_CHECK(!HasRigCalibration());
+    return FrameFromWorld();
+  }
+
+  inline Rigid3d SensorFromWorld(data_t data_id) const {
+    THROW_CHECK(HasData(data_id));
+    if (!HasRigCalibration()) {
+      return SensorFromWorld();
+    }
+    return rig_calibration_.GetSensorFromRig(data_id) * frame_from_world_;
+  }
+
+ private:
+  frame_t frame_id_;
+  std::unordered_set<data_t> data_ids_;
+  Rigid3d frame_from_world_;
+  std::shared_ptr<RigCalibration> rig_calibration_ = nullptr; // nullptr if no rig
+};
+
+}  // namespace colmap
diff --git a/src/colmap/scene/image.cc b/src/colmap/scene/image.cc
index 8e2740d568..45ac7012e6 100644
--- a/src/colmap/scene/image.cc
+++ b/src/colmap/scene/image.cc
@@ -87,11 +87,11 @@ bool Image::HasPoint3D(const point3D_t point3D_id) const {
 }
 
 Eigen::Vector3d Image::ProjectionCenter() const {
-  return cam_from_world_.rotation.inverse() * -cam_from_world_.translation;
+  return CamFromWorld().rotation.inverse() * -CamFromWorld().translation;
 }
 
 Eigen::Vector3d Image::ViewingDirection() const {
-  return cam_from_world_.rotation.toRotationMatrix().row(2);
+  return CamFromWorld().rotation.toRotationMatrix().row(2);
 }
 
 }  // namespace colmap

From 350eb86ee9b1517e2cccbbf6fb05da1f2d286996 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 04:00:10 +0200
Subject: [PATCH 02/36] update.

---
 src/colmap/scene/CMakeLists.txt |   1 +
 src/colmap/scene/frame.h        | 253 ++++++++++++++++++++++----------
 src/colmap/scene/image.h        |  29 ++++
 src/colmap/util/types.h         |   8 +
 4 files changed, 217 insertions(+), 74 deletions(-)

diff --git a/src/colmap/scene/CMakeLists.txt b/src/colmap/scene/CMakeLists.txt
index feb4ac051c..4023d40e09 100644
--- a/src/colmap/scene/CMakeLists.txt
+++ b/src/colmap/scene/CMakeLists.txt
@@ -38,6 +38,7 @@ COLMAP_ADD_LIBRARY(
         correspondence_graph.h correspondence_graph.cc
         database.h database.cc
         database_cache.h database_cache.cc
+        frame.h
         image.h image.cc
         point2d.h
         point3d.h
diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 2229f5f67e..ff805e28e2 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -39,112 +39,217 @@
 
 namespace colmap {
 
-typedef uint64_t frame_t;
-typedef uint32_t rig_t;
-
+// Sensor type
 enum class SensorType {
   Camera = 0,
   IMU = 1,
   Location = 2, // include GNSS, radios, compass, etc.
 };
 
-typedef data_t std::pair<SensorType, uint64_t>;
+// Unique identifier of the sensor
+typedef std::pair<SensorType, uint32_t> sensor_t;
+
+// Unique identifier of the data point from a sensor
+typedef std::pair<sensor_t, uint64_t> data_t;
 
+// Rig calibration storing the sensor from rig transformation
 class RigCalibration {
  public:
-  inline rig_t RigId() const {
-    return rig_id_;
-  }
+  // Access the unique identifier of the rig
+  inline rig_t RigId() const;
+  inline void SetRigId(rig_t rig_id);
 
-  inline data_t RefDataId() const {
-    return ref_data_id_;
-  }
+  // Add sensor into the rig
+  // ``AddReferenceSensor`` needs to called first before all the ``AddSensor`` operation
+  inline void AddReferenceSensor(sensor_t ref_sensor_id);
+  inline void AddSensor(sensor_t sensor_id, Rigid3d sensor_from_rig = Rigid3d(), bool is_fixed = false);
 
-  inline bool HasData(data_t data_id) const {
-    if (data_id == ref_data_id_)
-      return true;
-    else
-      return sensor_from_rig_.find(data_id) != sensor_from_rig_.end();
-  }
+  // Check whether the sensor exists in the rig
+  inline bool HasSensor(sensor_t sensor_id) const;
 
-  inline bool IsReference(data_t data_id) const {
-    return data_id == ref_data_id_;
-  }
+  // Count the number of sensors available in the rig
+  inline size_t NumSensors() const;
 
-  inline size_t CountData() const {
-    return sensor_from_rig_.size() + 1;
-  }
+  // Access the reference sensor id (default to be the first added sensor)
+  inline sensor_t RefSensorId() const;
 
-  inline Rigi3d& GetSensorFromRig(data_t data_id) {
-    THROW_CHECK(!IsReference(data_id));
-    auto it = sensor_from_rig_.find(data_id);
-    if (it == sensor_from_rig_.end())
-      LOG(FATAL_THROW) << StringPrintf("Data id (%d) not found in the rig calibration", data_id);
-    return sensor_from_rig_.at(data_id);
-  }
+  // Check if the sensor is the reference sensor of the rig
+  inline bool IsReference(sensor_t sensor_id) const;
 
-  inline const Rigid3d GetSensorFromRig(data_t data_id) const {
-    if (IsReference(data_id))
-      return Rigid3d(); // return identity
-    else {
-      auto it = sensor_from_rig_.find(data_id);
-      if (it == sensor_from_rig_.end())
-        LOG(FATAL_THROW) << StringPrintf("Data id (%d) not found in the rig calibration", data_id);
-      return sensor_from_rig_.at(data_id);
-    }
-  }
+  // Access sensor from rig transformations
+  inline Rigi3d& SensorFromRig(sensor_t sensor_id);
+  inline const Rigid3d& SensorFromRig(sensor_t sensor_id) const;
 
  private:
+  // Unique identifier of the device.
   rig_t rig_id_;
-  data_t ref_data_id_;
-  std::unordered_map<data_t, Rigid3d> sensor_from_rig_;
+
+  // Reference sensor id which has the identity transformation to the rig.
+  sensor_t ref_sensor_id_;
+
+  // list of sensors
+  std::unordered_set<sensor_t> sensor_ids_;
+
+  // sensor_from_rig transformation.
+  std::unordered_map<sensor_t, Rigid3d> map_sensor_from_rig_;
+  std::unordered_map<sensor_t, bool> is_fixed_sensor_from_rig_; // for optimization
 };
 
 struct Frame {
  public:
-  inline frame_t FrameId() const { return frame_id_; }
-  inline std::unordered_set<data_t>& DataIds() { return data_ids_; }
-  inline const std::unordered_set<data_t>& DataIds() const { return data_ids_; }
-  inline std::shared_ptr<RigCalibration> RigCalibration() const {
-    return rig_calibration_;
-  }
+  // Access the unique identifier of the frame
+  inline frame_t FrameId() const;
+  inline void SetFrameId(frame_t frame_id);
 
-  inline bool HasData(data_t data_id) const {
-    return data_ids_.find(data_id) != data_ids_.end();
-  }
+  // Access data ids
+  inline std::unordered_set<data_t>& DataIds();
+  inline const std::unordered_set<data_t>& DataIds() const;
+  inline void AddData(data_t data_id);
 
-  inline bool HasRigCalibration() const {
-    if (rig_calibration_ == nullptr)
-      return false;
-    else
-      return rig_calibration_.CountData() > 1;
-  }
+  // Check whether the data id is existent in the frame
+  inline bool HasData(data_t data_id) const;
 
-  inline const Rigid3d& FrameFromWorld() const { return frame_from_world_; }
-  inline Rigid3d& FrameFromWorld() { return frame_from_world_; }
+  // Access the unique identifier of the rig
+  inline rig_t RigId() const;
+  inline void SetRigId(rig_t rig_id);
 
-  inline const Rigid3d& SensorFromWorld() const {
-    THROW_CHECK(!HasRigCalibration());
-    return FrameFromWorld();
-  }
-  inline Rigid3d SensorFromWorld() const {
-    THROW_CHECK(!HasRigCalibration());
-    return FrameFromWorld();
-  }
+  // Check if the frame has a non-trivial rig calibration
+  inline std::shared_ptr<RigCalibration> RigCalibration() const;
+  inline void SetRigCalibration(const std::shared_ptr<RigCalibration>& rig_calibration);
+  inline bool HasRigCalibration() const;
 
-  inline Rigid3d SensorFromWorld(data_t data_id) const {
-    THROW_CHECK(HasData(data_id));
-    if (!HasRigCalibration()) {
-      return SensorFromWorld();
-    }
-    return rig_calibration_.GetSensorFromRig(data_id) * frame_from_world_;
-  }
+  // Access the frame from world transformation
+  inline const Rigid3d& FrameFromWorld() const;
+  inline Rigid3d& FrameFromWorld();
+  inline const Rigid3d& SensorFromWorld() const;
+  inline Rigid3d& SensorFromWorld();
+
+  // Get the sensor from world transformation
+  inline Rigid3d SensorFromWorld(data_t data_id) const;
 
  private:
   frame_t frame_id_;
   std::unordered_set<data_t> data_ids_;
+
+  // Store the frame_from_world transformation and an optional rig calibration. If the rig calibration is a nullptr, the frame becomes a single sensor case, where rig modeling is no longer needed.
   Rigid3d frame_from_world_;
-  std::shared_ptr<RigCalibration> rig_calibration_ = nullptr; // nullptr if no rig
+  rig_t rig_id_ = kInvalidRigId;
+  std::shared_ptr<RigCalibration> rig_calibration_ = nullptr; 
 };
 
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+rig_t RigCalibration::RigId() const {
+  return rig_id_;
+}
+
+void RigCalibration::SetRigId(rig_t rig_id) {
+  rig_id_ = rig_id;
+}
+
+bool RigCalibration::HasSensor(sensor_t sensor_id) const {
+  return sensor_ids_.find(sensor_id) != sensor_ids_.end();
+}
+
+size_t RigCalibration::NumSensors() const {
+  return sensor_ids_.size();
+}
+
+void AddReferenceSensor(sensor_t ref_sensor_id) {
+  THROW_CHECK(sensor_ids_.empty()); // The reference sensor must be added first
+  ref_sensor_id_ = ref_sensor_id;
+  sensor_ids_.insert(ref_sensor_id);
+}
+
+sensor_t RigCalibration::RefSensorId() const {
+  return ref_sensor_id_;
+}
+
+bool RigCalibration::IsReference(sensor_t sensor_id) const {
+  return sensor_id == ref_sensor_id_;
+}
+
+void RigCalibration::AddSensor(sensor_t sensor_id, Rigid3d sensor_from_rig, bool is_fixed) {
+  if (NumSensors() == 0)
+    LOG(FATAL_THROW) << "The reference sensor needs to added first before any sensor being added.";
+  else {
+    if (HasSensor(sensor_id))
+      LOG(FATAL_THROW) << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig", sensor_id.first, sensor_id.second);
+    map_sensor_from_rig_.emplace(sensor_id, sensor_from_rig);
+    is_fixed_sensor_from_rig_.emplace(sensor_id, is_fixed);
+    sensor_ids_.insert(sensor_id);
+  }
+}
+
+Rigi3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsRereference(sensor_id))
+  auto it = map_sensor_from_rig_.find(sensor_id);
+  if (it == map_sensor_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf("Sensor id (%d, %d) not found in the rig calibration", sensor_id.first, sensor_id.second);
+  return map_sensor_from_rig_.at(sensor_id);
+}
+
+const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
+  THROW_CHECK(!IsRereference(sensor_id))
+  auto it = map_sensor_from_rig_.find(sensor_id);
+  if (it == map_sensor_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf("Sensor id (%d, %d) not found in the rig calibration", sensor_id.first, sensor_id.second);
+  return map_sensor_from_rig_.at(sensor_id);
+}
+
+frame_t Frame::FrameId(frame_t frame_id) const { return frame_id_; }
+
+std::unordered_set<data_t>& Frame::DataIds() { return data_ids_; }
+
+const std::unordered_set<data_t>& Frame::DataIds() const { return data_ids_; }
+
+void Frame::AddData(data_t data_id) {
+  data_ids_.insert(data_id);
+}
+
+bool Frame::HasData(data_t data_id) const {
+  return data_ids_.find(data_id) != data_ids_.end();
+}
+
+rig_t Frame::RigId() const { return rig_id_; }
+
+void Frame::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
+
+std::shared_ptr<RigCalibration> Frame::RigCalibration() const {
+  return rig_calibration_;
+}
+
+void Frame::SetRigCalibration(const std::shared_ptr<RigCalibration>& rig_calibration) {
+  rig_calibration_ = rig_calibration;
+}
+
+bool Frame::HasRigCalibration() const {
+  if (rig_calibration_ == nullptr)
+    return false;
+  else
+    return rig_calibration_.NumSensors() > 1;
+}
+
+const Rigid3d& Frame::FrameFromWorld() const { return frame_from_world_; }
+Rigid3d& Frame::FrameFromWorld() { return frame_from_world_; }
+
+const Rigid3d& Frame::SensorFromWorld() const {
+  THROW_CHECK(!HasRigCalibration());
+  return FrameFromWorld();
+}
+Rigid3d& Frame::SensorFromWorld() {
+  THROW_CHECK(!HasRigCalibration());
+  return FrameFromWorld();
+}
+
+Rigid3d Frame::SensorFromWorld(data_t data_id) const {
+  THROW_CHECK(HasData(data_id));
+  if (!HasRigCalibration() || rig_calibration_->IsReference(data_id.first)) {
+    return SensorFromWorld();
+  }
+  return rig_calibration_->SensorFromRig(data_id.first) * frame_from_world_;
+}
+
 }  // namespace colmap
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index e6c7f70873..4acb353d5d 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -32,6 +32,7 @@
 #include "colmap/geometry/rigid3.h"
 #include "colmap/math/math.h"
 #include "colmap/scene/camera.h"
+#include "colmap/scene/frame.h"
 #include "colmap/scene/point2d.h"
 #include "colmap/scene/visibility_pyramid.h"
 #include "colmap/util/eigen_alignment.h"
@@ -79,6 +80,13 @@ class Image {
   // are part of a 3D point track.
   inline point2D_t NumPoints3D() const;
 
+  // [Optional] The corresponding frame of the image
+  inline frame_t FrameId() const;
+  inline void SetFrameId(frame_t frame_id);
+  inline std::shared_ptr<class Frame> Frame() const;
+  inline void SetFrame(const std::shared_ptr<class Frame>& frame);
+  inline bool HasFrame() const;
+
   // World to camera pose.
   inline const Rigid3d& CamFromWorld() const;
   inline Rigid3d& CamFromWorld();
@@ -124,7 +132,12 @@ class Image {
   // where `point3D_id != kInvalidPoint3DId`.
   point2D_t num_points3D_;
 
+  // [Optional] The corresponding frame (rig) of the image
+  frame_t frame_id_;
+  std::shared_ptr<class Frame> frame_ = nullptr;
+
   // The pose of the image, defined as the transformation from world to camera.
+  // Only useful when the corresponding frame (rig) does not exist.
   Rigid3d cam_from_world_;
 
   // All image points, including points that are not part of a 3D point track.
@@ -164,6 +177,22 @@ point2D_t Image::NumPoints2D() const {
 
 point2D_t Image::NumPoints3D() const { return num_points3D_; }
 
+frame_t Image::FrameId() const { return frame_id_; }
+
+void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
+
+std::shared_ptr<class Frame> Image::Frame() {
+  return frame_;
+}
+
+void Image::SetFrame(const std::shared_ptr<class Frame>& frame) {
+  frame_ = frame;
+}
+
+bool Image::HasFrame() const {
+  return frame_ != nullptr;
+}
+
 const Rigid3d& Image::CamFromWorld() const { return cam_from_world_; }
 
 Rigid3d& Image::CamFromWorld() { return cam_from_world_; }
diff --git a/src/colmap/util/types.h b/src/colmap/util/types.h
index 9ea0bed8a1..7601fb0dc2 100644
--- a/src/colmap/util/types.h
+++ b/src/colmap/util/types.h
@@ -81,6 +81,12 @@ typedef uint32_t camera_t;
 // Unique identifier for images.
 typedef uint32_t image_t;
 
+// Unique identifier for frames.
+typedef uint64_t frame_t;
+
+// Unique identiier for rigs.
+typedef uint32_t rig_t;
+
 // Each image pair gets a unique ID, see `Database::ImagePairToPairId`.
 typedef uint64_t image_pair_t;
 
@@ -95,6 +101,8 @@ typedef uint64_t point3D_t;
 // Values for invalid identifiers or indices.
 const camera_t kInvalidCameraId = std::numeric_limits<camera_t>::max();
 const image_t kInvalidImageId = std::numeric_limits<image_t>::max();
+const frame_t kInvalidFrameId = std::numeric_limits<frame_t>::max();
+const frame_t kInvalidRigId = std::numeric_limits<rig_t>::max();
 const image_pair_t kInvalidImagePairId =
     std::numeric_limits<image_pair_t>::max();
 const point2D_t kInvalidPoint2DIdx = std::numeric_limits<point2D_t>::max();

From a370d99004918487362958512c0c3195a47c8935 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 04:00:48 +0200
Subject: [PATCH 03/36] formatting.

---
 src/colmap/scene/frame.h | 68 +++++++++++++++++++++++-----------------
 src/colmap/scene/image.h |  8 ++---
 2 files changed, 41 insertions(+), 35 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index ff805e28e2..4173a0bc41 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -43,7 +43,7 @@ namespace colmap {
 enum class SensorType {
   Camera = 0,
   IMU = 1,
-  Location = 2, // include GNSS, radios, compass, etc.
+  Location = 2,  // include GNSS, radios, compass, etc.
 };
 
 // Unique identifier of the sensor
@@ -60,9 +60,12 @@ class RigCalibration {
   inline void SetRigId(rig_t rig_id);
 
   // Add sensor into the rig
-  // ``AddReferenceSensor`` needs to called first before all the ``AddSensor`` operation
+  // ``AddReferenceSensor`` needs to called first before all the ``AddSensor``
+  // operation
   inline void AddReferenceSensor(sensor_t ref_sensor_id);
-  inline void AddSensor(sensor_t sensor_id, Rigid3d sensor_from_rig = Rigid3d(), bool is_fixed = false);
+  inline void AddSensor(sensor_t sensor_id,
+                        Rigid3d sensor_from_rig = Rigid3d(),
+                        bool is_fixed = false);
 
   // Check whether the sensor exists in the rig
   inline bool HasSensor(sensor_t sensor_id) const;
@@ -92,7 +95,8 @@ class RigCalibration {
 
   // sensor_from_rig transformation.
   std::unordered_map<sensor_t, Rigid3d> map_sensor_from_rig_;
-  std::unordered_map<sensor_t, bool> is_fixed_sensor_from_rig_; // for optimization
+  std::unordered_map<sensor_t, bool>
+      is_fixed_sensor_from_rig_;  // for optimization
 };
 
 struct Frame {
@@ -115,7 +119,8 @@ struct Frame {
 
   // Check if the frame has a non-trivial rig calibration
   inline std::shared_ptr<RigCalibration> RigCalibration() const;
-  inline void SetRigCalibration(const std::shared_ptr<RigCalibration>& rig_calibration);
+  inline void SetRigCalibration(
+      const std::shared_ptr<RigCalibration>& rig_calibration);
   inline bool HasRigCalibration() const;
 
   // Access the frame from world transformation
@@ -131,52 +136,52 @@ struct Frame {
   frame_t frame_id_;
   std::unordered_set<data_t> data_ids_;
 
-  // Store the frame_from_world transformation and an optional rig calibration. If the rig calibration is a nullptr, the frame becomes a single sensor case, where rig modeling is no longer needed.
+  // Store the frame_from_world transformation and an optional rig calibration.
+  // If the rig calibration is a nullptr, the frame becomes a single sensor
+  // case, where rig modeling is no longer needed.
   Rigid3d frame_from_world_;
   rig_t rig_id_ = kInvalidRigId;
-  std::shared_ptr<RigCalibration> rig_calibration_ = nullptr; 
+  std::shared_ptr<RigCalibration> rig_calibration_ = nullptr;
 };
 
 ////////////////////////////////////////////////////////////////////////////////
 // Implementation
 ////////////////////////////////////////////////////////////////////////////////
 
-rig_t RigCalibration::RigId() const {
-  return rig_id_;
-}
+rig_t RigCalibration::RigId() const { return rig_id_; }
 
-void RigCalibration::SetRigId(rig_t rig_id) {
-  rig_id_ = rig_id;
-}
+void RigCalibration::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
 
 bool RigCalibration::HasSensor(sensor_t sensor_id) const {
   return sensor_ids_.find(sensor_id) != sensor_ids_.end();
 }
 
-size_t RigCalibration::NumSensors() const {
-  return sensor_ids_.size();
-}
+size_t RigCalibration::NumSensors() const { return sensor_ids_.size(); }
 
 void AddReferenceSensor(sensor_t ref_sensor_id) {
-  THROW_CHECK(sensor_ids_.empty()); // The reference sensor must be added first
+  THROW_CHECK(sensor_ids_.empty());  // The reference sensor must be added first
   ref_sensor_id_ = ref_sensor_id;
   sensor_ids_.insert(ref_sensor_id);
 }
 
-sensor_t RigCalibration::RefSensorId() const {
-  return ref_sensor_id_;
-}
+sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
 
 bool RigCalibration::IsReference(sensor_t sensor_id) const {
   return sensor_id == ref_sensor_id_;
 }
 
-void RigCalibration::AddSensor(sensor_t sensor_id, Rigid3d sensor_from_rig, bool is_fixed) {
+void RigCalibration::AddSensor(sensor_t sensor_id,
+                               Rigid3d sensor_from_rig,
+                               bool is_fixed) {
   if (NumSensors() == 0)
-    LOG(FATAL_THROW) << "The reference sensor needs to added first before any sensor being added.";
+    LOG(FATAL_THROW) << "The reference sensor needs to added first before any "
+                        "sensor being added.";
   else {
     if (HasSensor(sensor_id))
-      LOG(FATAL_THROW) << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig", sensor_id.first, sensor_id.second);
+      LOG(FATAL_THROW) << StringPrintf(
+          "Sensor id (%d, %d) is inserted twice into the rig",
+          sensor_id.first,
+          sensor_id.second);
     map_sensor_from_rig_.emplace(sensor_id, sensor_from_rig);
     is_fixed_sensor_from_rig_.emplace(sensor_id, is_fixed);
     sensor_ids_.insert(sensor_id);
@@ -187,7 +192,10 @@ Rigi3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
   THROW_CHECK(!IsRereference(sensor_id))
   auto it = map_sensor_from_rig_.find(sensor_id);
   if (it == map_sensor_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf("Sensor id (%d, %d) not found in the rig calibration", sensor_id.first, sensor_id.second);
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.first,
+        sensor_id.second);
   return map_sensor_from_rig_.at(sensor_id);
 }
 
@@ -195,7 +203,10 @@ const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
   THROW_CHECK(!IsRereference(sensor_id))
   auto it = map_sensor_from_rig_.find(sensor_id);
   if (it == map_sensor_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf("Sensor id (%d, %d) not found in the rig calibration", sensor_id.first, sensor_id.second);
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.first,
+        sensor_id.second);
   return map_sensor_from_rig_.at(sensor_id);
 }
 
@@ -205,9 +216,7 @@ std::unordered_set<data_t>& Frame::DataIds() { return data_ids_; }
 
 const std::unordered_set<data_t>& Frame::DataIds() const { return data_ids_; }
 
-void Frame::AddData(data_t data_id) {
-  data_ids_.insert(data_id);
-}
+void Frame::AddData(data_t data_id) { data_ids_.insert(data_id); }
 
 bool Frame::HasData(data_t data_id) const {
   return data_ids_.find(data_id) != data_ids_.end();
@@ -221,7 +230,8 @@ std::shared_ptr<RigCalibration> Frame::RigCalibration() const {
   return rig_calibration_;
 }
 
-void Frame::SetRigCalibration(const std::shared_ptr<RigCalibration>& rig_calibration) {
+void Frame::SetRigCalibration(
+    const std::shared_ptr<RigCalibration>& rig_calibration) {
   rig_calibration_ = rig_calibration;
 }
 
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 4acb353d5d..137c08309c 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -181,17 +181,13 @@ frame_t Image::FrameId() const { return frame_id_; }
 
 void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
-std::shared_ptr<class Frame> Image::Frame() {
-  return frame_;
-}
+std::shared_ptr<class Frame> Image::Frame() { return frame_; }
 
 void Image::SetFrame(const std::shared_ptr<class Frame>& frame) {
   frame_ = frame;
 }
 
-bool Image::HasFrame() const {
-  return frame_ != nullptr;
-}
+bool Image::HasFrame() const { return frame_ != nullptr; }
 
 const Rigid3d& Image::CamFromWorld() const { return cam_from_world_; }
 

From bc2414c9bcf51d9111850bec1b29b46b9e8eaa74 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 04:26:17 +0200
Subject: [PATCH 04/36] update.

---
 src/colmap/scene/frame.h | 46 +++++++++++++++++++++-------------------
 src/colmap/scene/image.h |  8 +++----
 2 files changed, 28 insertions(+), 26 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 4173a0bc41..d98be85f90 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -35,6 +35,7 @@
 #include "colmap/util/types.h"
 
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 namespace colmap {
@@ -80,7 +81,7 @@ class RigCalibration {
   inline bool IsReference(sensor_t sensor_id) const;
 
   // Access sensor from rig transformations
-  inline Rigi3d& SensorFromRig(sensor_t sensor_id);
+  inline Rigid3d& SensorFromRig(sensor_t sensor_id);
   inline const Rigid3d& SensorFromRig(sensor_t sensor_id) const;
 
  private:
@@ -99,7 +100,7 @@ class RigCalibration {
       is_fixed_sensor_from_rig_;  // for optimization
 };
 
-struct Frame {
+class Frame {
  public:
   // Access the unique identifier of the frame
   inline frame_t FrameId() const;
@@ -118,9 +119,9 @@ struct Frame {
   inline void SetRigId(rig_t rig_id);
 
   // Check if the frame has a non-trivial rig calibration
-  inline std::shared_ptr<RigCalibration> RigCalibration() const;
+  inline const std::shared_ptr<class RigCalibration> RigCalibration() const;
   inline void SetRigCalibration(
-      const std::shared_ptr<RigCalibration>& rig_calibration);
+      std::shared_ptr<class RigCalibration> rig_calibration);
   inline bool HasRigCalibration() const;
 
   // Access the frame from world transformation
@@ -141,7 +142,7 @@ struct Frame {
   // case, where rig modeling is no longer needed.
   Rigid3d frame_from_world_;
   rig_t rig_id_ = kInvalidRigId;
-  std::shared_ptr<RigCalibration> rig_calibration_ = nullptr;
+  std::shared_ptr<class RigCalibration> rig_calibration_ = nullptr;
 };
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -158,18 +159,12 @@ bool RigCalibration::HasSensor(sensor_t sensor_id) const {
 
 size_t RigCalibration::NumSensors() const { return sensor_ids_.size(); }
 
-void AddReferenceSensor(sensor_t ref_sensor_id) {
+void RigCalibration::AddReferenceSensor(sensor_t ref_sensor_id) {
   THROW_CHECK(sensor_ids_.empty());  // The reference sensor must be added first
   ref_sensor_id_ = ref_sensor_id;
   sensor_ids_.insert(ref_sensor_id);
 }
 
-sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
-
-bool RigCalibration::IsReference(sensor_t sensor_id) const {
-  return sensor_id == ref_sensor_id_;
-}
-
 void RigCalibration::AddSensor(sensor_t sensor_id,
                                Rigid3d sensor_from_rig,
                                bool is_fixed) {
@@ -188,10 +183,15 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
   }
 }
 
-Rigi3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
-  THROW_CHECK(!IsRereference(sensor_id))
-  auto it = map_sensor_from_rig_.find(sensor_id);
-  if (it == map_sensor_from_rig_.end())
+sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
+
+bool RigCalibration::IsReference(sensor_t sensor_id) const {
+  return sensor_id == ref_sensor_id_;
+}
+
+Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsReference(sensor_id));
+  if (map_sensor_from_rig_.find(sensor_id) == map_sensor_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
         sensor_id.first,
@@ -200,7 +200,7 @@ Rigi3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
 }
 
 const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
-  THROW_CHECK(!IsRereference(sensor_id))
+  THROW_CHECK(!IsReference(sensor_id));
   auto it = map_sensor_from_rig_.find(sensor_id);
   if (it == map_sensor_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
@@ -210,7 +210,9 @@ const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
   return map_sensor_from_rig_.at(sensor_id);
 }
 
-frame_t Frame::FrameId(frame_t frame_id) const { return frame_id_; }
+frame_t Frame::FrameId() const { return frame_id_; }
+
+void Frame::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
 std::unordered_set<data_t>& Frame::DataIds() { return data_ids_; }
 
@@ -226,20 +228,20 @@ rig_t Frame::RigId() const { return rig_id_; }
 
 void Frame::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
 
-std::shared_ptr<RigCalibration> Frame::RigCalibration() const {
+const std::shared_ptr<class RigCalibration> Frame::RigCalibration() const {
   return rig_calibration_;
 }
 
 void Frame::SetRigCalibration(
-    const std::shared_ptr<RigCalibration>& rig_calibration) {
+    std::shared_ptr<class RigCalibration> rig_calibration) {
   rig_calibration_ = rig_calibration;
 }
 
 bool Frame::HasRigCalibration() const {
-  if (rig_calibration_ == nullptr)
+  if (!rig_calibration_)
     return false;
   else
-    return rig_calibration_.NumSensors() > 1;
+    return rig_calibration_->NumSensors() > 1;
 }
 
 const Rigid3d& Frame::FrameFromWorld() const { return frame_from_world_; }
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 137c08309c..a729650289 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -83,8 +83,8 @@ class Image {
   // [Optional] The corresponding frame of the image
   inline frame_t FrameId() const;
   inline void SetFrameId(frame_t frame_id);
-  inline std::shared_ptr<class Frame> Frame() const;
-  inline void SetFrame(const std::shared_ptr<class Frame>& frame);
+  inline const std::shared_ptr<class Frame> Frame() const;
+  inline void SetFrame(std::shared_ptr<class Frame> frame);
   inline bool HasFrame() const;
 
   // World to camera pose.
@@ -181,9 +181,9 @@ frame_t Image::FrameId() const { return frame_id_; }
 
 void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
-std::shared_ptr<class Frame> Image::Frame() { return frame_; }
+const std::shared_ptr<class Frame> Image::Frame() const { return frame_; }
 
-void Image::SetFrame(const std::shared_ptr<class Frame>& frame) {
+void Image::SetFrame(std::shared_ptr<class Frame> frame) {
   frame_ = frame;
 }
 

From 2f79de6ba9ea810668ccb4a52daa53954491a4ef Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 04:42:07 +0200
Subject: [PATCH 05/36] update.

---
 src/colmap/scene/frame.h | 24 +++++++++++-------------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index d98be85f90..eb92bd9fdf 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -29,13 +29,11 @@
 
 #pragma once
 
-#include "colmap/geometry/pose.h"
-#include "colmap/scene/camera.h"
-#include "colmap/scene/reconstruction.h"
+#include "colmap/geometry/rigid3.h"
 #include "colmap/util/types.h"
 
-#include <unordered_map>
-#include <unordered_set>
+#include <map>
+#include <set>
 #include <vector>
 
 namespace colmap {
@@ -92,11 +90,11 @@ class RigCalibration {
   sensor_t ref_sensor_id_;
 
   // list of sensors
-  std::unordered_set<sensor_t> sensor_ids_;
+  std::set<sensor_t> sensor_ids_;
 
   // sensor_from_rig transformation.
-  std::unordered_map<sensor_t, Rigid3d> map_sensor_from_rig_;
-  std::unordered_map<sensor_t, bool>
+  std::map<sensor_t, Rigid3d> map_sensor_from_rig_;
+  std::map<sensor_t, bool>
       is_fixed_sensor_from_rig_;  // for optimization
 };
 
@@ -107,8 +105,8 @@ class Frame {
   inline void SetFrameId(frame_t frame_id);
 
   // Access data ids
-  inline std::unordered_set<data_t>& DataIds();
-  inline const std::unordered_set<data_t>& DataIds() const;
+  inline std::set<data_t>& DataIds();
+  inline const std::set<data_t>& DataIds() const;
   inline void AddData(data_t data_id);
 
   // Check whether the data id is existent in the frame
@@ -135,7 +133,7 @@ class Frame {
 
  private:
   frame_t frame_id_;
-  std::unordered_set<data_t> data_ids_;
+  std::set<data_t> data_ids_;
 
   // Store the frame_from_world transformation and an optional rig calibration.
   // If the rig calibration is a nullptr, the frame becomes a single sensor
@@ -214,9 +212,9 @@ frame_t Frame::FrameId() const { return frame_id_; }
 
 void Frame::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
-std::unordered_set<data_t>& Frame::DataIds() { return data_ids_; }
+std::set<data_t>& Frame::DataIds() { return data_ids_; }
 
-const std::unordered_set<data_t>& Frame::DataIds() const { return data_ids_; }
+const std::set<data_t>& Frame::DataIds() const { return data_ids_; }
 
 void Frame::AddData(data_t data_id) { data_ids_.insert(data_id); }
 

From 6058852662edd5fce49b84ecd7747814e843198f Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 05:30:52 +0200
Subject: [PATCH 06/36] formatting.

---
 src/colmap/scene/frame.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index eb92bd9fdf..cf00abd742 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -94,8 +94,7 @@ class RigCalibration {
 
   // sensor_from_rig transformation.
   std::map<sensor_t, Rigid3d> map_sensor_from_rig_;
-  std::map<sensor_t, bool>
-      is_fixed_sensor_from_rig_;  // for optimization
+  std::map<sensor_t, bool> is_fixed_sensor_from_rig_;  // for optimization
 };
 
 class Frame {

From 873f6274a2bfdd2f50252c7ba2bd03c17be71a5b Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 06:02:25 +0200
Subject: [PATCH 07/36] update.

---
 src/colmap/scene/frame.h | 10 ++++-----
 src/colmap/scene/image.h | 44 ++++++++++++++++++++++++++++++++++------
 2 files changed, 43 insertions(+), 11 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index cf00abd742..003264bf29 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -128,7 +128,7 @@ class Frame {
   inline Rigid3d& SensorFromWorld();
 
   // Get the sensor from world transformation
-  inline Rigid3d SensorFromWorld(data_t data_id) const;
+  inline Rigid3d SensorFromWorld(sensor_t sensor_id) const;
 
  private:
   frame_t frame_id_;
@@ -253,12 +253,12 @@ Rigid3d& Frame::SensorFromWorld() {
   return FrameFromWorld();
 }
 
-Rigid3d Frame::SensorFromWorld(data_t data_id) const {
-  THROW_CHECK(HasData(data_id));
-  if (!HasRigCalibration() || rig_calibration_->IsReference(data_id.first)) {
+Rigid3d Frame::SensorFromWorld(sensor_t sensor_id) const {
+  if (!HasRigCalibration() || rig_calibration_->IsReference(sensor_id)) {
     return SensorFromWorld();
   }
-  return rig_calibration_->SensorFromRig(data_id.first) * frame_from_world_;
+  THROW_CHECK(rig_calibration_->HasSensor(sensor_id));
+  return rig_calibration_->SensorFromRig(sensor_id) * frame_from_world_;
 }
 
 }  // namespace colmap
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index a729650289..ba928f2257 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -86,9 +86,17 @@ class Image {
   inline const std::shared_ptr<class Frame> Frame() const;
   inline void SetFrame(std::shared_ptr<class Frame> frame);
   inline bool HasFrame() const;
+  // Check if the cam_from_world needs to be composited with rig calibration.
+  inline bool HasNonTrivialFrame() const;
 
   // World to camera pose.
+  // Access cam_from_world transformation with a constant reference. While
+  // calling this method, the cam_from_world will also be synced from the
+  // transformations in the frame (rig) if needed
   inline const Rigid3d& CamFromWorld() const;
+  // Access cam_from_world as a reference to do in-place update and
+  // optimization. Will throw an error if the image has a non trivial frame
+  // (rig) attached to it.
   inline Rigid3d& CamFromWorld();
 
   // Access the coordinates of image points.
@@ -138,7 +146,7 @@ class Image {
 
   // The pose of the image, defined as the transformation from world to camera.
   // Only useful when the corresponding frame (rig) does not exist.
-  Rigid3d cam_from_world_;
+  mutable Rigid3d cam_from_world_;
 
   // All image points, including points that are not part of a 3D point track.
   std::vector<struct Point2D> points2D_;
@@ -183,15 +191,39 @@ void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
 const std::shared_ptr<class Frame> Image::Frame() const { return frame_; }
 
-void Image::SetFrame(std::shared_ptr<class Frame> frame) {
-  frame_ = frame;
-}
+void Image::SetFrame(std::shared_ptr<class Frame> frame) { frame_ = frame; }
 
 bool Image::HasFrame() const { return frame_ != nullptr; }
 
-const Rigid3d& Image::CamFromWorld() const { return cam_from_world_; }
+bool Image::HasNonTrivialFrame() const {
+  return HasFrame() && frame_->HasRigCalibration() &&
+         !frame_->RigCalibration()->IsReference(
+             std::make_pair(SensorType::Camera, CameraId()));
+}
 
-Rigid3d& Image::CamFromWorld() { return cam_from_world_; }
+const Rigid3d& Image::CamFromWorld() const { 
+  if (HasNonTrivialFrame()) {
+    // sync cam from world
+    sensor_t sensor_id = std::make_pair(SensorType::Camera, CameraId());
+    cam_from_world_ = frame_->SensorFromWorld(sensor_id);
+    return cam_from_world_;
+  }
+  else if (HasFrame()) {
+    return frame_->SensorFromWorld();
+  }
+  else
+    return cam_from_world_;
+}
+
+Rigid3d& Image::CamFromWorld() {
+  if (HasNonTrivialFrame())
+    LOG(FATAL_THROW) << "No reference available for cam_from_world transformation, since composition with rig calibration is needed";
+  if (HasFrame()) {
+    return frame_->SensorFromWorld();
+  }
+  else
+    return cam_from_world_;
+}
 
 const struct Point2D& Image::Point2D(const point2D_t point2D_idx) const {
   return points2D_.at(point2D_idx);

From bff70977f60381b9c2ddbdbd733cbd477a0c90fd Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 06:41:43 +0200
Subject: [PATCH 08/36] formatting.

---
 src/colmap/scene/image.h | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index ba928f2257..2706ae4cfb 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -201,27 +201,26 @@ bool Image::HasNonTrivialFrame() const {
              std::make_pair(SensorType::Camera, CameraId()));
 }
 
-const Rigid3d& Image::CamFromWorld() const { 
+const Rigid3d& Image::CamFromWorld() const {
   if (HasNonTrivialFrame()) {
     // sync cam from world
     sensor_t sensor_id = std::make_pair(SensorType::Camera, CameraId());
     cam_from_world_ = frame_->SensorFromWorld(sensor_id);
     return cam_from_world_;
-  }
-  else if (HasFrame()) {
+  } else if (HasFrame()) {
     return frame_->SensorFromWorld();
-  }
-  else
+  } else
     return cam_from_world_;
 }
 
 Rigid3d& Image::CamFromWorld() {
   if (HasNonTrivialFrame())
-    LOG(FATAL_THROW) << "No reference available for cam_from_world transformation, since composition with rig calibration is needed";
+    LOG(FATAL_THROW)
+        << "No reference available for cam_from_world transformation, since "
+           "composition with rig calibration is needed";
   if (HasFrame()) {
     return frame_->SensorFromWorld();
-  }
-  else
+  } else
     return cam_from_world_;
 }
 

From 213715d3010362c87d46974b67d20dc9c858cd57 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Sat, 10 Aug 2024 10:00:04 +0200
Subject: [PATCH 09/36] minor.

---
 src/colmap/scene/frame.h | 6 +++---
 src/colmap/scene/image.h | 4 +++-
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 003264bf29..ec431ed1eb 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -63,7 +63,7 @@ class RigCalibration {
   // operation
   inline void AddReferenceSensor(sensor_t ref_sensor_id);
   inline void AddSensor(sensor_t sensor_id,
-                        Rigid3d sensor_from_rig = Rigid3d(),
+                        const Rigid3d& sensor_from_rig = Rigid3d(),
                         bool is_fixed = false);
 
   // Check whether the sensor exists in the rig
@@ -163,7 +163,7 @@ void RigCalibration::AddReferenceSensor(sensor_t ref_sensor_id) {
 }
 
 void RigCalibration::AddSensor(sensor_t sensor_id,
-                               Rigid3d sensor_from_rig,
+                               const Rigid3d& sensor_from_rig,
                                bool is_fixed) {
   if (NumSensors() == 0)
     LOG(FATAL_THROW) << "The reference sensor needs to added first before any "
@@ -231,7 +231,7 @@ const std::shared_ptr<class RigCalibration> Frame::RigCalibration() const {
 
 void Frame::SetRigCalibration(
     std::shared_ptr<class RigCalibration> rig_calibration) {
-  rig_calibration_ = rig_calibration;
+  rig_calibration_ = std::move(rig_calibration);
 }
 
 bool Frame::HasRigCalibration() const {
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 2706ae4cfb..9c41ece743 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -191,7 +191,9 @@ void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
 const std::shared_ptr<class Frame> Image::Frame() const { return frame_; }
 
-void Image::SetFrame(std::shared_ptr<class Frame> frame) { frame_ = frame; }
+void Image::SetFrame(std::shared_ptr<class Frame> frame) {
+  frame_ = std::move(frame);
+}
 
 bool Image::HasFrame() const { return frame_ != nullptr; }
 

From da433a11f01677e63416229dd219d757204d1526 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 27 Aug 2024 21:29:19 +0200
Subject: [PATCH 10/36] update.

---
 src/colmap/scene/frame.h | 8 +++++++-
 src/colmap/util/types.h  | 2 +-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index ec431ed1eb..24a4b91ccb 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -51,7 +51,13 @@ typedef std::pair<SensorType, uint32_t> sensor_t;
 // Unique identifier of the data point from a sensor
 typedef std::pair<sensor_t, uint64_t> data_t;
 
-// Rig calibration storing the sensor from rig transformation
+// Rig calibration storing the sensor from rig transformation.
+// The reference sensor shares identity poses with the device.
+// This design is mainly for two purposes:
+// 1) In the visual-inertial optimization one of the IMUs is generally used as
+// the reference frame since it is metric.
+// 2) Not having a reference frame brings a 6 DoF Gauge for each rig, which is
+// not ideal particularly when it comes to covariance estimation.
 class RigCalibration {
  public:
   // Access the unique identifier of the rig
diff --git a/src/colmap/util/types.h b/src/colmap/util/types.h
index 7f4dc9dadf..4b1428087c 100644
--- a/src/colmap/util/types.h
+++ b/src/colmap/util/types.h
@@ -85,7 +85,7 @@ typedef uint32_t image_t;
 // Unique identifier for frames.
 typedef uint64_t frame_t;
 
-// Unique identiier for rigs.
+// Unique identifier for rigs.
 typedef uint32_t rig_t;
 
 // Each image pair gets a unique ID, see `Database::ImagePairToPairId`.

From 3229804725f414f47f9c4c1bd7efbd9b0795fa76 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 17:06:52 +0100
Subject: [PATCH 11/36] cr

---
 src/colmap/scene/frame.h | 30 +++++++++++++++---------------
 src/colmap/util/types.h  |  2 +-
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 24a4b91ccb..44cf111728 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -33,6 +33,7 @@
 #include "colmap/util/types.h"
 
 #include <map>
+#include <memory>
 #include <set>
 #include <vector>
 
@@ -69,7 +70,7 @@ class RigCalibration {
   // operation
   inline void AddReferenceSensor(sensor_t ref_sensor_id);
   inline void AddSensor(sensor_t sensor_id,
-                        const Rigid3d& sensor_from_rig = Rigid3d(),
+                        const Rigid3d& sensor_from_rig,
                         bool is_fixed = false);
 
   // Check whether the sensor exists in the rig
@@ -99,7 +100,7 @@ class RigCalibration {
   std::set<sensor_t> sensor_ids_;
 
   // sensor_from_rig transformation.
-  std::map<sensor_t, Rigid3d> map_sensor_from_rig_;
+  std::map<sensor_t, Rigid3d> sensors_from_rig_;
   std::map<sensor_t, bool> is_fixed_sensor_from_rig_;  // for optimization
 };
 
@@ -122,7 +123,7 @@ class Frame {
   inline void SetRigId(rig_t rig_id);
 
   // Check if the frame has a non-trivial rig calibration
-  inline const std::shared_ptr<class RigCalibration> RigCalibration() const;
+  inline const std::shared_ptr<class RigCalibration>& RigCalibration() const;
   inline void SetRigCalibration(
       std::shared_ptr<class RigCalibration> rig_calibration);
   inline bool HasRigCalibration() const;
@@ -175,12 +176,11 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
     LOG(FATAL_THROW) << "The reference sensor needs to added first before any "
                         "sensor being added.";
   else {
-    if (HasSensor(sensor_id))
-      LOG(FATAL_THROW) << StringPrintf(
-          "Sensor id (%d, %d) is inserted twice into the rig",
-          sensor_id.first,
-          sensor_id.second);
-    map_sensor_from_rig_.emplace(sensor_id, sensor_from_rig);
+    THROW_CHECK(!HasSensor(sensor_id))
+        << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig",
+                        sensor_id.first,
+                        sensor_id.second);
+    sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
     is_fixed_sensor_from_rig_.emplace(sensor_id, is_fixed);
     sensor_ids_.insert(sensor_id);
   }
@@ -194,23 +194,23 @@ bool RigCalibration::IsReference(sensor_t sensor_id) const {
 
 Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
   THROW_CHECK(!IsReference(sensor_id));
-  if (map_sensor_from_rig_.find(sensor_id) == map_sensor_from_rig_.end())
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
         sensor_id.first,
         sensor_id.second);
-  return map_sensor_from_rig_.at(sensor_id);
+  return sensors_from_rig_.at(sensor_id);
 }
 
 const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
   THROW_CHECK(!IsReference(sensor_id));
-  auto it = map_sensor_from_rig_.find(sensor_id);
-  if (it == map_sensor_from_rig_.end())
+  auto it = sensors_from_rig_.find(sensor_id);
+  if (it == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
         sensor_id.first,
         sensor_id.second);
-  return map_sensor_from_rig_.at(sensor_id);
+  return sensors_from_rig_.at(sensor_id);
 }
 
 frame_t Frame::FrameId() const { return frame_id_; }
@@ -231,7 +231,7 @@ rig_t Frame::RigId() const { return rig_id_; }
 
 void Frame::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
 
-const std::shared_ptr<class RigCalibration> Frame::RigCalibration() const {
+const std::shared_ptr<class RigCalibration>& Frame::RigCalibration() const {
   return rig_calibration_;
 }
 
diff --git a/src/colmap/util/types.h b/src/colmap/util/types.h
index 4b1428087c..06e3126f92 100644
--- a/src/colmap/util/types.h
+++ b/src/colmap/util/types.h
@@ -83,7 +83,7 @@ typedef uint32_t camera_t;
 typedef uint32_t image_t;
 
 // Unique identifier for frames.
-typedef uint64_t frame_t;
+typedef uint32_t frame_t;
 
 // Unique identifier for rigs.
 typedef uint32_t rig_t;

From c65d5779406bb0287d44f4aa7d014cb51bad7b22 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 17:09:36 +0100
Subject: [PATCH 12/36] cr

---
 src/colmap/scene/image.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 95f56cf9d5..24bb1ae4c1 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -90,7 +90,7 @@ class Image {
   // [Optional] The corresponding frame of the image
   inline frame_t FrameId() const;
   inline void SetFrameId(frame_t frame_id);
-  inline const std::shared_ptr<class Frame> Frame() const;
+  inline const std::shared_ptr<class Frame>& Frame() const;
   inline void SetFrame(std::shared_ptr<class Frame> frame);
   inline bool HasFrame() const;
   // Check if the cam_from_world needs to be composited with rig calibration.
@@ -225,7 +225,7 @@ frame_t Image::FrameId() const { return frame_id_; }
 
 void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
 
-const std::shared_ptr<class Frame> Image::Frame() const { return frame_; }
+const std::shared_ptr<class Frame>& Image::Frame() const { return frame_; }
 
 void Image::SetFrame(std::shared_ptr<class Frame> frame) {
   frame_ = std::move(frame);

From 24f1ce411073746373ccd09ae10de11cc1de323c Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 17:34:02 +0100
Subject: [PATCH 13/36] cr

---
 src/colmap/scene/frame.h | 11 ++++++-----
 src/colmap/scene/image.h |  4 ++--
 2 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 44cf111728..7c08d4df5a 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -40,11 +40,12 @@
 namespace colmap {
 
 // Sensor type
-enum class SensorType {
-  Camera = 0,
-  IMU = 1,
-  Location = 2,  // include GNSS, radios, compass, etc.
-};
+MAKE_ENUM_CLASS(SensorType,
+                0,
+                CAMERA,
+                IMU,
+                LOCATION  // include GNSS, radios, compass, etc.
+);
 
 // Unique identifier of the sensor
 typedef std::pair<SensorType, uint32_t> sensor_t;
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index bb913f5341..17ff6a3ea9 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -238,14 +238,14 @@ bool Image::HasFrame() const { return frame_ != nullptr; }
 bool Image::HasNonTrivialFrame() const {
   return HasFrame() && frame_->HasRigCalibration() &&
          !frame_->RigCalibration()->IsReference(
-             std::make_pair(SensorType::Camera, CameraId()));
+             std::make_pair(SensorType::CAMERA, CameraId()));
 }
 
 const Rigid3d& Image::CamFromWorld() const {
   if (HasNonTrivialFrame()) {
     // TODO: fix this
     // sync cam from world
-    sensor_t sensor_id = std::make_pair(SensorType::Camera, CameraId());
+    sensor_t sensor_id = std::make_pair(SensorType::CAMERA, CameraId());
     cam_from_world_ = frame_->SensorFromWorld(sensor_id);
     return *cam_from_world_;
   } else if (HasFrame()) {

From 05d2ade79b3cf81b0943fd10eca7bf242ec9110a Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 17:38:56 +0100
Subject: [PATCH 14/36] cr

---
 src/colmap/scene/frame.h | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 7c08d4df5a..a54212cc4e 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -30,6 +30,7 @@
 #pragma once
 
 #include "colmap/geometry/rigid3.h"
+#include "colmap/util/enum_utils.h"
 #include "colmap/util/types.h"
 
 #include <map>
@@ -40,11 +41,11 @@
 namespace colmap {
 
 // Sensor type
-MAKE_ENUM_CLASS(SensorType,
-                0,
-                CAMERA,
-                IMU,
-                LOCATION  // include GNSS, radios, compass, etc.
+MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType,
+                                0,
+                                CAMERA,
+                                IMU,
+                                LOCATION  // include GNSS, radios, compass, etc.
 );
 
 // Unique identifier of the sensor

From b2e54293f7f893f3f81ebfc01ed106c7eecc18ef Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 17:49:02 +0100
Subject: [PATCH 15/36] cr

---
 src/colmap/scene/frame.h | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index a54212cc4e..bc60a0f0f2 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -71,9 +71,7 @@ class RigCalibration {
   // ``AddReferenceSensor`` needs to called first before all the ``AddSensor``
   // operation
   inline void AddReferenceSensor(sensor_t ref_sensor_id);
-  inline void AddSensor(sensor_t sensor_id,
-                        const Rigid3d& sensor_from_rig,
-                        bool is_fixed = false);
+  inline void AddSensor(sensor_t sensor_id, const Rigid3d& sensor_from_rig);
 
   // Check whether the sensor exists in the rig
   inline bool HasSensor(sensor_t sensor_id) const;
@@ -103,7 +101,6 @@ class RigCalibration {
 
   // sensor_from_rig transformation.
   std::map<sensor_t, Rigid3d> sensors_from_rig_;
-  std::map<sensor_t, bool> is_fixed_sensor_from_rig_;  // for optimization
 };
 
 class Frame {
@@ -172,8 +169,7 @@ void RigCalibration::AddReferenceSensor(sensor_t ref_sensor_id) {
 }
 
 void RigCalibration::AddSensor(sensor_t sensor_id,
-                               const Rigid3d& sensor_from_rig,
-                               bool is_fixed) {
+                               const Rigid3d& sensor_from_rig) {
   if (NumSensors() == 0)
     LOG(FATAL_THROW) << "The reference sensor needs to added first before any "
                         "sensor being added.";
@@ -183,7 +179,6 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
                         sensor_id.first,
                         sensor_id.second);
     sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
-    is_fixed_sensor_from_rig_.emplace(sensor_id, is_fixed);
     sensor_ids_.insert(sensor_id);
   }
 }

From 885593b3f56675cc3fde032cc4987d25dd720fb2 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 20:22:31 +0100
Subject: [PATCH 16/36] fix todos. make frame_from_world optional

---
 src/colmap/scene/frame.h | 48 ++++++++++++++------
 src/colmap/scene/image.h | 94 +++++++++++++++++++++++++++++-----------
 2 files changed, 103 insertions(+), 39 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index bc60a0f0f2..1573919f1d 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -130,8 +130,12 @@ class Frame {
   // Access the frame from world transformation
   inline const Rigid3d& FrameFromWorld() const;
   inline Rigid3d& FrameFromWorld();
-  inline const Rigid3d& SensorFromWorld() const;
-  inline Rigid3d& SensorFromWorld();
+  inline const std::optional<Rigid3d>& MaybeFrameFromWorld() const;
+  inline std::optional<Rigid3d>& MaybeFrameFromWorld();
+  inline void SetFrameFromWorld(const Rigid3d& frame_from_world);
+  inline void SetFrameFromWorld(const std::optional<Rigid3d>& frame_from_world);
+  inline bool HasPose() const;
+  inline void ResetPose();
 
   // Get the sensor from world transformation
   inline Rigid3d SensorFromWorld(sensor_t sensor_id) const;
@@ -143,7 +147,7 @@ class Frame {
   // Store the frame_from_world transformation and an optional rig calibration.
   // If the rig calibration is a nullptr, the frame becomes a single sensor
   // case, where rig modeling is no longer needed.
-  Rigid3d frame_from_world_;
+  std::optional<Rigid3d> frame_from_world_;
   rig_t rig_id_ = kInvalidRigId;
   std::shared_ptr<class RigCalibration> rig_calibration_ = nullptr;
 };
@@ -244,24 +248,42 @@ bool Frame::HasRigCalibration() const {
     return rig_calibration_->NumSensors() > 1;
 }
 
-const Rigid3d& Frame::FrameFromWorld() const { return frame_from_world_; }
-Rigid3d& Frame::FrameFromWorld() { return frame_from_world_; }
+const Rigid3d& Frame::FrameFromWorld() const {
+  THROW_CHECK(frame_from_world_) << "Frame does not have a valid pose.";
+  return *frame_from_world_;
+}
+
+Rigid3d& Frame::FrameFromWorld() {
+  THROW_CHECK(frame_from_world_) << "Frame does not have a valid pose.";
+  return *frame_from_world_;
+}
+
+const std::optional<Rigid3d>& Frame::MaybeFrameFromWorld() const {
+  return frame_from_world_;
+}
 
-const Rigid3d& Frame::SensorFromWorld() const {
-  THROW_CHECK(!HasRigCalibration());
-  return FrameFromWorld();
+std::optional<Rigid3d>& Frame::MaybeFrameFromWorld() {
+  return frame_from_world_;
 }
-Rigid3d& Frame::SensorFromWorld() {
-  THROW_CHECK(!HasRigCalibration());
-  return FrameFromWorld();
+
+void Frame::SetFrameFromWorld(const Rigid3d& frame_from_world) {
+  frame_from_world_ = frame_from_world;
+}
+
+void Frame::SetFrameFromWorld(const std::optional<Rigid3d>& frame_from_world) {
+  frame_from_world_ = frame_from_world;
 }
 
+bool Frame::HasPose() const { return frame_from_world_.has_value(); }
+
+void Frame::ResetPose() { frame_from_world_.reset(); }
+
 Rigid3d Frame::SensorFromWorld(sensor_t sensor_id) const {
   if (!HasRigCalibration() || rig_calibration_->IsReference(sensor_id)) {
-    return SensorFromWorld();
+    return FrameFromWorld();
   }
   THROW_CHECK(rig_calibration_->HasSensor(sensor_id));
-  return rig_calibration_->SensorFromRig(sensor_id) * frame_from_world_;
+  return rig_calibration_->SensorFromRig(sensor_id) * FrameFromWorld();
 }
 
 }  // namespace colmap
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 17ff6a3ea9..5a32b1351d 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -95,13 +95,13 @@ class Image {
   inline bool HasNonTrivialFrame() const;
 
   // World to camera pose.
-  // Access cam_from_world transformation with a constant reference. While
-  // calling this method, the cam_from_world will also be synced from the
-  // transformations in the frame (rig) if needed
+  // Get the value (copy) of cam_from_world. This also supports non-trivial
+  // frame (rig).
+  inline Rigid3d CamFromWorldValue() const;
+
+  // The following methods only works for non-composited pose. Will throw an
+  // error if the image has a non-trivial frame (rig) attached to it.
   inline const Rigid3d& CamFromWorld() const;
-  // Access cam_from_world as a reference to do in-place update and
-  // optimization. Will throw an error if the image has a non trivial frame
-  // (rig) attached to it.
   inline Rigid3d& CamFromWorld();
   inline const std::optional<Rigid3d>& MaybeCamFromWorld() const;
   inline std::optional<Rigid3d>& MaybeCamFromWorld();
@@ -163,7 +163,9 @@ class Image {
 
   // The pose of the image, defined as the transformation from world to camera.
   // Only useful when the corresponding frame (rig) does not exist.
-  mutable std::optional<Rigid3d> cam_from_world_;
+  // TODO: use a trivial frame for non-rig setup and remove necessity of this
+  // member.
+  std::optional<Rigid3d> cam_from_world_;
 
   // All image points, including points that are not part of a 3D point track.
   std::vector<struct Point2D> points2D_;
@@ -241,15 +243,23 @@ bool Image::HasNonTrivialFrame() const {
              std::make_pair(SensorType::CAMERA, CameraId()));
 }
 
-const Rigid3d& Image::CamFromWorld() const {
-  if (HasNonTrivialFrame()) {
-    // TODO: fix this
-    // sync cam from world
+Rigid3d Image::CamFromWorldValue() const {
+  if (HasFrame()) {
     sensor_t sensor_id = std::make_pair(SensorType::CAMERA, CameraId());
-    cam_from_world_ = frame_->SensorFromWorld(sensor_id);
+    return frame_->SensorFromWorld(sensor_id);
+  } else {
+    THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";
     return *cam_from_world_;
-  } else if (HasFrame()) {
-    return frame_->SensorFromWorld();
+  }
+}
+
+const Rigid3d& Image::CamFromWorld() const {
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "No reference available for cam_from_world transformation, since "
+         "composition with rig calibration is needed";
+
+  if (HasFrame()) {
+    return frame_->FrameFromWorld();
   } else {
     THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";
     return *cam_from_world_;
@@ -257,12 +267,12 @@ const Rigid3d& Image::CamFromWorld() const {
 }
 
 Rigid3d& Image::CamFromWorld() {
-  if (HasNonTrivialFrame())
-    LOG(FATAL_THROW)
-        << "No reference available for cam_from_world transformation, since "
-           "composition with rig calibration is needed";
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "No reference available for cam_from_world transformation, since "
+         "composition with rig calibration is needed";
+
   if (HasFrame()) {
-    return frame_->SensorFromWorld();
+    return frame_->FrameFromWorld();
   } else {
     THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";
     return *cam_from_world_;
@@ -270,27 +280,59 @@ Rigid3d& Image::CamFromWorld() {
 }
 
 const std::optional<Rigid3d>& Image::MaybeCamFromWorld() const {
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "No reference available for cam_from_world transformation, since "
+         "composition with rig calibration is needed";
+  if (HasFrame()) {
+    return frame_->MaybeFrameFromWorld();
+  }
   return cam_from_world_;
 }
 
-std::optional<Rigid3d>& Image::MaybeCamFromWorld() { return cam_from_world_; }
+std::optional<Rigid3d>& Image::MaybeCamFromWorld() {
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "No reference available for cam_from_world transformation, since "
+         "composition with rig calibration is needed";
+  if (HasFrame()) {
+    return frame_->MaybeFrameFromWorld();
+  }
+  return cam_from_world_;
+}
 
 void Image::SetCamFromWorld(const Rigid3d& cam_from_world) {
-  cam_from_world_ = cam_from_world;
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "No reference available for cam_from_world transformation, since "
+         "composition with rig calibration is needed";
+  if (HasFrame()) {
+    frame_->SetFrameFromWorld(cam_from_world);
+  } else {
+    cam_from_world_ = cam_from_world;
+  }
 }
 
 void Image::SetCamFromWorld(const std::optional<Rigid3d>& cam_from_world) {
-  cam_from_world_ = cam_from_world;
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "No reference available for cam_from_world transformation, since "
+         "composition with rig calibration is needed";
+  if (HasFrame()) {
+    frame_->SetFrameFromWorld(cam_from_world);
+  } else {
+    cam_from_world_ = cam_from_world;
+  }
 }
 
 bool Image::HasPose() const {
-  // TODO: fix this function
-  return cam_from_world_.has_value();
+  if (HasFrame())
+    return frame_->HasPose();
+  else
+    return cam_from_world_.has_value();
 }
 
 void Image::ResetPose() {
-  // TODO: fix this function
-  cam_from_world_.reset();
+  if (HasFrame())
+    frame_->ResetPose();
+  else
+    cam_from_world_.reset();
 }
 
 const struct Point2D& Image::Point2D(const point2D_t point2D_idx) const {

From 07cc0ef3a751c8f4ee6e204c1d50530488035ed7 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 20:29:14 +0100
Subject: [PATCH 17/36] include <optional>

---
 src/colmap/scene/frame.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 1573919f1d..868c2a942a 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -35,6 +35,7 @@
 
 #include <map>
 #include <memory>
+#include <optional>
 #include <set>
 #include <vector>
 

From b68160d259ab96f18939e3950dbbcc7d8f264a84 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 20:40:49 +0100
Subject: [PATCH 18/36] cam_from_world_value in pycolmap

---
 src/pycolmap/scene/image.cc | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/pycolmap/scene/image.cc b/src/pycolmap/scene/image.cc
index 4985cc96e6..2f84bfee41 100644
--- a/src/pycolmap/scene/image.cc
+++ b/src/pycolmap/scene/image.cc
@@ -86,7 +86,14 @@ void BindImage(py::module& m) {
           py::overload_cast<const std::optional<Rigid3d>&>(
               &Image::SetCamFromWorld),
           "The pose of the image, defined as the transformation from world to "
-          "camera space. None if the image is not registered.")
+          "camera space. None if the image is not registered. Will throw an "
+          "error if a non-trivial frame (rig) is present.")
+      .def_property_readonly(
+          "cam_from_world_value",
+          &Image::CamFromWorldValue,
+          "The pose of the image, defined as the transformation from world to "
+          "camera space. This method is read-only and support non-trivial "
+          "frame (rig).")
       .def_property_readonly(
           "has_pose", &Image::HasPose, "Whether the image has a valid pose.")
       .def("reset_pose", &Image::ResetPose, "Invalidate the pose of the image.")

From 63055e92e794a62a63ade40408fa2a355fd60ce2 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Mon, 3 Feb 2025 21:26:29 +0100
Subject: [PATCH 19/36] custom structures for sensor_t and data_t

---
 src/colmap/scene/frame.h | 67 ++++++++++++++++++++++++++++++++++------
 src/colmap/scene/image.h |  4 +--
 2 files changed, 59 insertions(+), 12 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 868c2a942a..763dc3be93 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -33,10 +33,13 @@
 #include "colmap/util/enum_utils.h"
 #include "colmap/util/types.h"
 
+#include <cstdint>
+#include <functional>
 #include <map>
 #include <memory>
 #include <optional>
 #include <set>
+#include <tuple>
 #include <vector>
 
 namespace colmap {
@@ -49,11 +52,32 @@ MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType,
                                 LOCATION  // include GNSS, radios, compass, etc.
 );
 
-// Unique identifier of the sensor
-typedef std::pair<SensorType, uint32_t> sensor_t;
+struct sensor_t {
+  SensorType type;
+  uint32_t id;
+  sensor_t(const SensorType& type, uint32_t id) : type(type), id(id) {}
 
-// Unique identifier of the data point from a sensor
-typedef std::pair<sensor_t, uint64_t> data_t;
+  bool operator<(const sensor_t& other) const {
+    return std::tie(type, id) < std::tie(other.type, other.id);
+  }
+  bool operator==(const sensor_t& other) const {
+    return type == other.type && id == other.id;
+  }
+};
+
+struct data_t {
+  sensor_t sensor_id;
+  uint64_t id;
+  data_t(const sensor_t& sensor_id, uint64_t id)
+      : sensor_id(sensor_id), id(id) {}
+
+  bool operator<(const data_t& other) const {
+    return std::tie(sensor_id, id) < std::tie(other.sensor_id, other.id);
+  }
+  bool operator==(const data_t& other) const {
+    return sensor_id == other.sensor_id && id == other.id;
+  }
+};
 
 // Rig calibration storing the sensor from rig transformation.
 // The reference sensor shares identity poses with the device.
@@ -181,8 +205,8 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
   else {
     THROW_CHECK(!HasSensor(sensor_id))
         << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig",
-                        sensor_id.first,
-                        sensor_id.second);
+                        sensor_id.type,
+                        sensor_id.id);
     sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
     sensor_ids_.insert(sensor_id);
   }
@@ -199,8 +223,8 @@ Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
   if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.first,
-        sensor_id.second);
+        sensor_id.type,
+        sensor_id.id);
   return sensors_from_rig_.at(sensor_id);
 }
 
@@ -210,8 +234,8 @@ const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
   if (it == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.first,
-        sensor_id.second);
+        sensor_id.type,
+        sensor_id.id);
   return sensors_from_rig_.at(sensor_id);
 }
 
@@ -288,3 +312,26 @@ Rigid3d Frame::SensorFromWorld(sensor_t sensor_id) const {
 }
 
 }  // namespace colmap
+
+namespace std {
+template <>
+struct hash<colmap::sensor_t> {
+  std::size_t operator()(const colmap::sensor_t& s) const noexcept {
+    return std::hash<std::pair<uint32_t, uint32_t>>{}(
+        std::make_pair(static_cast<uint32_t>(s.type), s.id));
+  }
+};
+
+// [Reference]
+// https://stackoverflow.com/questions/26705751/why-is-the-magic-number-in-boosthash-combine-specified-in-hex
+template <>
+struct hash<colmap::data_t> {
+  std::size_t operator()(const colmap::data_t& d) const noexcept {
+    size_t h1 =
+        std::hash<uint64_t>{}(std::hash<colmap::sensor_t>{}(d.sensor_id));
+    size_t h2 = std::hash<uint64_t>{}(d.id);
+    return h1 ^ (h2 + 0x9e3779b9 + (h1 << 6) + (h1 >> 2));
+  }
+};
+
+}  // namespace std
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 5a32b1351d..88663b4610 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -240,12 +240,12 @@ bool Image::HasFrame() const { return frame_ != nullptr; }
 bool Image::HasNonTrivialFrame() const {
   return HasFrame() && frame_->HasRigCalibration() &&
          !frame_->RigCalibration()->IsReference(
-             std::make_pair(SensorType::CAMERA, CameraId()));
+             sensor_t(SensorType::CAMERA, CameraId()));
 }
 
 Rigid3d Image::CamFromWorldValue() const {
   if (HasFrame()) {
-    sensor_t sensor_id = std::make_pair(SensorType::CAMERA, CameraId());
+    sensor_t sensor_id = sensor_t(SensorType::CAMERA, CameraId());
     return frame_->SensorFromWorld(sensor_id);
   } else {
     THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";

From f94ccede2e699cb5402527f74a6ae8df9dd4550f Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 13:00:58 +0100
Subject: [PATCH 20/36] cr.

---
 src/colmap/scene/frame.h | 55 ++++++++++++++++++++--------------------
 1 file changed, 27 insertions(+), 28 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 763dc3be93..99714c2776 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// Copyright (c), ETH Zurich and UNC Chapel Hill.
 // All rights reserved.
 //
 // Redistribution and use in source and binary forms, with or without
@@ -45,22 +45,17 @@
 namespace colmap {
 
 // Sensor type
-MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType,
-                                0,
-                                CAMERA,
-                                IMU,
-                                LOCATION  // include GNSS, radios, compass, etc.
-);
+MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType, 0, CAMERA, IMU);
 
 struct sensor_t {
   SensorType type;
   uint32_t id;
   sensor_t(const SensorType& type, uint32_t id) : type(type), id(id) {}
 
-  bool operator<(const sensor_t& other) const {
+  inline bool operator<(const sensor_t& other) const {
     return std::tie(type, id) < std::tie(other.type, other.id);
   }
-  bool operator==(const sensor_t& other) const {
+  inline bool operator==(const sensor_t& other) const {
     return type == other.type && id == other.id;
   }
 };
@@ -71,10 +66,10 @@ struct data_t {
   data_t(const sensor_t& sensor_id, uint64_t id)
       : sensor_id(sensor_id), id(id) {}
 
-  bool operator<(const data_t& other) const {
+  inline bool operator<(const data_t& other) const {
     return std::tie(sensor_id, id) < std::tie(other.sensor_id, other.id);
   }
-  bool operator==(const data_t& other) const {
+  inline bool operator==(const data_t& other) const {
     return sensor_id == other.sensor_id && id == other.id;
   }
 };
@@ -93,9 +88,9 @@ class RigCalibration {
   inline void SetRigId(rig_t rig_id);
 
   // Add sensor into the rig
-  // ``AddReferenceSensor`` needs to called first before all the ``AddSensor``
+  // ``AddRefSensor`` needs to called first before all the ``AddSensor``
   // operation
-  inline void AddReferenceSensor(sensor_t ref_sensor_id);
+  inline void AddRefSensor(sensor_t ref_sensor_id);
   inline void AddSensor(sensor_t sensor_id, const Rigid3d& sensor_from_rig);
 
   // Check whether the sensor exists in the rig
@@ -191,25 +186,25 @@ bool RigCalibration::HasSensor(sensor_t sensor_id) const {
 
 size_t RigCalibration::NumSensors() const { return sensor_ids_.size(); }
 
-void RigCalibration::AddReferenceSensor(sensor_t ref_sensor_id) {
-  THROW_CHECK(sensor_ids_.empty());  // The reference sensor must be added first
+void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
+  THROW_CHECK(sensor_ids_.empty())
+      << "Only one reference sensor is accepted and must be added before all "
+         "the sensors";
   ref_sensor_id_ = ref_sensor_id;
   sensor_ids_.insert(ref_sensor_id);
 }
 
 void RigCalibration::AddSensor(sensor_t sensor_id,
                                const Rigid3d& sensor_from_rig) {
-  if (NumSensors() == 0)
-    LOG(FATAL_THROW) << "The reference sensor needs to added first before any "
-                        "sensor being added.";
-  else {
-    THROW_CHECK(!HasSensor(sensor_id))
-        << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig",
-                        sensor_id.type,
-                        sensor_id.id);
-    sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
-    sensor_ids_.insert(sensor_id);
-  }
+  THROW_CHECK(NumSensors() >= 1)
+      << "The reference sensor needs to added first before any "
+         "sensor being added.";
+  THROW_CHECK(!HasSensor(sensor_id))
+      << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig",
+                      sensor_id.type,
+                      sensor_id.id);
+  sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
+  sensor_ids_.insert(sensor_id);
 }
 
 sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
@@ -219,7 +214,9 @@ bool RigCalibration::IsReference(sensor_t sensor_id) const {
 }
 
 Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
-  THROW_CHECK(!IsReference(sensor_id));
+  THROW_CHECK(!IsReference(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
   if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
@@ -229,7 +226,9 @@ Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
 }
 
 const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
-  THROW_CHECK(!IsReference(sensor_id));
+  THROW_CHECK(!IsReference(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
   auto it = sensors_from_rig_.find(sensor_id);
   if (it == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(

From 4e2f06cb48ee8301d85189cb30ae0321b0751f5f Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 13:21:10 +0100
Subject: [PATCH 21/36] remove the need of sensor_ids

---
 src/colmap/scene/frame.h | 30 ++++++++++++++++--------------
 1 file changed, 16 insertions(+), 14 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 99714c2776..49c01c749d 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -45,11 +45,11 @@
 namespace colmap {
 
 // Sensor type
-MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType, 0, CAMERA, IMU);
+MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType, -1, INVALID, CAMERA, IMU);
 
 struct sensor_t {
   SensorType type;
-  uint32_t id;
+  uint32_t id;  // This can be camera_t / imu_t (not supported yet)
   sensor_t(const SensorType& type, uint32_t id) : type(type), id(id) {}
 
   inline bool operator<(const sensor_t& other) const {
@@ -59,10 +59,12 @@ struct sensor_t {
     return type == other.type && id == other.id;
   }
 };
+const sensor_t kInvalidSensorId =
+    sensor_t(SensorType::INVALID, std::numeric_limits<uint32_t>::max());
 
 struct data_t {
   sensor_t sensor_id;
-  uint64_t id;
+  uint64_t id;  // This can be image_t / imu_measurement_t (not supported yet)
   data_t(const sensor_t& sensor_id, uint64_t id)
       : sensor_id(sensor_id), id(id) {}
 
@@ -73,6 +75,8 @@ struct data_t {
     return sensor_id == other.sensor_id && id == other.id;
   }
 };
+const data_t kInvalidDataId =
+    data_t(kInvalidSensorId, std::numeric_limits<uint64_t>::max());
 
 // Rig calibration storing the sensor from rig transformation.
 // The reference sensor shares identity poses with the device.
@@ -114,10 +118,7 @@ class RigCalibration {
   rig_t rig_id_;
 
   // Reference sensor id which has the identity transformation to the rig.
-  sensor_t ref_sensor_id_;
-
-  // list of sensors
-  std::set<sensor_t> sensor_ids_;
+  sensor_t ref_sensor_id_ = kInvalidSensorId;
 
   // sensor_from_rig transformation.
   std::map<sensor_t, Rigid3d> sensors_from_rig_;
@@ -181,17 +182,19 @@ rig_t RigCalibration::RigId() const { return rig_id_; }
 void RigCalibration::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
 
 bool RigCalibration::HasSensor(sensor_t sensor_id) const {
-  return sensor_ids_.find(sensor_id) != sensor_ids_.end();
+  return sensor_id == ref_sensor_id_ ||
+         sensors_from_rig_.find(sensor_id) != sensors_from_rig_.end();
 }
 
-size_t RigCalibration::NumSensors() const { return sensor_ids_.size(); }
+size_t RigCalibration::NumSensors() const {
+  return sensors_from_rig_.size() + (ref_sensor_id_ == kInvalidSensorId) ? 0
+                                                                         : 1;
+}
 
 void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
-  THROW_CHECK(sensor_ids_.empty())
-      << "Only one reference sensor is accepted and must be added before all "
-         "the sensors";
+  THROW_CHECK(ref_sensor_id_ == kInvalidSensorId)
+      << "Reference sensor already set";
   ref_sensor_id_ = ref_sensor_id;
-  sensor_ids_.insert(ref_sensor_id);
 }
 
 void RigCalibration::AddSensor(sensor_t sensor_id,
@@ -204,7 +207,6 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
                       sensor_id.type,
                       sensor_id.id);
   sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
-  sensor_ids_.insert(sensor_id);
 }
 
 sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }

From 068ce0630dee101e9e546b651daa3e45070745ce Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 13:25:18 +0100
Subject: [PATCH 22/36] improve comments

---
 src/colmap/scene/frame.h | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 49c01c749d..2a6a5606c6 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -48,8 +48,11 @@ namespace colmap {
 MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType, -1, INVALID, CAMERA, IMU);
 
 struct sensor_t {
+  // Type of the sensor (INVALID / CAMERA / IMU)
   SensorType type;
-  uint32_t id;  // This can be camera_t / imu_t (not supported yet)
+  // Unique identifier of the sensor.
+  // This can be camera_t / imu_t (not supported yet)
+  uint32_t id;
   sensor_t(const SensorType& type, uint32_t id) : type(type), id(id) {}
 
   inline bool operator<(const sensor_t& other) const {
@@ -63,8 +66,11 @@ const sensor_t kInvalidSensorId =
     sensor_t(SensorType::INVALID, std::numeric_limits<uint32_t>::max());
 
 struct data_t {
+  // Unique identifer of the sensor
   sensor_t sensor_id;
-  uint64_t id;  // This can be image_t / imu_measurement_t (not supported yet)
+  // Unique identifier of the data (measurement)
+  // This can be image_t / imu_measurement_t (not supported yet)
+  uint64_t id;
   data_t(const sensor_t& sensor_id, uint64_t id)
       : sensor_id(sensor_id), id(id) {}
 

From 0a65a4e0dbefb8bd39a760b52ac15805459ccedc Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 13:28:21 +0100
Subject: [PATCH 23/36] move add sensor to cc

---
 src/colmap/scene/CMakeLists.txt |  2 +-
 src/colmap/scene/frame.cc       | 52 +++++++++++++++++++++++++++++++++
 src/colmap/scene/frame.h        | 22 ++------------
 3 files changed, 55 insertions(+), 21 deletions(-)
 create mode 100644 src/colmap/scene/frame.cc

diff --git a/src/colmap/scene/CMakeLists.txt b/src/colmap/scene/CMakeLists.txt
index 687167e644..77bddd2672 100644
--- a/src/colmap/scene/CMakeLists.txt
+++ b/src/colmap/scene/CMakeLists.txt
@@ -38,7 +38,7 @@ COLMAP_ADD_LIBRARY(
         correspondence_graph.h correspondence_graph.cc
         database.h database.cc
         database_cache.h database_cache.cc
-        frame.h
+        frame.h frame.cc
         image.h image.cc
         point2d.h point2d.cc
         point3d.h point3d.cc
diff --git a/src/colmap/scene/frame.cc b/src/colmap/scene/frame.cc
new file mode 100644
index 0000000000..52615208d2
--- /dev/null
+++ b/src/colmap/scene/frame.cc
@@ -0,0 +1,52 @@
+// Copyright (c), ETH Zurich and UNC Chapel Hill.
+// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "colmap/scene/frame.h"
+
+namespace colmap {
+
+void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
+  THROW_CHECK(ref_sensor_id_ == kInvalidSensorId)
+      << "Reference sensor already set";
+  ref_sensor_id_ = ref_sensor_id;
+}
+
+void RigCalibration::AddSensor(sensor_t sensor_id,
+                               const Rigid3d& sensor_from_rig) {
+  THROW_CHECK(NumSensors() >= 1)
+      << "The reference sensor needs to added first before any "
+         "sensor being added.";
+  THROW_CHECK(!HasSensor(sensor_id))
+      << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig",
+                      sensor_id.type,
+                      sensor_id.id);
+  sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
+}
+
+}  // namespace colmap
diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 2a6a5606c6..38d4b7a66a 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -100,8 +100,8 @@ class RigCalibration {
   // Add sensor into the rig
   // ``AddRefSensor`` needs to called first before all the ``AddSensor``
   // operation
-  inline void AddRefSensor(sensor_t ref_sensor_id);
-  inline void AddSensor(sensor_t sensor_id, const Rigid3d& sensor_from_rig);
+  void AddRefSensor(sensor_t ref_sensor_id);
+  void AddSensor(sensor_t sensor_id, const Rigid3d& sensor_from_rig);
 
   // Check whether the sensor exists in the rig
   inline bool HasSensor(sensor_t sensor_id) const;
@@ -197,24 +197,6 @@ size_t RigCalibration::NumSensors() const {
                                                                          : 1;
 }
 
-void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
-  THROW_CHECK(ref_sensor_id_ == kInvalidSensorId)
-      << "Reference sensor already set";
-  ref_sensor_id_ = ref_sensor_id;
-}
-
-void RigCalibration::AddSensor(sensor_t sensor_id,
-                               const Rigid3d& sensor_from_rig) {
-  THROW_CHECK(NumSensors() >= 1)
-      << "The reference sensor needs to added first before any "
-         "sensor being added.";
-  THROW_CHECK(!HasSensor(sensor_id))
-      << StringPrintf("Sensor id (%d, %d) is inserted twice into the rig",
-                      sensor_id.type,
-                      sensor_id.id);
-  sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
-}
-
 sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
 
 bool RigCalibration::IsReference(sensor_t sensor_id) const {

From 4b05f56e88fb2371018f77c0df0c46d93cc781ec Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 13:33:15 +0100
Subject: [PATCH 24/36] minor

---
 src/colmap/scene/frame.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 38d4b7a66a..492330c14f 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -148,10 +148,11 @@ class Frame {
   inline rig_t RigId() const;
   inline void SetRigId(rig_t rig_id);
 
-  // Check if the frame has a non-trivial rig calibration
+  // Access the rig calibration
   inline const std::shared_ptr<class RigCalibration>& RigCalibration() const;
   inline void SetRigCalibration(
       std::shared_ptr<class RigCalibration> rig_calibration);
+  // Check if the frame has a non-trivial rig calibration
   inline bool HasRigCalibration() const;
 
   // Access the frame from world transformation

From 2be8a147b6a8de1c3aed65d571c97660156e9a0f Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 13:36:21 +0100
Subject: [PATCH 25/36] change the id in data_t from uint64_t to uint32_t

---
 src/colmap/scene/frame.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index 492330c14f..c6083d6d83 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -70,7 +70,7 @@ struct data_t {
   sensor_t sensor_id;
   // Unique identifier of the data (measurement)
   // This can be image_t / imu_measurement_t (not supported yet)
-  uint64_t id;
+  uint32_t id;
   data_t(const sensor_t& sensor_id, uint64_t id)
       : sensor_id(sensor_id), id(id) {}
 
@@ -82,7 +82,7 @@ struct data_t {
   }
 };
 const data_t kInvalidDataId =
-    data_t(kInvalidSensorId, std::numeric_limits<uint64_t>::max());
+    data_t(kInvalidSensorId, std::numeric_limits<uint32_t>::max());
 
 // Rig calibration storing the sensor from rig transformation.
 // The reference sensor shares identity poses with the device.

From 355c0a28d95c9ca680acc5c2ff4dd7fff604c2de Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 14:11:37 +0100
Subject: [PATCH 26/36] fix

---
 src/colmap/scene/frame.h | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index c6083d6d83..bb20fb1778 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -61,6 +61,9 @@ struct sensor_t {
   inline bool operator==(const sensor_t& other) const {
     return type == other.type && id == other.id;
   }
+  inline bool operator!=(const sensor_t& other) const {
+    return !(*this == other);
+  }
 };
 const sensor_t kInvalidSensorId =
     sensor_t(SensorType::INVALID, std::numeric_limits<uint32_t>::max());
@@ -80,6 +83,9 @@ struct data_t {
   inline bool operator==(const data_t& other) const {
     return sensor_id == other.sensor_id && id == other.id;
   }
+  inline bool operator!=(const data_t& other) const {
+    return !(*this == other);
+  }
 };
 const data_t kInvalidDataId =
     data_t(kInvalidSensorId, std::numeric_limits<uint32_t>::max());
@@ -194,8 +200,9 @@ bool RigCalibration::HasSensor(sensor_t sensor_id) const {
 }
 
 size_t RigCalibration::NumSensors() const {
-  return sensors_from_rig_.size() + (ref_sensor_id_ == kInvalidSensorId) ? 0
-                                                                         : 1;
+  size_t n_sensors = sensors_from_rig_.size();
+  if (ref_sensor_id_ != kInvalidSensorId) n_sensors += 1;
+  return n_sensors;
 }
 
 sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }

From 05f739763620d4c6183ec605a72f5a80b4bc7f0a Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 16:16:56 +0100
Subject: [PATCH 27/36] deprecate cam_from_world and always use frame pointer

---
 src/colmap/scene/frame.cc |  4 ++
 src/colmap/scene/frame.h  |  6 ++-
 src/colmap/scene/image.cc |  3 +-
 src/colmap/scene/image.h  | 86 ++++++++-------------------------------
 4 files changed, 29 insertions(+), 70 deletions(-)

diff --git a/src/colmap/scene/frame.cc b/src/colmap/scene/frame.cc
index 52615208d2..a45b689d8c 100644
--- a/src/colmap/scene/frame.cc
+++ b/src/colmap/scene/frame.cc
@@ -31,6 +31,8 @@
 
 namespace colmap {
 
+RigCalibration::RigCalibration() {}
+
 void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
   THROW_CHECK(ref_sensor_id_ == kInvalidSensorId)
       << "Reference sensor already set";
@@ -49,4 +51,6 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
   sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
 }
 
+Frame::Frame() {}
+
 }  // namespace colmap
diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index bb20fb1778..ab971678dc 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -99,6 +99,8 @@ const data_t kInvalidDataId =
 // not ideal particularly when it comes to covariance estimation.
 class RigCalibration {
  public:
+  RigCalibration();
+
   // Access the unique identifier of the rig
   inline rig_t RigId() const;
   inline void SetRigId(rig_t rig_id);
@@ -138,6 +140,8 @@ class RigCalibration {
 
 class Frame {
  public:
+  Frame();
+
   // Access the unique identifier of the frame
   inline frame_t FrameId() const;
   inline void SetFrameId(frame_t frame_id);
@@ -175,7 +179,7 @@ class Frame {
   inline Rigid3d SensorFromWorld(sensor_t sensor_id) const;
 
  private:
-  frame_t frame_id_;
+  frame_t frame_id_ = kInvalidFrameId;
   std::set<data_t> data_ids_;
 
   // Store the frame_from_world transformation and an optional rig calibration.
diff --git a/src/colmap/scene/image.cc b/src/colmap/scene/image.cc
index e1d1e0eb14..9ccc3234cf 100644
--- a/src/colmap/scene/image.cc
+++ b/src/colmap/scene/image.cc
@@ -39,7 +39,8 @@ Image::Image()
       name_(""),
       camera_id_(kInvalidCameraId),
       camera_ptr_(nullptr),
-      num_points3D_(0) {}
+      num_points3D_(0),
+      frame_(std::make_shared<class Frame>()) {}
 
 void Image::SetPoints2D(const std::vector<Eigen::Vector2d>& points) {
   THROW_CHECK(points2D_.empty());
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 88663b4610..13768251a3 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -90,7 +90,6 @@ class Image {
   inline void SetFrameId(frame_t frame_id);
   inline const std::shared_ptr<class Frame>& Frame() const;
   inline void SetFrame(std::shared_ptr<class Frame> frame);
-  inline bool HasFrame() const;
   // Check if the cam_from_world needs to be composited with rig calibration.
   inline bool HasNonTrivialFrame() const;
 
@@ -157,15 +156,9 @@ class Image {
   // where `point3D_id != kInvalidPoint3DId`.
   point2D_t num_points3D_;
 
-  // [Optional] The corresponding frame (rig) of the image
-  frame_t frame_id_;
-  std::shared_ptr<class Frame> frame_ = nullptr;
-
-  // The pose of the image, defined as the transformation from world to camera.
-  // Only useful when the corresponding frame (rig) does not exist.
-  // TODO: use a trivial frame for non-rig setup and remove necessity of this
-  // member.
-  std::optional<Rigid3d> cam_from_world_;
+  // The corresponding frame (rig) of the image. By default a trivial frame will
+  // be initialized for each image with frame_id_ = kInvalidFrameId.
+  std::shared_ptr<class Frame> frame_;
 
   // All image points, including points that are not part of a 3D point track.
   std::vector<struct Point2D> points2D_;
@@ -225,9 +218,9 @@ point2D_t Image::NumPoints2D() const {
 
 point2D_t Image::NumPoints3D() const { return num_points3D_; }
 
-frame_t Image::FrameId() const { return frame_id_; }
+frame_t Image::FrameId() const { return frame_->FrameId(); }
 
-void Image::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
+void Image::SetFrameId(frame_t frame_id) { frame_->SetFrameId(frame_id); }
 
 const std::shared_ptr<class Frame>& Image::Frame() const { return frame_; }
 
@@ -235,105 +228,63 @@ void Image::SetFrame(std::shared_ptr<class Frame> frame) {
   frame_ = std::move(frame);
 }
 
-bool Image::HasFrame() const { return frame_ != nullptr; }
-
 bool Image::HasNonTrivialFrame() const {
-  return HasFrame() && frame_->HasRigCalibration() &&
+  THROW_CHECK(frame_) << "Invalid shared pointer to the corresponding frame";
+  return frame_->HasRigCalibration() &&
          !frame_->RigCalibration()->IsReference(
              sensor_t(SensorType::CAMERA, CameraId()));
 }
 
 Rigid3d Image::CamFromWorldValue() const {
-  if (HasFrame()) {
-    sensor_t sensor_id = sensor_t(SensorType::CAMERA, CameraId());
-    return frame_->SensorFromWorld(sensor_id);
-  } else {
-    THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";
-    return *cam_from_world_;
-  }
+  sensor_t sensor_id = sensor_t(SensorType::CAMERA, CameraId());
+  return frame_->SensorFromWorld(sensor_id);
 }
 
 const Rigid3d& Image::CamFromWorld() const {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
          "composition with rig calibration is needed";
-
-  if (HasFrame()) {
-    return frame_->FrameFromWorld();
-  } else {
-    THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";
-    return *cam_from_world_;
-  }
+  return frame_->FrameFromWorld();
 }
 
 Rigid3d& Image::CamFromWorld() {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
          "composition with rig calibration is needed";
-
-  if (HasFrame()) {
-    return frame_->FrameFromWorld();
-  } else {
-    THROW_CHECK(cam_from_world_) << "Image does not have a valid pose.";
-    return *cam_from_world_;
-  }
+  return frame_->FrameFromWorld();
 }
 
 const std::optional<Rigid3d>& Image::MaybeCamFromWorld() const {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
          "composition with rig calibration is needed";
-  if (HasFrame()) {
-    return frame_->MaybeFrameFromWorld();
-  }
-  return cam_from_world_;
+  return frame_->MaybeFrameFromWorld();
 }
 
 std::optional<Rigid3d>& Image::MaybeCamFromWorld() {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
          "composition with rig calibration is needed";
-  if (HasFrame()) {
-    return frame_->MaybeFrameFromWorld();
-  }
-  return cam_from_world_;
+  return frame_->MaybeFrameFromWorld();
 }
 
 void Image::SetCamFromWorld(const Rigid3d& cam_from_world) {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
          "composition with rig calibration is needed";
-  if (HasFrame()) {
-    frame_->SetFrameFromWorld(cam_from_world);
-  } else {
-    cam_from_world_ = cam_from_world;
-  }
+  frame_->SetFrameFromWorld(cam_from_world);
 }
 
 void Image::SetCamFromWorld(const std::optional<Rigid3d>& cam_from_world) {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
          "composition with rig calibration is needed";
-  if (HasFrame()) {
-    frame_->SetFrameFromWorld(cam_from_world);
-  } else {
-    cam_from_world_ = cam_from_world;
-  }
+  frame_->SetFrameFromWorld(cam_from_world);
 }
 
-bool Image::HasPose() const {
-  if (HasFrame())
-    return frame_->HasPose();
-  else
-    return cam_from_world_.has_value();
-}
+bool Image::HasPose() const { return frame_->HasPose(); }
 
-void Image::ResetPose() {
-  if (HasFrame())
-    frame_->ResetPose();
-  else
-    cam_from_world_.reset();
-}
+void Image::ResetPose() { frame_->ResetPose(); }
 
 const struct Point2D& Image::Point2D(const point2D_t point2D_idx) const {
   return points2D_.at(point2D_idx);
@@ -350,8 +301,7 @@ std::vector<struct Point2D>& Image::Points2D() { return points2D_; }
 bool Image::operator==(const Image& other) const {
   return image_id_ == other.image_id_ && camera_id_ == other.camera_id_ &&
          name_ == other.name_ && num_points3D_ == other.num_points3D_ &&
-         cam_from_world_ == other.cam_from_world_ &&
-         points2D_ == other.points2D_;
+         Frame() == other.Frame() && points2D_ == other.points2D_;
 }
 
 bool Image::operator!=(const Image& other) const { return !(*this == other); }

From 1e72385c14fd892f4338c5b01453fae9583154dc Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 16:20:52 +0100
Subject: [PATCH 28/36] minor

---
 src/colmap/scene/image.h | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 13768251a3..8952856a26 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -218,9 +218,15 @@ point2D_t Image::NumPoints2D() const {
 
 point2D_t Image::NumPoints3D() const { return num_points3D_; }
 
-frame_t Image::FrameId() const { return frame_->FrameId(); }
+frame_t Image::FrameId() const {
+  THROW_CHECK(frame_) << "Invalid pointer to the corresponding frame";
+  return frame_->FrameId();
+}
 
-void Image::SetFrameId(frame_t frame_id) { frame_->SetFrameId(frame_id); }
+void Image::SetFrameId(frame_t frame_id) {
+  THROW_CHECK(frame_) << "Invalid pointer to the corresponding frame";
+  frame_->SetFrameId(frame_id);
+}
 
 const std::shared_ptr<class Frame>& Image::Frame() const { return frame_; }
 
@@ -229,7 +235,7 @@ void Image::SetFrame(std::shared_ptr<class Frame> frame) {
 }
 
 bool Image::HasNonTrivialFrame() const {
-  THROW_CHECK(frame_) << "Invalid shared pointer to the corresponding frame";
+  THROW_CHECK(frame_) << "Invalid pointer to the corresponding frame";
   return frame_->HasRigCalibration() &&
          !frame_->RigCalibration()->IsReference(
              sensor_t(SensorType::CAMERA, CameraId()));

From e22e2cd3df535a46411e242f505150ca3f42aa00 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 18:01:09 +0100
Subject: [PATCH 29/36] fix copy construction and assignment for Image class

---
 src/colmap/scene/image.cc      | 39 ++++++++++++++++++++++++++++++++++
 src/colmap/scene/image.h       | 35 +++++++++++++++++++++---------
 src/colmap/scene/image_test.cc | 22 +++++++++++++++++++
 3 files changed, 86 insertions(+), 10 deletions(-)

diff --git a/src/colmap/scene/image.cc b/src/colmap/scene/image.cc
index 9ccc3234cf..f72e1d0a8a 100644
--- a/src/colmap/scene/image.cc
+++ b/src/colmap/scene/image.cc
@@ -42,6 +42,45 @@ Image::Image()
       num_points3D_(0),
       frame_(std::make_shared<class Frame>()) {}
 
+Image::Image(const Image& other)
+    : image_id_(other.ImageId()),
+      name_(other.Name()),
+      camera_id_(other.CameraId()),
+      camera_ptr_(nullptr),
+      num_points3D_(other.NumPoints3D()),
+      points2D_(other.Points2D()) {
+  if (other.HasCameraPtr()) {
+    camera_ptr_ = other.CameraPtr();
+  }
+  if (other.HasNonTrivialFrame()) {
+    frame_ = other.Frame();
+  } else {
+    frame_ = std::make_shared<class Frame>();
+    frame_->SetFrameFromWorld(other.MaybeCamFromWorld());
+  }
+}
+
+Image& Image::operator=(const Image& other) {
+  if (this != &other) {
+    image_id_ = other.ImageId();
+    name_ = other.Name();
+    camera_id_ = other.CameraId();
+    camera_ptr_ = nullptr;
+    if (other.HasCameraPtr()) {
+      camera_ptr_ = other.CameraPtr();
+    }
+    num_points3D_ = other.NumPoints3D();
+    points2D_ = other.Points2D();
+    if (other.HasNonTrivialFrame()) {
+      frame_ = other.Frame();
+    } else {
+      frame_ = std::make_shared<class Frame>();
+      frame_->SetFrameFromWorld(other.MaybeCamFromWorld());
+    }
+  }
+  return *this;
+}
+
 void Image::SetPoints2D(const std::vector<Eigen::Vector2d>& points) {
   THROW_CHECK(points2D_.empty());
   points2D_.resize(points.size());
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 8952856a26..4463128876 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -55,6 +55,11 @@ class Image {
  public:
   Image();
 
+  // Copy construct/assign.
+  // Initialize a new Frame object if the image has a trivial frame.
+  Image(const Image& other);
+  Image& operator=(const Image& other);
+
   // Access the unique identifier of the image.
   inline image_t ImageId() const;
   inline void SetImageId(image_t image_id);
@@ -249,48 +254,53 @@ Rigid3d Image::CamFromWorldValue() const {
 const Rigid3d& Image::CamFromWorld() const {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
-         "composition with rig calibration is needed";
+         "composition with rig calibration is needed.";
   return frame_->FrameFromWorld();
 }
 
 Rigid3d& Image::CamFromWorld() {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
-         "composition with rig calibration is needed";
+         "composition with rig calibration is needed.";
   return frame_->FrameFromWorld();
 }
 
 const std::optional<Rigid3d>& Image::MaybeCamFromWorld() const {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
-         "composition with rig calibration is needed";
+         "composition with rig calibration is needed.";
   return frame_->MaybeFrameFromWorld();
 }
 
 std::optional<Rigid3d>& Image::MaybeCamFromWorld() {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
-         "composition with rig calibration is needed";
+         "composition with rig calibration is needed.";
   return frame_->MaybeFrameFromWorld();
 }
 
 void Image::SetCamFromWorld(const Rigid3d& cam_from_world) {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
-         "composition with rig calibration is needed";
+         "composition with rig calibration is needed.";
   frame_->SetFrameFromWorld(cam_from_world);
 }
 
 void Image::SetCamFromWorld(const std::optional<Rigid3d>& cam_from_world) {
   THROW_CHECK(!HasNonTrivialFrame())
       << "No reference available for cam_from_world transformation, since "
-         "composition with rig calibration is needed";
+         "composition with rig calibration is needed.";
   frame_->SetFrameFromWorld(cam_from_world);
 }
 
 bool Image::HasPose() const { return frame_->HasPose(); }
 
-void Image::ResetPose() { frame_->ResetPose(); }
+void Image::ResetPose() {
+  THROW_CHECK(!HasNonTrivialFrame())
+      << "Resetting the global pose of a non-trivial frame (rig) from a single "
+         "sensor is not supported.";
+  frame_->ResetPose();
+}
 
 const struct Point2D& Image::Point2D(const point2D_t point2D_idx) const {
   return points2D_.at(point2D_idx);
@@ -305,9 +315,14 @@ const std::vector<struct Point2D>& Image::Points2D() const { return points2D_; }
 std::vector<struct Point2D>& Image::Points2D() { return points2D_; }
 
 bool Image::operator==(const Image& other) const {
-  return image_id_ == other.image_id_ && camera_id_ == other.camera_id_ &&
-         name_ == other.name_ && num_points3D_ == other.num_points3D_ &&
-         Frame() == other.Frame() && points2D_ == other.points2D_;
+  bool res = image_id_ == other.image_id_ && camera_id_ == other.camera_id_ &&
+             name_ == other.name_ && num_points3D_ == other.num_points3D_ &&
+             HasPose() == other.HasPose() && points2D_ == other.points2D_;
+  if (!HasPose()) {
+    return res;
+  } else {
+    return res && CamFromWorldValue() == other.CamFromWorldValue();
+  }
 }
 
 bool Image::operator!=(const Image& other) const { return !(*this == other); }
diff --git a/src/colmap/scene/image_test.cc b/src/colmap/scene/image_test.cc
index be1400ad6b..7f51a232f0 100644
--- a/src/colmap/scene/image_test.cc
+++ b/src/colmap/scene/image_test.cc
@@ -124,6 +124,28 @@ TEST(Image, SetResetPose) {
   EXPECT_ANY_THROW(image.CamFromWorld());
 }
 
+TEST(Image, ConstructCopy) {
+  Image image;
+  image.SetCamFromWorld(Rigid3d());
+  Image image_copy = Image(image);
+  EXPECT_EQ(image, image_copy);
+  EXPECT_EQ(Rigid3d(), image_copy.CamFromWorld());
+  image_copy.ResetPose();
+  EXPECT_TRUE(image.HasPose());
+  EXPECT_FALSE(image_copy.HasPose());
+}
+
+TEST(Image, AssignCopy) {
+  Image image;
+  image.SetCamFromWorld(Rigid3d());
+  Image image_copy = image;
+  EXPECT_EQ(image, image_copy);
+  EXPECT_EQ(Rigid3d(), image_copy.CamFromWorld());
+  image_copy.ResetPose();
+  EXPECT_TRUE(image.HasPose());
+  EXPECT_FALSE(image_copy.HasPose());
+}
+
 TEST(Image, NumPoints2D) {
   Image image;
   EXPECT_EQ(image.NumPoints2D(), 0);

From ed5aa77554c7946628e1eb0ba285e6e054d107c3 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 18:14:44 +0100
Subject: [PATCH 30/36] minor fix

---
 src/colmap/scene/frame.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index ab971678dc..e73530fc2f 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -74,7 +74,7 @@ struct data_t {
   // Unique identifier of the data (measurement)
   // This can be image_t / imu_measurement_t (not supported yet)
   uint32_t id;
-  data_t(const sensor_t& sensor_id, uint64_t id)
+  data_t(const sensor_t& sensor_id, uint32_t id)
       : sensor_id(sensor_id), id(id) {}
 
   inline bool operator<(const data_t& other) const {

From 2519bfa3f358df8bb98154d7b4fe261fc1780694 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Tue, 4 Feb 2025 18:38:21 +0100
Subject: [PATCH 31/36] make clang-tidy happy

---
 src/colmap/scene/reconstruction.cc         | 4 ++--
 src/colmap/scene/reconstruction_io.cc      | 4 ++--
 src/colmap/scene/reconstruction_test.cc    | 2 +-
 src/colmap/scene/synthetic.cc              | 2 +-
 src/colmap/sfm/observation_manager_test.cc | 2 +-
 5 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/colmap/scene/reconstruction.cc b/src/colmap/scene/reconstruction.cc
index 5641002b9f..d2a899247b 100644
--- a/src/colmap/scene/reconstruction.cc
+++ b/src/colmap/scene/reconstruction.cc
@@ -267,7 +267,7 @@ void Reconstruction::DeleteAllPoints2DAndPoints3D() {
     new_image.SetCameraId(image.second.CameraId());
     new_image.SetCameraPtr(image.second.CameraPtr());
     new_image.SetCamFromWorld(image.second.MaybeCamFromWorld());
-    image.second = std::move(new_image);
+    image.second = new_image;
   }
 }
 
@@ -400,7 +400,7 @@ Reconstruction Reconstruction::Crop(const Eigen::AlignedBox3d& bbox) const {
     for (point2D_t point2D_idx = 0; point2D_idx < num_points2D; ++point2D_idx) {
       new_image.ResetPoint3DForPoint2D(point2D_idx);
     }
-    cropped_reconstruction.AddImage(std::move(new_image));
+    cropped_reconstruction.AddImage(new_image);
   }
   std::unordered_set<image_t> registered_image_ids;
   for (const auto& point3D : points3D_) {
diff --git a/src/colmap/scene/reconstruction_io.cc b/src/colmap/scene/reconstruction_io.cc
index 76fe045867..cea0b2bccb 100644
--- a/src/colmap/scene/reconstruction_io.cc
+++ b/src/colmap/scene/reconstruction_io.cc
@@ -214,7 +214,7 @@ void ReadImagesText(Reconstruction& reconstruction, std::istream& stream) {
       }
     }
 
-    reconstruction.AddImage(std::move(image));
+    reconstruction.AddImage(image);
   }
 }
 
@@ -377,7 +377,7 @@ void ReadImagesBinary(Reconstruction& reconstruction, std::istream& stream) {
       }
     }
 
-    reconstruction.AddImage(std::move(image));
+    reconstruction.AddImage(image);
   }
 }
 
diff --git a/src/colmap/scene/reconstruction_test.cc b/src/colmap/scene/reconstruction_test.cc
index 7eb1a16c5a..acc1f78fef 100644
--- a/src/colmap/scene/reconstruction_test.cc
+++ b/src/colmap/scene/reconstruction_test.cc
@@ -90,7 +90,7 @@ void GenerateReconstruction(const image_t num_images,
     image.SetPoints2D(
         std::vector<Eigen::Vector2d>(kNumPoints2D, Eigen::Vector2d::Zero()));
     image.SetCamFromWorld(Rigid3d());
-    reconstruction->AddImage(std::move(image));
+    reconstruction->AddImage(image);
   }
 }
 
diff --git a/src/colmap/scene/synthetic.cc b/src/colmap/scene/synthetic.cc
index fd8d2298c0..47f1d93352 100644
--- a/src/colmap/scene/synthetic.cc
+++ b/src/colmap/scene/synthetic.cc
@@ -314,7 +314,7 @@ void SynthesizeDataset(const SyntheticDatasetOptions& options,
 
     image.SetImageId(image_id);
     image.SetPoints2D(points2D);
-    reconstruction->AddImage(std::move(image));
+    reconstruction->AddImage(image);
     reconstruction->RegisterImage(image_id);
   }
 
diff --git a/src/colmap/sfm/observation_manager_test.cc b/src/colmap/sfm/observation_manager_test.cc
index ef48d5c560..d7e159d218 100644
--- a/src/colmap/sfm/observation_manager_test.cc
+++ b/src/colmap/sfm/observation_manager_test.cc
@@ -51,7 +51,7 @@ void GenerateReconstruction(const image_t num_images,
     image.SetPoints2D(
         std::vector<Eigen::Vector2d>(kNumPoints2D, Eigen::Vector2d::Zero()));
     image.SetCamFromWorld(Rigid3d());
-    reconstruction.AddImage(std::move(image));
+    reconstruction.AddImage(image);
   }
 }
 

From c14fdf66df193596fd1f1dc8a03752d93b1877f8 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 5 Feb 2025 00:08:38 +0100
Subject: [PATCH 32/36] improve code + add tests for frame.h

---
 src/colmap/scene/CMakeLists.txt |   5 ++
 src/colmap/scene/frame.cc       |   6 +-
 src/colmap/scene/frame.h        | 138 +++++++++++++++++++++++++-------
 src/colmap/scene/frame_test.cc  | 104 ++++++++++++++++++++++++
 src/colmap/scene/image.h        |   2 +-
 5 files changed, 221 insertions(+), 34 deletions(-)
 create mode 100644 src/colmap/scene/frame_test.cc

diff --git a/src/colmap/scene/CMakeLists.txt b/src/colmap/scene/CMakeLists.txt
index 77bddd2672..7cfbcce590 100644
--- a/src/colmap/scene/CMakeLists.txt
+++ b/src/colmap/scene/CMakeLists.txt
@@ -85,6 +85,11 @@ COLMAP_ADD_TEST(
     SRCS database_test.cc
     LINK_LIBS colmap_scene
 )
+COLMAP_ADD_TEST(
+    NAME frame_test
+    SRCS frame_test.cc
+    LINK_LIBS colmap_scene
+)
 COLMAP_ADD_TEST(
     NAME image_test
     SRCS image_test.cc
diff --git a/src/colmap/scene/frame.cc b/src/colmap/scene/frame.cc
index a45b689d8c..7c15bf2e45 100644
--- a/src/colmap/scene/frame.cc
+++ b/src/colmap/scene/frame.cc
@@ -31,8 +31,6 @@
 
 namespace colmap {
 
-RigCalibration::RigCalibration() {}
-
 void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
   THROW_CHECK(ref_sensor_id_ == kInvalidSensorId)
       << "Reference sensor already set";
@@ -40,7 +38,7 @@ void RigCalibration::AddRefSensor(sensor_t ref_sensor_id) {
 }
 
 void RigCalibration::AddSensor(sensor_t sensor_id,
-                               const Rigid3d& sensor_from_rig) {
+                               const std::optional<Rigid3d>& sensor_from_rig) {
   THROW_CHECK(NumSensors() >= 1)
       << "The reference sensor needs to added first before any "
          "sensor being added.";
@@ -51,6 +49,4 @@ void RigCalibration::AddSensor(sensor_t sensor_id,
   sensors_from_rig_.emplace(sensor_id, sensor_from_rig);
 }
 
-Frame::Frame() {}
-
 }  // namespace colmap
diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index e73530fc2f..bf4effcf6d 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -53,7 +53,8 @@ struct sensor_t {
   // Unique identifier of the sensor.
   // This can be camera_t / imu_t (not supported yet)
   uint32_t id;
-  sensor_t(const SensorType& type, uint32_t id) : type(type), id(id) {}
+  constexpr sensor_t(const SensorType& type, uint32_t id)
+      : type(type), id(id) {}
 
   inline bool operator<(const sensor_t& other) const {
     return std::tie(type, id) < std::tie(other.type, other.id);
@@ -65,7 +66,7 @@ struct sensor_t {
     return !(*this == other);
   }
 };
-const sensor_t kInvalidSensorId =
+constexpr sensor_t kInvalidSensorId =
     sensor_t(SensorType::INVALID, std::numeric_limits<uint32_t>::max());
 
 struct data_t {
@@ -74,7 +75,7 @@ struct data_t {
   // Unique identifier of the data (measurement)
   // This can be image_t / imu_measurement_t (not supported yet)
   uint32_t id;
-  data_t(const sensor_t& sensor_id, uint32_t id)
+  constexpr data_t(const sensor_t& sensor_id, uint32_t id)
       : sensor_id(sensor_id), id(id) {}
 
   inline bool operator<(const data_t& other) const {
@@ -87,7 +88,7 @@ struct data_t {
     return !(*this == other);
   }
 };
-const data_t kInvalidDataId =
+constexpr data_t kInvalidDataId =
     data_t(kInvalidSensorId, std::numeric_limits<uint32_t>::max());
 
 // Rig calibration storing the sensor from rig transformation.
@@ -99,17 +100,17 @@ const data_t kInvalidDataId =
 // not ideal particularly when it comes to covariance estimation.
 class RigCalibration {
  public:
-  RigCalibration();
+  RigCalibration() = default;
 
   // Access the unique identifier of the rig
   inline rig_t RigId() const;
   inline void SetRigId(rig_t rig_id);
 
-  // Add sensor into the rig
-  // ``AddRefSensor`` needs to called first before all the ``AddSensor``
-  // operation
+  // Add sensor into the rig. ``AddRefSensor`` needs to called first before all
+  // the ``AddSensor`` operations
   void AddRefSensor(sensor_t ref_sensor_id);
-  void AddSensor(sensor_t sensor_id, const Rigid3d& sensor_from_rig);
+  void AddSensor(sensor_t sensor_id,
+                 const std::optional<Rigid3d>& sensor_from_rig = std::nullopt);
 
   // Check whether the sensor exists in the rig
   inline bool HasSensor(sensor_t sensor_id) const;
@@ -121,26 +122,35 @@ class RigCalibration {
   inline sensor_t RefSensorId() const;
 
   // Check if the sensor is the reference sensor of the rig
-  inline bool IsReference(sensor_t sensor_id) const;
+  inline bool IsRefSensor(sensor_t sensor_id) const;
 
   // Access sensor from rig transformations
   inline Rigid3d& SensorFromRig(sensor_t sensor_id);
   inline const Rigid3d& SensorFromRig(sensor_t sensor_id) const;
+  inline std::optional<Rigid3d>& MaybeSensorFromRig(sensor_t sensor_id);
+  inline const std::optional<Rigid3d>& MaybeSensorFromRig(
+      sensor_t sensor_id) const;
+  inline void SetSensorFromRig(sensor_t sensor_id,
+                               const Rigid3d& sensor_from_rig);
+  inline void SetSensorFromRig(sensor_t sensor_id,
+                               const std::optional<Rigid3d>& sensor_from_rig);
+  inline bool HasSensorFromRig(sensor_t sensor_id) const;
+  inline void ResetSensorFromRig(sensor_t sensor_id);
 
  private:
   // Unique identifier of the device.
-  rig_t rig_id_;
+  rig_t rig_id_ = kInvalidRigId;
 
   // Reference sensor id which has the identity transformation to the rig.
   sensor_t ref_sensor_id_ = kInvalidSensorId;
 
   // sensor_from_rig transformation.
-  std::map<sensor_t, Rigid3d> sensors_from_rig_;
+  std::map<sensor_t, std::optional<Rigid3d>> sensors_from_rig_;
 };
 
 class Frame {
  public:
-  Frame();
+  Frame() = default;
 
   // Access the unique identifier of the frame
   inline frame_t FrameId() const;
@@ -154,10 +164,6 @@ class Frame {
   // Check whether the data id is existent in the frame
   inline bool HasData(data_t data_id) const;
 
-  // Access the unique identifier of the rig
-  inline rig_t RigId() const;
-  inline void SetRigId(rig_t rig_id);
-
   // Access the rig calibration
   inline const std::shared_ptr<class RigCalibration>& RigCalibration() const;
   inline void SetRigCalibration(
@@ -186,6 +192,8 @@ class Frame {
   // If the rig calibration is a nullptr, the frame becomes a single sensor
   // case, where rig modeling is no longer needed.
   std::optional<Rigid3d> frame_from_world_;
+
+  // [Optional] Rig calibration
   rig_t rig_id_ = kInvalidRigId;
   std::shared_ptr<class RigCalibration> rig_calibration_ = nullptr;
 };
@@ -211,12 +219,12 @@ size_t RigCalibration::NumSensors() const {
 
 sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
 
-bool RigCalibration::IsReference(sensor_t sensor_id) const {
+bool RigCalibration::IsRefSensor(sensor_t sensor_id) const {
   return sensor_id == ref_sensor_id_;
 }
 
 Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
-  THROW_CHECK(!IsReference(sensor_id))
+  THROW_CHECK(!IsRefSensor(sensor_id))
       << "No reference is available for the SensorFromRig transformation of "
          "the reference sensor, which is identity";
   if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
@@ -224,15 +232,43 @@ Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
         "Sensor id (%d, %d) not found in the rig calibration",
         sensor_id.type,
         sensor_id.id);
-  return sensors_from_rig_.at(sensor_id);
+  THROW_CHECK(sensors_from_rig_.at(sensor_id))
+      << "The corresponding sensor does not have a valid transformation.";
+  return *sensors_from_rig_.at(sensor_id);
 }
 
 const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
-  THROW_CHECK(!IsReference(sensor_id))
+  THROW_CHECK(!IsRefSensor(sensor_id))
       << "No reference is available for the SensorFromRig transformation of "
          "the reference sensor, which is identity";
-  auto it = sensors_from_rig_.find(sensor_id);
-  if (it == sensors_from_rig_.end())
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  THROW_CHECK(sensors_from_rig_.at(sensor_id))
+      << "The corresponding sensor does not have a valid transformation.";
+  return *sensors_from_rig_.at(sensor_id);
+}
+
+std::optional<Rigid3d>& RigCalibration::MaybeSensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  return sensors_from_rig_.at(sensor_id);
+}
+
+const std::optional<Rigid3d>& RigCalibration::MaybeSensorFromRig(
+    sensor_t sensor_id) const {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
     LOG(FATAL_THROW) << StringPrintf(
         "Sensor id (%d, %d) not found in the rig calibration",
         sensor_id.type,
@@ -240,6 +276,56 @@ const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
   return sensors_from_rig_.at(sensor_id);
 }
 
+void RigCalibration::SetSensorFromRig(sensor_t sensor_id,
+                                      const Rigid3d& sensor_from_rig) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "Cannot set the SensorFromRig transformation of the reference sensor, "
+         "which is fixed to identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  sensors_from_rig_.at(sensor_id) = sensor_from_rig;
+}
+
+void RigCalibration::SetSensorFromRig(
+    sensor_t sensor_id, const std::optional<Rigid3d>& sensor_from_rig) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "Cannot set the SensorFromRig transformation of the reference sensor, "
+         "which is fixed to identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  sensors_from_rig_.at(sensor_id) = sensor_from_rig;
+}
+
+bool RigCalibration::HasSensorFromRig(sensor_t sensor_id) const {
+  if (IsRefSensor(sensor_id))
+    return true;  // SensorFromRig for the reference sensor is always identity
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  return sensors_from_rig_.at(sensor_id).has_value();
+}
+
+void RigCalibration::ResetSensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "Cannot reset the SensorFromRig transformation of the reference "
+         "sensor, "
+         "which is fixed to identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  sensors_from_rig_.at(sensor_id).reset();
+}
+
 frame_t Frame::FrameId() const { return frame_id_; }
 
 void Frame::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
@@ -254,10 +340,6 @@ bool Frame::HasData(data_t data_id) const {
   return data_ids_.find(data_id) != data_ids_.end();
 }
 
-rig_t Frame::RigId() const { return rig_id_; }
-
-void Frame::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
-
 const std::shared_ptr<class RigCalibration>& Frame::RigCalibration() const {
   return rig_calibration_;
 }
@@ -305,7 +387,7 @@ bool Frame::HasPose() const { return frame_from_world_.has_value(); }
 void Frame::ResetPose() { frame_from_world_.reset(); }
 
 Rigid3d Frame::SensorFromWorld(sensor_t sensor_id) const {
-  if (!HasRigCalibration() || rig_calibration_->IsReference(sensor_id)) {
+  if (!HasRigCalibration() || rig_calibration_->IsRefSensor(sensor_id)) {
     return FrameFromWorld();
   }
   THROW_CHECK(rig_calibration_->HasSensor(sensor_id));
diff --git a/src/colmap/scene/frame_test.cc b/src/colmap/scene/frame_test.cc
new file mode 100644
index 0000000000..7313ca8433
--- /dev/null
+++ b/src/colmap/scene/frame_test.cc
@@ -0,0 +1,104 @@
+// Copyright (c), ETH Zurich and UNC Chapel Hill.
+// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "colmap/scene/frame.h"
+
+#include <gtest/gtest.h>
+
+namespace colmap {
+namespace {
+
+Rigid3d TestRigid3d() {
+  return Rigid3d(Eigen::Quaterniond::UnitRandom(), Eigen::Vector3d::Random());
+}
+
+TEST(RigCalibration, Default) {
+  RigCalibration calib;
+  EXPECT_EQ(calib.RigId(), kInvalidRigId);
+  EXPECT_EQ(calib.RefSensorId(), kInvalidSensorId);
+  EXPECT_EQ(calib.NumSensors(), 0);
+}
+
+TEST(RigCalibration, SetUp) {
+  RigCalibration calib;
+  calib.AddRefSensor(sensor_t(SensorType::IMU, 0));
+  calib.AddSensor(sensor_t(SensorType::IMU, 1), TestRigid3d());
+  calib.AddSensor(sensor_t(SensorType::CAMERA, 0), TestRigid3d());
+  calib.AddSensor(sensor_t(SensorType::CAMERA, 1)); // no input sensor_from_rig
+
+  EXPECT_EQ(calib.NumSensors(), 4);
+  EXPECT_EQ(calib.RefSensorId().type, SensorType::IMU);
+  EXPECT_EQ(calib.RefSensorId().id, 0);
+  EXPECT_TRUE(calib.IsRefSensor(sensor_t(SensorType::IMU, 0)));
+  EXPECT_FALSE(calib.IsRefSensor(sensor_t(SensorType::IMU, 1)));
+  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::IMU, 0)));
+  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::IMU, 1)));
+  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::CAMERA, 0)));
+  EXPECT_FALSE(calib.HasSensorFromRig(sensor_t(SensorType::CAMERA, 1)));
+  EXPECT_TRUE(calib.HasSensor(sensor_t(SensorType::CAMERA, 1)));
+}
+
+TEST(Frame, Default) {
+  Frame frame;
+  EXPECT_EQ(frame.FrameId(), kInvalidFrameId);
+  EXPECT_FALSE(frame.HasPose());
+  EXPECT_FALSE(frame.HasRigCalibration());
+}
+
+TEST(Frame, SetUp) {
+  sensor_t s1(SensorType::IMU, 0);
+  sensor_t s2(SensorType::CAMERA, 0);
+  std::shared_ptr<RigCalibration> calib = std::make_shared<RigCalibration>();
+  calib->AddRefSensor(s1);
+  calib->AddSensor(s2, TestRigid3d());
+
+  Frame frame;
+  frame.AddData(data_t(s1, 2));
+  frame.AddData(data_t(s2, 5));
+  frame.SetRigCalibration(calib);
+  EXPECT_TRUE(frame.HasRigCalibration());
+  EXPECT_FALSE(frame.HasPose());
+}
+
+TEST(Frame, SetResetPose) {
+  Frame frame;
+  EXPECT_FALSE(frame.HasPose());
+  EXPECT_ANY_THROW(frame.FrameFromWorld());
+  frame.SetFrameFromWorld(Rigid3d());
+  EXPECT_TRUE(frame.HasPose());
+  EXPECT_EQ(frame.FrameFromWorld().rotation.coeffs(),
+            Eigen::Quaterniond::Identity().coeffs());
+  EXPECT_EQ(frame.FrameFromWorld().translation, Eigen::Vector3d::Zero());
+  frame.ResetPose();
+  EXPECT_FALSE(frame.HasPose());
+  EXPECT_ANY_THROW(frame.FrameFromWorld());
+}
+
+}  // namespace
+}  // namespace colmap
diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 4463128876..7fd1d21e75 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -242,7 +242,7 @@ void Image::SetFrame(std::shared_ptr<class Frame> frame) {
 bool Image::HasNonTrivialFrame() const {
   THROW_CHECK(frame_) << "Invalid pointer to the corresponding frame";
   return frame_->HasRigCalibration() &&
-         !frame_->RigCalibration()->IsReference(
+         !frame_->RigCalibration()->IsRefSensor(
              sensor_t(SensorType::CAMERA, CameraId()));
 }
 

From 83fabbd4ca27a753d4de3985b0ed3ac98806a066 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 5 Feb 2025 00:11:27 +0100
Subject: [PATCH 33/36] format

---
 src/colmap/scene/frame_test.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/colmap/scene/frame_test.cc b/src/colmap/scene/frame_test.cc
index 7313ca8433..d492eca8a9 100644
--- a/src/colmap/scene/frame_test.cc
+++ b/src/colmap/scene/frame_test.cc
@@ -50,7 +50,7 @@ TEST(RigCalibration, SetUp) {
   calib.AddRefSensor(sensor_t(SensorType::IMU, 0));
   calib.AddSensor(sensor_t(SensorType::IMU, 1), TestRigid3d());
   calib.AddSensor(sensor_t(SensorType::CAMERA, 0), TestRigid3d());
-  calib.AddSensor(sensor_t(SensorType::CAMERA, 1)); // no input sensor_from_rig
+  calib.AddSensor(sensor_t(SensorType::CAMERA, 1));  // no input sensor_from_rig
 
   EXPECT_EQ(calib.NumSensors(), 4);
   EXPECT_EQ(calib.RefSensorId().type, SensorType::IMU);

From 9ae245cc54ec99ee629b64fd932d8f6de4d5ede7 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 5 Feb 2025 09:42:21 +0100
Subject: [PATCH 34/36] update

---
 src/colmap/scene/frame.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index bf4effcf6d..a585ae20f2 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -194,7 +194,6 @@ class Frame {
   std::optional<Rigid3d> frame_from_world_;
 
   // [Optional] Rig calibration
-  rig_t rig_id_ = kInvalidRigId;
   std::shared_ptr<class RigCalibration> rig_calibration_ = nullptr;
 };
 

From 37ce8e6191f1a4b3d80881fb51d5a5cbbf88332f Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 5 Feb 2025 22:45:05 +0100
Subject: [PATCH 35/36] rename CamFromWorldValue to ComposeCamFromWorld

---
 src/colmap/scene/image.h    |  6 +++---
 src/pycolmap/scene/image.cc | 11 +++++------
 2 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
index 7fd1d21e75..64a1256ba4 100644
--- a/src/colmap/scene/image.h
+++ b/src/colmap/scene/image.h
@@ -101,7 +101,7 @@ class Image {
   // World to camera pose.
   // Get the value (copy) of cam_from_world. This also supports non-trivial
   // frame (rig).
-  inline Rigid3d CamFromWorldValue() const;
+  inline Rigid3d ComposeCamFromWorld() const;
 
   // The following methods only works for non-composited pose. Will throw an
   // error if the image has a non-trivial frame (rig) attached to it.
@@ -246,7 +246,7 @@ bool Image::HasNonTrivialFrame() const {
              sensor_t(SensorType::CAMERA, CameraId()));
 }
 
-Rigid3d Image::CamFromWorldValue() const {
+Rigid3d Image::ComposeCamFromWorld() const {
   sensor_t sensor_id = sensor_t(SensorType::CAMERA, CameraId());
   return frame_->SensorFromWorld(sensor_id);
 }
@@ -321,7 +321,7 @@ bool Image::operator==(const Image& other) const {
   if (!HasPose()) {
     return res;
   } else {
-    return res && CamFromWorldValue() == other.CamFromWorldValue();
+    return res && ComposeCamFromWorld() == other.ComposeCamFromWorld();
   }
 }
 
diff --git a/src/pycolmap/scene/image.cc b/src/pycolmap/scene/image.cc
index 2f84bfee41..c996b456a4 100644
--- a/src/pycolmap/scene/image.cc
+++ b/src/pycolmap/scene/image.cc
@@ -88,12 +88,11 @@ void BindImage(py::module& m) {
           "The pose of the image, defined as the transformation from world to "
           "camera space. None if the image is not registered. Will throw an "
           "error if a non-trivial frame (rig) is present.")
-      .def_property_readonly(
-          "cam_from_world_value",
-          &Image::CamFromWorldValue,
-          "The pose of the image, defined as the transformation from world to "
-          "camera space. This method is read-only and support non-trivial "
-          "frame (rig).")
+      .def("compose_cam_from_world",
+           &Image::ComposeCamFromWorld,
+           "The pose of the image, defined as the transformation from world to "
+           "camera space. This method is read-only and support non-trivial "
+           "frame (rig).")
       .def_property_readonly(
           "has_pose", &Image::HasPose, "Whether the image has a valid pose.")
       .def("reset_pose", &Image::ResetPose, "Invalidate the pose of the image.")

From 2fa5bafaf28289f3cfa3b492ddd75b389ad9dd42 Mon Sep 17 00:00:00 2001
From: B1ueber2y <b1ueber2y@gmail.com>
Date: Wed, 5 Feb 2025 23:04:48 +0100
Subject: [PATCH 36/36] move RigCalibration to separate file at
 sensor/rig_calib.h

---
 src/colmap/scene/CMakeLists.txt               |   2 +-
 src/colmap/scene/frame.h                      | 215 +-------------
 src/colmap/scene/frame_test.cc                |  26 --
 src/colmap/sensor/CMakeLists.txt              |  11 +-
 .../{scene/frame.cc => sensor/rig_calib.cc}   |   2 +-
 src/colmap/sensor/rig_calib.h                 | 268 ++++++++++++++++++
 src/colmap/sensor/rig_calib_test.cc           |  68 +++++
 7 files changed, 348 insertions(+), 244 deletions(-)
 rename src/colmap/{scene/frame.cc => sensor/rig_calib.cc} (98%)
 create mode 100644 src/colmap/sensor/rig_calib.h
 create mode 100644 src/colmap/sensor/rig_calib_test.cc

diff --git a/src/colmap/scene/CMakeLists.txt b/src/colmap/scene/CMakeLists.txt
index 7cfbcce590..c6181c6971 100644
--- a/src/colmap/scene/CMakeLists.txt
+++ b/src/colmap/scene/CMakeLists.txt
@@ -38,7 +38,7 @@ COLMAP_ADD_LIBRARY(
         correspondence_graph.h correspondence_graph.cc
         database.h database.cc
         database_cache.h database_cache.cc
-        frame.h frame.cc
+        frame.h
         image.h image.cc
         point2d.h point2d.cc
         point3d.h point3d.cc
diff --git a/src/colmap/scene/frame.h b/src/colmap/scene/frame.h
index a585ae20f2..5bce9fddda 100644
--- a/src/colmap/scene/frame.h
+++ b/src/colmap/scene/frame.h
@@ -30,7 +30,7 @@
 #pragma once
 
 #include "colmap/geometry/rigid3.h"
-#include "colmap/util/enum_utils.h"
+#include "colmap/sensor/rig_calib.h"
 #include "colmap/util/types.h"
 
 #include <cstdint>
@@ -44,31 +44,6 @@
 
 namespace colmap {
 
-// Sensor type
-MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType, -1, INVALID, CAMERA, IMU);
-
-struct sensor_t {
-  // Type of the sensor (INVALID / CAMERA / IMU)
-  SensorType type;
-  // Unique identifier of the sensor.
-  // This can be camera_t / imu_t (not supported yet)
-  uint32_t id;
-  constexpr sensor_t(const SensorType& type, uint32_t id)
-      : type(type), id(id) {}
-
-  inline bool operator<(const sensor_t& other) const {
-    return std::tie(type, id) < std::tie(other.type, other.id);
-  }
-  inline bool operator==(const sensor_t& other) const {
-    return type == other.type && id == other.id;
-  }
-  inline bool operator!=(const sensor_t& other) const {
-    return !(*this == other);
-  }
-};
-constexpr sensor_t kInvalidSensorId =
-    sensor_t(SensorType::INVALID, std::numeric_limits<uint32_t>::max());
-
 struct data_t {
   // Unique identifer of the sensor
   sensor_t sensor_id;
@@ -91,63 +66,6 @@ struct data_t {
 constexpr data_t kInvalidDataId =
     data_t(kInvalidSensorId, std::numeric_limits<uint32_t>::max());
 
-// Rig calibration storing the sensor from rig transformation.
-// The reference sensor shares identity poses with the device.
-// This design is mainly for two purposes:
-// 1) In the visual-inertial optimization one of the IMUs is generally used as
-// the reference frame since it is metric.
-// 2) Not having a reference frame brings a 6 DoF Gauge for each rig, which is
-// not ideal particularly when it comes to covariance estimation.
-class RigCalibration {
- public:
-  RigCalibration() = default;
-
-  // Access the unique identifier of the rig
-  inline rig_t RigId() const;
-  inline void SetRigId(rig_t rig_id);
-
-  // Add sensor into the rig. ``AddRefSensor`` needs to called first before all
-  // the ``AddSensor`` operations
-  void AddRefSensor(sensor_t ref_sensor_id);
-  void AddSensor(sensor_t sensor_id,
-                 const std::optional<Rigid3d>& sensor_from_rig = std::nullopt);
-
-  // Check whether the sensor exists in the rig
-  inline bool HasSensor(sensor_t sensor_id) const;
-
-  // Count the number of sensors available in the rig
-  inline size_t NumSensors() const;
-
-  // Access the reference sensor id (default to be the first added sensor)
-  inline sensor_t RefSensorId() const;
-
-  // Check if the sensor is the reference sensor of the rig
-  inline bool IsRefSensor(sensor_t sensor_id) const;
-
-  // Access sensor from rig transformations
-  inline Rigid3d& SensorFromRig(sensor_t sensor_id);
-  inline const Rigid3d& SensorFromRig(sensor_t sensor_id) const;
-  inline std::optional<Rigid3d>& MaybeSensorFromRig(sensor_t sensor_id);
-  inline const std::optional<Rigid3d>& MaybeSensorFromRig(
-      sensor_t sensor_id) const;
-  inline void SetSensorFromRig(sensor_t sensor_id,
-                               const Rigid3d& sensor_from_rig);
-  inline void SetSensorFromRig(sensor_t sensor_id,
-                               const std::optional<Rigid3d>& sensor_from_rig);
-  inline bool HasSensorFromRig(sensor_t sensor_id) const;
-  inline void ResetSensorFromRig(sensor_t sensor_id);
-
- private:
-  // Unique identifier of the device.
-  rig_t rig_id_ = kInvalidRigId;
-
-  // Reference sensor id which has the identity transformation to the rig.
-  sensor_t ref_sensor_id_ = kInvalidSensorId;
-
-  // sensor_from_rig transformation.
-  std::map<sensor_t, std::optional<Rigid3d>> sensors_from_rig_;
-};
-
 class Frame {
  public:
   Frame() = default;
@@ -201,130 +119,6 @@ class Frame {
 // Implementation
 ////////////////////////////////////////////////////////////////////////////////
 
-rig_t RigCalibration::RigId() const { return rig_id_; }
-
-void RigCalibration::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
-
-bool RigCalibration::HasSensor(sensor_t sensor_id) const {
-  return sensor_id == ref_sensor_id_ ||
-         sensors_from_rig_.find(sensor_id) != sensors_from_rig_.end();
-}
-
-size_t RigCalibration::NumSensors() const {
-  size_t n_sensors = sensors_from_rig_.size();
-  if (ref_sensor_id_ != kInvalidSensorId) n_sensors += 1;
-  return n_sensors;
-}
-
-sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
-
-bool RigCalibration::IsRefSensor(sensor_t sensor_id) const {
-  return sensor_id == ref_sensor_id_;
-}
-
-Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "No reference is available for the SensorFromRig transformation of "
-         "the reference sensor, which is identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  THROW_CHECK(sensors_from_rig_.at(sensor_id))
-      << "The corresponding sensor does not have a valid transformation.";
-  return *sensors_from_rig_.at(sensor_id);
-}
-
-const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "No reference is available for the SensorFromRig transformation of "
-         "the reference sensor, which is identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  THROW_CHECK(sensors_from_rig_.at(sensor_id))
-      << "The corresponding sensor does not have a valid transformation.";
-  return *sensors_from_rig_.at(sensor_id);
-}
-
-std::optional<Rigid3d>& RigCalibration::MaybeSensorFromRig(sensor_t sensor_id) {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "No reference is available for the SensorFromRig transformation of "
-         "the reference sensor, which is identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  return sensors_from_rig_.at(sensor_id);
-}
-
-const std::optional<Rigid3d>& RigCalibration::MaybeSensorFromRig(
-    sensor_t sensor_id) const {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "No reference is available for the SensorFromRig transformation of "
-         "the reference sensor, which is identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  return sensors_from_rig_.at(sensor_id);
-}
-
-void RigCalibration::SetSensorFromRig(sensor_t sensor_id,
-                                      const Rigid3d& sensor_from_rig) {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "Cannot set the SensorFromRig transformation of the reference sensor, "
-         "which is fixed to identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  sensors_from_rig_.at(sensor_id) = sensor_from_rig;
-}
-
-void RigCalibration::SetSensorFromRig(
-    sensor_t sensor_id, const std::optional<Rigid3d>& sensor_from_rig) {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "Cannot set the SensorFromRig transformation of the reference sensor, "
-         "which is fixed to identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  sensors_from_rig_.at(sensor_id) = sensor_from_rig;
-}
-
-bool RigCalibration::HasSensorFromRig(sensor_t sensor_id) const {
-  if (IsRefSensor(sensor_id))
-    return true;  // SensorFromRig for the reference sensor is always identity
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  return sensors_from_rig_.at(sensor_id).has_value();
-}
-
-void RigCalibration::ResetSensorFromRig(sensor_t sensor_id) {
-  THROW_CHECK(!IsRefSensor(sensor_id))
-      << "Cannot reset the SensorFromRig transformation of the reference "
-         "sensor, "
-         "which is fixed to identity";
-  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
-    LOG(FATAL_THROW) << StringPrintf(
-        "Sensor id (%d, %d) not found in the rig calibration",
-        sensor_id.type,
-        sensor_id.id);
-  sensors_from_rig_.at(sensor_id).reset();
-}
-
 frame_t Frame::FrameId() const { return frame_id_; }
 
 void Frame::SetFrameId(frame_t frame_id) { frame_id_ = frame_id; }
@@ -396,13 +190,6 @@ Rigid3d Frame::SensorFromWorld(sensor_t sensor_id) const {
 }  // namespace colmap
 
 namespace std {
-template <>
-struct hash<colmap::sensor_t> {
-  std::size_t operator()(const colmap::sensor_t& s) const noexcept {
-    return std::hash<std::pair<uint32_t, uint32_t>>{}(
-        std::make_pair(static_cast<uint32_t>(s.type), s.id));
-  }
-};
 
 // [Reference]
 // https://stackoverflow.com/questions/26705751/why-is-the-magic-number-in-boosthash-combine-specified-in-hex
diff --git a/src/colmap/scene/frame_test.cc b/src/colmap/scene/frame_test.cc
index d492eca8a9..0d6866ca26 100644
--- a/src/colmap/scene/frame_test.cc
+++ b/src/colmap/scene/frame_test.cc
@@ -38,32 +38,6 @@ Rigid3d TestRigid3d() {
   return Rigid3d(Eigen::Quaterniond::UnitRandom(), Eigen::Vector3d::Random());
 }
 
-TEST(RigCalibration, Default) {
-  RigCalibration calib;
-  EXPECT_EQ(calib.RigId(), kInvalidRigId);
-  EXPECT_EQ(calib.RefSensorId(), kInvalidSensorId);
-  EXPECT_EQ(calib.NumSensors(), 0);
-}
-
-TEST(RigCalibration, SetUp) {
-  RigCalibration calib;
-  calib.AddRefSensor(sensor_t(SensorType::IMU, 0));
-  calib.AddSensor(sensor_t(SensorType::IMU, 1), TestRigid3d());
-  calib.AddSensor(sensor_t(SensorType::CAMERA, 0), TestRigid3d());
-  calib.AddSensor(sensor_t(SensorType::CAMERA, 1));  // no input sensor_from_rig
-
-  EXPECT_EQ(calib.NumSensors(), 4);
-  EXPECT_EQ(calib.RefSensorId().type, SensorType::IMU);
-  EXPECT_EQ(calib.RefSensorId().id, 0);
-  EXPECT_TRUE(calib.IsRefSensor(sensor_t(SensorType::IMU, 0)));
-  EXPECT_FALSE(calib.IsRefSensor(sensor_t(SensorType::IMU, 1)));
-  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::IMU, 0)));
-  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::IMU, 1)));
-  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::CAMERA, 0)));
-  EXPECT_FALSE(calib.HasSensorFromRig(sensor_t(SensorType::CAMERA, 1)));
-  EXPECT_TRUE(calib.HasSensor(sensor_t(SensorType::CAMERA, 1)));
-}
-
 TEST(Frame, Default) {
   Frame frame;
   EXPECT_EQ(frame.FrameId(), kInvalidFrameId);
diff --git a/src/colmap/sensor/CMakeLists.txt b/src/colmap/sensor/CMakeLists.txt
index 12f780ed83..d52bfe75aa 100644
--- a/src/colmap/sensor/CMakeLists.txt
+++ b/src/colmap/sensor/CMakeLists.txt
@@ -36,11 +36,13 @@ COLMAP_ADD_LIBRARY(
         bitmap.h bitmap.cc
         database.h database.cc
         models.h models.cc
+        rig_calib.h rig_calib.cc
         specs.h specs.cc
     PUBLIC_LINK_LIBS
         Ceres::ceres
         Eigen3::Eigen
     PRIVATE_LINK_LIBS
+        colmap_geometry
         colmap_util
         colmap_vlfeat
         freeimage::FreeImage
@@ -53,13 +55,18 @@ COLMAP_ADD_TEST(
         colmap_sensor
         freeimage::FreeImage
 )
-COLMAP_ADD_TEST( 
+COLMAP_ADD_TEST(
     NAME database_test
     SRCS database_test.cc
     LINK_LIBS colmap_sensor
 )
-COLMAP_ADD_TEST( 
+COLMAP_ADD_TEST(
     NAME models_test
     SRCS models_test.cc
     LINK_LIBS colmap_sensor
 )
+COLMAP_ADD_TEST(
+    NAME rig_calib_test
+    SRCS rig_calib_test.cc
+    LINK_LIBS colmap_sensor
+)
diff --git a/src/colmap/scene/frame.cc b/src/colmap/sensor/rig_calib.cc
similarity index 98%
rename from src/colmap/scene/frame.cc
rename to src/colmap/sensor/rig_calib.cc
index 7c15bf2e45..4c2b08b44e 100644
--- a/src/colmap/scene/frame.cc
+++ b/src/colmap/sensor/rig_calib.cc
@@ -27,7 +27,7 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#include "colmap/scene/frame.h"
+#include "colmap/sensor/rig_calib.h"
 
 namespace colmap {
 
diff --git a/src/colmap/sensor/rig_calib.h b/src/colmap/sensor/rig_calib.h
new file mode 100644
index 0000000000..1d36779d49
--- /dev/null
+++ b/src/colmap/sensor/rig_calib.h
@@ -0,0 +1,268 @@
+// Copyright (c), ETH Zurich and UNC Chapel Hill.
+// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS 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 "colmap/geometry/rigid3.h"
+#include "colmap/util/enum_utils.h"
+#include "colmap/util/types.h"
+
+#include <cstdint>
+#include <functional>
+#include <map>
+#include <memory>
+#include <optional>
+#include <set>
+#include <tuple>
+#include <vector>
+
+namespace colmap {
+
+// Sensor type
+MAKE_ENUM_CLASS_OVERLOAD_STREAM(SensorType, -1, INVALID, CAMERA, IMU);
+
+struct sensor_t {
+  // Type of the sensor (INVALID / CAMERA / IMU)
+  SensorType type;
+  // Unique identifier of the sensor.
+  // This can be camera_t / imu_t (not supported yet)
+  uint32_t id;
+  constexpr sensor_t(const SensorType& type, uint32_t id)
+      : type(type), id(id) {}
+
+  inline bool operator<(const sensor_t& other) const {
+    return std::tie(type, id) < std::tie(other.type, other.id);
+  }
+  inline bool operator==(const sensor_t& other) const {
+    return type == other.type && id == other.id;
+  }
+  inline bool operator!=(const sensor_t& other) const {
+    return !(*this == other);
+  }
+};
+constexpr sensor_t kInvalidSensorId =
+    sensor_t(SensorType::INVALID, std::numeric_limits<uint32_t>::max());
+
+// Rig calibration storing the sensor from rig transformation.
+// The reference sensor shares identity poses with the device.
+// This design is mainly for two purposes:
+// 1) In the visual-inertial optimization one of the IMUs is generally used as
+// the reference frame since it is metric.
+// 2) Not having a reference frame brings a 6 DoF Gauge for each rig, which is
+// not ideal particularly when it comes to covariance estimation.
+class RigCalibration {
+ public:
+  RigCalibration() = default;
+
+  // Access the unique identifier of the rig
+  inline rig_t RigId() const;
+  inline void SetRigId(rig_t rig_id);
+
+  // Add sensor into the rig. ``AddRefSensor`` needs to called first before all
+  // the ``AddSensor`` operations
+  void AddRefSensor(sensor_t ref_sensor_id);
+  void AddSensor(sensor_t sensor_id,
+                 const std::optional<Rigid3d>& sensor_from_rig = std::nullopt);
+
+  // Check whether the sensor exists in the rig
+  inline bool HasSensor(sensor_t sensor_id) const;
+
+  // Count the number of sensors available in the rig
+  inline size_t NumSensors() const;
+
+  // Access the reference sensor id (default to be the first added sensor)
+  inline sensor_t RefSensorId() const;
+
+  // Check if the sensor is the reference sensor of the rig
+  inline bool IsRefSensor(sensor_t sensor_id) const;
+
+  // Access sensor from rig transformations
+  inline Rigid3d& SensorFromRig(sensor_t sensor_id);
+  inline const Rigid3d& SensorFromRig(sensor_t sensor_id) const;
+  inline std::optional<Rigid3d>& MaybeSensorFromRig(sensor_t sensor_id);
+  inline const std::optional<Rigid3d>& MaybeSensorFromRig(
+      sensor_t sensor_id) const;
+  inline void SetSensorFromRig(sensor_t sensor_id,
+                               const Rigid3d& sensor_from_rig);
+  inline void SetSensorFromRig(sensor_t sensor_id,
+                               const std::optional<Rigid3d>& sensor_from_rig);
+  inline bool HasSensorFromRig(sensor_t sensor_id) const;
+  inline void ResetSensorFromRig(sensor_t sensor_id);
+
+ private:
+  // Unique identifier of the device.
+  rig_t rig_id_ = kInvalidRigId;
+
+  // Reference sensor id which has the identity transformation to the rig.
+  sensor_t ref_sensor_id_ = kInvalidSensorId;
+
+  // sensor_from_rig transformation.
+  std::map<sensor_t, std::optional<Rigid3d>> sensors_from_rig_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+rig_t RigCalibration::RigId() const { return rig_id_; }
+
+void RigCalibration::SetRigId(rig_t rig_id) { rig_id_ = rig_id; }
+
+bool RigCalibration::HasSensor(sensor_t sensor_id) const {
+  return sensor_id == ref_sensor_id_ ||
+         sensors_from_rig_.find(sensor_id) != sensors_from_rig_.end();
+}
+
+size_t RigCalibration::NumSensors() const {
+  size_t n_sensors = sensors_from_rig_.size();
+  if (ref_sensor_id_ != kInvalidSensorId) n_sensors += 1;
+  return n_sensors;
+}
+
+sensor_t RigCalibration::RefSensorId() const { return ref_sensor_id_; }
+
+bool RigCalibration::IsRefSensor(sensor_t sensor_id) const {
+  return sensor_id == ref_sensor_id_;
+}
+
+Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  THROW_CHECK(sensors_from_rig_.at(sensor_id))
+      << "The corresponding sensor does not have a valid transformation.";
+  return *sensors_from_rig_.at(sensor_id);
+}
+
+const Rigid3d& RigCalibration::SensorFromRig(sensor_t sensor_id) const {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  THROW_CHECK(sensors_from_rig_.at(sensor_id))
+      << "The corresponding sensor does not have a valid transformation.";
+  return *sensors_from_rig_.at(sensor_id);
+}
+
+std::optional<Rigid3d>& RigCalibration::MaybeSensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  return sensors_from_rig_.at(sensor_id);
+}
+
+const std::optional<Rigid3d>& RigCalibration::MaybeSensorFromRig(
+    sensor_t sensor_id) const {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "No reference is available for the SensorFromRig transformation of "
+         "the reference sensor, which is identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  return sensors_from_rig_.at(sensor_id);
+}
+
+void RigCalibration::SetSensorFromRig(sensor_t sensor_id,
+                                      const Rigid3d& sensor_from_rig) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "Cannot set the SensorFromRig transformation of the reference sensor, "
+         "which is fixed to identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  sensors_from_rig_.at(sensor_id) = sensor_from_rig;
+}
+
+void RigCalibration::SetSensorFromRig(
+    sensor_t sensor_id, const std::optional<Rigid3d>& sensor_from_rig) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "Cannot set the SensorFromRig transformation of the reference sensor, "
+         "which is fixed to identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  sensors_from_rig_.at(sensor_id) = sensor_from_rig;
+}
+
+bool RigCalibration::HasSensorFromRig(sensor_t sensor_id) const {
+  if (IsRefSensor(sensor_id))
+    return true;  // SensorFromRig for the reference sensor is always identity
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  return sensors_from_rig_.at(sensor_id).has_value();
+}
+
+void RigCalibration::ResetSensorFromRig(sensor_t sensor_id) {
+  THROW_CHECK(!IsRefSensor(sensor_id))
+      << "Cannot reset the SensorFromRig transformation of the reference "
+         "sensor, "
+         "which is fixed to identity";
+  if (sensors_from_rig_.find(sensor_id) == sensors_from_rig_.end())
+    LOG(FATAL_THROW) << StringPrintf(
+        "Sensor id (%d, %d) not found in the rig calibration",
+        sensor_id.type,
+        sensor_id.id);
+  sensors_from_rig_.at(sensor_id).reset();
+}
+
+}  // namespace colmap
+
+namespace std {
+template <>
+struct hash<colmap::sensor_t> {
+  std::size_t operator()(const colmap::sensor_t& s) const noexcept {
+    return std::hash<std::pair<uint32_t, uint32_t>>{}(
+        std::make_pair(static_cast<uint32_t>(s.type), s.id));
+  }
+};
+
+}  // namespace std
diff --git a/src/colmap/sensor/rig_calib_test.cc b/src/colmap/sensor/rig_calib_test.cc
new file mode 100644
index 0000000000..7ac5b07394
--- /dev/null
+++ b/src/colmap/sensor/rig_calib_test.cc
@@ -0,0 +1,68 @@
+// Copyright (c), ETH Zurich and UNC Chapel Hill.
+// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "colmap/sensor/rig_calib.h"
+
+#include <gtest/gtest.h>
+
+namespace colmap {
+namespace {
+
+Rigid3d TestRigid3d() {
+  return Rigid3d(Eigen::Quaterniond::UnitRandom(), Eigen::Vector3d::Random());
+}
+
+TEST(RigCalibration, Default) {
+  RigCalibration calib;
+  EXPECT_EQ(calib.RigId(), kInvalidRigId);
+  EXPECT_EQ(calib.RefSensorId(), kInvalidSensorId);
+  EXPECT_EQ(calib.NumSensors(), 0);
+}
+
+TEST(RigCalibration, SetUp) {
+  RigCalibration calib;
+  calib.AddRefSensor(sensor_t(SensorType::IMU, 0));
+  calib.AddSensor(sensor_t(SensorType::IMU, 1), TestRigid3d());
+  calib.AddSensor(sensor_t(SensorType::CAMERA, 0), TestRigid3d());
+  calib.AddSensor(sensor_t(SensorType::CAMERA, 1));  // no input sensor_from_rig
+
+  EXPECT_EQ(calib.NumSensors(), 4);
+  EXPECT_EQ(calib.RefSensorId().type, SensorType::IMU);
+  EXPECT_EQ(calib.RefSensorId().id, 0);
+  EXPECT_TRUE(calib.IsRefSensor(sensor_t(SensorType::IMU, 0)));
+  EXPECT_FALSE(calib.IsRefSensor(sensor_t(SensorType::IMU, 1)));
+  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::IMU, 0)));
+  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::IMU, 1)));
+  EXPECT_TRUE(calib.HasSensorFromRig(sensor_t(SensorType::CAMERA, 0)));
+  EXPECT_FALSE(calib.HasSensorFromRig(sensor_t(SensorType::CAMERA, 1)));
+  EXPECT_TRUE(calib.HasSensor(sensor_t(SensorType::CAMERA, 1)));
+}
+
+}  // namespace
+}  // namespace colmap