diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 92d1673793..d0d912ee9a 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -125,12 +125,14 @@ class GTSAM_EXPORT OrientedPlane3 { } /// Return the normal - inline Unit3 normal() const { + inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const { + if (H) *H << I_2x2, Z_2x1; return n_; } /// Return the perpendicular distance to the origin - inline double distance() const { + inline double distance(OptionalJacobian<1, 3> H = boost::none) const { + if (H) *H << 0,0,1; return d_; } }; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 533041a2cf..9eb5e2f29c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -166,6 +166,48 @@ TEST(OrientedPlane3, jacobian_retract) { } } +//******************************************************************************* +TEST(OrientedPlane3, jacobian_normal) { + Matrix23 H_actual, H_expected; + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + + std::function f = std::bind( + &OrientedPlane3::normal, std::placeholders::_1, boost::none); + + H_expected = numericalDerivative11(f, plane); + plane.normal(H_actual); + EXPECT(assert_equal(H_actual, H_expected, 1e-5)); +} + +//******************************************************************************* +TEST(OrientedPlane3, jacobian_distance) { + Matrix13 H_actual, H_expected; + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + + std::function f = std::bind( + &OrientedPlane3::distance, std::placeholders::_1, boost::none); + + H_expected = numericalDerivative11(f, plane); + plane.distance(H_actual); + EXPECT(assert_equal(H_actual, H_expected, 1e-5)); +} + +//******************************************************************************* +TEST(OrientedPlane3, getMethodJacobians) { + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + Matrix33 H_retract, H_getters; + Matrix23 H_normal; + Matrix13 H_distance; + + // confirm the getters are exactly on the tangent space + Vector3 v(0, 0, 0); + plane.retract(v, H_retract); + plane.normal(H_normal); + plane.distance(H_distance); + H_getters << H_normal, H_distance; + EXPECT(assert_equal(H_retract, H_getters, 1e-5)); +} + /* ************************************************************************* */ int main() { srand(time(nullptr)); diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 24476cb5e2..d1bfab7f2f 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -12,6 +12,7 @@ #include #include #include +#include #include namespace gtsam { @@ -26,6 +27,10 @@ inline Point2_ transformTo(const Pose2_& x, const Point2_& p) { return Point2_(x, &Pose2::transformTo, p); } +inline Double_ range(const Point2_& p, const Point2_& q) { + return Double_(Range(), p, q); +} + // 3D Geometry typedef Expression Point3_; @@ -33,6 +38,7 @@ typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; typedef Expression Line3_; +typedef Expression OrientedPlane3_; inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformTo, p); @@ -48,6 +54,10 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) { return Line3_(f, wTc, wL); } +inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) { + return Pose3_(p, &Pose3::transformPoseTo, q); +} + inline Point3_ normalize(const Point3_& a){ Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize; return Point3_(f, a); @@ -70,16 +80,28 @@ namespace internal { inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { return pose.rotation(H); } + +inline Point3 translation(const Pose3& pose, OptionalJacobian<3, 6> H) { + return pose.translation(H); +} } // namespace internal inline Rot3_ rotation(const Pose3_& pose) { return Rot3_(internal::rotation, pose); } +inline Point3_ translation(const Pose3_& pose) { + return Point3_(internal::translation, pose); +} + inline Point3_ rotate(const Rot3_& x, const Point3_& p) { return Point3_(x, &Rot3::rotate, p); } +inline Point3_ point3(const Unit3_& v) { + return Point3_(&Unit3::point3, v); +} + inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) { return Unit3_(x, &Rot3::rotate, p); } @@ -92,6 +114,14 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) { return Unit3_(x, &Rot3::unrotate, p); } +inline Double_ distance(const OrientedPlane3_& p) { + return Double_(&OrientedPlane3::distance, p); +} + +inline Unit3_ normal(const OrientedPlane3_& p) { + return Unit3_(&OrientedPlane3::normal, p); +} + // Projection typedef Expression Cal3_S2_; @@ -143,6 +173,11 @@ Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } +template +inline Pose3_ getPose(const Expression > & cam) { + return Pose3_(&PinholeCamera::getPose, cam); +} + /// logmap // TODO(dellaert): Should work but fails because of a type deduction conflict.