From bc180e4b8ccf2d102eef5ae9c94c4be21eb0d27d Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin <15985472+sarlinpe@users.noreply.github.com> Date: Fri, 9 Oct 2026 14:51:20 +0000 Subject: [PATCH] Support time-varying camera intrinsics (zoom) - --time-varying-intrinsics (vidmap.run and the frontend CLI): one camera per frame in the database and mapper state (per_image camera policy) with per-view camera priors. - Frontend initialization: per-frame predicted focals, median-filtered and Gaussian-smoothed in log space (vidmap/utils/camera_smoothing.py) to reject single-frame prediction spikes. - Mapper: raw per-frame focal priors, plus native Ceres log relative focal priors between consecutive keyframe cameras in view graph calibration and bundle adjustment (LogRelativeFocalPriorCostFunction, analytic Jacobians). The Cauchy loss acts as a quadratic smoothness term within continuous footage and lets the focal jump at cuts or abrupt zooms without explicit segmentation. - uncalib/da3 mapping defaults: focal prior weight 1e-3 in VGC and multiplier 0.01 in BA, so near-zero-baseline pairs cannot collapse the focal lengths. - Tests: smoothing, option validation, per-frame cameras, priors, multi-camera BA, and the native relative focal priors (C++ and Python). --- extensions/colmap/CMakeLists.txt | 1 + .../src/bindings/bundle_adjustment_problem.cc | 16 + extensions/colmap/src/bindings/records.cc | 10 + extensions/colmap/src/bindings/view_graph.cc | 3 +- .../colmap/src/stages/intrinsics_prior.h | 68 ++++ .../src/stages/view_graph_calibration.cc | 22 +- .../colmap/src/vidmap_native/focal_prior.h | 18 ++ .../colmap/src/vidmap_native/view_graph.h | 3 +- .../tests/cpp/native_api_invariants_test.cc | 55 ++++ tests/test_time_varying_intrinsics.py | 306 ++++++++++++++++++ vidmap/configs/mapping/uncalib/da3.yaml | 6 +- vidmap/datasets/local.py | 9 +- vidmap/frontend/cli.py | 4 +- vidmap/frontend/colmap_database.py | 4 +- vidmap/frontend/identity.py | 4 + vidmap/frontend/options/preparation.py | 5 + vidmap/frontend/pipeline.py | 8 +- vidmap/frontend/preparation/camera_priors.py | 26 +- .../preparation/geometric_verification.py | 6 + vidmap/frontend/runner.py | 1 + vidmap/mapper/focal_prior.py | 27 ++ vidmap/mapper/inputs/snapshot.py | 6 +- vidmap/mapper/options/view_graph.py | 2 + .../stages/bundle_adjustment/adjuster.py | 24 +- .../stages/bundle_adjustment/problem.py | 16 + .../mapper/stages/view_graph_calibration.py | 19 ++ vidmap/reconstruction.py | 1 + vidmap/run.py | 5 + vidmap/run_options.py | 8 + vidmap/utils/camera_smoothing.py | 52 +++ 30 files changed, 719 insertions(+), 16 deletions(-) create mode 100644 tests/test_time_varying_intrinsics.py create mode 100644 vidmap/utils/camera_smoothing.py diff --git a/extensions/colmap/CMakeLists.txt b/extensions/colmap/CMakeLists.txt index 6ddc1c3..9e899e2 100644 --- a/extensions/colmap/CMakeLists.txt +++ b/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( diff --git a/extensions/colmap/src/bindings/bundle_adjustment_problem.cc b/extensions/colmap/src/bindings/bundle_adjustment_problem.cc index cf048ba..1d44599 100644 --- a/extensions/colmap/src/bindings/bundle_adjustment_problem.cc +++ b/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( + new vidmap::LogRelativeFocalPriorCostFunction( + camera1.params.size(), + std::vector(indices1.begin(), indices1.end()), + camera2.params.size(), + std::vector(indices2.begin(), indices2.end()), + target_log_ratio, + sigma_log_ratio)); + }); } diff --git a/extensions/colmap/src/bindings/records.cc b/extensions/colmap/src/bindings/records.cc index 8fc7bdd..be626d4 100644 --- a/extensions/colmap/src/bindings/records.cc +++ b/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_(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 diff --git a/extensions/colmap/src/bindings/view_graph.cc b/extensions/colmap/src/bindings/view_graph.cc index e6d397c..5b3aea6 100644 --- a/extensions/colmap/src/bindings/view_graph.cc +++ b/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{}); + py::arg("focal_priors") = std::vector{}, + py::arg("relative_focal_priors") = std::vector{}); } } // namespace vidmap diff --git a/extensions/colmap/src/stages/intrinsics_prior.h b/extensions/colmap/src/stages/intrinsics_prior.h index bcbaa5b..5fb1301 100644 --- a/extensions/colmap/src/stages/intrinsics_prior.h +++ b/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 focal_indices1, + const int num_camera_params2, + std::vector 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(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(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 focal_indices1_; + std::vector focal_indices2_; + double target_log_ratio_; + double inverse_sigma_log_ratio_; +}; + } // namespace vidmap diff --git a/extensions/colmap/src/stages/view_graph_calibration.cc b/extensions/colmap/src/stages/view_graph_calibration.cc index 18acfe9..6122f2b 100644 --- a/extensions/colmap/src/stages/view_graph_calibration.cc +++ b/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& focal_priors) { + const std::vector& focal_priors, + const std::vector& relative_focal_priors) { ValidateCalibrationOptions(options); sidecars.Validate(reconstruction); std::unordered_set 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); diff --git a/extensions/colmap/src/vidmap_native/focal_prior.h b/extensions/colmap/src/vidmap_native/focal_prior.h index 0858ff3..5dd698e 100644 --- a/extensions/colmap/src/vidmap_native/focal_prior.h +++ b/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 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 + diff --git a/extensions/colmap/src/vidmap_native/view_graph.h b/extensions/colmap/src/vidmap_native/view_graph.h index 866716b..2772bcb 100644 --- a/extensions/colmap/src/vidmap_native/view_graph.h +++ b/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&); + const std::vector&, + const std::vector&); } // namespace vidmap diff --git a/extensions/colmap/tests/cpp/native_api_invariants_test.cc b/extensions/colmap/tests/cpp/native_api_invariants_test.cc index c3288ae..bef2428 100644 --- a/extensions/colmap/tests/cpp/native_api_invariants_test.cc +++ b/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 idxs1; + for (int i = 0; i < focal_count1; ++i) idxs1.push_back(i); + std::vector 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 p1(dim1, 500.0); + std::vector p2(dim2, 600.0); + std::vector 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; } diff --git a/tests/test_time_varying_intrinsics.py b/tests/test_time_varying_intrinsics.py new file mode 100644 index 0000000..1d97983 --- /dev/null +++ b/tests/test_time_varying_intrinsics.py @@ -0,0 +1,306 @@ +"""Tests for time-varying camera intrinsics modeling and temporal focal smoothing.""" + +from __future__ import annotations + +import tempfile +from pathlib import Path +from types import SimpleNamespace + +import h5py +import numpy as np +import pycolmap +import pytest +import vidmap_native._core as native +from PIL import Image + +from vidmap.datasets.local import LocalImageParser +from vidmap.frontend.options.preparation import CameraPriorEstimationOptions +from vidmap.frontend.preparation.camera_priors import apply_camera_priors +from vidmap.mapper.focal_prior import load_focal_prior +from vidmap.utils.camera_smoothing import smooth_temporal_focals + + +def test_smooth_temporal_focals_outlier_rejection(): + # Constant focal sequence with an extreme isolated spike. + raw = [600.0, 600.0, 600.0, 1800.0, 600.0, 600.0, 600.0] + smoothed = smooth_temporal_focals(raw, window_size=5, gaussian_sigma=1.0) + assert len(smoothed) == len(raw) + # The 1800 spike should be completely eliminated. + assert np.all(smoothed < 650.0) + assert np.all(smoothed > 550.0) + assert np.isclose(smoothed[3], 600.0, atol=10.0) + + +def test_smooth_temporal_focals_zooming_trajectory(): + # Linear zoom from 500 to 1000 over 15 frames with 2 random outliers. + t = np.linspace(0, 1, 15) + gt_zoom = 500.0 + 500.0 * t + noisy_zoom = gt_zoom.copy() + noisy_zoom[4] = 1600.0 # outlier spike + noisy_zoom[10] = 300.0 # outlier dip + + smoothed = smooth_temporal_focals(noisy_zoom, window_size=5, gaussian_sigma=1.0) + # Outliers should be suppressed and smoothed should stay close to ground truth. + max_err = np.max(np.abs(smoothed - gt_zoom)) + raw_max_err = np.max(np.abs(noisy_zoom - gt_zoom)) + assert max_err < 0.25 * raw_max_err + assert smoothed[4] < 1000.0 + assert smoothed[10] > 600.0 + + +def test_smooth_temporal_focals_edge_cases(): + # Empty or short sequences. + assert len(smooth_temporal_focals([])) == 0 + single = np.array([500.0]) + np.testing.assert_array_equal(smooth_temporal_focals(single), single) + pair = np.array([500.0, 600.0]) + np.testing.assert_array_equal(smooth_temporal_focals(pair), pair) + + # Invalid values. + with pytest.raises(ValueError, match="positive and finite"): + smooth_temporal_focals([500.0, -100.0, 500.0]) + with pytest.raises(ValueError, match="positive and finite"): + smooth_temporal_focals([500.0, np.nan, 500.0]) + with pytest.raises(ValueError, match="1D sequence"): + smooth_temporal_focals(np.ones((3, 3))) + + +def test_camera_prior_options_validation(): + # Valid time-varying config. + opts = CameraPriorEstimationOptions( + estimator="da3", inference="per_view", time_varying=True + ) + assert opts.time_varying is True + + # time_varying requires per_view inference. + with pytest.raises( + ValueError, match="Time-varying camera priors require per_view inference" + ): + CameraPriorEstimationOptions( + estimator="geocalib", inference="selected_batch", time_varying=True + ) + + # time_varying requires an estimator. + with pytest.raises( + ValueError, match="Time-varying camera priors require an estimator" + ): + CameraPriorEstimationOptions( + estimator="none", + initialization="supplied", + inference="per_view", + time_varying=True, + ) + + +def test_local_image_parser_time_varying_cameras(): + with tempfile.TemporaryDirectory() as tmpdir: + image_dir = Path(tmpdir) + imnames = ["frame_001.jpg", "frame_002.jpg", "frame_003.jpg"] + for name in imnames: + img = Image.new("RGB", (640, 480), color=(100, 100, 100)) + img.save(image_dir / name) + + # Standard uncalibrated parser (shared camera). + parser_shared = LocalImageParser( + image_dir=image_dir, + imnames=imnames, + estimate_intrinsics=True, + time_varying_intrinsics=False, + ) + assert len(parser_shared.rec.cameras) == 1 + for img in parser_shared.rec.images.values(): + assert img.camera_id == 1 + + # Time-varying uncalibrated parser (per-frame cameras). + parser_varying = LocalImageParser( + image_dir=image_dir, + imnames=imnames, + estimate_intrinsics=True, + time_varying_intrinsics=True, + ) + assert len(parser_varying.rec.cameras) == 3 + camera_ids = {img.camera_id for img in parser_varying.rec.images.values()} + assert camera_ids == {1, 2, 3} + + +def test_apply_camera_priors_time_varying(): + rec = pycolmap.Reconstruction() + imnames = ["001.jpg", "002.jpg", "003.jpg"] + for i, name in enumerate(imnames, start=1): + cam = pycolmap.Camera.create_from_model_name(i, "PINHOLE", 1000.0, 640, 480) + rec.add_camera_with_trivial_rig(cam) + rec.add_image_with_trivial_frame( + pycolmap.Image(name=name, camera_id=i, image_id=i) + ) + + # Raw results: 500, 1500 (outlier), 520 + results = [ + { + "K": np.array([[500.0, 0, 320.0], [0, 500.0, 240.0], [0, 0, 1.0]]), + "image_size": (640, 480), + }, + { + "K": np.array([[1500.0, 0, 320.0], [0, 1500.0, 240.0], [0, 0, 1.0]]), + "image_size": (640, 480), + }, + { + "K": np.array([[520.0, 0, 320.0], [0, 520.0, 240.0], [0, 0, 1.0]]), + "image_size": (640, 480), + }, + ] + + apply_camera_priors( + results=results, + shared=False, + reconstruction=rec, + time_varying=True, + names=imnames, + ) + + focal_cam1 = rec.cameras[1].focal_length_x + focal_cam2 = rec.cameras[2].focal_length_x + focal_cam3 = rec.cameras[3].focal_length_x + + # Frame 2 outlier (1500) should be smoothed down. + assert focal_cam2 < 600.0 + assert np.isclose(focal_cam1, 500.0, atol=25.0) + assert np.isclose(focal_cam3, 520.0, atol=25.0) + + +def test_load_focal_prior_time_varying(tmp_path): + h5_path = tmp_path / "depth_maps.h5" + imnames = ["001.jpg", "002.jpg", "003.jpg"] + with h5py.File(h5_path, "w") as hfile: + for name, f in zip(imnames, [500.0, 1500.0, 520.0], strict=True): + grp = hfile.create_group(name) + grp.create_dataset("image_size", data=np.array([640, 480])) + grp.create_dataset( + "K", + data=np.array( + [[f, 0, 320.0], [0, f, 240.0], [0, 0, 1.0]], dtype=np.float32 + ), + ) + grp.create_dataset("focal_std_px", data=np.array([10.0], dtype=np.float32)) + + rec = pycolmap.Reconstruction() + for i, name in enumerate(imnames, start=1): + cam = pycolmap.Camera.create_from_model_name(i, "PINHOLE", 1000.0, 640, 480) + rec.add_camera_with_trivial_rig(cam) + rec.add_image_with_trivial_frame( + pycolmap.Image(name=name, camera_id=i, image_id=i) + ) + + state = SimpleNamespace(reconstruction=rec, image_order=[1, 2, 3]) + # One raw (unsmoothed) observation per frame camera. + prior = load_focal_prior(h5_path, state) + assert set(prior.keys()) == {1, 2, 3} + assert prior[2][0][0] == 1500.0 + + +def test_bundle_adjustment_relative_focal_prior_cost(): + # The BA relative log-focal cost pulls an outlier focal toward its temporal neighbors. + import pyceres + from vidmap_native import bundle_adjustment as ba_costs + + cameras = [pycolmap.Camera.create_from_model_name(i, "PINHOLE", f, 640, 480) for i, f in [(1, 500.0), (2, 600.0), (3, 520.0)]] + params = [np.array(camera.params) for camera in cameras] + problem = pyceres.Problem() + for camera, block, sigma in zip(cameras, params, (0.01, 0.5, 0.01)): + focal = float(camera.mean_focal_length()) + problem.add_residual_block(ba_costs.focal_prior_cost(camera, focal, sigma), None, [block]) + for i, j in ((0, 1), (1, 2)): + problem.add_residual_block( + ba_costs.relative_focal_prior_cost(cameras[i], cameras[j], 0.0, 0.05), None, [params[i], params[j]] + ) + for block in params: + problem.set_manifold(block, pyceres.SubsetManifold(4, [2, 3])) + summary = pyceres.SolverSummary() + pyceres.solve(pyceres.SolverOptions(), problem, summary) + assert summary.IsSolutionUsable() + assert params[1][0] < 540.0 and abs(params[0][0] - 500.0) < 5.0 and abs(params[2][0] - 520.0) < 5.0 + assert params[1][0] == pytest.approx(params[1][1]) # fx and fy move together + + +def test_build_colmap_database_per_image_camera_policy(tmp_path): + from vidmap.frontend.colmap_database import build_colmap_database + + features_path = tmp_path / "features.h5" + with h5py.File(features_path, "w") as hfile: + for name in ("001.jpg", "002.jpg"): + grp = hfile.create_group(name) + grp.create_dataset("keypoints", data=np.zeros((5, 2), dtype=np.float32)) + + rec = pycolmap.Reconstruction() + cam1 = pycolmap.Camera.create_from_model_name(1, "PINHOLE", 500.0, 640, 480) + cam2 = pycolmap.Camera.create_from_model_name(2, "PINHOLE", 800.0, 640, 480) + rec.add_camera_with_trivial_rig(cam1) + rec.add_camera_with_trivial_rig(cam2) + rec.add_image_with_trivial_frame( + pycolmap.Image(name="001.jpg", camera_id=1, image_id=1) + ) + rec.add_image_with_trivial_frame( + pycolmap.Image(name="002.jpg", camera_id=2, image_id=2) + ) + + db_path = tmp_path / "test.db" + build_colmap_database( + db_path, + rec, + ["001.jpg", "002.jpg"], + features_path, + [("001.jpg", "002.jpg")], + camera_policy="per_image", + prior_focal_length=False, + matches={("001.jpg", "002.jpg"): np.zeros((0, 2), dtype=np.uint32)}, + ) + + db = pycolmap.Database.open(db_path) + assert db.num_cameras() == 2 + assert db.read_camera(0).params[0] == 500.0 + assert db.read_camera(1).params[0] == 800.0 + assert db.read_image(1).camera_id == 0 + assert db.read_image(2).camera_id == 1 + db.close() + + +def test_native_relative_focal_priors_vgc(): + from vidmap.mapper.focal_prior import native_focal_priors, native_relative_focal_priors + + reconstruction = pycolmap.Reconstruction() + sidecars = native.MappingSidecars() + graph = pycolmap.PoseGraph() + for cid, f in [(1, 500.0), (2, 600.0), (3, 520.0)]: + reconstruction.add_camera_with_trivial_rig(pycolmap.Camera.create_from_model_name(cid, "PINHOLE", f, 640, 480)) + image = pycolmap.Image(image_id=cid, camera_id=cid, name=f"00{cid}.jpg", keypoints=np.zeros((2, 2))) + reconstruction.add_image_with_trivial_frame(image, pycolmap.Rigid3d()) + data = native.ImageData() + data.depth_values = np.ones(2) + data.depth_stddevs = np.full(2, 0.1) + data.depth_validity = np.ones(2, dtype=np.uint8) + sidecars.add_image(cid, data) + + angle = 0.2 + rotation = np.array([[np.cos(angle), 0, np.sin(angle)], [0, 1, 0], [-np.sin(angle), 0, np.cos(angle)]]) + translation_skew = np.array([[0, -0.4, 0.3], [0.4, 0, -0.2], [-0.3, 0.2, 0]]) + inv_K = np.linalg.inv(np.array([[500.0, 0, 320.0], [0, 500.0, 240.0], [0, 0, 1]])) + for id1, id2 in [(1, 2), (2, 3)]: + pair = native.PairData() + pair.all_matches = np.zeros((0, 2), dtype=np.uint32) + pair.inlier_indices = np.zeros(0, dtype=np.int32) + pair.are_loop_closure = np.zeros(0, dtype=np.uint8) + pair.geometry.config = 3 # UNCALIBRATED + pair.geometry.F = inv_K.T @ translation_skew @ rotation @ inv_K + sidecars.add_pair(pycolmap.image_pair_to_pair_id(id1, id2), pair) + graph.add_edge(id1, id2, pycolmap.PoseGraphEdge()) + + options = pycolmap.ViewGraphCalibrationOptions() + options.min_focal_length_ratio = 0.01 + options.max_focal_length_ratio = 100.0 + unary = native_focal_priors({1: ((500.0, 0.1),), 2: ((600.0, 0.5),), 3: ((520.0, 0.1),)}, camera_ids=[1, 2, 3], loss="cauchy") + relative = native_relative_focal_priors([(1, 2), (2, 3)], loss="cauchy", scale=0.05, weight=10.0) + native.calibrate_focal_lengths(options, reconstruction, graph, sidecars, unary, relative) + focals = {cid: reconstruction.camera(cid).mean_focal_length() for cid in (1, 2, 3)} + # Camera 2 is pulled toward cameras 1 and 3 by the relative constraints. + assert focals[2] < 540.0 + assert focals[1] > 490.0 + assert focals[3] < 530.0 diff --git a/vidmap/configs/mapping/uncalib/da3.yaml b/vidmap/configs/mapping/uncalib/da3.yaml index 0648bdf..2645cb8 100644 --- a/vidmap/configs/mapping/uncalib/da3.yaml +++ b/vidmap/configs/mapping/uncalib/da3.yaml @@ -5,15 +5,15 @@ mapper: ba: focal_prior: enabled: true - # 20x VGC's base weight; BA stays fixed, while VGC scales by + # 10x VGC's base weight; BA stays fixed, while VGC scales by # eligible pairs / original predictions. - weight_multiplier: 2.0e-4 + weight_multiplier: 0.01 robust_scale: 1.0 # Robust-loss scale in whitened residual units. normal_loss: huber annealing_loss: cauchy vgc: calibration: - focal_prior_weight: 1.0e-5 + focal_prior_weight: 1.0e-3 normalize_weight_by_pair_count: true filter: enabled: false diff --git a/vidmap/datasets/local.py b/vidmap/datasets/local.py index 5c00fdb..906c174 100644 --- a/vidmap/datasets/local.py +++ b/vidmap/datasets/local.py @@ -132,6 +132,7 @@ def __init__( imnames: Sequence[str] | None = None, intrinsics_path: str | Path | None = None, estimate_intrinsics: bool = False, + time_varying_intrinsics: bool = False, ) -> None: self.rgb_dir = Path(image_dir).expanduser() if not self.rgb_dir.is_dir(): @@ -141,7 +142,13 @@ def __init__( if estimate_intrinsics: if intrinsics_path is not None: raise ValueError("intrinsics_path cannot be combined with estimate_intrinsics=True") - intrinsics: Mapping = {1: {"params": [1000.0, 1000.0, 1000.0, 1000.0], "images": "all"}} + if time_varying_intrinsics: + intrinsics: Mapping = { + i: {"params": [1000.0, 1000.0, 1000.0, 1000.0], "images": [name]} + for i, name in enumerate(self.imnames, start=1) + } + else: + intrinsics: Mapping = {1: {"params": [1000.0, 1000.0, 1000.0, 1000.0], "images": "all"}} else: if intrinsics_path is None: raise ValueError("intrinsics_path is required when estimate_intrinsics=False") diff --git a/vidmap/frontend/cli.py b/vidmap/frontend/cli.py index 2d6e614..5521f73 100644 --- a/vidmap/frontend/cli.py +++ b/vidmap/frontend/cli.py @@ -51,11 +51,13 @@ def main(argv=None): parser = build_parser() args, overrides = parse_config_args(parser, argv) - from vidmap.run_options import RunOptions + from vidmap.run_options import TIME_VARYING_INTRINSICS_OVERRIDES, RunOptions from vidmap.utils.logging import configure_logging run_options = RunOptions.from_namespace(args) configure_logging(run_options.verbosity) + if args.time_varying_intrinsics: + overrides = [*overrides, *TIME_VARYING_INTRINSICS_OVERRIDES] conf = config_from_args(args, overrides) if args.mapper_inputs and not args.cache_depth_maps: parser.error("--mapper-inputs requires --cache-depth-maps") diff --git a/vidmap/frontend/colmap_database.py b/vidmap/frontend/colmap_database.py index afe36b5..babb0e3 100644 --- a/vidmap/frontend/colmap_database.py +++ b/vidmap/frontend/colmap_database.py @@ -225,6 +225,7 @@ def create_database_from_frontend( database_path: Path, *, estimate_intrinsics: bool, + time_varying_intrinsics: bool = False, matches: dict[ImagePair, object] | None = None, ) -> dict[str, int]: """Build a COLMAP database from a validated frontend result.""" @@ -249,13 +250,14 @@ def create_database_from_frontend( if matches is None else tuple(sorted(matches, key=lambda pair: (str(pair[0]), str(pair[1])))) ) + camera_policy = "per_image" if (time_varying_intrinsics or not estimate_intrinsics) else "shared" return build_colmap_database( database_path, initial_reconstruction, frontend_result.keyframe_sequence, paths.sparse_features_path, import_pairs, - camera_policy="shared" if estimate_intrinsics else "per_image", + camera_policy=camera_policy, prior_focal_length=not estimate_intrinsics, sparse_matches_path=paths.sparse_matches_path if matches is None else None, matches=matches, diff --git a/vidmap/frontend/identity.py b/vidmap/frontend/identity.py index 72c578c..857b18a 100644 --- a/vidmap/frontend/identity.py +++ b/vidmap/frontend/identity.py @@ -41,6 +41,7 @@ def frontend_config_identity(conf) -> dict[str, object]: "estimator": conf.pipeline.camera_priors.estimator, "inference": conf.pipeline.camera_priors.inference, "initialization": conf.pipeline.camera_priors.initialization, + "time_varying": conf.pipeline.camera_priors.time_varying, }, } @@ -61,6 +62,7 @@ class FrontendIdentity: estimator: str inference: str initialization: str + time_varying: bool = False @classmethod def from_config( @@ -89,6 +91,7 @@ def from_config( estimator=conf.pipeline.camera_priors.estimator, inference=conf.pipeline.camera_priors.inference, initialization=conf.pipeline.camera_priors.initialization, + time_varying=conf.pipeline.camera_priors.time_varying, ) def as_dict(self) -> dict[str, object]: @@ -107,5 +110,6 @@ def as_dict(self) -> dict[str, object]: "estimator": self.estimator, "inference": self.inference, "initialization": self.initialization, + "time_varying": self.time_varying, }, } diff --git a/vidmap/frontend/options/preparation.py b/vidmap/frontend/options/preparation.py index 3a47010..581338f 100644 --- a/vidmap/frontend/options/preparation.py +++ b/vidmap/frontend/options/preparation.py @@ -15,6 +15,7 @@ class CameraPriorEstimationOptions: inference: Literal["per_view", "selected_batch"] = "selected_batch" initialization: Literal["predicted", "supplied"] = "predicted" max_images: int = 30 + time_varying: bool = False @model_validator(mode="after") def validate_applicable_options(self): @@ -22,6 +23,10 @@ def validate_applicable_options(self): raise ValueError("Selected-batch inference requires GeoCalib") if self.estimator == "none" and self.initialization != "supplied": raise ValueError("Predicted initialization requires an estimator") + if self.time_varying and self.inference != "per_view": + raise ValueError("Time-varying camera priors require per_view inference") + if self.time_varying and self.estimator == "none": + raise ValueError("Time-varying camera priors require an estimator") return self diff --git a/vidmap/frontend/pipeline.py b/vidmap/frontend/pipeline.py index 8befe1d..898c340 100644 --- a/vidmap/frontend/pipeline.py +++ b/vidmap/frontend/pipeline.py @@ -176,6 +176,7 @@ def _execute_verified_boundary(self, *, pre_geom_db_stop: bool): self.scene_parser, reference_image_names=list(self.reference_image_names) ) if self.options.camera_priors.initialization == "predicted": + time_varying = self.options.camera_priors.time_varying shared = self.options.camera_priors.inference == "selected_batch" if self.options.camera_priors.estimator == "da3": path = tracking.paths.depth_maps_path @@ -184,7 +185,11 @@ def _execute_verified_boundary(self, *, pre_geom_db_stop: bool): names = ("batch_calibration",) if shared else tracking.keyframe_sequence with h5py.File(path, "r") as hfile: apply_camera_priors( - results=[hfile[name] for name in names], shared=shared, reconstruction=reconstruction + results=[hfile[name] for name in names], + shared=shared, + reconstruction=reconstruction, + time_varying=time_varying, + names=names if not shared else None, ) correspondence_filter = CorrespondenceFilter( @@ -211,6 +216,7 @@ def _execute_verified_boundary(self, *, pre_geom_db_stop: bool): replay=self.replay, repro_dir=self.repro_dir, estimate_intrinsics=self.estimate_intrinsics, + time_varying_intrinsics=self.options.camera_priors.time_varying, pre_geom_db_stop=pre_geom_db_stop, ) verification = geometric_verifier.verify(tracking, reconstruction, filtered.tcorr) diff --git a/vidmap/frontend/preparation/camera_priors.py b/vidmap/frontend/preparation/camera_priors.py index 7e2a0dd..6f076a0 100644 --- a/vidmap/frontend/preparation/camera_priors.py +++ b/vidmap/frontend/preparation/camera_priors.py @@ -54,8 +54,8 @@ def _apply_shared_focal(camera: pycolmap.Camera, focal: float, principal_point=N camera.params = params -def apply_camera_priors(*, results, shared, reconstruction): - """Use the shared estimate or median predicted focal for the input cameras.""" +def apply_camera_priors(*, results, shared, reconstruction, time_varying: bool = False, names=None): + """Use the shared estimate, time-varying filtered sequence, or median predicted focal for the input cameras.""" if not results: raise ValueError("Predicted initialization requires calibration results") cameras = {image.camera_id: reconstruction.cameras[image.camera_id] for image in reconstruction.images.values()} @@ -72,6 +72,28 @@ def apply_camera_priors(*, results, shared, reconstruction): K = np.asarray(results[0]["K"], dtype=np.float32) for camera in cameras.values(): _apply_calibration(camera, K[[0, 1], [0, 1]], K[:2, 2]) + elif time_varying: + if names is None or len(names) != len(results): + raise ValueError("Time-varying camera priors require image names matching results") + from vidmap.utils.camera_smoothing import smooth_temporal_focals + + intrinsics = np.asarray([result["K"] for result in results], dtype=np.float32) + raw_focals = (intrinsics[:, 0, 0] + intrinsics[:, 1, 1]).astype(np.float64) / 2.0 + smoothed_focals = smooth_temporal_focals(raw_focals) + + name_to_camera = {img.name: reconstruction.cameras[img.camera_id] for img in reconstruction.images.values()} + assigned_camera_ids = set() + for name, focal in zip(names, smoothed_focals, strict=True): + cam = name_to_camera.get(name) + if cam is not None: + pp = (cam.width / 2.0, cam.height / 2.0) + _apply_shared_focal(cam, float(focal), pp) + assigned_camera_ids.add(cam.camera_id) + + median_focal = float(np.median(smoothed_focals)) + for camera in cameras.values(): + if camera.camera_id not in assigned_camera_ids: + _apply_shared_focal(camera, median_focal, (camera.width / 2.0, camera.height / 2.0)) else: intrinsics = np.asarray([result["K"] for result in results], dtype=np.float32) median_focal = float(np.median((intrinsics[:, 0, 0] + intrinsics[:, 1, 1]).astype(np.float64) / 2.0)) diff --git a/vidmap/frontend/preparation/geometric_verification.py b/vidmap/frontend/preparation/geometric_verification.py index 76da3ad..c8249f3 100644 --- a/vidmap/frontend/preparation/geometric_verification.py +++ b/vidmap/frontend/preparation/geometric_verification.py @@ -115,6 +115,7 @@ def __init__( replay: ReplayCache, repro_dir: Path | None, estimate_intrinsics: bool, + time_varying_intrinsics: bool = False, pre_geom_db_stop: bool = False, ): self.options = options @@ -122,6 +123,7 @@ def __init__( self.replay = replay self.repro_dir = repro_dir self.estimate_intrinsics = estimate_intrinsics + self.time_varying_intrinsics = time_varying_intrinsics self.pre_geom_db_stop = pre_geom_db_stop def verify( @@ -147,6 +149,7 @@ def verify( initial_reconstruction, tcorr, estimate_intrinsics=self.estimate_intrinsics, + time_varying_intrinsics=self.time_varying_intrinsics, database_path=temporary_path, pre_geom_db_stop=self.pre_geom_db_stop, ) @@ -182,6 +185,7 @@ def _verify_database( tcorr: Mapping[ImagePair, Any], *, estimate_intrinsics: bool, + time_varying_intrinsics: bool = False, database_path: Path, pre_geom_db_stop: bool, ) -> frozenset[ImagePair] | None: @@ -191,6 +195,7 @@ def _verify_database( initial_reconstruction, database_path, estimate_intrinsics=estimate_intrinsics, + time_varying_intrinsics=time_varying_intrinsics, matches=tcorr, ) @@ -201,6 +206,7 @@ def _verify_database( initial_reconstruction, database_path, estimate_intrinsics=estimate_intrinsics, + time_varying_intrinsics=time_varying_intrinsics, matches=tcorr, ) diff --git a/vidmap/frontend/runner.py b/vidmap/frontend/runner.py index 3f08cf8..cf82184 100644 --- a/vidmap/frontend/runner.py +++ b/vidmap/frontend/runner.py @@ -71,6 +71,7 @@ def run_local_frontend( imnames=imnames, intrinsics_path=intrinsics_path, estimate_intrinsics=conf.pipeline.camera_priors.initialization == "predicted", + time_varying_intrinsics=conf.pipeline.camera_priors.time_varying, ) reference_image_ids = tuple(scene_parser.rec.images) identity = FrontendIdentity.from_config( diff --git a/vidmap/mapper/focal_prior.py b/vidmap/mapper/focal_prior.py index 5953ff9..02e2bcb 100644 --- a/vidmap/mapper/focal_prior.py +++ b/vidmap/mapper/focal_prior.py @@ -19,6 +19,33 @@ def native_focal_priors(prior, *, camera_ids, loss, scale=1.0, weight=1.0): return records +def native_relative_focal_priors( + consecutive_camera_pairs: list[tuple[int, int]], + *, + loss: str = "cauchy", + scale: float = 0.05, + weight: float = 1.0, +): + import pycolmap + + from vidmap.mapper.native.extension import native + + records = [] + seen = set() + for cam_id1, cam_id2 in consecutive_camera_pairs: + if cam_id1 == cam_id2 or (cam_id1, cam_id2) in seen: + continue + seen.add((cam_id1, cam_id2)) + record = native.LogRelativeFocalPriorRecord() + record.camera_id1 = cam_id1 + record.camera_id2 = cam_id2 + record.target_log_ratio = 0.0 + record.sigma_log_ratio = 1.0 + record.loss = pycolmap.create_ceres_loss_function(pycolmap.LossFunctionType(loss.upper()), scale, weight) + records.append(record) + return records + + def load_focal_prior(path, state, *, log_focal_stddev=None, shared=False): images = state.reconstruction.images cameras = state.reconstruction.cameras diff --git a/vidmap/mapper/inputs/snapshot.py b/vidmap/mapper/inputs/snapshot.py index ee4cce3..8293516 100644 --- a/vidmap/mapper/inputs/snapshot.py +++ b/vidmap/mapper/inputs/snapshot.py @@ -46,7 +46,8 @@ "boundary_options", } ) -BOUNDARY_OPTION_KEYS = frozenset({"estimator", "inference", "initialization"}) +BOUNDARY_OPTION_KEYS = frozenset({"estimator", "inference", "initialization", "time_varying"}) +LEGACY_BOUNDARY_OPTION_KEYS = frozenset({"estimator", "inference", "initialization"}) def calibration_artifact_name(estimator: str, inference: str) -> str | None: @@ -232,10 +233,11 @@ def _validate_frontend_identity(identity: object, *, manifest_path: Path) -> Non options = identity["boundary_options"] if ( not isinstance(options, dict) - or set(options) != BOUNDARY_OPTION_KEYS + or (set(options) != BOUNDARY_OPTION_KEYS and set(options) != LEGACY_BOUNDARY_OPTION_KEYS) or options["estimator"] not in {"da3", "geocalib", "none"} or options["inference"] not in {"per_view", "selected_batch"} or options["initialization"] not in {"predicted", "supplied"} + or not isinstance(options.get("time_varying", False), bool) or (options["inference"] == "selected_batch" and options["estimator"] != "geocalib") or (options["estimator"] == "none" and options["initialization"] != "supplied") ): diff --git a/vidmap/mapper/options/view_graph.py b/vidmap/mapper/options/view_graph.py index 0a7e6db..fa0aa07 100644 --- a/vidmap/mapper/options/view_graph.py +++ b/vidmap/mapper/options/view_graph.py @@ -28,6 +28,8 @@ class VGCCalibrationOptions: # Outer prior coefficient before multiplication by eligible pairs / images. focal_prior_weight: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 1.0e-5 normalize_weight_by_pair_count: bool = True + relative_focal_weight: Annotated[float, Field(ge=0, allow_inf_nan=False)] = 1.0e-1 + relative_focal_loss_scale: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 0.05 @pydantic_dataclass(frozen=True, config=ConfigDict(extra="forbid", strict=True)) diff --git a/vidmap/mapper/stages/bundle_adjustment/adjuster.py b/vidmap/mapper/stages/bundle_adjustment/adjuster.py index e926f98..31bf7ee 100644 --- a/vidmap/mapper/stages/bundle_adjustment/adjuster.py +++ b/vidmap/mapper/stages/bundle_adjustment/adjuster.py @@ -459,17 +459,36 @@ def solve_problem( ) intrinsics_priors = [] + relative_intrinsics_priors = [] prior_options = self.options.focal_prior if self.optimize_intrinsics and not policy.fix_intrinsics and prior_options.enabled: if not self.focal_prior: raise ValueError("No frozen original focal observations; cannot enable a missing prior") + loss_name = prior_options.annealing_loss if policy.refinement else prior_options.normal_loss + prior_weight = prior_options.weight_multiplier * self.point_budget_scale intrinsics_priors = native_focal_priors( self.focal_prior, camera_ids=camera_ids, - loss=prior_options.annealing_loss if policy.refinement else prior_options.normal_loss, + loss=loss_name, scale=prior_options.robust_scale, - weight=prior_options.weight_multiplier * self.point_budget_scale, + weight=prior_weight, ) + if len(camera_ids) > 1 and len(optimized_image_ids) > 1: + from vidmap.mapper.focal_prior import native_relative_focal_priors + + consec_cam_pairs = [] + for i in range(len(optimized_image_ids) - 1): + c1 = self.reconstruction.images[optimized_image_ids[i]].camera_id + c2 = self.reconstruction.images[optimized_image_ids[i + 1]].camera_id + if c1 != c2: + consec_cam_pairs.append((c1, c2)) + if consec_cam_pairs: + relative_intrinsics_priors = native_relative_focal_priors( + consec_cam_pairs, + loss=loss_name, + scale=prior_options.robust_scale, + weight=prior_weight, + ) depth_batches = [] depth_scales = [] @@ -557,6 +576,7 @@ def solve_problem( depth_scales, intrinsics_priors, self.solve_state, + relative_intrinsics_priors=relative_intrinsics_priors, playback_callback=playback_sink, playback_options=playback_options, ) diff --git a/vidmap/mapper/stages/bundle_adjustment/problem.py b/vidmap/mapper/stages/bundle_adjustment/problem.py index 623a10e..69f31f4 100644 --- a/vidmap/mapper/stages/bundle_adjustment/problem.py +++ b/vidmap/mapper/stages/bundle_adjustment/problem.py @@ -48,6 +48,7 @@ class BundleAdjustmentDiagnostics(SolverDiagnostics): num_reprojection_residuals: int = 0 num_depth_residuals: int = 0 num_intrinsics_prior_residuals: int = 0 + num_relative_intrinsics_prior_residuals: int = 0 num_scale_prior_residuals: int = 0 @@ -66,6 +67,7 @@ def run_bundle_adjustment( intrinsics_priors, state, *, + relative_intrinsics_priors=(), playback_callback=None, playback_options=None, ): @@ -101,6 +103,20 @@ def run_bundle_adjustment( problem.add_residual_block(ba_costs.focal_prior_cost(camera, focal, stddev), prior.loss, [params]) diagnostics.num_intrinsics_prior_residuals += 1 + # Relative log-focal priors between consecutive cameras (time-varying intrinsics). + for prior in relative_intrinsics_priors: + prior.validate() + camera1, camera2 = reconstruction.camera(prior.camera_id1), reconstruction.camera(prior.camera_id2) + params1, params2 = camera1.params, camera2.params + if not (problem.has_parameter_block(params1) and problem.has_parameter_block(params2)): + continue + problem.add_residual_block( + ba_costs.relative_focal_prior_cost(camera1, camera2, prior.target_log_ratio, prior.sigma_log_ratio), + prior.loss, + [params1, params2], + ) + diagnostics.num_relative_intrinsics_prior_residuals += 1 + scale_records = {record.image_id: record for record in depth_scales} scales = {image_id: np.array([record.log_scale], dtype=float) for image_id, record in scale_records.items()} depth_losses = [] diff --git a/vidmap/mapper/stages/view_graph_calibration.py b/vidmap/mapper/stages/view_graph_calibration.py index 9f87ed9..0ffbaab 100644 --- a/vidmap/mapper/stages/view_graph_calibration.py +++ b/vidmap/mapper/stages/view_graph_calibration.py @@ -91,12 +91,31 @@ def calibrate(self) -> None: loss="cauchy", weight=weight, ) + relative_focal_priors = [] + rel_weight = self.options.relative_focal_weight + if len(cameras) > 1 and self.consecutive_pair_ids and rel_weight > 0: + from vidmap.mapper.focal_prior import native_relative_focal_priors + + consec_cam_pairs = [] + for pid in self.consecutive_pair_ids: + image_id1, image_id2 = pycolmap.pair_id_to_image_pair(pid) + cid1 = state.reconstruction.images[image_id1].camera_id + cid2 = state.reconstruction.images[image_id2].camera_id + if cid1 != cid2: + consec_cam_pairs.append((cid1, cid2)) + relative_focal_priors = native_relative_focal_priors( + consec_cam_pairs, + loss="cauchy", + scale=self.options.relative_focal_loss_scale, + weight=rel_weight, + ) invalid_count = native.calibrate_focal_lengths( vgc_options, state.reconstruction, state.pose_graph, state.sidecars, focal_priors, + relative_focal_priors, ) logger.info( "VGC: invalidated %d / %d pairs (residual^2 > %.4f)", diff --git a/vidmap/reconstruction.py b/vidmap/reconstruction.py index da29bb8..3afb809 100644 --- a/vidmap/reconstruction.py +++ b/vidmap/reconstruction.py @@ -61,6 +61,7 @@ def reconstruct( imnames=imnames, intrinsics_path=intrinsics_path, estimate_intrinsics=estimate_intrinsics, + time_varying_intrinsics=frontend_conf.pipeline.camera_priors.time_varying, ) output_dir = workspace if output_dir is None else Path(output_dir).expanduser() run_options = RunOptions() if run_options is None else run_options diff --git a/vidmap/run.py b/vidmap/run.py index 43d5235..ad2118a 100644 --- a/vidmap/run.py +++ b/vidmap/run.py @@ -54,6 +54,11 @@ def main(argv=None): except ValueError as error: parser.error(str(error).replace("Pipeline config", "End-to-end config")) + if args.time_varying_intrinsics: + from vidmap.run_options import TIME_VARYING_INTRINSICS_OVERRIDES + + frontend_overrides = [*frontend_overrides, *TIME_VARYING_INTRINSICS_OVERRIDES] + from vidmap.configuration.build import build_frontend_config, build_mapping_config from vidmap.configuration.names import FRONTEND_CONFIG_DIR, MAPPING_CONFIG_DIR, resolve_config_path diff --git a/vidmap/run_options.py b/vidmap/run_options.py index 3df5408..37f67df 100644 --- a/vidmap/run_options.py +++ b/vidmap/run_options.py @@ -5,6 +5,9 @@ from argparse import Namespace from dataclasses import dataclass +# Frontend config overrides implied by --time-varying-intrinsics (one camera per frame needs per-view priors). +TIME_VARYING_INTRINSICS_OVERRIDES = ("camera_priors.time_varying=true", "camera_priors.inference=per_view") + @dataclass(frozen=True) class RunOptions: @@ -86,6 +89,11 @@ def add_run_arguments(parser, *, mapping: bool, profiling: bool = False) -> None choices=["auto", "cpu", "gpu", "cuda", "mps"], help="Compute device for neural networks (default: auto; choices: auto, cpu, gpu, cuda, mps).", ) + parser.add_argument( + "--time-varying-intrinsics", + action="store_true", + help="Model and estimate time-varying camera intrinsics across frames.", + ) parser.set_defaults( save_playback_trace=False, playback_trace_stride=3, diff --git a/vidmap/utils/camera_smoothing.py b/vidmap/utils/camera_smoothing.py new file mode 100644 index 0000000..2d5c2b5 --- /dev/null +++ b/vidmap/utils/camera_smoothing.py @@ -0,0 +1,52 @@ +"""Temporal filtering and smoothing for camera intrinsic sequences.""" + +from __future__ import annotations + +from collections.abc import Sequence + +import numpy as np + + +def smooth_temporal_focals( + focals: Sequence[float] | np.ndarray, + window_size: int = 5, + gaussian_sigma: float = 1.0, +) -> np.ndarray: + """Apply outlier rejection (median filter) and temporal Gaussian smoothing to focal lengths. + + Filters in log-space so relative zoom ratios are treated symmetrically. + """ + arr = np.asarray(focals, dtype=np.float64) + if arr.ndim != 1: + raise ValueError(f"Focal lengths must be a 1D sequence, got shape {arr.shape}") + if len(arr) <= 2: + return arr.copy() + if np.any(arr <= 0) or not np.all(np.isfinite(arr)): + raise ValueError("Focal lengths must be positive and finite") + + log_f = np.log(arr) + n = len(log_f) + + # 1. 1D median filter to remove neural prediction spikes. + w = min(window_size, n if n % 2 == 1 else n - 1) + w = max(1, w) + if w > 1: + pad = w // 2 + padded = np.pad(log_f, pad, mode="edge") + med = np.array([np.median(padded[i : i + w]) for i in range(n)], dtype=np.float64) + else: + med = log_f.copy() + + # 2. 1D Gaussian smoothing to ensure C^inf focal curve. + if gaussian_sigma > 0 and n > 2: + radius = int(np.ceil(3 * gaussian_sigma)) + radius = min(radius, n) + x = np.arange(-radius, radius + 1, dtype=np.float64) + kernel = np.exp(-0.5 * (x / gaussian_sigma) ** 2) + kernel /= kernel.sum() + padded_med = np.pad(med, radius, mode="edge") + smooth_log = np.convolve(padded_med, kernel, mode="valid") + else: + smooth_log = med + + return np.exp(smooth_log)