Use upstream depth costs with log-scale optimization

extensions/colmap/CMakeLists.txt

@@ -15,9 +15,11 @@ find_package(Python REQUIRED COMPONENTS Interpreter Development.Module)
 find_package(pybind11 3.1.0 CONFIG REQUIRED)
 
 set(VIDMAP_COLMAP_VERSION "4.3.0")
+# Pending dependency: unpublished COLMAP depth implementation.
+# Public builds are blocked until it is published and this pin is updated.
 set(
   VIDMAP_COLMAP_REVISION
-  "b1972bc226f58e095c8b75bc214f4bb2d8ba57e4"
+  "6cde3c7f078aa53aa2ecb39d900c0f89121819a2"
   CACHE STRING
   "Upstream COLMAP revision used by vidmap_native"
 )
@@ -72,7 +74,6 @@ set(
     src/bindings/mapping_sidecars.cc
     src/bindings/view_graph.cc
     src/bindings/tracks.cc
-    src/bindings/global_positioning.cc
 )
 
 pybind11_add_module(

extensions/colmap/src/bindings/bindings.h

@@ -8,6 +8,5 @@ void BindRecords(pybind11::module_& module);
 void BindMappingSidecars(pybind11::module_& module);
 void BindViewGraph(pybind11::module_& module);
 void BindTracks(pybind11::module_& module);
-void BindGlobalPositioning(pybind11::module_& module);
 
 }  // namespace vidmap

extensions/colmap/src/bindings/bundle_adjustment_problem.cc

@@ -1,79 +1,15 @@
-#include "colmap/estimators/ceres_loss_function.h"
-#include "colmap/scene/reconstruction.h"
+#include "colmap/scene/camera.h"
 
-#include <stdexcept>
+#include <memory>
+#include <vector>
 
-#include "stages/depth_prior.h"
 #include "stages/intrinsics_prior.h"
