Covariance estimation for bundle adjustment with Schur elimination #2610

B1ueber2y · 2024-06-21T18:57:33Z

This PR contains the covariance estimation for bundle adjustment with handcrafted Schur elimination, which has been forever missing from the ceres solver. Also, the interface for world_from_cam to cam_from_world conversion for the 6x6 covariance is provided with the adjoint feature from #2598

This is a practically extremely useful feature since the ceres covariance computation will generally fail for large-scale bundle adjustment problem mainly due to poorly conditioned 3D points. Many of these points generally cannot be easily identified with heuristics such as triangulation angles, making the ceres covariance computation intractable. Here are some examples of this long standing issue in the ceres development thread (seems that they stop developing this feature):

https://groups.google.com/g/ceres-solver/c/719fVUZqOZk
https://groups.google.com/g/ceres-solver/c/1SrLa1Pr8D4

With this new feature we will be able to reliably extract covariance from the bundle adjustment problem as long as the camera poses are well constrained (i.e. without Gauge ambiguity), even with poorly conditioned points. The covariance computation will also be faster with the use of Schur elimination.

Here is a minimal testing example in Python (If we feel necessary I can also try to write a unit test in C++). To be able to run the test one only needs a bundle adjusted COLMAP SfM model without rank deficiency. I am using the SfM model produced by the pycolmap example from here: https://github.com/colmap/colmap/blob/main/pycolmap/example.py

import pycolmap
import pyceres
import numpy as np

# setup bundle adjustment
recon = pycolmap.Reconstruction("example/sfm/0")
ba_options = pycolmap.BundleAdjustmentOptions()
ba_config = pycolmap.BundleAdjustmentConfig()
for img_id in recon.images:
    ba_config.add_image(img_id)
bundle_adjuster = pycolmap.BundleAdjuster(ba_options, ba_config)
bundle_adjuster.set_up_problem(recon, pyceres.TrivialLoss())
problem = bundle_adjuster.problem 

# if needed we can add more constraints here (e.g. PGO, control points).

# constrain problem (must-do for visual-only SfM to remove gauge)
image1_id, image2_id = 1, 2
problem.set_parameter_block_constant(recon.images[image1_id].cam_from_world.rotation.quat)
problem.set_parameter_block_constant(recon.images[image1_id].cam_from_world.translation)
problem.set_manifold(recon.images[image2_id].cam_from_world.translation, pyceres.SubsetManifold(3, np.array([0])))

# solve
solver_options = pyceres.SolverOptions()
summary = pyceres.SolverSummary()
pyceres.solve(solver_options, problem, summary)

# [TEST] covariance: direct interface
covs = pycolmap.estimate_pose_covariance_from_ba(problem, recon)
covs_from_ceres = pycolmap.estimate_pose_covariance_from_ba_ceres_backend(bundle_adjuster.problem, recon)
for img_id, _ in covs.items():
    assert covs[img_id].shape[0] == 5 if img_id == image2_id else 6 
    np.testing.assert_allclose(covs[img_id], covs_from_ceres[img_id], atol=1e-5)

# [TEST] cross-image covariance: pybind classes
cov_estimator = pycolmap.BundleAdjustmentCovarianceEstimator(problem, recon)
cov_estimator.compute()
cov_estimator_ceres = pycolmap.BundleAdjustmentCovarianceEstimatorCeresBackend(problem, recon)
cov_estimator_ceres.compute()
for i in np.arange(2, 10): # in this example model image_ids are from 1 to 11.
    cov = cov_estimator.get_pose_covariance(i, i + 1)
    cov_from_ceres = cov_estimator_ceres.get_pose_covariance(i, i + 1)
    np.testing.assert_allclose(cov, cov_from_ceres, atol=1e-5)

# [TEST] multi-image covariance
cov = cov_estimator.get_pose_covariance([3, 5, 7, 8])
cov_from_ceres = cov_estimator.get_pose_covariance([3, 5, 7, 8])
assert cov.shape[0] == 24 # 6 * 4
np.testing.assert_allclose(cov, cov_from_ceres, atol=1e-5)

