Support time-varying camera intrinsics and zoom modeling

extensions/colmap/CMakeLists.txt

@@ -91,6 +91,7 @@ target_include_directories(
   vidmap_native_core
   PRIVATE
     src
+    ${VIDMAP_COLMAP_INCLUDE_DIRS}
 )
 target_link_libraries(vidmap_native_core PRIVATE vidmap_native_algorithms)
 target_compile_definitions(

extensions/colmap/src/bindings/bundle_adjustment_problem.cc

@@ -85,4 +85,20 @@ PYBIND11_MODULE(bundle_adjustment, m) {
                   focal,
                   stddev));
         });
+  m.def("relative_focal_prior_cost",
+        [](const colmap::Camera& camera1,
+           const colmap::Camera& camera2,
+           double target_log_ratio,
+           double sigma_log_ratio) {
+          const auto indices1 = camera1.FocalLengthIdxs();
+          const auto indices2 = camera2.FocalLengthIdxs();
+          return std::shared_ptr<ceres::CostFunction>(
+              new vidmap::LogRelativeFocalPriorCostFunction(
+                  camera1.params.size(),
+                  std::vector<std::size_t>(indices1.begin(), indices1.end()),
+                  camera2.params.size(),
+                  std::vector<std::size_t>(indices2.begin(), indices2.end()),
+                  target_log_ratio,
+                  sigma_log_ratio));
+        });
 }

extensions/colmap/src/bindings/records.cc

@@ -32,5 +32,15 @@ void BindRecords(py::module_& m) {
       .def_readwrite("observations", &LogFocalPriorRecord::observations)
       .def_readwrite("loss", &LogFocalPriorRecord::loss)
       .def("validate", &LogFocalPriorRecord::Validate);
+  py::class_<LogRelativeFocalPriorRecord>(m, "LogRelativeFocalPriorRecord")
+      .def(py::init<>())
+      .def_readwrite("camera_id1", &LogRelativeFocalPriorRecord::camera_id1)
+      .def_readwrite("camera_id2", &LogRelativeFocalPriorRecord::camera_id2)
+      .def_readwrite("target_log_ratio",
+                     &LogRelativeFocalPriorRecord::target_log_ratio)
+      .def_readwrite("sigma_log_ratio",
+                     &LogRelativeFocalPriorRecord::sigma_log_ratio)
+      .def_readwrite("loss", &LogRelativeFocalPriorRecord::loss)
+      .def("validate", &LogRelativeFocalPriorRecord::Validate);
 }
 }  // namespace vidmap

extensions/colmap/src/bindings/view_graph.cc

@@ -38,6 +38,7 @@ void BindViewGraph(py::module_& m) {
         py::arg("reconstruction"),
         py::arg("pose_graph"),
         py::arg("sidecars"),
-        py::arg("focal_priors") = std::vector<LogFocalPriorRecord>{});
+        py::arg("focal_priors") = std::vector<LogFocalPriorRecord>{},
+        py::arg("relative_focal_priors") = std::vector<LogRelativeFocalPriorRecord>{});
 }
 }  // namespace vidmap

extensions/colmap/src/stages/intrinsics_prior.h

@@ -50,4 +50,72 @@ class LogMeanFocalPriorCostFunction final : public ceres::CostFunction {
   double inverse_sigma_log_focal_;
 };
 
