diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 533c042c0..84f0de5e8 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -17,18 +17,20 @@ namespace gtsam { template class GroupConcept { private: - static void concept_check(const T& t) { + static T concept_check(const T& t) { /** assignment */ T t2 = t; - /** compose with another object */ - T compose_ret = t.compose(t2); - - /** invert the object and yield a new one */ - T inverse_ret = t.inverse(); - /** identity */ T identity = T::identity(); + + /** compose with another object */ + T compose_ret = identity.compose(t2); + + /** invert the object and yield a new one */ + T inverse_ret = compose_ret.inverse(); + + return inverse_ret; } }; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 932e069ef..bc530a9f5 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -78,7 +78,7 @@ template class LieConcept { private: /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { + static T concept_check(const T& t) { /** assignment */ T t2 = t; @@ -95,7 +95,9 @@ private: Vector logmap_identity_ret = T::Logmap(t); /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = t.between(t2); + T between_ret = expmap_identity_ret.between(t2); + + return between_ret; } }; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 430be4f7d..b84a6b525 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -65,7 +65,7 @@ template class ManifoldConcept { private: /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { + static T concept_check(const T& t) { /** assignment */ T t2 = t; @@ -84,6 +84,8 @@ private: * Returns local coordinates of another object */ Vector localCoords_ret = t.localCoordinates(t2); + + return retract_ret; } }; diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index cb75a5801..31012aa44 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -541,7 +541,7 @@ TEST (Serialization, visual_system) { graph.addPointConstraint(1, pt1); graph.addPointPrior(1, pt2, model3); graph.addPoseConstraint(1, pose1); - graph.addPosePrior(1, pose3, model6); + graph.addPosePrior(1, pose2, model6); EXPECT(equalsObj(values)); EXPECT(equalsObj(graph));