Skip to content

switch Hat/Vee to orthonormal sl(4) basis and update adjoint/tests #2360

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

Merged
dellaert merged 3 commits into from
Jan 19, 2026
Merged

switch Hat/Vee to orthonormal sl(4) basis and update adjoint/tests #2360

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
aa5e42b
switch Hat/Vee to orthonormal sl(4) basis and update adjoint/tests
yluo5820 Jan 17, 2026
80ac1ff
refactor Hat/Vee to use SO4 helpers and symmetric split
yluo5820 Jan 19, 2026
a32dfc1
add more documentation
yluo5820 Jan 19, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
169 changes: 112 additions & 57 deletions gtsam/geometry/SL4.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
*/

#include <gtsam/geometry/SL4.h>
#include <gtsam/geometry/SO4.h>

// To use exp(), log()
#include <cmath>
Expand All @@ -15,59 +16,106 @@
using namespace std;

namespace {
Eigen::Matrix<double, 15, 15> I_15x15 =
Eigen::Matrix<double, 15, 15>::Identity();
using gtsam::Matrix44;
using gtsam::Vector6;

Eigen::Matrix<double, 16, 15> setVecToAlgMatrix() {
Eigen::Matrix<double, 16, 15> alg = Eigen::Matrix<double, 16, 15>::Zero();
constexpr double kInvSqrt2 = 0.7071067811865475244;
constexpr double kInvSqrt6 = 0.4082482904638630164;
constexpr double kInvSqrt12 = 0.2886751345948128823;

// 12 Off-diagonal E_ij generators
int k = 0;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
if (i != j) {
alg(i * 4 + j, k++) = 1.0;
}
}
}
Vector6 SkewToSO4(const Vector6& r) {
Vector6 so4;
so4 << r(5), -r(4), r(2), -r(3), r(1), -r(0);
return so4;
}

// For Diagonal generators B1 = diag(1, -1, 0, 0)
alg(0, 12) = 1.0;
alg(5, 12) = -1.0;
Vector6 SO4ToSkew(const Vector6& so4) {
Vector6 r;
r << -so4(5), so4(4), so4(2), -so4(3), -so4(1), so4(0);
return r;
}

// For B2 = diag(0, 1, -1, 0)
alg(5, 13) = 1.0;
alg(10, 13) = -1.0;
Matrix44 HatSym4(const Vector6& s) {
Matrix44 A = Matrix44::Zero();
A(0, 1) = s(0);
A(1, 0) = s(0);
A(0, 2) = s(1);
A(2, 0) = s(1);
A(0, 3) = s(2);
A(3, 0) = s(2);
A(1, 2) = s(3);
A(2, 1) = s(3);
A(1, 3) = s(4);
A(3, 1) = s(4);
A(2, 3) = s(5);
A(3, 2) = s(5);
return A;
}

// For B3 = diag(0, 0, 1, -1)
alg(10, 14) = 1.0;
alg(15, 14) = -1.0;
Vector6 VeeSym4(const Matrix44& A) {
Vector6 s;
s << A(0, 1), A(0, 2), A(0, 3), A(1, 2), A(1, 3), A(2, 3);
return s;
}

Eigen::Matrix<double, 16, 15> setVecToAlgMatrix() {
Eigen::Matrix<double, 16, 15> alg = Eigen::Matrix<double, 16, 15>::Zero();

int k = 0;
auto set_skew = [&](int i, int j) {
alg(i * 4 + j, k) = kInvSqrt2;
alg(j * 4 + i, k) = -kInvSqrt2;
++k;
};
auto set_sym = [&](int i, int j) {
alg(i * 4 + j, k) = kInvSqrt2;
alg(j * 4 + i, k) = kInvSqrt2;
++k;
};

// Rotations (skew-symmetric).
set_skew(0, 1);
set_skew(0, 2);
set_skew(0, 3);
set_skew(1, 2);
set_skew(1, 3);
set_skew(2, 3);

// Symmetric off-diagonal shears.
set_sym(0, 1);
set_sym(0, 2);
set_sym(0, 3);
set_sym(1, 2);
set_sym(1, 3);
set_sym(2, 3);

// Traceless diagonal scalings.
alg(0, k) = kInvSqrt2;
alg(5, k) = -kInvSqrt2;
++k;

alg(0, k) = kInvSqrt6;
alg(5, k) = kInvSqrt6;
alg(10, k) = -2.0 * kInvSqrt6;
++k;

alg(0, k) = kInvSqrt12;
alg(5, k) = kInvSqrt12;
alg(10, k) = kInvSqrt12;
alg(15, k) = -3.0 * kInvSqrt12;
++k;

return alg;
}

