diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c9dfcf487..c6da8e80b 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -226,11 +226,9 @@ namespace gtsam { // between = compose(p2,inverse(p1)); Pose3 Pose3::between(const Pose3& p2, boost::optional H1, boost::optional H2) const { - Matrix invH; - Pose3 invp1 = inverse(invH); - Matrix composeH1; - Pose3 result = invp1.compose(p2, composeH1, H2); - if (H1) *H1 = composeH1 * invH; + Pose3 result = inverse()*p2; + if (H1) *H1 = -result.inverse().adjointMap(); + if (H2) *H2 = I6; return result; } diff --git a/gtsam/geometry/tests/timePose3.cpp b/gtsam/geometry/tests/timePose3.cpp index 2931a9059..9b125cf28 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/gtsam/geometry/tests/timePose3.cpp @@ -40,10 +40,13 @@ int main() double x=1.0/norm, y=4.0/norm, z=2.0/norm; Vector v = Vector_(6,x,y,z,0.1,0.2,-0.1); Pose3 T = Pose3::Expmap(Vector_(6,0.1,0.1,0.2,0.1, 0.4, 0.2)), T2 = T.retract(v); + Matrix H1,H2; TEST("retract", T.retract(v)) TEST("Expmap", T*Pose3::Expmap(v)) TEST("localCoordinates", T.localCoordinates(T2)) + TEST("between", T.between(T2)) + TEST("between-derivatives", T.between(T2,H1,H2)) TEST("Logmap", Pose3::Logmap(T.between(T2))) return 0;