Merge branch 'master' of https://github.com/siims/lsd_slam

README.md

@@ -49,7 +49,7 @@ We tested LSD-SLAM on two different system configurations, using Ubuntu 12.04 (P
 ## 2.1 ROS fuerte + Ubuntu 12.04
 Install system dependencies:
 
-    sudo apt-get install ros-fuerte-libg2o liblapack-dev libblas-dev freeglut3-dev libqglviewer-qt4-dev
+    sudo apt-get install ros-fuerte-libg2o liblapack-dev libblas-dev freeglut3-dev libqglviewer-qt4-dev libsuitesparse-dev libx11-dev
 
 In your ROS package path, clone the repository:
 
@@ -78,7 +78,7 @@ For this you need to create a rosbuild workspace (if you don't have one yet), us
 
 Install system dependencies:
 
-    sudo apt-get install ros-indigo-libg2o ros-indigo-cv-bridge liblapack-dev libblas-dev freeglut3-dev libqglviewer-dev
+    sudo apt-get install ros-indigo-libg2o ros-indigo-cv-bridge liblapack-dev libblas-dev freeglut3-dev libqglviewer-dev libsuitesparse-dev libx11-dev
 
 In your ROS package path, clone the repository:
 
@@ -144,17 +144,17 @@ LSD-SLAM operates on a pinhole camera model, however we give the option to undis
 
 #### Calibration File for FOV camera model:
 
-    d1 d2 d3 d4 d5
+    fx/width fy/height cx/width cy/height d
     in_width in_height
     "crop" / "full" / "none" / "e1 e2 e3 e4 0"
-    out\_width out_height
+    out_width out_height
 
 
-Here, d1 to d5 are fx/width fy/height cx/width cy/height d, as given by the PTAM cameracalibrator, in\_width and in\_height is the input image size, and out\_width out\_height is the desired undistorted image size. The third line specifies how the image is distorted, either by specifying a desired camera matrix in the same format as d1-d4, or by specifying "crop", which crops the image to maximal size.
+Here, the values in the first line are the camera intrinsics and radial distortion parameter as given by the PTAM cameracalibrator, in\_width and in\_height is the input image size, and out\_width out\_height is the desired undistorted image size. The latter can be chosen freely, however 640x480 is recommended as explained in section 3.1.6. The third line specifies how the image is distorted, either by specifying a desired camera matrix in the same format as the first four intrinsic parameters, or by specifying "crop", which crops the image to maximal size while including only valid image pixels.
 
 
 #### Calibration File for Pre-Rectified Images
-This one is with no radial distortion, as a special case of ATAN camera model but without the computational cost:
+This one is without radial distortion correction, as a special case of ATAN camera model but without the computational cost:
 
     fx/width fy/height cx/width cy/height 0
     width height
@@ -164,7 +164,7 @@ This one is with no radial distortion, as a special case of ATAN camera model bu
 
 #### Calibration File for OpenCV camera model [untested!]:
 
-    fx fy cx cy d1 d2 d3 d4
+    fx fy cx cy k1 k2 p1 p2
     inputWidth inputHeight
     "crop" / "full" / "none" / "e1 e2 e3 e4 0"
     outputWidth outputHeight
@@ -196,7 +196,7 @@ or (for ROS indigo)
     rosrun rqt_reconfigure rqt_reconfigure
 
 Parameters are split into two parts, ones that enable / disable various sorts of debug output in `/LSD_SLAM/Debug`, and ones that affect the actual algorithm, in `/LSD_SLAM`.
-
+Note that debug output options from `/LSD_SLAM/Debug` only work if lsd\_slam\_core is built with debug info, e.g. with `set(ROS_BUILD_TYPE RelWithDebInfo)`. 
 
 * `minUseGrad`: [double] Minimal absolute image gradient for a pixel to be used at all. Increase if your camera has large image noise, decrease if you have low image-noise and want to also exploit small gradients.
 * `cameraPixelNoise`: [double] Image intensity noise used for e.g. tracking weight calculation. Should be set larger than the actual sensor-noise, to also account for noise originating from discretization / linear interpolation.
@@ -243,17 +243,23 @@ Useful for debug output are:
 
 ## 3.2 LSD-SLAM Viewer
 The viewer is only for visualization. It can also be used to output a generated point cloud as .ply.
-Start it using
+For live operation, start it using
 
     rosrun lsd_slam_viewer viewer
 
 You can use rosbag to record and re-play the output generated by certain trajectories. Record & playback using
 
-    rosbag record /lsd_slam/graph /lsd_slam/keyframes /lsd_slam/liveframes -o file.bag
-    rosbag play file.bag
+    rosbag record /lsd_slam/graph /lsd_slam/keyframes /lsd_slam/liveframes -o file_pc.bag
+    rosbag play file_pc.bag
 
 You should never have to restart the viewer node, it resets the graph automatically.
 
+If you just want to lead a certain pointcloud from a .bag file into the viewer, you 
+can directly do that using 
+
+    rosrun lsd_slam_viewer viewer file_pc.bag
+
+
 
 ### 3.2.1 Useful Hotkeys
 

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
@@ -83,7 +84,7 @@ include_directories(
 
 # build shared library.
 rosbuild_add_library(lsdslam SHARED ${SOURCE_FILES})
-target_link_libraries(lsdslam ${FABMAP_LIB} g2o_core g2o_stuff csparse cxsparse g2o_solver_csparse g2o_csparse_extension g2o_types_sim3 g2o_types_sba)
+target_link_libraries(lsdslam ${FABMAP_LIB} g2o_core g2o_stuff csparse cxsparse g2o_solver_csparse g2o_csparse_extension g2o_types_sim3 g2o_types_sba X11)
 rosbuild_link_boost(lsdslam thread)
 
 

lsd_slam_core/calib/OpenCV_example_calib.cfg

@@ -1,4 +1,4 @@
-500 500 350 240 0 0 0 0 0
+500 500 350 240 0 0 0 0
 752 480
 crop
 640 480

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