colmap · Zador-Pataki · Apr 17, 2025 · Apr 23, 2025 · Apr 23, 2025 · Apr 23, 2025
diff --git a/src/colmap/estimators/bundle_adjustment.cc b/src/colmap/estimators/bundle_adjustment.cc
@@ -263,6 +263,9 @@ ceres::LossFunction* BundleAdjustmentOptions::CreateLossFunction() const {
       break;
   }
   THROW_CHECK_NOTNULL(loss_function);
+  if (loss_function_magnitude != 1.0) {
+    loss_function = new ceres::ScaledLoss(loss_function, loss_function_magnitude, ceres::TAKE_OWNERSHIP);
+  }
   return loss_function;
 }
 
@@ -1175,4 +1178,55 @@ void PrintSolverSummary(const ceres::Solver::Summary& summary,
   LOG(INFO) << log.str();
 }
 
+void DepthPriorBundleAdjuster(ceres::Problem* problem,
+    image_t image_id,
+    const std::vector<point3D_t>& point3D_ids,
+    const std::vector<double>& depths,
+    const std::vector<double>& loss_magnitudes,
+    const std::vector<double>& loss_params,
+    BundleAdjustmentOptions::LossFunctionType loss_type,
+    double* shift_scale_ptr,  
+    Reconstruction& reconstruction,
+    bool logloss,
+    bool fix_shift,
+    bool fix_scale) {
+  if (point3D_ids.size() != depths.size() ||
+      point3D_ids.size() != loss_magnitudes.size() ||
+      point3D_ids.size() != loss_params.size()) {
+    throw std::runtime_error("Input vectors must have the same size");
+  }
+
+  Image& image = reconstruction.Image(image_id);
+
+  double* cam_rot = image.CamFromWorld().rotation.coeffs().data();
+  double* cam_trans = image.CamFromWorld().translation.data();
+
+  for (size_t i = 0; i < point3D_ids.size(); ++i) {
+    point3D_t pt_id = point3D_ids[i];
+    double depth = depths[i];
+    double loss_magnitude = loss_magnitudes[i];
+    double loss_param = loss_params[i];
+
+    Point3D& point3D = reconstruction.Point3D(pt_id);
+
+    ceres::CostFunction* cost_function = logloss
+      ? LogScaledDepthErrorCostFunction::Create(depth)
+      : ScaledDepthErrorCostFunction::Create(depth);
+
+    BundleAdjustmentOptions loss_opts;
+    loss_opts.loss_function_type = loss_type;
+    loss_opts.loss_function_scale = loss_param;
+    loss_opts.loss_function_magnitude = loss_magnitude;
+
+    ceres::LossFunction* loss_function = loss_opts.CreateLossFunction();
+
+    problem->AddResidualBlock(cost_function,
+        loss_function,
+        cam_rot,
+        cam_trans,
+        point3D.xyz.data(),
+        shift_scale_ptr);
+  }
+}
+
 }  // namespace colmap
