Fixed abort due to unaligned memory allocation in SmartFactorBase

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h

@@ -11,7 +11,8 @@
 #ifndef EIGEN_STDVECTOR_H
 #define EIGEN_STDVECTOR_H
 
-#include "Eigen/src/StlSupport/details.h"
+//#include "Eigen/src/StlSupport/details.h"
+#include "gtsam/3rdparty/Eigen/Eigen/src/StlSupport/details.h"
 
 /**
  * This section contains a convenience MACRO which allows an easy specialization of

gtsam/slam/JacobianFactorQ.h

@@ -37,7 +37,7 @@ public:
   }
 
   /// Constructor
-  JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D, Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks,
       const Matrix& E, const Matrix3& P, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
       JacobianSchurFactor<D>() {

gtsam/slam/JacobianFactorSVD.h

@@ -36,7 +36,7 @@ public:
   }
 
   /// Constructor
-  JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
+  JacobianFactorSVD(const std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, const Matrix& Enull, const Vector& b,
       const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
     size_t numKeys = Enull.rows() / 2;
     size_t j = 0, m2 = 2*numKeys-3;

gtsam/slam/SmartFactorBase.h

@@ -33,6 +33,7 @@
 #include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
 #include <vector>
+#include <gtsam/3rdparty/Eigen/Eigen/StdVector>
 
 namespace gtsam {
 /// Base class with no internal point, completely functional
@@ -65,6 +66,8 @@ protected:
 
 public:
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
@@ -258,7 +261,7 @@ public:
   // ****************************************************************************************************
   /// Compute F, E only (called below in both vanilla and SVD versions)
   /// Given a Point3, assumes dimensionality is 3
-  double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
+  double computeJacobians(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, Matrix& E,
       Vector& b, const Cameras& cameras, const Point3& point) const {
 
     size_t numKeys = this->keys_.size();
@@ -295,7 +298,7 @@ public:
 
   // ****************************************************************************************************
   /// Version that computes PointCov, with optional lambda parameter
-  double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
+  double computeJacobians(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, Matrix& E,
       Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point,
       double lambda = 0.0, bool diagonalDamping = false) const {
 
@@ -326,7 +329,7 @@ public:
       const double lambda = 0.0) const {
 
     size_t numKeys = this->keys_.size();
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>> Fblocks;
     double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
         lambda);
     F = zeros(Z::Dim() * numKeys, D * numKeys);
@@ -339,7 +342,7 @@ public:
 
   // ****************************************************************************************************
   /// SVD version
-  double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
+  double computeJacobiansSVD(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, Matrix& Enull,
       Vector& b, const Cameras& cameras, const Point3& point, double lambda =
           0.0, bool diagonalDamping = false) const {
 
@@ -639,7 +642,7 @@ public:
   boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>> Fblocks;
     Matrix E;
     Matrix3 PointCov;
     Vector b;
@@ -652,7 +655,7 @@ public:
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
     size_t numKeys = this->keys_.size();
-    std::vector < KeyMatrix2D > Fblocks;
+    std::vector < KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks;
     Vector b;
     Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3);
     computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);

gtsam/slam/SmartStereoProjectionFactor.h

@@ -43,6 +43,8 @@ protected:
 
 public:
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   SmartStereoProjectionFactorState() {
   }
   // Hessian representation (after Schur complement)

gtsam/slam/SmartStereoProjectionPoseFactor.h

@@ -47,6 +47,8 @@ protected:
 
 public:
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// shorthand for base class type
   typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
 

gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -79,6 +79,7 @@ void stereo_projectToMultipleCameras(
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor) {
+  fprintf(stderr,"Test 1 Complete");
   SmartFactor::shared_ptr factor1(new SmartFactor());
 }