diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c134860bb..03c6bff3f 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -165,6 +165,16 @@ public: */ Vector3 calibrate(const Vector3& p) const; + /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) + inline Cal3_S2 between(const Cal3_S2& q, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(5); + if(H2) *H2 = eye(5); + return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); + } + + /// @} /// @name Manifold /// @{ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index d77a534d6..86c0b7e33 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -93,6 +93,17 @@ TEST( Cal3_S2, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } +/* ************************************************************************* */ +TEST(Cal3_S2, between) { + Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9); + Matrix H1, H2; + + EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); + EXPECT(assert_equal(-eye(5), H1)); + EXPECT(assert_equal(eye(5), H2)); + +} + /* ************************************************************************* */ int main() { TestResult tr;