Fix for STL container of objects containing fixed-size Eigen objects

Examples/ROS/ORB_VIO/src/ros_vio.cc

@@ -119,7 +119,7 @@ int main(int argc, char **argv)
 
         if(bdata)
         {
-            std::vector<ORB_SLAM2::IMUData> vimuData;
+            ORB_SLAM2::IMUData::vector_t vimuData;
             //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec());
             for(unsigned int i=0;i<vimuMsg.size();i++)
             {
@@ -200,7 +200,7 @@ int main(int argc, char **argv)
 
             if(bdata)
             {
-                std::vector<ORB_SLAM2::IMUData> vimuData;
+                ORB_SLAM2::IMUData::vector_t vimuData;
                 //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec());
                 for(unsigned int i=0;i<vimuMsg.size();i++)
                 {

include/Frame.h

@@ -36,6 +36,8 @@
 #include <IMU/NavState.h>
 #include <IMU/IMUPreintegrator.h>
 
+#include <Eigen/StdVector>
+
 namespace ORB_SLAM2
 {
 #define FRAME_GRID_ROWS 48
@@ -50,7 +52,7 @@ class Frame
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     // Constructor for Monocular VI
-    Frame(const cv::Mat &imGray, const double &timeStamp, const std::vector<IMUData> &vimu, ORBextractor* extractor,ORBVocabulary* voc,
+    Frame(const cv::Mat &imGray, const double &timeStamp, const IMUData::vector_t &vimu, ORBextractor* extractor,ORBVocabulary* voc,
           cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, KeyFrame* pLastKF=NULL);
 
     void ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator& imupreint) const;
@@ -63,7 +65,7 @@ class Frame
     void SetNavStateBiasAcc(const Vector3d &ba);
 
     // IMU Data from last Frame to this Frame
-    std::vector<IMUData> mvIMUDataSinceLastFrame;
+    IMUData::vector_t mvIMUDataSinceLastFrame;
 
     // For pose optimization, use as prior and prior information(inverse covariance)
     Matrix<double,15,15> mMargCovInv;

include/KeyFrame.h

@@ -34,6 +34,8 @@
 #include "IMU/NavState.h"
 #include "IMU/IMUPreintegrator.h"
 
+#include <Eigen/StdVector>
+
 namespace ORB_SLAM2
 {
 
@@ -47,13 +49,13 @@ class KeyFrame
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, std::vector<IMUData> vIMUData, KeyFrame* pLastKF=NULL);
+    KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, const IMUData::vector_t& vIMUData, KeyFrame* pLastKF=NULL);
     KeyFrame* GetPrevKeyFrame(void);
     KeyFrame* GetNextKeyFrame(void);
     void SetPrevKeyFrame(KeyFrame* pKF);
     void SetNextKeyFrame(KeyFrame* pKF);
 
-    std::vector<IMUData> GetVectorIMUData(void);
+    IMUData::vector_t GetVectorIMUData(void);
     void AppendIMUDataToFront(KeyFrame* pPrevKF);
     void ComputePreInt(void);
 
@@ -92,7 +94,7 @@ class KeyFrame
 
     // IMU Data from lask KeyFrame to this KeyFrame
     std::mutex mMutexIMUData;
-    std::vector<IMUData> mvIMUData;
+    IMUData::vector_t mvIMUData;
     IMUPreintegrator mIMUPreInt;
 
 

include/Optimizer.h

@@ -27,6 +27,8 @@
 #include "LoopClosing.h"
 #include "Frame.h"
 
+#include <Eigen/StdVector>
+
 #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
 
 namespace ORB_SLAM2
@@ -51,8 +53,8 @@ class Optimizer
 
     Vector3d static OptimizeInitialGyroBias(const std::list<KeyFrame*> &lLocalKeyFrames);
     Vector3d static OptimizeInitialGyroBias(const std::vector<KeyFrame*> &vLocalKeyFrames);
-    Vector3d static OptimizeInitialGyroBias(const std::vector<Frame> &vFrames);
-    Vector3d static OptimizeInitialGyroBias(const vector<cv::Mat>& vTwc, const vector<IMUPreintegrator>& vImuPreInt);
+    Vector3d static OptimizeInitialGyroBias(const std::vector<Frame, Eigen::aligned_allocator<Frame> > &vFrames);
+    Vector3d static OptimizeInitialGyroBias(const vector<cv::Mat>& vTwc, const IMUPreintegrator::vector_t& vImuPreInt);
 
     void static LocalBundleAdjustment(KeyFrame *pKF, const std::list<KeyFrame*> &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, LocalMapping* pLM=NULL);
 

include/System.h

@@ -82,7 +82,7 @@ class System
     // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
     // Returns the camera pose (empty if tracking fails).
     cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp);
-    cv::Mat TrackMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu, const double &timestamp);
+    cv::Mat TrackMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double &timestamp);
 
     // This stops local mapping thread (map building) and performs only camera tracking.
     void ActivateLocalizationMode();

include/Tracking.h

@@ -43,6 +43,8 @@
 
 #include <mutex>
 
+#include <Eigen/StdVector> // vector of objects that contain fixed-size vectorizable eigen types need special care
+
 namespace ORB_SLAM2
 {
 
@@ -63,7 +65,7 @@ class Tracking
     bool mbRelocBiasPrepare;
     void RecomputeIMUBiasAndCurrentNavstate(NavState& nscur);
     // 20 Frames are used to compute bias
-    vector<Frame> mv20FramesReloc;
+    vector<Frame, Eigen::aligned_allocator<Frame> > mv20FramesReloc;
 
     // Predict the NavState of Current Frame by IMU
     void PredictNavStateByIMU(bool bMapUpdated);
@@ -73,11 +75,11 @@ class Tracking
     bool TrackLocalMapWithIMU(bool bMapUpdated=false);
 
     ConfigParam* mpParams;
-    cv::Mat GrabImageMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu, const double &timestamp);
+    cv::Mat GrabImageMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double &timestamp);
     // IMU Data since last KF. Append when new data is provided
     // Should be cleared in 1. initialization beginning, 2. new keyframe created.
