From a176b9715a56bab2259f78dbff79ca85a2fc537e Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Mon, 22 Sep 2014 12:50:33 +0200 Subject: [PATCH] use Eigen::aligned_allocator in std containers --- lsd_slam_core/src/DataStructures/Frame.h | 6 ++++-- lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h | 13 +++++++------ .../src/GlobalMapping/TrackableKeyFrameSearch.cpp | 14 ++++++++------ .../src/GlobalMapping/TrackableKeyFrameSearch.h | 5 +++-- lsd_slam_core/src/SlamSystem.cpp | 14 ++++++++------ lsd_slam_core/src/SlamSystem.h | 2 +- lsd_slam_core/src/Tracking/Relocalizer.cpp | 2 +- lsd_slam_core/src/Tracking/Relocalizer.h | 2 +- 8 files changed, 33 insertions(+), 25 deletions(-) diff --git a/lsd_slam_core/src/DataStructures/Frame.h b/lsd_slam_core/src/DataStructures/Frame.h index 3c9c52c9..1ff2f530 100644 --- a/lsd_slam_core/src/DataStructures/Frame.h +++ b/lsd_slam_core/src/DataStructures/Frame.h @@ -156,10 +156,12 @@ class Frame /** Pointers to all adjacent Frames in graph. empty for non-keyframes.*/ - std::unordered_set< Frame* > neighbors; + std::unordered_set< Frame*, std::hash, std::equal_to, + Eigen::aligned_allocator< Frame* > > neighbors; /** Multi-Map indicating for which other keyframes with which initialization tracking failed.*/ - std::unordered_multimap< Frame*, Sim3 > trackingFailed; + std::unordered_multimap< Frame*, Sim3, std::hash, std::equal_to, + Eigen::aligned_allocator< std::pair > > trackingFailed; // flag set when depth is updated. diff --git a/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h b/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h index b8c20880..8e71f8a9 100644 --- a/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h +++ b/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h @@ -139,18 +139,19 @@ friend class IntegrationTest; // contains ALL keyframes, as soon as they are "finished". // does NOT yet contain the keyframe that is currently being created. boost::shared_mutex keyframesAllMutex; - std::vector< Frame* > keyframesAll; + std::vector< Frame*, Eigen::aligned_allocator > keyframesAll; /** Maps frame ids to keyframes. Contains ALL Keyframes allocated, including the one that currently being created. */ /* this is where the shared pointers of Keyframe Frames are kept, so they are not deleted ever */ boost::shared_mutex idToKeyFrameMutex; - std::unordered_map< int, std::shared_ptr > idToKeyFrame; + std::unordered_map< int, std::shared_ptr, std::hash, std::equal_to, + Eigen::aligned_allocator< std::pair > > > idToKeyFrame; // contains ALL edges, as soon as they are created boost::shared_mutex edgesListsMutex; - std::vector< KFConstraintStruct* > edgesAll; + std::vector< KFConstraintStruct*, Eigen::aligned_allocator > edgesAll; @@ -158,7 +159,7 @@ friend class IntegrationTest; // the corresponding frame may have been removed / deleted in the meantime. // these are the ones that are also referenced by the corresponding Frame / Keyframe object boost::shared_mutex allFramePosesMutex; - std::vector allFramePoses; + std::vector > allFramePoses; // contains all keyframes in graph, in some arbitrary (random) order. if a frame is re-tracked, @@ -174,8 +175,8 @@ friend class IntegrationTest; /** Pose graph representation in g2o */ g2o::SparseOptimizer graph; - std::vector< Frame* > newKeyframesBuffer; - std::vector< KFConstraintStruct* > newEdgeBuffer; + std::vector< Frame*, Eigen::aligned_allocator > newKeyframesBuffer; + std::vector< KFConstraintStruct*, Eigen::aligned_allocator > newEdgeBuffer; int nextEdgeId; diff --git a/lsd_slam_core/src/GlobalMapping/TrackableKeyFrameSearch.cpp b/lsd_slam_core/src/GlobalMapping/TrackableKeyFrameSearch.cpp index f1499f2e..dddc86f2 100644 --- a/lsd_slam_core/src/GlobalMapping/TrackableKeyFrameSearch.cpp +++ b/lsd_slam_core/src/GlobalMapping/TrackableKeyFrameSearch.cpp @@ -53,7 +53,7 @@ TrackableKeyFrameSearch::~TrackableKeyFrameSearch() -std::vector TrackableKeyFrameSearch::findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales) +std::vector > TrackableKeyFrameSearch::findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales) { // basically the maximal angle-difference in viewing direction is angleTH*(average FoV). // e.g. if the FoV is 130°, then it is angleTH*130°. @@ -63,7 +63,7 @@ std::vector TrackableKeyFrameSearch::findEuclideanOverlapFram Eigen::Vector3d pos = frame->getScaledCamToWorld().translation(); Eigen::Vector3d viewingDir = frame->getScaledCamToWorld().rotationMatrix().rightCols<1>(); - std::vector potentialReferenceFrames; + std::vector > potentialReferenceFrames; float distFacReciprocal = 1; if(checkBothScales) @@ -102,7 +102,8 @@ std::vector TrackableKeyFrameSearch::findEuclideanOverlapFram Frame* TrackableKeyFrameSearch::findRePositionCandidate(Frame* frame, float maxScore) { - std::vector potentialReferenceFrames = findEuclideanOverlapFrames(frame, maxScore / (KFDistWeight*KFDistWeight), 0.75); + std::vector > potentialReferenceFrames = + findEuclideanOverlapFrames(frame, maxScore / (KFDistWeight*KFDistWeight), 0.75); float bestScore = maxScore; float bestDist, bestUsage; @@ -170,12 +171,13 @@ Frame* TrackableKeyFrameSearch::findRePositionCandidate(Frame* frame, float maxS } } -std::unordered_set TrackableKeyFrameSearch::findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP, bool closenessTH) +std::unordered_set, std::equal_to, Eigen::aligned_allocator< Frame* > > TrackableKeyFrameSearch::findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP, bool closenessTH) { - std::unordered_set results; + std::unordered_set, std::equal_to, Eigen::aligned_allocator< Frame* > > results; // Add all candidates that are similar in an euclidean sense. - std::vector potentialReferenceFrames = findEuclideanOverlapFrames(keyframe, closenessTH * 15 / (KFDistWeight*KFDistWeight), 1.0 - 0.25 * closenessTH, true); + std::vector > potentialReferenceFrames = + findEuclideanOverlapFrames(keyframe, closenessTH * 15 / (KFDistWeight*KFDistWeight), 1.0 - 0.25 * closenessTH, true); for(unsigned int i=0;i #include #include +#include #include "util/SophusUtil.h" #ifdef HAVE_FABMAP @@ -65,7 +66,7 @@ class TrackableKeyFrameSearch * Finds candidates for trackable frames. * Returns the most likely candidates first. */ - std::unordered_set findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP=true, bool closenessTH=1.0); + std::unordered_set, std::equal_to, Eigen::aligned_allocator< Frame* > > findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP=true, bool closenessTH=1.0); Frame* findRePositionCandidate(Frame* frame, float maxScore=1); @@ -84,7 +85,7 @@ class TrackableKeyFrameSearch * Uses FabMap internally. */ Frame* findAppearanceBasedCandidate(Frame* keyframe); - std::vector findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales = false); + std::vector > findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales = false); #ifdef HAVE_FABMAP std::unordered_map fabmapIDToKeyframe; diff --git a/lsd_slam_core/src/SlamSystem.cpp b/lsd_slam_core/src/SlamSystem.cpp index 7300c476..d1e1dbf4 100644 --- a/lsd_slam_core/src/SlamSystem.cpp +++ b/lsd_slam_core/src/SlamSystem.cpp @@ -1234,10 +1234,11 @@ int SlamSystem::findConstraintsForNewKeyFrames(Frame* newKeyFrame, bool forcePar newKeyFrame->lastConstraintTrackedCamToWorld = newKeyFrame->getScaledCamToWorld(); // =============== get all potential candidates and their initial relative pose. ================= - std::vector constraints; + std::vector > constraints; Frame* fabMapResult = 0; - std::unordered_set candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH); - std::map< Frame*, Sim3 > candidateToFrame_initialEstimateMap; + std::unordered_set, std::equal_to, + Eigen::aligned_allocator< Frame* > > candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH); + std::map< Frame*, Sim3, std::less, Eigen::aligned_allocator > > candidateToFrame_initialEstimateMap; // erase the ones that are already neighbours. @@ -1271,8 +1272,9 @@ int SlamSystem::findConstraintsForNewKeyFrames(Frame* newKeyFrame, bool forcePar // =============== distinguish between close and "far" candidates in Graph ================= // Do a first check on trackability of close candidates. - std::unordered_set closeCandidates; - std::vector farCandidates; + std::unordered_set, std::equal_to, + Eigen::aligned_allocator< Frame* > > closeCandidates; + std::vector > farCandidates; Frame* parent = newKeyFrame->hasTrackingParent() ? newKeyFrame->getTrackingParent() : 0; int closeFailed = 0; @@ -1686,7 +1688,7 @@ SE3 SlamSystem::getCurrentPoseEstimate() return camToWorld; } -std::vector SlamSystem::getAllPoses() +std::vector > SlamSystem::getAllPoses() { return keyFrameGraph->allFramePoses; } diff --git a/lsd_slam_core/src/SlamSystem.h b/lsd_slam_core/src/SlamSystem.h index a65bb66f..68492403 100644 --- a/lsd_slam_core/src/SlamSystem.h +++ b/lsd_slam_core/src/SlamSystem.h @@ -108,7 +108,7 @@ friend class IntegrationTest; void publishKeyframeGraph(); - std::vector getAllPoses(); + std::vector > getAllPoses(); diff --git a/lsd_slam_core/src/Tracking/Relocalizer.cpp b/lsd_slam_core/src/Tracking/Relocalizer.cpp index 938ee6c6..73661e59 100644 --- a/lsd_slam_core/src/Tracking/Relocalizer.cpp +++ b/lsd_slam_core/src/Tracking/Relocalizer.cpp @@ -91,7 +91,7 @@ void Relocalizer::updateCurrentFrame(std::shared_ptr currentFrame) int pressedKey = Util::waitKey(1); handleKey(pressedKey); } -void Relocalizer::start(std::vector &allKeyframesList) +void Relocalizer::start(std::vector > &allKeyframesList) { // make KFForReloc List KFForReloc.clear(); diff --git a/lsd_slam_core/src/Tracking/Relocalizer.h b/lsd_slam_core/src/Tracking/Relocalizer.h index cf8a562e..ab5a5b64 100644 --- a/lsd_slam_core/src/Tracking/Relocalizer.h +++ b/lsd_slam_core/src/Tracking/Relocalizer.h @@ -39,7 +39,7 @@ class Relocalizer ~Relocalizer(); void updateCurrentFrame(std::shared_ptr currentFrame); - void start(std::vector &allKeyframesList); + void start(std::vector > &allKeyframesList); void stop(); bool waitResult(int milliseconds);