diff --git a/extensions/colmap/CMakeLists.txt b/extensions/colmap/CMakeLists.txt index c1291bd..55f8392 100644 --- a/extensions/colmap/CMakeLists.txt +++ b/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( diff --git a/extensions/colmap/src/bindings/bindings.h b/extensions/colmap/src/bindings/bindings.h index 0ead8ea..896a8f6 100644 --- a/extensions/colmap/src/bindings/bindings.h +++ b/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 diff --git a/extensions/colmap/src/bindings/bundle_adjustment_problem.cc b/extensions/colmap/src/bindings/bundle_adjustment_problem.cc index c0e3039..1c3dccd 100644 --- a/extensions/colmap/src/bindings/bundle_adjustment_problem.cc +++ b/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 +#include +#include -#include "stages/depth_prior.h" #include "stages/intrinsics_prior.h" -#include "stages/rig_geometry.h" -#include -#include #include 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& point_ids, - const Eigen::VectorXd& depths, - const Eigen::VectorXd& robust_scales, - const Eigen::VectorXd& weights, - const Eigen::Matrix& robust_mask, - colmap::CeresLossFunctionType loss_type, - py::array_t 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>; - auto losses = std::make_unique(); - 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(ptr); }), - added); - }); m.def("focal_prior_cost", [](const colmap::Camera& camera, double focal, double stddev) { const auto indices = camera.FocalLengthIdxs(); diff --git a/extensions/colmap/src/bindings/global_positioning.cc b/extensions/colmap/src/bindings/global_positioning.cc deleted file mode 100644 index 2d95647..0000000 --- a/extensions/colmap/src/bindings/global_positioning.cc +++ /dev/null @@ -1,16 +0,0 @@ -#include "vidmap_native/global_positioning.h" - -#include "bindings.h" - -namespace py = pybind11; - -namespace vidmap { - -void BindGlobalPositioning(py::module_& m) { - py::enum_(m, "MetricDepthResidualType") - .value("LINEAR", MetricDepthResidualType::kLinear) - .value("LOG", MetricDepthResidualType::kLog) - .value("LOG_LINEAR", MetricDepthResidualType::kLogLinear); -} - -} // namespace vidmap diff --git a/extensions/colmap/src/bindings/global_positioning_problem.cc b/extensions/colmap/src/bindings/global_positioning_problem.cc index 341b3d2..988cd76 100644 --- a/extensions/colmap/src/bindings/global_positioning_problem.cc +++ b/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 #include -#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& selections, + [](colmap::GlobalPositioner& positioner, + const colmap::Reconstruction& reconstruction, + const std::vector& selections, const vidmap::MappingSidecars& sidecars, const std::map& outlier_masks) { - std::map, ObservationSelection> groups; + using Depths = + std::map>; + using Key = std::tuple; + std::map> groups; std::map 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& 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 log_depth_scale, + const std::shared_ptr& 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, diff --git a/extensions/colmap/src/bindings/module.cc b/extensions/colmap/src/bindings/module.cc index 57cc395..a911ae1 100644 --- a/extensions/colmap/src/bindings/module.cc +++ b/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); } diff --git a/extensions/colmap/src/stages/depth_prior.h b/extensions/colmap/src/stages/depth_prior.h deleted file mode 100644 index 71e0676..0000000 --- a/extensions/colmap/src/stages/depth_prior.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include "colmap/estimators/cost_functions/quaternion_utils.h" -#include "colmap/estimators/cost_functions/utils.h" -#include "colmap/geometry/rigid3.h" - -#include -#include - -namespace vidmap { - -struct LogScaledDepthErrorCostFunctor - : public colmap:: - AutoDiffCostFunctor { - explicit LogScaledDepthErrorCostFunctor( - double depth, const colmap::Rigid3d& sensor_from_rig = colmap::Rigid3d()) - : depth_(depth), - sensor_z_axis_(sensor_from_rig.rotation().toRotationMatrix().row(2)), - sensor_z_offset_(sensor_from_rig.translation().z()), - direct_depth_(sensor_z_axis_ == Eigen::Vector3d::UnitZ() && - sensor_z_offset_ == 0.0) {} - - template - bool operator()(const T* pose, - const T* point3D, - const T* log_scale, - T* residuals) const { - const Eigen::Matrix rotated_point = - colmap::EigenQuaternionMap(pose) * - colmap::EigenVector3Map(point3D); - const T predicted_depth = - direct_depth_ - ? rotated_point[2] + pose[6] - : sensor_z_axis_.cast().dot( - rotated_point + colmap::EigenVector3Map(pose + 4)) + - T(sensor_z_offset_); - if (predicted_depth <= T(0.0)) { - residuals[0] = T(0.0); - return true; - } - residuals[0] = - ceres::log(predicted_depth) - (ceres::log(T(depth_)) + log_scale[0]); - return true; - } - - private: - const double depth_; - const Eigen::Vector3d sensor_z_axis_; - const double sensor_z_offset_; - const bool direct_depth_; -}; - -} // namespace vidmap diff --git a/extensions/colmap/src/stages/metric_depth.h b/extensions/colmap/src/stages/metric_depth.h deleted file mode 100644 index c878bd3..0000000 --- a/extensions/colmap/src/stages/metric_depth.h +++ /dev/null @@ -1,108 +0,0 @@ -#pragma once - -#include "colmap/estimators/cost_functions/utils.h" - -#include -#include - -#include "vidmap_native/global_positioning.h" -#include -#include -#include - -namespace vidmap { - -struct MetricDepthError - : public colmap::AutoDiffCostFunctor { - MetricDepthError( - const Eigen::Quaterniond& rotation, - double depth_prior, - double sigma_depth, - bool use_log_scale, - MetricDepthResidualType residual_type, - bool zero_residual_behind, - double log_linear_threshold, - const Eigen::Vector3d& rig_translation = Eigen::Vector3d::Zero()) - : rotation_(rotation), - rig_translation_(rig_translation), - depth_prior_(depth_prior), - sigma_depth_(sigma_depth), - use_log_scale_(use_log_scale), - residual_type_(residual_type), - zero_residual_behind_(zero_residual_behind), - log_linear_threshold_(log_linear_threshold) { - if (sigma_depth <= 1e-9) { - throw std::invalid_argument("depth standard deviation must be positive"); - } - if (residual_type == MetricDepthResidualType::kLogLinear && - log_linear_threshold <= 0.0) { - throw std::invalid_argument("log-linear threshold must be positive"); - } - } - - template - bool operator()(const T* camera_center, - const T* point3D, - const T* depth_map_scale, - T* residuals) const { - using Vector3 = Eigen::Matrix; - const Vector3 point_world = Eigen::Map(point3D); - const Vector3 center_world = Eigen::Map(camera_center); - Vector3 displacement = point_world - center_world; - if (!rig_translation_.isZero(0.0)) - displacement += rig_translation_.cast(); - const T estimated_depth = (rotation_.cast() * displacement)[2]; - const T scale = - use_log_scale_ ? ceres::exp(depth_map_scale[0]) : depth_map_scale[0]; - const T scaled_prior = scale * T(depth_prior_); - const T scaled_stddev = scale * T(sigma_depth_); - - if (zero_residual_behind_ && estimated_depth <= T(0.0)) { - residuals[0] = T(0.0); - return true; - } - - T depth_residual; - T weight; - if (residual_type_ != MetricDepthResidualType::kLinear) { - const T safe_prior = std::max(T(depth_prior_), T(1e-6)); - const T log_stddev = T(sigma_depth_) / safe_prior; - const T log_weight = T(1.0) / std::max(T(1e-6), log_stddev); - if (residual_type_ == MetricDepthResidualType::kLogLinear) { - const T threshold = T(log_linear_threshold_); - const T safe_scaled_prior = std::max(scaled_prior, T(1e-6)); - if (estimated_depth > threshold) { - depth_residual = ceres::log(std::max(estimated_depth, T(1e-6)) / - safe_scaled_prior); - } else { - depth_residual = ceres::log(threshold / safe_scaled_prior) + - (estimated_depth - threshold); - } - weight = log_weight; - } else if (estimated_depth > T(0.0)) { - depth_residual = ceres::log(std::max(estimated_depth, T(1e-6)) / - std::max(scaled_prior, T(1e-6))); - weight = log_weight; - } else { - depth_residual = estimated_depth - scaled_prior; - weight = T(1.0) / std::max(T(1e-6), scaled_stddev); - } - } else { - depth_residual = estimated_depth - scaled_prior; - weight = T(1.0) / std::max(T(1e-6), scaled_stddev); - } - residuals[0] = weight * depth_residual; - return true; - } - - const Eigen::Quaterniond rotation_; - const Eigen::Vector3d rig_translation_; - const double depth_prior_; - const double sigma_depth_; - const bool use_log_scale_; - const MetricDepthResidualType residual_type_; - const bool zero_residual_behind_; - const double log_linear_threshold_; -}; - -} // namespace vidmap diff --git a/extensions/colmap/src/vidmap_native/global_positioning.h b/extensions/colmap/src/vidmap_native/global_positioning.h deleted file mode 100644 index 5bb7c23..0000000 --- a/extensions/colmap/src/vidmap_native/global_positioning.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -namespace vidmap { - -enum class MetricDepthResidualType { - kLinear, - kLog, - kLogLinear, -}; - -} // namespace vidmap diff --git a/vidmap/mapper/options/positioning.py b/vidmap/mapper/options/positioning.py index a5fd51a..885768e 100644 --- a/vidmap/mapper/options/positioning.py +++ b/vidmap/mapper/options/positioning.py @@ -32,16 +32,13 @@ class GPCommonOptions: use_metric_depth_constraint: bool = True bearing_kp_stddev: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 8.0 optimize_depth_map_scales: bool = True - use_log_scale_for_depth_map_scales: bool = False - smooth_log_linear_transition: bool = False - log_linear_threshold: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 0.1 + log_linear_threshold: Annotated[float, Field(gt=0, lt=1, allow_inf_nan=False)] = 0.1 random_seed: Annotated[int, Field(ge=-1)] = 1 loss_function_type: NativeLossName = "huber" loss_function_weight: Annotated[float, Field(ge=0, allow_inf_nan=False)] = 1.0 loss_function_scale: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 0.1 apply_uncalibrated_loss_downweight: bool = False num_threads: Optional[int] = None - zero_residuals_behind_camera: bool = False @model_validator(mode="after") def validate_threads(self): @@ -97,7 +94,6 @@ class GPSecondPassOptions: enabled: bool = True scale_prior_stddev: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 0.05 max_iterations: Annotated[int, Field(gt=0)] = 100 - use_log_depth_residual: bool = True relax_angular_stddevs: Annotated[float, Field(gt=0, allow_inf_nan=False)] = 1.0 scale_reg_loss_name: ScaleLossName = "huber" scale_reg_weight: Annotated[float, Field(ge=0, allow_inf_nan=False)] = 1.0 diff --git a/vidmap/mapper/stages/bundle_adjustment/adjuster.py b/vidmap/mapper/stages/bundle_adjustment/adjuster.py index 2d29077..c695777 100644 --- a/vidmap/mapper/stages/bundle_adjustment/adjuster.py +++ b/vidmap/mapper/stages/bundle_adjustment/adjuster.py @@ -515,22 +515,19 @@ def solve_problem( depths = depths[mask] variances = variances[mask] - inverse_uncertainty = 1 / variances.clip(1e-6, None) - point3D_ids = point3D_ids[mask] risky_mask = risky_mask[mask] if param_multiplier == 0: continue - params = param_multiplier * variances**0.5 / depths - magnitudes = depth_magnitude_multiplier * depths**2 * inverse_uncertainty depth_batches.append( DepthObservationBatch( image_id=image_id, - point_ids=point3D_ids, + point2D_indices=point2D_indices[mask], depths=depths, - robust_scales=params, - weights=magnitudes, + stddevs=np.sqrt(variances), robust_mask=np.ones(len(depths), dtype=bool) if robust_risky_depth else ~risky_mask, loss_type=depth_loss_type, + robust_scale=param_multiplier, + weight=depth_magnitude_multiplier, ) ) diff --git a/vidmap/mapper/stages/bundle_adjustment/problem.py b/vidmap/mapper/stages/bundle_adjustment/problem.py index 623a10e..d77a74e 100644 --- a/vidmap/mapper/stages/bundle_adjustment/problem.py +++ b/vidmap/mapper/stages/bundle_adjustment/problem.py @@ -16,12 +16,59 @@ @dataclass class DepthObservationBatch: image_id: int - point_ids: np.ndarray + point2D_indices: np.ndarray depths: np.ndarray - robust_scales: np.ndarray - weights: np.ndarray + stddevs: np.ndarray robust_mask: np.ndarray loss_type: pycolmap.LossFunctionType + robust_scale: float = 1.0 + weight: float = 1.0 + + def loss_groups(self): + count = len(self.point2D_indices) + if any(np.shape(values) != (count,) for values in (self.depths, self.stddevs, self.robust_mask)): + raise ValueError("invalid BA depth observation arrays") + if len(np.unique(self.point2D_indices)) != count: + raise ValueError("duplicate BA depth observation") + if ( + not np.all(np.isfinite(self.depths)) + or np.any(self.depths <= 0) + or not np.all(np.isfinite(self.stddevs)) + or np.any(self.stddevs <= 0) + or not np.isfinite(self.robust_scale) + or self.robust_scale <= 0 + or not np.isfinite(self.weight) + or self.weight < 0 + ): + raise ValueError("invalid BA depth or loss values") + if self.weight == 0: + return + variance = self.stddevs**2 + trivial = ~self.robust_mask | (self.loss_type == pycolmap.LossFunctionType.TRIVIAL) + robust_weight = self.weight * self.robust_scale**2 + floor = 1e-6 + # The variance floor changes magnitude, but leaves the robust threshold unchanged. + for row in np.flatnonzero(~trivial & (variance < floor)): + yield ( + { + int(self.point2D_indices[row]): ( + float(self.depths[row]), + float(self.robust_scale * self.stddevs[row]), + ) + }, + self.loss_type, + robust_weight * variance[row] / floor, + ) + for mask, stddevs, loss_type, weight in ( + (~trivial & (variance >= floor), self.robust_scale * self.stddevs, self.loss_type, robust_weight), + (trivial, np.sqrt(np.maximum(variance, floor)), pycolmap.LossFunctionType.TRIVIAL, self.weight), + ): + observations = { + int(index): (float(depth), float(stddev)) + for index, depth, stddev in zip(self.point2D_indices[mask], self.depths[mask], stddevs[mask]) + } + if observations: + yield observations, loss_type, weight @dataclass @@ -103,7 +150,6 @@ def run_bundle_adjustment( 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 = [] active_images = set(image_ids) for batch in depth_batches: image_id = batch.image_id @@ -111,21 +157,24 @@ def run_bundle_adjustment( continue if image_id not in scales: raise ValueError("depth constraints require a scale record") - losses, count = ba_costs.append_depth_observations( - problem, - reconstruction, - image_id, - batch.point_ids, - batch.depths, - batch.robust_scales, - batch.weights, - batch.robust_mask, - batch.loss_type, - scales[image_id], - not options.refine_rig_from_world - or config.has_constant_rig_from_world_pose(reconstruction.image(image_id).frame_id), - ) - depth_losses.append(losses) + image = reconstruction.image(image_id) + pose = image.frame.rig_from_world.params + fix_pose = not options.refine_rig_from_world or config.has_constant_rig_from_world_pose(image.frame_id) + if not fix_pose and not problem.has_parameter_block(pose): + continue + count = 0 + for observations, loss_type, weight in batch.loss_groups(): + loss = pycolmap.create_ceres_loss_function(loss_type, 1.0, weight) + count += pycolmap.append_ba_depth_constraints( + owner, + reconstruction, + image_id, + observations, + scales[image_id], + loss_function=loss, + ) + if count and fix_pose: + problem.set_parameter_block_constant(pose) diagnostics.num_depth_residuals += count for image_id, record in scale_records.items(): diff --git a/vidmap/mapper/stages/global_positioning/problem.py b/vidmap/mapper/stages/global_positioning/problem.py index fc48716..3634e35 100644 --- a/vidmap/mapper/stages/global_positioning/problem.py +++ b/vidmap/mapper/stages/global_positioning/problem.py @@ -240,45 +240,35 @@ def run_global_positioning( normal_depth = _loss(pass_options.loss_normal_depth) loop_depth = _loss(pass_options.loss_lc_depth) outlier_depth = _loss(options.track_filter.loss_normal_depth_outlier) if depth_outliers_marked else trivial - depth_groups, counts = gp_costs.split_depth_observations(selections, sidecars, depth_outlier_masks or {}) + depth_groups, counts = gp_costs.split_depth_observations( + positioner, + reconstruction, + selections, + sidecars, + {} if depth_outlier_masks is None else depth_outlier_masks, + ) for image_id in counts: - value = initial_depth_map_scales.get(image_id, 1.0) - depth_scales[image_id] = np.array( - [np.log(max(1e-9, value)) if common.use_log_scale_for_depth_map_scales else value], dtype=float - ) - for (loop, outlier), selection in depth_groups.items(): + value = initial_depth_map_scales[image_id] if image_id in initial_depth_map_scales else 1.0 + depth_scales[image_id] = np.array([np.log(value)], dtype=float) + depth_options = pycolmap.GPDepthConstraintOptions( + residual_type=pycolmap.DepthResidualType.LOG_LINEAR if second_pass else pycolmap.DepthResidualType.LINEAR, + log_linear_threshold=common.log_linear_threshold, + ) + for (loop, outlier, image_id), (depths, explicit_observations) in depth_groups.items(): loss = loop_depth if loop else outlier_depth if outlier else normal_depth - gp_costs.append_depth_observations( - problem, - reconstruction, - sidecars, - selection, - depth_scales, - loss, - common.use_log_scale_for_depth_map_scales, - ( - native.MetricDepthResidualType.LOG_LINEAR - if common.smooth_log_linear_transition - else ( - native.MetricDepthResidualType.LOG - if second_pass and options.second_pass.use_log_depth_residual - else native.MetricDepthResidualType.LINEAR - ) - ), - common.zero_residuals_behind_camera, - common.log_linear_threshold, - ) - diagnostics.num_metric_depth_residuals = sum(counts.values()) + scale = depth_scales[image_id] + if depths: + diagnostics.num_metric_depth_residuals += pycolmap.append_gp_depth_constraints( + positioner, reconstruction, image_id, depths, scale, depth_options, loss + ) + if len(explicit_observations): + diagnostics.num_metric_depth_residuals += gp_costs.append_lc_depths( + problem, reconstruction, sidecars, explicit_observations, scale, loss, depth_options + ) 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]]), - ) + prior_cost = pyceres.factors.NormalPrior(np.zeros(1), np.array([[pass_options.scale_prior_stddev**2]])) for image_id, count in counts.items(): - scale = depth_scales[image_id] - problem.add_residual_block(prior_cost, gp_costs.scaled_loss(prior_loss, count), [scale]) - if not common.use_log_scale_for_depth_map_scales: - problem.set_parameter_lower_bound(scale, 0, 1e-5) + problem.add_residual_block(prior_cost, gp_costs.scaled_loss(prior_loss, count), [depth_scales[image_id]]) diagnostics.num_scale_prior_residuals = len(counts) # Add optional temporal acceleration priors. temporal = options.temporal_acceleration @@ -356,9 +346,7 @@ def run_global_positioning( pose.translation = -(pose.rotation * center) result.success = True - result.depth_map_scales = { - i: float(np.exp(v[0]) if common.use_log_scale_for_depth_map_scales else v[0]) for i, v in depth_scales.items() - } + result.depth_map_scales = {i: float(np.exp(v[0])) for i, v in depth_scales.items()} diagnostics.num_depth_map_scales = len(depth_scales) diagnostics.num_camera_centers = len(frame_centers) diagnostics.num_point3D_parameters = num_points diff --git a/vidmap/runtime_variants.py b/vidmap/runtime_variants.py index ab75761..a29599c 100644 --- a/vidmap/runtime_variants.py +++ b/vidmap/runtime_variants.py @@ -17,7 +17,9 @@ class ColmapRuntimeVariant: COLMAP_RUNTIME_VARIANTS = { "stock": ColmapRuntimeVariant( name="stock", - revision="b1972bc226f58e095c8b75bc214f4bb2d8ba57e4", + # Pending dependency: unpublished COLMAP depth implementation. + # Public builds are blocked until it is published and this pin is updated. + revision="6cde3c7f078aa53aa2ecb39d900c0f89121819a2", pycolmap_major_minor=(4, 3), canonical_environment="VIDMAP_STOCK_COLMAP", ),