Merge pull request #1376 from BrettRD/develop

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_;
   }
 };

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<Unit3(const OrientedPlane3&)> 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<double(const OrientedPlane3&)> 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));

gtsam/slam/expressions.h

@@ -12,6 +12,7 @@
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Line3.h>
+#include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/geometry/PinholeCamera.h>
 
 namespace gtsam {
@@ -26,13 +27,18 @@ 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<Point2, Point2>(), p, q);
+}
+
 // 3D Geometry
 
 typedef Expression<Point3> Point3_;
 typedef Expression<Unit3> Unit3_;
 typedef Expression<Rot3> Rot3_;
 typedef Expression<Pose3> Pose3_;
 typedef Expression<Line3> Line3_;
+typedef Expression<OrientedPlane3> 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> Cal3_S2_;
@@ -143,6 +173,11 @@ Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
   return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
 }
 
+template <class CALIBRATION>
+inline Pose3_ getPose(const Expression<PinholeCamera<CALIBRATION> > & cam) {
+  return Pose3_(&PinholeCamera<CALIBRATION>::getPose, cam);
+}
+
 
 /// logmap
 // TODO(dellaert): Should work but fails because of a type deduction conflict.