Merge remote-tracking branch 'origin/develop' into feature/frobeniusfactor

gtsam.h

@@ -281,7 +281,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -2984,6 +2984,7 @@ class PreintegratedImuMeasurements {
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;
@@ -3045,6 +3046,7 @@ class PreintegratedCombinedMeasurements {
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;

gtsam/3rdparty/CMakeLists.txt

@@ -17,7 +17,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	endforeach(eigen_dir)
 
 	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
-		message("-- Installing Eigen Unsupported modules")
+		message(STATUS "Installing Eigen Unsupported modules")
 		# do the same for the unsupported eigen folder
 		file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
 

gtsam/base/GenericValue.h

@@ -181,7 +181,7 @@ public:
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
   enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues

gtsam/base/Manifold.h

@@ -214,7 +214,7 @@ public:
   enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
   };
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept

gtsam/base/make_shared.h

@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2020, 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 make_shared.h
+ * @brief make_shared trampoline function to ensure proper alignment
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/base/types.h>
+
+#include <Eigen/Core>
+
+#include <boost/make_shared.hpp>
+
+#include <type_traits>
+
+namespace gtsam {
+  /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
+  template<bool B, class T = void>
+  using enable_if_t = typename std::enable_if<B, T>::type;
+}
+
+namespace gtsam {
+
+  /**
+   * Add our own `make_shared` as a layer of wrapping on `boost::make_shared`
+   * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs
+   * at runtime, which is notoriously hard to debug.
+   *
+   * Explanation
+   * ===============
+   * The template `needs_eigen_aligned_allocator<T>::value` will evaluate to `std::true_type` if the type alias
+   * `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the
+   * `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro.
+   *
+   * This function declaration will only be taken when the above condition is true, so if some object does not need to
+   * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for
+   * `boost::make_shared`.
+   *
+   * @tparam T The type of object being constructed
+   * @tparam Args Type of the arguments of the constructor
+   * @param args Arguments of the constructor
+   * @return The object created as a boost::shared_ptr<T>
+   */
+  template<typename T, typename ... Args>
+  gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
+    return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
+  }
+
+  /// Fall back to the boost version if no need for alignment
+  template<typename T, typename ... Args>
+  gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
+    return boost::make_shared<T>(std::forward<Args>(args)...);
+  }
+
+}

gtsam/base/serialization.h

