Skip to content
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.

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

Faster SE(3) and SE_2(3) exponential maps #1979

Merged
merged 6 commits into from
Jan 16, 2025
Merged

Conversation

dellaert
Copy link
Member

@dellaert dellaert commented Jan 15, 2025

DexpFunctor was instantiated several times:

  • for computing R via SO3::Expmap
  • for ExmapTranslation, twice so in NavState

This PR

  • gets rid of ExpmapTranslation and inlines it in the 4 places it was used;
  • in the Pose3 and Navstate Expmaps, we also now use the one instantiated DexpFunctor to compute the rotation;
  • according to Barfoot Eq 8.83, X = J_r * inv(J_l) = R^T, so substitute that wherever we used X before: another big saving!
  • in addition, I realized why R^T: it's the jacobian of Pose3::Create !

This PR saves a bunch of compute (many sines and cosines, and computation of skew W and W*W) and is more "transparent" about what is going on. In addition, the X=R^T saves even more on top of that.

@dellaert dellaert requested a review from ProfFan January 15, 2025 05:21
@dellaert
Copy link
Member Author

@timbarfoot I was inspired to look at this code more critically because of you :-)

gtsam/geometry/Pose3.cpp Show resolved Hide resolved
@dellaert dellaert merged commit 61f2fdd into develop Jan 16, 2025
33 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants