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

Open
wants to merge 4 commits into
base: develop
Choose a base branch
from

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!

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 :-)

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.

1 participant