@@ -42,123 +42,218 @@
 
 namespace gtsam {
 
-// Serialization directly to strings in compressed format
+/** @name Standard serialization
-template<class T>
+ *  Serialization in default compressed format
-std::string serialize(const T& input) {
+ */
-  std::ostringstream out_archive_stream;
+///@{
+/// serializes to a stream
+template <class T>
+void serializeToStream(const T& input, std::ostream& out_archive_stream) {
   boost::archive::text_oarchive out_archive(out_archive_stream);
   out_archive << input;
-  return out_archive_stream.str();
 }
 
-template<class T>
+/// deserializes from a stream
-void deserialize(const std::string& serialized, T& output) {
+template <class T>
-  std::istringstream in_archive_stream(serialized);
+void deserializeFromStream(std::istream& in_archive_stream, T& output) {
   boost::archive::text_iarchive in_archive(in_archive_stream);
   in_archive >> output;
 }
 
-template<class T>
+/// serializes to a string
+template <class T>
+std::string serializeToString(const T& input) {
+  std::ostringstream out_archive_stream;
+  serializeToStream(input, out_archive_stream);
+  return out_archive_stream.str();
+}
+
+/// deserializes from a string
+template <class T>
+void deserializeFromString(const std::string& serialized, T& output) {
+  std::istringstream in_archive_stream(serialized);
+  deserializeFromStream(in_archive_stream, output);
+}
+
+/// serializes to a file
+template <class T>
 bool serializeToFile(const T& input, const std::string& filename) {
   std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
+  if (!out_archive_stream.is_open()) return false;
-    return false;
+  serializeToStream(input, out_archive_stream);
-  boost::archive::text_oarchive out_archive(out_archive_stream);
-  out_archive << input;
   out_archive_stream.close();
   return true;
 }
 
-template<class T>
+/// deserializes from a file
+template <class T>
 bool deserializeFromFile(const std::string& filename, T& output) {
   std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
+  if (!in_archive_stream.is_open()) return false;
-    return false;
+  deserializeFromStream(in_archive_stream, output);
-  boost::archive::text_iarchive in_archive(in_archive_stream);
-  in_archive >> output;
   in_archive_stream.close();
   return true;
 }
 
-// Serialization to XML format with named structures
+/// serializes to a string
-template<class T>
+template <class T>
-std::string serializeXML(const T& input, const std::string& name="data") {
+std::string serialize(const T& input) {
-  std::ostringstream out_archive_stream;
+  return serializeToString(input);
-  // braces to flush out_archive as it goes out of scope before taking str()
-  // fixes crash with boost 1.66-1.68
-  // see https://github.com/boostorg/serialization/issues/82
-  {
-    boost::archive::xml_oarchive out_archive(out_archive_stream);
-    out_archive << boost::serialization::make_nvp(name.c_str(), input);
-  }
-  return out_archive_stream.str();
 }
 
-template<class T>
+/// deserializes from a string
-void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
+template <class T>
-  std::istringstream in_archive_stream(serialized);
+void deserialize(const std::string& serialized, T& output) {
+  deserializeFromString(serialized, output);
+}
+///@}
+
+/** @name XML Serialization
+ *  Serialization to XML format with named structures
+ */
+///@{
+/// serializes to a stream in XML
+template <class T>
+void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
+                  const std::string& name = "data") {
+  boost::archive::xml_oarchive out_archive(out_archive_stream);
+  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+}
+
+/// deserializes from a stream in XML
+template <class T>
+void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
+                    const std::string& name = "data") {
   boost::archive::xml_iarchive in_archive(in_archive_stream);
   in_archive >> boost::serialization::make_nvp(name.c_str(), output);
 }
 
-template<class T>
+/// serializes to a string in XML
-bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") {
+template <class T>
-  std::ofstream out_archive_stream(filename.c_str());
+std::string serializeToXMLString(const T& input,
-  if (!out_archive_stream.is_open())
+                         const std::string& name = "data") {
-    return false;
-  boost::archive::xml_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);;
-  out_archive_stream.close();
-  return true;
-}
-
-template<class T>
-bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
-  std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
-    return false;
-  boost::archive::xml_iarchive in_archive(in_archive_stream);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
-  in_archive_stream.close();
-  return true;
-}
-
-// Serialization to binary format with named structures
-template<class T>
-std::string serializeBinary(const T& input, const std::string& name="data") {
   std::ostringstream out_archive_stream;
-  boost::archive::binary_oarchive out_archive(out_archive_stream);
+  serializeToXMLStream(input, out_archive_stream, name);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
   return out_archive_stream.str();
 }
 
-template<class T>
+/// deserializes from a string in XML
-void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
+template <class T>
+void deserializeFromXMLString(const std::string& serialized, T& output,
+                    const std::string& name = "data") {
   std::istringstream in_archive_stream(serialized);
-  boost::archive::binary_iarchive in_archive(in_archive_stream);
+  deserializeFromXMLStream(in_archive_stream, output, name);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
 }
 
-template<class T>
+/// serializes to an XML file
-bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
+template <class T>
+bool serializeToXMLFile(const T& input, const std::string& filename,
+                        const std::string& name = "data") {
   std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
+  if (!out_archive_stream.is_open()) return false;
-    return false;
+  serializeToXMLStream(input, out_archive_stream, name);
-  boost::archive::binary_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
   out_archive_stream.close();
   return true;
 }
 
-template<class T>
+/// deserializes from an XML file
-bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
+template <class T>
+bool deserializeFromXMLFile(const std::string& filename, T& output,
+                            const std::string& name = "data") {
   std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
+  if (!in_archive_stream.is_open()) return false;
-    return false;
+  deserializeFromXMLStream(in_archive_stream, output, name);
-  boost::archive::binary_iarchive in_archive(in_archive_stream);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
   in_archive_stream.close();
   return true;
 }
 
