Added rtabmap-camera tool. Added Export button in Calibration dialog to

save calibration to yaml file (same format as ROS).

tools/CMakeLists.txt

@@ -2,6 +2,7 @@
 ADD_SUBDIRECTORY( ConsoleApp )
 ADD_SUBDIRECTORY( ImagesJoiner )
 ADD_SUBDIRECTORY( ExtractObject )
+ADD_SUBDIRECTORY( Camera )
 ADD_SUBDIRECTORY( CameraRGBD )
 
 IF(OPENCV_NONFREE_FOUND)

tools/Calibration/main.cpp

@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -35,7 +36,7 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-calibration driver\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+			"  driver       Driver number to use: 0=USB camera, 1=OpenNI-PCL, 2=OpenNI2, 3=Freenect, 4=OpenNI-CV, 5=OpenNI-CV-ASUS\n\n");
 	exit(1);
 }
 
@@ -52,21 +53,26 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 4)
+		if(driver < 0 || driver > 5)
 		{
-			UERROR("driver should be between 0 and 4.");
+			UERROR("driver should be between 0 and 5.");
 			showUsage();
 		}
 	}
 	UINFO("Using driver %d", driver);
 
 	float imageRate = 1.0f;
+	rtabmap::Camera * cameraUsb = 0;
 	rtabmap::CameraRGBD * camera = 0;
 	if(driver == 0)
 	{
-		camera = new rtabmap::CameraOpenni("", imageRate);
+		cameraUsb = new rtabmap::CameraVideo(0, imageRate);
 	}
 	else if(driver == 1)
+	{
+		camera = new rtabmap::CameraOpenni("", imageRate);
+	}
+	else if(driver == 2)
 	{
 		if(!rtabmap::CameraOpenNI2::available())
 		{
@@ -75,7 +81,7 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraOpenNI2(imageRate);
 	}
-	else if(driver == 2)
+	else if(driver == 3)
 	{
 		if(!rtabmap::CameraFreenect::available())
 		{
@@ -84,7 +90,7 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraFreenect(0, imageRate);
 	}
-	else if(driver == 3)
+	else if(driver == 4)
 	{
 		if(!rtabmap::CameraOpenNICV::available())
 		{
@@ -93,7 +99,7 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraOpenNICV(false, imageRate);
 	}
-	else if(driver == 4)
+	else if(driver == 5)
 	{
 		if(!rtabmap::CameraOpenNICV::available())
 		{
@@ -107,21 +113,36 @@ int main(int argc, char * argv[])
 		UFATAL("");
 	}
 
-	if(!camera->init())
-	{
-		printf("Camera init failed!\n");
-		delete camera;
-		exit(1);
-	}
+	rtabmap::CameraThread * cameraThread = 0;
 
-	rtabmap::CameraThread cameraThread(camera);
+	if(cameraUsb)
+	{
+		if(!cameraUsb->init())
+		{
+			printf("Camera init failed!\n");
+			delete cameraUsb;
+			exit(1);
+		}
+		cameraThread = new rtabmap::CameraThread(cameraUsb);
+	}
+	else if(camera)
+	{
+		if(!camera->init())
+		{
+			printf("Camera init failed!\n");
+			delete camera;
+			exit(1);
+		}
+		cameraThread = new rtabmap::CameraThread(camera);
+	}
 
 	QApplication app(argc, argv);
 	rtabmap::CalibrationDialog dialog;
 	dialog.registerToEventsManager();
 
 	dialog.show();
-	cameraThread.start();
+	cameraThread->start();
 	app.exec();
-	cameraThread.join(true);
+	cameraThread->join(true);
+	delete cameraThread;
 }

tools/Camera/CMakeLists.txt

@@ -0,0 +1,25 @@
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(camera main.cpp)
+TARGET_LINK_LIBRARIES(camera rtabmap_core rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( camera 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-camera)
+  
+INSTALL(TARGETS camera
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/Camera/main.cpp

@@ -0,0 +1,213 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/DBReader.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <stdio.h>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-camera [option] \n"
+			" Options:\n"
+			"    --device #            USB camera device id (default 0).\n"
+			"    --rate #              Frame rate (default 30 Hz). 0 means as fast as possible.\n"
+			"    --path ""             Path to a directory of images or a video file.\n"
+			"    --calibration ""      Calibration file (*.yaml).\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	int device = 0;
+	std::string path;
+	float rate = 30.0f;
+	std::string calibrationFile;
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "--rate") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = std::atof(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--device") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				device = std::atoi(argv[i]);
+				if(device < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--path") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				path = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--calibration") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				calibrationFile = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	if(path.empty())
+	{
+		UINFO("Using device %d", device);
+	}
+	else
+	{
+		UINFO("Using path %s", path.c_str());
+	}
+
+	rtabmap::Camera * camera = 0;
+	rtabmap::DBReader * dbReader = 0;
+
+	if(!path.empty())
+	{
+		if(UFile::exists(path))
+		{
+			if(UFile::getExtension(path).compare("db") == 0)
+			{
+				dbReader = new rtabmap::DBReader(path, rate);
+			}
+			else
+			{
+				camera = new rtabmap::CameraVideo(path, rate);
+			}
+		}
+		else if(UDirectory::exists(path))
+		{
+			camera = new rtabmap::CameraImages(path, rate);
+		}
+		else
+		{
+			UERROR("Path not valid! \"%s\"", path.c_str());
+			return -1;
+		}
+	}
+	else
+	{
+		camera = new rtabmap::CameraVideo(device, rate);
+	}
+
+	if(camera)
+	{
+		if(!camera->init())
+		{
+			delete camera;
+			UERROR("Cannot initialize the camera.");
+			return -1;
+		}
+
+		if(!calibrationFile.empty())
+		{
+			UINFO("Set calibration: %s", calibrationFile.c_str());
+			camera->setCalibration(calibrationFile);
+		}
+	}
+
+	if(dbReader)
+	{
+		if(!dbReader->init())
+		{
+			delete dbReader;
+			UERROR("Cannot initialize the camera.");
+			return -1;
+		}
+	}
+
+	cv::Mat rgb;
+	rgb = camera?camera->takeImage():dbReader->getNextData().image();
+	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
+	while(!rgb.empty())
+	{
+		cv::imshow("Video", rgb); // show frame
+
+		int c = cv::waitKey(10); // wait 10 ms or for key stroke
+		if(c == 27)
+			break; // if ESC, break and quit
+
+		rgb = camera?camera->takeImage():dbReader->getNextData().image();
+	}
+	cv::destroyWindow("Video");
+	if(camera)
+	{
+		delete camera;
+	}
+	if(dbReader)
+	{
+		delete dbReader;
+	}
+	return 0;
+}