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

Just making sure Adjoints are correct

release/4.3a0
Frank Dellaert 2018-11-10 01:18:27 -05:00
parent 4ca70bea3a
commit 48117d35ff
2 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/geometry/tests/testPose2.cpp
View File

@ -172,6 +172,35 @@ TEST(Pose2, expmap_c_full)
EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6));
}
/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose2, Adjoint_full) {
Pose2 T(1, 2, 3);
Pose2 expected = T * Pose2::Expmap(screwPose2::xi) * T.inverse();
Vector xiprime = T.Adjoint(screwPose2::xi);
EXPECT(assert_equal(expected, Pose2::Expmap(xiprime), 1e-6));
Vector3 xi2(4, 5, 6);
Pose2 expected2 = T * Pose2::Expmap(xi2) * T.inverse();
Vector xiprime2 = T.Adjoint(xi2);
EXPECT(assert_equal(expected2, Pose2::Expmap(xiprime2), 1e-6));
}
/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(Pose2, Adjoint_hat) {
Pose2 T(1, 2, 3);
auto hat = [](const Vector& xi) { return ::wedge<Pose2>(xi); };
Matrix3 expected = T.matrix() * hat(screwPose2::xi) * T.matrix().inverse();
Matrix3 xiprime = hat(T.Adjoint(screwPose2::xi));
EXPECT(assert_equal(expected, xiprime, 1e-6));
Vector3 xi2(4, 5, 6);
Matrix3 expected2 = T.matrix() * hat(xi2) * T.matrix().inverse();
Matrix3 xiprime2 = hat(T.Adjoint(xi2));
EXPECT(assert_equal(expected2, xiprime2, 1e-6));
}
/* ************************************************************************* */
TEST(Pose2, logmap) {
Pose2 pose0(M_PI/2.0, Point2(1, 2));

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

@ -143,6 +143,24 @@ TEST(Pose3, Adjoint_full)
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
}
/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(Pose3, Adjoint_hat)
{
auto hat = [](const Vector& xi) { return ::wedge<Pose3>(xi); };
Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse();
Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi));
EXPECT(assert_equal(expected, xiprime, 1e-6));
Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse();
Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi));
EXPECT(assert_equal(expected2, xiprime2, 1e-6));
Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse();
Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi));
EXPECT(assert_equal(expected3, xiprime3, 1e-6));
}
/* ************************************************************************* */
/** Agrawal06iros version of exponential map */
Pose3 Agrawal06iros(const Vector& xi) {