+class LogRelativeFocalPriorCostFunction final : public ceres::CostFunction {
+ public:
+  LogRelativeFocalPriorCostFunction(const int num_camera_params1,
+                                    std::vector<std::size_t> focal_indices1,
+                                    const int num_camera_params2,
+                                    std::vector<std::size_t> focal_indices2,
+                                    const double target_log_ratio = 0.0,
+                                    const double sigma_log_ratio = 1.0)
+      : focal_indices1_(std::move(focal_indices1)),
+        focal_indices2_(std::move(focal_indices2)),
+        target_log_ratio_(target_log_ratio),
+        inverse_sigma_log_ratio_(1.0 / sigma_log_ratio) {
+    set_num_residuals(1);
+    mutable_parameter_block_sizes()->push_back(num_camera_params1);
+    mutable_parameter_block_sizes()->push_back(num_camera_params2);
+  }
+
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override {
+    const double* cam1 = parameters[0];
+    const double* cam2 = parameters[1];
+    double mean_focal1 = 0.0;
+    for (const std::size_t index : focal_indices1_) {
+      mean_focal1 += cam1[index];
+    }
+    mean_focal1 /= static_cast<double>(focal_indices1_.size());
+    if (!std::isfinite(mean_focal1) || mean_focal1 <= 0.0) return false;
+
+    double mean_focal2 = 0.0;
+    for (const std::size_t index : focal_indices2_) {
+      mean_focal2 += cam2[index];
+    }
+    mean_focal2 /= static_cast<double>(focal_indices2_.size());
+    if (!std::isfinite(mean_focal2) || mean_focal2 <= 0.0) return false;
+
+    residuals[0] =
+        ((std::log(mean_focal2) - std::log(mean_focal1)) - target_log_ratio_) *
+        inverse_sigma_log_ratio_;
+
+    if (jacobians != nullptr) {
+      if (jacobians[0] != nullptr) {
+        std::fill(jacobians[0], jacobians[0] + parameter_block_sizes()[0], 0.0);
+        const double derivative1 =
+            -inverse_sigma_log_ratio_ / (mean_focal1 * focal_indices1_.size());
+        for (const std::size_t index : focal_indices1_) {
+          jacobians[0][index] = derivative1;
+        }
+      }
+      if (jacobians[1] != nullptr) {
+        std::fill(jacobians[1], jacobians[1] + parameter_block_sizes()[1], 0.0);
+        const double derivative2 =
+            inverse_sigma_log_ratio_ / (mean_focal2 * focal_indices2_.size());
+        for (const std::size_t index : focal_indices2_) {
+          jacobians[1][index] = derivative2;
+        }
+      }
+    }
+    return true;
+  }
+
+ private:
+  std::vector<std::size_t> focal_indices1_;
+  std::vector<std::size_t> focal_indices2_;
+  double target_log_ratio_;
+  double inverse_sigma_log_ratio_;
+};
+
 }  // namespace vidmap

extensions/colmap/src/stages/view_graph_calibration.cc

@@ -37,7 +37,8 @@ std::size_t CalibrateFocalLengths(
     colmap::Reconstruction& reconstruction,
     colmap::PoseGraph& graph,
     const MappingSidecars& sidecars,
-    const std::vector<LogFocalPriorRecord>& focal_priors) {
+    const std::vector<LogFocalPriorRecord>& focal_priors,
+    const std::vector<LogRelativeFocalPriorRecord>& relative_focal_priors) {
   ValidateCalibrationOptions(options);
   sidecars.Validate(reconstruction);
   std::unordered_set<CameraId> prior_camera_ids;
@@ -47,6 +48,11 @@ std::size_t CalibrateFocalLengths(
       throw std::invalid_argument("VGC focal priors require unique cameras");
     }
   }
+  for (const auto& prior : relative_focal_priors) {
+    prior.Validate();
+    reconstruction.Camera(prior.camera_id1);
+    reconstruction.Camera(prior.camera_id2);
+  }
   struct FocalLengthCalibInput {
     PairId pair_id;
     CameraId camera_id1;
@@ -118,6 +124,20 @@ std::size_t CalibrateFocalLengths(
     }
   }
 
+  // Relative log-focal priors between (consecutive) cameras of time-varying intrinsics.
+  for (const auto& prior : relative_focal_priors) {
+    problem.AddResidualBlock(
+        new LogRelativeFocalPriorCostFunction(1,
+                                              {0},
+                                              1,
+                                              {0},
+                                              prior.target_log_ratio,
+                                              prior.sigma_log_ratio),
+        prior.loss.get(),
+        &focal_lengths.at(prior.camera_id1),
+        &focal_lengths.at(prior.camera_id2));
+  }
+
   std::size_t num_cameras = 0;
   for (auto& [camera_id, camera] : cameras) {
     double* focal = &focal_lengths.at(camera_id);

extensions/colmap/src/vidmap_native/focal_prior.h

@@ -22,4 +22,22 @@ struct LogFocalPriorRecord {
   }
 };
 
