diff --git a/src/active_search.h b/src/active_search.h
index 7af3a63b96e5e0378cfe77240ecc262372069ab5..33625850f611fc33e3e8f62ce6310c7dc4b12ab4 100644
--- a/src/active_search.h
+++ b/src/active_search.h
@@ -116,6 +116,8 @@ class ActiveSearchGrid {
         int margin_;
 
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         /**
          * \brief Void constructor
          *
diff --git a/src/ceres_wrapper/cost_function_wrapper.h b/src/ceres_wrapper/cost_function_wrapper.h
index c8668959e0a113842113ab3777d438bc24da5cf7..d6ecac9ffadc76df53ec238aab1e04981ef7863a 100644
--- a/src/ceres_wrapper/cost_function_wrapper.h
+++ b/src/ceres_wrapper/cost_function_wrapper.h
@@ -8,6 +8,9 @@
 // CERES
 #include "ceres/cost_function.h"
 
+// EIGEN
+#include <Eigen/StdVector>
+
 namespace wolf {
 
 class CostFunctionWrapper : public ceres::CostFunction
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 01ff8c88b541550503be36307b3cc17d9ec1eeab..381cd8285ef3d34c174e89dc59a9f97278b2165d 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -4,6 +4,7 @@
 
 //Wolf includes
 #include "constraint_base.h"
+#include <Eigen/StdVector>
 
 namespace wolf {
 
diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h
index 71fb7d29dfee7720527d6bb9864d6c829e60ced2..6e007f65806e1f1ecb56d88aaefbb8059cb9799b 100644
--- a/src/constraint_gps_pseudorange_2D.h
+++ b/src/constraint_gps_pseudorange_2D.h
@@ -75,6 +75,9 @@ protected:
     Eigen::Vector3s sat_position_;
     Scalar pseudorange_;
 
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
 };
 
 /*
diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_gps_pseudorange_3D.h
index 4d85a066e9c4c2d0cb314a7ce47f169721fbc1c4..2edf0aa3c77cf31f038d1eb71b54137c8eb0fddc 100644
--- a/src/constraint_gps_pseudorange_3D.h
+++ b/src/constraint_gps_pseudorange_3D.h
@@ -74,8 +74,9 @@ protected:
     Eigen::Vector3s sat_position_;
     Scalar pseudorange_;
 
-
 public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
     static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
                                         NodeBase* _correspondant_ptr = nullptr)
     {
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index 8bb0045147fafa7f05143626d94bf9eec5091ef1..df4ef4792b789610d855ab988a527dd04aa8736e 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -3,6 +3,7 @@
 
 //Wolf includes
 #include "constraint_relative_2D_analytic.h"
+#include <Eigen/StdVector>
 
 namespace wolf {
 
diff --git a/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h
index 80926e11811706b4d1eeba03ca78ba9326bd079e..c85735d2e17e66018271be0135d69081acd5c19b 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/constraint_relative_2D_analytic.h
@@ -4,6 +4,7 @@
 //Wolf includes
 #include "constraint_analytic.h"
 #include "landmark_base.h"
+#include <Eigen/StdVector>
 
 namespace wolf {
 
diff --git a/src/examples/test_capture_laser_2D.cpp b/src/examples/test_capture_laser_2D.cpp
index 5d64d279e7e4723aafda319bef5e52a88a5a6d05..f5c43ac41a2732c51104b5b08fec1f08b2980392 100644
--- a/src/examples/test_capture_laser_2D.cpp
+++ b/src/examples/test_capture_laser_2D.cpp
@@ -5,6 +5,9 @@
 //wolf
 #include "capture_laser_2D.h"
 
+// Eigen in std vector
+#include <Eigen/StdVector>
+
 //main
 int main(int argc, char *argv[])
 {
@@ -91,8 +94,7 @@ int main(int argc, char *argv[])
     device_pose << 0,0,0,0,0,0; //origin, no rotation
     TimeStamp time_stamp;
     time_stamp.setToNow();
-    std::list<Eigen::Vector4s> corner_list;
-    std::list<Eigen::Vector4s>::iterator corner_it;
+    std::list<Eigen::Vector4s, Eigen::aligned_allocator<Eigen::Vector4s> > corner_list;
     
     //Create Device objects 
     //SensorLaser2D device(device_pose, ranges.size(), M_PI, 0.2, 30.0, 0.01);
@@ -120,7 +122,7 @@ int main(int argc, char *argv[])
 
     //print corners
     std::cout << "CORNER LIST" << std::endl;            
-    for ( corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it ++ )
+    for (auto corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it ++ )
     {
         std::cout << corner_it->x() << " , " << corner_it->y() << std::endl;
     }
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index 3950c6a62b5e0d4535c9c6f71b79748855b719fc..b96a83d8bd0b03585f6dc35dcaf5d47096278c54 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -18,6 +18,7 @@
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
+#include <Eigen/StdVector> // Eigen in std vector
 
 namespace wolf{
 // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
diff --git a/src/feature_gps_pseudorange.h b/src/feature_gps_pseudorange.h
index 431efb32ab5a47db507d7c36a0a5d2f02107b2cc..eec2ed45721bb9747d757fbb1622fd768e2d345d 100644
--- a/src/feature_gps_pseudorange.h
+++ b/src/feature_gps_pseudorange.h
@@ -21,6 +21,8 @@ protected:
     Scalar pseudorange_;
 
 public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
     FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, Scalar _pseudorange, Scalar _covariance);
 
     /** \brief Default destructor (not recommended)
diff --git a/src/feature_line_2D.h b/src/feature_line_2D.h
index 721a27ecaddc766616242ddc694d1afa2dc688a6..be36db1859a46f9756785816db3cf46a5b1fe8ba 100644
--- a/src/feature_line_2D.h
+++ b/src/feature_line_2D.h
@@ -23,6 +23,8 @@ class FeatureLine2D : public FeatureBase
         Eigen::Vector3s last_point_;
         
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         /** \brief Constructor 
          * 
          * Constructor, with measurement and descriptor
diff --git a/src/landmark_line_2D.h b/src/landmark_line_2D.h
index 65979866805a82ffe448e760c477a7aad3b82f89..8a016eeffc418b636445601f103ab9afcc866774 100644
--- a/src/landmark_line_2D.h
+++ b/src/landmark_line_2D.h
@@ -20,6 +20,8 @@ class LandmarkLine2D : public LandmarkBase
         Eigen::Vector3s point2_;
         
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         /** \brief Constructor with homogeneous parameters of the line
          *
          * \param _p_ptr homogeneous parameters of the line: (a,b,c) from ax+by+c=0, normalized according line/sqrt(a*a+b*b) 
diff --git a/src/processor_laser_2D.cpp b/src/processor_laser_2D.cpp
index 59adf8e1f0e2e3dc7ae7b1c058830f2ddf2825a4..4492fb540d9ed2831650bc92e927d53c29accafe 100644
--- a/src/processor_laser_2D.cpp
+++ b/src/processor_laser_2D.cpp
@@ -138,8 +138,8 @@ void ProcessorLaser2D::establishConstraintsMHTree()
         Eigen::VectorXs squared_mahalanobis_distances;
 
         // COMPUTING ALL EXPECTED FEATURES
-        std::map<LandmarkBase*, Eigen::Vector4s> expected_features;
-        std::map<LandmarkBase*, Eigen::Matrix3s> expected_features_covs;
+        std::map<LandmarkBase*, Eigen::Vector4s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Vector4s> > > expected_features;
+        std::map<LandmarkBase*, Eigen::Matrix3s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Matrix3s> > > expected_features_covs;
         unsigned int i = 0;
         for (auto j_it = getProblem()->getMapPtr()->getLandmarkListPtr()->begin(); j_it != getProblem()->getMapPtr()->getLandmarkListPtr()->end(); j_it++, i++)
             computeExpectedFeature(*j_it, expected_features[*j_it], expected_features_covs[*j_it]);
diff --git a/src/processor_tracker_feature_corner.h b/src/processor_tracker_feature_corner.h
index e72e943e92ac90291d268041ada37d57d977e0ba..277dd9ac5a0bd64a4da7d119c57cf32a86e954d8 100644
--- a/src/processor_tracker_feature_corner.h
+++ b/src/processor_tracker_feature_corner.h
@@ -59,6 +59,8 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
         bool extrinsics_transformation_computed_;
 
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         ProcessorTrackerFeatureCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
                                       const unsigned int& _n_corners_th);
         virtual ~ProcessorTrackerFeatureCorner();
diff --git a/src/processor_tracker_landmark_corner.cpp b/src/processor_tracker_landmark_corner.cpp
index 0c8e5b0909ec06f53627ee05dc00375ba2a6d6e7..ac128b498e6f2d5e4b93c85c1d32eeb71f1d4a1d 100644
--- a/src/processor_tracker_landmark_corner.cpp
+++ b/src/processor_tracker_landmark_corner.cpp
@@ -50,8 +50,8 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis
     Scalar dm2;
 
     // COMPUTING ALL EXPECTED FEATURES
-    std::map<LandmarkBase*, Eigen::Vector4s> expected_features;
-    std::map<LandmarkBase*, Eigen::Matrix3s> expected_features_covs;
+    std::map<LandmarkBase*, Eigen::Vector4s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Vector4s> > > expected_features;
+    std::map<LandmarkBase*, Eigen::Matrix3s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Matrix3s> > > expected_features_covs;
     for (auto landmark : not_matched_landmarks)
         expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
 
@@ -103,8 +103,8 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis
         unsigned int ii, jj;
 
         // COMPUTING ALL EXPECTED FEATURES
-        std::map<LandmarkBase*, Eigen::Vector4s> expected_features;
-        std::map<LandmarkBase*, Eigen::Matrix3s> expected_features_covs;
+        std::map<LandmarkBase*, Eigen::Vector4s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Vector4s> > > expected_features;
+        std::map<LandmarkBase*, Eigen::Matrix3s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Matrix3s> > > expected_features_covs;
         for (auto landmark : _landmarks_corner_searched)
             expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
         //std::cout << "expected features!" << std::endl;
diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h
index 0a8a5ac2e7ea1fd01d78d194b2587c72c66c6a4c..c94d0b14eb686082aa8d16a0b255b501e4b96ee8 100644
--- a/src/processor_tracker_landmark_corner.h
+++ b/src/processor_tracker_landmark_corner.h
@@ -77,6 +77,8 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         bool extrinsics_transformation_computed_;
 
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         ProcessorTrackerLandmarkCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
                                        const unsigned int& _new_corners_th, const unsigned int& _loop_frames_th);
 
diff --git a/src/processor_tracker_landmark_polyline.h b/src/processor_tracker_landmark_polyline.h
index f8da14b4e4921156a5bb146d371dbcbdebe89726..fc663e8b1ddb6f1e035064de7ee922ef4d66b4fc 100644
--- a/src/processor_tracker_landmark_polyline.h
+++ b/src/processor_tracker_landmark_polyline.h
@@ -79,6 +79,8 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
         bool extrinsics_transformation_computed_;
 
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         ProcessorTrackerLandmarkPolyline(const ProcessorParamsPolyline& _params);
 
         virtual ~ProcessorTrackerLandmarkPolyline();
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index a3e25abfc5b1a59cce6512af21075710ef87f096..d03a615b386afd6b1b0b53305bd67379d776d82b 100644
--- a/src/sensor_camera.h
+++ b/src/sensor_camera.h
@@ -34,27 +34,29 @@ class SensorCamera : public SensorBase
          **/
         SensorCamera(StateBlock* _p_ptr, StateBlock* _o_ptr, StateBlock* _intr_ptr, int _img_width, int _img_height);
 
-    SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera * _intrinsics_ptr);
+        SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera * _intrinsics_ptr);
 
