From f9e8b64f15512f4b4ea473ec884dc1d782552251 Mon Sep 17 00:00:00 2001 From: Guilherme Lawless Date: Thu, 20 Jul 2017 17:23:01 +0200 Subject: [PATCH 1/3] Fix for STL container of objects containing fixed-size Eigen objects --- include/Optimizer.h | 4 +++- include/Tracking.h | 4 +++- src/Optimizer.cc | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/include/Optimizer.h b/include/Optimizer.h index 8779db8..19ce321 100644 --- a/include/Optimizer.h +++ b/include/Optimizer.h @@ -27,6 +27,8 @@ #include "LoopClosing.h" #include "Frame.h" +#include + #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" namespace ORB_SLAM2 @@ -51,7 +53,7 @@ class Optimizer Vector3d static OptimizeInitialGyroBias(const std::list &lLocalKeyFrames); Vector3d static OptimizeInitialGyroBias(const std::vector &vLocalKeyFrames); - Vector3d static OptimizeInitialGyroBias(const std::vector &vFrames); + Vector3d static OptimizeInitialGyroBias(const std::vector > &vFrames); Vector3d static OptimizeInitialGyroBias(const vector& vTwc, const vector& vImuPreInt); void static LocalBundleAdjustment(KeyFrame *pKF, const std::list &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, LocalMapping* pLM=NULL); diff --git a/include/Tracking.h b/include/Tracking.h index 9cb144b..a82f107 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -43,6 +43,8 @@ #include +#include // 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 mv20FramesReloc; + vector > mv20FramesReloc; // Predict the NavState of Current Frame by IMU void PredictNavStateByIMU(bool bMapUpdated); diff --git a/src/Optimizer.cc b/src/Optimizer.cc index 0fc907a..d7203df 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -2779,7 +2779,7 @@ void Optimizer::LocalBundleAdjustmentNavState(KeyFrame *pCurKF, const std::list< } -Vector3d Optimizer::OptimizeInitialGyroBias(const std::vector &vFrames) +Vector3d Optimizer::OptimizeInitialGyroBias(const std::vector > &vFrames) { //size_t N = vpKFs.size(); Matrix4d Tbc = ConfigParam::GetEigTbc(); From 07d9d89e231150302c64eea7a091c614a22f3284 Mon Sep 17 00:00:00 2001 From: Guilherme Lawless Date: Tue, 25 Jul 2017 11:52:38 +0200 Subject: [PATCH 2/3] IMUData - Solve Eigen unaligned arrays when using STL Containers --- Examples/ROS/ORB_VIO/src/ros_vio.cc | 4 ++-- include/Frame.h | 6 ++++-- include/KeyFrame.h | 8 +++++--- include/System.h | 2 +- include/Tracking.h | 6 +++--- src/Frame.cc | 4 ++-- src/IMU/imudata.h | 4 ++++ src/KeyFrame.cc | 6 +++--- src/LocalMapping.cc | 4 +++- src/System.cc | 2 +- src/Tracking.cc | 6 +++--- 11 files changed, 31 insertions(+), 21 deletions(-) diff --git a/Examples/ROS/ORB_VIO/src/ros_vio.cc b/Examples/ROS/ORB_VIO/src/ros_vio.cc index 350bfd1..30c32f8 100644 --- a/Examples/ROS/ORB_VIO/src/ros_vio.cc +++ b/Examples/ROS/ORB_VIO/src/ros_vio.cc @@ -119,7 +119,7 @@ int main(int argc, char **argv) if(bdata) { - std::vector vimuData; + ORB_SLAM2::IMUData::vector_t vimuData; //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec()); for(unsigned int i=0;i vimuData; + ORB_SLAM2::IMUData::vector_t vimuData; //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec()); for(unsigned int i=0;i #include +#include + 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 &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 mvIMUDataSinceLastFrame; + IMUData::vector_t mvIMUDataSinceLastFrame; // For pose optimization, use as prior and prior information(inverse covariance) Matrix mMargCovInv; diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 5b5085e..5a5d868 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -34,6 +34,8 @@ #include "IMU/NavState.h" #include "IMU/IMUPreintegrator.h" +#include + namespace ORB_SLAM2 { @@ -47,13 +49,13 @@ class KeyFrame public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, std::vector 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 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 mvIMUData; + IMUData::vector_t mvIMUData; IMUPreintegrator mIMUPreInt; diff --git a/include/System.h b/include/System.h index efefd98..8a8d876 100644 --- a/include/System.h +++ b/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 ×tamp); - cv::Mat TrackMonoVI(const cv::Mat &im, const std::vector &vimu, const double ×tamp); + cv::Mat TrackMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp); // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); diff --git a/include/Tracking.h b/include/Tracking.h index a82f107..7fd511f 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -75,11 +75,11 @@ class Tracking bool TrackLocalMapWithIMU(bool bMapUpdated=false); ConfigParam* mpParams; - cv::Mat GrabImageMonoVI(const cv::Mat &im, const std::vector &vimu, const double ×tamp); + cv::Mat GrabImageMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp); // IMU Data since last KF. Append when new data is provided // Should be cleared in 1. initialization beginning, 2. new keyframe created. - std::vector mvIMUSinceLastKF; - IMUPreintegrator GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector& vIMUSInceLastKF); + IMUData::vector_t mvIMUSinceLastKF; + IMUPreintegrator GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const IMUData::vector_t& vIMUSInceLastKF); IMUPreintegrator GetIMUPreIntSinceLastFrame(Frame* pCurF, Frame* pLastF); diff --git a/src/Frame.cc b/src/Frame.cc index 15e8ffa..a2d013f 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -43,7 +43,7 @@ void Frame::ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator // Reset pre-integrator first IMUPreInt.reset(); - const std::vector& 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 &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(NULL)), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) diff --git a/src/IMU/imudata.h b/src/IMU/imudata.h index 2aba5ab..4fb3107 100644 --- a/src/IMU/imudata.h +++ b/src/IMU/imudata.h @@ -2,6 +2,8 @@ #define IMUDATA_H #include +#include +#include namespace ORB_SLAM2 { @@ -10,8 +12,10 @@ using namespace Eigen; class IMUData { + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef std::vector > vector_t; // covariance of measurement static Matrix3d _gyrMeasCov; diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index c56a9b5..0db00ec 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -83,7 +83,7 @@ void KeyFrame::SetNextKeyFrame(KeyFrame* pKF) mpNextKeyFrame = pKF; } -std::vector KeyFrame::GetVectorIMUData(void) +IMUData::vector_t KeyFrame::GetVectorIMUData(void) { unique_lock lock(mMutexIMUData); return mvIMUData; @@ -91,7 +91,7 @@ std::vector KeyFrame::GetVectorIMUData(void) void KeyFrame::AppendIMUDataToFront(KeyFrame* pPrevKF) { - std::vector vimunew = pPrevKF->GetVectorIMUData(); + IMUData::vector_t vimunew = pPrevKF->GetVectorIMUData(); { unique_lock 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 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), diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 895c576..2218551 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -26,6 +26,8 @@ #include #include "Converter.h" +#include + namespace ORB_SLAM2 { using namespace std; @@ -42,7 +44,7 @@ class KeyFrameInit KeyFrameInit* mpPrevKeyFrame; cv::Mat Twc; IMUPreintegrator mIMUPreInt; - std::vector mvIMUData; + IMUData::vector_t mvIMUData; Vector3d bg; diff --git a/src/System.cc b/src/System.cc index 5347fd4..f4bf6dd 100644 --- a/src/System.cc +++ b/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 &vimu, const double ×tamp) +cv::Mat System::TrackMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp) { if(mSensor!=MONOCULAR) { diff --git a/src/Tracking.cc b/src/Tracking.cc index daa028e..343d223 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -400,7 +400,7 @@ bool Tracking::TrackWithIMU(bool bMapUpdated) } -IMUPreintegrator Tracking::GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector& 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 &vimu, const double ×tamp) +cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp) { 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 vimu1,vimu2; + IMUData::vector_t vimu1,vimu2; for(size_t i=0; i Date: Tue, 25 Jul 2017 12:14:46 +0200 Subject: [PATCH 3/3] IMUPreintegrator - Solve Eigen unaligned arrays when using STL Containers --- include/Optimizer.h | 2 +- src/IMU/IMUPreintegrator.h | 2 ++ src/LocalMapping.cc | 2 +- src/Optimizer.cc | 2 +- src/Tracking.cc | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/Optimizer.h b/include/Optimizer.h index 19ce321..cc62e8f 100644 --- a/include/Optimizer.h +++ b/include/Optimizer.h @@ -54,7 +54,7 @@ class Optimizer Vector3d static OptimizeInitialGyroBias(const std::list &lLocalKeyFrames); Vector3d static OptimizeInitialGyroBias(const std::vector &vLocalKeyFrames); Vector3d static OptimizeInitialGyroBias(const std::vector > &vFrames); - Vector3d static OptimizeInitialGyroBias(const vector& vTwc, const vector& vImuPreInt); + Vector3d static OptimizeInitialGyroBias(const vector& vTwc, const IMUPreintegrator::vector_t& vImuPreInt); void static LocalBundleAdjustment(KeyFrame *pKF, const std::list &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, LocalMapping* pLM=NULL); diff --git a/src/IMU/IMUPreintegrator.h b/src/IMU/IMUPreintegrator.h index 9058de5..c1a14c4 100644 --- a/src/IMU/IMUPreintegrator.h +++ b/src/IMU/IMUPreintegrator.h @@ -2,6 +2,7 @@ #define TVISLAM_IMUPREINTEGRATOR_H #include +#include #include "IMU/imudata.h" #include "so3.h" @@ -18,6 +19,7 @@ class IMUPreintegrator { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef std::vector > vector_t; IMUPreintegrator(); IMUPreintegrator(const IMUPreintegrator& pre); diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 2218551..2f00802 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -246,7 +246,7 @@ bool LocalMapping::TryInitVIO(void) int N = vScaleGravityKF.size(); KeyFrame* pNewestKF = vScaleGravityKF[N-1]; vector vTwc; - vector vIMUPreInt; + IMUPreintegrator::vector_t vIMUPreInt; // Store initialization-required KeyFrame data vector vKFInit; diff --git a/src/Optimizer.cc b/src/Optimizer.cc index d7203df..b4c3046 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -2918,7 +2918,7 @@ Vector3d Optimizer::OptimizeInitialGyroBias(const std::vector &vpKFs return vBgEst->estimate(); } -Vector3d Optimizer::OptimizeInitialGyroBias(const vector& vTwc, const vector& vImuPreInt) +Vector3d Optimizer::OptimizeInitialGyroBias(const vector& vTwc, const IMUPreintegrator::vector_t& vImuPreInt) { int N = vTwc.size(); if(vTwc.size()!=vImuPreInt.size()) cerr<<"vTwc.size()!=vImuPreInt.size()"< v19IMUPreint; + IMUPreintegrator::vector_t v19IMUPreint; v19IMUPreint.reserve(20-1); for(size_t i=0; i