Skip to content

Commit 5b34831

Browse files
dellaertdellaert
authored
Merge pull request borglab#2184 from borglab/feature/Gal3_update
Add Gal3 wrapper and...
2 parents e683623 + a6ccabf commit 5b34831

File tree

8 files changed

+238
-11
lines changed

8 files changed

+238
-11
lines changed

‎.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,3 +23,4 @@ xcode/
2323

2424
# MyST build outputs
2525
_build
26+
GEMINI.md

‎gtsam/geometry/Gal3.cpp‎

Lines changed: 53 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -41,9 +41,6 @@ namespace gtsam {
4141
namespace { // Anonymous namespace for internal linkage
4242
constexpr double kSmallAngleThreshold = 1e-10;
4343

44-
// The type of the Lie algebra (matrix representation)
45-
using LieAlgebra = Matrix5;
46-
4744
// Helper functions for accessing tangent vector components
4845
Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
4946
Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
@@ -55,6 +52,45 @@ namespace { // Anonymous namespace for internal linkage
5552
Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
5653
Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
5754

55+
// The 25x10 matrix of vectorized generators for SGal(3).
56+
// The columns correspond to perturbations in [rho, nu, theta, t_tan].
57+
// The tangent vector is [rho(3), nu(3), theta(3), t_tan(1)].
58+
// P_gal3 = [vec(G_rho), vec(G_nu), vec(G_theta), vec(G_t)]
59+
static const Eigen::Matrix<double, 25, 10> P_gal3 =
60+
(Eigen::Matrix<double, 25, 10>() <<
61+
// rho(0-2), nu(3-5), theta(6-8), t_tan(9)
62+
// --- M(i,0) --- rows 0-4
63+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
64+
0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
65+
0, 0, 0, 0, 0, 0, 0, -1, 0, 0,
66+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
67+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
68+
// --- M(i,1) --- rows 5-9
69+
0, 0, 0, 0, 0, 0, 0, 0, -1, 0,
70+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
71+
0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
72+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
73+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
74+
// --- M(i,2) --- rows 10-14
75+
0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
76+
0, 0, 0, 0, 0, 0, -1, 0, 0, 0,
77+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
78+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
79+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
80+
// --- M(i,3) --- rows 15-19
81+
0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
82+
0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
83+
0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
84+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
85+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
86+
// --- M(i,4) --- rows 20-24
87+
1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
88+
0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
89+
0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
90+
0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
91+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
92+
).finished();
93+
5894
} // end anonymous namespace
5995

6096
//------------------------------------------------------------------------------
@@ -162,8 +198,6 @@ const double& Gal3::time(OptionalJacobian<1, 10> H) const {
162198
return t_;
163199
}
164200

165-
//------------------------------------------------------------------------------
166-
// Matrix Representation
167201
//------------------------------------------------------------------------------
168202
Matrix5 Gal3::matrix() const {
169203
// Returns 5x5 matrix representation as in Equation 9, Page 5
@@ -177,6 +211,20 @@ Matrix5 Gal3::matrix() const {
177211
return M;
178212
}
179213

214+
//------------------------------------------------------------------------------
215+
Vector25 Gal3::vec(OptionalJacobian<25, 10> H) const {
216+
const Matrix5 T = this->matrix();
217+
if (H) {
218+
// The Jacobian is given by the formula H = (I_5 ⊗ T) * P_gal3
219+
// where P_gal3 is the matrix of vectorized generators.
220+
// This can be implemented efficiently with block-wise multiplication.
221+
*H << T * P_gal3.block<5, 10>(0, 0), T* P_gal3.block<5, 10>(5, 0),
222+
T* P_gal3.block<5, 10>(10, 0), T* P_gal3.block<5, 10>(15, 0),
223+
T* P_gal3.block<5, 10>(20, 0);
224+
}
225+
return Eigen::Map<const Vector25>(T.data());
226+
}
227+
180228
//------------------------------------------------------------------------------
181229
// Stream operator
182230
//------------------------------------------------------------------------------

‎gtsam/geometry/Gal3.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,8 @@ using Vector10 = Eigen::Matrix<double, 10, 1>;
3737
using Matrix5 = Eigen::Matrix<double, 5, 5>;
3838
// Define Matrix10 for Jacobians
3939
using Matrix10 = Eigen::Matrix<double, 10, 10>;
40+
// Define Vector25 for vec() method
41+
using Vector25 = Eigen::Matrix<double, 25, 1>;
4042

4143
/**
4244
* Represents an element of the 3D Galilean group SGal(3).
@@ -50,6 +52,7 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
5052
double t_; ///< Time component
5153

5254
public:
55+
using LieAlgebra = Matrix5;
5356

5457
/// The dimension of the tangent space
5558
inline static constexpr size_t dimension = 10;
@@ -119,6 +122,9 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
119122
/// Return 5x5 homogeneous matrix representation
120123
Matrix5 matrix() const;
121124

125+
/// Vectorize 5x5 matrix into a 25-dim vector.
126+
Vector25 vec(OptionalJacobian<25, 10> H = {}) const;
127+
122128
/// @}
123129
/// @name Testable
124130
/// @{

‎gtsam/geometry/geometry.i‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1344,6 +1344,47 @@ class Similarity3 {
13441344
double scale() const;
13451345
};
13461346

1347+
#include <gtsam/geometry/Gal3.h>
1348+
class Gal3 {
1349+
// Standard Constructors
1350+
Gal3();
1351+
Gal3(const gtsam::Rot3& R, const gtsam::Point3& r, const gtsam::Vector3& v, double t);
1352+
Gal3(const gtsam::Matrix5& M);
1353+
1354+
// Testable
1355+
void print(string s = "") const;
1356+
bool equals(const gtsam::Gal3& other, double tol) const;
1357+
1358+
// Group
1359+
static gtsam::Gal3 Identity();
1360+
gtsam::Gal3 inverse() const;
1361+
gtsam::Gal3 compose(const gtsam::Gal3& other) const;
1362+
gtsam::Gal3 between(const gtsam::Gal3& other) const;
1363+
1364+
// Operator Overloads
1365+
gtsam::Gal3 operator*(const gtsam::Gal3& other) const;
1366+
1367+
// Lie Group
1368+
static gtsam::Gal3 Expmap(const gtsam::Vector10& xi);
1369+
static gtsam::Vector10 Logmap(const gtsam::Gal3& g);
1370+
gtsam::Gal3 expmap(const gtsam::Vector10& xi);
1371+
gtsam::Vector10 logmap(const gtsam::Gal3& g);
1372+
1373+
// Manifold
1374+
gtsam::Gal3 retract(const gtsam::Vector10& xi) const;
1375+
gtsam::Vector10 localCoordinates(const gtsam::Gal3& g) const;
1376+
1377+
// Component Access
1378+
const gtsam::Rot3& rotation() const;
1379+
const gtsam::Point3& translation() const;
1380+
const gtsam::Vector3& velocity() const;
1381+
const double& time() const;
1382+
1383+
// Other methods
1384+
gtsam::Matrix5 matrix() const;
1385+
gtsam::Vector25 vec() const;
1386+
};
1387+
13471388
template <T = {gtsam::PinholePoseCal3_S2}>
13481389
class CameraSet {
13491390
CameraSet();

‎gtsam/geometry/tests/testGal3.cpp‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1170,6 +1170,31 @@ TEST(Gal3, ExpLog_NearZero) {
11701170
EXPECT(assert_equal(xi_zero, Gal3::Logmap(Gal3::Expmap(xi_zero)), kTol));
11711171
}
11721172

1173+
//******************************************************************************
1174+
TEST(Gal3, vec) {
1175+
// Create a non-trivial Gal3 object
1176+
const Rot3 R_test = Rot3::Rodrigues(0.1, 0.2, 0.3);
1177+
const Point3 r_test(1.0, 2.0, 3.0);
1178+
const Velocity3 v_test(0.4, 0.5, 0.6);
1179+
const double t_test = 0.7;
1180+
const Gal3 gal3(R_test, r_test, v_test, t_test);
1181+
1182+
// 1. Test the Value
1183+
const Matrix5 T = gal3.matrix();
1184+
const Vector25 expected_vec = Eigen::Map<const Vector25>(T.data());
1185+
Vector25 actual_vec = gal3.vec();
1186+
EXPECT(assert_equal(expected_vec, actual_vec, 1e-9));
1187+
1188+
// 2. Test the Jacobian
1189+
Eigen::Matrix<double, 25, 10> H_actual;
1190+
gal3.vec(H_actual);
1191+
auto vec_fun = [](const Gal3& g) -> Vector25 {
1192+
return g.vec();
1193+
};
1194+
Matrix H_numerical = numericalDerivative11<Vector25, Gal3, 10>(vec_fun, gal3);
1195+
EXPECT(assert_equal(H_numerical, H_actual, 1e-7));
1196+
}
1197+
11731198
/* ************************************************************************* */
11741199
int main() {
11751200
TestResult tr;

‎gtsam/slam/slam.i‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ namespace gtsam {
99
#include <gtsam/navigation/ImuBias.h>
1010
#include <gtsam/geometry/Similarity2.h>
1111
#include <gtsam/geometry/Similarity3.h>
12+
#include <gtsam/geometry/Gal3.h>
1213

1314
// ######
1415

@@ -586,23 +587,23 @@ T FindKarcherMean(const std::vector<T>& elements);
586587
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
587588
size_t d);
588589

589-
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
590+
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Gal3}>
590591
class FrobeniusPrior : gtsam::NoiseModelFactor {
591592
FrobeniusPrior(gtsam::Key j, const gtsam::Matrix& M,
592593
const gtsam::noiseModel::Base* model);
593594

594595
gtsam::Vector evaluateError(const T& g) const;
595596
};
596597

597-
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
598+
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Gal3}>
598599
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
599600
FrobeniusFactor(gtsam::Key key1, gtsam::Key key2);
600601
FrobeniusFactor(gtsam::Key j1, gtsam::Key j2, gtsam::noiseModel::Base* model);
601602

602603
gtsam::Vector evaluateError(const T& T1, const T& T2);
603604
};
604605

605-
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
606+
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Gal3}>
606607
virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
607608
FrobeniusBetweenFactor(gtsam::Key j1, gtsam::Key j2, const T& T12);
608609
FrobeniusBetweenFactor(gtsam::Key key1, gtsam::Key key2, const T& T12,

‎gtsam/slam/tests/testFrobeniusFactor.cpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include <gtsam/geometry/Similarity3.h>
2828
#include <gtsam/geometry/SO3.h>
2929
#include <gtsam/geometry/SO4.h>
30+
#include <gtsam/geometry/Gal3.h>
3031
#include <gtsam/nonlinear/factorTesting.h>
3132
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
3233
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -338,6 +339,43 @@ TEST(FrobeniusBetweenFactorSimilarity3, evaluateError) {
338339
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
339340
}
340341

342+
/* ************************************************************************* */
343+
namespace gal3 {
344+
Gal3 id;
345+
Gal3 G1(Rot3::Rz(0.1), Point3(0.2, 0.3, 0.4), Velocity3(0.5, 0.6, 0.7), 0.8);
346+
Gal3 G2(Rot3::Rz(0.2), Point3(0.3, 0.4, 0.5), Velocity3(0.6, 0.7, 0.8), 0.9);
347+
} // namespace gal3
348+
349+
/* ************************************************************************* */
350+
TEST(FrobeniusFactorGal3, evaluateError) {
351+
using namespace ::gal3;
352+
auto factor = FrobeniusFactor<Gal3>(1, 2, noiseModel::Unit::Create(25));
353+
Vector actual = factor.evaluateError(G1, G2);
354+
Vector expected = G2.vec() - G1.vec();
355+
EXPECT(assert_equal(expected, actual, 1e-9));
356+
357+
Values values;
358+
values.insert(1, G1);
359+
values.insert(2, G2);
360+
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
361+
}
362+
363+
/* ************************************************************************* */
364+
TEST(FrobeniusBetweenFactorGal3, evaluateError) {
365+
using namespace ::gal3;
366+
auto factor = FrobeniusBetweenFactor<Gal3>(1, 2, G1.between(G2));
367+
Matrix H1, H2;
368+
Vector actual = factor.evaluateError(G1, G2, H1, H2);
369+
Vector expected(25);
370+
expected.setZero();
371+
EXPECT(assert_equal(expected, actual, 1e-9));
372+
373+
Values values;
374+
values.insert(1, G1);
375+
values.insert(2, G2);
376+
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
377+
}
378+
341379
/* ************************************************************************* */
342380
int main() {
343381
TestResult tr;

‎python/gtsam/tests/test_FrobeniusFactor.py‎

Lines changed: 70 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,40 @@
1212
import unittest
1313

1414
import numpy as np
15-
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
16-
ShonanFactor3, SOn)
15+
from gtsam import (
16+
Rot3,
17+
SO3,
18+
SO4,
19+
FrobeniusBetweenFactorSO4,
20+
FrobeniusFactorSO4,
21+
ShonanFactor3,
22+
SOn,
23+
Similarity2,
24+
Similarity3,
25+
FrobeniusFactorSimilarity2,
26+
FrobeniusBetweenFactorSimilarity2,
27+
FrobeniusFactorSimilarity3,
28+
FrobeniusBetweenFactorSimilarity3,
29+
Gal3,
30+
FrobeniusFactorGal3,
31+
FrobeniusBetweenFactorGal3,
32+
)
1733

1834
id = SO4()
1935
v1 = np.array([0, 0, 0, 0.1, 0, 0])
2036
Q1 = SO4.Expmap(v1)
2137
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
2238
Q2 = SO4.Expmap(v2)
2339

40+
P1_sim2 = Similarity2.Expmap(np.array([0.1, 0.2, 0.3, 0.4]))
41+
P2_sim2 = Similarity2.Expmap(np.array([0.2, 0.3, 0.4, 0.5]))
42+
43+
P1_sim3 = Similarity3.Expmap(np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]))
44+
P2_sim3 = Similarity3.Expmap(np.array([0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]))
45+
46+
G1_gal3 = Gal3(Rot3.Rz(0.1), np.array([0.2, 0.3, 0.4]), np.array([0.5, 0.6, 0.7]), 0.8)
47+
G2_gal3 = Gal3(Rot3.Rz(0.2), np.array([0.3, 0.4, 0.5]), np.array([0.6, 0.7, 0.8]), 0.9)
48+
2449