-} // \namespace gtsam
+/// serializes to a string in XML
+template <class T>
+std::string serializeXML(const T& input,
+                         const std::string& name = "data") {
+  return serializeToXMLString(input, name);
+}
+
+/// deserializes from a string in XML
+template <class T>
+void deserializeXML(const std::string& serialized, T& output,
+                    const std::string& name = "data") {
+  deserializeFromXMLString(serialized, output, name);
+}
+///@}
+
+/** @name Binary Serialization
+ *  Serialization to binary format with named structures
+ */
+///@{
+/// serializes to a stream in binary
+template <class T>
+void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
+                     const std::string& name = "data") {
+  boost::archive::binary_oarchive out_archive(out_archive_stream);
+  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+}
+
+/// deserializes from a stream in binary
+template <class T>
+void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
+                       const std::string& name = "data") {
+  boost::archive::binary_iarchive in_archive(in_archive_stream);
+  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
+}
+
+/// serializes to a string in binary
+template <class T>
+std::string serializeToBinaryString(const T& input,
+                            const std::string& name = "data") {
+  std::ostringstream out_archive_stream;
+  serializeToBinaryStream(input, out_archive_stream, name);
+  return out_archive_stream.str();
+}
+
+/// deserializes from a string in binary
+template <class T>
+void deserializeFromBinaryString(const std::string& serialized, T& output,
+                       const std::string& name = "data") {
+  std::istringstream in_archive_stream(serialized);
+  deserializeFromBinaryStream(in_archive_stream, output, name);
+}
+
+/// serializes to a binary file
+template <class T>
+bool serializeToBinaryFile(const T& input, const std::string& filename,
+                           const std::string& name = "data") {
+  std::ofstream out_archive_stream(filename.c_str());
+  if (!out_archive_stream.is_open()) return false;
+  serializeToBinaryStream(input, out_archive_stream, name);
+  out_archive_stream.close();
+  return true;
+}
+
+/// deserializes from a binary file
+template <class T>
+bool deserializeFromBinaryFile(const std::string& filename, T& output,
+                               const std::string& name = "data") {
+  std::ifstream in_archive_stream(filename.c_str());
+  if (!in_archive_stream.is_open()) return false;
+  deserializeFromBinaryStream(in_archive_stream, output, name);
+  in_archive_stream.close();
+  return true;
+}
+
+/// serializes to a string in binary
+template <class T>
+std::string serializeBinary(const T& input,
+                            const std::string& name = "data") {
+  return serializeToBinaryString(input, name);
+}
+
+/// deserializes from a string in binary
+template <class T>
+void deserializeBinary(const std::string& serialized, T& output,
+                       const std::string& name = "data") {
+  deserializeFromBinaryString(serialized, output, name);
+}
+///@}
+
+}  // namespace gtsam

gtsam/base/serializationTestHelpers.h

@@ -26,6 +26,7 @@
 #include <gtsam/base/serialization.h>
 
 #include <boost/serialization/serialization.hpp>
+#include <boost/filesystem.hpp>
 
 
 // whether to print the serialized text to stdout
@@ -40,22 +41,37 @@ T create() {
   return T();
 }
 