diff --git a/src/colmap/estimators/bundle_adjustment.h b/src/colmap/estimators/bundle_adjustment.h
@@ -117,6 +117,9 @@ struct BundleAdjustmentOptions {
 
   // Scaling factor determines residual at which robustification takes place.
   double loss_function_scale = 1.0;
+
+  // Magnitude determines the scaling of the residual
+  double loss_function_magnitude = 1.0;
 
   // Whether to refine the focal length parameter group.
   bool refine_focal_length = true;
@@ -245,6 +248,20 @@ std::unique_ptr<BundleAdjuster> CreatePosePriorBundleAdjuster(
     std::unordered_map<image_t, PosePrior> pose_priors,
     Reconstruction& reconstruction);
 
+// Adds a depth prior to the bundle adjustment problem.
+void DepthPriorBundleAdjuster(ceres::Problem* problem,
+                              image_t image_id,
+                              const std::vector<point3D_t>& point3D_ids,
+                              const std::vector<double>& depths,
+                              const std::vector<double>& loss_magnitudes,
+                              const std::vector<double>& loss_params,
+                              BundleAdjustmentOptions::LossFunctionType loss_type,
+                              double* shift_scale_ptr, 
+                              Reconstruction& reconstruction,
+                              bool logloss = false,
+                              bool fix_shift = false,
+                              bool fix_scale = false);
+
 void PrintSolverSummary(const ceres::Solver::Summary& summary,
                         const std::string& header);
 

diff --git a/src/colmap/estimators/cost_functions.h b/src/colmap/estimators/cost_functions.h
@@ -530,6 +530,104 @@ class CovarianceWeightedCostFunctor {
   const CostFunctor cost_;
 };
 
+// Cost function for constraining the depth of a point in the camera frame 
+// with depth priors.
+struct ScaledDepthErrorCostFunction {
+  public:
+   ScaledDepthErrorCostFunction(const double depth) : depth_(depth) {}
+   static ceres::CostFunction* Create(const double depth) {
+     return (
+         new ceres::
+             AutoDiffCostFunction<ScaledDepthErrorCostFunction, 1, 4, 3, 3, 2>(
+                 new ScaledDepthErrorCostFunction(depth)));
+   }
+   template <typename T>
+   bool operator()(const T* const cam_from_world_rotation,
+                   const T* const cam_from_world_translation,
+                   const T* const point3D,
+                   const T* const shift_scale,
+                   T* residuals) const {
+     *residuals = (EigenQuaternionMap<T>(cam_from_world_rotation) *
+                   EigenVector3Map<T>(point3D))[2] +
+                  cam_from_world_translation[2] - shift_scale[0] -
+                  T(depth_) * exp(shift_scale[1]);
+     return true;
+   }
+  private:
+   const double depth_;
+ };
+
+// Cost function for constraining the depth of a point in the camera frame 
+// with depth priors, with a fixed camera pose.
+ struct ScaledDepthErrorConstantPoseCostFunction
+     : public ScaledDepthErrorCostFunction {
+   using Parent = ScaledDepthErrorCostFunction;
+  public:
+   ScaledDepthErrorConstantPoseCostFunction(const Rigid3d& cam_from_world,
+                                            const double depth)
+       : Parent(depth), cam_from_world_(cam_from_world) {}
+   static ceres::CostFunction* Create(const Rigid3d& cam_from_world,
+                                      const double depth) {
+     return (new ceres::AutoDiffCostFunction<
+             ScaledDepthErrorConstantPoseCostFunction,
+             1,
+             3,
+             2>(
+         new ScaledDepthErrorConstantPoseCostFunction(cam_from_world, depth)));
+   }
+   template <typename T>
+   bool operator()(const T* const point3D,
+                   const T* const shift_scale,
+                   T* residuals) const {
+     const Eigen::Quaternion<T> cam_from_world_rotation =
+         cam_from_world_.rotation.cast<T>();
+     const Eigen::Matrix<T, 3, 1> cam_from_world_translation =
+         cam_from_world_.translation.cast<T>();
+     return Parent::operator()(cam_from_world_rotation.coeffs().data(),
+                               cam_from_world_translation.data(),
+                               point3D,
+                               shift_scale,
+                               residuals);
+   }
+  private:
+   const Rigid3d& cam_from_world_;
+ };
+
+ // Cost function for constraining the depth of a point in the camera frame
+ // with depth priors, with a fixed camera pose, in log space.
+ struct LogScaledDepthErrorCostFunction {
+  public:
+   LogScaledDepthErrorCostFunction(const double depth) : depth_(depth) {}
+
+   static ceres::CostFunction* Create(const double depth) {
+     return new ceres::AutoDiffCostFunction<LogScaledDepthErrorCostFunction, 1, 4, 3, 3, 2>(
+         new LogScaledDepthErrorCostFunction(depth));
+   }
+
+   template <typename T>
+   bool operator()(const T* const cam_from_world_rotation,
+                   const T* const cam_from_world_translation,
+                   const T* const point3D,
+                   const T* const shift_scale,
+                   T* residuals) const {
+     // Compute the predicted depth in the camera frame.
+     T d_pred = (EigenQuaternionMap<T>(cam_from_world_rotation) *
+                 EigenVector3Map<T>(point3D))[2] +
+                cam_from_world_translation[2];
+
+     if (d_pred <= T(0)) {
+       *residuals = T(0);
+       return true;
+     }
+
+     *residuals = ceres::log(d_pred) - (ceres::log(T(depth_)) + shift_scale[1]);
+     return true;
+   }
+
+  private:
+   const double depth_;
+ };
+
 template <template <typename> class CostFunctor, typename... Args>
 ceres::CostFunction* CreateCameraCostFunction(
     const CameraModelId camera_model_id, Args&&... args) {

diff --git a/src/colmap/estimators/cost_functions_test.cc b/src/colmap/estimators/cost_functions_test.cc
@@ -457,5 +457,44 @@ TEST(CovarianceWeightedCostFunctor, AbsolutePosePositionPriorCostFunctor) {
       residuals[2], -0.5 * std::sqrt(2) * world_from_cam.translation[2], 1e-6);
 }
 
+TEST(ScaledDepthError, Nominal) {
+  const double depth_prior = 2.0;
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      ScaledDepthErrorCostFunction::Create(depth_prior));
+  double cam_from_world_rotation[4] = {0, 0, 0, 1};
+  double cam_from_world_translation[3] = {0, 0, 0};
+  double point3D[3] = {0, 0, 3};
+  double shift_scale[2] = {0, 0};
+  double residuals[1];
+  const double* parameters[4] = {cam_from_world_rotation,
+                                 cam_from_world_translation,
+                                 point3D,
+                                 shift_scale};
+  EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], 1);
+  point3D[2] = 2;
+  EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], 0);
+  cam_from_world_translation[2] = 2;
+  EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], 2);
+  cam_from_world_translation[0] = 1;
+  cam_from_world_translation[1] = 3;
+  EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], 2);
+  shift_scale[1] = std::log(2);
+  EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], 0);
+  shift_scale[0] = 1;
+  EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], -1);
+  std::unique_ptr<ceres::CostFunction> cost_function_with_noise(
+      IsotropicNoiseCostFunctionWrapper<ScaledDepthErrorCostFunction>::Create(
+          2.0, depth_prior));
+  EXPECT_TRUE(
+      cost_function_with_noise->Evaluate(parameters, residuals, nullptr));
+  EXPECT_EQ(residuals[0], -0.5);
+}
+
 }  // namespace
 }  // namespace colmap