-    std::vector<IMUData> mvIMUSinceLastKF;
-    IMUPreintegrator GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector<IMUData>& vIMUSInceLastKF);
+    IMUData::vector_t mvIMUSinceLastKF;
+    IMUPreintegrator GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const IMUData::vector_t& vIMUSInceLastKF);
     IMUPreintegrator GetIMUPreIntSinceLastFrame(Frame* pCurF, Frame* pLastF);
 
 

src/Frame.cc

@@ -43,7 +43,7 @@ void Frame::ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator
     // Reset pre-integrator first
     IMUPreInt.reset();
 
-    const std::vector<IMUData>& vIMUSInceLastFrame = mvIMUDataSinceLastFrame;
+    const IMUData::vector_t& vIMUSInceLastFrame = mvIMUDataSinceLastFrame;
 
     Vector3d bg = pLastF->GetNavState().Get_BiasGyr();
     Vector3d ba = pLastF->GetNavState().Get_BiasAcc();
@@ -140,7 +140,7 @@ void Frame::SetNavState(const NavState& ns)
     mNavState = ns;
 }
 
-Frame::Frame(const cv::Mat &imGray, const double &timeStamp, const std::vector<IMUData> &vimu, ORBextractor* extractor,ORBVocabulary* voc,
+Frame::Frame(const cv::Mat &imGray, const double &timeStamp, const IMUData::vector_t &vimu, ORBextractor* extractor,ORBVocabulary* voc,
              cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, KeyFrame* pLastKF)
     :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
      mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)

src/IMU/IMUPreintegrator.h

@@ -2,6 +2,7 @@
 #define TVISLAM_IMUPREINTEGRATOR_H
 
 #include <Eigen/Dense>
+#include <Eigen/StdVector>
 
 #include "IMU/imudata.h"
 #include "so3.h"
@@ -18,6 +19,7 @@ class IMUPreintegrator
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    typedef std::vector<IMUPreintegrator, Eigen::aligned_allocator<IMUPreintegrator> > vector_t;
 
     IMUPreintegrator();
     IMUPreintegrator(const IMUPreintegrator& pre);

src/IMU/imudata.h

@@ -2,6 +2,8 @@
 #define IMUDATA_H
 
 #include <Eigen/Dense>
+#include <Eigen/StdVector>
+#include <vector>
 
 namespace ORB_SLAM2
 {
@@ -10,8 +12,10 @@ using namespace Eigen;
 
 class IMUData
 {
+
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    typedef std::vector<IMUData, Eigen::aligned_allocator<IMUData> > vector_t;
 
     // covariance of measurement
     static Matrix3d _gyrMeasCov;

src/KeyFrame.cc

@@ -83,15 +83,15 @@ void KeyFrame::SetNextKeyFrame(KeyFrame* pKF)
     mpNextKeyFrame = pKF;
 }
 
-std::vector<IMUData> KeyFrame::GetVectorIMUData(void)
+IMUData::vector_t KeyFrame::GetVectorIMUData(void)
 {
     unique_lock<mutex> lock(mMutexIMUData);
     return mvIMUData;
 }
 
 void KeyFrame::AppendIMUDataToFront(KeyFrame* pPrevKF)
 {
-    std::vector<IMUData> vimunew = pPrevKF->GetVectorIMUData();
+    IMUData::vector_t vimunew = pPrevKF->GetVectorIMUData();
     {
         unique_lock<mutex> lock(mMutexIMUData);
         vimunew.insert(vimunew.end(), mvIMUData.begin(), mvIMUData.end());
@@ -268,7 +268,7 @@ void KeyFrame::ComputePreInt(void)
 //-------------------------------------------------------------------------------------------
 //-------------------------------------------------------------------------------------------
 
-KeyFrame::KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, std::vector<IMUData> vIMUData, KeyFrame* pPrevKF):
+KeyFrame::KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, const IMUData::vector_t& vIMUData, KeyFrame* pPrevKF):
     mnFrameId(F.mnId),  mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
     mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
     mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),

