12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

AdjointTranspose

release/4.3a0
Gerry Chen 2021-10-04 04:58:16 -04:00
parent bcafdb7557
commit ecf53a6854
3 changed files with 81 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/geometry/Pose3.cpp
View File

@ -100,6 +100,42 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this,
return result;
}
/* ************************************************************************* */
/// The dual version of Adjoint
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this,
OptionalJacobian<6, 6> H_x) const {
// Ad^T * xi = [ R^T R^T.[-t] . [w
// 0 R^T ] v]
// Declarations and aliases
Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv;
Vector6 result;
const Vector3 &w = x.head<3>(), &v = x.tail<3>();
auto Rv = result.tail<3>();
// Calculations
const Vector3 Rw =
R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr);
Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr);
const Vector3 tv = cross(t_, v, tv_H_t, tv_H_v);
const Vector3 Rtv = R_.unrotate(tv, Rtv_H_R, Rtv_H_tv);
result.head<3>() = Rw - Rtv;
// Jacobians
if (H_this) {
*H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t
/* */ Rv_H_R, /* */ Z_3x3)
.finished();
}
if (H_x) {
*H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, //
/* */ Z_3x3, Rv_H_v)
.finished();
}
// Return
return result;
}
/* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector6& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));

5
gtsam/geometry/Pose3.h
View File

@ -157,6 +157,11 @@ public:
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_xib = boost::none) const;
/// The dual version of Adjoint
Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_x = boost::none) const;
/**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
* [ad(w,v)] = [w^, zero3; v^, w^]

41
gtsam/geometry/tests/testPose3.cpp
View File

@ -151,7 +151,11 @@ TEST(Pose3, Adjoint_jacobians)
{
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
std::function<Vector6(const Pose3&, const Vector6&)> Adjoint_proxy =
[](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); };
[&](const Pose3& T, const Vector6& xi) {
Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi);
EXPECT(assert_equal(res1, res2));
return res1;
};
gtsam::Vector6 xi =
(gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
@ -175,6 +179,41 @@ TEST(Pose3, Adjoint_jacobians)
EXPECT(assert_equal(expectedH2, actualH2));
}
/* ************************************************************************* */
// Check AdjointTranspose and jacobians
TEST(Pose3, AdjointTranspose)
{
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
std::function<Vector6(const Pose3&, const Vector6&)> AdjointTranspose_proxy =
[&](const Pose3& T, const Vector6& xi) {
Vector6 res1 = T.AdjointMap().transpose() * xi,
res2 = T.AdjointTranspose(xi);
EXPECT(assert_equal(res1, res2));
return res1;
};
gtsam::Vector6 xi =
(gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
T.AdjointTranspose(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi);
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);
EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2, actualH2));
T2.AdjointTranspose(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi);
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2));
T3.AdjointTranspose(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi);
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi);
EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2, actualH2));
}
/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(Pose3, Adjoint_hat)