+// Creates or empties a folder in the build folder and returns the relative path
+boost::filesystem::path resetFilesystem(
+    boost::filesystem::path folder = "actual") {
+  boost::filesystem::remove_all(folder);
+  boost::filesystem::create_directory(folder);
+  return folder;
+}
+
 // Templated round-trip serialization
 template<class T>
 void roundtrip(const T& input, T& output) {
-  // Serialize
   std::string serialized = serialize(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
   deserialize(serialized, output);
 }
 
-// This version requires equality operator
+// Templated round-trip serialization using a file
+template<class T>
+void roundtripFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.dat";
+  serializeToFile(input, path.string());
+  deserializeFromFile(path.string(), output);
+}
+
+// This version requires equality operator and uses string and file round-trips
 template<class T>
 bool equality(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtrip<T>(input,output);
-  return input==output;
+  roundtripFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
@@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
 // Templated round-trip serialization using XML
 template<class T>
 void roundtripXML(const T& input, T& output) {
-  // Serialize
   std::string serialized = serializeXML<T>(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
-  // De-serialize
   deserializeXML(serialized, output);
 }
 
+// Templated round-trip serialization using XML File
+template<class T>
+void roundtripXMLFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.xml";
+  serializeToXMLFile(input, path.string());
+  deserializeFromXMLFile(path.string(), output);
+}
+
 // This version requires equality operator
 template<class T>
 bool equalityXML(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtripXML<T>(input,output);
-  return input==output;
+  roundtripXMLFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
@@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
 // Templated round-trip serialization using XML
 template<class T>
 void roundtripBinary(const T& input, T& output) {
-  // Serialize
   std::string serialized = serializeBinary<T>(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
-  // De-serialize
   deserializeBinary(serialized, output);
 }
 
+// Templated round-trip serialization using Binary file
+template<class T>
+void roundtripBinaryFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.bin";
+  serializeToBinaryFile(input, path.string());
+  deserializeFromBinaryFile(path.string(), output);
+}
+
 // This version requires equality operator
 template<class T>
 bool equalityBinary(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtripBinary<T>(input,output);
-  return input==output;
+  roundtripBinaryFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable

gtsam/base/types.h

@@ -230,3 +230,50 @@ namespace std {
 #ifdef ERROR
 #undef ERROR
 #endif
+
+namespace gtsam {
+
+  /// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::`
+  template<typename ...> using void_t = void;
+
+  /**
+   * A SFINAE trait to mark classes that need special alignment.
+   *
+   * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python
+   * wrappers to work properly.
+   *
+   * Explanation
+   * =============
+   * When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template
+   * will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`.
+   *
+   * Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`.
+   *
+   * Please refer to `gtsam/base/make_shared.h` for an example.
+   */
+  template<typename, typename = void_t<>>
+  struct needs_eigen_aligned_allocator : std::false_type {
+  };
+  template<typename T>
+  struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
+  };
+
+}
+
+/**
+ * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
+ * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
+ * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+  using _eigen_aligned_allocator_trait = void;
+
+/**
+ * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
+ * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
+ * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+  using _eigen_aligned_allocator_trait = void;

gtsam/geometry/BearingRange.h

@@ -162,7 +162,7 @@ private:
     NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
   };
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Declare this to be both Testable and a Manifold

gtsam/geometry/CameraSet.h

@@ -319,7 +319,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<class CAMERA>

gtsam/geometry/EssentialMatrix.h

@@ -212,7 +212,7 @@ class EssentialMatrix {
   /// @}
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<>

gtsam/geometry/PinholeCamera.h

@@ -325,7 +325,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits

gtsam/geometry/PinholePose.h

@@ -222,7 +222,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholeBaseK
 
@@ -425,7 +425,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholePose
 

gtsam/geometry/Pose2.h

@@ -317,7 +317,7 @@ public:
 
 public:
   // Align for Point2, which is either derived from, or is typedef, of Vector2
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */

gtsam/geometry/Pose3.h

@@ -355,7 +355,7 @@ public:
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 // Pose3 class

gtsam/geometry/Rot3.h

@@ -544,7 +544,7 @@ namespace gtsam {
 #ifdef GTSAM_USE_QUATERNIONS
   // only align if quaternion, Matrix3 has no alignment requirements
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
   };
 

gtsam/geometry/SOn.h

@@ -20,8 +20,8 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/make_shared.h>
 #include <gtsam/dllexport.h>
-
 #include <Eigen/Core>
 
 #include <iostream> // TODO(frank): how to avoid?
@@ -54,7 +54,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
   using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
 
  protected:
   MatrixNN matrix_;  ///< Rotation matrix

gtsam/geometry/Unit3.h

@@ -214,7 +214,7 @@ private:
   /// @}
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // Define GTSAM traits

gtsam/geometry/triangulation.h

@@ -215,7 +215,7 @@ struct CameraProjectionMatrix {
 private:
   const Matrix3 K_;
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**

gtsam/navigation/AttitudeFactor.h

@@ -139,7 +139,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
@@ -219,7 +219,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits

gtsam/navigation/CombinedImuFactor.h

@@ -100,7 +100,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 
@@ -210,7 +210,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -332,7 +332,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // class CombinedImuFactor
 

gtsam/navigation/ImuBias.h

@@ -171,7 +171,7 @@ private:
 
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
   /// @}
 
 }; // ConstantBias class

gtsam/navigation/PreintegratedRotation.h

@@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 
@@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation {
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
   public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 

gtsam/navigation/PreintegrationBase.h

@@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase {
 #endif
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 }  /// namespace gtsam

gtsam/navigation/PreintegrationParams.h

@@ -84,7 +84,7 @@ protected:
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 

gtsam/navigation/TangentPreintegration.h

@@ -141,7 +141,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam

gtsam/nonlinear/ExpressionFactor.h

@@ -209,7 +209,7 @@ private:
  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
   public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 // ExpressionFactor
 

gtsam/nonlinear/NonlinearEquality.h

@@ -175,7 +175,7 @@ public:
 
   /// @}
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
 private:
 
@@ -265,7 +265,7 @@ public:
     traits<X>::Print(value_, "Value");
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
 private:
 
@@ -331,7 +331,7 @@ public:
     return traits<X>::Local(x1,x2);
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
 private:
 

gtsam/nonlinear/PriorFactor.h

@@ -114,7 +114,7 @@ namespace gtsam {
 	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 	enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
   public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
 
 } /// namespace gtsam

gtsam/nonlinear/internal/ExpressionNode.h

@@ -150,7 +150,7 @@ public:
     return constant_;
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 //-----------------------------------------------------------------------------

gtsam/slam/BetweenFactor.h

@@ -126,7 +126,7 @@ namespace gtsam {
 	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
     public:
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+      GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits

gtsam/slam/EssentialMatrixConstraint.h

@@ -105,7 +105,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // \class EssentialMatrixConstraint
 

gtsam/slam/EssentialMatrixFactor.h

@@ -81,7 +81,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -201,7 +201,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor2
 
@@ -286,7 +286,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor3
 

gtsam/slam/ProjectionFactor.h

@@ -189,7 +189,7 @@ namespace gtsam {
     }
 
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
   /// traits

gtsam/slam/RotateFactor.h

@@ -113,7 +113,7 @@ public:
     return error;
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 }  // namespace gtsam
 

gtsam/slam/SmartFactorBase.h

@@ -81,7 +81,7 @@ protected:
   mutable FBlocks Fs;
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;

gtsam_unstable/slam/PoseToPointFactor.h

@@ -0,0 +1,90 @@
+/**
+ *  @file   PoseToPointFactor.hpp
+ *  @brief  This factor can be used to track a 3D landmark over time by
+ *providing local measurements of its location.
+ *  @author David Wisth
+ **/
+#pragma once
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <ostream>
+
+namespace gtsam {
+
+/**
+ * A class for a measurement between a pose and a point.
+ * @addtogroup SLAM
+ */
+class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
+ private:
+  typedef PoseToPointFactor This;
+  typedef NoiseModelFactor2<Pose3, Point3> Base;
+
+  Point3 measured_; /** the point measurement in local coordinates */
+
+ public:
+  // shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
+
+  /** default constructor - only use for serialization */
+  PoseToPointFactor() {}
+
+  /** Constructor */
+  PoseToPointFactor(Key key1, Key key2, const Point3& measured,
+                    const SharedNoiseModel& model)
+      : Base(model, key1, key2), measured_(measured) {}
+
+  virtual ~PoseToPointFactor() {}
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+                                               DefaultKeyFormatter) const {
+    std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
+              << keyFormatter(this->key2()) << ")\n"
+              << "  measured: " << measured_.transpose() << std::endl;
+    this->noiseModel_->print("  noise model: ");
+  }
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected,
+                      double tol = 1e-9) const {
+    const This* e = dynamic_cast<const This*>(&expected);
+    return e != nullptr && Base::equals(*e, tol) &&
+           traits<Point3>::Equals(this->measured_, e->measured_, tol);
+  }
+
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors
+   * @brief Error = wTwi.inverse()*wPwp - measured_
+   * @param wTwi The pose of the sensor in world coordinates
+   * @param wPwp The estimated point location in world coordinates
+   *
+   * Note: measured_ and the error are in local coordiantes.
+   */
+  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    return wTwi.transformTo(wPwp, H1, H2) - measured_;
+  }
+
+  /** return the measured */
+  const Point3& measured() const { return measured_; }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(measured_);
+  }
+
+};  // \class PoseToPointFactor
+
+}  // namespace gtsam

