externalised cv imageDisplay to be only called from one thread

LinuxDoom · Sep 29, 2014 · ffd766e · ffd766e
1 parent bfe5fa6
commit ffd766e
Show file tree

Hide file tree

Showing 5 changed files with 101 additions and 11 deletions.
diff --git a/lsd_slam_core/CMakeLists.txt b/lsd_slam_core/CMakeLists.txt
@@ -38,7 +38,7 @@ add_definitions("-DENABLE_SSE")
 
 # Also add some useful compiler flag
 set(CMAKE_CXX_FLAGS
-   "${CMAKE_CXX_FLAGS} ${SSE_FLAGS} -march=native  -std=c++0x"
+   "${CMAKE_CXX_FLAGS} ${SSE_FLAGS}  -std=c++0x -g"
 ) 
 
 # Set source files

diff --git a/lsd_slam_core/src/IOWrapper/ImageDisplay.h b/lsd_slam_core/src/IOWrapper/ImageDisplay.h
@@ -30,6 +30,14 @@ namespace lsd_slam
 namespace Util
 {
 
+	struct DisplayImageObect
+	{
+		cv::Mat img;
+		std::string name;
+		bool autoSize;
+	};
+
+
 /// Image display function working on different platforms.
 /// On Android, the window name is ignored as all images are output fullscreen.
 void displayImage(const char* windowName, const cv::Mat& image, bool autoSize = true);

diff --git a/lsd_slam_core/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp b/lsd_slam_core/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp
@@ -34,22 +34,83 @@ namespace lsd_slam
 namespace Util
 {
 
+	const bool useImageDisplayThread = true;
+
+
 	std::unordered_set<std::string> openWindows;
 	boost::mutex openCVdisplayMutex;
+	boost::condition_variable  openCVdisplaySignal;
 
-void displayImage(const char* windowName, const cv::Mat& image, bool autoSize)
+
+	boost::thread* imageDisplayThread = 0;
+	std::vector<DisplayImageObect> displayQueue;
+	bool imageThreadKeepRunning = true;
+
+
+void displayThreadLoop()
 {
+	printf("started image display thread!\n");
 	boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
-	if(!autoSize)
+	while(imageThreadKeepRunning)
+	{
+		openCVdisplaySignal.wait(lock);
+
+		if(!imageThreadKeepRunning)
+			break;
+
+		while(displayQueue.size() > 0)
+		{
+			if(!displayQueue.back().autoSize)
+			{
+				if(openWindows.find(displayQueue.back().name) == openWindows.end())
+				{
+					cv::namedWindow(displayQueue.back().name, cv::WINDOW_NORMAL);
+					cv::resizeWindow(displayQueue.back().name, displayQueue.back().img.cols, displayQueue.back().img.rows);
+					openWindows.insert(displayQueue.back().name);
+				}
+			}
+			cv::imshow(displayQueue.back().name, displayQueue.back().img);
+			displayQueue.pop_back();
+		}
+	}
+	cv::destroyAllWindows();
+	openWindows.clear();
+
+	printf("ended image display thread!\n");
+}
+void makeDisplayThread()
+{
+	imageThreadKeepRunning = true;
+	imageDisplayThread = new boost::thread(&displayThreadLoop);
+}
+void displayImage(const char* windowName, const cv::Mat& image, bool autoSize)
+{
+	if(useImageDisplayThread)
 	{
-		if(openWindows.find(windowName) == openWindows.end())
+		if(imageDisplayThread == 0)
+			makeDisplayThread();
+
+		boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
+		displayQueue.push_back(DisplayImageObect());
+		displayQueue.back().autoSize = autoSize;
+		displayQueue.back().img = image.clone();
+		displayQueue.back().name = windowName;
+
+		openCVdisplaySignal.notify_one();
+	}
+	else
+	{
+		if(!autoSize)
 		{
-			cv::namedWindow(windowName, cv::WINDOW_NORMAL);
-			cv::resizeWindow(windowName, image.cols, image.rows);
-			openWindows.insert(windowName);
+			if(openWindows.find(windowName) == openWindows.end())
+			{
+				cv::namedWindow(windowName, cv::WINDOW_NORMAL);
+				cv::resizeWindow(windowName, image.cols, image.rows);
+				openWindows.insert(windowName);
+			}
 		}
+		cv::imshow(windowName, image);
 	}
-	cv::imshow(windowName, image);
 	//cv::waitKey(1);
 }
 
@@ -67,8 +128,26 @@ int waitKeyNoConsume(int milliseconds)
 void closeAllWindows()
 {
 	boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
-	cv::destroyAllWindows();
-	openWindows.clear();
+
+	if(useImageDisplayThread)
+	{
+
+		if(imageDisplayThread != 0)
+		{
+			imageThreadKeepRunning = false;
+			openCVdisplaySignal.notify_all();
+			printf("waiting for image display thread to end!\n");
+			lock.unlock();
+			imageDisplayThread->join();
+			printf("done waiting for image display thread to end!\n");
+			imageDisplayThread = 0;
+		}
+	}
+	else
+	{
+		cv::destroyAllWindows();
+		openWindows.clear();
+	}
 }
 }
 

diff --git a/lsd_slam_core/src/SlamSystem.cpp b/lsd_slam_core/src/SlamSystem.cpp
@@ -163,6 +163,9 @@ SlamSystem::~SlamSystem()
 	delete keyFrameGraph;
 
 	FrameMemory::getInstance().releaseBuffes();
+
+
+	Util::closeAllWindows();
 }
 
 void SlamSystem::setVisualization(Output3DWrapper* outputWrapper)

diff --git a/lsd_slam_core/src/Tracking/SE3Tracker.cpp b/lsd_slam_core/src/Tracking/SE3Tracker.cpp
@@ -449,7 +449,7 @@ SE3 SE3Tracker::trackFrame(
 
 
 	if(plotTracking)
-		cv::imshow("TrackingResidual", debugImageResiduals);
+		Util::displayImage("TrackingResidual", debugImageResiduals, false);
 
 
 	if(enablePrintDebugInfo && printTrackingIterationInfo)