Skip to content

Commit d1d92d9

Browse files
authored
Add COVARIANCE reweighting for Ceres rotation averaging (colmap#4953)
## Summary Adds `RotationAveragingReweighting::COVARIANCE` to whiten relative-rotation constraints in the Ceres rotation averaging backend using per-edge rotation covariances: - `CeresRotationAverager` whitens each residual, $`r_{ij} \mapsto \Sigma_{R,ij}^{-1/2} r_{ij}`$, where $`\Sigma_{R,ij}`$ is regularized with an isotropic floor, $`\Sigma_{R,ij} + \sigma_{\text{floor}}^2 I_3`$ (`CeresRotationAveragerOptions::covariance_sigma_floor_deg`, 0.02° by default). The pose graph keeps the raw covariances. - The robust loss acts on the whitened, unitless residual with scale `CeresRotationAveragerOptions::covariance_loss_scale` (3.0 standard deviations by default), in place of `loss_function_scale` (in degrees). - Splits Ceres rotation averaging preparation into `CheckPoseGraph`, `PrepareWeights`, and `InitializeRotations` (reusing `internal::TryComputeLeftSqrtInformation` and `DynamicCovarianceWeightedCostFunction::FromLeftSqrtInformation`) so both `RotationEstimator` and `CeresRotationAverager` validate connectivity, sensor rotations, loss scales, `covariance_sigma_floor_deg`, and that every valid edge has a finite, symmetric positive-definite floored covariance before MST initialization modifies the reconstruction, without repeating connected-components or per-edge Cholesky factorizations. - Adds `use_rotation_covariance` to `InitializeFromMaximumSpanningTree`, preferring edges with the smallest $`\mathrm{tr}(\Sigma_R)`$ via weights $`-\log \mathrm{tr}(\Sigma_R)`$. `RotationEstimator` runs MST initialization once before the warm-start. - Keeps the Huber warm-start on angular residuals with `UNIFORM` weights so confidently wrong relative rotations (e.g., from symmetric structures) do not dominate early iterations. - `COVARIANCE` is unsupported by `L1_IRLS` (throws), and is ignored with a warning when `CERES` falls back to `L1_IRLS` for gravity priors. - Binds `RotationAveragingReweighting.COVARIANCE`, `covariance_loss_scale`, and `covariance_sigma_floor_deg` in pycolmap. ## Tests - `CeresRotationAverager.CovarianceWhitening`, `CovarianceDownweightsUncertainEdges`, `CovarianceReweightingWithRigs`, and `RotationEstimator.CeresChecksInputsBeforeInitialization` in `rotation_averaging_ceres_test.cc`. - `RotationAveraging.InitializeFromMaximumSpanningTreeWithCovariance` and `RotationAveraging.CovarianceReweighting` in `rotation_averaging_test.cc`. - Python binding tests in `motion_averaging_test.py` and `rotation_averaging_ceres_test.py`. <sub>Stack created with <a href="https://github.com/github/gh-stack">GitHub Stacks CLI</a> • <a href="https://gh.io/stacks-feedback">Give Feedback 💬</a></sub>
1 parent 5b6da88 commit d1d92d9

11 files changed

Lines changed: 850 additions & 171 deletions

‎src/colmap/estimators/cost_functions/utils.h‎

Lines changed: 49 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,12 @@
66
#include "colmap/util/logging.h"
77

88
#include <memory>
9+
#include <optional>
910
#include <vector>
1011

12+
#include <Eigen/Cholesky>
1113
#include <Eigen/Core>
14+
#include <Eigen/LU>
1215
#include <ceres/ceres.h>
1316

1417
namespace colmap {
@@ -93,14 +96,32 @@ auto LastValueParameterPack(Args&&... args) {
9396
namespace internal {
9497

9598
template <int kNumResiduals>
96-
Eigen::Matrix<double, kNumResiduals, kNumResiduals> ComputeLeftSqrtInformation(
99+
std::optional<Eigen::Matrix<double, kNumResiduals, kNumResiduals>>
100+
TryComputeLeftSqrtInformation(
97101
const Eigen::Matrix<double, kNumResiduals, kNumResiduals>& cov) {
98102
using CovMat = Eigen::Matrix<double, kNumResiduals, kNumResiduals>;
99-
const CovMat info = cov.inverse();
100-
const Eigen::LLT<CovMat> llt(info);
101-
THROW_CHECK(info.allFinite() && llt.info() == Eigen::Success)
102-
<< "Covariance matrix is not positive definite";
103-
return llt.matrixL().transpose();
103+
if (!cov.allFinite() || !cov.isApprox(cov.transpose(), 1e-5)) {
104+
return std::nullopt;
105+
}
106+
const CovMat info = (0.5 * (cov + cov.transpose())).inverse();
107+
if (!info.allFinite()) {
108+
return std::nullopt;
109+
}
110+
const Eigen::LLT<CovMat> llt(0.5 * (info + info.transpose()));
111+
if (llt.info() != Eigen::Success) {
112+
return std::nullopt;
113+
}
114+
return CovMat(llt.matrixL().transpose());
115+
}
116+
117+
template <int kNumResiduals>
118+
Eigen::Matrix<double, kNumResiduals, kNumResiduals> ComputeLeftSqrtInformation(
119+
const Eigen::Matrix<double, kNumResiduals, kNumResiduals>& cov) {
120+
std::optional<Eigen::Matrix<double, kNumResiduals, kNumResiduals>>
121+
left_sqrt_info = TryComputeLeftSqrtInformation<kNumResiduals>(cov);
122+
THROW_CHECK(left_sqrt_info.has_value())
123+
<< "Covariance matrix is not symmetric positive definite";
124+
return *left_sqrt_info;
104125
}
105126

106127
} // namespace internal
@@ -220,6 +241,13 @@ class DynamicCovarianceWeightedCostFunction : public ceres::CostFunction {
220241
*mutable_parameter_block_sizes() = cost_->parameter_block_sizes();
221242
}
222243

244+
static DynamicCovarianceWeightedCostFunction* FromLeftSqrtInformation(
245+
const CovMat& left_sqrt_info, ceres::CostFunction* cost) {
246+
return new DynamicCovarianceWeightedCostFunction(
247+
std::unique_ptr<ceres::CostFunction>(THROW_CHECK_NOTNULL(cost)),
248+
left_sqrt_info);
249+
}
250+
223251
bool Evaluate(double const* const* parameters,
224252
double* residuals,
225253
double** jacobians) const override {
@@ -236,15 +264,27 @@ class DynamicCovarianceWeightedCostFunction : public ceres::CostFunction {
236264
}
237265
Eigen::Map<
238266
Eigen::
239-
Matrix<double, kNumResiduals, Eigen::Dynamic, Eigen::RowMajor>>(
240-
jacobians[i], kNumResiduals, block_sizes[i])
241-
.applyOnTheLeft(left_sqrt_info_);
267+
Matrix<double, kNumResiduals, Eigen::Dynamic, Eigen::RowMajor>>
268+
jacobian(jacobians[i], kNumResiduals, block_sizes[i]);
269+
for (int col = 0; col < block_sizes[i]; ++col) {
270+
jacobian.col(col) = left_sqrt_info_ * jacobian.col(col);
271+
}
242272
}
243273
}
244274
return true;
245275
}
246276

247277
private:
278+
DynamicCovarianceWeightedCostFunction(
279+
std::unique_ptr<ceres::CostFunction> cost, const CovMat& left_sqrt_info)
280+
: cost_(std::move(THROW_CHECK_NOTNULL(cost))),
281+
left_sqrt_info_(left_sqrt_info) {
282+
THROW_CHECK(left_sqrt_info_.allFinite());
283+
THROW_CHECK_EQ(cost_->num_residuals(), kNumResiduals);
284+
set_num_residuals(kNumResiduals);
285+
*mutable_parameter_block_sizes() = cost_->parameter_block_sizes();
286+
}
287+
248288
// Declared first, such that the wrapped cost function is owned, and thus
249289
// released, if computing the square root information throws.
250290
const std::unique_ptr<ceres::CostFunction> cost_;

‎src/colmap/estimators/rotation_averaging.cc‎

Lines changed: 33 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,9 @@
1111
#include "colmap/util/threading.h"
1212

1313
#include <algorithm>
14+
#include <cmath>
1415
#include <queue>
16+
#include <stdexcept>
1517

1618
namespace colmap {
1719
namespace {
@@ -47,12 +49,15 @@ bool AllSensorsFromRigKnown(const NodeHashMap<rig_t, Rig>& rigs) {
4749
return all_known;
4850
}
4951

50-
// Compute maximum spanning tree of the pose graph weighted by inlier count.
51-
// Returns the root image_id and populates the parents map.
52+
// Compute maximum spanning tree of the pose graph weighted by rotation
53+
// certainty (if use_rotation_covariance) or inlier count, with rig calibration
54+
// edges taking precedence. Returns the root image_id and populates the parents
55+
// map.
5256
image_t ComputeMaximumPoseGraphSpanningTree(
5357
const PoseGraph& pose_graph,
5458
const FlatHashSet<image_t>& image_ids,
55-
NodeHashMap<image_t, image_t>& parents) {
59+
NodeHashMap<image_t, image_t>& parents,
60+
bool use_rotation_covariance) {
5661
// Build mapping between image_id and contiguous indices.
5762
NodeHashMap<image_t, int> image_id_to_idx;
5863
std::vector<image_t> idx_to_image_id;
@@ -82,12 +87,28 @@ image_t ComputeMaximumPoseGraphSpanningTree(
8287
continue;
8388
}
8489
edges.emplace_back(it1->second, it2->second);
85-
weights.push_back(static_cast<float>(edge.num_matches));
8690
if (edge.prior) {
91+
// Re-weighted below, so it needs no covariance.
92+
weights.push_back(0);
8793
prior_indices.push_back(weights.size() - 1);
94+
continue;
95+
}
96+
if (use_rotation_covariance) {
97+
THROW_CHECK(edge.cam2_from_cam1_rotation_cov.has_value())
98+
<< "Missing rotation covariance of edge (" << image_id1 << ", "
99+
<< image_id2 << ")";
100+
THROW_CHECK(edge.cam2_from_cam1_rotation_cov->allFinite())
101+
<< "Non-finite rotation covariance of edge (" << image_id1 << ", "
102+
<< image_id2 << ")";
103+
// The trace is the expected squared angular error in radians. Only the
104+
// order of the weights matters, which its negative logarithm keeps
105+
// within single precision.
106+
weights.push_back(static_cast<float>(-std::log(std::max<double>(
107+
edge.cam2_from_cam1_rotation_cov->cast<double>().trace(), 1e-12))));
88108
} else {
89-
max_weight = std::max(max_weight, weights.back());
109+
weights.push_back(static_cast<float>(edge.num_matches));
90110
}
111+
max_weight = std::max(max_weight, weights.back());
91112
}
92113

93114
// Fixed rig calibration takes precedence over measured relative rotations.
@@ -261,21 +282,6 @@ Reconstruction CreateExpandedReconstruction(
261282
return recon_expanded;
262283
}
263284

264-
std::optional<PoseGraph> MaybeFilterPoseGraphToActiveImages(
265-
const PoseGraph& pose_graph, const FlatHashSet<image_t>& active_image_ids) {
266-
for (const auto& [pair_id, _] : pose_graph.ValidEdges()) {
267-
const auto [image_id1, image_id2] = PairIdToImagePair(pair_id);
268-
if (!active_image_ids.count(image_id1) ||
269-
!active_image_ids.count(image_id2)) {
270-
PoseGraph filtered_pose_graph = pose_graph;
271-
filtered_pose_graph.InvalidatePairsOutsideActiveImageIds(
272-
active_image_ids);
273-
return filtered_pose_graph;
274-
}
275-
}
276-
return std::nullopt;
277-
}
278-
279285
} // namespace
280286

281287
RotationEstimatorBackendOptions::RotationEstimatorBackendOptions()
@@ -554,8 +560,12 @@ bool RotationEstimator::SolveRotationAveraging(
554560
return SolveRotationAveragingWithCeres(
555561
pose_graph, active_image_ids, reconstruction);
556562
}
563+
// L1_IRLS ignores COVARIANCE reweighting (equivalent to UNIFORM).
557564
LOG(WARNING) << "Gravity priors are not supported by the CERES rotation "
558565
"averaging backend, falling back to L1_IRLS";
566+
} else if (options_.reweighting == RotationAveragingReweighting::COVARIANCE) {
567+
LOG(FATAL_THROW) << "COVARIANCE reweighting is not implemented for the "
568+
"L1_IRLS rotation averaging backend";
559569
}
560570

561571
// Initialize rotations from maximum spanning tree. Note that without
@@ -582,50 +592,12 @@ bool RotationEstimator::SolveRotationAveraging(
582592
return true;
583593
}
584594

585-
bool RotationEstimator::SolveRotationAveragingWithCeres(
586-
const PoseGraph& pose_graph,
587-
const FlatHashSet<image_t>& active_image_ids,
588-
Reconstruction& reconstruction) {
589-
THROW_CHECK_NOTNULL(options_.ceres);
590-
591-
const std::optional<PoseGraph> filtered_pose_graph =
592-
MaybeFilterPoseGraphToActiveImages(pose_graph, active_image_ids);
593-
const PoseGraph& active_pose_graph =
594-
filtered_pose_graph.has_value() ? *filtered_pose_graph : pose_graph;
595-
596-
const auto solve = [&](const RotationEstimatorOptions& options) {
597-
auto averager = CreateDefaultCeresRotationAverager(
598-
options, active_pose_graph, reconstruction);
599-
const ceres::Solver::Summary summary = averager->Solve();
600-
if (VLOG_IS_ON(2)) {
601-
LOG(INFO) << summary.FullReport();
602-
} else {
603-
VLOG(1) << summary.BriefReport();
604-
}
605-
return summary.IsSolutionUsable();
606-
};
607-
608-
RotationEstimatorOptions options = options_;
609-
if (options.ceres->max_num_warm_start_iterations > 0) {
610-
RotationEstimatorOptions warm_start_options = options;
611-
warm_start_options.ceres->loss_function_type = CeresLossFunctionType::HUBER;
612-
warm_start_options.ceres->solver_options.max_num_iterations =
613-
options.ceres->max_num_warm_start_iterations;
614-
if (!solve(warm_start_options)) {
615-
return false;
616-
}
617-
// Continue from the warm-started rotations.
618-
options.skip_initialization = true;
619-
}
620-
621-
return solve(options);
622-
}
623-
624595
void InitializeFromMaximumSpanningTree(
625596
const PoseGraph& pose_graph,
626597
const FlatHashSet<image_t>& active_image_ids,
627598
Reconstruction& reconstruction,
628-
bool refine_sensor_from_rig) {
599+
bool refine_sensor_from_rig,
600+
bool use_rotation_covariance) {
629601
const auto cam_from_rig = [&](image_t image_id) {
630602
const auto& image = reconstruction.Image(image_id);
631603
return image.IsRefInFrame() ? Rigid3d()
@@ -636,7 +608,7 @@ void InitializeFromMaximumSpanningTree(
636608
// Compute maximum spanning tree over active images.
637609
NodeHashMap<image_t, image_t> parents;
638610
const image_t root = ComputeMaximumPoseGraphSpanningTree(
639-
pose_graph, active_image_ids, parents);
611+
pose_graph, active_image_ids, parents, use_rotation_covariance);
640612
THROW_CHECK(active_image_ids.count(root));
641613

642614
// Iterate through the tree to initialize the rotation.

‎src/colmap/estimators/rotation_averaging.h‎

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,14 @@ namespace colmap {
2323
// INLIER_MATCH_COUNT: weight each constraint by the number of inlier
2424
// two-view matches (PoseGraph::Edge::num_matches) of the corresponding
2525
// edge, normalized to (0, 1].
26-
MAKE_ENUM_CLASS_OVERLOAD_STREAM(RotationAveragingReweighting,
27-
0,
28-
UNIFORM,
29-
INLIER_MATCH_COUNT);
26+
// COVARIANCE: whiten each constraint with the covariance of its relative
27+
// rotation (PoseGraph::Edge::cam2_from_cam1_rotation_cov), regularized with
28+
// CeresRotationAveragerOptions::covariance_sigma_floor_deg. Every valid
29+
// edge must have a covariance. Only supported by the CERES backend, which
30+
// then uses CeresRotationAveragerOptions::covariance_loss_scale. It is
31+
// ignored when the CERES backend falls back to L1_IRLS for gravity priors.
32+
MAKE_ENUM_CLASS_OVERLOAD_STREAM(
33+
RotationAveragingReweighting, 0, UNIFORM, INLIER_MATCH_COUNT, COVARIANCE);
3034

3135
// Solver backend for rotation averaging.
3236
// L1_IRLS: L1 regression (ADMM) followed by IRLS using CHOLMOD.
@@ -143,14 +147,17 @@ class RotationEstimator {
143147
const RotationEstimatorOptions options_;
144148
};
145149

146-
// Initializes rotations from maximum spanning tree.
147-
// For calibrated multi-camera rigs, add their connections with
150+
// Initializes rotations from maximum spanning tree, which prefers the edges
151+
// with the most inlier matches or, if use_rotation_covariance, the most certain
152+
// relative rotations, whose covariance every measured edge must then have. For
153+
// calibrated multi-camera rigs, add their connections with
148154
// PoseGraph::AddRigEdges before initialization.
149155
void InitializeFromMaximumSpanningTree(
150156
const PoseGraph& pose_graph,
151157
const FlatHashSet<image_t>& active_image_ids,
152158
Reconstruction& reconstruction,
153-
bool refine_sensor_from_rig);
159+
bool refine_sensor_from_rig,
160+
bool use_rotation_covariance = false);
154161

155162
// Initialize rig rotations by averaging per-image rotations.
156163
// Estimates cam_from_rig for cameras with unknown calibration,

0 commit comments

Comments
 (0)