Skip to content

Commit 13cc819

Browse files
dellaertdellaert
committed
transposes
1 parent 9f9ba85 commit 13cc819

File tree

7 files changed

+173
-109
lines changed

7 files changed

+173
-109
lines changed

‎gtsam/geometry/ExtendedPose3-inl.h‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -250,6 +250,35 @@ ExtendedPose3<K, Derived>::Adjoint(const TangentVector& xi_b,
250250
return Ad * xi_b;
251251
}
252252

253+
template <int K, class Derived>
254+
typename ExtendedPose3<K, Derived>::TangentVector
255+
ExtendedPose3<K, Derived>::AdjointTranspose(const TangentVector& x,
256+
ChartJacobian H_this,
257+
ChartJacobian H_x) const {
258+
const Jacobian Ad = AdjointMap();
259+
const TangentVector AdTx = Ad.transpose() * x;
260+
261+
if (H_this) {
262+
if constexpr (dimension == Eigen::Dynamic) {
263+
H_this->setZero(x.size(), x.size());
264+
} else {
265+
H_this->setZero();
266+
}
267+
268+
H_this->block(0, 0, 3, 3) = skewSymmetric(AdTx.template head<3>());
269+
const Eigen::Index k = static_cast<Eigen::Index>(this->k());
270+
for (Eigen::Index i = 0; i < k; ++i) {
271+
const Eigen::Index idx = 3 + 3 * i;
272+
const Matrix3 x_i_hat = skewSymmetric(AdTx.template segment<3>(idx));
273+
H_this->block(0, idx, 3, 3) = x_i_hat;
274+
H_this->block(idx, 0, 3, 3) = x_i_hat;
275+
}
276+
}
277+
278+
if (H_x) *H_x = Ad.transpose();
279+
return AdTx;
280+
}
281+
253282
template <int K, class Derived>
254283
typename ExtendedPose3<K, Derived>::Jacobian
255284
ExtendedPose3<K, Derived>::adjointMap(const TangentVector& xi) {
@@ -301,6 +330,34 @@ ExtendedPose3<K, Derived>::adjoint(const TangentVector& xi,
301330
return ad_xi * y;
302331
}
303332

333+
template <int K, class Derived>
334+
typename ExtendedPose3<K, Derived>::TangentVector
335+
ExtendedPose3<K, Derived>::adjointTranspose(const TangentVector& xi,
336+
const TangentVector& y,
337+
ChartJacobian Hxi,
338+
ChartJacobian H_y) {
339+
if (Hxi) {
340+
if constexpr (dimension == Eigen::Dynamic) {
341+
Hxi->setZero(xi.size(), xi.size());
342+
} else {
343+
Hxi->setZero();
344+
}
345+
for (Eigen::Index i = 0; i < xi.size(); ++i) {
346+
TangentVector dxi;
347+
if constexpr (dimension == Eigen::Dynamic) {
348+
dxi = TangentVector::Zero(xi.size());
349+
} else {
350+
dxi = TangentVector::Zero();
351+
}
352+
dxi(i) = 1.0;
353+
Hxi->col(i) = adjointMap(dxi).transpose() * y;
354+
}
355+
}
356+
const Jacobian adT_xi = adjointMap(xi).transpose();
357+
if (H_y) *H_y = adT_xi;
358+
return adT_xi * y;
359+
}
360+
304361
template <int K, class Derived>
305362
typename ExtendedPose3<K, Derived>::Jacobian
306363
ExtendedPose3<K, Derived>::ExpmapDerivative(const TangentVector& xi) {

‎gtsam/geometry/ExtendedPose3.h‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -282,6 +282,18 @@ class ExtendedPose3
282282
TangentVector Adjoint(const TangentVector& xi_b, ChartJacobian H_this = {},
283283
ChartJacobian H_xib = {}) const;
284284

285+
/**
286+
* Dual adjoint action on a tangent covector.
287+
*
288+
* @param x Tangent vector in R^dim.
289+
* @param H_this Optional Jacobian in R^(dimxdim).
290+
* @param H_x Optional Jacobian in R^(dimxdim).
291+
* @return Ad_X^T x in R^dim.
292+
*/
293+
TangentVector AdjointTranspose(const TangentVector& x,
294+
ChartJacobian H_this = {},
295+
ChartJacobian H_x = {}) const;
296+
285297
/**
286298
* Lie algebra adjoint map.
287299
*
@@ -302,6 +314,20 @@ class ExtendedPose3
302314
static TangentVector adjoint(const TangentVector& xi, const TangentVector& y,
303315
ChartJacobian Hxi = {}, ChartJacobian H_y = {});
304316

317+
/**
318+
* Dual Lie bracket action ad_xi^T(y).
319+
*
320+
* @param xi Tangent vector in R^dim.
321+
* @param y Tangent vector in R^dim.
322+
* @param Hxi Optional Jacobian in R^(dimxdim).
323+
* @param H_y Optional Jacobian in R^(dimxdim).
324+
* @return ad_xi^T(y) in R^dim.
325+
*/
326+
static TangentVector adjointTranspose(const TangentVector& xi,
327+
const TangentVector& y,
328+
ChartJacobian Hxi = {},
329+
ChartJacobian H_y = {});
330+
305331
/**
306332
* Jacobian of Expmap.
307333
*

‎gtsam/geometry/Pose3.cpp‎

Lines changed: 0 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -57,46 +57,6 @@ Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
5757
return Pose3(p);
5858
}
5959

60-
/* ************************************************************************* */
61-
/// The dual version of Adjoint
62-
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
63-
OptionalJacobian<6, 6> H_x) const {
64-
const Matrix6 Ad = AdjointMap();
65-
const Vector6 AdTx = Ad.transpose() * x;
66-
67-
// Jacobians
68-
// See docs/math.pdf for more details.
69-
if (H_pose) {
70-
const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
71-
v_T_hat = skewSymmetric(AdTx.tail<3>());
72-
*H_pose << w_T_hat, v_T_hat, //
73-
/* */ v_T_hat, Z_3x3;
74-
}
75-
if (H_x) {
76-
*H_x = Ad.transpose();
77-
}
78-
79-
return AdTx;
80-
}
81-
82-
/* ************************************************************************* */
83-
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
84-
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
85-
if (Hxi) {
86-
Hxi->setZero();
87-
for (int i = 0; i < 6; ++i) {
88-
Vector6 dxi;
89-
dxi.setZero();
90-
dxi(i) = 1.0;
91-
Matrix6 GTi = adjointMap(dxi).transpose();
92-
Hxi->col(i) = GTi * y;
93-
}
94-
}
95-
const Matrix6& adT_xi = adjointMap(xi).transpose();
96-
if (H_y) *H_y = adT_xi;
97-
return adT_xi * y;
98-
}
99-
10060
/* ************************************************************************* */
10161
void Pose3::print(const std::string& s) const {
10262
std::cout << (s.empty() ? s : s + " ") << *this << std::endl;

‎gtsam/geometry/Pose3.h‎

Lines changed: 0 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -140,22 +140,10 @@ class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> {
140140
/// Exponential map at identity.
141141
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
142142

143-
/// The dual version of Adjoint
144-
Vector6 AdjointTranspose(const Vector6& x,
145-
OptionalJacobian<6, 6> H_this = {},
146-
OptionalJacobian<6, 6> H_x = {}) const;
147-
148143
// temporary fix for wrappers until case issue is resolved
149144
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
150145
static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
151146

152-
/**
153-
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
154-
*/
155-
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
156-
OptionalJacobian<6, 6> Hxi = {},
157-
OptionalJacobian<6, 6> H_y = {});
158-
159147
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
160148
struct GTSAM_EXPORT ChartAtOrigin {
161149
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});

