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

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

corelib/include/rtabmap/core/Camera.h

@@ -61,6 +61,10 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setImageSize(unsigned int width, unsigned int height);
 
+	void setCalibration(const std::string & fileName);
+	void setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients);
+	void resetCalibration();
+
 protected:
 	/**
 	 * Constructor
@@ -78,6 +82,8 @@ private:
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
 	UTimer * _frameRateTimer;
+	cv::Mat _k; // camera_matrix
+	cv::Mat _d; // distorsion_coefficients
 };
 
 

corelib/src/Camera.cpp

@@ -74,6 +74,79 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
 	height = _imageHeight;
 }
 
+void Camera::setCalibration(const std::string & fileName)
+{
+	if(UFile::getExtension(fileName).compare("yaml") == 0)
+	{
+		cv::FileStorage fs;
+		fs.open(fileName, cv::FileStorage::READ);
+
+		if (!fs.isOpened())
+		{
+			UERROR("Failed to open file \"%s\"", fileName.c_str());
+			return;
+		}
+
+		cv::Mat k,d;
+
+		cv::FileNode n = fs["camera_matrix"];
+		int rows = n["rows"];
+		int cols = n["cols"];
+		std::vector<double> data;
+		n["data"] >> data;
+		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
+		{
+			k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+		}
+
+		cv::FileNode nd = fs["distortion_coefficients"];
+		rows = nd["rows"];
+		cols = nd["cols"];
+		data.clear();
+		nd["data"] >> data;
+		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
+		{
+			d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+		}
+
+		if(k.empty())
+		{
+			UERROR("Failed to load \"camera_matrix\" matrix.");
+		}
+		if(d.empty())
+		{
+			UERROR("Failed to load \"distortion_coefficients\" matrix.");
+		}
+		if(!k.empty() && !d.empty())
+		{
+			this->setCalibration(k, d);
+		}
+	}
+	else
+	{
+		UERROR("Calibration file must be in \"*.yaml\" format");
+	}
+}
+
+void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients)
+{
+	UASSERT(cameraMatrix.type() == CV_64FC1 &&
+			cameraMatrix.rows == 3 &&
+			cameraMatrix.cols == 3);
+	UASSERT(distorsionCoefficients.type() == CV_64FC1 &&
+			distorsionCoefficients.rows ==1 &&
+			(distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8));
+
+	_k = cameraMatrix;
+	_d = distorsionCoefficients;
+}
+
+void Camera::resetCalibration()
+{
+	_k = cv::Mat();
+	_d = cv::Mat();
+}
+
 cv::Mat Camera::takeImage()
 {
 	cv::Mat img;
@@ -99,6 +172,11 @@ cv::Mat Camera::takeImage()
 
 	UTimer timer;
 	img = this->captureImage();
+	if(!img.empty() && !_k.empty() && !_d.empty())
+	{
+		cv::Mat temp = img.clone();
+		cv::undistort(temp, img, _k, _d);
+	}
 	UDEBUG("Time capturing image = %fs", timer.ticks());
 	return img;
 }

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -64,6 +64,7 @@ private slots:
 	void processImage(const cv::Mat & image);
 	void restart();
 	void calibrate();
+	void save();
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);

guilib/src/CalibrationDialog.cpp

@@ -33,6 +33,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/highgui/highgui.hpp>
 
+#include <QtGui/QFileDialog>
+#include <QtGui/QMessageBox>
+
 #include <rtabmap/core/CameraEvent.h>
 #include <rtabmap/gui/UCv2Qt.h>
 
@@ -57,6 +60,7 @@ CalibrationDialog::CalibrationDialog(QWidget * parent) :
 
 	connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
 	connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
+	connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
 
 	connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
 	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
@@ -254,6 +258,7 @@ void CalibrationDialog::restart()
 	ui_->pushButton_calibrate->setEnabled(false);
 	ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
 	ui_->checkBox_rectified->setEnabled(false);
+	//ui_->pushButton_save->setEnabled(false);
 
 	ui_->progressBar_count->reset();
 	ui_->progressBar_count->setMaximum(COUNT_MIN);
@@ -343,6 +348,34 @@ void CalibrationDialog::calibrate()
 		ui_->checkBox_rectified->setChecked(true);
 
 		ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
+		ui_->pushButton_save->setEnabled(true);
+	}
+}
+
+void CalibrationDialog::save()
+{
+	QString fileName = QFileDialog::getSaveFileName(this, tr("Export"), "calibration.yaml", "*.yaml");
+
+	if(!fileName.isEmpty())
+	{
+		cv::FileStorage fs(fileName.toStdString(), cv::FileStorage::WRITE);
+
+		// export in ROS calibration format
+		fs << "camera_matrix" << "{";
+		fs << "rows" << cameraMatrix_.rows;
+		fs << "cols" << cameraMatrix_.cols;
+		fs << "data" << std::vector<double>((double*)cameraMatrix_.data, ((double*)cameraMatrix_.data)+(cameraMatrix_.rows*cameraMatrix_.cols));
+		fs << "}";
+
+		fs << "distortion_coefficients" << "{";
+		fs << "rows" << distCoeffs_.rows;
+		fs << "cols" << distCoeffs_.cols;
+		fs << "data" << std::vector<double>((double*)distCoeffs_.data, ((double*)distCoeffs_.data)+(distCoeffs_.rows*distCoeffs_.cols));
+		fs << "}";
+
+		fs.release();
+		QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(fileName));
+		UINFO("Saved \"%s\"!", fileName.toStdString().c_str());
 	}
 }
 

guilib/src/ui/calibrationDialog.ui

@@ -199,6 +199,9 @@
           <string>Camera intrinsic parameters</string>
          </property>
          <layout class="QFormLayout" name="formLayout_2">
+          <property name="fieldGrowthPolicy">
+           <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+          </property>
           <item row="0" column="0">
            <widget class="QLabel" name="label_8">
             <property name="text">
@@ -328,6 +331,13 @@
          </layout>
         </widget>
        </item>
+       <item>
+        <widget class="QPushButton" name="pushButton_save">
+         <property name="text">
+          <string>Export calibration (*.yaml)</string>
+         </property>
+        </widget>
+       </item>
        <item>
         <layout class="QHBoxLayout" name="horizontalLayout">
          <item>

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;
+}

utilite/include/rtabmap/utilite/UMath.h

@@ -533,18 +533,68 @@ inline T uVariance(const std::vector<T> & v, const T & m)
 }
 
 /**
- * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
- * @return the norm of the vector
+ * Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3 + ...
+ * @return the squared norm of the vector
  */
 template<class T>
-inline T uNorm(const std::vector<T> & v)
+inline T uNormSquared(const std::vector<T> & v)
 {
 	float sum = 0.0f;
 	for(unsigned int i=0; i<v.size(); ++i)
 	{
 		sum += v[i]*v[i];
 	}
-	return std::sqrt(sum);
+	return sum;
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const std::vector<T> & v)
+{
+	return std::sqrt(uNormSquared(v));
+}
+
+/**
+ * Get the squared norm of the vector : return x1*x1 + x2*x2
+ * @return the squared norm of the vector
+ */
+template<class T>
+inline T uNormSquared(const T & x1, const T & x2)
+{
+	return x1*x1 + x2*x2;
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const T & x1, const T & x2)
+{
+	return std::sqrt(uNormSquared(x1, x2));
+}
+
+/**
+ * Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3
+ * @return the squared norm of the vector
+ */
+template<class T>
+inline T uNormSquared(const T & x1, const T & x2, const T & x3)
+{
+	return x1*x1 + x2*x2 + x3*x3;
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const T & x1, const T & x2, const T & x3)
+{
+	return std::sqrt(uNormSquared(x1, x2, x3));
 }
 
 /**