diff --git a/src/colmap/estimators/covariance.cc b/src/colmap/estimators/covariance.cc
@@ -416,6 +416,9 @@ std::vector<PoseParam> GetPoseParams(const Reconstruction& reconstruction,
   std::vector<PoseParam> params;
   params.reserve(reconstruction.NumImages());
   for (const auto& [image_id, image] : reconstruction.Images()) {
+    if (!image.HasPose()) {
+      continue;
+    }
     const double* qvec = image.CamFromWorld().rotation.coeffs().data();
     if (!problem.HasParameterBlock(qvec) ||
         problem.IsParameterBlockConstant(const_cast<double*>(qvec))) {

diff --git a/src/colmap/scene/image.cc b/src/colmap/scene/image.cc
@@ -118,6 +118,31 @@ void Image::ResetPoint3DForPoint2D(const point2D_t point2D_idx) {
   }
 }
 
+std::vector<point3D_t> Image::Point3DIds(const std::optional<std::vector<point2D_t>>& point2D_idxs) const {
+  std::vector<point3D_t> point3D_ids;
+  if (!point2D_idxs.has_value()) {
+    point3D_ids.resize(NumPoints2D());
+    for (point2D_t idx = 0; idx < NumPoints2D(); ++idx) {
+      point3D_ids[idx] = Point2D(idx).point3D_id;
+    }
+  } else {
+    point3D_ids.resize(point2D_idxs->size());
+    for (size_t i = 0; i < point2D_idxs->size(); ++i) {
+      point3D_ids[i] = Point2D((*point2D_idxs)[i]).point3D_id;
+    }
+  }
+
+  return point3D_ids;
+}
+
+Eigen::MatrixXd Image::KeypointCoordinates(const std::vector<point2D_t>& point2D_idxs) const {
+  Eigen::MatrixXd coords(point2D_idxs.size(), 2);
+  for (size_t i = 0; i < point2D_idxs.size(); ++i) {
+    coords.row(i) = Point2D(point2D_idxs[i]).xy;
+  }
+  return coords;
+}
+
 bool Image::HasPoint3D(const point3D_t point3D_id) const {
   return std::find_if(points2D_.begin(),
                       points2D_.end(),

diff --git a/src/colmap/scene/image.h b/src/colmap/scene/image.h
@@ -125,6 +125,14 @@ class Image {
   void SetPoints2D(const std::vector<Eigen::Vector2d>& points);
   void SetPoints2D(const std::vector<struct Point2D>& points);
 
+  std::vector<point3D_t> Point3DIds(
+    const std::optional<std::vector<point2D_t>>& point2D_idxs = std::nullopt
+  ) const;
+
+  Eigen::MatrixXd KeypointCoordinates(
+    const std::vector<point2D_t>& point2D_idxs
+  ) const;
+
   // Set the point as triangulated, i.e. it is part of a 3D point track.
   void SetPoint3DForPoint2D(point2D_t point2D_idx, point3D_t point3D_id);
 

diff --git a/src/colmap/scene/image_test.cc b/src/colmap/scene/image_test.cc
@@ -164,6 +164,27 @@ TEST(Image, NumPoints3D) {
   EXPECT_EQ(image.NumPoints3D(), 2);
 }
 
+TEST(Image, Point3DIds) {
+  Image image;
+  image.SetPoints2D(std::vector<Eigen::Vector2d>(3));
+  EXPECT_EQ(image.Point3DIds().size(), 3);
+  EXPECT_EQ(image.Point3DIds()[0], kInvalidPoint3DId);
+  EXPECT_EQ(image.Point3DIds()[1], kInvalidPoint3DId);
+  EXPECT_EQ(image.Point3DIds()[2], kInvalidPoint3DId);
+
+  image.SetPoint3DForPoint2D(0, 42);
+  image.SetPoint3DForPoint2D(1, 43);
+  auto ids_all = image.Point3DIds();
+  EXPECT_EQ(ids_all[0], 42);
+  EXPECT_EQ(ids_all[1], 43);
+  EXPECT_EQ(ids_all[2], kInvalidPoint3DId);
+
+  auto ids_subset = image.Point3DIds(std::vector<point2D_t>{1, 2});
+  EXPECT_EQ(ids_subset.size(), 2);
+  EXPECT_EQ(ids_subset[0], 43);
+  EXPECT_EQ(ids_subset[1], kInvalidPoint3DId);
+}
+
 TEST(Image, Points2D) {
   Image image;
   EXPECT_EQ(image.Points2D().size(), 0);
@@ -189,6 +210,23 @@ TEST(Image, Points2DWith3D) {
   EXPECT_EQ(image.NumPoints3D(), 1);
 }
 
+TEST(Image, KeypointCoordinates) {
+  Image image;
+  std::vector<Point2D> points2D(3);
+  points2D[0].xy = Eigen::Vector2d(1.0, 2.0);
+  points2D[1].xy = Eigen::Vector2d(3.0, 4.0);
+  points2D[2].xy = Eigen::Vector2d(5.0, 6.0);
+  image.SetPoints2D(points2D);
+
+  Eigen::MatrixXd coords = image.KeypointCoordinates({0, 2});
+  ASSERT_EQ(coords.rows(), 2);
+  ASSERT_EQ(coords.cols(), 2);
+  EXPECT_EQ(coords(0, 0), 1.0);
+  EXPECT_EQ(coords(0, 1), 2.0);
+  EXPECT_EQ(coords(1, 0), 5.0);
+  EXPECT_EQ(coords(1, 1), 6.0);
+}
+
 TEST(Image, Points3D) {
   Image image;
   image.SetPoints2D(std::vector<Eigen::Vector2d>(2));

diff --git a/src/colmap/scene/reconstruction.cc b/src/colmap/scene/reconstruction.cc
@@ -271,6 +271,17 @@ void Reconstruction::DeleteAllPoints2DAndPoints3D() {
   }
 }
 
+Eigen::MatrixXd Reconstruction::Point3DCoordinates(
+    const std::vector<point3D_t>& point3D_ids) const {
+  Eigen::MatrixXd coords(point3D_ids.size(), 3);
+  for (size_t i = 0; i < point3D_ids.size(); ++i) {
+    auto it = points3D_.find(point3D_ids[i]);
+    THROW_CHECK(it != points3D_.end());
+    coords.row(i) = it->second.xyz;
+  }
+  return coords;
+}
+
 void Reconstruction::RegisterImage(const image_t image_id) {
   THROW_CHECK(Image(image_id).HasPose());
   reg_image_ids_.insert(image_id);

diff --git a/src/colmap/scene/reconstruction.h b/src/colmap/scene/reconstruction.h
@@ -139,6 +139,9 @@ class Reconstruction {
   // Delete all 2D points of all images and all 3D points.
   void DeleteAllPoints2DAndPoints3D();
 
+  // Return the 3D coordinates of the given point IDs.
+  Eigen::MatrixXd Point3DCoordinates(const std::vector<point3D_t>& point3D_ids) const;
+
   // Register an existing image.
   void RegisterImage(image_t image_id);