Eigen::Matrix<double, 15, 16> setAlgtoVecMatrix() {
Eigen::Matrix<double, 15, 16> mat;
mat << 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 1., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., -1.;
return mat;
Eigen::Matrix<double, 15, 16> setAlgtoVecMatrix(
const Eigen::Matrix<double, 16, 15>& vec_to_alg) {
return vec_to_alg.transpose();
}

// ALG_TO_VEC * VEC_TO_ALG is equals to I_15x15
// For the orthonormal basis, ALG_TO_VEC * VEC_TO_ALG is the identity.
const Eigen::Matrix<double, 16, 15> VEC_TO_ALG = setVecToAlgMatrix();
const Eigen::Matrix<double, 15, 16> ALG_TO_VEC = setAlgtoVecMatrix();
const Eigen::Matrix<double, 15, 16> ALG_TO_VEC = setAlgtoVecMatrix(VEC_TO_ALG);

} // namespace
namespace gtsam {
Expand Down Expand Up @@ -185,28 +233,35 @@ Matrix44 SL4::Hat(const Vector& xi) {
"SL4::Hat: xi must be a vector of size 15. Got size " +
std::to_string(xi.size()));
}
Matrix44 A;
const double d11 = xi(12);
const double d22 = -xi(12) + xi(13);
const double d33 = -xi(13) + xi(14);
const double d44 = -xi(14);
const Vector6 r = kInvSqrt2 * xi.head<6>();
const Vector6 s = kInvSqrt2 * xi.segment<6>(6);

Matrix44 A = SO4::Hat(SkewToSO4(r)) + HatSym4(s);

A << d11, xi(0), xi(1), xi(2), xi(3), d22, xi(4), xi(5), xi(6), xi(7), d33,
xi(8), xi(9), xi(10), xi(11), d44;
// Traceless diagonal scalings.
const double a = kInvSqrt2 * xi(12);
const double b = kInvSqrt6 * xi(13);
const double c = kInvSqrt12 * xi(14);

Vector4 diag;
diag << a + b + c, -a + b + c, -2.0 * b + c, -3.0 * c;
A.diagonal() += diag;

return A;
}

