Skip to content

Commit e81d808

Browse files
dellaertdellaert
authored
Merge pull request #2207 from borglab/feature/sl4
Add SL4 to geometry
2 parents a22b8db + 2b56701 commit e81d808

File tree

15 files changed

+889
-112
lines changed

15 files changed

+889
-112
lines changed

‎gtsam/geometry/Pose3.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ class GTSAM_EXPORT Pose3: public MatrixLieGroup<Pose3, 6, 4> {
111111
return Pose3();
112112
}
113113

114-
/// inverse transformation with derivatives
114+
/// inverse transformation
115115
Pose3 inverse() const;
116116

117117
/// compose syntactic sugar

‎gtsam/geometry/SL4.cpp‎

Lines changed: 167 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
/**
2+
* @file SL4.cpp
3+
* @brief Projective Special Linear Group (PSL(4, R)) Pose
4+
* @author: Hyungtae Lim
5+
*/
6+
7+
#include <gtsam/geometry/SL4.h>
8+
9+
// To use exp(), log()
10+
#include <unsupported/Eigen/MatrixFunctions>
11+
12+
using namespace std;
13+
14+
namespace {
15+
Eigen::Matrix<double, 15, 15> I_15x15 =
16+
Eigen::Matrix<double, 15, 15>::Identity();
17+
18+
Eigen::Matrix<double, 16, 15> setVecToAlgMatrix() {
19+
Eigen::Matrix<double, 16, 15> alg = Eigen::Matrix<double, 16, 15>::Zero();
20+
21+
// 12 Off-diagonal E_ij generators
22+
int k = 0;
23+
for (int i = 0; i < 4; ++i) {
24+
for (int j = 0; j < 4; ++j) {
25+
if (i != j) {
26+
alg(i * 4 + j, k++) = 1.0;
27+
}
28+
}
29+
}
30+
31+
// For Diagonal generators B1 = diag(1, -1, 0, 0)
32+
alg(0, 12) = 1.0;
33+
alg(5, 12) = -1.0;
34+
35+
// For B2 = diag(0, 1, -1, 0)
36+
alg(5, 13) = 1.0;
37+
alg(10, 13) = -1.0;
38+
39+
// For B3 = diag(0, 0, 1, -1)
40+
alg(10, 14) = 1.0;
41+
alg(15, 14) = -1.0;
42+
43+
return alg;
44+
}
45+
46+
Eigen::Matrix<double, 15, 16> setAlgtoVecMatrix() {
47+
Eigen::Matrix<double, 15, 16> mat;
48+
mat << 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
49+
1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.,
50+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
51+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
52+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
53+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
54+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
55+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
56+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
57+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
58+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 1., 0., 0., 0., 0., 0.,
59+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 1., 0., 0.,
60+
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
61+
0., 0., 0., 0., 0., -1.;
62+
return mat;
63+
}
64+
65+
// ALG_TO_VEC * VEC_TO_ALG is equals to I_15x15
66+
const Eigen::Matrix<double, 16, 15> VEC_TO_ALG = setVecToAlgMatrix();
67+
const Eigen::Matrix<double, 15, 16> ALG_TO_VEC = setAlgtoVecMatrix();
68+
} // namespace
69+
namespace gtsam {
70+
71+
SL4::SL4(const Matrix44& pose) {
72+
double det = pose.determinant();
73+
if (det <= 0.0) {
74+
throw std::runtime_error(
75+
"Matrix determinant must be positive for SL(4) normalization. Got det "
76+
"= " +
77+
std::to_string(det));
78+
}
79+
80+
T_ = pose / std::pow(det, 1.0 / 4.0);
81+
}
82+
83+
/* ************************************************************************* */
84+
void SL4::print(const std::string& s) const { cout << s << T_ << "\n"; }
85+
86+
/* ************************************************************************* */
87+
bool SL4::equals(const SL4& sl4, double tol) const {
88+
return T_.isApprox(sl4.T_, tol);
89+
}
90+
/* ************************************************************************* */
91+
SL4 SL4::ChartAtOrigin::Retract(const Vector15& v, ChartJacobian H) {
92+
assert(v.size() == 15);
93+
SL4 retracted(I_4x4 + Hat(v));
94+
if (H) throw std::runtime_error("SL4::Retract: Jacobian not implemented.");
95+
return retracted;
96+
}
97+
98+
/* ************************************************************************* */
99+
Vector15 SL4::ChartAtOrigin::Local(const SL4& sl4, ChartJacobian H) {
100+
Vector xi = Vee(sl4.T_ - I_4x4);
101+
if (H) throw std::runtime_error("SL4::Local: Jacobian not implemented.");
102+
return xi;
103+
}
104+
105+
/* ************************************************************************* */
106+
SL4 SL4::Expmap(const Vector& xi, SL4Jacobian H) {
107+
assert(xi.size() == 15);
108+
const auto& A = Hat(xi);
109+
110+
if (H) throw std::runtime_error("SL4::Expmap: Jacobian not implemented.");
111+
112+
// NOTE(hlim):
113+
// The cost of the computation is approximately 20n^3 for matrices of size n.
114+
// The number 20 depends weakly on the norm of the matrix.
115+
// See
116+
// https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html
117+
118+
return SL4(A.exp());
119+
}
120+
121+
/* ************************************************************************* */
122+
Vector SL4::Logmap(const SL4& p, SL4Jacobian H) {
123+
if (H) throw std::runtime_error("SL4::Logmap: Jacobian not implemented.");
124+
return Vee(p.T_.log());
125+
}
126+
127+
/* ************************************************************************* */
128+
Matrix15x15 SL4::AdjointMap() const {
129+
Matrix44 H_inv_T = T_.inverse().transpose();
130+
Matrix16x16 C_H;
131+
132+
// Kronecker product H ⊗ H^{-T}
133+
for (int i = 0; i < 4; ++i)
134+
for (int j = 0; j < 4; ++j)
135+
C_H.block<4, 4>(i * 4, j * 4) = T_(i, j) * H_inv_T;
136+
137+
return ALG_TO_VEC * C_H * VEC_TO_ALG;
138+
}
139+
140+
/* ************************************************************************* */
141+
Matrix44 SL4::Hat(const Vector& xi) {
142+
assert(xi.size() == 15);
143+
Matrix44 A;
144+
const double d11 = xi(12);
145+
const double d22 = -xi(12) + xi(13);
146+
const double d33 = -xi(13) + xi(14);
147+
const double d44 = -xi(14);
148+
149+
A << d11, xi(0), xi(1), xi(2), xi(3), d22, xi(4), xi(5), xi(6), xi(7), d33,
150+
xi(8), xi(9), xi(10), xi(11), d44;
151+
152+
return A;
153+
}
154+
155+
/* ************************************************************************* */
156+
// NOTE(hlim): Why 'X'? - I just follow the convention of GTSAM
157+
Vector SL4::Vee(const Matrix44& X) {
158+
Vector vec(15);
159+
const double x12 = X(0, 0);
160+
const double x13 = X(1, 1) + x12;
161+
const double x14 = -X(3, 3);
162+
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),
163+
X(2, 3), X(3, 0), X(3, 1), X(3, 2), x12, x13, x14;
164+
return vec;
165+
}
166+
167+
} // namespace gtsam

