add image write

lsd_slam_core/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp

@@ -8,10 +8,7 @@
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
-*
-* LSD-SLAM is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License

lsd_slam_core/src/SlamSystem.cpp

@@ -141,8 +141,8 @@ SlamSystem::~SlamSystem()
 	newConstraintCreatedSignal.notify_all();
 
 	thread_mapping.join();
-	thread_constraint_search.join();
-	thread_optimization.join();
+	//thread_constraint_search.join();
+	//thread_optimization.join();
 	printf("DONE waiting for SlamSystem's threads to exit\n");
 
 	if(trackableKeyFrameSearch != 0) delete trackableKeyFrameSearch;
@@ -690,6 +690,7 @@ void SlamSystem::debugDisplayDepthMap()
 
 	if(onSceenInfoDisplay)
 		printMessageOnCVImage(map->debugImageDepth, buf1, buf2);
+	displayMatQueue.push(map->debugImageDepth);
 	if (displayDepthMap)
 		Util::displayImage( "DebugWindow DEPTH", map->debugImageDepth, false );
 

lsd_slam_core/src/SlamSystem.h

@@ -20,6 +20,8 @@
 
 #pragma once
 #include <vector>
+#include <queue>
+
 #include <boost/thread.hpp>
 #include <boost/thread/shared_mutex.hpp>
 #include <boost/thread/condition_variable.hpp>
@@ -110,7 +112,7 @@ friend class IntegrationTest;
 
 	std::vector<FramePoseStruct*, Eigen::aligned_allocator<lsd_slam::FramePoseStruct*> > getAllPoses();
 
-
+	std::queue<cv::Mat> displayMatQueue;
 
 	float msTrackFrame, msOptimizationIteration, msFindConstraintsItaration, msFindReferences;
 	int nTrackFrame, nOptimizationIteration, nFindConstraintsItaration, nFindReferences;
@@ -202,8 +204,8 @@ friend class IntegrationTest;
 
 	// threads
 	boost::thread thread_mapping;
-	boost::thread thread_constraint_search;
-	boost::thread thread_optimization;
+	//boost::thread thread_constraint_search;
+	//boost::thread thread_optimization;
 	bool keepRunning; // used only on destruction to signal threads to finish.
 
 

lsd_slam_core/src/main_on_images.cpp

@@ -255,8 +255,12 @@ int main( int argc, char** argv )
 			system->trackFrame(image.data, runningIDX ,hz == 0,fakeTimeStamp);
 			std::cout<<"index is "<<runningIDX<<std::endl;	
 		}
-		cv::imshow("DisplayImg", image);
-		cv::waitKey(1);
+		if (!system->displayMatQueue.empty()){
+			std::cout<<"queue size is "<<system->displayMatQueue.size()<<std::endl;
+			std::string image_name = std::to_string(runningIDX) + "_temp.jpg";
+			cv::imwrite(image_name, system->displayMatQueue.front());
+			system->displayMatQueue.pop();
+		}
 		runningIDX++;
 		fakeTimeStamp+=0.03;
 
@@ -282,8 +286,15 @@ int main( int argc, char** argv )
 		//	break;
 	}
 
+	while(!system->displayMatQueue.empty()){
+		std::cout<<"queue size is "<<system->displayMatQueue.size()<<std::endl;
+		std::string image_name = std::to_string(runningIDX) + "_temp.jpg";
+		cv::imwrite(image_name, system->displayMatQueue.front());
+		system->displayMatQueue.pop();
+		runningIDX++;
+	}
 
-	system->finalize();
+	//system->finalize();