Support bearing factors between Pose3 values

I am modelling a system in which there are two bodies, and one can
observe the other but cannot estimate the other's pose. This is
perfectly modeled by a BearingRangeFactor, but without this patch, you
cannot make a BearingRangeFactor between two Pose3 values. This adds
support for that by extending the Pose3 class to support calling
bearing() on another Pose3.

gtsam/geometry/Pose3.cpp

@@ -378,6 +378,15 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
   }
 }
 
+/* ************************************************************************* */
+Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1,
+                     OptionalJacobian<2, 6> H2) const {
+  if (H2) {
+    return bearing(point.translation(), H1, H2.cols<3>(3));
+  }
+  return bearing(point.translation(), H1, boost::none);
+}
+
 /* ************************************************************************* */
 boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
   const size_t n = abPointPairs.size();

gtsam/geometry/Pose3.h

@@ -282,6 +282,15 @@ public:
   Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
       OptionalJacobian<2, 3> H2 = boost::none) const;
 
+  /**
+   * Calculate bearing to another pose
+   * @param other 3D location and orientation of other body. The orientation
+   * information is ignored.
+   * @return bearing (Unit3)
+   */
+  Unit3 bearing(const Pose3& point, OptionalJacobian<2, 6> H1 = boost::none,
+      OptionalJacobian<2, 6> H2 = boost::none) const;
+
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -353,6 +362,9 @@ struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
 template <>
 struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
 
+template<>
+struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
+
 template <typename T>
 struct Range<Pose3, T> : HasRange<Pose3, T, double> {};