‎gtsam/geometry/SL4.h‎

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
/**
2+
* @file SL4.h
3+
* @brief Projective Special Linear Group (SL(4, R)) factor
4+
* @author: Hyungtae Lim
5+
*/
6+
7+
#pragma once
8+
9+
#include <gtsam/base/Matrix.h>
10+
#include <gtsam/base/MatrixLieGroup.h>
11+
#include <gtsam/base/OptionalJacobian.h>
12+
#include <gtsam/base/Vector.h>
13+
#include <gtsam/config.h>
14+
#include <gtsam/dllexport.h>
15+
16+
#if GTSAM_ENABLE_BOOST_SERIALIZATION
17+
#include <gtsam/base/MatrixSerialization.h>
18+
#endif
19+
20+
#include <string>
21+
22+
using SL4Jacobian = gtsam::OptionalJacobian<15, 15>;
23+
24+
using Matrix15x15 = Eigen::Matrix<double, 15, 15>;
25+
using Matrix16x16 = Eigen::Matrix<double, 16, 16>;
26+
27+
namespace gtsam {
28+
// NOTE(hlim): Strictly speaking, it should be expressed as SL(4, ℝ),
29+
// but for simplicity, we omit ℝ, assuming our target is over the real numbers.
30+
// And the variable `sl4` represents SL(4, ℝ).
31+
class GTSAM_EXPORT SL4 : public MatrixLieGroup<SL4, 15, 4> {
32+
public:
33+
static const size_t dimension = 15;
34+
35+
protected:
36+
Matrix44 T_;
37+
38+
public:
39+
/// @name Standard Constructors
40+
/// @{
41+
42+
/// Default constructor initializes at origin
43+
SL4() : T_(Matrix44::Identity()) {}
44+
45+
/// Copy constructor
46+
SL4(const Matrix44& pose);
47+
48+
SL4(const SL4& pose) = default;
49+
50+
SL4& operator=(const SL4& pose) = default;
51+
52+
/** print with optional string */
53+
void print(const std::string& s = "") const;
54+
55+
/** assert equality up to a tolerance */
56+
bool equals(const SL4& sl4, double tol = 1e-9) const;
57+
58+
/** convert to 4*4 matrix */
59+
inline const Matrix44& matrix() const { return T_; }
60+
61+
/// @}
62+
/// @name Group
63+
/// @{
64+
65+
/// identity for group operation
66+
static SL4 Identity() { return SL4(); }
67+
68+
/// inverse transformation
69+
SL4 inverse() const { return SL4(T_.inverse()); }
70+
71+
/// Group operation
72+
SL4 operator*(const SL4& other) const { return SL4(T_ * other.T_); }
73+
74+
/// @}
75+
/// @name Lie Group
76+
/// @{
77+
78+
// compose and between provided by LieGroup
79+
80+
/// Adjoint representation of the tangent space
81+
Matrix15x15 AdjointMap() const;
82+
83+
/// Version with derivative version added by LieGroup
84+
using LieGroup<SL4, 15>::inverse;
85+
86+
/// Exponential map at identity - create an element from canonical coordinates
87+
static SL4 Expmap(const Vector& xi, SL4Jacobian H = {});
88+
89+
/// Log map at identity - return the canonical coordinates of this element
90+
static Vector Logmap(const SL4& p, SL4Jacobian H = {});
91+
92+
// Chart at origin
93+
struct GTSAM_EXPORT ChartAtOrigin {
94+
static SL4 Retract(const Vector15& xi, ChartJacobian Hxi = {});
95+
static Vector15 Local(const SL4& pose, ChartJacobian Hpose = {});
96+
};
97+
98+
// retract and localCoordinates provided by LieGroup
99+
100+
/// @}
101+
/// @name Matrix Lie Group
102+
/// @{
103+
104+
using LieAlgebra = Matrix44;
105+
static Matrix44 Hat(const Vector& xi);
106+
static Vector Vee(const Matrix44& X);
107+
108+
/// @}
109+
110+
private:
111+
#if GTSAM_ENABLE_BOOST_SERIALIZATION
112+
/// Serialization function
113+
friend class boost::serialization::access;
114+
template <class Archive>
115+
void serialize(Archive& ar, const unsigned int /*version*/) {
116+
ar& BOOST_SERIALIZATION_NVP(T_);
117+
}
118+
#endif
119+
}; // \class SL4
120+
121+
template <>
122+
struct traits<SL4> : public internal::MatrixLieGroup<SL4, 4> {};
123+
124+
template <>
125+
struct traits<const SL4> : public internal::MatrixLieGroup<SL4, 4> {};
126+
127+
} // namespace gtsam