‎gtsam/geometry/tests/testExtendedPose3.cpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121
#include <gtsam/base/numericalDerivative.h>
2222
#include <gtsam/geometry/ExtendedPose3.h>
2323

24+
#include <functional>
25+
2426
using namespace gtsam;
2527

2628
using ExtendedPose33 = ExtendedPose3<3>;
@@ -158,6 +160,93 @@ TEST(ExtendedPose3, AdjointConsistency) {
158160
EXPECT(assert_equal(d_generic, d.AdjointMap(), 1e-9));
159161
}
160162

163+
//******************************************************************************
164+
TEST(ExtendedPose3, AdjointTranspose) {
165+
const ExtendedPose33 f(kR1, kX1);
166+
const ExtendedPose3d d(kR1, ExtendedPose3d::Matrix3K(kX1));
167+
const Vector12 x = kXi;
168+
const Vector x_dynamic = x;
169+
170+
EXPECT(assert_equal(Vector(f.AdjointMap().transpose() * x),
171+
Vector(f.AdjointTranspose(x))));
172+
EXPECT(assert_equal(d.AdjointMap().transpose() * x_dynamic,
173+
d.AdjointTranspose(x_dynamic)));
174+
175+
std::function<Vector12(const ExtendedPose33&, const Vector12&)>
176+
f_adjoint_transpose = [](const ExtendedPose33& g, const Vector12& v) {
177+
return Vector12(g.AdjointTranspose(v));
178+
};
179+
Matrix Hf_state, Hf_x;
180+
f.AdjointTranspose(x, Hf_state, Hf_x);
181+
EXPECT(assert_equal(numericalDerivative21(f_adjoint_transpose, f, x), Hf_state,
182+
1e-8));
183+
EXPECT(
184+
assert_equal(numericalDerivative22(f_adjoint_transpose, f, x), Hf_x));
185+
186+
std::function<Vector(const ExtendedPose3d&, const Vector&)>
187+
d_adjoint_transpose = [](const ExtendedPose3d& g, const Vector& v) {
188+
return g.AdjointTranspose(v);
189+
};
190+
Matrix Hd_state, Hd_x;
191+
d.AdjointTranspose(x_dynamic, Hd_state, Hd_x);
192+
EXPECT(assert_equal(
193+
numericalDerivative21<Vector, ExtendedPose3d, Vector, 12>(
194+
d_adjoint_transpose, d, x_dynamic),
195+
Hd_state, 1e-8));
196+
EXPECT(assert_equal(
197+
numericalDerivative22<Vector, ExtendedPose3d, Vector, 12>(
198+
d_adjoint_transpose, d, x_dynamic),
199+
Hd_x));
200+
}
201+
202+
//******************************************************************************
203+
TEST(ExtendedPose3, adjointTranspose) {
204+
const Vector12 xi = kXi;
205+
const Vector12 y =
206+
(Vector12() << 0.03, -0.07, 0.02, 0.4, 0.1, -0.2, -0.3, 0.5, 0.9, 0.2,
207+
-0.8, 0.6)
208+
.finished();
209+
const Vector xi_dynamic = xi;
210+
const Vector y_dynamic = y;
211+
212+
EXPECT(assert_equal(Vector(ExtendedPose33::adjointMap(xi).transpose() * y),
213+
Vector(ExtendedPose33::adjointTranspose(xi, y))));
214+
EXPECT(assert_equal(ExtendedPose3d::adjointMap(xi_dynamic).transpose() *
215+
y_dynamic,
216+
ExtendedPose3d::adjointTranspose(xi_dynamic, y_dynamic)));
217+
218+
std::function<Vector12(const Vector12&, const Vector12&)>
219+
f_adjoint_transpose = [](const Vector12& x, const Vector12& v) {
220+
return Vector12(ExtendedPose33::adjointTranspose(x, v));
221+
};
222+
Matrix Hf_xi, Hf_y;
223+
EXPECT(assert_equal(Vector(ExtendedPose33::adjointTranspose(xi, y, Hf_xi, Hf_y)),
224+
Vector(f_adjoint_transpose(xi, y))));
225+
EXPECT(
226+
assert_equal(numericalDerivative21(f_adjoint_transpose, xi, y, 1e-5), Hf_xi,
227+
1e-5));
228+
EXPECT(
229+
assert_equal(numericalDerivative22(f_adjoint_transpose, xi, y, 1e-5), Hf_y,
230+
1e-5));
231+
232+
std::function<Vector(const Vector&, const Vector&)> d_adjoint_transpose =
233+
[](const Vector& x, const Vector& v) {
234+
return ExtendedPose3d::adjointTranspose(x, v);
235+
};
236+
Matrix Hd_xi, Hd_y;
237+
EXPECT(assert_equal(ExtendedPose3d::adjointTranspose(xi_dynamic, y_dynamic,
238+
Hd_xi, Hd_y),
239+
d_adjoint_transpose(xi_dynamic, y_dynamic)));
240+
EXPECT(assert_equal(
241+
numericalDerivative21<Vector, Vector, Vector, 12>(
242+
d_adjoint_transpose, xi_dynamic, y_dynamic, 1e-5),
243+
Hd_xi, 1e-5));
244+
EXPECT(assert_equal(
245+
numericalDerivative22<Vector, Vector, Vector, 12>(
246+
d_adjoint_transpose, xi_dynamic, y_dynamic, 1e-5),
247+
Hd_y, 1e-5));
248+
}
249+
161250
//******************************************************************************
162251
TEST(ExtendedPose3, Derivatives) {
163252
const ExtendedPose33 f(kR1, kX1);

‎gtsam/navigation/NavState.cpp‎

Lines changed: 0 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -121,50 +121,6 @@ bool NavState::equals(const NavState& other, double tol) const {
121121
return Base::equals(other, tol);
122122
}
123123

124-
//------------------------------------------------------------------------------
125-
/// The dual version of Adjoint
126-
Vector9 NavState::AdjointTranspose(const Vector9& x, OptionalJacobian<9, 9> H_state,
127-
OptionalJacobian<9, 9> H_x) const {
128-
const Matrix9 Ad = AdjointMap();
129-
const Vector9 AdTx = Ad.transpose() * x;
130-
131-
// Jacobians
132-
if (H_state) {
133-
//TODO(Varun)
134-
// const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
135-
// v_T_hat = skewSymmetric(AdTx.segment<3>(3)),
136-
// a_T_hat = skewSymmetric(AdTx.tail<3>());
137-
// *H_state << w_T_hat, v_T_hat, //
138-
// /* */ v_T_hat, Z_3x3;
139-
throw std::runtime_error(
140-
"AdjointTranpose H_state Jacobian not implemented.");
141-
}
142-
if (H_x) {
143-
*H_x = Ad.transpose();
144-
}
145-
146-
return AdTx;
147-
}
148-
149-
//------------------------------------------------------------------------------
150-
Vector9 NavState::adjointTranspose(const Vector9& xi, const Vector9& y,
151-
OptionalJacobian<9, 9> Hxi,
152-
OptionalJacobian<9, 9> H_y) {
153-
if (Hxi) {
154-
Hxi->setZero();
155-
for (int i = 0; i < 9; ++i) {
156-
Vector9 dxi;
157-
dxi.setZero();
158-
dxi(i) = 1.0;
159-
Matrix9 GTi = adjointMap(dxi).transpose();
160-
Hxi->col(i) = GTi * y;
161-
}
162-
}
163-
const Matrix9& adT_xi = adjointMap(xi).transpose();
164-
if (H_y) *H_y = adT_xi;
165-
return adT_xi * y;
166-
}
167-
168124
//------------------------------------------------------------------------------
169125
NavState NavState::retract(const Vector9& xi, //
170126
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {

‎gtsam/navigation/NavState.h‎

Lines changed: 1 addition & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -197,21 +197,9 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> {
197197
{}) const;
198198

199199
/// @}
200-
/// @name Lie Group
200+
/// @name Lie Group (all Lie group operations are implemented in ExtendedPose3)
201201
/// @{
202202

203-
/// The dual version of Adjoint
204-
Vector9 AdjointTranspose(const Vector9& x,
205-
OptionalJacobian<9, 9> H_this = {},
206-
OptionalJacobian<9, 9> H_x = {}) const;
207-
208-
/**
209-
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
210-
*/
211-
static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y,
212-
OptionalJacobian<9, 9> Hxi = {},
213-
OptionalJacobian<9, 9> H_y = {});
214-
215203
/// @}
216204
/// @name Dynamics
217205
/// @{

0 commit comments

Comments
 (0)