Skip to content

Commit 30a1a81

Browse files
authored
Support covariance whitening in CeresRotationAverager (colmap#4952)
## Summary Supports whitening relative-rotation residuals in `CeresRotationAverager` for both fixed-size camera pairs and dynamic multi-sensor rig blocks: - Adds `DynamicCovarianceWeightedCostFunction<kNumResiduals>` in `estimators/cost_functions/utils.h`, the runtime parameter-block counterpart of `CovarianceWeightedCostFunction` reusing `internal::ComputeLeftSqrtInformation`. - Documents the right-perturbation convention of `RelativeRotationCostFunctor` (`hat_sensor2_from_sensor1 = sensor2_from_sensor1_prior * Exp(r)`). - Adds an optional `cam2_from_cam1_cov` argument to `CeresRotationAverager::AddRelativeRotationResidual` (and `CeresRotationAverager.add_relative_rotation_residual` in pycolmap) to whiten individual relative-rotation residuals with `DynamicCovarianceWeightedCostFunction<3>`. ## Tests - `DynamicCovarianceWeightedCostFunction.MatchesCovarianceWeighted` and `CovarianceWeightedCostFunction.RejectsNonPositiveDefiniteCovariance` in `estimators/cost_functions/utils_test.cc`. - `CeresRotationAverager.WhitensAddedRelativeRotationResidual` in `rotation_averaging_ceres_test.cc` (both fixed 2-block frames and dynamic rig sensors). - Python binding test in `src/pycolmap/estimators/rotation_averaging_ceres_test.py`. PR stack (3/5 splitting colmap#4823, plus colmap#4912): 1. colmap#4950 2. colmap#4951 3. **colmap#4952** (this PR) 4. colmap#4953 5. colmap#4823 6. colmap#4912
1 parent 77cbe48 commit 30a1a81

8 files changed

Lines changed: 208 additions & 6 deletions

File tree

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,11 @@ namespace colmap {
1313

1414
// Angular error in radians between two camera rotations and a relative
1515
// rotation, expressed in the frame of sensor 1. Parameter blocks are unit
16-
// quaternions in Eigen (x, y, z, w) order.
16+
// quaternions in Eigen (x, y, z, w) order. The residual follows the right
17+
// perturbation convention, i.e., it is the rotation vector r such that
18+
// hat_sensor2_from_sensor1 = sensor2_from_sensor1_prior * Exp(r), so a
19+
// covariance of the relative rotation prior must be expressed in the frame of
20+
// sensor 1.
1721
struct RelativeRotationCostFunctor {
1822
template <typename T>
1923
bool operator()(const T* const sensor1_from_world_rotation,

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

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -202,6 +202,55 @@ class CovarianceWeightedCostFunctor {
202202
}
203203
};
204204

205+
// Counterpart of CovarianceWeightedCostFunction for cost functions whose
206+
// parameter block layout is only known at runtime (e.g. dynamic autodiff
207+
// costs). The number of residuals is fixed at compile time.
208+
template <int kNumResiduals>
209+
class DynamicCovarianceWeightedCostFunction : public ceres::CostFunction {
210+
public:
211+
using CovMat = Eigen::Matrix<double, kNumResiduals, kNumResiduals>;
212+
213+
DynamicCovarianceWeightedCostFunction(const CovMat& cov,
214+
ceres::CostFunction* cost)
215+
: cost_(THROW_CHECK_NOTNULL(cost)),
216+
left_sqrt_info_(
217+
internal::ComputeLeftSqrtInformation<kNumResiduals>(cov)) {
218+
THROW_CHECK_EQ(cost_->num_residuals(), kNumResiduals);
219+
set_num_residuals(kNumResiduals);
220+
*mutable_parameter_block_sizes() = cost_->parameter_block_sizes();
221+
}
222+
223+
bool Evaluate(double const* const* parameters,
224+
double* residuals,
225+
double** jacobians) const override {
226+
if (!cost_->Evaluate(parameters, residuals, jacobians)) {
227+
return false;
228+
}
229+
Eigen::Map<Eigen::Matrix<double, kNumResiduals, 1>>(residuals)
230+
.applyOnTheLeft(left_sqrt_info_);
231+
if (jacobians != nullptr) {
232+
const std::vector<int32_t>& block_sizes = parameter_block_sizes();
233+
for (size_t i = 0; i < block_sizes.size(); ++i) {
234+
if (jacobians[i] == nullptr) {
235+
continue;
236+
}
237+
Eigen::Map<
238+
Eigen::
239+
Matrix<double, kNumResiduals, Eigen::Dynamic, Eigen::RowMajor>>(
240+
jacobians[i], kNumResiduals, block_sizes[i])
241+
.applyOnTheLeft(left_sqrt_info_);
242+
}
243+
}
244+
return true;
245+
}
246+
247+
private:
248+
// Declared first, such that the wrapped cost function is owned, and thus
249+
// released, if computing the square root information throws.
250+
const std::unique_ptr<ceres::CostFunction> cost_;
251+
const CovMat left_sqrt_info_;
252+
};
253+
205254
// Whitens the residuals and jacobians of an inner cost function with
206255
// per-residual standard deviations. The diagonal counterpart of
207256
// CovarianceWeightedCostFunction.

‎src/colmap/estimators/cost_functions/utils_test.cc‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -210,6 +210,49 @@ TEST(CovarianceWeightedCostFunction, WrapsAnalyticalCostFunction) {
210210
EXPECT_THAT(residuals_only, EigenMatrixNear(autodiff_residuals, 1e-10));
211211
}
212212

213+
TEST(DynamicCovarianceWeightedCostFunction, MatchesCovarianceWeighted) {
214+
const Eigen::Vector3d param0(1, 2, 3);
215+
const Eigen::Vector3d param1(4, 6, 5);
216+
Eigen::Matrix3d covariance;
217+
covariance << 4.0, 0.5, 0.2, 0.5, 2.0, -0.3, 0.2, -0.3, 1.0;
218+
219+
std::unique_ptr<ceres::CostFunction> fixed_cost_function(
220+
CovarianceWeightedCostFunctor<NormalErrorCostFunctor<3>>::Create(
221+
covariance));
222+
std::unique_ptr<ceres::CostFunction> dynamic_cost_function(
223+
new DynamicCovarianceWeightedCostFunction<3>(
224+
covariance, NormalErrorCostFunctor<3>::Create()));
225+
EXPECT_EQ(dynamic_cost_function->num_residuals(), 3);
226+
EXPECT_EQ(dynamic_cost_function->parameter_block_sizes(),
227+
fixed_cost_function->parameter_block_sizes());
228+
229+
const double* parameters[2] = {param0.data(), param1.data()};
230+
Eigen::Vector3d fixed_residuals;
231+
Eigen::Vector3d dynamic_residuals;
232+
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> fixed_jacobian0;
233+
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> fixed_jacobian1;
234+
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> dynamic_jacobian1;
235+
double* fixed_jacobians[2] = {fixed_jacobian0.data(), fixed_jacobian1.data()};
236+
// Skip the jacobian of the first block, as Ceres does for constant blocks.
237+
double* dynamic_jacobians[2] = {nullptr, dynamic_jacobian1.data()};
238+
EXPECT_TRUE(fixed_cost_function->Evaluate(
239+
parameters, fixed_residuals.data(), fixed_jacobians));
240+
EXPECT_TRUE(dynamic_cost_function->Evaluate(
241+
parameters, dynamic_residuals.data(), dynamic_jacobians));
242+
EXPECT_THAT(dynamic_residuals, EigenMatrixNear(fixed_residuals, 1e-10));
243+
EXPECT_THAT(dynamic_jacobian1, EigenMatrixNear(fixed_jacobian1, 1e-10));
244+
// Whitened squared norm equals the Mahalanobis distance.
245+
const Eigen::Vector3d error = param0 - param1;
246+
EXPECT_NEAR(dynamic_residuals.squaredNorm(),
247+
error.dot(covariance.ldlt().solve(error)),
248+
1e-10);
249+
250+
Eigen::Vector3d residuals_only;
251+
EXPECT_TRUE(dynamic_cost_function->Evaluate(
252+
parameters, residuals_only.data(), nullptr));
253+
EXPECT_THAT(residuals_only, EigenMatrixNear(fixed_residuals, 1e-10));
254+
}
255+
213256
TEST(CovarianceWeightedCostFunction, RejectsNonPositiveDefiniteCovariance) {
214257
// The wrapped cost function is owned, and thus released, on throw.
215258
for (const Eigen::Matrix3d& cov :
@@ -218,6 +261,9 @@ TEST(CovarianceWeightedCostFunction, RejectsNonPositiveDefiniteCovariance) {
218261
EXPECT_THROW(CovarianceWeightedCostFunction<NormalErrorCostFunctor<3>>(
219262
cov, NormalErrorCostFunctor<3>::Create()),
220263
std::invalid_argument);
264+
EXPECT_THROW(DynamicCovarianceWeightedCostFunction<3>(
265+
cov, NormalErrorCostFunctor<3>::Create()),
266+
std::invalid_argument);
221267
}
222268
EXPECT_THROW(CovarianceWeightedCostFunction<NormalErrorCostFunctor<3>>(
223269
Eigen::Matrix3d::Identity(), nullptr),

‎src/colmap/estimators/rotation_averaging_ceres.cc‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -192,7 +192,8 @@ void CeresRotationAverager::AddRelativeRotationResidual(
192192
const image_t image_id1,
193193
const image_t image_id2,
194194
const Eigen::Quaterniond& cam2_from_cam1,
195-
const std::shared_ptr<ceres::LossFunction>& loss_function) {
195+
const std::shared_ptr<ceres::LossFunction>& loss_function,
196+
const std::optional<Eigen::Matrix3d>& cam2_from_cam1_cov) {
196197
const Image& image1 = reconstruction_.Image(image_id1);
197198
const Image& image2 = reconstruction_.Image(image_id2);
198199
Frame& frame1 = *image1.FramePtr();
@@ -219,10 +220,17 @@ void CeresRotationAverager::AddRelativeRotationResidual(
219220
if (loss != nullptr) {
220221
losses_.try_emplace(loss, loss_function);
221222
}
223+
// Whitens the residual with the covariance of the relative rotation, if any.
224+
const auto maybe_whiten = [&](ceres::CostFunction* cost_function) {
225+
return cam2_from_cam1_cov.has_value()
226+
? new DynamicCovarianceWeightedCostFunction<3>(
227+
*cam2_from_cam1_cov, cost_function)
228+
: cost_function;
229+
};
222230
if (sensor1_from_rig_rot_ptr == nullptr &&
223231
sensor2_from_rig_rot_ptr == nullptr && !same_frame) {
224232
problem_->AddResidualBlock(
225-
RelativeRotationCostFunctor::Create(cam2_from_cam1),
233+
maybe_whiten(RelativeRotationCostFunctor::Create(cam2_from_cam1)),
226234
loss,
227235
rig1_from_world_rot_ptr,
228236
rig2_from_world_rot_ptr);
@@ -252,7 +260,8 @@ void CeresRotationAverager::AddRelativeRotationResidual(
252260
for (size_t i = 0; i < parameter_blocks.size(); ++i)
253261
cost_function->AddParameterBlock(4);
254262
cost_function->SetNumResiduals(3);
255-
problem_->AddResidualBlock(cost_function, loss, parameter_blocks);
263+
problem_->AddResidualBlock(
264+
maybe_whiten(cost_function), loss, parameter_blocks);
256265
}
257266

258267
std::unique_ptr<CeresRotationAverager> CreateDefaultCeresRotationAverager(

‎src/colmap/estimators/rotation_averaging_ceres.h‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,9 @@
99
#include "colmap/util/types.h"
1010

1111
#include <memory>
12+
#include <optional>
1213

14+
#include <Eigen/Core>
1315
#include <ceres/ceres.h>
1416

1517
namespace colmap {
@@ -57,11 +59,15 @@ class CeresRotationAverager {
5759
ceres::Problem& Problem();
5860
const ceres::Problem& Problem() const;
5961
// Frame and sensor rotations for both images must be configured in Problem().
62+
// If given, the residual is whitened with the covariance of the relative
63+
// rotation, expressed in the frame of camera 1 (right perturbation, see
64+
// RelativeRotationCostFunctor).
6065
void AddRelativeRotationResidual(
6166
image_t image_id1,
6267
image_t image_id2,
6368
const Eigen::Quaterniond& cam2_from_cam1,
64-
const std::shared_ptr<ceres::LossFunction>& loss_function);
69+
const std::shared_ptr<ceres::LossFunction>& loss_function,
70+
const std::optional<Eigen::Matrix3d>& cam2_from_cam1_cov = std::nullopt);
6571

6672
private:
6773
void InitializeRotations(const RotationEstimatorOptions& options,

‎src/colmap/estimators/rotation_averaging_ceres_test.cc‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -478,5 +478,82 @@ TEST(RotationEstimatorOptions, DeepCopiesCeresOptions) {
478478
EXPECT_EQ(assigned.ceres->loss_function_scale, 0.1);
479479
}
480480

481+
Eigen::Matrix3d AnisotropicCovariance() {
482+
const Eigen::Matrix3d R =
483+
Eigen::AngleAxisd(0.4, Eigen::Vector3d(1, 2, 3).normalized())
484+
.toRotationMatrix();
485+
return R * Eigen::Vector3d(1e-4, 4e-4, 9e-4).asDiagonal() * R.transpose();
486+
}
487+
488+
// Residual convention: hat_cam2_from_cam1 = prior * Exp(residual).
489+
Eigen::Vector3d RelativeRotationResidual(const Eigen::Quaterniond& prior,
490+
const Eigen::Quaterniond& estimate) {
491+
const Eigen::AngleAxisd error(prior.inverse() * estimate);
492+
return error.angle() * error.axis();
493+
}
494+
495+
double EvaluateCost(CeresRotationAverager& averager) {
496+
double cost = 0;
497+
THROW_CHECK(averager.Problem().Evaluate(
498+
ceres::Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr));
499+
return cost;
500+
}
501+
502+
TEST(CeresRotationAverager, WhitensAddedRelativeRotationResidual) {
503+
const Eigen::Matrix3d cov = AnisotropicCovariance();
504+
RotationEstimatorOptions options;
505+
options.skip_initialization = true;
506+
options.refine_sensor_from_rig = false;
507+
options.ceres->loss_function_type = CeresLossFunctionType::TRIVIAL;
508+
509+
// Trivial frames use the fixed-size cost function.
510+
{
511+
Reconstruction reconstruction = MakeTrivialReconstruction({1, 2});
512+
reconstruction.Frame(2).RigFromWorld().rotation() =
513+
ZRotation(0.25) *
514+
Eigen::Quaterniond(Eigen::AngleAxisd(0.05, Eigen::Vector3d::UnitX()));
515+
const Eigen::Quaterniond prior = ZRotation(0.2);
516+
PoseGraph graph;
517+
graph.AddEdge(1, 2, Edge(prior));
518+
const Eigen::Vector3d residual =
519+
RelativeRotationResidual(prior, RelativeRotation(reconstruction, 1, 2));
520+
521+
auto averager =
522+
CreateDefaultCeresRotationAverager(options, graph, reconstruction);
523+
const double unwhitened = 0.5 * residual.squaredNorm();
524+
EXPECT_NEAR(EvaluateCost(*averager), unwhitened, 1e-12);
525+
averager->AddRelativeRotationResidual(1, 2, prior, nullptr, cov);
526+
EXPECT_NEAR(EvaluateCost(*averager),
527+
unwhitened + 0.5 * residual.dot(cov.ldlt().solve(residual)),
528+
1e-9);
529+
}
530+
531+
// Rig sensors use the dynamic cost function.
532+
{
533+
Reconstruction reconstruction = MakeRigReconstruction();
534+
const auto images = RigImages(reconstruction);
535+
const image_t image_id1 = images[0][1];
536+
const image_t image_id2 = images[1][0];
537+
const Eigen::Quaterniond estimate =
538+
RelativeRotation(reconstruction, image_id1, image_id2);
539+
const Eigen::Quaterniond prior =
540+
Eigen::Quaterniond(Eigen::AngleAxisd(0.03, Eigen::Vector3d::UnitY())) *
541+
estimate;
542+
PoseGraph graph;
543+
graph.AddEdge(image_id1, image_id2, Edge(prior));
544+
const Eigen::Vector3d residual = RelativeRotationResidual(prior, estimate);
545+
546+
auto averager =
547+
CreateDefaultCeresRotationAverager(options, graph, reconstruction);
548+
const double unwhitened = 0.5 * residual.squaredNorm();
549+
EXPECT_NEAR(EvaluateCost(*averager), unwhitened, 1e-12);
550+
averager->AddRelativeRotationResidual(
551+
image_id1, image_id2, prior, nullptr, cov);
552+
EXPECT_NEAR(EvaluateCost(*averager),
553+
unwhitened + 0.5 * residual.dot(cov.ldlt().solve(residual)),
554+
1e-9);
555+
}
556+
}
557+
481558
} // namespace
482559
} // namespace colmap

‎src/pycolmap/estimators/rotation_averaging_ceres.cc‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -53,8 +53,11 @@ void BindCeresRotationAverager(py::module& m) {
5353
"image_id2"_a,
5454
"cam2_from_cam1"_a,
5555
"loss"_a,
56+
"cam2_from_cam1_cov"_a = py::none(),
5657
"Add a relative-rotation residual between images whose frame and "
57-
"sensor rotations are configured in the problem.")
58+
"sensor rotations are configured in the problem. If given, the "
59+
"residual is whitened with the 3x3 covariance of the relative "
60+
"rotation, expressed in the frame of camera 1.")
5861
.def_property_readonly(
5962
"problem",
6063
py::overload_cast<>(&CeresRotationAverager::Problem),

‎src/pycolmap/estimators/rotation_averaging_ceres_test.py‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,14 @@ def Evaluate(self, squared_norm: float, rho: np.ndarray) -> None:
4949
image_id1, image_id2, pycolmap.Rigid3d().rotation, loss
5050
)
5151
assert averager.problem.num_residual_blocks() == 101
52+
averager.add_relative_rotation_residual(
53+
image_id1,
54+
image_id2,
55+
pycolmap.Rigid3d().rotation,
56+
loss,
57+
cam2_from_cam1_cov=np.eye(3),
58+
)
59+
assert averager.problem.num_residual_blocks() == 102
5260
del loss
5361
del reconstruction
5462
gc.collect()

0 commit comments

Comments
 (0)