‎gtsam/geometry/geometry.i‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -634,6 +634,53 @@ class Pose3 {
634634
void serialize() const;
635635
};
636636

637+
#include <gtsam/geometry/SL4.h>
638+
class SL4 {
639+
// Standard constructors
640+
SL4();
641+
SL4(const gtsam::Matrix4& T);
642+
643+
// Testable
644+
void print(string s = "") const;
645+
bool equals(const gtsam::SL4& sl4, double tol) const;
646+
647+
// Group
648+
static gtsam::SL4 Identity();
649+
gtsam::SL4 inverse() const;
650+
gtsam::SL4 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
651+
gtsam::SL4 compose(const gtsam::SL4& sl4) const;
652+
gtsam::SL4 compose(const gtsam::SL4& sl4,
653+
Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
654+
gtsam::SL4 between(const gtsam::SL4& sl4) const;
655+
gtsam::SL4 between(const gtsam::SL4& sl4,
656+
Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
657+
658+
// Operator overload
659+
gtsam::SL4 operator*(const gtsam::SL4& sl4) const;
660+
661+
// Lie group
662+
static gtsam::SL4 Expmap(gtsam::Vector v);
663+
static gtsam::Vector Logmap(const gtsam::SL4& g);
664+
gtsam::SL4 expmap(gtsam::Vector v);
665+
gtsam::Vector logmap(const gtsam::SL4& g);
666+
static gtsam::Matrix Hat(const gtsam::Vector& xi);
667+
static gtsam::Vector Vee(const gtsam::Matrix& X);
668+
669+
// Manifold
670+
gtsam::SL4 retract(gtsam::Vector v,
671+
Eigen::Ref<Eigen::MatrixXd> Horigin,
672+
Eigen::Ref<Eigen::MatrixXd> Hv) const;
673+
gtsam::Vector localCoordinates(const gtsam::SL4& g,
674+
Eigen::Ref<Eigen::MatrixXd> Horigin,
675+
Eigen::Ref<Eigen::MatrixXd> Hp2) const;
676+
677+
// Interface
678+
gtsam::Matrix4 matrix() const;
679+
680+
// Serialization
681+
void serialize() const;
682+
};
683+
637684
// Used in Matlab wrapper
638685
class Pose3Pairs {
639686
Pose3Pairs();

‎gtsam/geometry/tests/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
# if GTSAM_ENABLE_BOOST_SERIALIZATION is OFF then exclude some tests
22
if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
3-
# create a semicolon seperated list of files to exclude
3+
# create a semicolon separated list of files to exclude
44
set(EXCLUDE_TESTS "testSerializationGeometry.cpp")
55
else()
66
set(EXCLUDE_TESTS "")

0 commit comments

Comments
 (0)