/* ************************************************************************* */
// NOTE(hlim): Why 'X'? - I just follow the convention of GTSAM
Vector SL4::Vee(const Matrix44& X) {
Vector vec(15);
const double x12 = X(0, 0);
const double x13 = X(1, 1) + x12;
const double x14 = -X(3, 3);
vec << X(0, 1), X(0, 2), X(0, 3), X(1, 0), X(1, 2), X(1, 3), X(2, 0), X(2, 1),
X(2, 3), X(3, 0), X(3, 1), X(3, 2), x12, x13, x14;
return vec;
// Used consistent notation with Hat()
Vector SL4::Vee(const Matrix44& A) {
Vector xi(15);
const Matrix44 skew = A - A.transpose();
const Matrix44 sym = A + A.transpose();
xi.head<6>() = kInvSqrt2 * SO4ToSkew(SO4::Vee(skew));
xi.segment<6>(6) = kInvSqrt2 * VeeSym4(sym);
xi(12) = kInvSqrt2 * (A(0, 0) - A(1, 1));
xi(13) = kInvSqrt6 * (A(0, 0) + A(1, 1) - 2.0 * A(2, 2));
xi(14) = kInvSqrt12 * (A(0, 0) + A(1, 1) + A(2, 2) - 3.0 * A(3, 3));
return xi;
}

} // namespace gtsam
19 changes: 19 additions & 0 deletions gtsam/geometry/SL4.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,25 @@ class GTSAM_EXPORT SL4 : public MatrixLieGroup<SL4, 15, 4> {
/// @{

using LieAlgebra = Matrix44;
/**
* Lie algebra coordinates for sl(4) using an orthonormal basis.
*
* We use the orthogonal vector-space decomposition:
* sl(4) = so(4) ⊕ sym_off(4) ⊕ diag_traceless(4)
* where so(4) is skew-symmetric rotations, sym_off is symmetric off-diagonal
* shears, and diag_traceless is the 3-D traceless diagonal subspace.
*
* Ordering of xi (15x1):
* [r12 r13 r14 r23 r24 r34 s12 s13 s14 s23 s24 s34 h1 h2 h3]
*
* Basis:
* - r_ij scale (E_ij - E_ji)/sqrt(2): skew-symmetric rotations.
* - s_ij scale (E_ij + E_ji)/sqrt(2): symmetric off-diagonal shears.
* - h1,h2,h3 scale orthonormal traceless diagonals:
* H1 = (1/sqrt(2)) diag( 1, -1, 0, 0)
* H2 = (1/sqrt(6)) diag( 1, 1, -2, 0)
* H3 = (1/sqrt(12)) diag( 1, 1, 1, -3)
*/
static Matrix44 Hat(const Vector& xi);
static Vector Vee(const Matrix44& X);

Expand Down
92 changes: 91 additions & 1 deletion gtsam/geometry/tests/testSL4.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SL4.h>

#include <cmath>
#include <vector>

using namespace std;
using namespace gtsam;

Expand Down Expand Up @@ -171,9 +174,96 @@ TEST(SL4, HatVeeAreInverses) {
EXPECT(assert_equal(xi0, xi_recovered, 1e-8));
}

/* ************************************************************************* */
TEST(SL4, HatTraceIsZero) {
Vector15 eta =
(Vector15() << 0.31, -0.22, 0.14, -0.09, 0.27, -0.18, 0.05, -0.12, 0.08,
0.11, -0.06, 0.02, 0.07, -0.04, 0.13)
.finished();
Matrix4 A = SL4::Hat(eta);
EXPECT_DOUBLES_EQUAL(0.0, A.trace(), 1e-12);
}

/* ************************************************************************* */
TEST(SL4, HatVeeRoundTrip) {
Vector15 eta =
(Vector15() << -0.25, 0.19, -0.11, 0.07, -0.03, 0.29, -0.17, 0.09, 0.21,
-0.13, 0.04, -0.06, 0.15, -0.08, 0.02)
.finished();
Vector eta_recovered = SL4::Vee(SL4::Hat(eta));
EXPECT(assert_equal(eta, eta_recovered, 1e-12));
}

/* ************************************************************************* */
TEST(SL4, HatBasisIsOrthonormal) {
std::vector<Matrix4> generators;
generators.reserve(15);

for (int k = 0; k < 15; ++k) {
Vector15 e = Vector15::Zero();
e(k) = 1.0;
generators.push_back(SL4::Hat(e));
}

for (int i = 0; i < 15; ++i) {
for (int j = 0; j < 15; ++j) {
const double inner =
(generators[i].array() * generators[j].array()).sum();
const double expected = (i == j) ? 1.0 : 0.0;
EXPECT_DOUBLES_EQUAL(expected, inner, 1e-12);
}
}
}

/* ************************************************************************* */
TEST(SL4, HatMatchesOrthonormalDefinition) {
const double inv_sqrt2 = 1.0 / std::sqrt(2.0);
const double inv_sqrt6 = 1.0 / std::sqrt(6.0);
const double inv_sqrt12 = 1.0 / std::sqrt(12.0);

const Vector15 eta = xi0;
Matrix4 expected = Matrix4::Zero();

expected(0, 1) += inv_sqrt2 * eta(0);
expected(1, 0) -= inv_sqrt2 * eta(0);
expected(0, 2) += inv_sqrt2 * eta(1);
expected(2, 0) -= inv_sqrt2 * eta(1);
expected(0, 3) += inv_sqrt2 * eta(2);
expected(3, 0) -= inv_sqrt2 * eta(2);
expected(1, 2) += inv_sqrt2 * eta(3);
expected(2, 1) -= inv_sqrt2 * eta(3);
expected(1, 3) += inv_sqrt2 * eta(4);
expected(3, 1) -= inv_sqrt2 * eta(4);
expected(2, 3) += inv_sqrt2 * eta(5);
expected(3, 2) -= inv_sqrt2 * eta(5);

expected(0, 1) += inv_sqrt2 * eta(6);
expected(1, 0) += inv_sqrt2 * eta(6);
expected(0, 2) += inv_sqrt2 * eta(7);
expected(2, 0) += inv_sqrt2 * eta(7);
expected(0, 3) += inv_sqrt2 * eta(8);
expected(3, 0) += inv_sqrt2 * eta(8);
expected(1, 2) += inv_sqrt2 * eta(9);
expected(2, 1) += inv_sqrt2 * eta(9);
expected(1, 3) += inv_sqrt2 * eta(10);
expected(3, 1) += inv_sqrt2 * eta(10);
expected(2, 3) += inv_sqrt2 * eta(11);
expected(3, 2) += inv_sqrt2 * eta(11);

const double a = inv_sqrt2 * eta(12);
const double b = inv_sqrt6 * eta(13);
const double c = inv_sqrt12 * eta(14);
expected(0, 0) = a + b + c;
expected(1, 1) = -a + b + c;
expected(2, 2) = -2.0 * b + c;
expected(3, 3) = -3.0 * c;

EXPECT(assert_equal(expected, SL4::Hat(eta), 1e-12));
}

/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
/* ************************************************************************* */
Loading