Absolute location priors in rotation averaging, global positioning, and BA

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)

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 <stdexcept>
@@ -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<double, Eigen::Dynamic, 2, Eigen::RowMajor>&
+             points2D,
+         const Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>&
+             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<ceres::LossFunction> 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<ceres::LossFunction>;
+        return py::make_tuple(
+            py::capsule(new Loss(std::move(loss)),
+                        [](void* ptr) { delete static_cast<Loss*>(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.");
 }

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<double, py::array::c_style> center,
+         const Eigen::Matrix3d& cam_from_world_rotation,
+         const Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>&
+             points,
+         const Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>&
+             bearings,
+         const Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>&
+             stddevs,
+         const std::shared_ptr<ceres::LossFunction>& 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<Eigen::Vector3d> points;
+          std::vector<double> scales;
+          std::shared_ptr<ceres::LossFunction> loss;
+        };
+        auto storage = std::make_unique<Storage>();
+        storage->points.reserve(count);
+        storage->scales.reserve(count);
+        storage->loss = loss;
+        double* center_data = center.mutable_data();
+        const Eigen::Map<const Eigen::Vector3d> 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<Storage*>(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.");
 }