Skip to content

Commit 9f9ba85

Browse files
dellaertdellaert
committed
Address review comments
1 parent fc993a2 commit 9f9ba85

File tree

3 files changed

+31
-46
lines changed

3 files changed

+31
-46
lines changed

‎gtsam/geometry/ExtendedPose3-inl.h‎

Lines changed: 9 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -40,32 +40,20 @@ void ExtendedPose3<K, Derived>::ZeroJacobian(ChartJacobian H, Eigen::Index d) {
4040
}
4141
}
4242

43-
template <int K, class Derived>
44-
template <int K_, typename>
45-
ExtendedPose3<K, Derived>::ExtendedPose3()
46-
: R_(Rot3::Identity()), t_(Matrix3K::Zero()) {}
47-
48-
template <int K, class Derived>
49-
template <int K_, typename>
50-
ExtendedPose3<K, Derived>::ExtendedPose3(size_t k)
51-
: R_(Rot3::Identity()), t_(3, static_cast<Eigen::Index>(k)) {
52-
t_.setZero();
53-
}
54-
5543
template <int K, class Derived>
5644
ExtendedPose3<K, Derived>::ExtendedPose3(const Rot3& R, const Matrix3K& x)
5745
: R_(R), t_(x) {}
5846

5947
template <int K, class Derived>
60-
ExtendedPose3<K, Derived>::ExtendedPose3(const LieAlgebra& T) {
48+
ExtendedPose3<K, Derived>::ExtendedPose3(const MatrixRep& T) {
6149
const Eigen::Index n = T.rows();
6250
if constexpr (K == Eigen::Dynamic) {
6351
if (T.cols() != n || n < 3) {
6452
throw std::invalid_argument("ExtendedPose3: invalid matrix shape.");
6553
}
6654
t_.resize(3, n - 3);
6755
} else {
68-
if (n != matrix_dim || T.cols() != matrix_dim) {
56+
if (n != matrixDim || T.cols() != matrixDim) {
6957
throw std::invalid_argument("ExtendedPose3: invalid matrix shape.");
7058
}
7159
}
@@ -125,19 +113,6 @@ bool ExtendedPose3<K, Derived>::equals(const ExtendedPose3& other,
125113
return R_.equals(other.R_, tol) && equal_with_abs_tol(t_, other.t_, tol);
126114
}
127115

128-
template <int K, class Derived>
129-
template <int K_, typename>
130-
typename ExtendedPose3<K, Derived>::This ExtendedPose3<K, Derived>::Identity() {
131-
return MakeReturn(ExtendedPose3());
132-
}
133-
134-
template <int K, class Derived>
135-
template <int K_, typename>
136-
typename ExtendedPose3<K, Derived>::This ExtendedPose3<K, Derived>::Identity(
137-
size_t k) {
138-
return MakeReturn(ExtendedPose3(k));
139-
}
140-
141116
template <int K, class Derived>
142117
typename ExtendedPose3<K, Derived>::This ExtendedPose3<K, Derived>::inverse()
143118
const {
@@ -386,15 +361,15 @@ ExtendedPose3<K, Derived>::ChartAtOrigin::Local(const This& pose,
386361
}
387362

388363
template <int K, class Derived>
389-
typename ExtendedPose3<K, Derived>::LieAlgebra
364+
typename ExtendedPose3<K, Derived>::MatrixRep
390365
ExtendedPose3<K, Derived>::matrix() const {
391-
LieAlgebra M;
392-
if constexpr (matrix_dim == Eigen::Dynamic) {
366+
MatrixRep M;
367+
if constexpr (matrixDim == Eigen::Dynamic) {
393368
const Eigen::Index k = static_cast<Eigen::Index>(this->k());
394369
const Eigen::Index n = 3 + k;
395-
M = LieAlgebra::Identity(n, n);
370+
M = MatrixRep::Identity(n, n);
396371
} else {
397-
M = LieAlgebra::Identity();
372+
M = MatrixRep::Identity();
398373
}
399374
M.template block<3, 3>(0, 0) = R_.matrix();
400375
M.block(0, 3, 3, static_cast<Eigen::Index>(this->k())) = t_;
@@ -406,7 +381,7 @@ typename ExtendedPose3<K, Derived>::LieAlgebra ExtendedPose3<K, Derived>::Hat(
406381
const TangentVector& xi) {
407382
const Eigen::Index k = static_cast<Eigen::Index>(RuntimeK(xi));
408383
LieAlgebra X;
409-
if constexpr (matrix_dim == Eigen::Dynamic) {
384+
if constexpr (matrixDim == Eigen::Dynamic) {
410385
X.setZero(3 + k, 3 + k);
411386
} else {
412387
X.setZero();
@@ -430,7 +405,7 @@ ExtendedPose3<K, Derived>::Vee(const LieAlgebra& X) {
430405
if constexpr (K == Eigen::Dynamic) {
431406
return X.cols() - 3;
432407
} else {
433-
if (X.rows() != matrix_dim) {
408+
if (X.rows() != matrixDim) {
434409
throw std::invalid_argument(
435410
"ExtendedPose3::Vee: invalid matrix shape.");
436411
}

‎gtsam/geometry/ExtendedPose3.h‎

Lines changed: 21 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -56,18 +56,21 @@ class ExtendedPose3
5656
ExtendedPose3<K, void>, Derived>;
5757
inline constexpr static int dimension =
5858
(K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K;
59-
inline constexpr static int matrix_dim =
59+
inline constexpr static int matrixDim =
6060
(K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K;
6161

62-
using Base = MatrixLieGroup<This, dimension, matrix_dim>;
62+
using Base = MatrixLieGroup<This, dimension, matrixDim>;
6363
using TangentVector = typename Base::TangentVector;
6464
using Jacobian = typename Base::Jacobian;
6565
using ChartJacobian = typename Base::ChartJacobian;
6666
using ComponentJacobian =
6767
std::conditional_t<dimension == Eigen::Dynamic,
6868
OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>,
6969
OptionalJacobian<3, dimension>>;
70-
using LieAlgebra = Eigen::Matrix<double, matrix_dim, matrix_dim>;
70+
/// Homogeneous matrix representation in the group.
71+
using MatrixRep = Eigen::Matrix<double, matrixDim, matrixDim>;
72+
/// Lie algebra matrix type used by Hat/Vee.
73+
using LieAlgebra = Eigen::Matrix<double, matrixDim, matrixDim>;
7174
using Matrix3K = Eigen::Matrix<double, 3, K>;
7275

7376
static_assert(K == Eigen::Dynamic || K >= 1,
@@ -93,7 +96,7 @@ class ExtendedPose3
9396
* The manifold dimension is 3+3k and matrix size is (3+k)x(3+k).
9497
*/
9598
template <int K_ = K, typename = IsFixed<K_>>
96-
ExtendedPose3();
99+
ExtendedPose3() : R_(Rot3::Identity()), t_(Matrix3K::Zero()) {}
97100

98101
/**
99102
* Construct a dynamic-size identity element.
@@ -103,7 +106,10 @@ class ExtendedPose3
103106
* The manifold dimension is 3+3k and matrix size is (3+k)x(3+k).
104107
*/
105108
template <int K_ = K, typename = IsDynamic<K_>>
106-
explicit ExtendedPose3(size_t k = 0);
109+
explicit ExtendedPose3(size_t k = 0)
110+
: R_(Rot3::Identity()), t_(3, static_cast<Eigen::Index>(k)) {
111+
t_.setZero();
112+
}
107113

108114
/** Copy constructor. */
109115
ExtendedPose3(const ExtendedPose3&) = default;
@@ -125,7 +131,7 @@ class ExtendedPose3
125131
* @param T Homogeneous matrix in R^((3+k)x(3+k)).
126132
* Top-left 3x3 is R, top-right 3xk stores x_1..x_k.
127133
*/
128-
explicit ExtendedPose3(const LieAlgebra& T);
134+
explicit ExtendedPose3(const MatrixRep& T);
129135

130136
/// @}
131137
/// @name Access
@@ -206,7 +212,9 @@ class ExtendedPose3
206212
* @return Identity with manifold dimension 3+3k and matrix size 3+k.
207213
*/
208214
template <int K_ = K, typename = IsFixed<K_>>
209-
static This Identity();
215+
static This Identity() {
216+
return MakeReturn(ExtendedPose3());
217+
}
210218

211219
/**
212220
* Identity element for dynamic-size K.
@@ -215,7 +223,9 @@ class ExtendedPose3
215223
* @return Identity with manifold dimension 3+3k and matrix size 3+k.
216224
*/
217225
template <int K_ = K, typename = IsDynamic<K_>>
218-
static This Identity(size_t k = 0);
226+
static This Identity(size_t k = 0) {
227+
return MakeReturn(ExtendedPose3(k));
228+
}
219229

220230
/**
221231
* Group inverse.
@@ -348,7 +358,7 @@ class ExtendedPose3
348358
*
349359
* @return Matrix in R^((3+k)x(3+k)).
350360
*/
351-
LieAlgebra matrix() const;
361+
MatrixRep matrix() const;
352362

353363
/**
354364
* Hat operator from tangent to Lie algebra.
@@ -411,12 +421,12 @@ using ExtendedPose3Dynamic = ExtendedPose3<Eigen::Dynamic>;
411421
template <int K, class Derived>
412422
struct traits<ExtendedPose3<K, Derived>>
413423
: public internal::MatrixLieGroup<ExtendedPose3<K, Derived>,
414-
ExtendedPose3<K, Derived>::matrix_dim> {};
424+
ExtendedPose3<K, Derived>::matrixDim> {};
415425

416426
template <int K, class Derived>
417427
struct traits<const ExtendedPose3<K, Derived>>
418428
: public internal::MatrixLieGroup<ExtendedPose3<K, Derived>,
419-
ExtendedPose3<K, Derived>::matrix_dim> {};
429+
ExtendedPose3<K, Derived>::matrixDim> {};
420430

421431
} // namespace gtsam
422432

‎gtsam/navigation/tests/testNavState.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -592,7 +592,7 @@ TEST(NavState, Adjoint_compose_full) {
592592
}
593593

594594
/* ************************************************************************* */
595-
TEST(NavState, expmaps_galore_full) {
595+
TEST(NavState, ExpmapsGaloreFull) {
596596
Vector xi;
597597
NavState actual;
598598
xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();

0 commit comments

Comments
 (0)