SOT(3) Implementation - eqVIO 1/3#2448
Conversation
|
|
||
| /// Group multiplication: (R1,c1)*(R2,c2) = (R1*R2, c1*c2). | ||
| SOT3 operator*(const SOT3& other) const { | ||
| return SOT3(R_ * other.R_, c_ * other.c_); |
There was a problem hiding this comment.
If this is true, is SOT3 then not just a product group? :-)
It could be that we need things in the EqVio filter that they are not provided by product group, but if not, it's just a typedef :-/
There was a problem hiding this comment.
Could still be the case that we need to extend it, in which case we need to convert the product group to a CRPT, as I did for ExtendedPose3
There was a problem hiding this comment.
I also noticed that it can be represented as a product group, but thought it might be beneficial to stay faithful to van Goor's SOT(3) implementation from his library just in case it is needed down the road during my port.
That being said, maybe I can club this implementation together with the VIOGroup implementation, at which point I will know for sure whether it needs to be extended?
There was a problem hiding this comment.
Well, how is Van Goor's implementation different from product group? Will read answer in the morning :-)
There was a problem hiding this comment.
You are correct that the group itself is mathematically a product group.
What I meant by staying faithful to LiePP was that it is not implemented as a composition of groups, but as a dedicated type that exposes wedge, vee, asMatrix, adjoint as all explicit 4x4 matrix operations.
In order to expose those same functions in a product group setting, correct me if I am wrong, but I assume we would have to make SOT(3) a wrapper around ProductLieGroup<SO3, double> and manually define them.
However, after tracing through the code, I am also noticing now that wedge and vee are only defined for the purposes of van Goor's library, and aren't used for the rest of the eqVIO implementation. Maybe in this case it would make more sense to just define it as a product group in GTSAM.
There was a problem hiding this comment.
If we need those, 2 ways to go: CRTP+extend, or actually define ProductMatrixLieGroup - let’s chat offline. Try with ProductLieGroup for now.
There was a problem hiding this comment.
Pull request overview
Adds the SOT3 (Scaled Orthogonal Transform) Lie group to gtsam/geometry as a building block for the upcoming eqVIO implementation, along with a dedicated unit test suite.
Changes:
- Introduces
gtsam::SOT3, aMatrixLieGrouprepresentingSO(3) × R_{>0}with 4×4 block-diagonal matrix form. - Implements core Lie-group operations (Expmap/Logmap, Hat/Vee, AdjointMap, group ops, and action on
R^3). - Adds
CppUnitLitetests covering group/manifold invariants and Jacobians via numerical differentiation.
Reviewed changes
Copilot reviewed 3 out of 3 changed files in this pull request and generated 3 comments.
| File | Description |
|---|---|
| gtsam/geometry/SOT3.h | Declares the SOT3 public API and Lie-group interface. |
| gtsam/geometry/SOT3.cpp | Implements SOT3 operations (matrix form, Expmap/Logmap, adjoint, etc.). |
| gtsam/geometry/tests/testSOT3.cpp | Adds unit tests for algebra/group operations, invariants, and Jacobians. |
| private: | ||
| SO3 R_; ///< Rotation component R_Q in SO(3). | ||
| double c_; ///< Scale component c_Q > 0. | ||
| }; |
There was a problem hiding this comment.
Most geometry Lie-group types in this repo provide Boost-serialization support behind #if GTSAM_ENABLE_BOOST_SERIALIZATION (e.g., Pose2 has a serialize() method in gtsam/geometry/Pose2.h:335-343, Similarity3 in gtsam/geometry/Similarity3.h:222-231). SOT3 currently has no serialization path, which can limit interoperability with existing serialization utilities/tests. Consider adding a serialize() method (for R_ and c_) and, if appropriate, extending testSerializationGeometry.cpp to cover SOT3.
| /// Construct from rotation and positive scale. | ||
| SOT3(const SO3& R, double c) : R_(R), c_(c) {} | ||
|
|
There was a problem hiding this comment.
SOT3 is defined with c>0, but the public constructor currently accepts any c (including 0/negative). This allows creating invalid group elements that will later cause undefined results (e.g., inverse() divides by 0 and Logmap() takes log(c_)). Consider enforcing the invariant at construction time (e.g., throw std::invalid_argument if c <= 0) and/or guarding in inverse()/Logmap() to fail fast with a clear error.
| explicit SOT3(const MatrixNN& M) | ||
| : R_(SO3(M.topLeftCorner<3, 3>())), c_(M(3, 3)) {} | ||
|
|
There was a problem hiding this comment.
The 4x4-matrix constructor assumes M is exactly [[R,0],[0,c]] but does not validate the zero blocks or that c is positive. This can silently accept arbitrary matrices and produce an invalid SOT3 instance. Suggest adding structural checks (similar to Gal3::Gal3(const Matrix5&) throwing std::invalid_argument for malformed matrices) and rejecting M(3,3) <= 0.
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
We will likely close after chatting more
|
Closed, new revisions in separate PR. |
Hello!
This is PR
1/3as pertaining to the eqVIO implementation in GTSAM!Changes:
MatrixLieGroup, asSOT3.handSOT3.cppingtsam/geometrygtsam/geometry/testsContent of this group was adapted from LiePP's (van Goor's lie library) implementation of SOT(3).
As a refresher, here is the definition:
Here is the PR roadmap for the eqVIO implementation:
VIOGroupimplementation, derived fromProductGroupandPowerGroupEqVIOFilterimplementation