Merge branch '32bit'

lsd_slam_core/CMakeLists.txt

@@ -18,6 +18,7 @@ set(CMAKE_MODULE_PATH   ${PROJECT_SOURCE_DIR}/cmake ${CMAKE_MODULE_PATH})
 
 find_package(Eigen3 REQUIRED)
 find_package(SuiteParse REQUIRED) # Apparently needed by g2o
+find_package(X11 REQUIRED)
 
 # FabMap
 # uncomment this part to enable fabmap

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/DataStructures/FrameMemory.cpp

@@ -51,7 +51,7 @@ void FrameMemory::releaseBuffes()
 
 		for(unsigned int i=0;i<p.second.size();i++)
 		{
-			delete (char*)p.second[i];
+			Eigen::internal::aligned_free(p.second[i]);
 			bufferSizes.erase(p.second[i]);
 		}
 
@@ -121,7 +121,7 @@ void* FrameMemory::allocateBuffer(unsigned int size)
 {
 	//printf("allocateFloatBuffer(%d)\n", size);
 
-	void* buffer = (void*)(new char[size]);
+	void* buffer = Eigen::internal::aligned_malloc(size);
 	bufferSizes.insert(std::make_pair(buffer, size));
 	return buffer;
 }

lsd_slam_core/src/DataStructures/FrameMemory.h

@@ -25,7 +25,7 @@
 #include <deque>
 #include <list>
 #include <boost/thread/shared_mutex.hpp>
-
+#include <Eigen/Core> //For EIGEN MACRO
 
 namespace lsd_slam
 {
@@ -35,6 +35,8 @@ class Frame;
 class FrameMemory
 {
 public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 	/** Returns the global instance. Creates it when the method is first called. */
 	static FrameMemory& getInstance();
 

lsd_slam_core/src/DepthEstimation/DepthMap.cpp

@@ -48,7 +48,8 @@ DepthMap::DepthMap(int w, int h, const Eigen::Matrix3f& K)
 	otherDepthMap = new DepthMapPixelHypothesis[width*height];
 	currentDepthMap = new DepthMapPixelHypothesis[width*height];
 
-	validityIntegralBuffer =new int[width*height];
+	validityIntegralBuffer = (int*)Eigen::internal::aligned_malloc(width*height*sizeof(int));
+
 
 
 
@@ -94,9 +95,7 @@ DepthMap::~DepthMap()
 	delete[] otherDepthMap;
 	delete[] currentDepthMap;
 
-	delete[] validityIntegralBuffer;
-
-
+	Eigen::internal::aligned_free((void*)validityIntegralBuffer);
 }
 
 

lsd_slam_core/src/DepthEstimation/DepthMap.h

@@ -42,6 +42,8 @@ class KeyFrameGraph;
 class DepthMap
 {
 public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 	DepthMap(int w, int h, const Eigen::Matrix3f& K);
 	DepthMap(const DepthMap&) = delete;
 	DepthMap& operator=(const DepthMap&) = delete;

lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h

@@ -41,6 +41,8 @@ class FramePoseStruct;
 
 struct KFConstraintStruct
 {
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 	inline KFConstraintStruct()
 	{
 		firstFrame = secondFrame = 0;
@@ -87,6 +89,8 @@ class KeyFrameGraph
 {
 friend class IntegrationTest;
 public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 	/** Constructs an empty pose graph. */
 	KeyFrameGraph();
 
@@ -139,26 +143,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 +179,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
@@ -57,6 +58,8 @@ struct TrackableKFStruct
 class TrackableKeyFrameSearch
 {
 public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 	/** Constructor. */
 	TrackableKeyFrameSearch(KeyFrameGraph* graph, int w, int h, Eigen::Matrix3f K);
 	~TrackableKeyFrameSearch();
@@ -65,7 +68,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 +87,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/IOWrapper/ROS/ROSImageStreamThread.cpp

@@ -53,8 +53,6 @@ ROSImageStreamThread::ROSImageStreamThread()
 	lastSEQ = 0;
 
 	haveCalib = false;
-	undistorter = 0;
-
 }
 
 ROSImageStreamThread::~ROSImageStreamThread()

lsd_slam_core/src/LiveSLAMWrapper.h

@@ -48,6 +48,8 @@ struct LiveSLAMWrapper : public Notifiable
 {
 friend class LiveSLAMWrapperROS;
 public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 	LiveSLAMWrapper(InputImageStream* imageStream, Output3DWrapper* outputWrapper);
 
 	/** Destructor. */

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);

lsd_slam_core/src/Tracking/SE3Tracker.cpp

@@ -26,6 +26,8 @@
 #include "IOWrapper/ImageDisplay.h"
 #include "Tracking/least_squares.h"
 
+#include <Eigen/Core>
+
 namespace lsd_slam
 {
 
@@ -62,16 +64,16 @@ SE3Tracker::SE3Tracker(int w, int h, Eigen::Matrix3f K)
 	cyi = KInv(1,2);
 
 
-	buf_warped_residual = new float[w*h];
-	buf_warped_dx = new float[w*h];
-	buf_warped_dy = new float[w*h];
-	buf_warped_x = new float[w*h];
-	buf_warped_y = new float[w*h];
-	buf_warped_z = new float[w*h];
+	buf_warped_residual = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_dx = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_dy = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_x = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_y = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_z = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
 
-	buf_d = new float[w*h];
-	buf_idepthVar = new float[w*h];
-	buf_weight_p = new float[w*h];
+	buf_d = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_idepthVar = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_weight_p = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
 
 	buf_warped_size = 0;
 
@@ -100,16 +102,16 @@ SE3Tracker::~SE3Tracker()
 	debugImageOldImageWarped.release();
 
 
-	delete[] buf_warped_residual;
-	delete[] buf_warped_dx;
-	delete[] buf_warped_dy;
-	delete[] buf_warped_x;
-	delete[] buf_warped_y;
-	delete[] buf_warped_z;
+	Eigen::internal::aligned_free((void*)buf_warped_residual);
+	Eigen::internal::aligned_free((void*)buf_warped_dx);
+	Eigen::internal::aligned_free((void*)buf_warped_dy);
+	Eigen::internal::aligned_free((void*)buf_warped_x);
+	Eigen::internal::aligned_free((void*)buf_warped_y);
+	Eigen::internal::aligned_free((void*)buf_warped_z);
 
-	delete[] buf_d;
-	delete[] buf_idepthVar;
-	delete[] buf_weight_p;
+	Eigen::internal::aligned_free((void*)buf_d);
+	Eigen::internal::aligned_free((void*)buf_idepthVar);
+	Eigen::internal::aligned_free((void*)buf_weight_p);
 }