Merged in haldean/gtsam/feature/pose3-bearing (pull request #290)

Support bearing factors between Pose3 values

Approved-by: Jing Dong <jdong@gatech.edu>
Approved-by: Chris Beall <chrisbeall@gmail.com>

gtsam/geometry/Pose3.cpp

@@ -378,6 +378,16 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
   }
 }
 
+/* ************************************************************************* */
+Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1,
+                     OptionalJacobian<2, 6> H2) const {
+  if (H2) {
+    H2->setZero();
+    return bearing(pose.translation(), H1, H2.cols<3>(3));
+  }
+  return bearing(pose.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& pose, 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> {};
 

gtsam/geometry/tests/testPose3.cpp

@@ -708,6 +708,27 @@ TEST(Pose3, Bearing2) {
   EXPECT(assert_equal(expectedH2, actualH2));
 }
 
+TEST(Pose3, PoseToPoseBearing) {
+  Matrix expectedH1, actualH1, expectedH2, actualH2, H2block;
+  EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9));
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2);
+
+  // Since the second pose is treated as a point, the value calculated by
+  // numericalDerivative22 only depends on the position of the pose. Here, we
+  // calculate the Jacobian w.r.t. the second pose's position, and then augment
+  // that with zeroes in the block that is w.r.t. the second pose's
+  // orientation.
+  H2block = numericalDerivative22(bearing_proxy, xl1, l2);
+  expectedH2 = Matrix(2, 6);
+  expectedH2.setZero();
+  expectedH2.block<2, 3>(0, 3) = H2block;
+
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, unicycle )
 {

python/handwritten/geometry/Pose3.cpp

@@ -53,6 +53,9 @@ void exportPose3(){
   // function pointers to desambiguate range() calls
   double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const  = &Pose3::range;
   double (Pose3::*range2)(const Pose3 &,  OptionalJacobian<1,6>, OptionalJacobian<1,6>) const  = &Pose3::range;
+  // function pointers to desambiguate bearing() calls
+  Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const  = &Pose3::bearing;
+  Unit3 (Pose3::*bearing2)(const Pose3 &,  OptionalJacobian<2,6>, OptionalJacobian<2,6>) const  = &Pose3::bearing;
 
   class_<Pose3>("Pose3")
       .def(init<>())
@@ -65,7 +68,6 @@ void exportPose3(){
       .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol")))
       .def("identity", &Pose3::identity)
       .staticmethod("identity")
-      .def("bearing", &Pose3::bearing)
       .def("matrix", &Pose3::matrix)
       .def("transform_from", &Pose3::transform_from,
            transform_from_overloads(args("point", "H1", "H2")))
@@ -88,5 +90,6 @@ void exportPose3(){
       .def("between", between2, between_overloads())
       .def("range", range1, range_overloads())
       .def("range", range2, range_overloads())
-      .def("bearing", &Pose3::bearing, bearing_overloads());
+      .def("bearing", bearing1, bearing_overloads())
+      .def("bearing", bearing2, bearing_overloads());
 }