-    /** \brief Default destructor (not recommended)
-     *
-     * Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
-     *
-     **/
-    virtual ~SensorCamera();
+        /** \brief Default destructor (not recommended)
+         *
+         * Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
+         *
+         **/
+        virtual ~SensorCamera();
 
-    Eigen::VectorXs getDistortionVector(){return distortion_;}
-    Eigen::VectorXs getCorrectionVector(){return correction_;}
-    int getImgWidth(){return img_width_;}
-    int getImgHeight(){return img_height_;}
+        Eigen::VectorXs getDistortionVector(){return distortion_;}
+        Eigen::VectorXs getCorrectionVector(){return correction_;}
+        int getImgWidth(){return img_width_;}
+        int getImgHeight(){return img_height_;}
 
     private:
-    int img_width_;
-    int img_height_;
-    Eigen::VectorXs distortion_;
-    Eigen::VectorXs correction_;
+        int img_width_;
+        int img_height_;
+        Eigen::VectorXs distortion_;
+        Eigen::VectorXs correction_;
 
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
+
         static SensorBase* create(const std::string & _unique_name, //
                                   const Eigen::VectorXs& _extrinsics, //
                                   const IntrinsicsBase* _intrinsics);
diff --git a/src/solver/cost_function_base.h b/src/solver/cost_function_base.h
index 01d790371b3bd71f54093228e2c3e2f26ffac619..76ba74da37e4fda29c140ecd033ede9597834dd8 100644
--- a/src/solver/cost_function_base.h
+++ b/src/solver/cost_function_base.h
@@ -9,6 +9,7 @@
 #define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
 
 #include "wolf.h"
+#include <Eigen/StdVector>
 
 class CostFunctionBase
 {
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index 1d039060c6ee827a8de9986518f0af12763441fa..781a9d22b45b30728b3cbc92affb28f4629bbebd 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -30,6 +30,7 @@
 // eigen includes
 #include <eigen3/Eigen/OrderingMethods>
 #include <eigen3/Eigen/SparseQR>
+#include <Eigen/StdVector>
 
 namespace wolf
 {