diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h index 40a9abefa..ea3b571c2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h +++ b/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 diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..1e264ccba 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -37,7 +37,7 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector>& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e28185038..558a6c740 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -36,7 +36,7 @@ public: } /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, + JacobianFactorSVD(const std::vector>& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index eab7810db..a185cefd5 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -33,6 +33,7 @@ #include #include #include +#include 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 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& Fblocks, Matrix& E, + double computeJacobians(std::vector>& 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& Fblocks, Matrix& E, + double computeJacobians(std::vector>& 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 Fblocks; + std::vector> 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& Fblocks, Matrix& Enull, + double computeJacobiansSVD(std::vector>& 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 > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; + std::vector> Fblocks; Matrix E; Matrix3 PointCov; Vector b; @@ -652,7 +655,7 @@ public: boost::shared_ptr 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 > Fblocks; Vector b; Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 679a6548d..96f5568dd 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -43,6 +43,8 @@ protected: public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SmartStereoProjectionFactorState() { } // Hessian representation (after Schur complement) diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index b6fad38fa..9b7763ef2 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -47,6 +47,8 @@ protected: public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// shorthand for base class type typedef SmartStereoProjectionFactor Base; diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4e3cbf70d..b5ee3d61a 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/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()); }