src/LocalMapping.cc

@@ -26,6 +26,8 @@
 #include<mutex>
 #include "Converter.h"
 
+#include <Eigen/StdVector>
+
 namespace ORB_SLAM2
 {
 using namespace std;
@@ -42,7 +44,7 @@ class KeyFrameInit
     KeyFrameInit* mpPrevKeyFrame;
     cv::Mat Twc;
     IMUPreintegrator mIMUPreInt;
-    std::vector<IMUData> mvIMUData;
+    IMUData::vector_t mvIMUData;
     Vector3d bg;
 
 
@@ -244,7 +246,7 @@ bool LocalMapping::TryInitVIO(void)
     int N = vScaleGravityKF.size();
     KeyFrame* pNewestKF = vScaleGravityKF[N-1];
     vector<cv::Mat> vTwc;
-    vector<IMUPreintegrator> vIMUPreInt;
+    IMUPreintegrator::vector_t vIMUPreInt;
     // Store initialization-required KeyFrame data
     vector<KeyFrameInit*> vKFInit;
 

src/Optimizer.cc

@@ -2779,7 +2779,7 @@ void Optimizer::LocalBundleAdjustmentNavState(KeyFrame *pCurKF, const std::list<
 
 }
 
-Vector3d Optimizer::OptimizeInitialGyroBias(const std::vector<Frame> &vFrames)
+Vector3d Optimizer::OptimizeInitialGyroBias(const std::vector<Frame, Eigen::aligned_allocator<Frame> > &vFrames)
 {
     //size_t N = vpKFs.size();
     Matrix4d Tbc = ConfigParam::GetEigTbc();
@@ -2918,7 +2918,7 @@ Vector3d Optimizer::OptimizeInitialGyroBias(const std::vector<KeyFrame *> &vpKFs
     return vBgEst->estimate();
 }
 
-Vector3d Optimizer::OptimizeInitialGyroBias(const vector<cv::Mat>& vTwc, const vector<IMUPreintegrator>& vImuPreInt)
+Vector3d Optimizer::OptimizeInitialGyroBias(const vector<cv::Mat>& vTwc, const IMUPreintegrator::vector_t& vImuPreInt)
 {
     int N = vTwc.size(); if(vTwc.size()!=vImuPreInt.size()) cerr<<"vTwc.size()!=vImuPreInt.size()"<<endl;
     Matrix4d Tbc = ConfigParam::GetEigTbc();

src/System.cc

@@ -98,7 +98,7 @@ void System::SaveKeyFrameTrajectoryNavState(const string &filename)
     cout << endl << "NavState trajectory saved!" << endl;
 }
 
-cv::Mat System::TrackMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu, const double &timestamp)
+cv::Mat System::TrackMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double &timestamp)
 {
     if(mSensor!=MONOCULAR)
     {

src/Tracking.cc

@@ -70,7 +70,7 @@ void Tracking::RecomputeIMUBiasAndCurrentNavstate(NavState& nscur)
         frame.SetNavStateBiasGyr(bg);
     }
     // Re-compute IMU pre-integration
-    vector<IMUPreintegrator> v19IMUPreint;
+    IMUPreintegrator::vector_t v19IMUPreint;
     v19IMUPreint.reserve(20-1);
     for(size_t i=0; i<N; i++)
     {
@@ -400,7 +400,7 @@ bool Tracking::TrackWithIMU(bool bMapUpdated)
 }
 
 
-IMUPreintegrator Tracking::GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector<IMUData>& vIMUSInceLastKF)
+IMUPreintegrator Tracking::GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const IMUData::vector_t& vIMUSInceLastKF)
 {
     // Reset pre-integrator first
     IMUPreintegrator IMUPreInt;
@@ -461,7 +461,7 @@ IMUPreintegrator Tracking::GetIMUPreIntSinceLastFrame(Frame* pCurF, Frame* pLast
 }
 
 
-cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu, const double &timestamp)
+cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double &timestamp)
 {
     mvIMUSinceLastKF.insert(mvIMUSinceLastKF.end(), vimu.begin(),vimu.end());
     mImGray = im;
@@ -1138,7 +1138,7 @@ void Tracking::MonocularInitialization()
 void Tracking::CreateInitialMapMonocular()
 {
     // The first imu package include 2 parts for KF1 and KF2
-    vector<IMUData> vimu1,vimu2;
+    IMUData::vector_t vimu1,vimu2;
     for(size_t i=0; i<mvIMUSinceLastKF.size(); i++)
     {
         IMUData imu = mvIMUSinceLastKF[i];