gtsam_unstable/slam/tests/testPoseToPointFactor.h

@@ -0,0 +1,86 @@
+/**
+ * @file   testPoseToPointFactor.cpp
+ * @brief
+ * @author David Wisth
+ * @date   June 20, 2020
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam_unstable/slam/PoseToPointFactor.h>
+
+using namespace gtsam;
+using namespace gtsam::noiseModel;
+
+/// Verify zero error when there is no noise
+TEST(PoseToPointFactor, errorNoiseless) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(0.0, 0.0, 0.0);
+  Point3 measured = t + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = Vector3(0.0, 0.0, 0.0);
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/// Verify expected error in test scenario
+TEST(PoseToPointFactor, errorNoise) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(-1.0, 0.5, 0.3);
+  Point3 measured = t + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = noise;
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/// Check Jacobians are correct
+TEST(PoseToPointFactor, jacobian) {
+  // Measurement
+  gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
+
+  // Linearisation point
+  gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
+  gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
+  Pose3 p(p_R, p_t);
+
+  gtsam::Point3 l = gtsam::Point3(3, 0, 5);
+
+  // Factor
+  Key pose_key(1);
+  Key point_key(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+  PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
+                       boost::none, boost::none);
+  Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
+  Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
+
+  // Use the factor to calculate the derivative
+  Matrix actual_H1;
+  Matrix actual_H2;
+  factor.evaluateError(p, l, actual_H1, actual_H2);
+
+  // Verify we get the expected error
+  EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

wrap/Module.cpp

@@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
                  "        T* get()\n"
                  "        long use_count() const\n"
                  "        T& operator*()\n\n"
-                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n"
+                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n";
+
+  // gtsam alignment-friendly shared_ptr
+  pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n"
                  "    cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
 
   for(const TypedefPair& types: typedefs)

wrap/tests/expected-cython/geometry.pxd

@@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost":
         T& operator*()
 
     cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)
+
+cdef extern from "gtsam/base/make_shared.h" namespace "gtsam":
     cdef shared_ptr[T] make_shared[T](const T& r)
 
 cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":