use Eigen::aligned_allocator in std containers

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<Frame*>, std::equal_to<Frame*>,
+		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<Frame*>, std::equal_to<Frame*>,
+		Eigen::aligned_allocator< std::pair<const Frame*,Sim3> > > trackingFailed;
 
 
 	// flag set when depth is updated.

lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h

@@ -139,26 +139,27 @@ 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<Frame*> > 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<Frame> > idToKeyFrame;
+	std::unordered_map< int, std::shared_ptr<Frame>, std::hash<int>, std::equal_to<int>,
+	Eigen::aligned_allocator< std::pair<const int, std::shared_ptr<Frame> > > > idToKeyFrame;
 
 
 	// contains ALL edges, as soon as they are created
 	boost::shared_mutex edgesListsMutex;
-	std::vector< KFConstraintStruct* > edgesAll;
+	std::vector< KFConstraintStruct*, Eigen::aligned_allocator<KFConstraintStruct*> > edgesAll;
 
 
 
 	// contains ALL frame poses, chronologically, as soon as they are tracked.
 	// 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<FramePoseStruct* > allFramePoses;
+	std::vector<FramePoseStruct*, Eigen::aligned_allocator<FramePoseStruct*> > 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<Frame*> > newKeyframesBuffer;
+	std::vector< KFConstraintStruct*, Eigen::aligned_allocator<FramePoseStruct*> > newEdgeBuffer;
 
 
 	int nextEdgeId;

lsd_slam_core/src/GlobalMapping/TrackableKeyFrameSearch.cpp

@@ -53,7 +53,7 @@ TrackableKeyFrameSearch::~TrackableKeyFrameSearch()
 
 
 
-std::vector<TrackableKFStruct> TrackableKeyFrameSearch::findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales)
+std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > 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<TrackableKFStruct> TrackableKeyFrameSearch::findEuclideanOverlapFram
 	Eigen::Vector3d pos = frame->getScaledCamToWorld().translation();
 	Eigen::Vector3d viewingDir = frame->getScaledCamToWorld().rotationMatrix().rightCols<1>();
 
-	std::vector<TrackableKFStruct> potentialReferenceFrames;
+	std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > potentialReferenceFrames;
 
 	float distFacReciprocal = 1;
 	if(checkBothScales)
@@ -102,7 +102,8 @@ std::vector<TrackableKFStruct> TrackableKeyFrameSearch::findEuclideanOverlapFram
 
 Frame* TrackableKeyFrameSearch::findRePositionCandidate(Frame* frame, float maxScore)
 {
-	std::vector<TrackableKFStruct> potentialReferenceFrames = findEuclideanOverlapFrames(frame, maxScore / (KFDistWeight*KFDistWeight), 0.75);
+    std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > 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<Frame*> TrackableKeyFrameSearch::findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP, bool closenessTH)
+std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>, Eigen::aligned_allocator< Frame* > > TrackableKeyFrameSearch::findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP, bool closenessTH)
 {
-	std::unordered_set<Frame*> results;
+	std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>, Eigen::aligned_allocator< Frame* > > results;
 
 	// Add all candidates that are similar in an euclidean sense.
-	std::vector<TrackableKFStruct> potentialReferenceFrames = findEuclideanOverlapFrames(keyframe, closenessTH * 15 / (KFDistWeight*KFDistWeight), 1.0 - 0.25 * closenessTH, true);
+    std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > potentialReferenceFrames =
+            findEuclideanOverlapFrames(keyframe, closenessTH * 15 / (KFDistWeight*KFDistWeight), 1.0 - 0.25 * closenessTH, true);
 	for(unsigned int i=0;i<potentialReferenceFrames.size();i++)
 		results.insert(potentialReferenceFrames[i].ref);
 

lsd_slam_core/src/GlobalMapping/TrackableKeyFrameSearch.h

@@ -22,6 +22,7 @@
 #include <vector>
 #include <unordered_map>
 #include <unordered_set>
+#include <Eigen/StdVector>
 #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<Frame*> findCandidates(Frame* keyframe, Frame* &fabMapResult_out, bool includeFABMAP=true, bool closenessTH=1.0);
+	std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>, 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<TrackableKFStruct> findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales = false);
+	std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales = false);
 
 #ifdef HAVE_FABMAP
 	std::unordered_map<int, Frame*> fabmapIDToKeyframe;

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<KFConstraintStruct*> constraints;
+	std::vector<KFConstraintStruct*, Eigen::aligned_allocator<KFConstraintStruct*> > constraints;
 	Frame* fabMapResult = 0;
-	std::unordered_set<Frame*> candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH);
-	std::map< Frame*, Sim3 > candidateToFrame_initialEstimateMap;
+	std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>,
+		Eigen::aligned_allocator< Frame* > > candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH);
+	std::map< Frame*, Sim3, std::less<Frame*>, Eigen::aligned_allocator<std::pair<Frame*, Sim3> > > 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<Frame*> closeCandidates;
-	std::vector<Frame*> farCandidates;
+	std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>,
+		Eigen::aligned_allocator< Frame* > > closeCandidates;
+	std::vector<Frame*, Eigen::aligned_allocator<Frame*> > farCandidates;
 	Frame* parent = newKeyFrame->hasTrackingParent() ? newKeyFrame->getTrackingParent() : 0;
 
 	int closeFailed = 0;
@@ -1686,7 +1688,7 @@ SE3 SlamSystem::getCurrentPoseEstimate()
 	return camToWorld;
 }
 
-std::vector<FramePoseStruct*> SlamSystem::getAllPoses()
+std::vector<FramePoseStruct*, Eigen::aligned_allocator<FramePoseStruct*> > SlamSystem::getAllPoses()
 {
 	return keyFrameGraph->allFramePoses;
 }

lsd_slam_core/src/SlamSystem.h

@@ -108,7 +108,7 @@ friend class IntegrationTest;
 
 	void publishKeyframeGraph();
 
-	std::vector<FramePoseStruct*> getAllPoses();
+	std::vector<FramePoseStruct*, Eigen::aligned_allocator<lsd_slam::FramePoseStruct*> > getAllPoses();
 
 
 

lsd_slam_core/src/Tracking/Relocalizer.cpp

@@ -91,7 +91,7 @@ void Relocalizer::updateCurrentFrame(std::shared_ptr<Frame> currentFrame)
 	int pressedKey = Util::waitKey(1);
 	handleKey(pressedKey);
 }
-void Relocalizer::start(std::vector<Frame*> &allKeyframesList)
+void Relocalizer::start(std::vector<Frame*, Eigen::aligned_allocator<lsd_slam::Frame*> > &allKeyframesList)
 {
 	// make KFForReloc List
 	KFForReloc.clear();

lsd_slam_core/src/Tracking/Relocalizer.h

@@ -39,7 +39,7 @@ class Relocalizer
 	~Relocalizer();
 
 	void updateCurrentFrame(std::shared_ptr<Frame> currentFrame);
-	void start(std::vector<Frame*> &allKeyframesList);
+    void start(std::vector<Frame*, Eigen::aligned_allocator<lsd_slam::Frame*> > &allKeyframesList);
 	void stop();
 
 	bool waitResult(int milliseconds);