diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index dd6e24f9a..809d7846e 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -96,7 +96,7 @@ public: const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; - adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Z_3x3, Z_3x3, 1; + adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; return adj; } @@ -161,7 +161,7 @@ public: return Similarity3( // r.retract(v.head<3>()), // retract rotation using v[0,1,2] Point3(v.segment<3>(3)), // Retract the translation - v[6]); //finally, update scale using v[6] + 1.0 + v[6]); //finally, update scale using v[6] } /// 7-dimensional vector v in tangent space that makes other = this->retract(v) @@ -171,7 +171,7 @@ public: v.head<3>() = r.localCoordinates(other.R_); v.segment<3>(3) = other.t_.vector(); //v.segment<3>(3) = translation().localCoordinates(other.translation()); - v[6] = other.s_; + v[6] = other.s_ - 1.0; return v; } }; @@ -237,6 +237,19 @@ TEST(Similarity3, Getters2) { EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); } +TEST(Similarity3, AdjointMap) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix7 result; + result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, + 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, + -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, + 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, + 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, + 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, + 0, 0, 0, 0, 0, 0, 1.0000; + EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); +} + //****************************************************************************** TEST(Similarity3, Manifold) { EXPECT_LONGS_EQUAL(7, Similarity3::Dim());