Extended Pose SE_k(3)

[dellaert]

A major refactor that brings in $SE_k(3)\doteq SO(3) \ltimes (\mathbb{R}^3)^k$ and eliminates a lot of duplicate code in Pose3 and NavState. In addition, $SE_k(3)$ can now be used in invariant filter implementations. A selection with $k={2, 3, 4, 6}$ is exposed in the wrapper as well.

What changed

• Added a new generic ExtendedPose3<K, Derived> in geometry, supporting:
  • fixed K >= 1
  • dynamic K = Eigen::Dynamic (runtime k constructor)
• Implemented exactly the required Manifold / LieGroup / MatrixLieGroup surface.
• Used CRTP (Derived defaulted) so derived classes keep concrete return types (compose, inverse, Expmap, etc.) without heavy wrappers.
• Refactored:
  • Pose3 -> ExtendedPose3<1, Pose3>
  • NavState -> ExtendedPose3<2, NavState>
  • Removed duplicated implementations now provided by base.
• Updated docs:
  • ExtendedPose3 notebook with Lie-group math intro, Colab preamble tags, and Python examples
  • updated .github/copilot-instructions.md notebook conventions.

Why this is better

• Single implementation for SE_k(3) math across Pose3, NavState, and future k.

Tests run

C++

• make -j6 testExtendedPose3.run ✅
• make -j6 testPose3.run ✅
• make -j6 testNavState.run ✅

Wrapper/build

• make -j6 install ✅
• make -j6 python-install ✅
• make -j6 python-test ✅

[dellaert]

\bench

[dellaert]

/bench

[ProfFan]

[dellaert]

/bench

[dellaert]

/bench

[dellaert]

/bench

[ProfFan]

Result is cached, errrrgh, I should add a command to force re-bench

[dellaert]

I don't trust the Mac benchmarks - probably was run on a different machine from cached, @ProfFan and I discussed a fix to benchmark flow (ps are we running that in release!? numbers below are much better, on an M1, with TBB). Here are my results:

New:

Dataset	Legacy (Cholesky) s	New (Solver) s	Speedup
dubrovnik-16-22106-pre.txt	1.424	0.873	1.631x
dubrovnik-88-64298-pre.txt	9.052	4.845	1.868x
dubrovnik-135-90642-pre.txt	10.072	5.755	1.750x

Old:

Dataset	Legacy (Cholesky) s	New (Solver) s	Speedup
dubrovnik-16-22106-pre.txt	1.248	0.676	1.847x
dubrovnik-88-64298-pre.txt	9.812	4.035	2.432x
dubrovnik-135-90642-pre.txt	11.023	6.256	1.762x

So I think there is no regression and this can be merged. The Pose3-specific Expmap was a bit faster than without, although we should have much stronger benchmarks to really conclude that...

[varunagrawal]

Did a first pass and there is some weird naming that raised some flags.

I am really liking the de-duplication induced, however, I am a bit skeptical about the need to support Eigen::Dynamic since I haven't found any use of $SE_k(3)$ with $k > 6$.

Once I get a response on the naming comments, I can finish looking at the rest of the gtsam/geometry/ExtendedPose3-inl.h file and approve.

[varunagrawal]

I believe this looks good.