+// Pairwise relative focal constraint between two cameras:
+// residuals[0] = ((log(f2) - log(f1)) - target_log_ratio) * (1 / sigma_log_ratio).
+struct LogRelativeFocalPriorRecord {
+  CameraId camera_id1 = 0;
+  CameraId camera_id2 = 0;
+  double target_log_ratio = 0.0;
+  double sigma_log_ratio = 1.0;
+  std::shared_ptr<ceres::LossFunction> loss;
+
+  void Validate() const {
+    if (camera_id1 == camera_id2 || sigma_log_ratio <= 0.0 ||
+        !std::isfinite(sigma_log_ratio) || !std::isfinite(target_log_ratio)) {
+      throw std::invalid_argument("invalid log-relative-focal prior");
+    }
+  }
+};
+
 }  // namespace vidmap
+

extensions/colmap/src/vidmap_native/view_graph.h

@@ -34,5 +34,6 @@ std::size_t CalibrateFocalLengths(const colmap::ViewGraphCalibrationOptions&,
                                   colmap::Reconstruction&,
                                   colmap::PoseGraph&,
                                   const MappingSidecars&,
-                                  const std::vector<LogFocalPriorRecord>&);
+                                  const std::vector<LogFocalPriorRecord>&,
+                                  const std::vector<LogRelativeFocalPriorRecord>&);
 }  // namespace vidmap

extensions/colmap/tests/cpp/native_api_invariants_test.cc

@@ -81,10 +81,65 @@ void TestFrozenLogFocalJacobian() {
   }
 }
 
+void TestRelativeLogFocalJacobian() {
+  const double target_ratio = std::log(1.2);
+  const double sigma_log = 0.05;
+  for (const int dim1 : {1, 3, 4}) {
+    for (const int dim2 : {1, 3, 4}) {
+      const int focal_count1 = dim1 == 4 ? 2 : 1;
+      const int focal_count2 = dim2 == 4 ? 2 : 1;
+      std::vector<std::size_t> idxs1;
+      for (int i = 0; i < focal_count1; ++i) idxs1.push_back(i);
+      std::vector<std::size_t> idxs2;
+      for (int i = 0; i < focal_count2; ++i) idxs2.push_back(i);
+
+      vidmap::LogRelativeFocalPriorCostFunction cost(
+          dim1, idxs1, dim2, idxs2, target_ratio, sigma_log);
+
+      std::vector<double> p1(dim1, 500.0);
+      std::vector<double> p2(dim2, 600.0);
+      std::vector<double> j1(dim1), j2(dim2);
+      const double* blocks[] = {p1.data(), p2.data()};
+      double* jacobians[] = {j1.data(), j2.data()};
+      double residual;
+      Check(cost.Evaluate(blocks, &residual, jacobians),
+            "relative focal cost evaluation failed");
+      const double expected_res =
+          ((std::log(600.0) - std::log(500.0)) - target_ratio) / sigma_log;
+      Check(std::abs(residual - expected_res) < 1e-12,
+            "relative focal residual mismatch");
+
+      // Check jacobians with finite differences
+      const double step = 1e-4;
+      for (int i = 0; i < dim1; ++i) {
+        double plus, minus;
+        p1[i] += step;
+        cost.Evaluate(blocks, &plus, nullptr);
+        p1[i] -= 2 * step;
+        cost.Evaluate(blocks, &minus, nullptr);
+        p1[i] += step;
+        Check(std::abs(j1[i] - (plus - minus) / (2 * step)) < 1e-8,
+              "relative focal p1 finite-difference mismatch");
+      }
+      for (int i = 0; i < dim2; ++i) {
+        double plus, minus;
+        p2[i] += step;
+        cost.Evaluate(blocks, &plus, nullptr);
+        p2[i] -= 2 * step;
+        cost.Evaluate(blocks, &minus, nullptr);
+        p2[i] += step;
+        Check(std::abs(j2[i] - (plus - minus) / (2 * step)) < 1e-8,
+              "relative focal p2 finite-difference mismatch");
+      }
+    }
+  }
+}
+
 }  // namespace
 
 int main() {
   TestOptionValidation();
   TestFrozenLogFocalJacobian();
+  TestRelativeLogFocalJacobian();
   return 0;
 }