From 733089cb8fdf2879beefa5abd006587a6ecf3e81 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 12 Jul 2016 12:48:39 +0200
Subject: [PATCH] Solving assert for fixed size eigen objects alignment
http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html
---
src/active_search.h | 2 ++
src/ceres_wrapper/cost_function_wrapper.h | 3 +++
src/constraint_analytic.h | 1 +
src/constraint_gps_pseudorange_2D.h | 3 +++
src/constraint_gps_pseudorange_3D.h | 3 ++-
src/constraint_odom_2D_analytic.h | 1 +
src/constraint_relative_2D_analytic.h | 1 +
src/examples/test_capture_laser_2D.cpp | 8 +++---
src/examples/test_wolf_prunning.cpp | 1 +
src/feature_gps_pseudorange.h | 2 ++
src/feature_line_2D.h | 2 ++
src/landmark_line_2D.h | 2 ++
src/processor_laser_2D.cpp | 4 +--
src/processor_tracker_feature_corner.h | 2 ++
src/processor_tracker_landmark_corner.cpp | 8 +++---
src/processor_tracker_landmark_corner.h | 2 ++
src/processor_tracker_landmark_polyline.h | 2 ++
src/sensor_camera.h | 32 ++++++++++++-----------
src/solver/cost_function_base.h | 1 +
src/solver/qr_solver.h | 1 +
20 files changed, 56 insertions(+), 25 deletions(-)
diff --git a/src/active_search.h b/src/active_search.h
index 7af3a63b9..33625850f 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 c8668959e..d6ecac9ff 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 01ff8c88b..381cd8285 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 71fb7d29d..6e007f658 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 4d85a066e..2edf0aa3c 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 8bb004514..df4ef4792 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 80926e118..c85735d2e 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 5d64d279e..f5c43ac41 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 3950c6a62..b96a83d8b 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 431efb32a..eec2ed457 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 721a27eca..be36db185 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 659798668..8a016eeff 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 59adf8e1f..4492fb540 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 e72e943e9..277dd9ac5 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 0c8e5b090..ac128b498 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 0a8a5ac2e..c94d0b14e 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 f8da14b4e..fc663e8b1 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 a3e25abfc..d03a615b3 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 01d790371..76ba74da3 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 1d039060c..781a9d22b 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
{
--
GitLab