Fix more memory alignment issues

gtsam/geometry/CalibratedCamera.h

@@ -230,6 +230,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(pose_);
   }
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 };
 // end of class PinholeBase
 
@@ -413,6 +416,9 @@ private:
   }
 
   /// @}
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits

gtsam/geometry/PinholeCamera.h

@@ -324,6 +324,8 @@ private:
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits

gtsam/geometry/PinholePose.h

@@ -220,7 +220,9 @@ private:
     & boost::serialization::make_nvp("PinholeBase",
         boost::serialization::base_object<PinholeBase>(*this));
   }
-
+  
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholeBaseK
 
@@ -422,6 +424,8 @@ private:
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholePose
 

gtsam/geometry/Unit3.h

@@ -208,6 +208,8 @@ private:
 
   /// @}
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // Define GTSAM traits

gtsam/geometry/triangulation.cpp

@@ -24,7 +24,7 @@
 namespace gtsam {
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
 
   // number of cameras
@@ -53,7 +53,7 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
-Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
+Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
 
   Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);

gtsam/geometry/triangulation.h

@@ -53,7 +53,7 @@ public:
  * @return Triangulated point, in homogeneous coordinates
  */
 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol = 1e-9);
 
 /**
@@ -64,7 +64,7 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  * @return Triangulated Point3
  */
 GTSAM_EXPORT Point3 triangulateDLT(
-    const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, 
     double rank_tol = 1e-9);
 
@@ -213,6 +213,8 @@ struct CameraProjectionMatrix {
   }
 private:
   const Matrix3 K_;
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -238,7 +240,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  std::vector<Matrix34> projection_matrices;
+  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
   CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
   for(const Pose3& pose: poses)
     projection_matrices.push_back(createP(pose));
@@ -288,7 +290,7 @@ Point3 triangulatePoint3(
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  std::vector<Matrix34> projection_matrices;
+  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
   for(const CAMERA& camera: cameras)
     projection_matrices.push_back(
         CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(

gtsam/navigation/TangentPreintegration.h

@@ -135,6 +135,9 @@ private:
     ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
     ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
   }
+  
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam

gtsam/slam/EssentialMatrixFactor.h

@@ -80,6 +80,8 @@ public:
     return error;
   }
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -198,6 +200,8 @@ public:
     return f_ * reprojectionError;
   }
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor2
 
@@ -281,6 +285,8 @@ public:
     }
   }
 
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor3
 

gtsam/slam/RegularImplicitSchurFactor.h

@@ -252,7 +252,7 @@ public:
     y += F.transpose() * e3;
   }
 
-  typedef std::vector<Vector2> Error2s;
+  typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
 
   /**
    * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)