wraps more getters in Expressions

gtsam/geometry/OrientedPlane3.h

@@ -125,12 +125,14 @@ public:
   }
 
   /// 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/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,6 +27,11 @@ 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_;
@@ -33,6 +39,7 @@ 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 +55,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 +81,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 +115,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 +174,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.