8000 Add cost function for 3D alignment (with covariance) by B1ueber2y · Pull Request #2621 · colmap/colmap · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Add cost function for 3D alignment (with covariance) #2621

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom
Merged
merged 27 commits into from
Jul 28, 2024
Merged

Add cost function for 3D alignment (with covariance) #2621

merged 27 commits into from
Jul 28, 2024

Conversation

B1ueber2y
Copy link
Contributor
@B1ueber2y B1ueber2y commented Jun 28, 2024 •
edited
Loading

Cost function for 3D alignment with similarity transform, with covariance on the target 3D point/location prior.

Also with the following side updates:

  1. Split ceres manifolds to a stand-alone header
  2. update the scale binding for Rigid3d to support numpy array, such that we can fix (i.e. set constant) or parameterize it with pyceres
  3. Add a new manifold with exponential mapping to ensure the value to be positive, motivated by the common practice for paraemeterizing the scale parameters. This is installed to pycolmap when pyceres is detected in the Python environment.

@B1ueber2y B1ueber2y requested review from sarlinpe and ahojnnes June 28, 2024 16:45
sarlinpe
sarlinpe reviewed Jun 28, 2024
View reviewed changes
@B1ueber2y B1ueber2y marked this pull request as draft June 29, 2024 13:56
sarlinpe
sarlinpe reviewed Jun 29, 2024
View reviewed changes
@B1ueber2y
Copy link
Contributor Author
B1ueber2y commented Jun 29, 2024 •
edited
Loading

Python minimal test for the PositiveExponentialManifold:

import numpy as np
import pyceres
import pycolmap

def optimize_scale(sim3, scale_prior):
    prob = pyceres.Problem()
    prob.add_residual_block(
        pyceres.factors.NormalPrior(scale_prior, np.eye(1)),
        pyceres.TrivialLoss(),
        [sim3.scale],
    )
    prob.set_manifold(
        sim3.scale, pycolmap.manifold.PositiveExponentialManifold(1)
    )
    options = pyceres.SolverOptions()
    summary = pyceres.SolverSummary()
    pyceres.solve(options, prob, summary)


sim3d = pycolmap.Sim3d()
optimize_scale(sim3d, np.array([5.0]))
np.testing.assert_allclose(sim3d.scale, np.array([5.0]), atol=1e-5)
optimize_scale(sim3d, np.array([-1.0]))
np.testing.assert_allclose(sim3d.scale, np.array([0.0]), atol=1e-5)

Python minimal test for Point3dAlignmentCost on top of some variable points and pycolmap.Sim3d:

import numpy as np
import pyceres
import pycolmap
import copy

sim3 = pycolmap.Sim3d()
target_sim3 = pycolmap.Sim3d(
    2.0, np.array([0.0, 0.0, 0.0, 1.0]), np.array([2.0, 3.0, 4.0])
)
ref_points = [np.array([k, k, k]).astype(float) for k in range(10)]

prob = pyceres.Problem()
points = []
for i, ref_point in enumerate(ref_points):
    if i < 3:
        point = ref_point + np.ones(3) * 0.5  # add noise
    else:
        point = ref_point
    points.append(point)
    target_point = target_sim3 * ref_point
    cost = pycolmap.cost_functions.Point3dAlignmentCost(target_point, np.eye(3))
    prob.add_residual_block(
        cost,
        pyceres.TrivialLoss(),
        [points[i], sim3.rotation.quat, sim3.translation, sim3.scale],
    )
    if i >= 3:
        prob.set_parameter_block_constant(ref_points[i])
prob.set_manifold(sim3.rotation.quat, pyceres.QuaternionManifold())
prob.set_manifold(sim3.scale, pycolmap.manifold.PositiveExponentialManifold(1))
options = pyceres.SolverOptions()
summary = pyceres.SolverSummary()
pyceres.solve(options, prob, summary)

np.testing.assert_allclose(points[0], ref_points[0], atol=1e-5)
np.testing.assert_allclose(points[1], ref_points[1], atol=1e-5)
np.testing.assert_allclose(points[2], ref_points[2], atol=1e-5)
np.testing.assert_allclose(sim3.scale, target_sim3.scale, atol=1e-5)
np.testing.assert_allclose(
    sim3.rotation.quat, target_sim3.rotation.quat, atol=1e-5
)
np.testing.assert_allclose(sim3.translation, target_sim3.translation, atol=1e-5)

@B1ueber2y B1ueber2y marked this pull request as ready for review June 29, 2024 22:57
@B1ueber2y B1ueber2y marked this pull request as draft June 29, 2024 23:04
sarlinpe
sarlinpe reviewed Jun 30, 2024
View reviewed changes
@B1ueber2y B1ueber2y marked this pull request as ready for review June 30, 2024 09:15
@B1ueber2y
Copy link
Contributor Author

I have updated the description for this PR according to the new changes. PTAL when time permits : ) @ahojnnes

ahojnnes
ahojnnes reviewed Jul 21, 2024
View reviewed changes
ahojnnes
ahojnnes reviewed Jul 21, 2024
View reviewed changes
@B1ueber2y B1ueber2y merged commit 03556bb into colmap:main Jul 28, 2024
16 checks passed
@B1ueber2y B1ueber2y deleted the features/alignment_cost branch July 28, 2024 07:47
@B1ueber2y B1ueber2y mentioned this pull request Jul 31, 2024
Merged
@B1ueber2y
Copy link
Contributor Author
B1ueber2y commented Jul 31, 2024 •
edited
Loading

This feature is actually broken with wheel installation (only works with build from source) after 42ed7bc, potentially due to #2632 and #2634. Details and fixes redirected here: cvg/pyceres#52

bo-rc pushed a commit to bo-rc/colmap that referenced this pull request Aug 15, 2024
@B1ueber2y @bo-rc
Add cost function for 3D alignment (with covariance) (colmap#2621)
ebe88f6 
* add impl.

* add pybind

* add tests.

* update.

* update.

* refactor. separate the manifold file. Add PositiveExponentialManifold

* formatting.

* update.

* add scale_ptr for sim3d pybind.

* fix.

* fix.

* update.

* fix binding for sim3d.scale.

* update.

* formatting.

* test pyceres.

* test.

* update.

* move pyceres checker to pycolmap/helpers.h + minor.

* sjfkdjs for ci.

* for ci.

* minor.

* minor.
@BrewTestBot BrewTestBot mentioned this pull request Nov 28, 2024
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@ahojnnes ahojnnes ahojnnes approved these changes

@sarlinpe sarlinpe sarlinpe approved these changes

4B7C
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@B1ueber2y @ahojnnes @sarlinpe
0