2550
class TestFrobeniusFactorSO4(unittest.TestCase):
2651
"""Test FrobeniusFactor factors."""
@@ -29,7 +54,7 @@ def test_frobenius_factor(self):
2954
"""Test creation of a factor that calculates the Frobenius norm."""
3055
factor = FrobeniusFactorSO4(1, 2)
3156
actual = factor.evaluateError(Q1, Q2)
32-
expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
57+
expected = (Q2.matrix() - Q1.matrix()).transpose().reshape((16,))
3358
np.testing.assert_array_equal(actual, expected)
3459

3560
def test_frobenius_between_factor(self):
@@ -52,5 +77,47 @@ def test_frobenius_wormhole_factor(self):
5277
np.testing.assert_array_almost_equal(actual, expected, decimal=4)
5378

5479

80+
class TestFrobeniusFactorSimilarity2(unittest.TestCase):
81+
def test_frobenius_factor(self):
82+
factor = FrobeniusFactorSimilarity2(1, 2)
83+
actual = factor.evaluateError(P1_sim2, P2_sim2)
84+
expected = (P2_sim2.matrix() - P1_sim2.matrix()).transpose().reshape((9,))
85+
np.testing.assert_allclose(actual, expected, atol=1e-9)
86+
87+
def test_frobenius_between_factor(self):
88+
factor = FrobeniusBetweenFactorSimilarity2(1, 2, P1_sim2.between(P2_sim2))
89+
actual = factor.evaluateError(P1_sim2, P2_sim2)
90+
expected = np.zeros((9,))
91+
np.testing.assert_allclose(actual, expected, atol=1e-9)
92+
93+
94+
class TestFrobeniusFactorSimilarity3(unittest.TestCase):
95+
def test_frobenius_factor(self):
96+
factor = FrobeniusFactorSimilarity3(1, 2)
97+
actual = factor.evaluateError(P1_sim3, P2_sim3)
98+
expected = (P2_sim3.matrix() - P1_sim3.matrix()).transpose().reshape((16,))
99+
np.testing.assert_allclose(actual, expected, atol=1e-9)
100+
101+
def test_frobenius_between_factor(self):
102+
factor = FrobeniusBetweenFactorSimilarity3(1, 2, P1_sim3.between(P2_sim3))
103+
actual = factor.evaluateError(P1_sim3, P2_sim3)
104+
expected = np.zeros((16,))
105+
np.testing.assert_allclose(actual, expected, atol=1e-9)
106+
107+
108+
class TestFrobeniusFactorGal3(unittest.TestCase):
109+
def test_frobenius_factor(self):
110+
factor = FrobeniusFactorGal3(1, 2)
111+
actual = factor.evaluateError(G1_gal3, G2_gal3)
112+
expected = (G2_gal3.matrix() - G1_gal3.matrix()).transpose().reshape((25,))
113+
np.testing.assert_allclose(actual, expected, atol=1e-9)
114+
115+
def test_frobenius_between_factor(self):
116+
factor = FrobeniusBetweenFactorGal3(1, 2, G1_gal3.between(G2_gal3))
117+
actual = factor.evaluateError(G1_gal3, G2_gal3)
118+
expected = np.zeros((25,))
119+
np.testing.assert_allclose(actual, expected, atol=1e-9)
120+
121+
55122
if __name__ == "__main__":
56123
unittest.main()

0 commit comments

Comments
 (0)