From d02b33af8819b77a2c7670b46f0d9af6bfe812fc Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 10:16:55 -0500
Subject: [PATCH] Deprecated ProductManifold as has alignment issues and is
 overly obfuscating.

---
 gtsam/base/Manifold.h         |  5 +-
 gtsam/geometry/BearingRange.h | 91 +++++++++++++++++++++++++++--------
 2 files changed, 75 insertions(+), 21 deletions(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index f89680b7c..7bfdd7ea5 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -168,9 +168,9 @@ struct FixedDimension {
       "FixedDimension instantiated for dymanically-sized type.");
 };
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 /// Helper class to construct the product manifold of two other manifolds, M1 and M2
-/// Assumes nothing except manifold structure for M1 and M2, and the existence
-/// of default constructor for those types
+/// Deprecated because of limited usefulness, maximum obfuscation 
 template<typename M1, typename M2>
 class ProductManifold: public std::pair<M1, M2> {
   BOOST_CONCEPT_ASSERT((IsManifold<M1>));
@@ -221,6 +221,7 @@ public:
 template<typename M1, typename M2>
 struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
 };
+#endif
 
 } // \ namespace gtsam
 
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index b1e003864..9de5a07d8 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -45,23 +45,33 @@ struct Range;
  * and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
  */
 template <typename A1, typename A2>
-struct BearingRange
-    : public ProductManifold<typename Bearing<A1, A2>::result_type,
-                             typename Range<A1, A2>::result_type> {
+struct BearingRange {
+private:
   typedef typename Bearing<A1, A2>::result_type B;
   typedef typename Range<A1, A2>::result_type R;
-  typedef ProductManifold<B, R> Base;
+  B bearing_;
+  R range_;
+
+public:
+  enum { dimB = traits<B>::dimension };
+  enum { dimR = traits<R>::dimension };
+  enum { dimension = dimB + dimR };
+
+  /// @name Standard Constructors
+  /// @{
 
   BearingRange() {}
-  BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
-  BearingRange(const B& b, const R& r) : Base(b, r) {}
+  BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
 
   /// Prediction function that stacks measurements
   static BearingRange Measure(
-      const A1& a1, const A2& a2,
-      OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
-      OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
-          boost::none) {
+    const A1& a1, const A2& a2,
+    OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
+    OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
     typename MakeJacobian<B, A1>::type HB1;
     typename MakeJacobian<B, A2>::type HB2;
     typename MakeJacobian<R, A1>::type HR1;
@@ -75,32 +85,75 @@ struct BearingRange
     return BearingRange(b, r);
   }
 
+  /// @}
+  /// @name Testable
+  /// @{
+
   void print(const std::string& str = "") const {
     std::cout << str;
-    traits<B>::Print(this->first, "bearing ");
-    traits<R>::Print(this->second, "range ");
+    traits<B>::Print(bearing_, "bearing ");
+    traits<R>::Print(range_, "range ");
   }
   bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
-    return traits<B>::Equals(this->first, m2.first, tol) &&
-           traits<R>::Equals(this->second, m2.second, tol);
+    return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
+      traits<R>::Equals(range_, m2.range_, tol);
   }
 
- private:
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  /// Retract delta to manifold
+  BearingRange retract(const TangentVector& xi) const {
+    B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
+    R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
+    return BearingRange(m1, m2);
+  }
+
+  /// Compute the coordinates in the tangent space
+  TangentVector localCoordinates(const BearingRange& other) const {
+    typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
+    typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+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);
+    ar& boost::serialization::make_nvp("bearing", bearing_);
+    ar& boost::serialization::make_nvp("range", range_);
   }
 
   friend class boost::serialization::access;
+
+  /// @}
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum {
+    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
+  };
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Declare this to be both Testable and a Manifold
 template <typename A1, typename A2>
 struct traits<BearingRange<A1, A2> >
-    : Testable<BearingRange<A1, A2> >,
-      internal::ManifoldTraits<BearingRange<A1, A2> > {};
+  : Testable<BearingRange<A1, A2> >,
+  internal::ManifoldTraits<BearingRange<A1, A2> > {};
 
 // Helper class for to implement Range traits for classes with a bearing method
 // For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say