# [TEST] covariance on intrinsics
cov_estimator.compute_full()
cov_estimator_ceres.compute_full()
for camera_id in recon.cameras:
    params = recon.cameras[camera_id].params
    cov_params = cov_estimator.get_covariance(params)
    assert cov_params.shape[0] == 2 # (f, k1)
    cov_params_from_ceres = cov_estimator.get_covariance(params)
    np.testing.assert_allclose(cov_params, cov_params_from_ceres, atol=1e-5)

sarlinpe · 2024-06-21T23:00:29Z

Very nice, thanks! I will review this in more details on Monday.

B1ueber2y · 2024-06-22T11:03:12Z

Let me restructure this now to support cross-pose covariance and also covariance for other parameters.

B1ueber2y · 2024-06-22T15:12:17Z

New feature arised from the new update:

cross-image covariance
covariance on other variables (except for points)
error handling for rank deficient jacobian.

The testing example is updated in the PR description to incorporate tests for the first two features in the new update.

src/pycolmap/estimators/covariance.cc

src/colmap/estimators/covariance.cc

src/pycolmap/estimators/covariance.cc

src/colmap/estimators/covariance.cc

ahojnnes · 2024-06-26T11:05:30Z

src/colmap/estimators/covariance.cc

+  return true;
+}
+
+bool BundleAdjustmentCovarianceEstimatorCeresBackend::Compute() {


Would it make sense to call the two methods "ComputePoseCov" and "ComputePoseAndPointCov" ?

The first method is to indeed compute pose covariance, but the second is to compute the covariance of all variables except for the points, as described in the header, and I felt that ComputePoseAndOtherVariablesCov is too long here.

src/colmap/estimators/covariance.cc

src/colmap/estimators/covariance_test.cc

ahojnnes · 2024-06-26T15:31:07Z

Thanks, great work.

cdcseacave · 2025-05-19T14:19:30Z

@B1ueber2y thank you for this contribution. I am trying to port most of your math into my custom SfM pipeline for estimating cov for poses, and it works great for normal scenes; however for some scenes it either fails or the cov is inf, and I am not sure why. These seems to happen in scenes where there are some poses not well constrained, and I can figure out the exact cause: weak connections (constraints) for these poses, or something else. The failure is in "Simplicial LDLT for computing L_inv failed" with "NumericalIssue if the matrix.appears to be negative."
Any idea how to solve this? I do not know the math for this part well enough.

B1ueber2y · 2025-05-21T16:36:56Z

@B1ueber2y thank you for this contribution. I am trying to port most of your math into my custom SfM pipeline for estimating cov for poses, and it works great for normal scenes; however for some scenes it either fails or the cov is inf, and I am not sure why. These seems to happen in scenes where there are some poses not well constrained, and I can figure out the exact cause: weak connections (constraints) for these poses, or something else. The failure is in "Simplicial LDLT for computing L_inv failed" with "NumericalIssue if the matrix.appears to be negative." Any idea how to solve this? I do not know the math for this part well enough.

Hey. For those poses that are not well constrained, it is possible that there is rank deficiency (or large condition number) when inverting the Hessian, resulting in failures. This actually makes sense since the covariance on certain direction would be infinite, and is not useful to inspect for downstream applications. You would need to either remove the ambiguity by adding additional constraints, or resort to more advanced techniques to deal with the nullspace (e.g., https://dspace.cvut.cz/bitstream/handle/10467/108057/F3-D-2023-Polic-Michal-Polic%20-%20disertacni%20prace.pdf). Thanks!

cdcseacave · 2025-05-22T09:05:01Z

@B1ueber2y Makes sens, thank you. Is it possible to detect only which poses are not well constrained? That in itself would be a valuable info to have for the SfM

B1ueber2y · 2025-05-23T07:17:23Z

@B1ueber2y Makes sens, thank you. Is it possible to detect only which poses are not well constrained? That in itself would be a valuable info to have for the SfM

The rank deficiency may not be specific to one pose, but can be a global behavior. For example, if the Gauge is not constrained, all the poses can be ambiguous up to a similarity transformation, but the relative poses are well constrained.

cdcseacave · 2025-05-23T14:03:09Z

Gauge is constrained on all degrees of freedom (first camera pose is constant), except explicit lock on scale (though it is constrained too indirectly by the relative pose loss)

B1ueber2y added 7 commits June 21, 2024 19:25

add impl.

81287eb

formatting.

4658739

update.

e16deca

update damping factor to 1e-6.

6141b48

update.

18fb734

fix log.

045b732

update.

e81e53e

B1ueber2y requested review from ahojnnes, mihaidusmanu and sarlinpe June 21, 2024 18:58

B1ueber2y added 8 commits June 21, 2024 21:05

update.

66a4ff1

use macro to adapt old ceres convention.

2bcaa24

formatting.

063df95

update.

d49caf0

ddd.

1863d8d

formatting.

8f6add1

minor improvement on the performance

5eca966

Merge branch 'main' into features/covariance_schur

b75f144

B1ueber2y marked this pull request as draft June 22, 2024 10:54

restructure to support cross-image covariance.

5ae8db7

B1ueber2y added 4 commits June 22, 2024 17:21

adapt to different eigen version.

5e023c7

support all eigen version.

af31b38

update.

6e87a94

fsjsdklf.

36c70f9

B1ueber2y marked this pull request as ready for review June 22, 2024 15:45

B1ueber2y added 2 commits June 22, 2024 18:21

minor.

d0e60ea

minor.

1188acd

sarlinpe reviewed Jun 25, 2024

View reviewed changes

src/pycolmap/estimators/covariance.cc Outdated Show resolved Hide resolved

sarlinpe reviewed Jun 25, 2024

View reviewed changes

src/colmap/estimators/covariance.cc Outdated Show resolved Hide resolved

B1ueber2y added 3 commits June 26, 2024 00:36

update.

d7b21cd

update.

696f3b9

formatting.

0a43d2b

sarlinpe reviewed Jun 26, 2024

View reviewed changes

src/pycolmap/estimators/covariance.cc Outdated Show resolved Hide resolved

sarlinpe reviewed Jun 26, 2024

View reviewed changes

src/pycolmap/estimators/covariance.cc Outdated Show resolved Hide resolved

B1ueber2y added 2 commits June 26, 2024 09:53

update.

41f4097

update.

fe93e52

sarlinpe approved these changes Jun 26, 2024

View reviewed changes

ahojnnes reviewed Jun 26, 2024

View reviewed changes

src/colmap/estimators/covariance.cc Outdated Show resolved Hide resolved

ahojnnes reviewed Jun 26, 2024

View reviewed changes

src/colmap/estimators/covariance.cc Outdated Show resolved Hide resolved

ahojnnes reviewed Jun 26, 2024

View reviewed changes

src/colmap/estimators/covariance.cc Outdated Show resolved Hide resolved

ahojnnes reviewed Jun 26, 2024

View reviewed changes

src/colmap/estimators/covariance.cc Outdated Show resolved Hide resolved

ahojnnes reviewed Jun 26, 2024

View reviewed changes

src/colmap/estimators/covariance.cc Outdated Show resolved Hide resolved

ahojnnes reviewed Jun 26, 2024

View reviewed changes

src/colmap/estimators/covariance_test.cc Outdated Show resolved Hide resolved

B1ueber2y added 4 commits June 26, 2024 13:25

update.

8b5034f

fix test.

7d80bb7

formatting.

c1212fd

update.

2f20a31

ahojnnes merged commit 0a1ae65 into colmap:main Jun 26, 2024

B1ueber2y deleted the features/covariance_schur branch June 26, 2024 15:31

B1ueber2y mentioned this pull request Jul 2, 2024

Faster covariance computation for small blocks #2633

Merged

Covariance estimation for bundle adjustment with Schur elimination #2610

Covariance estimation for bundle adjustment with Schur elimination #2610

Uh oh!

Conversation

B1ueber2y commented Jun 21, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

sarlinpe commented Jun 21, 2024

Uh oh!

B1ueber2y commented Jun 22, 2024

Uh oh!

B1ueber2y commented Jun 22, 2024

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

ahojnnes Jun 26, 2024

Choose a reason for hiding this comment

Uh oh!

B1ueber2y Jun 26, 2024

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

ahojnnes commented Jun 26, 2024

Uh oh!

cdcseacave commented May 19, 2025

Uh oh!

B1ueber2y commented May 21, 2025

Uh oh!

cdcseacave commented May 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

B1ueber2y commented May 23, 2025

Uh oh!

cdcseacave commented May 23, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

B1ueber2y commented Jun 21, 2024 •

edited

Loading

cdcseacave commented May 22, 2025 •

edited

Loading