From b6a785e2d99e1e501fc32847893a375ec04cccd1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 12 Jul 2015 20:16:06 -0700
Subject: [PATCH] BearingRange type

---
 gtsam/geometry/BearingRange.h             | 97 +++++++++++++++++++++++
 gtsam/geometry/tests/testBearingRange.cpp | 61 ++++++++++++++
 2 files changed, 158 insertions(+)
 create mode 100644 gtsam/geometry/BearingRange.h
 create mode 100644 gtsam/geometry/tests/testBearingRange.cpp

diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
new file mode 100644
index 000000000..38fb47067
--- /dev/null
+++ b/gtsam/geometry/BearingRange.h
@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file BearingRange.h
+ * @date July, 2015
+ * @author Frank Dellaert
+ * @brief Bearing-Range product
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Testable.h>
+#include <boost/concept/assert.hpp>
+
+namespace gtsam {
+
+// forward declaration of Bearing functor, assumed partially specified
+template <typename A1, typename A2>
+struct Bearing;
+
+// forward declaration of Range functor, assumed partially specified
+template <typename A1, typename A2>
+struct Range;
+
+/**
+ * Bearing-Range product for a particular A1,A2 combination
+ */
+template <typename A1, typename A2>
+struct BearingRange
+    : public ProductManifold<typename Bearing<A1, A2>::result_type,
+                             typename Range<A1, A2>::result_type> {
+  typedef typename Bearing<A1, A2>::result_type B;
+  typedef typename Range<A1, A2>::result_type R;
+
+  BearingRange() {}
+  BearingRange(const ProductManifold<B, R>& br) : ProductManifold<B, R>(br) {}
+  BearingRange(const B& b, const R& r) : ProductManifold<B, R>(b, r) {}
+
+  /// Prediction function that stacks measurements
+  //  BearingRange operator()(
+  //      const A1& a1, const A2& a2,
+  //      typename MakeOptionalJacobian<BearingRange, A1>::type H1,
+  //      typename MakeOptionalJacobian<BearingRange, A2>::type H2) {
+  //    typename MakeJacobian<B, A1>::type HB1;
+  //    typename MakeJacobian<B, A2>::type HB2;
+  //    typename MakeJacobian<R, A1>::type HR1;
+  //    typename MakeJacobian<R, A2>::type HR2;
+  //
+  //    B b = Bearing<A1, A2>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
+  //    R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
+  //
+  //    if (H1) *H1 << HB1, HR1;
+  //    if (H2) *H2 << HB2, HR2;
+  //    return BearingRange(b, r);
+  //  }
+  //
+ private:
+  /// Serialization function
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("bearing", this->first);
+    ar& boost::serialization::make_nvp("range", this->second);
+  }
+
+  friend class boost::serialization::access;
+};
+
+template <typename A1, typename A2>
+struct traits<BearingRange<A1, A2> >
+    //    : internal::ManifoldTraits<BearingRange<A1, A2> >
+    {
+  typedef typename Bearing<A1, A2>::result_type B;
+  typedef typename Range<A1, A2>::result_type R;
+
+  static void Print(const BearingRange<A1, A2>& m,
+                    const std::string& str = "") {
+    traits<B>::Print(m.first, str);
+    traits<R>::Print(m.second, str);
+  }
+  static bool Equals(const BearingRange<A1, A2>& m1,
+                     const BearingRange<A1, A2>& m2, double tol = 1e-8) {
+    return traits<B>::Equals(m1.first, m2.first, tol) &&
+           traits<R>::Equals(m1.second, m2.second, tol);
+  }
+};
+
+}  // namespace gtsam
diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp
new file mode 100644
index 000000000..b12a5a288
--- /dev/null
+++ b/gtsam/geometry/tests/testBearingRange.cpp
@@ -0,0 +1,61 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testBearingRange.cpp
+ *  @brief Unit tests for BearingRange Class
+ *  @author Frank Dellaert
+ *  @date July 2015
+ */
+
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/serializationTestHelpers.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace serializationTestHelpers;
+
+typedef BearingRange<Pose2, Point2> BearingRange2D;
+BearingRange2D br2D(1, 2);
+
+typedef BearingRange<Pose3, Point3> BearingRange3D;
+BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1);
+
+/* ************************************************************************* */
+TEST(BearingRange, Serialization2D) {
+  EXPECT(equalsObj<BearingRange2D>(br2D));
+  EXPECT(equalsXML<BearingRange2D>(br2D));
+  EXPECT(equalsBinary<BearingRange2D>(br2D));
+}
+
+/* ************************************************************************* */
+TEST(BearingRange, 2D) {}
+
+/* ************************************************************************* */
+TEST(BearingRange, Serialization3D) {
+  EXPECT(equalsObj<BearingRange3D>(br3D));
+  EXPECT(equalsXML<BearingRange3D>(br3D));
+  EXPECT(equalsBinary<BearingRange3D>(br3D));
+}
+
+/* ************************************************************************* */
+TEST(BearingRange, 3D) {}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */