diff --git a/extensions/colmap/CMakeLists.txt b/extensions/colmap/CMakeLists.txt index 9e899e2..164f18f 100644 --- a/extensions/colmap/CMakeLists.txt +++ b/extensions/colmap/CMakeLists.txt @@ -141,3 +141,9 @@ set_target_properties(vidmap_ba_costs PROPERTIES OUTPUT_NAME bundle_adjustment) target_include_directories(vidmap_ba_costs PRIVATE src ${VIDMAP_COLMAP_INCLUDE_DIRS}) target_link_libraries(vidmap_ba_costs PRIVATE ${VIDMAP_COLMAP_ESTIMATORS_TARGET}) install(TARGETS vidmap_ba_costs LIBRARY DESTINATION vidmap_native) + +pybind11_add_module(vidmap_ra_costs NO_EXTRAS src/bindings/rotation_averaging_problem.cc) +set_target_properties(vidmap_ra_costs PROPERTIES OUTPUT_NAME rotation_averaging) +target_include_directories(vidmap_ra_costs PRIVATE src ${VIDMAP_COLMAP_INCLUDE_DIRS}) +target_link_libraries(vidmap_ra_costs PRIVATE ${VIDMAP_COLMAP_ESTIMATORS_TARGET}) +install(TARGETS vidmap_ra_costs LIBRARY DESTINATION vidmap_native) diff --git a/extensions/colmap/src/bindings/bundle_adjustment_problem.cc b/extensions/colmap/src/bindings/bundle_adjustment_problem.cc index 1d44599..4271734 100644 --- a/extensions/colmap/src/bindings/bundle_adjustment_problem.cc +++ b/extensions/colmap/src/bindings/bundle_adjustment_problem.cc @@ -1,4 +1,5 @@ #include "colmap/estimators/ceres_loss_function.h" +#include "colmap/estimators/cost_functions/reprojection_error.h" #include "colmap/scene/reconstruction.h" #include @@ -101,4 +102,66 @@ PYBIND11_MODULE(bundle_adjustment, m) { target_log_ratio, sigma_log_ratio)); }); + m.def( + "append_constant_point_reprojections", + [](ceres::Problem& problem, + colmap::Reconstruction& reconstruction, + colmap::image_t image_id, + const Eigen::Matrix& + points2D, + const Eigen::Matrix& + points3D, + colmap::CeresLossFunctionType loss_type, + double loss_scale, + double weight) { + if (points3D.rows() != points2D.rows() || !points2D.allFinite() || + !points3D.allFinite() || !std::isfinite(loss_scale) || + loss_scale <= 0 || !std::isfinite(weight) || weight < 0) { + throw std::invalid_argument( + "invalid BA constant-point reprojection arrays"); + } + // The upstream BA problem owns costs, but not loss functions. + std::shared_ptr loss = + colmap::CreateCeresLossFunction(loss_type, loss_scale, weight); + auto& image = reconstruction.Image(image_id); + auto& camera = reconstruction.Camera(image.CameraId()); + double* pose = image.FramePtr()->RigFromWorld().params.data(); + size_t added = 0; + if (weight > 0 && problem.HasParameterBlock(pose) && + problem.HasParameterBlock(camera.params.data())) { + if (!image.IsRefInFrame()) { + throw std::invalid_argument( + "VidMap constant-point reprojections require a reference " + "camera"); + } + for (Eigen::Index i = 0; i < points2D.rows(); ++i) { + problem.AddResidualBlock( + colmap::CreateCameraCostFunction< + colmap::ReprojErrorConstantPoint3DCostFunctor>( + camera.model_id, + Eigen::Vector2d(points2D.row(i).transpose()), + Eigen::Vector3d(points3D.row(i).transpose())), + loss.get(), + pose, + camera.params.data()); + ++added; + } + } + using Loss = std::shared_ptr; + return py::make_tuple( + py::capsule(new Loss(std::move(loss)), + [](void* ptr) { delete static_cast(ptr); }), + added); + }, + py::arg("problem"), + py::arg("reconstruction"), + py::arg("image_id"), + py::arg("points2D"), + py::arg("points3D"), + py::arg("loss_type"), + py::arg("loss_scale"), + py::arg("weight"), + "Add reprojection residuals of constant world points into an image " + "whose pose and camera are already in the problem. Keep the returned " + "loss alive while using the problem."); } diff --git a/extensions/colmap/src/bindings/global_positioning_problem.cc b/extensions/colmap/src/bindings/global_positioning_problem.cc index 9d044d1..af1d577 100644 --- a/extensions/colmap/src/bindings/global_positioning_problem.cc +++ b/extensions/colmap/src/bindings/global_positioning_problem.cc @@ -555,4 +555,81 @@ PYBIND11_MODULE(global_positioning, m) { vidmap::TemporalAccelerationCostFunctor::Create( dt_prev, dt_next, 1.0 / stddev)); }); + m.def( + "append_bearing_observations", + [](ceres::Problem& problem, + py::array_t center, + const Eigen::Matrix3d& cam_from_world_rotation, + const Eigen::Matrix& + points, + const Eigen::Matrix& + bearings, + const Eigen::Matrix& + stddevs, + const std::shared_ptr& loss) { + const auto count = points.rows(); + if (center.size() != 3 || !center.writeable() || + bearings.rows() != count || stddevs.rows() != count || + !cam_from_world_rotation.allFinite() || !points.allFinite() || + !bearings.allFinite() || !stddevs.allFinite() || + (count > 0 && stddevs.minCoeff() <= 0.0)) { + throw std::invalid_argument("invalid bearing observation arrays"); + } + // Constant world points and per-observation scales, owned by the + // returned capsule. + struct Storage { + std::vector points; + std::vector scales; + std::shared_ptr loss; + }; + auto storage = std::make_unique(); + storage->points.reserve(count); + storage->scales.reserve(count); + storage->loss = loss; + double* center_data = center.mutable_data(); + const Eigen::Map center_xyz(center_data); + const Eigen::Matrix3d world_from_cam = + cam_from_world_rotation.transpose(); + for (Eigen::Index i = 0; i < count; ++i) { + const Eigen::Vector3d bearing = bearings.row(i).normalized(); + if (!bearing.allFinite()) continue; + storage->points.emplace_back(points.row(i).transpose()); + Eigen::Vector3d& point = storage->points.back(); + const double distance = (point - center_xyz).norm(); + storage->scales.push_back( + distance > 1e-6 ? std::clamp(1.0 / distance, 1e-3, 10.0) : 1.0); + double& scale = storage->scales.back(); + // Whiten in the camera frame: Sigma_world = R^T diag(sigma^2) R. + const Eigen::Matrix3d covariance = + world_from_cam * + stddevs.row(i).array().square().matrix().asDiagonal() * + cam_from_world_rotation; + problem.AddResidualBlock( + colmap::CovarianceWeightedCostFunctor< + colmap::BATAPairwiseDirectionCostFunctor>:: + Create(covariance, world_from_cam * bearing), + loss.get(), + center_data, + point.data(), + &scale); + problem.SetParameterBlockConstant(point.data()); + problem.SetParameterLowerBound(&scale, 0, 1e-3); + } + const size_t added = storage->scales.size(); + return py::make_tuple( + py::capsule(storage.release(), + [](void* ptr) { delete static_cast(ptr); }), + added); + }, + py::arg("problem"), + py::arg("center").noconvert(), + py::arg("cam_from_world_rotation"), + py::arg("points"), + py::arg("bearings"), + py::arg("stddevs"), + py::arg("loss"), + "Constrain a camera center by unit bearings (in the camera frame) to " + "constant world points, with per-axis stddevs in the camera frame. " + "Keep the returned storage and the center alive while using the " + "problem."); } diff --git a/extensions/colmap/src/bindings/rotation_averaging_problem.cc b/extensions/colmap/src/bindings/rotation_averaging_problem.cc new file mode 100644 index 0000000..fc5fbfb --- /dev/null +++ b/extensions/colmap/src/bindings/rotation_averaging_problem.cc @@ -0,0 +1,238 @@ +#include "colmap/estimators/cost_functions/utils.h" +#include "colmap/math/math.h" +#include "colmap/scene/reconstruction.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace { + +// 3-DoF rotation error in the camera frame, r = Log(R_cw * R_prior_cw^T), on an +// Eigen quaternion (x, y, z, w) parameter block. This matches the tangent space +// of COLMAP's absolute pose priors. +struct AbsoluteRotationPriorCostFunctor + : public colmap:: + AutoDiffCostFunctor { + explicit AbsoluteRotationPriorCostFunctor( + const Eigen::Quaterniond& cam_from_world_prior) + : world_from_cam_prior_(cam_from_world_prior.normalized().inverse()) {} + + template + bool operator()(const T* const cam_from_world, T* residuals) const { + const Eigen::Map> rotation(cam_from_world); + const Eigen::Quaternion error = + rotation * world_from_cam_prior_.cast(); + const T error_wxyz[4] = {error.w(), error.x(), error.y(), error.z()}; + ceres::QuaternionToAngleAxis(error_wxyz, residuals); + return true; + } + + const Eigen::Quaterniond world_from_cam_prior_; +}; + +struct RotationPrior { + double* block; + Eigen::Quaterniond cam_from_world; + Eigen::Matrix3d covariance; + double confidence; +}; + +double* FrameRotationBlock(colmap::Reconstruction& reconstruction, + colmap::image_t image_id) { + auto& image = reconstruction.Image(image_id); + if (!image.IsRefInFrame()) { + throw std::invalid_argument( + "Rotation priors require reference cameras in their frames"); + } + return image.FramePtr()->RigFromWorld().rotation().coeffs().data(); +} + +// Rotate the world frame of all solved rotations so that they agree with the +// priors: R_cw <- R_cw * R_align. This preserves all relative rotations. +void AlignToPriors(ceres::Problem& problem, + const std::vector& priors, + double max_error_deg) { + const auto rotation = [](const double* block) { + return Eigen::Quaterniond(Eigen::Map(block)) + .normalized(); + }; + const double truncation = colmap::DegToRad(max_error_deg); + double best_cost = std::numeric_limits::infinity(); + Eigen::Quaterniond best_align = Eigen::Quaterniond::Identity(); + for (const RotationPrior& candidate : priors) { + // R_prior = R_cw * R_align => R_align = R_cw^-1 * R_prior. + const Eigen::Quaterniond align = + rotation(candidate.block).inverse() * candidate.cam_from_world; + double cost = 0.0; + for (const RotationPrior& prior : priors) { + const double angle = + (rotation(prior.block) * align).angularDistance(prior.cam_from_world); + cost += prior.confidence * std::min(angle, truncation); + } + if (cost < best_cost) { + best_cost = cost; + best_align = align.normalized(); + } + } + + // Refine with a confidence-weighted tangent mean over the inliers. + Eigen::Vector3d delta_sum = Eigen::Vector3d::Zero(); + double weight_sum = 0.0; + for (const RotationPrior& prior : priors) { + const Eigen::Quaterniond align = + rotation(prior.block).inverse() * prior.cam_from_world; + if (best_align.angularDistance(align) > truncation) continue; + const Eigen::AngleAxisd angle_axis(best_align.inverse() * align); + Eigen::Vector3d delta = angle_axis.angle() * angle_axis.axis(); + if (angle_axis.angle() > EIGEN_PI) { + delta = (angle_axis.angle() - 2.0 * EIGEN_PI) * angle_axis.axis(); + } + delta_sum += prior.confidence * delta; + weight_sum += prior.confidence; + } + if (weight_sum > 0.0) { + const Eigen::Vector3d mean_delta = delta_sum / weight_sum; + const double mean_angle = mean_delta.norm(); + if (mean_angle > 1e-12) { + best_align = (best_align * Eigen::Quaterniond(Eigen::AngleAxisd( + mean_angle, mean_delta / mean_angle))) + .normalized(); + } + } + + std::vector blocks; + problem.GetParameterBlocks(&blocks); + for (double* block : blocks) { + if (problem.ParameterBlockSize(block) != 4 || + problem.IsParameterBlockConstant(block)) { + continue; + } + Eigen::Map aligned(block); + aligned = (rotation(block) * best_align).normalized(); + } +} + +} // namespace + +PYBIND11_MODULE(rotation_averaging, m) { + py::module_::import("pyceres"); + m.def( + "add_rotation_priors", + [](ceres::Problem& problem, + colmap::Reconstruction& reconstruction, + const std::vector& image_ids, + const std::vector& cam_from_world_xyzw, + const std::vector& covariances, + const std::vector& confidences, + double weight, + double cauchy_scale, + double ref_sigma_deg, + double min_sigma_deg, + bool align, + double align_max_error_deg) { + const size_t count = image_ids.size(); + if (cam_from_world_xyzw.size() != count || + covariances.size() != count || confidences.size() != count) { + throw std::invalid_argument("invalid rotation prior arrays"); + } + if (!(weight >= 0.0) || !(cauchy_scale > 0.0) || + !(ref_sigma_deg > 0.0) || !(min_sigma_deg > 0.0) || + !(align_max_error_deg > 0.0)) { + throw std::invalid_argument("invalid rotation prior options"); + } + std::vector priors; + for (size_t i = 0; i < count; ++i) { + const Eigen::Vector4d& xyzw = cam_from_world_xyzw[i]; + if (!xyzw.allFinite() || xyzw.squaredNorm() <= 1e-12 || + !covariances[i].allFinite() || !std::isfinite(confidences[i]) || + confidences[i] < 0.0) { + throw std::invalid_argument("invalid rotation prior"); + } + if (confidences[i] == 0.0 || + !reconstruction.ExistsImage(image_ids[i]) || + !reconstruction.Image(image_ids[i]).HasFramePtr() || + !reconstruction.Image(image_ids[i]).FramePtr()->HasPose()) { + continue; + } + double* block = FrameRotationBlock(reconstruction, image_ids[i]); + if (!problem.HasParameterBlock(block)) continue; + priors.push_back( + {block, + Eigen::Quaterniond(xyzw[3], xyzw[0], xyzw[1], xyzw[2]) + .normalized(), + covariances[i], + confidences[i]}); + } + + using Losses = std::vector>; + auto losses = std::make_unique(); + if (weight > 0.0 && !priors.empty()) { + // The priors fix the gauge, so free the frame held constant by the + // averager. + for (const auto& [frame_id, const_frame] : reconstruction.Frames()) { + if (!const_frame.HasPose()) continue; + double* block = reconstruction.Frame(frame_id) + .RigFromWorld() + .rotation() + .coeffs() + .data(); + if (problem.HasParameterBlock(block) && + problem.IsParameterBlockConstant(block)) { + problem.SetParameterBlockVariable(block); + } + } + if (align) AlignToPriors(problem, priors, align_max_error_deg); + + const double min_sigma = colmap::DegToRad(min_sigma_deg); + const double ref_sigma = colmap::DegToRad(ref_sigma_deg); + losses->reserve(priors.size()); + for (const RotationPrior& prior : priors) { + Eigen::Matrix3d covariance = + 0.5 * (prior.covariance + prior.covariance.transpose()); + covariance.diagonal().array() += min_sigma * min_sigma; + losses->push_back(std::make_unique( + new ceres::CauchyLoss(cauchy_scale), + weight * prior.confidence * ref_sigma * ref_sigma, + ceres::TAKE_OWNERSHIP)); + problem.AddResidualBlock( + colmap::CovarianceWeightedCostFunctor< + AbsoluteRotationPriorCostFunctor>:: + Create(covariance, prior.cam_from_world), + losses->back().get(), + prior.block); + } + } + const size_t added = losses->size(); + return py::make_tuple( + py::capsule(losses.release(), + [](void* ptr) { delete static_cast(ptr); }), + added); + }, + py::arg("problem"), + py::arg("reconstruction"), + py::arg("image_ids"), + py::arg("cam_from_world_xyzw"), + py::arg("covariances"), + py::arg("confidences"), + py::arg("weight"), + py::arg("cauchy_scale"), + py::arg("ref_sigma_deg"), + py::arg("min_sigma_deg"), + py::arg("align"), + py::arg("align_max_error_deg") = 10.0, + "Add absolute rotation priors on the frame rotations of a rotation " + "averaging problem. With align, first rotate the world frame of the " + "current rotations onto the priors. Keep the returned losses alive " + "while using the problem."); +} diff --git a/tests/test_location_priors.py b/tests/test_location_priors.py new file mode 100644 index 0000000..7179568 --- /dev/null +++ b/tests/test_location_priors.py @@ -0,0 +1,220 @@ +"""Tests for loading absolute location priors from the generic .npz interface.""" + +from types import SimpleNamespace + +import numpy as np +import pyceres +import pycolmap +from scipy.spatial.transform import Rotation + +from vidmap.mapper.location_priors import LocationAnchorPrior, LocationPriorSet, load_location_priors +from vidmap.mapper.native.extension import native +from vidmap.mapper.native.state import SolveState +from vidmap.mapper.options.location_priors import LocationPriorOptions +from vidmap.mapper.options.view_graph import RAOptions +from vidmap.mapper.stages.rotation_averaging import RotationAverager + + +def _write_priors(path, image_names, confidence, num_matches): + count = len(image_names) + indptr = np.concatenate([[0], np.cumsum(num_matches)]).astype(np.int32) + num_points = int(indptr[-1]) + np.savez( + path, + image_names=np.array(image_names), + confidence=np.asarray(confidence, dtype=np.float32), + R_cam_from_world=np.tile(np.eye(3), (count, 1, 1)), + cov_cam_from_world=np.tile(np.eye(6) * 1e-4, (count, 1, 1)), + points3D=np.arange(num_points * 3, dtype=np.float64).reshape(num_points, 3), + match_indptr=indptr, + match_point_indices=np.arange(num_points, dtype=np.int32), + match_uv_norm=np.full((num_points, 2), 0.5, dtype=np.float32), + ) + + +def _solve_state(names): + images = {i: SimpleNamespace(name=name) for i, name in enumerate(names, start=1)} + return SimpleNamespace(reconstruction=SimpleNamespace(images=images)) + + +def test_load_location_priors_matching_and_filtering(tmp_path): + path = tmp_path / "priors.npz" + _write_priors( + path, + # exact name, timestamp within 1 ms, low confidence, too few matches, unknown frame + [ + "0000000001.000000000.jpg", + "0000000002.000400000.jpg", + "0000000003.000000000.jpg", + "0000000004.000000000.jpg", + "0000000099.000000000.jpg", + ], + confidence=[0.9, 0.8, 0.01, 0.9, 0.9], + num_matches=[5, 6, 5, 2, 5], + ) + state = _solve_state( + [ + "0000000001.000000000.jpg", + "0000000002.000000000.jpg", + "0000000003.000000000.jpg", + "0000000004.000000000.jpg", + ] + ) + priors = load_location_priors(LocationPriorOptions(enabled=True, path=str(path)), state) + + assert sorted(priors.anchors) == [1, 2] + assert priors.anchors[2].image_name == "0000000002.000400000.jpg" + assert len(priors.anchors[2].prior_point_ids) == 6 + np.testing.assert_array_equal(priors.anchors[2].points3D_xyz, priors.mapped_points_xyz[5:11]) + assert priors.anchors[1].cov_rot_cam_from_world.shape == (3, 3) + + +def test_load_location_priors_disabled(tmp_path): + assert load_location_priors(LocationPriorOptions(enabled=False, path=None), _solve_state([])) is None + + +def _anchor(image_id, R_cam_from_world, points3D=np.zeros((0, 3)), uv_norm=np.zeros((0, 2))): + return LocationAnchorPrior( + image_id=image_id, + image_name=f"{image_id}.jpg", + confidence=1.0, + R_cam_from_world=R_cam_from_world, + cov_rot_cam_from_world=np.eye(3) * np.deg2rad(1.0) ** 2, + prior_point_ids=np.arange(len(points3D), dtype=np.uint32), + points3D_xyz=np.asarray(points3D, dtype=np.float64), + uv_norm=np.asarray(uv_norm, dtype=np.float64), + ) + + +def test_rotation_averaging_aligns_to_location_priors(): + # Ground-truth rotations in the prior world frame; the relative rotations alone leave the world frame free. + rotations = Rotation.from_euler("xyz", [[30, 5, 0], [32, 10, 1], [35, 14, 2], [36, 20, 2]], degrees=True) + reconstruction = pycolmap.Reconstruction() + reconstruction.add_camera_with_trivial_rig(pycolmap.Camera.create_from_model_name(1, "PINHOLE", 500.0, 640, 480)) + graph = pycolmap.PoseGraph() + sidecars = native.MappingSidecars() + for image_id in range(1, 5): + image = pycolmap.Image(image_id=image_id, camera_id=1, name=f"{image_id}.jpg", keypoints=np.zeros((1, 2))) + reconstruction.add_image_with_trivial_frame(image, pycolmap.Rigid3d()) + sidecars.add_image(image_id, native.ImageData()) + for first, second in ((1, 2), (2, 3), (3, 4)): + pair = native.PairData() + pair.all_matches = np.zeros((1, 2), dtype=np.uint32) + pair.inlier_indices = np.zeros(1, dtype=np.int32) + pair.are_loop_closure = np.zeros(1, dtype=np.uint8) + pair.has_relative_pose = True + pair_id = pycolmap.image_pair_to_pair_id(first, second) + sidecars.add_pair(pair_id, pair) + edge = pycolmap.PoseGraphEdge() + edge.valid = True + relative = rotations[second - 1] * rotations[first - 1].inv() + edge.cam2_from_cam1 = pycolmap.Rigid3d(rotation=pycolmap.Rotation3d(relative.as_matrix())) + graph.add_edge(first, second, edge) + + priors = LocationPriorSet( + options=LocationPriorOptions(enabled=True), + anchors={i: _anchor(i, rotations[i - 1].as_matrix()) for i in (2, 4)}, + mapped_points_xyz=np.zeros((0, 3)), + ) + averager = RotationAverager( + solve_state=SolveState(reconstruction, graph, sidecars), + options=RAOptions(), + sequence_id_to_index={i: i - 1 for i in range(1, 5)}, + filtered_consecutive_pair_ids=set(), + replay=None, + location_priors=priors, + ) + assert averager.run_pass() + for image_id in range(1, 5): + estimate = reconstruction.image(image_id).cam_from_world().rotation.matrix() + error = Rotation.from_matrix(estimate @ rotations[image_id - 1].as_matrix().T).magnitude() + assert np.rad2deg(error) < 0.01 + + +def test_bearing_observations_position_a_camera(): + from vidmap_native import global_positioning as gp_costs + + R_cw = Rotation.from_euler("xyz", [10, -20, 5], degrees=True).as_matrix() + true_center = np.array([1.0, -2.0, 0.5]) + points = np.array([[5.0, 1.0, 20.0], [-4.0, 2.0, 15.0], [0.5, -3.0, 25.0], [2.0, 4.0, 18.0]]) @ R_cw + true_center + bearings = (points - true_center) @ R_cw.T + bearings /= np.linalg.norm(bearings, axis=1, keepdims=True) + center = np.zeros(3) + problem = pyceres.Problem() + storage, count = gp_costs.append_bearing_observations( + problem, center, R_cw, points, bearings, np.full((len(points), 3), 1e-3), pyceres.TrivialLoss() + ) + assert count == len(points) + summary = pyceres.SolverSummary() + pyceres.solve(pyceres.SolverOptions(), problem, summary) + assert summary.IsSolutionUsable() + np.testing.assert_allclose(center, true_center, atol=1e-4) + + +def test_constant_point_reprojections_position_a_camera(): + from vidmap_native import bundle_adjustment as ba_costs + + reconstruction = pycolmap.Reconstruction() + camera = pycolmap.Camera.create_from_model_name(1, "PINHOLE", 500.0, 640, 480) + reconstruction.add_camera_with_trivial_rig(camera) + true_pose = pycolmap.Rigid3d(pycolmap.Rotation3d(np.array([0.1, -0.2, 0.05])), np.array([0.3, -0.1, 0.4])) + image = pycolmap.Image(image_id=1, camera_id=1, name="1.jpg") + reconstruction.add_image_with_trivial_frame(image, pycolmap.Rigid3d(true_pose.rotation, np.zeros(3))) + points3D = true_pose.inverse() * np.array([[1.0, 0.5, 8.0], [-1.0, 0.2, 6.0], [0.3, -1.0, 9.0], [0.8, 0.9, 7.0]]) + points2D = camera.img_from_cam(true_pose * points3D) + + pose = reconstruction.frame(reconstruction.image(1).frame_id).rig_from_world.params + params = reconstruction.camera(1).params + problem = pyceres.Problem() + problem.add_parameter_block(pose, 7) + problem.set_manifold(pose, pyceres.SubsetManifold(7, [0, 1, 2, 3])) + problem.add_parameter_block(params, 4) + problem.set_parameter_block_constant(params) + loss, count = ba_costs.append_constant_point_reprojections( + problem, reconstruction, 1, points2D, points3D, pycolmap.LossFunctionType.TRIVIAL, 1.0, 1.0 + ) + assert count == len(points3D) + summary = pyceres.SolverSummary() + pyceres.solve(pyceres.SolverOptions(), problem, summary) + assert summary.IsSolutionUsable() + np.testing.assert_allclose(reconstruction.image(1).cam_from_world().translation, true_pose.translation, atol=1e-6) + + +def test_gp1_alignment_is_per_covisibility_component(monkeypatch): + # Two parts without shared points (e.g. across a cut), each off by a different similarity in GP1. + reconstruction = pycolmap.Reconstruction() + reconstruction.add_camera_with_trivial_rig(pycolmap.Camera.create_from_model_name(1, "PINHOLE", 500.0, 640, 480)) + centers = {1: [0.0, 0, 0], 2: [1.0, 0, 0], 3: [5.0, 0, 0], 4: [6.0, 0, 0], 5: [7.0, 0, 0]} + for image_id, center in centers.items(): + image = pycolmap.Image(image_id=image_id, camera_id=1, name=f"{image_id}.jpg", keypoints=np.zeros((30, 2))) + reconstruction.add_image_with_trivial_frame(image, pycolmap.Rigid3d(pycolmap.Rotation3d(), -np.array(center))) + for component in ((1, 2), (3, 4, 5)): + for k in range(25): + track = pycolmap.Track() + for image_id in component: + track.add_element(image_id, k) + reconstruction.add_point3D(np.array([centers[component[0]][0], 0.0, 10.0 + k]), track) + transforms = { + 1: (1.0, np.array([10.0, 0, 0])), + 2: (1.0, np.array([10.0, 0, 0])), + 3: (2.0, np.array([0, -50.0, 0])), + 4: (2.0, np.array([0, -50.0, 0])), + 5: (2.0, np.array([0, -50.0, 0])), + } + expected = {i: transforms[i][0] * np.array(c) + transforms[i][1] for i, c in centers.items()} + monkeypatch.setattr( + LocationPriorSet, + "_resect_camera_center_from_rays", + staticmethod(lambda rec, anchor: expected[anchor.image_id]), + ) + priors = LocationPriorSet( + options=LocationPriorOptions(enabled=True, use_in_gp1=False, gp_alignment_max_angle_error_deg=180.0), + anchors={i: _anchor(i, np.eye(3)) for i in centers}, + mapped_points_xyz=np.zeros((0, 3)), + ) + scales = priors.align_gp1_to_location_priors_4dof(reconstruction, {i: 1.0 for i in centers}) + for image_id, center in expected.items(): + np.testing.assert_allclose(reconstruction.image(image_id).projection_center(), center, atol=1e-9) + assert scales == {1: 1.0, 2: 1.0, 3: 2.0, 4: 2.0, 5: 2.0} + point = next(p for p in reconstruction.points3D.values() if p.track.elements[0].image_id == 3) + np.testing.assert_allclose(point.xyz[1], -50.0) diff --git a/vidmap/datasets/local.py b/vidmap/datasets/local.py index 906c174..a942e60 100644 --- a/vidmap/datasets/local.py +++ b/vidmap/datasets/local.py @@ -133,6 +133,7 @@ def __init__( intrinsics_path: str | Path | None = None, estimate_intrinsics: bool = False, time_varying_intrinsics: bool = False, + gt_reconstruction_path: str | Path | None = None, ) -> None: self.rgb_dir = Path(image_dir).expanduser() if not self.rgb_dir.is_dir(): @@ -161,6 +162,11 @@ def __init__( if not isinstance(intrinsics, Mapping) or not intrinsics: raise ValueError(f"Intrinsics file {path} must define at least one camera mapping") + gt_poses_by_name: dict[str, pycolmap.Rigid3d] = {} + if gt_reconstruction_path is not None: + gt_rec = pycolmap.Reconstruction(Path(gt_reconstruction_path).expanduser()) + gt_poses_by_name = {str(img.name): img.cam_from_world() for img in gt_rec.images.values() if img.has_pose} + self.rec = pycolmap.Reconstruction() self.reconstruction_dir = None image_camera_ids = _add_cameras(self.rec, intrinsics, tuple(self.imnames), self.rgb_dir) @@ -171,4 +177,7 @@ def __init__( camera_id=image_camera_ids[image_name], image_id=image_id, ) - self.rec.add_image_with_trivial_frame(image) + if image_name in gt_poses_by_name: + self.rec.add_image_with_trivial_frame(image, gt_poses_by_name[image_name]) + else: + self.rec.add_image_with_trivial_frame(image) diff --git a/vidmap/frontend/keyframes/selector.py b/vidmap/frontend/keyframes/selector.py index 4fc8973..2f60361 100644 --- a/vidmap/frontend/keyframes/selector.py +++ b/vidmap/frontend/keyframes/selector.py @@ -148,7 +148,9 @@ def _compute_aliked_features(self, frame_index): def _select_keyframe(self, is_gt_frame): if is_gt_frame: return self.pair_source_idx - if self.last_good_frame_idx is not None and self.last_good_frame_idx > self.seg_start_frame: + if self.last_good_frame_idx is not None and self.last_good_frame_idx > max( + self.seg_start_frame, self.keyframe_ids[-1] + ): return self.last_good_frame_idx candidate = self.pair_source_idx if candidate == self.keyframe_ids[-1]: @@ -196,7 +198,11 @@ def _process_sparse_pair(self, pair_match_lr, pair_cert_lr): self.conf.max_normalized_keypoint_drift, ) - is_gt_frame = self.pair_source_idx in self.gt_frame_indices and self.pair_source_idx != self.seg_start_frame + is_gt_frame = ( + self.pair_source_idx in self.gt_frame_indices + and self.pair_source_idx != self.seg_start_frame + and self.pair_source_idx > self.keyframe_ids[-1] + ) if motion_score > self.conf.target_frac or is_gt_frame: new_keyframe = self._select_keyframe(is_gt_frame) if new_keyframe <= self.keyframe_ids[-1]: diff --git a/vidmap/frontend/runner.py b/vidmap/frontend/runner.py index cf82184..575582f 100644 --- a/vidmap/frontend/runner.py +++ b/vidmap/frontend/runner.py @@ -54,6 +54,7 @@ def run_local_frontend( workspace: str | Path, imnames=None, intrinsics_path: str | Path | None = None, + gt_reconstruction_path: str | Path | None = None, force_frontend: bool = False, cache_depth_maps: bool = False, mapper_inputs_path: str | Path | None = None, @@ -72,6 +73,7 @@ def run_local_frontend( intrinsics_path=intrinsics_path, estimate_intrinsics=conf.pipeline.camera_priors.initialization == "predicted", time_varying_intrinsics=conf.pipeline.camera_priors.time_varying, + gt_reconstruction_path=gt_reconstruction_path, ) reference_image_ids = tuple(scene_parser.rec.images) identity = FrontendIdentity.from_config( diff --git a/vidmap/map.py b/vidmap/map.py index d0a2f26..951573f 100644 --- a/vidmap/map.py +++ b/vidmap/map.py @@ -39,6 +39,11 @@ def build_parser() -> ArgumentParser: ) parser.add_argument("-o", "--overwrite", action="store_true") parser.add_argument("--name", type=str) + parser.add_argument( + "--location-priors", + type=str, + help="Optional .npz of absolute location priors (per-image rotations and 2D-3D correspondences to world points) used in rotation averaging, global positioning, and BA.", + ) from vidmap.run_options import add_run_arguments add_run_arguments(parser, mapping=True) @@ -58,10 +63,21 @@ def _run_local(mapping_conf, frontend_conf, args, run_options) -> int: overwrite_outputs=args.overwrite, output_dir=output_dir, ) - reconstruction_dir = output_dir / "rec" - reconstruction_dir.mkdir(parents=True, exist_ok=True) - reconstruction.write(reconstruction_dir) - logger.info("Reconstruction written to %s", reconstruction_dir) + if run_options.split_cuts: + from vidmap.mapper.sub_reconstruction import decompose_reconstruction, export_sub_reconstructions + + models = decompose_reconstruction( + reconstruction, + min_shared_points=run_options.min_covisibility_points, + min_model_size=run_options.min_model_size, + ) + written_recs = export_sub_reconstructions(models, output_dir) + logger.info("Reconstruction written to %s", [str(p) for p in written_recs]) + else: + reconstruction_dir = output_dir / "rec" + reconstruction_dir.mkdir(parents=True, exist_ok=True) + reconstruction.write(reconstruction_dir) + logger.info("Reconstruction written to %s", reconstruction_dir) return 0 @@ -87,6 +103,15 @@ def main(argv=None): run_options = RunOptions.from_namespace(args) configure_logging(run_options.verbosity) frontend_overrides, mapping_overrides = _split_stage_overrides(overrides) + if args.time_varying_intrinsics: + from vidmap.run_options import TIME_VARYING_INTRINSICS_OVERRIDES + + frontend_overrides = (*frontend_overrides, *TIME_VARYING_INTRINSICS_OVERRIDES) + if args.location_priors: + mapping_overrides = tuple(mapping_overrides) + ( + "mapper.location_priors.enabled=true", + f"mapper.location_priors.path={args.location_priors}", + ) frontend_conf = build_frontend_config( resolve_config_path(args.frontend_conf, FRONTEND_CONFIG_DIR), source_name=args.frontend_conf, diff --git a/vidmap/mapper/location_priors.py b/vidmap/mapper/location_priors.py new file mode 100644 index 0000000..97ab298 --- /dev/null +++ b/vidmap/mapper/location_priors.py @@ -0,0 +1,595 @@ +"""Load absolute location priors (rotations and 2D-3D correspondences) and add them to the RA, GP and BA problems.""" + +from __future__ import annotations + +import logging +import re +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +import pyceres +import pycolmap +from vidmap_native import bundle_adjustment as ba_costs +from vidmap_native import global_positioning as gp_costs +from vidmap_native import rotation_averaging as ra_costs + +from vidmap.mapper.native.state import SolveState +from vidmap.mapper.options.location_priors import LocationPriorOptions +from vidmap.mapper.sub_reconstruction import detect_covisibility_components + +logger = logging.getLogger(__name__) +_TIMESTAMP = re.compile(r"^(\d+(?:\.\d+)?)(?:-|$)") + + +def _angular_stds_to_xyz_covar(bearings: np.ndarray, angular_stds: np.ndarray) -> np.ndarray: + """Propagate diagonal image-plane angular covariance to unit bearings.""" + count = bearings.shape[0] + image_covariance = np.zeros((count, 3, 3), dtype=np.float64) + image_covariance[:, 0, 0] = angular_stds[:, 0] ** 2 + image_covariance[:, 1, 1] = angular_stds[:, 1] ** 2 + + bearing_z = np.abs(bearings[:, 2])[:, None, None] + outer_product = np.einsum("ni,nj->nij", bearings, bearings) + jacobian = bearing_z * (np.eye(3, dtype=np.float64)[None, :, :] - outer_product) + return np.einsum("nij,njk,nlk->nil", jacobian, image_covariance, jacobian) + + +def _parse_timestamp_us(name: str) -> int | None: + match = _TIMESTAMP.match(Path(name).stem) + if match is None: + return None + try: + return int(round(float(match.group(1)) * 1e6)) + except ValueError: + return None + + +@dataclass(frozen=True, kw_only=True) +class LocationAnchorPrior: + """Location prior of one matched VidMap keyframe.""" + + image_id: int + image_name: str + confidence: float + R_cam_from_world: np.ndarray + cov_rot_cam_from_world: np.ndarray + prior_point_ids: np.ndarray + points3D_xyz: np.ndarray + uv_norm: np.ndarray + + +@dataclass(frozen=True, kw_only=True) +class LocationPriorSet: + """Location priors matched to the images of a reconstruction.""" + + options: LocationPriorOptions + anchors: dict[int, LocationAnchorPrior] + mapped_points_xyz: np.ndarray + + def active_image_ids(self, reconstruction: pycolmap.Reconstruction) -> list[int]: + """Return sorted image_ids of prior anchors that currently have a valid pose.""" + return [ + image_id + for image_id in sorted(self.anchors.keys()) + if image_id in reconstruction.images and reconstruction.image(image_id).has_pose + ] + + def num_active_anchors(self, reconstruction: pycolmap.Reconstruction) -> int: + return len(self.active_image_ids(reconstruction)) + + def add_rotation_priors(self, problem, reconstruction: pycolmap.Reconstruction, *, align: bool): + """Add absolute rotation priors to a rotation averaging problem (Stage 1). + + With align, the current (e.g. spanning-tree) rotations are first rotated into the prior world frame. + Returns the storage to keep alive while using the problem and the number of priors. + """ + if not self.options.enabled or not self.options.use_in_rotation_averaging: + return None, 0 + image_ids, quaternions, covariances, confidences = [], [], [], [] + for image_id in sorted(self.anchors.keys()): + if image_id not in reconstruction.images: + continue + anchor = self.anchors[image_id] + rot = pycolmap.Rotation3d(np.asarray(anchor.R_cam_from_world, dtype=np.float64)) + cov = np.asarray(anchor.cov_rot_cam_from_world, dtype=np.float64).copy() + cov = 0.5 * (cov + cov.T) + eigvals = np.linalg.eigvalsh(cov) + if eigvals.min() <= 1e-12: + cov = cov + (1e-10 - min(0.0, float(eigvals.min()))) * np.eye(3, dtype=np.float64) + image_ids.append(int(image_id)) + quaternions.append(np.asarray(rot.quat, dtype=np.float64)) + covariances.append(cov) + confidences.append(float(anchor.confidence)) + storage, count = ra_costs.add_rotation_priors( + problem, + reconstruction, + image_ids, + quaternions, + covariances, + confidences, + weight=float(self.options.ra_weight), + cauchy_scale=float(self.options.ra_cauchy_scale), + ref_sigma_deg=float(self.options.ra_ref_sigma_deg), + min_sigma_deg=float(self.options.ra_min_sigma_deg), + align=align, + ) + logger.info("Using %d location rotation priors in rotation averaging", count) + return storage, count + + @staticmethod + def _resect_camera_center_from_rays( + reconstruction: pycolmap.Reconstruction, + anchor: LocationAnchorPrior, + ) -> np.ndarray | None: + """Compute the camera center in prior world coordinates by intersecting 2D-3D rays with the current R_cw.""" + if len(anchor.prior_point_ids) < 3: + return None + image = reconstruction.image(anchor.image_id) + if not image.has_pose: + return None + camera = reconstruction.camera(image.camera_id) + R_cw = np.asarray(image.cam_from_world().rotation.matrix(), dtype=np.float64) + + xy_px = anchor.uv_norm * np.array([float(camera.width), float(camera.height)], dtype=np.float64) + cam_pts = np.asarray(camera.cam_from_img(xy_px), dtype=np.float64) + if cam_pts.ndim != 2 or cam_pts.shape[0] < 3: + return None + bearings_cam = np.column_stack([cam_pts, np.ones(len(cam_pts), dtype=np.float64)]) + norms = np.linalg.norm(bearings_cam, axis=1, keepdims=True) + valid = np.isfinite(norms[:, 0]) & (norms[:, 0] > 1e-9) + if int(valid.sum()) < 3: + return None + bearings_cam = bearings_cam[valid] / norms[valid] + pts_world = np.asarray(anchor.points3D_xyz[valid], dtype=np.float64) + + # World ray directions: v_world = R_cw^T @ b_cam + v_world = bearings_cam @ R_cw + eye = np.eye(3, dtype=np.float64)[None, :, :] + proj = eye - v_world[:, :, None] @ v_world[:, None, :] + A = np.sum(proj, axis=0) + b = np.sum(proj @ pts_world[:, :, None], axis=0)[:, 0] + eigvals = np.linalg.eigvalsh(A) + if float(eigvals.min()) <= 1e-3: + return None + center = np.linalg.solve(A, b) + if not np.all(np.isfinite(center)): + return None + + # Verify cheirality (majority of 3D points must lie in front of resected center along rays) + depths = np.sum((pts_world - center[None, :]) * v_world, axis=1) + if float(np.median(depths)) <= 0.05: + return None + + # Refine (Cx, Cy, Cz, log_f) with robust Cauchy 2D reprojection error so far landmarks + # (100-300m) or rough initial focal lengths do not bias the resected center along the optical axis. + from scipy.optimize import least_squares + + f_init = float(camera.mean_focal_length()) + cx, cy = float(camera.width) / 2.0, float(camera.height) / 2.0 + duv = xy_px[valid] - np.array([cx, cy], dtype=np.float64)[None, :] + x0 = np.array([center[0], center[1], center[2], np.log(max(f_init, 1.0))], dtype=np.float64) + + def _res_fn(x: np.ndarray) -> np.ndarray: + c = x[:3] + f = np.exp(np.clip(x[3], 3.0, 10.0)) + p_cam = (pts_world - c[None, :]) @ R_cw.T + z = np.maximum(p_cam[:, 2], 0.1) + pred = f * (p_cam[:, :2] / z[:, None]) + return (pred - duv).ravel() / 2.0 + + try: + sol = least_squares(_res_fn, x0, loss="cauchy", f_scale=2.0, max_nfev=50) + if sol.success and np.all(np.isfinite(sol.x[:3])): + c_ref = sol.x[:3] + depths_ref = ((pts_world - c_ref[None, :]) @ R_cw.T)[:, 2] + if float(np.median(depths_ref)) > 0.05: + center = c_ref + except Exception: + pass + return center + + def _estimate_scale_translation( + self, P_gp1: np.ndarray, P_prior: np.ndarray, W: np.ndarray + ) -> tuple[float, np.ndarray, float]: + """Robustly estimate (scale, translation) with P_prior ~ scale * P_gp1 + translation. + + Uses 2-point RANSAC and a weighted least-squares refit. Returns the scale, translation, and inlier threshold. + """ + num_anchors = len(P_gp1) + prior_extent = float(np.linalg.norm(P_prior.max(axis=0) - P_prior.min(axis=0))) if num_anchors >= 2 else 0.0 + threshold = max(float(self.options.gp_ransac_inlier_threshold_m), 0.05 * prior_extent) + + if num_anchors == 1: + scale = 1.0 + translation = P_prior[0] - P_gp1[0] + inlier_mask = np.ones(1, dtype=bool) + elif prior_extent < float(self.options.gp_alignment_min_extent_for_scale_m): + # The anchors are too close together (e.g. a stationary camera) to observe the scale: keep the GP1 + # (metric depth) scale and estimate the translation only, by truncated voting and an inlier mean. + scale = 1.0 + offsets = P_prior - P_gp1 + candidates = [offsets[i] for i in range(num_anchors)] + if self.options.use_in_gp1: + candidates.append(np.zeros(3, dtype=np.float64)) + scores = [ + np.sum(W * np.minimum(np.linalg.norm(offsets - c[None, :], axis=1), threshold)) for c in candidates + ] + translation = candidates[int(np.argmin(scores))] + inliers = np.linalg.norm(offsets - translation[None, :], axis=1) < threshold + if np.any(inliers): + translation = np.sum(W[inliers, None] * offsets[inliers], axis=0) / np.sum(W[inliers]) + else: + # Downweight spatially clustered stationary anchors so a stop at a traffic light + # does not dominate RANSAC over moving segments of the trajectory. + r_cell = max(1.0, 0.05 * prior_extent) + dist_sq_mat = np.sum((P_prior[:, None, :] - P_prior[None, :, :]) ** 2, axis=2) + density = np.sum(np.exp(-0.5 * dist_sq_mat / (r_cell**2)), axis=1) + W_spatial = W / np.maximum(density, 1.0) + + min_baseline_sq = max(0.2, 0.15 * prior_extent) ** 2 + best_score = float("inf") + best_scale = 1.0 + best_translation = np.mean(P_prior - P_gp1, axis=0) + if self.options.use_in_gp1: + id_errs = np.linalg.norm(P_gp1 - P_prior, axis=1) + best_score = float(np.sum(W_spatial * np.minimum(id_errs, threshold))) + best_scale = 1.0 + best_translation = np.zeros(3, dtype=np.float64) + + for min_b_sq in (min_baseline_sq, 0.04): + found_pair = False + for i in range(num_anchors): + for j in range(i + 1, num_anchors): + d_gp1 = P_gp1[j] - P_gp1[i] + d_prior = P_prior[j] - P_prior[i] + norm_gp1_sq = float(np.dot(d_gp1, d_gp1)) + norm_prior_sq = float(np.dot(d_prior, d_prior)) + if norm_gp1_sq < 0.01 or norm_prior_sq < min_b_sq: + continue + cand_scale = float(np.dot(d_prior, d_gp1) / norm_gp1_sq) + if cand_scale <= 0.05 or cand_scale >= 20.0: + continue + found_pair = True + wi, wj = W[i], W[j] + cand_trans = ( + wi * (P_prior[i] - cand_scale * P_gp1[i]) + wj * (P_prior[j] - cand_scale * P_gp1[j]) + ) / (wi + wj) + errs = np.linalg.norm(cand_scale * P_gp1 + cand_trans[None, :] - P_prior, axis=1) + score = float(np.sum(W_spatial * np.minimum(errs, threshold))) + if score < best_score: + best_score = score + best_scale = cand_scale + best_translation = cand_trans + if found_pair: + break + + errs = np.linalg.norm(best_scale * P_gp1 + best_translation[None, :] - P_prior, axis=1) + inlier_mask = errs < threshold + if int(inlier_mask.sum()) >= 2: + w_inl = W_spatial[inlier_mask] + w_norm = (w_inl / np.sum(w_inl))[:, None] + mu_gp1 = np.sum(w_norm * P_gp1[inlier_mask], axis=0) + mu_prior = np.sum(w_norm * P_prior[inlier_mask], axis=0) + gp1_zero = P_gp1[inlier_mask] - mu_gp1[None, :] + prior_zero = P_prior[inlier_mask] - mu_prior[None, :] + denom = float(np.sum(w_norm * (gp1_zero**2))) + numer = float(np.sum(w_norm * (gp1_zero * prior_zero))) + if denom > 1e-6 and 0.05 < (numer / denom) < 20.0: + refit_scale = numer / denom + refit_trans = mu_prior - refit_scale * mu_gp1 + refit_errs = np.linalg.norm(refit_scale * P_gp1 + refit_trans[None, :] - P_prior, axis=1) + refit_score = float(np.sum(W_spatial * np.minimum(refit_errs, threshold))) + if refit_score <= best_score: + best_scale = refit_scale + best_translation = refit_trans + scale = best_scale + translation = best_translation + + return float(scale), np.asarray(translation, dtype=np.float64), threshold + + def align_gp1_to_location_priors_4dof( + self, + reconstruction: pycolmap.Reconstruction, + dmap_scales: dict[int, float], + ) -> dict[int, float]: + """Align the GP1 reconstruction (centers, tracks, depth scales) to the prior frame via 4-DoF (scale, t). + + Since rotations are already in the prior orientation frame from Rotation Averaging, + this estimates only a scale s > 0 (if >= 2 anchors) and a translation t in R^3 + from the resected 2D-3D ray centers using 2-point RANSAC + weighted least-squares. + Parts of the reconstruction that are only weakly connected (e.g. across video cuts) are not constrained + relative to each other in GP1, so each covisibility component is aligned to its own anchors. + """ + if not self.options.enabled or not self.options.use_in_global_positioning: + return dict(dmap_scales) + + # Covisibility components of the GP1 tracks, ignoring observations that GP1 could not explain. + filtered = pycolmap.Reconstruction(reconstruction) + pycolmap.ObservationManager(filtered).filter_points3D_with_large_reprojection_error( + float(self.options.gp_alignment_max_angle_error_deg), + filtered.point3D_ids(), + pycolmap.ReprojectionErrorType.ANGULAR, + ) + components = detect_covisibility_components( + filtered, + min_shared_points=int(self.options.gp_alignment_min_shared_points), + min_component_size=1, + ) + del filtered + component_of = {image_id: index for index, component in enumerate(components) for image_id in component} + + anchors_by_component: dict[int, list[tuple[np.ndarray, np.ndarray, float]]] = {} + for image_id in self.active_image_ids(reconstruction): + anchor = self.anchors[image_id] + c_prior = self._resect_camera_center_from_rays(reconstruction, anchor) + if c_prior is None or image_id not in component_of: + continue + c_gp1 = np.asarray(reconstruction.image(image_id).projection_center(), dtype=np.float64) + if not np.all(np.isfinite(c_gp1)): + continue + anchors_by_component.setdefault(component_of[image_id], []).append( + (c_gp1, c_prior, max(float(anchor.confidence), 1e-3)) + ) + + if not anchors_by_component: + logger.warning("Location-prior 4-DoF GP1 alignment: no valid resected anchors; skipping alignment") + return dict(dmap_scales) + + transforms: dict[int, tuple[float, np.ndarray]] = {} + for index in sorted(anchors_by_component): + P_gp1, P_prior, W = (np.asarray(values, dtype=np.float64) for values in zip(*anchors_by_component[index])) + scale, translation, threshold = self._estimate_scale_translation(P_gp1, P_prior, W) + final_errs = np.linalg.norm(scale * P_gp1 + translation[None, :] - P_prior, axis=1) + logger.info( + "Location-prior 4-DoF GP1 alignment%s: scale=%.4f, inliers=%d/%d (<%.1fm), median_err=%.3fm", + ( + f" of component {index + 1}/{len(components)} ({len(components[index])} images)" + if len(anchors_by_component) > 1 + else "" + ), + scale, + int((final_errs < threshold).sum()), + len(P_gp1), + threshold, + float(np.median(final_errs)), + ) + transforms[index] = (scale, translation) + + if len(transforms) == 1: + # A single anchored part: transform all posed images and 3D points, c_new = scale * c_old + translation. + ((scale, translation),) = transforms.values() + reconstruction.transform(pycolmap.Sim3d(scale, pycolmap.Rotation3d(), translation)) + return {int(img_id): float(val) * scale for img_id, val in dmap_scales.items()} + + # Transform each anchored component separately; components without anchors are left unchanged. + image_scales = {} + for index, (scale, translation) in transforms.items(): + for image_id in components[index]: + image = reconstruction.image(image_id) + pose = image.frame.rig_from_world + center = scale * np.asarray(image.projection_center(), dtype=np.float64) + translation + image.frame.rig_from_world = pycolmap.Rigid3d(pose.rotation, -(pose.rotation * center)) + image_scales[image_id] = scale + for point3D_id in reconstruction.point3D_ids(): + point = reconstruction.point3D(point3D_id) + votes = [component_of.get(element.image_id) for element in point.track.elements] + votes = [index for index in votes if index is not None] + if not votes: + continue + index = max(set(votes), key=votes.count) + if index in transforms: + scale, translation = transforms[index] + point.xyz = scale * point.xyz + translation + return {int(img_id): float(val) * image_scales.get(int(img_id), 1.0) for img_id, val in dmap_scales.items()} + + def append_gp_observations( + self, + problem, + reconstruction: pycolmap.Reconstruction, + centers: dict[int, np.ndarray], + *, + stage: str = "gp2", + ): + """Add bearing constraints from the anchor centers to the prior 3D points in Global Positioning. + + Returns the storage to keep alive while using the problem and the number of residuals. + """ + if not self.options.enabled or not self.options.use_in_global_positioning: + return [], 0 + if stage == "gp1" and not self.options.use_in_gp1: + return [], 0 + + storage = [] + num_residuals = 0 + kp_stddev_px = float(self.options.gp1_kp_stddev_px if stage == "gp1" else self.options.gp_kp_stddev_px) + cauchy_scale = float(self.options.gp1_cauchy_scale) if stage == "gp1" else float(self.options.gp_cauchy_scale) + base_weight = float(self.options.gp_weight) + + for image_id in self.active_image_ids(reconstruction): + if image_id not in centers: + continue + anchor = self.anchors[image_id] + image = reconstruction.image(image_id) + camera = reconstruction.camera(image.camera_id) + fx, fy = float(camera.focal_length_x), float(camera.focal_length_y) + xy_px = anchor.uv_norm * np.array([float(camera.width), float(camera.height)], dtype=np.float64) + cam_pts = np.asarray(camera.cam_from_img(xy_px), dtype=np.float64) + if cam_pts.ndim != 2 or len(cam_pts) == 0: + continue + bearings = np.column_stack([cam_pts, np.ones(len(cam_pts), dtype=np.float64)]) + norms = np.linalg.norm(bearings, axis=1, keepdims=True) + valid = np.isfinite(norms[:, 0]) & (norms[:, 0] > 1e-9) + if not np.any(valid): + continue + bearings = bearings[valid] / norms[valid] + angular_stddevs_2d = kp_stddev_px * np.ones((len(bearings), 1), dtype=np.float64) / np.array([fx, fy]) + bearing_covars = _angular_stds_to_xyz_covar(bearings, angular_stddevs_2d) + angular_stddevs = np.sqrt(np.clip(np.diagonal(bearing_covars, axis1=1, axis2=2)[:, :2], 1e-18, None)) + angular_stddevs = np.maximum(angular_stddevs, 1e-9) + stddevs = np.column_stack([angular_stddevs, angular_stddevs.mean(axis=1)]) + + loss = pyceres.LossFunction( + dict(name="cauchy", params=[cauchy_scale], magnitude=base_weight * float(anchor.confidence)) + ) + observations, count = gp_costs.append_bearing_observations( + problem, + centers[image_id], + np.asarray(image.cam_from_world().rotation.matrix(), dtype=np.float64), + np.ascontiguousarray(anchor.points3D_xyz[valid], dtype=np.float64), + np.ascontiguousarray(bearings, dtype=np.float64), + np.ascontiguousarray(stddevs, dtype=np.float64), + loss, + ) + storage.append(observations) + num_residuals += count + + logger.info( + "Added %d location-prior 2D-3D positioning observations across %d active anchors", + num_residuals, + len(self.active_image_ids(reconstruction)), + ) + return storage, num_residuals + + def append_ba_constraints(self, problem, reconstruction: pycolmap.Reconstruction, image_ids): + """Add reprojection residuals of the prior 3D points into the anchors in Bundle Adjustment. + + Returns the storage to keep alive while using the problem and the number of residuals. + """ + if not self.options.enabled or not self.options.use_in_bundle_adjustment: + return [], 0 + + storage = [] + num_residuals = 0 + kp_stddev_px = float(self.options.ba_kp_stddev_px) + loss_type = getattr(pycolmap.LossFunctionType, self.options.ba_loss_name.upper()) + loss_scale_px = float(self.options.ba_loss_scale_px) + base_weight = float(self.options.ba_weight) + + image_ids = set(image_ids) + for image_id in self.active_image_ids(reconstruction): + if image_id not in image_ids: + continue + image = reconstruction.image(image_id) + anchor = self.anchors[image_id] + camera = reconstruction.camera(image.camera_id) + xy_px = anchor.uv_norm * np.array([float(camera.width), float(camera.height)], dtype=np.float64) + + # Filter out points currently behind the camera to avoid degenerate projection Jacobians + pts_cam = image.cam_from_world() * anchor.points3D_xyz + in_front = pts_cam[:, 2] > 0.05 + if not np.any(in_front): + continue + + loss, count = ba_costs.append_constant_point_reprojections( + problem, + reconstruction, + image_id, + np.ascontiguousarray(xy_px[in_front], dtype=np.float64), + np.ascontiguousarray(anchor.points3D_xyz[in_front], dtype=np.float64), + loss_type, + loss_scale_px, + base_weight * float(anchor.confidence) / (kp_stddev_px**2), + ) + storage.append(loss) + num_residuals += count + + return storage, num_residuals + + +def load_location_priors( + options: LocationPriorOptions, + solve_state: SolveState, +) -> LocationPriorSet | None: + """Load location priors from an .npz file and match them to images in solve_state. + + The priors come from any absolute localization of some frames against a georeferenced or otherwise fixed world + frame (the reconstruction is then expressed in that frame). Fields, for M prior frames, P world points and K + 2D-3D matches: + image_names (M,) str image file names (matched by basename, else by timestamp-like stem) + confidence (M,) float in [0, 1]; scales the prior weights, frames below min_confidence are skipped + R_cam_from_world (M, 3, 3) absolute camera rotations + cov_cam_from_world (M, 6, 6) pose covariance [rotation (rad), translation] in the COLMAP cam_from_world + tangent space; only the rotation block is used + points3D (P, 3) world points observed by the prior frames + match_indptr (M + 1,) CSR offsets of each frame's matches + match_point_indices (K,) index into points3D of each match + match_uv_norm (K, 2) image coordinates normalized by the image size, (x / width, y / height) + """ + if not options.enabled or not options.path: + return None + + npz_path = Path(options.path).expanduser().resolve() + if not npz_path.is_file(): + raise FileNotFoundError(f"Location priors file does not exist: {npz_path}") + + data = np.load(npz_path, allow_pickle=False) + image_names = [str(name) for name in data["image_names"]] + confidences = np.asarray(data["confidence"], dtype=np.float64) + R_cam_from_world = np.asarray(data["R_cam_from_world"], dtype=np.float64) + cov_cam_from_world = np.asarray(data["cov_cam_from_world"], dtype=np.float64) + points3D = np.asarray(data["points3D"], dtype=np.float64) + match_indptr = np.asarray(data["match_indptr"], dtype=np.int64) + match_point_indices = np.asarray(data["match_point_indices"], dtype=np.uint32) + match_uv_norm = np.asarray(data["match_uv_norm"], dtype=np.float64) + + # Map solve_state images by exact basename and by microsecond timestamp + id_by_basename: dict[str, int] = {} + id_by_ts_us: dict[int, int] = {} + for image_id, image in solve_state.reconstruction.images.items(): + basename = Path(image.name).name + id_by_basename[basename] = int(image_id) + ts_us = _parse_timestamp_us(basename) + if ts_us is not None: + id_by_ts_us[ts_us] = int(image_id) + + anchors: dict[int, LocationAnchorPrior] = {} + skipped_low_conf = 0 + skipped_unmatched = 0 + for idx, prior_name in enumerate(image_names): + conf = float(confidences[idx]) + start, end = int(match_indptr[idx]), int(match_indptr[idx + 1]) + num_matches = end - start + if conf < options.min_confidence or num_matches < options.min_inliers: + skipped_low_conf += 1 + continue + + basename = Path(prior_name).name + matched_id = id_by_basename.get(basename) + if matched_id is None: + ts_us = _parse_timestamp_us(basename) + if ts_us is not None and id_by_ts_us: + closest_ts = min(id_by_ts_us.keys(), key=lambda k: abs(k - ts_us)) + if abs(closest_ts - ts_us) <= 1000: + matched_id = id_by_ts_us[closest_ts] + if matched_id is None: + skipped_unmatched += 1 + continue + + pt_ids = match_point_indices[start:end].copy() + pts_xyz = points3D[pt_ids].copy() + uv_norm = match_uv_norm[start:end].copy() + + anchors[matched_id] = LocationAnchorPrior( + image_id=matched_id, + image_name=basename, + confidence=conf, + R_cam_from_world=R_cam_from_world[idx].copy(), + cov_rot_cam_from_world=cov_cam_from_world[idx, :3, :3].copy(), + prior_point_ids=pt_ids, + points3D_xyz=pts_xyz, + uv_norm=uv_norm, + ) + + logger.info( + "Loaded location priors from %s: %d matched keyframes (%d skipped low-conf/inliers, %d unmatched)", + npz_path.name, + len(anchors), + skipped_low_conf, + skipped_unmatched, + ) + return LocationPriorSet( + options=options, + anchors=anchors, + mapped_points_xyz=points3D, + ) diff --git a/vidmap/mapper/mapper.py b/vidmap/mapper/mapper.py index 63d4313..b6a1f3c 100644 --- a/vidmap/mapper/mapper.py +++ b/vidmap/mapper/mapper.py @@ -8,6 +8,7 @@ from vidmap.mapper.focal_prior import load_focal_prior from vidmap.mapper.inputs import MapperInputs from vidmap.mapper.inputs.snapshot import calibration_artifact_name +from vidmap.mapper.location_priors import load_location_priors from vidmap.utils.profiling import log_memory, record_timing, sync_time from .checkpoints import remove_disabled_intermediate_reconstructions @@ -153,6 +154,7 @@ def _prepare_calibration(self, solve_state, boundary): def _solve(self, mapping_stage_inputs, replay, playback_trace, prior): solve_start_time = sync_time() solve_state = mapping_stage_inputs.solve_state + location_priors = load_location_priors(self.conf.location_priors, solve_state) calibration = self.conf.calibration @@ -200,6 +202,7 @@ def _solve(self, mapping_stage_inputs, replay, playback_trace, prior): sequence_id_to_index=mapping_stage_inputs.sequence_id_to_index, filtered_consecutive_pair_ids=relative_pose.filtered_consecutive_pairs, replay=replay, + location_priors=location_priors, ) rotation_averager.average() @@ -235,6 +238,7 @@ def _solve(self, mapping_stage_inputs, replay, playback_trace, prior): options=self.conf.gp, output_dir=self.sfm_outputs_dir, replay=replay, + location_priors=location_priors, persist_intermediate_reconstructions=self.persist_intermediate_reconstructions, playback_trace=playback_trace, ) @@ -247,6 +251,7 @@ def _solve(self, mapping_stage_inputs, replay, playback_trace, prior): depth_stddev_multiplier=self.conf.mdrp.depth_stddev_multiplier, optimize_intrinsics=calibration.optimize_intrinsics, focal_prior=prior, + location_priors=location_priors, output_dir=self.sfm_outputs_dir, replay=replay, persist_intermediate_reconstructions=self.persist_intermediate_reconstructions, diff --git a/vidmap/mapper/options/location_priors.py b/vidmap/mapper/options/location_priors.py new file mode 100644 index 0000000..00e881c --- /dev/null +++ b/vidmap/mapper/options/location_priors.py @@ -0,0 +1,53 @@ +"""Options for absolute location priors (e.g. from visual localization) in RA, GP, and BA.""" + +from typing import Annotated, Literal, Optional + +from pydantic import ConfigDict, Field + +from vidmap.configuration.validators import dataclass as pydantic_dataclass + + +@pydantic_dataclass(frozen=True, config=ConfigDict(extra="forbid", strict=True)) +class LocationPriorOptions: + """Options for absolute rotation and 2D-3D correspondence constraints.""" + + enabled: bool = False + path: Optional[str] = None + min_confidence: Annotated[float, Field(ge=0.0, allow_inf_nan=False)] = 0.05 + min_inliers: Annotated[int, Field(ge=1)] = 4 + + # Stage 1: Rotation Averaging + use_in_rotation_averaging: bool = True + ra_weight: Annotated[float, Field(ge=0.0, allow_inf_nan=False)] = 20.0 + ra_cauchy_scale: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 3.0 + ra_ref_sigma_deg: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 1.0 + ra_min_sigma_deg: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 0.5 + + # Stage 2: Global Positioning + use_in_global_positioning: bool = True + use_in_gp1: bool = True + gp_weight: Annotated[float, Field(ge=0.0, allow_inf_nan=False)] = 1.0 + # The prior bearing residual is whitened by the keypoint stddev before the Cauchy loss, so the + # loss inflection in pixels is (cauchy_scale * kp_stddev_px). GP1 uses a stiff pull to initialize + # positions from the priors; GP2 is softer so that coherent blocks of prior outliers can be overruled. + gp1_kp_stddev_px: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 0.5 + gp1_cauchy_scale: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 48.0 + gp_kp_stddev_px: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 1.0 + gp_cauchy_scale: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 12.0 + gp_relaxed_scale_prior_stddev: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 5.0 + gp_ransac_inlier_threshold_m: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 3.0 + # Below this extent of the anchors, the GP1 scale is kept and only a translation is estimated. + gp_alignment_min_extent_for_scale_m: Annotated[float, Field(ge=0.0, allow_inf_nan=False)] = 2.0 + # GP1 is aligned to the priors separately for each part of the reconstruction whose images share fewer than + # gp_alignment_min_shared_points 3D points with the rest (e.g. across video cuts), counting only the points whose + # GP1 angular reprojection error is below gp_alignment_max_angle_error_deg. + gp_alignment_min_shared_points: Annotated[int, Field(ge=1)] = 20 + gp_alignment_max_angle_error_deg: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 2.0 + + # Stage 3: Bundle Adjustment + use_in_bundle_adjustment: bool = True + ba_weight: Annotated[float, Field(ge=0.0, allow_inf_nan=False)] = 1.0 + ba_kp_stddev_px: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 1.0 + ba_loss_name: Literal["trivial", "soft_l1", "cauchy", "huber"] = "cauchy" + ba_loss_scale_px: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 8.0 + ba_relaxed_scale_prior_stddev: Annotated[float, Field(gt=0.0, allow_inf_nan=False)] = 5.0 diff --git a/vidmap/mapper/options/mapper.py b/vidmap/mapper/options/mapper.py index 90c2002..261e874 100644 --- a/vidmap/mapper/options/mapper.py +++ b/vidmap/mapper/options/mapper.py @@ -8,6 +8,7 @@ from vidmap.configuration.validators import instantiate_nested_options from .calibration import CalibrationOptions +from .location_priors import LocationPriorOptions from .positioning import DepthConsistencyOptions, GPOptions, MapperTrackOptions from .refinement import BAOptions from .view_graph import InlierThresholdOptions, MDRPOptions, RAOptions, VGCOptions @@ -54,6 +55,7 @@ class MapperOptions: gp: GPOptions = dc_field(default_factory=GPOptions) ra: RAOptions = dc_field(default_factory=RAOptions) tracks: MapperTrackOptions = dc_field(default_factory=MapperTrackOptions) + location_priors: LocationPriorOptions = dc_field(default_factory=LocationPriorOptions) replay_cache: ReplayCacheOptions = dc_field(default_factory=ReplayCacheOptions) @@ -72,6 +74,7 @@ def instantiate_nested_option_groups(cls, raw): "gp": GPOptions, "ra": RAOptions, "tracks": MapperTrackOptions, + "location_priors": LocationPriorOptions, "replay_cache": ReplayCacheOptions, } raw = instantiate_nested_options(coerce_to_dict(raw), option_groups) diff --git a/vidmap/mapper/stages/bundle_adjustment/adjuster.py b/vidmap/mapper/stages/bundle_adjustment/adjuster.py index 31bf7ee..b3b19f7 100644 --- a/vidmap/mapper/stages/bundle_adjustment/adjuster.py +++ b/vidmap/mapper/stages/bundle_adjustment/adjuster.py @@ -13,6 +13,7 @@ import vidmap.utils.multiview_geometry as multiview_geometry from vidmap.mapper.checkpoints import reconstruction_checkpoint_directory from vidmap.mapper.focal_prior import native_focal_priors +from vidmap.mapper.location_priors import LocationPriorSet from vidmap.mapper.native.extension import native from vidmap.mapper.native.state import SolveState from vidmap.mapper.options.positioning import LossConfig @@ -74,6 +75,7 @@ class BundleAdjuster: output_dir: Path replay: ReplayCache focal_prior: dict[int, tuple[tuple[float, float], ...]] | None = None + location_priors: LocationPriorSet | None = None point_budget_scale: float = 1.0 persist_intermediate_reconstructions: bool = False playback_trace: PlaybackTraceRecorder | None = None @@ -160,8 +162,15 @@ def build_triangulator_options( } ) + @property + def _use_location_priors(self) -> bool: + priors = self.location_priors + return priors is not None and priors.options.enabled and priors.options.use_in_bundle_adjustment + def _refine_principal_point(self, remaining_solves: int) -> bool: # Count configured joint solves, excluding warm-up and point-only refinement. + if self._use_location_priors and len(self.reconstruction.cameras) > 1: + return False return self.optimize_intrinsics and remaining_solves < self.options.intrinsics.principal_point_last_n_solves def run_normal(self) -> None: @@ -170,6 +179,8 @@ def run_normal(self) -> None: while iteration < self.options.normal.iterations: if iteration == 0: self.log_depth_scales = {image_id: 0.0 for image_id in self.reconstruction.images.keys()} + if self._use_location_priors: + self.log_depth_scales.update(self.estimate_log_depth_scales()) self.log_depth_scales = self.reset_and_retriangulate( max_error_multiplier=self.options.first_iteration_error_multiplier * self.options.multiply_errors, ) @@ -442,6 +453,13 @@ def solve_problem( variable_point3D_ids = points.ids[ points.track_lengths < self.options.variable_point_track_length_threshold ].tolist() + location_priors = self.location_priors if self._use_location_priors and not policy.fix_all_poses else None + num_location_anchors = ( + len(set(location_priors.active_image_ids(self.reconstruction)) & set(optimized_image_ids)) + if location_priors is not None + else 0 + ) + relax_scale_prior = num_location_anchors >= 2 options, config = build_bundle_adjustment_options( reconstruction=self.reconstruction, image_order=optimized_image_ids, @@ -450,6 +468,8 @@ def solve_problem( optimize_intrinsics=self.optimize_intrinsics and not policy.fix_intrinsics, refine_principal_point=policy.refine_principal_point and not policy.fix_intrinsics, fix_rotations=policy.fix_rotations, + # Location priors fix the gauge. + fix_first_pose=num_location_anchors == 0, fix_all_poses=policy.fix_all_poses, reprojection_loss=reprojection_loss, reprojection_scale=self.options.reproj_loss_scale * keypoint_stddev, @@ -558,8 +578,12 @@ def solve_problem( image_id=image_id, log_scale=self.log_depth_scales[image_id], fix_scale=policy.fix_scale, - use_scale_prior=policy.regularize_scale, - scale_prior_stddev=depth_options.scale_std, + use_scale_prior=policy.regularize_scale and not relax_scale_prior, + scale_prior_stddev=( + float(self.location_priors.options.ba_relaxed_scale_prior_stddev) + if relax_scale_prior + else depth_options.scale_std + ), scale_prior_loss=LossConfig(name=depth_options.scale_reg_loss_name, weight=float(np.sum(mask))), ) ) @@ -577,6 +601,7 @@ def solve_problem( intrinsics_priors, self.solve_state, relative_intrinsics_priors=relative_intrinsics_priors, + location_priors=location_priors, playback_callback=playback_sink, playback_options=playback_options, ) diff --git a/vidmap/mapper/stages/bundle_adjustment/options.py b/vidmap/mapper/stages/bundle_adjustment/options.py index 275d499..18e1994 100644 --- a/vidmap/mapper/stages/bundle_adjustment/options.py +++ b/vidmap/mapper/stages/bundle_adjustment/options.py @@ -16,6 +16,7 @@ def build_bundle_adjustment_options( optimize_intrinsics, refine_principal_point, fix_rotations, + fix_first_pose=True, fix_all_poses=False, reprojection_loss, reprojection_scale, @@ -31,7 +32,7 @@ def build_bundle_adjustment_options( config.set_constant_cam_intrinsics(camera_id) for point_id in variable_point3D_ids: config.add_variable_point(point_id) - if image_order: + if image_order and fix_first_pose: config.set_constant_rig_from_world_pose(reconstruction.images[image_order[0]].frame_id) options = pycolmap.BundleAdjustmentOptions( refine_focal_length=True, diff --git a/vidmap/mapper/stages/bundle_adjustment/problem.py b/vidmap/mapper/stages/bundle_adjustment/problem.py index 69f31f4..5e54f4f 100644 --- a/vidmap/mapper/stages/bundle_adjustment/problem.py +++ b/vidmap/mapper/stages/bundle_adjustment/problem.py @@ -49,6 +49,7 @@ class BundleAdjustmentDiagnostics(SolverDiagnostics): num_depth_residuals: int = 0 num_intrinsics_prior_residuals: int = 0 num_relative_intrinsics_prior_residuals: int = 0 + num_location_reprojection_residuals: int = 0 num_scale_prior_residuals: int = 0 @@ -68,6 +69,7 @@ def run_bundle_adjustment( state, *, relative_intrinsics_priors=(), + location_priors=None, playback_callback=None, playback_options=None, ): @@ -117,6 +119,13 @@ def run_bundle_adjustment( ) diagnostics.num_relative_intrinsics_prior_residuals += 1 + # Reprojections of absolute location priors into their anchor images. + _location_storage = [] + if location_priors is not None: + _location_storage, diagnostics.num_location_reprojection_residuals = location_priors.append_ba_constraints( + problem, reconstruction, image_ids + ) + 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/global_positioning/positioner.py b/vidmap/mapper/stages/global_positioning/positioner.py index 3daa7c9..be8b079 100644 --- a/vidmap/mapper/stages/global_positioning/positioner.py +++ b/vidmap/mapper/stages/global_positioning/positioner.py @@ -13,6 +13,7 @@ from vidmap_native import global_positioning as gp_costs from vidmap.mapper.checkpoints import reconstruction_checkpoint_directory +from vidmap.mapper.location_priors import LocationPriorSet from vidmap.mapper.native.extension import native from vidmap.mapper.native.state import SolveState from vidmap.mapper.options.positioning import GPOptions @@ -170,6 +171,7 @@ class GlobalPositioner: options: GPOptions output_dir: Path replay: ReplayCache + location_priors: LocationPriorSet | None = None persist_intermediate_reconstructions: bool = False playback_trace: PlaybackTraceRecorder | None = None @@ -242,6 +244,10 @@ def run_pass( stddev = self.options.common.bearing_kp_stddev if stage == "gp2": stddev *= self.options.second_pass.relax_angular_stddevs + location_priors = self._location_priors(stage) + scale_prior_stddev = None + if location_priors is not None and location_priors.num_active_anchors(working) >= 2: + scale_prior_stddev = float(location_priors.options.gp_relaxed_scale_prior_stddev) result = run_global_positioning( self.options, working, @@ -257,6 +263,8 @@ def run_pass( playback_options=playback_options, playback_callback=callback, capture_state=record, + location_priors=location_priors, + scale_prior_stddev=scale_prior_stddev, ) if record: replay_result = self.result_for_replay(result) @@ -283,6 +291,14 @@ def run_pass( ) return result + def _location_priors(self, stage: str) -> LocationPriorSet | None: + priors = self.location_priors + if priors is None or not priors.options.enabled or not priors.options.use_in_global_positioning: + return None + if stage == "gp1" and not priors.options.use_in_gp1: + return None + return priors + def filter_tracks(self, working): focal_priors = { image.camera.has_prior_focal_length for image in working.images.values() if image.num_points3D > 0 @@ -345,6 +361,12 @@ def position(self) -> None: if self.options.second_pass.enabled: if self.options.track_filter.depth_prior_outlier_stages == "gp1": depth_masks = {} + initial_depth_map_scales = result.depth_map_scales + if self._location_priors("gp2") is not None: + # Bring GP1 into the prior frame (scale and translation) before refining with the priors. + initial_depth_map_scales = self.location_priors.align_gp1_to_location_priors_4dof( + working, dict(initial_depth_map_scales) + ) logger.info("Running second global positioning ...") self.run_pass( "gp2", @@ -353,7 +375,7 @@ def position(self) -> None: replay_images, depth_masks, temporal_prior_specs, - initial_depth_map_scales=result.depth_map_scales, + initial_depth_map_scales=initial_depth_map_scales, ) self.filter_tracks(working) self.solve_state.reconstruction = working diff --git a/vidmap/mapper/stages/global_positioning/problem.py b/vidmap/mapper/stages/global_positioning/problem.py index 0c6c166..bd69117 100644 --- a/vidmap/mapper/stages/global_positioning/problem.py +++ b/vidmap/mapper/stages/global_positioning/problem.py @@ -39,6 +39,7 @@ class GlobalPositioningDiagnostics(SolverDiagnostics): num_temporal_acceleration_residuals: int = 0 num_regular_observations_used: int = 0 num_loop_closure_observations_used: int = 0 + num_location_observations_used: int = 0 num_bata_scales: int = 0 num_depth_map_scales: int = 0 num_camera_centers: int = 0 @@ -110,6 +111,8 @@ def run_global_positioning( playback_options=None, playback_callback=None, capture_state=False, + location_priors=None, + scale_prior_stddev=None, ): """Prepare, extend and solve one GP pass on the caller's working reconstruction.""" common = options.common @@ -226,7 +229,13 @@ def run_global_positioning( for selection, loss in zip(selections[1:], geometry_losses[1:]) if len(selection) ] - if not options.common.use_metric_depth_constraint and gp_options.optimize_scales: + # Bearing constraints to absolute location priors, which also fix the gauge. + _location_storage, num_location = ( + location_priors.append_gp_observations(problem, reconstruction, centers, stage="gp2" if second_pass else "gp1") + if location_priors is not None + else ([], 0) + ) + if not options.common.use_metric_depth_constraint and gp_options.optimize_scales and not num_location: gp_costs.fix_first_observation_scale(problem, selections) regular, loop, num_points, has_support = gp_costs.observation_counts(selections) if warmup_rounds and not has_support: @@ -234,6 +243,8 @@ def run_global_positioning( diagnostics.num_regular_observations_used = regular diagnostics.num_loop_closure_observations_used = loop diagnostics.num_bata_residuals = diagnostics.num_bata_scales = regular + loop + diagnostics.num_location_observations_used = num_location + diagnostics.num_bata_residuals += num_location if capture_state: result.initial_bata_scales = _scale_values(selections) @@ -275,7 +286,7 @@ def run_global_positioning( prior_loss = _loss(LossConfig(name=pass_options.scale_reg_loss_name, weight=pass_options.scale_reg_weight)) prior_cost = pyceres.factors.NormalPrior( np.array([0.0 if common.use_log_scale_for_depth_map_scales else 1.0]), - np.array([[pass_options.scale_prior_stddev**2]]), + np.array([[(pass_options.scale_prior_stddev if scale_prior_stddev is None else scale_prior_stddev) ** 2]]), ) for image_id, count in counts.items(): scale = depth_scales[image_id] diff --git a/vidmap/mapper/stages/rotation_averaging.py b/vidmap/mapper/stages/rotation_averaging.py index 139887b..96a8172 100644 --- a/vidmap/mapper/stages/rotation_averaging.py +++ b/vidmap/mapper/stages/rotation_averaging.py @@ -10,6 +10,7 @@ import pycolmap from scipy.cluster.hierarchy import DisjointSet +from vidmap.mapper.location_priors import LocationPriorSet from vidmap.mapper.native.extension import native from vidmap.mapper.native.state import SolveState from vidmap.mapper.options.view_graph import RAOptions @@ -27,6 +28,7 @@ class RotationAverager: sequence_id_to_index: dict[int, int] filtered_consecutive_pair_ids: set[int] replay: ReplayCache + location_priors: LocationPriorSet | None = None def run_pass(self) -> bool: rec = self.solve_state.reconstruction @@ -98,6 +100,13 @@ def solve(self, graph: pycolmap.PoseGraph, tracking_graph: pycolmap.PoseGraph) - averager = pycolmap.create_default_ceres_rotation_averager( options, tracking_graph, self.solve_state.reconstruction ) + # Keep the prior losses alive while solving. + _prior_storage = None + if self.location_priors is not None: + # Align the spanning-tree initialization to the priors before solving. + _prior_storage, _ = self.location_priors.add_rotation_priors( + averager.problem, self.solve_state.reconstruction, align=not options.skip_initialization + ) for pair_id, edge in graph.edges.items(): if not edge.valid or pair_id in tracking_graph.edges or self.options.filter_risky_loop_closure_pairs: continue diff --git a/vidmap/run.py b/vidmap/run.py index ad2118a..2080361 100644 --- a/vidmap/run.py +++ b/vidmap/run.py @@ -25,6 +25,16 @@ def build_parser() -> ArgumentParser: ) parser.add_argument("--imnames", nargs="*", type=str) parser.add_argument("--intrinsics", type=str) + parser.add_argument( + "--gt-reconstruction", + type=str, + help="Optional ground-truth COLMAP reconstruction directory used to force GT keyframes.", + ) + parser.add_argument( + "--location-priors", + type=str, + help="Optional .npz of absolute location priors (per-image rotations and 2D-3D correspondences to world points) used in rotation averaging, global positioning, and BA.", + ) parser.add_argument("--name", type=str) parser.add_argument("--force-frontend", action="store_true") parser.add_argument( @@ -58,6 +68,15 @@ def main(argv=None): from vidmap.run_options import TIME_VARYING_INTRINSICS_OVERRIDES frontend_overrides = [*frontend_overrides, *TIME_VARYING_INTRINSICS_OVERRIDES] + if args.gt_reconstruction: + frontend_overrides = list(frontend_overrides) + [ + "keyframes.selection.force_gt_keyframes=true", + ] + if args.location_priors: + mapping_overrides = list(mapping_overrides) + [ + "mapper.location_priors.enabled=true", + f"mapper.location_priors.path={args.location_priors}", + ] 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 @@ -95,6 +114,7 @@ def main(argv=None): workspace=args.output, imnames=args.imnames, intrinsics_path=args.intrinsics, + gt_reconstruction_path=args.gt_reconstruction, force_frontend=args.force_frontend, cache_depth_maps=args.cache_depth_maps, device=run_options.device, diff --git a/vidmap/visualization/html/browser/browser_reconstruction.js b/vidmap/visualization/html/browser/browser_reconstruction.js index 772ee47..6f25dcb 100644 --- a/vidmap/visualization/html/browser/browser_reconstruction.js +++ b/vidmap/visualization/html/browser/browser_reconstruction.js @@ -92,7 +92,9 @@ localInput: options.localInput ?? null, covarianceCache: options.covarianceCache ?? null, database: database, - loopClosureMasks: loopClosureMasks + loopClosureMasks: loopClosureMasks, + gtTrajectory: options.gtTrajectory ?? null, + denseGtTrajectory: options.denseGtTrajectory ?? null }; } @@ -163,7 +165,9 @@ database: databaseEntry === undefined ? null : await decodedEmbeddedFile(databaseEntry, DATABASE_PATH), loopClosureMasks: masksEntry === undefined ? null - : await decodedEmbeddedFile(masksEntry, LOOP_CLOSURE_MASKS_PATH) + : await decodedEmbeddedFile(masksEntry, LOOP_CLOSURE_MASKS_PATH), + gtTrajectory: payload.gtTrajectory ?? null, + denseGtTrajectory: payload.denseGtTrajectory ?? null }); } @@ -691,6 +695,7 @@ setMinimumTrackLength(1); keyframeTimeline.pointOffsets = Array.from(loaded.points.pointOffsets); replaceEstimatedGeometry(loaded.cameras.centers, loaded.cameras.frusta); + replaceGtGeometry(selection.gtTrajectory, selection.denseGtTrajectory); let loopClosureStatus; if (selection.database !== null) { setStatus("model loaded; validating loop-closure masks…"); diff --git a/vidmap/visualization/html/browser/browser_scene.js b/vidmap/visualization/html/browser/browser_scene.js index 789c74b..cc6e4d6 100644 --- a/vidmap/visualization/html/browser/browser_scene.js +++ b/vidmap/visualization/html/browser/browser_scene.js @@ -152,6 +152,23 @@ __RECONSTRUCTION_SCRIPT__ let imageDirectory = null; let estimatedFrustaPositions = new Float32Array(); let estimatedPathPositions = new Float32Array(); + let gtPathPositions = new Float32Array(); + let gtKeyframePathCounts = []; + let gtKeyframeFrustaCounts = []; + let gtKeyframeCenters = new Float32Array(); + let gtFrustaPositions = new Float32Array(); + let gtCovEllipsesPositions = new Float32Array(); + let gtKeyframeConfidences = []; + let gtKeyframeNames = []; + let gtKeyframeInliers = []; + let gtKeyframePosStdMeters = []; + let gtKeyframeReprojRmsPx = []; + let gtMetadataByName = new Map(); + let denseGtPathPositions = new Float32Array(); + let denseGtKeyframePathCounts = []; + let denseGtKeyframeFrustaCounts = []; + let denseGtKeyframeCenters = new Float32Array(); + let denseGtFrustaPositions = new Float32Array(); let loopClosureSharedPoints = new Float32Array(); let loopClosureKeyframeIndices = new Uint32Array(); @@ -489,6 +506,7 @@ __RECONSTRUCTION_SCRIPT__ const empty = new Float32Array(); replacePointGeometry(empty, null); replaceEstimatedGeometry(empty, empty); + replaceGtGeometry(null, null); replaceLoopClosureGeometry(new Uint32Array(), new Float32Array()); error.style.display = "none"; error.textContent = ""; @@ -588,6 +606,18 @@ __RECONSTRUCTION_SCRIPT__ } const estimatedFrusta = wideLineSegments(estimatedFrustaPositions, 0xd62728, 0.45, 1); scene.add(estimatedFrusta); + const gtFrusta = wideLineSegments(gtFrustaPositions, 0x00c853, 0.85, 2); + gtFrusta.visible = false; + gtFrusta.renderOrder = 95; + scene.add(gtFrusta); + const gtCovEllipses = wideLineSegments(gtCovEllipsesPositions, 0x00897b, 0.78, 1.5); + gtCovEllipses.visible = false; + gtCovEllipses.renderOrder = 93; + scene.add(gtCovEllipses); + const denseGtFrusta = wideLineSegments(denseGtFrustaPositions, 0xeab308, 0.75, 1.5); + denseGtFrusta.visible = false; + denseGtFrusta.renderOrder = 90; + scene.add(denseGtFrusta); const initialLoopClosures = visibleLoopClosureData(keyframeTimeline.names.length - 1); const loopClosures = coloredWideLineSegments( initialLoopClosures.positions, @@ -595,6 +625,7 @@ __RECONSTRUCTION_SCRIPT__ 3, __LOOP_CLOSURE_MIN_SHARED_POINTS__ ); + loopClosures.visible = false; let visibleLoopClosurePositions = initialLoopClosures.positions; let visibleLoopClosureIndices = initialLoopClosures.edgeIndices; scene.add(loopClosures); @@ -687,32 +718,222 @@ __RECONSTRUCTION_SCRIPT__ result.renderOrder = 100; return result; } - function updatePathPrefix(object, positions, pointCount) { + function updatePathPrefix(object, positions, pointCount, extraVisible = true) { const replacement = new THREE.LineGeometry(); const hasSegments = pointCount >= 2; if (hasSegments) replacement.setPositions(positions.subarray(0, pointCount * 3)); object.geometry.dispose(); object.geometry = replacement; - object.visible = hasSegments && document.getElementById("paths-toggle").checked; + object.visible = hasSegments && document.getElementById("paths-toggle").checked && extraVisible; invalidateSceneRender(); } const estimatedPath = path(estimatedPathPositions, 0x0064ff); scene.add(estimatedPath); + const gtPath = path(gtPathPositions, 0x00c853); + scene.add(gtPath); + const denseGtPath = path(denseGtPathPositions, 0xeab308); + denseGtPath.visible = false; + denseGtPath.renderOrder = 98; + scene.add(denseGtPath); - function updateFrustaPrefix(object, base, centers, pointCount, size) { - const scaled = scaledFrusta(base, centers, size, pointCount); + function priorConfidenceRgb(conf) { + const t = Math.max(0, Math.min(1, (Number(conf) - 0.95) / 0.05)); + if (t < 0.5) { + const u = t * 2; + return [(235 - 15 * u) / 255, (45 + 115 * u) / 255, 0]; + } + const u = (t - 0.5) * 2; + return [(220 * (1 - u)) / 255, (160 + 40 * u) / 255, (85 * u) / 255]; + } + function perCameraVertexColors(confidences, cameraCount, verticesPerCamera) { + const colors = new Float32Array(cameraCount * verticesPerCamera * 3); + for (let c = 0; c < cameraCount; ++c) { + const conf = c < confidences.length ? confidences[c] : 1.0; + const rgb = priorConfidenceRgb(conf); + const base = c * verticesPerCamera * 3; + for (let v = 0; v < verticesPerCamera; ++v) { + colors[base + v * 3] = rgb[0]; + colors[base + v * 3 + 1] = rgb[1]; + colors[base + v * 3 + 2] = rgb[2]; + } + } + return colors; + } + function updateCameraSegmentsPrefix( + object, + base, + centers, + pointCount, + scaleFactor, + verticesPerCamera, + confidences = null, + solidColorHex = null + ) { + const scaled = scaledCameraSegments(base, centers, scaleFactor, pointCount, verticesPerCamera); const replacement = new THREE.LineSegmentsGeometry(); - if (scaled.length > 0) replacement.setPositions(scaled); + if (scaled.length > 0) { + replacement.setPositions(scaled); + if (confidences !== null && confidences.length > 0) { + const camCount = scaled.length / (verticesPerCamera * 3); + replacement.setColors(perCameraVertexColors(confidences, camCount, verticesPerCamera)); + object.material.vertexColors = true; + object.material.color.set(0xffffff); + object.material.needsUpdate = true; + } else if (solidColorHex !== null) { + object.material.vertexColors = false; + object.material.color.set(solidColorHex); + object.material.needsUpdate = true; + } + } object.geometry.dispose(); object.geometry = replacement; invalidateSceneRender(); } + function updateFrustaPrefix(object, base, centers, pointCount, size, confidences = null, solidColorHex = null) { + updateCameraSegmentsPrefix(object, base, centers, pointCount, size / 0.3, 16, confidences, solidColorHex); + } + function updateCovEllipsesPrefix( + object, + base, + centers, + pointCount, + sigmaScale, + confidences = null, + solidColorHex = null + ) { + updateCameraSegmentsPrefix(object, base, centers, pointCount, sigmaScale, 144, confidences, solidColorHex); + } function replaceEstimatedGeometry(centers, frusta) { estimatedPathPositions = centers; estimatedFrustaPositions = frusta; updateLoopClosureGeometry(Number(document.getElementById("keyframe-slider").value)); fitScene(); } + function replaceGtGeometry(gtTrajectory, denseGtTrajectory = null) { + const hasGt = ( + gtTrajectory !== null && typeof gtTrajectory === "object" && + Array.isArray(gtTrajectory.centers) && gtTrajectory.centers.length >= 6 + ); + gtPathPositions = hasGt ? Float32Array.from(gtTrajectory.centers) : new Float32Array(); + gtKeyframePathCounts = ( + hasGt && Array.isArray(gtTrajectory.keyframePathCounts) ? gtTrajectory.keyframePathCounts : [] + ); + gtKeyframeFrustaCounts = ( + hasGt && Array.isArray(gtTrajectory.keyframeFrustaCounts) + ? gtTrajectory.keyframeFrustaCounts + : gtKeyframePathCounts + ); + gtKeyframeCenters = ( + hasGt && Array.isArray(gtTrajectory.keyframeCenters) + ? Float32Array.from(gtTrajectory.keyframeCenters) + : new Float32Array() + ); + gtFrustaPositions = ( + hasGt && Array.isArray(gtTrajectory.keyframeFrusta) + ? Float32Array.from(gtTrajectory.keyframeFrusta) + : new Float32Array() + ); + gtCovEllipsesPositions = ( + hasGt && Array.isArray(gtTrajectory.keyframeCovEllipses) + ? Float32Array.from(gtTrajectory.keyframeCovEllipses) + : new Float32Array() + ); + gtKeyframeConfidences = ( + hasGt && Array.isArray(gtTrajectory.keyframeConfidences) + ? gtTrajectory.keyframeConfidences + : [] + ); + gtKeyframeNames = ( + hasGt && Array.isArray(gtTrajectory.keyframeNames) + ? gtTrajectory.keyframeNames + : [] + ); + gtKeyframeInliers = ( + hasGt && Array.isArray(gtTrajectory.keyframeInliers) + ? gtTrajectory.keyframeInliers + : [] + ); + gtKeyframePosStdMeters = ( + hasGt && Array.isArray(gtTrajectory.keyframePosStdMeters) + ? gtTrajectory.keyframePosStdMeters + : [] + ); + gtKeyframeReprojRmsPx = ( + hasGt && Array.isArray(gtTrajectory.keyframeReprojRmsPx) + ? gtTrajectory.keyframeReprojRmsPx + : [] + ); + gtMetadataByName = new Map(); + for (let i = 0; i < gtKeyframeNames.length; ++i) { + gtMetadataByName.set(gtKeyframeNames[i], { + index: i, + name: gtKeyframeNames[i], + confidence: i < gtKeyframeConfidences.length ? gtKeyframeConfidences[i] : null, + inliers: i < gtKeyframeInliers.length ? gtKeyframeInliers[i] : null, + posStdMeters: i < gtKeyframePosStdMeters.length ? gtKeyframePosStdMeters[i] : null, + reprojRmsPx: i < gtKeyframeReprojRmsPx.length ? gtKeyframeReprojRmsPx[i] : null + }); + } + const hasDenseGt = ( + denseGtTrajectory !== null && typeof denseGtTrajectory === "object" && + Array.isArray(denseGtTrajectory.centers) && denseGtTrajectory.centers.length >= 6 + ); + denseGtPathPositions = hasDenseGt ? Float32Array.from(denseGtTrajectory.centers) : new Float32Array(); + denseGtKeyframePathCounts = ( + hasDenseGt && Array.isArray(denseGtTrajectory.keyframePathCounts) + ? denseGtTrajectory.keyframePathCounts + : [] + ); + denseGtKeyframeFrustaCounts = ( + hasDenseGt && Array.isArray(denseGtTrajectory.keyframeFrustaCounts) + ? denseGtTrajectory.keyframeFrustaCounts + : denseGtKeyframePathCounts + ); + denseGtKeyframeCenters = ( + hasDenseGt && Array.isArray(denseGtTrajectory.keyframeCenters) + ? Float32Array.from(denseGtTrajectory.keyframeCenters) + : new Float32Array() + ); + denseGtFrustaPositions = ( + hasDenseGt && Array.isArray(denseGtTrajectory.keyframeFrusta) + ? Float32Array.from(denseGtTrajectory.keyframeFrusta) + : new Float32Array() + ); + const hasConf = hasGt && gtKeyframeConfidences.length > 0; + const hasCov = hasGt && gtCovEllipsesPositions.length > 0; + const gtFrustaLabel = document.getElementById("gt-frusta-label"); + const gtPathLabel = document.getElementById("gt-path-label"); + if (gtFrustaLabel !== null) gtFrustaLabel.textContent = (hasDenseGt || hasConf) ? "prior cameras" : "GT cameras"; + if (gtPathLabel !== null) gtPathLabel.textContent = (hasDenseGt || hasConf) ? "prior" : "GT"; + document.getElementById("gt-path-controls").hidden = !hasGt; + document.getElementById("gt-frusta-row").hidden = !(hasGt && gtFrustaPositions.length > 0); + const gtFrustaModeSetting = document.getElementById("gt-frusta-mode-setting"); + if (gtFrustaModeSetting !== null) gtFrustaModeSetting.hidden = !hasConf; + const gtCovRow = document.getElementById("gt-cov-row"); + if (gtCovRow !== null) gtCovRow.hidden = !hasCov; + document.getElementById("dense-gt-path-controls").hidden = !hasDenseGt; + document.getElementById("dense-gt-frusta-row").hidden = !(hasDenseGt && denseGtFrustaPositions.length > 0); + const gtFrustaColorInput = document.getElementById("gt-frusta-color"); + const gtFrustaColorMode = document.getElementById("gt-frusta-color-mode"); + if (gtFrustaColorInput !== null && gtFrustaColorMode !== null) { + gtFrustaColorInput.disabled = hasConf && gtFrustaColorMode.value === "confidence"; + } + const gtCovColorInput = document.getElementById("gt-cov-color"); + const gtCovColorMode = document.getElementById("gt-cov-color-mode"); + if (gtCovColorInput !== null && gtCovColorMode !== null) { + gtCovColorInput.disabled = hasConf && gtCovColorMode.value === "confidence"; + } + if (!hasGt) { + gtPath.visible = false; + gtFrusta.visible = false; + gtCovEllipses.visible = false; + } + if (!hasDenseGt) { + denseGtPath.visible = false; + denseGtFrusta.visible = false; + } + fitScene(); + } function showKeyframe(index) { if (keyframeTimeline.names.length === 0) return; const bounded = Math.max(0, Math.min(index, keyframeTimeline.names.length - 1)); @@ -727,11 +948,22 @@ __RECONSTRUCTION_SCRIPT__ const timestampText = keyframeTimeline.timestampsSeconds === null ? "" : ` · ${keyframeTimeline.timestampsSeconds[bounded].toFixed(3)} s`; + const kfName = keyframeTimeline.names[bounded]; + let priorStatusText = ""; + if (gtMetadataByName.has(kfName)) { + const meta = gtMetadataByName.get(kfName); + if (meta.confidence !== null) { + priorStatusText += ` · prior conf=${Number(meta.confidence).toFixed(4)}`; + } + if (Array.isArray(meta.posStdMeters) && meta.posStdMeters.length >= 1) { + priorStatusText += ` · σ_pos=${Number(meta.posStdMeters[0]).toFixed(2)}m`; + } + } document.getElementById("keyframe-label").textContent = - `${bounded + 1}/${keyframeTimeline.names.length}${timestampText} · ${keyframeTimeline.names[bounded]} · ${pointCount.toLocaleString()} cumulative points`; + `${bounded + 1}/${keyframeTimeline.names.length}${timestampText} · ${kfName} · ${pointCount.toLocaleString()} cumulative points${priorStatusText}`; const preview = document.getElementById("keyframe-preview"); if (imageDirectory !== null) { - loadImage(preview, imageDirectory, keyframeTimeline.names[bounded]); + loadImage(preview, imageDirectory, kfName); } preview.dataset.keyframeIndex = String(bounded); drawTrackedKeypoints(bounded); @@ -740,6 +972,82 @@ __RECONSTRUCTION_SCRIPT__ updateFrustaPrefix( estimatedFrusta, estimatedFrustaPositions, estimatedPathPositions, bounded + 1, estimatedSize ); + const gtPointCount = bounded < gtKeyframePathCounts.length + ? gtKeyframePathCounts[bounded] + : Math.floor(gtPathPositions.length / 3); + if (gtPathPositions.length >= 6) { + updatePathPrefix( + gtPath, + gtPathPositions, + gtPointCount, + document.getElementById("gt-path-toggle").checked + ); + } else { + gtPath.visible = false; + } + const gtFrustaCount = bounded < gtKeyframeFrustaCounts.length + ? gtKeyframeFrustaCounts[bounded] + : Math.floor(gtKeyframeCenters.length / 3); + if (gtFrustaPositions.length > 0) { + const gtSize = Number(document.getElementById("gt-frusta-size").value); + const gtColorMode = document.getElementById("gt-frusta-color-mode"); + const useGtConf = gtKeyframeConfidences.length > 0 && gtColorMode !== null && gtColorMode.value === "confidence"; + updateFrustaPrefix( + gtFrusta, + gtFrustaPositions, + gtKeyframeCenters, + gtFrustaCount, + gtSize, + useGtConf ? gtKeyframeConfidences : null, + document.getElementById("gt-frusta-color").value + ); + gtFrusta.visible = gtFrustaCount > 0 && document.getElementById("gt-frusta-toggle").checked; + } else { + gtFrusta.visible = false; + } + if (gtCovEllipsesPositions.length > 0) { + const covScale = Number(document.getElementById("gt-cov-scale").value); + const covColorMode = document.getElementById("gt-cov-color-mode"); + const useCovConf = gtKeyframeConfidences.length > 0 && covColorMode !== null && covColorMode.value === "confidence"; + updateCovEllipsesPrefix( + gtCovEllipses, + gtCovEllipsesPositions, + gtKeyframeCenters, + gtFrustaCount, + covScale, + useCovConf ? gtKeyframeConfidences : null, + document.getElementById("gt-cov-color").value + ); + const covToggle = document.getElementById("gt-cov-toggle"); + gtCovEllipses.visible = gtFrustaCount > 0 && covToggle !== null && covToggle.checked; + } else { + gtCovEllipses.visible = false; + } + const denseGtPointCount = bounded < denseGtKeyframePathCounts.length + ? denseGtKeyframePathCounts[bounded] + : Math.floor(denseGtPathPositions.length / 3); + if (denseGtPathPositions.length >= 6) { + updatePathPrefix( + denseGtPath, + denseGtPathPositions, + denseGtPointCount, + document.getElementById("dense-gt-path-toggle").checked + ); + } else { + denseGtPath.visible = false; + } + if (denseGtFrustaPositions.length > 0) { + const denseGtSize = Number(document.getElementById("dense-gt-frusta-size").value); + const denseGtFrustaCount = bounded < denseGtKeyframeFrustaCounts.length + ? denseGtKeyframeFrustaCounts[bounded] + : Math.floor(denseGtKeyframeCenters.length / 3); + updateFrustaPrefix( + denseGtFrusta, denseGtFrustaPositions, denseGtKeyframeCenters, denseGtFrustaCount, denseGtSize + ); + denseGtFrusta.visible = denseGtFrustaCount > 0 && document.getElementById("dense-gt-frusta-toggle").checked; + } else { + denseGtFrusta.visible = false; + } updateLoopClosureGeometry(bounded); } const keyframePreview = document.getElementById("keyframe-preview"); @@ -784,6 +1092,8 @@ __RECONSTRUCTION_SCRIPT__ function fitScene() { bounds.makeEmpty(); const fitPositions = estimatedPathPositions.length > 0 ? [estimatedPathPositions] : [pointPositions]; + if (gtPathPositions.length > 0) fitPositions.push(gtPathPositions); + else if (denseGtPathPositions.length > 0) fitPositions.push(denseGtPathPositions); for (const positions of fitPositions) { for (let index = 0; index < positions.length; index += 3) { fitPoint.set(positions[index], positions[index + 1], positions[index + 2]); @@ -886,7 +1196,17 @@ __RECONSTRUCTION_SCRIPT__ trackedKeypointsToggle.onchange = () => drawTrackedKeypoints(Number(keyframeSlider.value)); const estimatedFrustaToggle = document.getElementById("estimated-frusta-toggle"); estimatedFrustaToggle.onchange = event => estimatedFrusta.visible = event.target.checked; + const gtFrustaToggle = document.getElementById("gt-frusta-toggle"); + gtFrustaToggle.onchange = () => showKeyframe(Number(keyframeSlider.value)); + const gtCovToggle = document.getElementById("gt-cov-toggle"); + if (gtCovToggle !== null) { + gtCovToggle.onchange = () => showKeyframe(Number(keyframeSlider.value)); + } + const denseGtFrustaToggle = document.getElementById("dense-gt-frusta-toggle"); + denseGtFrustaToggle.onchange = () => showKeyframe(Number(keyframeSlider.value)); document.getElementById("paths-toggle").onchange = () => showKeyframe(Number(keyframeSlider.value)); + document.getElementById("gt-path-toggle").onchange = () => showKeyframe(Number(keyframeSlider.value)); + document.getElementById("dense-gt-path-toggle").onchange = () => showKeyframe(Number(keyframeSlider.value)); const loopClosuresToggle = document.getElementById("loop-closures-toggle"); loopClosuresToggle.onchange = () => updateLoopClosureGeometry(Number(keyframeSlider.value)); bindNumber("loop-closures-width", value => loopClosures.material.linewidth = value); @@ -898,19 +1218,24 @@ __RECONSTRUCTION_SCRIPT__ function bindNumber(id, update) { const input = document.getElementById(id); + if (input === null) return; input.oninput = () => { const value = Number(input.value); if (Number.isFinite(value) && value > 0) update(value); }; } - function scaledFrusta(base, centers, size, requestedCount = centers.length / 3) { - const verticesPerCamera = 16; + function scaledCameraSegments( + base, + centers, + scale, + requestedCount = centers.length / 3, + verticesPerCamera = 16 + ) { const cameraCount = Math.max(0, Math.floor(Math.min( requestedCount, centers.length / 3, base.length / (verticesPerCamera * 3) ))); const result = new Float32Array(cameraCount * verticesPerCamera * 3); - const scale = size / 0.3; for (let cameraIndex = 0; cameraIndex < cameraCount; ++cameraIndex) { const centerOffset = cameraIndex * 3; const centerX = centers[centerOffset]; @@ -927,6 +1252,10 @@ __RECONSTRUCTION_SCRIPT__ return result; } + function scaledFrusta(base, centers, size, requestedCount = centers.length / 3) { + return scaledCameraSegments(base, centers, size / 0.3, requestedCount, 16); + } + function updatePointSize(value) { const minimumSize = 1 / renderer.getPixelRatio(); const coverage = Math.min(value / minimumSize, 1); @@ -958,9 +1287,125 @@ __RECONSTRUCTION_SCRIPT__ bindNumber("estimated-frusta-width", value => estimatedFrusta.material.linewidth = value); document.getElementById("estimated-frusta-color").oninput = event => estimatedFrusta.material.color.set(event.target.value); - bindNumber("paths-width", value => estimatedPath.material.linewidth = value); + bindNumber( + "gt-frusta-size", + () => showKeyframe(Number(keyframeSlider.value)) + ); + bindNumber("gt-frusta-width", value => gtFrusta.material.linewidth = value); + const gtFrustaColorInput = document.getElementById("gt-frusta-color"); + const gtFrustaColorMode = document.getElementById("gt-frusta-color-mode"); + gtFrustaColorInput.oninput = () => showKeyframe(Number(keyframeSlider.value)); + if (gtFrustaColorMode !== null) { + gtFrustaColorMode.onchange = event => { + gtFrustaColorInput.disabled = gtKeyframeConfidences.length > 0 && event.target.value === "confidence"; + showKeyframe(Number(keyframeSlider.value)); + }; + } + bindNumber( + "gt-cov-scale", + () => showKeyframe(Number(keyframeSlider.value)) + ); + bindNumber("gt-cov-width", value => gtCovEllipses.material.linewidth = value); + const gtCovColorInput = document.getElementById("gt-cov-color"); + const gtCovColorMode = document.getElementById("gt-cov-color-mode"); + if (gtCovColorInput !== null) { + gtCovColorInput.oninput = () => showKeyframe(Number(keyframeSlider.value)); + } + if (gtCovColorMode !== null) { + gtCovColorMode.onchange = event => { + if (gtCovColorInput !== null) { + gtCovColorInput.disabled = gtKeyframeConfidences.length > 0 && event.target.value === "confidence"; + } + showKeyframe(Number(keyframeSlider.value)); + }; + } + bindNumber( + "dense-gt-frusta-size", + () => showKeyframe(Number(keyframeSlider.value)) + ); + bindNumber("dense-gt-frusta-width", value => denseGtFrusta.material.linewidth = value); + document.getElementById("dense-gt-frusta-color").oninput = + event => denseGtFrusta.material.color.set(event.target.value); + bindNumber("paths-width", value => { + estimatedPath.material.linewidth = value; + gtPath.material.linewidth = value; + denseGtPath.material.linewidth = value; + }); document.getElementById("estimated-path-color").oninput = event => estimatedPath.material.color.set(event.target.value); + document.getElementById("gt-path-color").oninput = + event => gtPath.material.color.set(event.target.value); + document.getElementById("dense-gt-path-color").oninput = + event => denseGtPath.material.color.set(event.target.value); + + const priorHoverTooltip = document.getElementById("prior-hover-tooltip"); + const hoverProjVec = new THREE.Vector3(); + function updatePriorHoverTooltip(clientX, clientY) { + if (priorHoverTooltip === null) return; + if ( + orbitPointerId !== null || + rollPointerId !== null || + (!gtFrusta.visible && !gtCovEllipses.visible) || + gtKeyframeCenters.length === 0 + ) { + priorHoverTooltip.hidden = true; + return; + } + const rect = renderer.domElement.getBoundingClientRect(); + if (clientX < rect.left || clientX >= rect.right || clientY < rect.top || clientY >= rect.bottom) { + priorHoverTooltip.hidden = true; + return; + } + const px = clientX - rect.left; + const py = clientY - rect.top; + const bounded = Math.max(0, Math.min(Number(keyframeSlider.value), keyframeTimeline.names.length - 1)); + const visibleCount = bounded < gtKeyframeFrustaCounts.length + ? gtKeyframeFrustaCounts[bounded] + : Math.floor(gtKeyframeCenters.length / 3); + let bestIdx = -1; + let bestDist = 18; + for (let i = 0; i < visibleCount; ++i) { + hoverProjVec.fromArray(gtKeyframeCenters, i * 3).project(camera); + if (hoverProjVec.z < -1 || hoverProjVec.z > 1) continue; + const sx = (hoverProjVec.x + 1) * rect.width * 0.5; + const sy = (1 - hoverProjVec.y) * rect.height * 0.5; + const d = Math.hypot(px - sx, py - sy); + if (d < bestDist) { + bestDist = d; + bestIdx = i; + } + } + if (bestIdx < 0) { + priorHoverTooltip.hidden = true; + return; + } + const name = bestIdx < gtKeyframeNames.length ? gtKeyframeNames[bestIdx] : `prior #${bestIdx + 1}`; + const lines = [`prior anchor: ${name}`]; + if (bestIdx < gtKeyframeConfidences.length) { + lines.push(`Confidence: ${Number(gtKeyframeConfidences[bestIdx]).toFixed(4)}`); + } + if (bestIdx < gtKeyframePosStdMeters.length && Array.isArray(gtKeyframePosStdMeters[bestIdx])) { + const s = gtKeyframePosStdMeters[bestIdx]; + lines.push( + `Position std (1σ): ${Number(s[0]).toFixed(3)} m (axes: ${Number(s[1]).toFixed(2)}, ${Number(s[2]).toFixed(2)}, ${Number(s[3]).toFixed(2)} m)` + ); + } + if (bestIdx < gtKeyframeInliers.length) { + const inl = gtKeyframeInliers[bestIdx]; + const rms = bestIdx < gtKeyframeReprojRmsPx.length ? Number(gtKeyframeReprojRmsPx[bestIdx]).toFixed(2) : null; + lines.push(rms !== null ? `2D-3D inliers: ${inl} (reproj RMS: ${rms} px)` : `2D-3D inliers: ${inl}`); + } + priorHoverTooltip.textContent = lines.join("\n"); + priorHoverTooltip.style.left = `${Math.min(window.innerWidth - 280, clientX + 14)}px`; + priorHoverTooltip.style.top = `${Math.min(window.innerHeight - 90, clientY + 14)}px`; + priorHoverTooltip.hidden = false; + } + renderer.domElement.addEventListener("pointermove", event => { + updatePriorHoverTooltip(event.clientX, event.clientY); + }); + renderer.domElement.addEventListener("pointerleave", () => { + if (priorHoverTooltip !== null) priorHoverTooltip.hidden = true; + }); document.getElementById("projection").onchange = event => { const nextCamera = event.target.value === "orthographic" @@ -1010,7 +1455,7 @@ __RECONSTRUCTION_SCRIPT__ function resize() { updateProjectionDimensions(); renderer.setSize(window.innerWidth, window.innerHeight); - for (const object of [estimatedFrusta, loopClosures, estimatedPath]) { + for (const object of [estimatedFrusta, gtFrusta, gtCovEllipses, denseGtFrusta, loopClosures, estimatedPath, gtPath, denseGtPath]) { object.material.resolution.set(window.innerWidth, window.innerHeight); } drawTrackedKeypoints(Number(keyframeSlider.value)); diff --git a/vidmap/visualization/html/browser/vidmap-viewer.html.in b/vidmap/visualization/html/browser/vidmap-viewer.html.in index df197e3..46c939e 100644 --- a/vidmap/visualization/html/browser/vidmap-viewer.html.in +++ b/vidmap/visualization/html/browser/vidmap-viewer.html.in @@ -103,6 +103,14 @@ button:hover, .file-button:hover { background: #eee; } #help { margin-top: 5px; color: #666; } #error { display: none; color: #a00; max-width: 480px; } + #prior-hover-tooltip { + position: fixed; z-index: 20; pointer-events: none; + padding: 6px 9px; border-radius: 5px; + background: rgba(20, 24, 30, 0.92); color: #f3f4f6; + font: 12px/1.45 system-ui, sans-serif; + box-shadow: 0 2px 10px rgba(0,0,0,.35); + white-space: pre-line; + } @@ -111,6 +119,7 @@ __SQLITE_SCRIPT__
+
Keyframe
@@ -224,13 +233,45 @@ __SQLITE_SCRIPT__
+ + +
+ +
- +
diff --git a/vidmap/visualization/html/embedded.py b/vidmap/visualization/html/embedded.py index 8d74e1d..16c8ca4 100644 --- a/vidmap/visualization/html/embedded.py +++ b/vidmap/visualization/html/embedded.py @@ -171,11 +171,327 @@ def _image_previews( } +def _camera_frustum_segments(image, camera) -> tuple[list[float], list[float]]: + center = np.asarray(image.projection_center(), dtype=np.float64) + width = float(camera.width) + height = float(camera.height) + fx = float(camera.focal_length_x) + fy = float(camera.focal_length_y) + cx = float(camera.principal_point_x) + cy = float(camera.principal_point_y) + image_extent = max(0.3 * width / 1024.0, 0.3 * height / 1024.0) + world_extent = 2.0 * max(width, height) / (fx + fy) + scale = 0.5 * image_extent / world_extent + rot_cw = np.asarray(image.cam_from_world().rotation.matrix(), dtype=np.float64) + pixel_corners = ((0.0, 0.0), (width, 0.0), (width, height), (0.0, height)) + corners = [] + for u, v in pixel_corners: + ray = np.array([(u - cx) / fx, (v - cy) / fy, 1.0], dtype=np.float64) + corners.append(center + 0.5 * scale * (rot_cw.T @ ray)) + segments: list[float] = [] + for corner in corners: + segments.extend(round(float(x), 5) for x in center) + segments.extend(round(float(x), 5) for x in corner) + for idx in range(4): + segments.extend(round(float(x), 5) for x in corners[idx]) + segments.extend(round(float(x), 5) for x in corners[(idx + 1) % 4]) + return [round(float(x), 5) for x in center], segments + + +def _compute_sim3_est_from_gt( + est_rec, + gt_rec, +): + import pycolmap + + from vidmap.benchmark.trajectory import align_umeyama_sim3 + + if est_rec.num_reg_images() < 2 or gt_rec.num_reg_images() < 2: + return None + + est_posed = sorted( + (img for img in est_rec.images.values() if img.has_pose), + key=lambda img: str(img.name), + ) + est_by_name = {str(img.name): img for img in est_posed} + min_est_name = str(est_posed[0].name) + max_est_name = str(est_posed[-1].name) + + gt_all_posed = sorted( + (img for img in gt_rec.images.values() if img.has_pose), + key=lambda img: str(img.name), + ) + gt_in_span = [img for img in gt_all_posed if min_est_name <= str(img.name) <= max_est_name] + if len(gt_in_span) < 2: + gt_in_span = gt_all_posed + gt_by_name = {str(img.name): img for img in gt_in_span} + + common_names = sorted(set(est_by_name) & set(gt_by_name)) + if len(common_names) >= 2: + p_es = np.array([est_by_name[n].projection_center() for n in common_names], dtype=np.float64) + p_gt = np.array([gt_by_name[n].projection_center() for n in common_names], dtype=np.float64) + else: + from vidmap.utils.trajectory import remap_poses_to_timeline + + mapped_est = remap_poses_to_timeline(gt_rec, est_rec).reconstruction + mapped_names = sorted( + str(img.name) for img in mapped_est.images.values() if img.has_pose and str(img.name) in gt_by_name + ) + if len(mapped_names) < 2: + return None + mapped_by_name = {str(img.name): img for img in mapped_est.images.values() if img.has_pose} + common_names = mapped_names + p_es = np.array([mapped_by_name[n].projection_center() for n in common_names], dtype=np.float64) + p_gt = np.array([gt_by_name[n].projection_center() for n in common_names], dtype=np.float64) + + if len(common_names) >= 3: + s_u, rot_u, trans_u = align_umeyama_sim3(p_es, p_gt) + if not np.isfinite(s_u) or s_u <= 1e-6: + return None + sim3_gt_from_est = pycolmap.Sim3d( + scale=float(s_u), + rotation=pycolmap.Rotation3d(rot_u), + translation=np.asarray(trans_u, dtype=np.float64), + ) + + def _med_err(sim3: pycolmap.Sim3d) -> float: + rot_m = np.asarray(sim3.rotation.matrix(), dtype=np.float64) + t_v = np.asarray(sim3.translation, dtype=np.float64) + aligned = float(sim3.scale) * (p_es @ rot_m.T) + t_v + return float(np.median(np.linalg.norm(p_gt - aligned, axis=1))) + + est_tmp = pycolmap.Reconstruction() + gt_tmp = pycolmap.Reconstruction() + for idx_n, n in enumerate(common_names, start=1): + e_im = est_by_name.get(n) + g_im = gt_by_name[n] + if e_im is None: + continue + if e_im.camera_id not in est_tmp.cameras: + est_tmp.add_camera_with_trivial_rig(est_rec.cameras[e_im.camera_id]) + if g_im.camera_id not in gt_tmp.cameras: + gt_tmp.add_camera_with_trivial_rig(gt_rec.cameras[g_im.camera_id]) + est_tmp.add_image_with_trivial_frame( + pycolmap.Image(image_id=idx_n, camera_id=e_im.camera_id, name=n), e_im.cam_from_world() + ) + gt_tmp.add_image_with_trivial_frame( + pycolmap.Image(image_id=idx_n, camera_id=g_im.camera_id, name=n), g_im.cam_from_world() + ) + + best_med = _med_err(sim3_gt_from_est) + if est_tmp.num_reg_images() >= 3: + for max_error in (0.1, 0.25, 0.5, 1.0, 2.5, 5.0, 10.0): + cand = pycolmap.align_reconstructions_via_proj_centers( + est_tmp, + gt_tmp, + max_proj_center_error=max_error, + ) + if cand is not None and np.isfinite(cand.scale) and cand.scale > 1e-6: + cand_med = _med_err(cand) + if cand_med < best_med: + best_med = cand_med + sim3_gt_from_est = cand + else: + dist_es = float(np.linalg.norm(p_es[1] - p_es[0])) + dist_gt = float(np.linalg.norm(p_gt[1] - p_gt[0])) + s_2 = dist_gt / max(1e-6, dist_es) if dist_es > 1e-6 else 1.0 + m_sum = np.zeros((3, 3), dtype=np.float64) + for n in common_names: + r_es = np.asarray(est_by_name[n].cam_from_world().rotation.matrix(), dtype=np.float64) + r_gt = np.asarray(gt_by_name[n].cam_from_world().rotation.matrix(), dtype=np.float64) + m_sum += r_gt.T @ r_es + u_m, _, vt_m = np.linalg.svd(m_sum) + s_diag = np.eye(3) + if np.linalg.det(u_m) * np.linalg.det(vt_m) < 0: + s_diag[2, 2] = -1.0 + rot_2 = u_m @ s_diag @ vt_m + trans_2 = p_gt.mean(axis=0) - s_2 * (rot_2 @ p_es.mean(axis=0)) + sim3_gt_from_est = pycolmap.Sim3d( + scale=float(s_2), + rotation=pycolmap.Rotation3d(rot_2), + translation=np.asarray(trans_2, dtype=np.float64), + ) + + return sim3_gt_from_est.inverse() + + +def _covariance_ellipsoid_segments( + center: np.ndarray, + cov_3x3: np.ndarray, + num_ring_segments: int = 24, +) -> list[float]: + eigvals, eigvecs = np.linalg.eigh(cov_3x3) + radii = np.sqrt(np.maximum(eigvals, 1e-12)) + angles = np.linspace(0.0, 2.0 * np.pi, num_ring_segments + 1, dtype=np.float64) + cos_a = np.cos(angles) + sin_a = np.sin(angles) + segments: list[float] = [] + for a, b in ((0, 1), (1, 2), (2, 0)): + va = eigvecs[:, a] * radii[a] + vb = eigvecs[:, b] * radii[b] + ring_pts = center[None, :] + cos_a[:, None] * va[None, :] + sin_a[:, None] * vb[None, :] + for s in range(num_ring_segments): + segments.extend(round(float(x), 5) for x in ring_pts[s]) + segments.extend(round(float(x), 5) for x in ring_pts[s + 1]) + return segments + + +def _trajectory_payload_from_sim3( + est_rec, + gt_reconstruction_dir: Path, + sim3_est_from_gt, + *, + frusta_at_est_keyframes_only: bool = False, +) -> dict[str, object] | None: + import bisect + import json + + import pycolmap + + if est_rec.num_reg_images() < 2 or sim3_est_from_gt is None: + return None + + est_posed = sorted( + (img for img in est_rec.images.values() if img.has_pose), + key=lambda img: str(img.name), + ) + est_by_name = {str(img.name): img for img in est_posed} + min_est_name = str(est_posed[0].name) + max_est_name = str(est_posed[-1].name) + + gt_in_est = pycolmap.Reconstruction(gt_reconstruction_dir) + if gt_in_est.num_reg_images() < 2: + return None + gt_in_est.transform(sim3_est_from_gt) + + gt_all_posed = sorted( + (img for img in gt_in_est.images.values() if img.has_pose), + key=lambda img: str(img.name), + ) + gt_in_span = [img for img in gt_all_posed if min_est_name <= str(img.name) <= max_est_name] + gt_posed = gt_in_span if len(gt_in_span) >= 2 else gt_all_posed + + gt_names = [str(img.name) for img in gt_posed] + centers: list[float] = [] + for img in gt_posed: + centers.extend(round(float(x), 5) for x in img.projection_center()) + + if frusta_at_est_keyframes_only and len(gt_posed) > len(est_posed): + frusta_imgs = [img for img in gt_posed if str(img.name) in est_by_name] + if len(frusta_imgs) < 2: + step = max(1, len(gt_posed) // max(1, len(est_posed))) + frusta_imgs = gt_posed[::step] + else: + frusta_imgs = gt_posed + + # Optional per-image metadata of the reference poses, next to the reference reconstruction: + # pose_covariances.npz: image_names (N,), cov_position (N, 3, 3) camera-center covariance in world units, + # confidence (N,), num_inliers (N,), reproj_rms_px (N,) + # pose_confidence.json: {image_name: confidence} (used if pose_covariances.npz is absent) + cov_npz_path = gt_reconstruction_dir / "pose_covariances.npz" + conf_json_path = gt_reconstruction_dir / "pose_confidence.json" + cov_info_by_name: dict[str, tuple[np.ndarray, float, int, float]] = {} + conf_only_by_name: dict[str, float] = {} + if cov_npz_path.is_file(): + cov_data = np.load(cov_npz_path) + c_names = [str(x) for x in cov_data["image_names"]] + c_pos = np.asarray(cov_data["cov_position"], dtype=np.float64) + c_conf = np.asarray(cov_data["confidence"], dtype=np.float64) + c_inl = np.asarray(cov_data["num_inliers"], dtype=np.int32) + c_rms = np.asarray(cov_data["reproj_rms_px"], dtype=np.float64) + for i, n in enumerate(c_names): + cov_info_by_name[n] = (c_pos[i], float(c_conf[i]), int(c_inl[i]), float(c_rms[i])) + elif conf_json_path.is_file(): + conf_only_by_name = { + str(k): float(v) for k, v in json.loads(conf_json_path.read_text(encoding="utf-8")).items() + } + + scale_est_from_gt = float(sim3_est_from_gt.scale) + rot_est_from_gt = np.asarray(sim3_est_from_gt.rotation.matrix(), dtype=np.float64) + + frusta_names = [str(img.name) for img in frusta_imgs] + keyframe_centers: list[float] = [] + keyframe_frusta: list[float] = [] + keyframe_cov_ellipses: list[float] = [] + keyframe_confidences: list[float] = [] + keyframe_inliers: list[int] = [] + keyframe_pos_std_meters: list[list[float]] = [] + keyframe_reproj_rms_px: list[float] = [] + + has_cov = bool(cov_info_by_name) + has_conf = bool(cov_info_by_name or conf_only_by_name) + + for img in frusta_imgs: + name = str(img.name) + cam = gt_in_est.cameras[img.camera_id] + kf_center, kf_frustum = _camera_frustum_segments(img, cam) + keyframe_centers.extend(kf_center) + keyframe_frusta.extend(kf_frustum) + if has_cov and name in cov_info_by_name: + cov_world, conf_val, inl_val, rms_val = cov_info_by_name[name] + cov_viewer = (scale_est_from_gt**2) * (rot_est_from_gt @ cov_world @ rot_est_from_gt.T) + center_vec = np.asarray(img.projection_center(), dtype=np.float64) + keyframe_cov_ellipses.extend(_covariance_ellipsoid_segments(center_vec, cov_viewer)) + keyframe_confidences.append(round(conf_val, 5)) + keyframe_inliers.append(inl_val) + evals_m = np.maximum(np.linalg.eigvalsh(cov_world), 0.0) + axes_m = np.sqrt(evals_m)[::-1] + std_tot_m = float(np.sqrt(np.sum(evals_m))) + keyframe_pos_std_meters.append( + [ + round(std_tot_m, 4), + round(float(axes_m[0]), 4), + round(float(axes_m[1]), 4), + round(float(axes_m[2]), 4), + ] + ) + keyframe_reproj_rms_px.append(round(rms_val, 3) if np.isfinite(rms_val) else 0.0) + elif has_cov: + center_vec = np.asarray(img.projection_center(), dtype=np.float64) + keyframe_cov_ellipses.extend(_covariance_ellipsoid_segments(center_vec, np.eye(3) * 1e-6)) + keyframe_confidences.append(round(conf_only_by_name.get(name, 1.0), 5)) + keyframe_inliers.append(0) + keyframe_pos_std_meters.append([0.0, 0.0, 0.0, 0.0]) + keyframe_reproj_rms_px.append(0.0) + elif has_conf: + keyframe_confidences.append(round(conf_only_by_name.get(name, 1.0), 5)) + + keyframe_path_counts: list[int] = [] + keyframe_frusta_counts: list[int] = [] + for idx, est_img in enumerate(est_posed): + name = str(est_img.name) + if idx == len(est_posed) - 1: + keyframe_path_counts.append(len(gt_names)) + keyframe_frusta_counts.append(len(frusta_names)) + else: + keyframe_path_counts.append(bisect.bisect_right(gt_names, name)) + keyframe_frusta_counts.append(bisect.bisect_right(frusta_names, name)) + + result: dict[str, object] = { + "centers": centers, + "keyframePathCounts": keyframe_path_counts, + "keyframeFrustaCounts": keyframe_frusta_counts, + "keyframeCenters": keyframe_centers, + "keyframeFrusta": keyframe_frusta, + "keyframeNames": frusta_names, + } + if has_conf: + result["keyframeConfidences"] = keyframe_confidences + if has_cov: + result["keyframeCovEllipses"] = keyframe_cov_ellipses + result["keyframeInliers"] = keyframe_inliers + result["keyframePosStdMeters"] = keyframe_pos_std_meters + result["keyframeReprojRmsPx"] = keyframe_reproj_rms_px + return result + + def _embedded_run_payload( run_dir: str | Path, *, reconstruction_dir: str | Path | None = None, images_dir: str | Path | None = None, + gt_reconstruction_dir: str | Path | None = None, + dense_gt_reconstruction_dir: str | Path | None = None, ) -> dict[str, object]: """Package one normalized run for the browser's ordinary load path.""" run = Path(run_dir).expanduser().resolve(strict=True) @@ -203,7 +519,7 @@ def _embedded_run_payload( covariance = reconstruction / "visualization_cache" / "point_covariance_rank_v2.bin" if covariance.is_file(): files["rec/visualization_cache/point_covariance_rank_v2.bin"] = _encoded_file(covariance) - return { + payload: dict[str, object] = { "files": files, "imagePreviews": _image_previews( run, @@ -211,6 +527,46 @@ def _embedded_run_payload( reconstruction_dir=reconstruction, ), } + gt_dir = ( + Path(gt_reconstruction_dir).expanduser().resolve(strict=True) + if gt_reconstruction_dir is not None + else (run / "gt_rec" if (run / "gt_rec" / "images.bin").is_file() else None) + ) + dense_gt_dir = ( + Path(dense_gt_reconstruction_dir).expanduser().resolve(strict=True) + if dense_gt_reconstruction_dir is not None + else (run / "dense_gt_rec" if (run / "dense_gt_rec" / "images.bin").is_file() else None) + ) + if gt_dir is not None or dense_gt_dir is not None: + import pycolmap + + est_rec = pycolmap.Reconstruction(reconstruction) + sim3_est_from_gt = None + if gt_dir is not None: + gt_rec = pycolmap.Reconstruction(gt_dir) + sim3_est_from_gt = _compute_sim3_est_from_gt(est_rec, gt_rec) + gt_trajectory = _trajectory_payload_from_sim3( + est_rec, + gt_dir, + sim3_est_from_gt, + frusta_at_est_keyframes_only=(dense_gt_dir is None), + ) + if gt_trajectory is not None: + payload["gtTrajectory"] = gt_trajectory + if dense_gt_dir is not None: + dense_sim3 = sim3_est_from_gt + if dense_sim3 is None: + dense_rec = pycolmap.Reconstruction(dense_gt_dir) + dense_sim3 = _compute_sim3_est_from_gt(est_rec, dense_rec) + dense_trajectory = _trajectory_payload_from_sim3( + est_rec, + dense_gt_dir, + dense_sim3, + frusta_at_est_keyframes_only=True, + ) + if dense_trajectory is not None: + payload["denseGtTrajectory"] = dense_trajectory + return payload def write_embedded_viewer_html( @@ -219,12 +575,16 @@ def write_embedded_viewer_html( *, reconstruction_dir: str | Path | None = None, images_dir: str | Path | None = None, + gt_reconstruction_dir: str | Path | None = None, + dense_gt_reconstruction_dir: str | Path | None = None, ) -> Path: """Write an embedded viewer that automatically loads one run or sub-reconstruction.""" payload = _embedded_run_payload( run_dir, reconstruction_dir=reconstruction_dir, images_dir=images_dir, + gt_reconstruction_dir=gt_reconstruction_dir, + dense_gt_reconstruction_dir=dense_gt_reconstruction_dir, ) return write_html(output, scene.render_viewer_html(embedded_run=payload)) @@ -235,6 +595,8 @@ def write_all_embedded_viewers( *, images_dir: str | Path | None = None, base_name: str | None = None, + gt_reconstruction_dir: str | Path | None = None, + dense_gt_reconstruction_dir: str | Path | None = None, ) -> list[Path]: """Write embedded viewer HTMLs for all reconstructions or sub-reconstructions in a run.""" run = Path(run_dir).expanduser().resolve(strict=True) @@ -259,6 +621,8 @@ def write_all_embedded_viewers( out_file, reconstruction_dir=sub_dir, images_dir=images_dir, + gt_reconstruction_dir=gt_reconstruction_dir, + dense_gt_reconstruction_dir=dense_gt_reconstruction_dir, ) written.append(out_file) else: @@ -269,6 +633,8 @@ def write_all_embedded_viewers( out_file, reconstruction_dir=rec_dir, images_dir=images_dir, + gt_reconstruction_dir=gt_reconstruction_dir, + dense_gt_reconstruction_dir=dense_gt_reconstruction_dir, ) written.append(out_file)