-#include "stages/rig_geometry.h"
-#include <pybind11/eigen.h>
-#include <pybind11/numpy.h>
 #include <pybind11/pybind11.h>
 
 namespace py = pybind11;
 
 PYBIND11_MODULE(bundle_adjustment, m) {
   py::module_::import("pyceres");
-  m.def("append_depth_observations",
-        [](ceres::Problem& problem,
-           colmap::Reconstruction& reconstruction,
-           colmap::image_t image_id,
-           const Eigen::Matrix<colmap::point3D_t, Eigen::Dynamic, 1>& point_ids,
-           const Eigen::VectorXd& depths,
-           const Eigen::VectorXd& robust_scales,
-           const Eigen::VectorXd& weights,
-           const Eigen::Matrix<bool, Eigen::Dynamic, 1>& robust_mask,
-           colmap::CeresLossFunctionType loss_type,
-           py::array_t<double, py::array::c_style> log_scale,
-           bool fix_pose) {
-          const auto count = point_ids.size();
-          if (depths.size() != count || robust_scales.size() != count ||
-              weights.size() != count || robust_mask.size() != count ||
-              log_scale.size() != 1 || !log_scale.writeable()) {
-            throw std::invalid_argument("invalid BA depth observation arrays");
-          }
-          if (!depths.allFinite() || (depths.array() <= 0).any() ||
-              !robust_scales.allFinite() ||
-              (robust_scales.array() <= 0).any() || !weights.allFinite() ||
-              (weights.array() < 0).any()) {
-            throw std::invalid_argument("invalid BA depth or loss values");
-          }
-          // The upstream BA problem owns costs, but not loss functions.
-          using Losses = std::vector<std::unique_ptr<ceres::LossFunction>>;
-          auto losses = std::make_unique<Losses>();
-          auto& image = reconstruction.Image(image_id);
-          const auto sensor_from_rig = vidmap::FixedSensorFromRig(image);
-          double* pose = image.FramePtr()->RigFromWorld().params.data();
-          if (fix_pose || problem.HasParameterBlock(pose)) {
-            losses->reserve(count);
-            for (Eigen::Index i = 0; i < count; ++i) {
-              if (weights[i] == 0.0) continue;
-              double* point = reconstruction.Point3D(point_ids[i]).xyz.data();
-              if (!problem.HasParameterBlock(point)) continue;
-              losses->push_back(colmap::CreateCeresLossFunction(
-                  robust_mask[i] ? loss_type
-                                 : colmap::CeresLossFunctionType::TRIVIAL,
-                  robust_scales[i],
-                  weights[i]));
-              problem.AddResidualBlock(
-                  vidmap::LogScaledDepthErrorCostFunctor::Create(
-                      depths[i], sensor_from_rig),
-                  losses->back().get(),
-                  pose,
-                  point,
-                  log_scale.mutable_data());
-            }
-            // Constant-pose reprojection costs omit this parameter block.
-            if (!losses->empty() && fix_pose) {
-              problem.SetParameterBlockConstant(pose);
-            }
-          }
-          const auto added = losses->size();
-          return py::make_tuple(
-              py::capsule(losses.release(),
-                          [](void* ptr) { delete static_cast<Losses*>(ptr); }),
-              added);
-        });
   m.def("focal_prior_cost",
         [](const colmap::Camera& camera, double focal, double stddev) {
           const auto indices = camera.FocalLengthIdxs();

extensions/colmap/src/bindings/global_positioning_problem.cc

@@ -1,3 +1,4 @@
+#include "colmap/estimators/cost_functions/depth.h"
 #include "colmap/estimators/cost_functions/motion_averaging.h"
 #include "colmap/estimators/cost_functions/utils.h"
 #include "colmap/estimators/global_positioning.h"
@@ -11,7 +12,6 @@
 #include <unordered_map>
 #include <unordered_set>
 
-#include "stages/metric_depth.h"
 #include "stages/rig_geometry.h"
 #include "stages/weighted_motion_averaging.h"
 #include "vidmap_native/mapping_sidecars.h"
@@ -466,10 +466,15 @@ PYBIND11_MODULE(global_positioning, m) {
       "Keep the returned scales, reconstruction, centers and loss alive "
       "while using the problem.");
   m.def("split_depth_observations",
-        [](const std::vector<ObservationSelection*>& selections,
+        [](colmap::GlobalPositioner& positioner,
+           const colmap::Reconstruction& reconstruction,
+           const std::vector<ObservationSelection*>& selections,
            const vidmap::MappingSidecars& sidecars,
            const std::map<colmap::image_t, vidmap::VectorXb>& outlier_masks) {
-          std::map<std::pair<bool, bool>, ObservationSelection> groups;
+          using Depths =
+              std::map<colmap::point2D_t, std::pair<double, double>>;
+          using Key = std::tuple<bool, bool, colmap::image_t>;
+          std::map<Key, std::pair<Depths, ObservationSelection>> groups;
           std::map<colmap::image_t, size_t> counts;
           for (const auto* selection : selections) {
             for (const auto& obs : selection->observations) {
@@ -482,43 +487,78 @@ PYBIND11_MODULE(global_positioning, m) {
               const bool outlier = mask == outlier_masks.end()
                                        ? MaskValue(data.is_depth_outlier, index)
                                        : MaskValue(mask->second, index);
-              groups[{obs.loop, outlier}].observations.push_back(obs);
+              auto& [depths, explicit_observations] =
+                  groups[{obs.loop, outlier, obs.ImageId()}];
+              const auto& image = reconstruction.Image(obs.ImageId());
+              if (!obs.loop &&
+                  image.Point2D(index).point3D_id == obs.point_id &&
+                  obs.center ==
+                      positioner.FrameCenterParameterBlock(image.FrameId())) {
+                THROW_CHECK(
+                    depths
+                        .emplace(index,
+                                 std::make_pair(data.depth_values(index),
+                                                data.depth_stddevs(index)))
+                        .second);
+              } else {
+                // LC associations may repeat a feature or use a different
+                // point. Warmup can also supply centers outside the positioner.
+                explicit_observations.observations.push_back(obs);
+              }
               ++counts[obs.ImageId()];
             }
           }
           return py::make_tuple(groups, counts);
         });
-  m.def("append_depth_observations",
-        [](ceres::Problem& problem,
-           const colmap::Reconstruction& reconstruction,
-           const vidmap::MappingSidecars& sidecars,
-           const ObservationSelection& selection,
-           const py::dict& scales,
-           const std::shared_ptr<ceres::LossFunction>& loss,
-           bool use_log_scales,
-           vidmap::MetricDepthResidualType residual_type,
-           bool zero_residual_behind,
-           double log_linear_threshold) {
-          const auto scale_pointers = CenterPointers(scales);
-          for (const auto& obs : selection.observations) {
-            const auto& data = sidecars.images.at(obs.ImageId());
-            auto* cost = vidmap::MetricDepthError::Create(
-                reconstruction.Image(obs.ImageId()).CamFromWorld().rotation(),
-                data.depth_values(obs.Index()),
-                data.depth_stddevs(obs.Index()),
-                use_log_scales,
-                residual_type,
-                zero_residual_behind,
-                log_linear_threshold,
-                vidmap::RigTranslationInWorld(
-                    reconstruction.Image(obs.ImageId())));
-            problem.AddResidualBlock(cost,
-                                     loss.get(),
-                                     obs.center,
-                                     obs.point,
-                                     scale_pointers.at(obs.ImageId()));
-          }
-        });
+  m.def(
+      "append_lc_depths",
+      [](ceres::Problem& problem,
+         const colmap::Reconstruction& reconstruction,
+         const vidmap::MappingSidecars& sidecars,
+         const ObservationSelection& selection,
+         py::array_t<double, py::array::c_style> log_depth_scale,
+         const std::shared_ptr<ceres::LossFunction>& loss,
+         const colmap::GPDepthConstraintOptions& options) {
+        THROW_CHECK_EQ(log_depth_scale.ndim(), 1);
+        THROW_CHECK_EQ(log_depth_scale.size(), 1);
+        THROW_CHECK(log_depth_scale.writeable());
+        THROW_CHECK(std::isfinite(*log_depth_scale.data()));
+        if (options.residual_type == colmap::DepthResidualType::kLogLinear) {
+          THROW_CHECK_GT(options.log_linear_threshold, 0);
+          THROW_CHECK_LT(options.log_linear_threshold, 1);
+        }
+        for (const auto& obs : selection.observations) {
+          const auto& image = reconstruction.Image(obs.ImageId());
+          const auto& data = sidecars.images.at(obs.ImageId());
+          const double depth = data.depth_values(obs.Index());
+          const double stddev = data.depth_stddevs(obs.Index());
+          THROW_CHECK(std::isfinite(depth) && depth > 0);
+          THROW_CHECK(std::isfinite(stddev) && stddev > 0);
+          THROW_CHECK(problem.HasParameterBlock(obs.point));
+          THROW_CHECK(problem.HasParameterBlock(obs.center));
+          const colmap::WorldFromCamCenterDepthCostFunctor depth_cost{
+              depth,
+              stddev,
+              options.residual_type,
+              options.log_linear_threshold,
+              image.CamFromWorld().rotation().toRotationMatrix().row(2)};
+          auto* cost = new ceres::AutoDiffCostFunction<
+              colmap::RigWorldFromCamCenterDepthConstantRigCostFunctor,
+              1,
+              3,
+              3,
+              1>(new colmap::RigWorldFromCamCenterDepthConstantRigCostFunctor{
+              depth_cost, vidmap::FixedSensorFromRig(image).translation().z()});
+          problem.AddResidualBlock(cost,
+                                   loss.get(),
+                                   obs.point,
+                                   obs.center,
+                                   log_depth_scale.mutable_data());
+        }
+        return selection.observations.size();
+      },
+      "Append explicit LC/warmup point and center blocks using COLMAP depth "
+      "costs. Keep the reconstruction, centers, log scale and loss alive.");
   m.def("playback_point_ids",
         [](ceres::Problem& problem,
            const colmap::Reconstruction& reconstruction,

extensions/colmap/src/bindings/module.cc

@@ -14,5 +14,4 @@ PYBIND11_MODULE(_core, m) {
   vidmap::BindMappingSidecars(m);
   vidmap::BindViewGraph(m);
   vidmap::BindTracks(m);
-  vidmap::BindGlobalPositioning(m);
 }