diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8b7771410..03071a4f2 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -152,7 +152,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const { /* ************************************************************************* */ Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { - return vector() - T2.vector(); + return T2.vector() - vector(); } } diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 0d263b625..e6c97cb1a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -28,13 +28,20 @@ Cal3DS2::Cal3DS2(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2::K() const { return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } +Matrix Cal3DS2::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} /* ************************************************************************* */ -Vector Cal3DS2::vector() const { return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); } +Vector Cal3DS2::vector() const { + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); +} /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); } +void Cal3DS2::print(const std::string& s) const { + gtsam::print(K(), s + ".K"); + gtsam::print(Vector(k()), s + ".k"); +} /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { @@ -173,10 +180,14 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { } /* ************************************************************************* */ -Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); } +Cal3DS2 Cal3DS2::retract(const Vector& d) const { + return Cal3DS2(vector() + d); +} /* ************************************************************************* */ -Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); } +Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { + return T2.vector() - vector(); +} } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index a62266046..c134860bb 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -186,7 +186,7 @@ public: /// Unretraction for the calibration Vector localCoordinates(const Cal3_S2& T2) const { - return vector() - T2.vector(); + return T2.vector() - vector(); } /// @} diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 7cec17b34..6e62d4be0 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -82,6 +82,7 @@ TEST( Cal3Bundler, retract) d << 10, 1e-3, 1e-3; Cal3Bundler actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 53a21a9fb..fc457767e 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -76,6 +76,18 @@ TEST( Cal3DS2, assert_equal) CHECK(assert_equal(K,K,1e-5)); } +/* ************************************************************************* */ +TEST( Cal3DS2, retract) +{ + Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6, + 2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9); + Vector d(9); + d << 1,2,3,4,5,6,7,8,9; + Cal3DS2 actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index e1dba2272..d77a534d6 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -82,6 +82,17 @@ TEST( Cal3_S2, assert_equal) CHECK(assert_equal(K,K1,1e-9)); } +/* ************************************************************************* */ +TEST( Cal3_S2, retract) +{ + Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5); + Vector d(5); + d << 1,2,3,4,5; + Cal3_S2 actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr;