diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 6d38cfa69..a60ec2a24 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -189,12 +189,19 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { double lambda = log(s.s_); Vector7 result; result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda; + if (Hm) { + // incomplete + } return result; } Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { Vector3 w(v.head<3>()); double lambda = v[6]; + if (Hm) { + Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>()); + // incomplete + } return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda)); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 9ad165180..0f31a6755 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -49,6 +49,13 @@ static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); // Simpler transform Similarity3 T4(Rot3(), Point3(1, 1, 0), 2); +//****************************************************************************** +TEST(Similarity3, concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + //****************************************************************************** TEST(Similarity3, Constructors) { Similarity3 test;