diff --git a/src/processor_base.h b/src/processor_base.h index f87df2394d54ad7abf44637d971fa550b857968a..19e5f665dcc484bb26a49b9ce98b458ba02f131a 100644 --- a/src/processor_base.h +++ b/src/processor_base.h @@ -40,6 +40,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce static unsigned int processor_id_count_; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; ProcessorBase(const std::string& _type, const Scalar& _time_tolerance = 0); virtual ~ProcessorBase(); void remove(); diff --git a/src/processor_motion.h b/src/processor_motion.h index cd1c41c7f9e2fbe784abfef79ba91768e2afba95..f1a19ebb2e606d51131ee7cf1e93be7a8dddf6c7 100644 --- a/src/processor_motion.h +++ b/src/processor_motion.h @@ -108,6 +108,7 @@ class ProcessorMotion : public ProcessorBase // This is the main public interface public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; ProcessorMotion(const std::string& _type, Size _state_size, Size _delta_size, diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h index be2654cf032ae7ab1484e162470e883ecb10b7f7..2e0e1be46be316dfcc4ccfa7e504a839e92b3f98 100644 --- a/src/processor_odom_2D.h +++ b/src/processor_odom_2D.h @@ -30,6 +30,7 @@ struct ProcessorParamsOdom2D : public ProcessorParamsBase class ProcessorOdom2D : public ProcessorMotion { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; ProcessorOdom2D(const Scalar& _traveled_dist_th = 1.0, const Scalar& _theta_traveled_th = 0.17, const Scalar& _cov_det_th = 1.0, diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h index 1a806888cff43bdbb3d84684258aca9ec641f48a..2437da8e1d8a2bf58d369e71c69c35937fef1f9b 100644 --- a/src/processor_odom_3D.h +++ b/src/processor_odom_3D.h @@ -66,6 +66,7 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D); class ProcessorOdom3D : public ProcessorMotion { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; ProcessorOdom3D(ProcessorOdom3DParamsPtr _params = nullptr, SensorOdom3DPtr _sensor_ptr = nullptr); virtual ~ProcessorOdom3D(); void setup(SensorOdom3DPtr sen_ptr); @@ -121,7 +122,7 @@ class ProcessorOdom3D : public ProcessorMotion Eigen::Map<Eigen::Vector3s> p_out_; Eigen::Map<const Eigen::Quaternions> q1_, q2_; Eigen::Map<Eigen::Quaternions> q_out_; - void remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out); + void remap(const Eigen::Ref<const Eigen::VectorXs> _x1, const Eigen::Ref<const Eigen::VectorXs> _x2, Eigen::Ref<Eigen::VectorXs> _x_out); // Factory method public: @@ -156,9 +157,9 @@ inline FeatureBasePtr ProcessorOdom3D::emplaceFeature(CaptureMotionPtr _capture_ return key_feature_ptr; } -inline void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, - const Eigen::VectorXs& _x2, - Eigen::VectorXs& _x_out) +inline void ProcessorOdom3D::remap(const Eigen::Ref<const Eigen::VectorXs> _x1, + const Eigen::Ref<const Eigen::VectorXs> _x2, + Eigen::Ref<Eigen::VectorXs> _x_out) { new (&p1_) Eigen::Map<const Eigen::Vector3s>(_x1.data()); new (&q1_) Eigen::Map<const Eigen::Quaternions>(_x1.data() + 3); @@ -168,6 +169,7 @@ inline void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, new (&q_out_) Eigen::Map<Eigen::Quaternions>(_x_out.data() + 3); } + } // namespace wolf #endif /* SRC_PROCESSOR_ODOM_3D_H_ */ diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h index 6cd2ad3368ab1c4f9c7612512ef63debbc355bfc..277e73b7073b09e236041e8875d7591501f5b3a0 100644 --- a/src/sensor_odom_2D.h +++ b/src/sensor_odom_2D.h @@ -21,7 +21,8 @@ class SensorOdom2D : public SensorBase Scalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation - public: + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; /** \brief Constructor with arguments * * Constructor with arguments diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h index c676edc95a66756a7d28b68299127dbd09a60d30..e360af1971b326ba311e04bb69313f35ca373830 100644 --- a/src/sensor_odom_3D.h +++ b/src/sensor_odom_3D.h @@ -44,6 +44,7 @@ class SensorOdom3D : public SensorBase Scalar min_rot_var_; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; /** \brief Constructor with arguments * * Constructor with arguments