Added rtabmap-kitti_dataset tool

corelib/include/rtabmap/core/CameraRGB.h

@@ -107,12 +107,14 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}
 
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setOdometryPath(const std::string & filePath, int format = 0)
 	{
 		_odometryPath = filePath;
 		_odometryFormat = format;
 	}
 
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
 		_groundTruthPath = filePath;
@@ -127,7 +129,11 @@ public:
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
-	bool readPoses(std::list<Transform> & outputPoses, std::list<double> & stamps, const std::string & filePath, int format) const;
+	bool readPoses(
+			std::list<Transform> & outputPoses,
+			std::list<double> & stamps,
+			const std::string & filePath,
+			int format) const;
 
 private:
 	std::string _path;

corelib/include/rtabmap/core/CameraThread.h

@@ -42,6 +42,8 @@ namespace rtabmap
 {
 
 class Camera;
+class CameraInfo;
+class SensorData;
 class StereoDense;
 
 /**
@@ -80,6 +82,8 @@ public:
 		_scanNormalsK = normalsK;
 	}
 
+	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
+
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}

corelib/include/rtabmap/core/Graph.h

@@ -53,7 +53,7 @@ bool RTABMAP_EXP exportPoses(
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, GPS (t,x,y)
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -48,6 +48,7 @@ public:
 	bool isGridFromDepth() const {return occupancyFromCloud_;}
 	bool isFullUpdate() const {return fullUpdate_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	int cacheSize() const {return (int)cache_.size();}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(

corelib/include/rtabmap/core/Rtabmap.h

@@ -59,11 +59,30 @@ public:
 	Rtabmap();
 	virtual ~Rtabmap();
 
-	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
+	/**
+	 * @brief Main loop of rtabmap.
+	 * @param data Sensor data to process.
+	 * @param odomPose Odometry pose, should be non-null for RGB-D SLAM mode.
+	 * @param covariance Odometry covariance.
+	 * @param externalStats External statistics to be saved in the database for convenience
+	 * @return true if data has been added to map.
+	 */
 	bool process(
 			const SensorData & data,
 			Transform odomPose,
-			const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+	// for convenience
+	bool process(
+			const SensorData & data,
+			Transform odomPose,
+			float odomLinearVariance,
+			float odomAngularVariance,
+			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+	// for convenience, loop closure detection only
+	bool process(
+			const cv::Mat & image,
+			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");

corelib/include/rtabmap/core/util3d.h

@@ -129,7 +129,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
 		const StereoCameraModel & model,
-		int decimation = 1,
+		float decimation = 1.0f,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,

corelib/src/CameraThread.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/StereoDense.h"
+#include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
 
 #include <rtabmap/utilite/UTimer.h>
@@ -129,181 +130,9 @@ void CameraThread::mainLoop()
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);
 
-	if(!data.imageRaw().empty())
+	if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
-		if(_colorOnly && !data.depthRaw().empty())
+		postUpdate(&data, &info);
-		{
-			data.setDepthOrRightRaw(cv::Mat());
-		}
-
-		if(_distortionModel && !data.depthRaw().empty())
-		{
-			UTimer timer;
-			if(_distortionModel->getWidth() == data.depthRaw().cols &&
-			   _distortionModel->getHeight() == data.depthRaw().rows	)
-			{
-				cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
-				_distortionModel->undistort(depth);
-				data.setDepthOrRightRaw(depth);
-			}
-			else
-			{
-				UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
-						_distortionModel->getWidth(), _distortionModel->getHeight(),
-						data.depthRaw().cols, data.depthRaw().rows);
-			}
-			info.timeUndistortDepth = timer.ticks();
-		}
-
-		if(_bilateralFiltering && !data.depthRaw().empty())
-		{
-			UTimer timer;
-			data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
-			info.timeBilateralFiltering = timer.ticks();
-		}
-
-		if(_imageDecimation>1 && !data.imageRaw().empty())
-		{
-			UDEBUG("");
-			UTimer timer;
-			if(!data.depthRaw().empty() &&
-			   !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
-			{
-				UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
-					   "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
-			}
-			else
-			{
-				data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
-				data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
-				std::vector<CameraModel> models = data.cameraModels();
-				for(unsigned int i=0; i<models.size(); ++i)
-				{
-					if(models[i].isValidForProjection())
-					{
-						models[i] = models[i].scaled(1.0/double(_imageDecimation));
-					}
-				}
-				data.setCameraModels(models);
-				StereoCameraModel stereoModel = data.stereoCameraModel();
-				if(stereoModel.isValidForProjection())
-				{
-					stereoModel.scale(1.0/double(_imageDecimation));
-					data.setStereoCameraModel(stereoModel);
-				}
-			}
-			info.timeImageDecimation = timer.ticks();
-		}
-		if(_mirroring && data.cameraModels().size() == 1)
-		{
-			UDEBUG("");
-			UTimer timer;
-			cv::Mat tmpRgb;
-			cv::flip(data.imageRaw(), tmpRgb, 1);
-			data.setImageRaw(tmpRgb);
-			UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
-			if(data.cameraModels().size() && data.cameraModels()[0].cx())
-			{
-				CameraModel tmpModel(
-						data.cameraModels()[0].fx(),
-						data.cameraModels()[0].fy(),
-						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
-						data.cameraModels()[0].cy(),
-						data.cameraModels()[0].localTransform());
-				data.setCameraModel(tmpModel);
-			}
-			if(!data.depthRaw().empty())
-			{
-				cv::Mat tmpDepth;
-				cv::flip(data.depthRaw(), tmpDepth, 1);
-				data.setDepthOrRightRaw(tmpDepth);
-			}
-			info.timeMirroring = timer.ticks();
-		}
-		if(_stereoToDepth && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
-		{
-			UDEBUG("");
-			UTimer timer;
-			cv::Mat depth = util2d::depthFromDisparity(
-					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
-					data.stereoCameraModel().left().fx(),
-					data.stereoCameraModel().baseline());
-			// set Tx for stereo bundle adjustment (when used)
-			CameraModel model = CameraModel(
-					data.stereoCameraModel().left().fx(),
-					data.stereoCameraModel().left().fy(),
-					data.stereoCameraModel().left().cx(),
-					data.stereoCameraModel().left().cy(),
-					data.stereoCameraModel().localTransform(),
-					-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
-			data.setCameraModel(model);
-			data.setDepthOrRightRaw(depth);
-			data.setStereoCameraModel(StereoCameraModel());
-			info.timeDisparity = timer.ticks();
-			UDEBUG("Computing disparity = %f s", info.timeDisparity);
-		}
-		if(_scanFromDepth &&
-			data.cameraModels().size() &&
-			data.cameraModels().at(0).isValidForProjection() &&
-			!data.depthRaw().empty())
-		{
-			UDEBUG("");
-			if(data.laserScanRaw().empty())
-			{
-				UASSERT(_scanDecimation >= 1);
-				UTimer timer;
-				pcl::IndicesPtr validIndices(new std::vector<int>);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
-						data,
-						_scanDecimation,
-						_scanMaxDepth,
-						_scanMinDepth,
-						validIndices.get());
-				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
-				cv::Mat scan;
-				const Transform & baseToScan = data.cameraModels()[0].localTransform();
-				if(validIndices->size())
-				{
-					if(_scanVoxelSize>0.0f)
-					{
-						cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
-						float ratio = float(cloud->size()) / float(validIndices->size());
-						maxPoints = ratio * maxPoints;
-					}
-					else if(!cloud->is_dense)
-					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
-						cloud = denseCloud;
-					}
-
-					if(cloud->size())
-					{
-						if(_scanNormalsK>0)
-						{
-							Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
-							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
-							pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-							pcl::concatenateFields(*cloud, *normals, *cloudNormals);
-							scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
-						}
-						else
-						{
-							scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
-						}
-					}
-				}
-				data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
-				info.timeScanFromDepth = timer.ticks();
-				UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
-			}
-			else
-			{
-				UWARN("Option to create laser scan from depth image is enabled, but "
-					  "there is already a laser scan in the captured sensor data. Scan from "
-					  "depth will not be created.");
-			}
-		}
 
 		info.cameraName = _camera->getSerial();
 		info.timeTotal = totalTime.ticks();
@@ -343,4 +172,181 @@ void CameraThread::mainLoopKill()
 	}
 }
 
+void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
+{
+	UASSERT(dataPtr!=0);
+	SensorData & data = *dataPtr;
+	if(_colorOnly && !data.depthRaw().empty())
+	{
+		data.setDepthOrRightRaw(cv::Mat());
+	}
+
+	if(_distortionModel && !data.depthRaw().empty())
+	{
+		UTimer timer;
+		if(_distortionModel->getWidth() == data.depthRaw().cols &&
+		   _distortionModel->getHeight() == data.depthRaw().rows	)
+		{
+			cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
+			_distortionModel->undistort(depth);
+			data.setDepthOrRightRaw(depth);
+		}
+		else
+		{
+			UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
+					_distortionModel->getWidth(), _distortionModel->getHeight(),
+					data.depthRaw().cols, data.depthRaw().rows);
+		}
+		if(info) info->timeUndistortDepth = timer.ticks();
+	}
+
+	if(_bilateralFiltering && !data.depthRaw().empty())
+	{
+		UTimer timer;
+		data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
+		if(info) info->timeBilateralFiltering = timer.ticks();
+	}
+
+	if(_imageDecimation>1 && !data.imageRaw().empty())
+	{
+		UDEBUG("");
+		UTimer timer;
+		if(!data.depthRaw().empty() &&
+		   !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
+		{
+			UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
+				   "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
+		}
+		else
+		{
+			data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
+			data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
+			std::vector<CameraModel> models = data.cameraModels();
+			for(unsigned int i=0; i<models.size(); ++i)
+			{
+				if(models[i].isValidForProjection())
+				{
+					models[i] = models[i].scaled(1.0/double(_imageDecimation));
+				}
+			}
+			data.setCameraModels(models);
+			StereoCameraModel stereoModel = data.stereoCameraModel();
+			if(stereoModel.isValidForProjection())
+			{
+				stereoModel.scale(1.0/double(_imageDecimation));
+				data.setStereoCameraModel(stereoModel);
+			}
+		}
+		if(info) info->timeImageDecimation = timer.ticks();
+	}
+	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
+	{
+		UDEBUG("");
+		UTimer timer;
+		cv::Mat tmpRgb;
+		cv::flip(data.imageRaw(), tmpRgb, 1);
+		data.setImageRaw(tmpRgb);
+		UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
+		if(data.cameraModels().size() && data.cameraModels()[0].cx())
+		{
+			CameraModel tmpModel(
+					data.cameraModels()[0].fx(),
+					data.cameraModels()[0].fy(),
+					float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
+					data.cameraModels()[0].cy(),
+					data.cameraModels()[0].localTransform());
+			data.setCameraModel(tmpModel);
+		}
+		if(!data.depthRaw().empty())
+		{
+			cv::Mat tmpDepth;
+			cv::flip(data.depthRaw(), tmpDepth, 1);
+			data.setDepthOrRightRaw(tmpDepth);
+		}
+		if(info) info->timeMirroring = timer.ticks();
+	}
+	if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
+	{
+		UDEBUG("");
+		UTimer timer;
+		cv::Mat depth = util2d::depthFromDisparity(
+				_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
+				data.stereoCameraModel().left().fx(),
+				data.stereoCameraModel().baseline());
+		// set Tx for stereo bundle adjustment (when used)
+		CameraModel model = CameraModel(
+				data.stereoCameraModel().left().fx(),
+				data.stereoCameraModel().left().fy(),
+				data.stereoCameraModel().left().cx(),
+				data.stereoCameraModel().left().cy(),
+				data.stereoCameraModel().localTransform(),
+				-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
+		data.setCameraModel(model);
+		data.setDepthOrRightRaw(depth);
+		data.setStereoCameraModel(StereoCameraModel());
+		if(info) info->timeDisparity = timer.ticks();
+	}
+	if(_scanFromDepth &&
+		data.cameraModels().size() &&
+		data.cameraModels().at(0).isValidForProjection() &&
+		!data.depthRaw().empty())
+	{
+		UDEBUG("");
+		if(data.laserScanRaw().empty())
+		{
+			UASSERT(_scanDecimation >= 1);
+			UTimer timer;
+			pcl::IndicesPtr validIndices(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+					data,
+					_scanDecimation,
+					_scanMaxDepth,
+					_scanMinDepth,
+					validIndices.get());
+			float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
+			cv::Mat scan;
+			const Transform & baseToScan = data.cameraModels()[0].localTransform();
+			if(validIndices->size())
+			{
+				if(_scanVoxelSize>0.0f)
+				{
+					cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
+					float ratio = float(cloud->size()) / float(validIndices->size());
+					maxPoints = ratio * maxPoints;
+				}
+				else if(!cloud->is_dense)
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
+					cloud = denseCloud;
+				}
+
+				if(cloud->size())
+				{
+					if(_scanNormalsK>0)
+					{
+						Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
+						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
+					}
+					else
+					{
+						scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
+					}
+				}
+			}
+			data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
+			if(info) info->timeScanFromDepth = timer.ticks();
+		}
+		else
+		{
+			UWARN("Option to create laser scan from depth image is enabled, but "
+				  "there is already a laser scan in the captured sensor data. Scan from "
+				  "depth will not be created.");
+		}
+	}
+}
+
 } // namespace rtabmap

corelib/src/DBDriverSqlite3.cpp

@@ -807,7 +807,7 @@ ParametersMap DBDriverSqlite3::getLastParametersQuery() const
 
 std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, double & stamp) const
 {
-	UDEBUG("");
+	UDEBUG("nodeId=%d", nodeId);
 	std::map<std::string, float> data;
 	if(_ppDb)
 	{

corelib/src/DBReader.cpp

@@ -357,7 +357,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 
 			int seq = *_currentId;
 			++_currentId;
-			if(data.imageCompressed().empty())
+			if(data.imageCompressed().empty() && weight>=0)
 			{
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}

corelib/src/Parameters.cpp

@@ -643,6 +643,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 					if(i < argc)
 					{
 						uInsert(out, ParametersPair(iter->first, argv[i]));
+						UINFO("Parsed parameter \"%s\"=\"%s\"", iter->first.c_str(), argv[i]);
 					}
 				}
 				else

corelib/src/Rtabmap.cpp

@@ -815,10 +815,39 @@ void Rtabmap::resetMemory()
 //============================================================
 // MAIN LOOP
 //============================================================
+bool Rtabmap::process(
+		const cv::Mat & image,
+		int id,
+		const std::map<std::string, float> & externalStats)
+{
+	return this->process(SensorData(image, id), Transform());
+}
+bool Rtabmap::process(
+			const SensorData & data,
+			Transform odomPose,
+			float odomLinearVariance,
+			float odomAngularVariance,
+			const std::map<std::string, float> & externalStats)
+{
+	if(!odomPose.isNull())
+	{
+		UASSERT(odomLinearVariance>0.0f);
+		UASSERT(odomAngularVariance>0.0f);
+	}
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+	covariance.at<double>(0,0) = odomLinearVariance;
+	covariance.at<double>(1,1) = odomLinearVariance;
+	covariance.at<double>(2,2) = odomLinearVariance;
+	covariance.at<double>(3,3) = odomAngularVariance;
+	covariance.at<double>(4,4) = odomAngularVariance;
+	covariance.at<double>(5,5) = odomAngularVariance;
+	return process(data, odomPose, covariance, externalStats);
+}
 bool Rtabmap::process(
 		const SensorData & data,
 		Transform odomPose,
-		const cv::Mat & covariance)
+		const cv::Mat & odomCovariance,
+		const std::map<std::string, float> & externalStats)
 {
 	UDEBUG("");
 
@@ -870,6 +899,10 @@ bool Rtabmap::process(
 	std::set<int> immunizedLocations;
 
 	statistics_ = Statistics(); // reset
+	for(std::map<std::string, float>::const_iterator iter=externalStats.begin(); iter!=externalStats.end(); ++iter)
+	{
+		statistics_.addStatistic(iter->first, iter->second);
+	}
 
 	//============================================================
 	// Wait for an image...
@@ -952,7 +985,7 @@ bool Rtabmap::process(
 	ULOGGER_INFO("Updating memory...");
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->update(data, odomPose, covariance, &statistics_))
+		if(!_memory->update(data, odomPose, odomCovariance, &statistics_))
 		{
 			return false;
 		}
@@ -982,8 +1015,8 @@ bool Rtabmap::process(
 	std::list<int> signaturesRemoved;
 	if(_rgbdSlamMode)
 	{
-		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), covariance.empty()?1.0f:(float)covariance.at<double>(0,0));
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
-		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), covariance.empty()?1.0f:(float)covariance.at<double>(5,5));
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));
 
 		//Verify if there was a rehearsal
 		int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -2727,11 +2760,6 @@ bool Rtabmap::process(
 	return true;
 }
 
-bool Rtabmap::process(const cv::Mat & image, int id)
-{
-	return this->process(SensorData(image, id), Transform());
-}
-
 // SETTERS
 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
 {

corelib/src/util3d.cpp

@@ -759,7 +759,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
 		const StereoCameraModel & model,
-		int decimation,
+		float decimation,
 		float maxDepth,
 		float minDepth,
 		std::vector<int> * validIndices,
@@ -770,20 +770,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
 	UASSERT(imageLeft.rows == imageRight.rows &&
 			imageLeft.cols == imageRight.cols);
-	UASSERT(decimation >= 1);
+	UASSERT(decimation >= 1.0f);
 
 	cv::Mat leftColor = imageLeft;
 	cv::Mat rightMono = imageRight;
 
 	StereoCameraModel modelDecimation = model;
 
-	if(leftColor.rows % decimation != 0 ||
+	if(decimation>1.0f)
-	   leftColor.cols % decimation != 0)
 	{
-		leftColor = util2d::decimate(leftColor, decimation);
+		cv::Mat resized;
-		rightMono = util2d::decimate(rightMono, decimation);
+		cv::resize(leftColor, resized, cv::Size(), 1.0f/decimation, 1.0f/decimation, cv::INTER_AREA);
+		leftColor = resized;
+		resized = cv::Mat();
+		cv::resize(rightMono, resized, cv::Size(), 1.0f/decimation, 1.0f/decimation, cv::INTER_AREA);
+		rightMono = resized;
 		modelDecimation.scale(1/float(decimation));
-		decimation = 1;
 	}
 
 	cv::Mat leftMono;
@@ -800,7 +802,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 			leftColor,
 			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
 			modelDecimation,
-			decimation,
+			1,
 			maxDepth,
 			minDepth,
 			validIndices);

guilib/src/ExportCloudsDialog.cpp

@@ -2314,7 +2314,15 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 				}
 				else
 				{
-					UERROR("Cloud %d not found in cache!", iter->first);
+					int weight = 0;
+					if(_dbDriver)
+					{
+						_dbDriver->getWeight(iter->first, weight);
+					}
+					if(weight>=0) // don't show error for intermediate nodes
+					{
+						UERROR("Cloud %d not found in cache!", iter->first);
+					}
 				}
 			}
 			else if(uContains(cachedClouds, iter->first))

guilib/src/MainWindow.cpp

@@ -2386,7 +2386,10 @@ void MainWindow::updateMapCloud(
 		else
 #endif
 		{
-			_occupancyGrid->update(poses);
+			if(_occupancyGrid->addedNodes().size() || _occupancyGrid->cacheSize()>0)
+			{
+				_occupancyGrid->update(poses);
+			}
 			if(stats)
 			{
 				stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
@@ -2505,7 +2508,6 @@ void MainWindow::updateMapCloud(
 
 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
 {
-	UDEBUG("");
 	UASSERT(!pose.isNull());
 	std::string cloudName = uFormat("cloud%d", nodeId);
 	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
@@ -2824,7 +2826,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 		}
 	}
 
-	UDEBUG("");
 	return outputPair;
 }
 

tools/CMakeLists.txt

@@ -5,6 +5,7 @@ ADD_SUBDIRECTORY( ExtractObject )
 ADD_SUBDIRECTORY( Camera )
 ADD_SUBDIRECTORY( CameraRGBD )
 ADD_SUBDIRECTORY( StereoEval )
+ADD_SUBDIRECTORY( KittiDataset )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/KittiDataset/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+endif()
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(kitti_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(kitti_dataset ${LIBRARIES})
+
+if(internal)
+  SET_TARGET_PROPERTIES( kitti_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-kitti_dataset)
+endif(internal)

tools/KittiDataset/main.cpp

@@ -0,0 +1,570 @@
+/*
+Copyright (c) 2010-2016, 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/OdometryF2M.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include <pcl/common/common.h>
+#include <stdio.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-kitti_dataset [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/KITTI/dataset/sequences/07\")\n"
+			"                        containing least calib.txt, times.txt, image_0 and image_1 folders.\n"
+			"                        Optional image_2, image_3 and velodyne folders.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --gt \"path\"        Ground truth path (e.g., ~/KITTI/devkit/cpp/data/odometry/poses/07.txt)\n"
+			"  --color            Use color images for stereo (image_2 and image_3 folders).\n"
+			"  --scan             Include velodyne scan in node's data.\n"
+			"  --scan_step #      Scan downsample step (default=10).\n"
+			"  --scan_voxel #.#   Scan voxel size (default 0.3 m).\n"
+			"  --scan_k           Scan normal K (default 20).\n"
+			"  --map_update  #    Do map update each X odometry frames (default=10, which\n"
+			"                        gives 1 Hz map update assuming images are at 10 Hz).\n\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ rtabmap-kitti_dataset \\\n"
+			"       --Vis/EstimationType 1\\\n"
+			"       --Vis/BundleAdjustment 1\\\n"
+			"       --Vis/PnPReprojError 1.5\\\n"
+			"       --Odom/GuessMotion true\\\n"
+			"       --OdomF2M/BundleAdjustment 1\\\n"
+			"       --Rtabmap/CreateIntermediateNodes true\\\n"
+			"       --gt \"~/KITTI/devkit/cpp/data/odometry/poses/07.txt\"\\\n"
+			"       ~/KITTI/dataset/sequences/07\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string seq;
+	int mapUpdate = 10;
+	bool color = false;
+	bool scan = false;
+	int scanStep = 10;
+	float scanVoxel = 0.3f;
+	int scanNormalK = 20;
+	std::string gtPath;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--map_update") == 0)
+			{
+				mapUpdate = atoi(argv[++i]);
+				if(mapUpdate <= 0)
+				{
+					printf("map_update should be > 0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_step") == 0)
+			{
+				scanStep = atoi(argv[++i]);
+				if(scanStep <= 0)
+				{
+					printf("scan_step should be > 0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_voxel") == 0)
+			{
+				scanVoxel = atof(argv[++i]);
+				if(scanVoxel < 0.0f)
+				{
+					printf("scan_voxel should be >= 0.0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_k") == 0)
+			{
+				scanNormalK = atoi(argv[++i]);
+				if(scanNormalK < 0)
+				{
+					printf("scanNormalK should be >= 0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--gt") == 0)
+			{
+				gtPath = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--color") == 0)
+			{
+				color = true;
+			}
+			else if(std::strcmp(argv[i], "--scan") == 0)
+			{
+				color = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+		}
+	}
+
+	seq = uSplit(path, '/').back();
+	if(seq.empty() || !(uStr2Int(seq)>=0 && uStr2Int(seq)<=21))
+	{
+		UWARN("Sequence number \"%s\" should be between 0 and 21 (official KITTI datasets).", seq.c_str());
+		seq.clear();
+	}
+	std::string pathLeftImages  = path+(color?"/image_2":"/image_0");
+	std::string pathRightImages = path+(color?"/image_3":"/image_1");
+	std::string pathCalib = path+"/calib.txt";
+	std::string pathTimes = path+"/times.txt";
+	std::string pathScan;
+
+	printf("Paths:\n"
+			"   Sequence number:  %s\n"
+			"   Sequence path:    %s\n"
+			"   Output:           %s\n"
+			"   left images:      %s\n"
+			"   right images:     %s\n"
+			"   calib.txt:        %s\n"
+			"   times.txt:        %s\n",
+			seq.c_str(),
+			path.c_str(),
+			output.c_str(),
+			pathLeftImages.c_str(),
+			pathRightImages.c_str(),
+			pathCalib.c_str(),
+			pathTimes.c_str());
+	if(!gtPath.empty())
+	{
+		gtPath = uReplaceChar(gtPath, '~', UDirectory::homeDir());
+		gtPath = uReplaceChar(gtPath, '\\', '/');
+		printf("   Ground Truth:      %s\n", gtPath.c_str());
+		if(!UFile::exists(gtPath))
+		{
+			UERROR("Ground truth file path is not valid: \"%s\"", gtPath.c_str());
+			return -1;
+		}
+	}
+	if(scan)
+	{
+		pathScan = path+"/velodyne";
+		printf("   Scan:              %s\n", pathScan.c_str());
+		printf("   Scan step:         %d\n", scanStep);
+		printf("   Scan voxel:        %fm\n", scanVoxel);
+		printf("   Scan normal k:     %d\n", scanNormalK);
+	}
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+
+	// convert calib.txt to rtabmap format (yaml)
+	FILE * pFile = 0;
+	pFile = fopen(pathCalib.c_str(),"r");
+	if(!pFile)
+	{
+		UERROR("Cannot open calibration file \"%s\"", pathCalib.c_str());
+		return -1;
+	}
+	cv::Mat_<double> P0(3,4);
+	cv::Mat_<double> P1(3,4);
+	cv::Mat_<double> P2(3,4);
+	cv::Mat_<double> P3(3,4);
+	char skipStr[10];
+	fscanf (pFile, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", skipStr,
+			&P0(0, 0), &P0(0, 1), &P0(0, 2), &P0(0, 3),
+			&P0(1, 0), &P0(1, 1), &P0(1, 2), &P0(1, 3),
+			&P0(2, 0), &P0(2, 1), &P0(2, 2), &P0(2, 3));
+	fscanf (pFile, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", skipStr,
+			&P1(0, 0), &P1(0, 1), &P1(0, 2), &P1(0, 3),
+			&P1(1, 0), &P1(1, 1), &P1(1, 2), &P1(1, 3),
+			&P1(2, 0), &P1(2, 1), &P1(2, 2), &P1(2, 3));
+	fscanf (pFile, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", skipStr,
+			&P2(0, 0), &P2(0, 1), &P2(0, 2), &P2(0, 3),
+			&P2(1, 0), &P2(1, 1), &P2(1, 2), &P2(1, 3),
+			&P2(2, 0), &P2(2, 1), &P2(2, 2), &P2(2, 3));
+	fscanf (pFile, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", skipStr,
+			&P3(0, 0), &P3(0, 1), &P3(0, 2), &P3(0, 3),
+			&P3(1, 0), &P3(1, 1), &P3(1, 2), &P3(1, 3),
+			&P3(2, 0), &P3(2, 1), &P3(2, 2), &P3(2, 3));
+	fclose (pFile);
+	// get image size
+	UDirectory dir(pathLeftImages);
+	std::string firstImage = dir.getNextFileName();
+	cv::Mat image = cv::imread(dir.getNextFilePath());
+	if(image.empty())
+	{
+		UERROR("Failed to read first image of \"%s\"", firstImage.c_str());
+		return -1;
+	}
+	StereoCameraModel model("rtabmap_calib"+seq,
+			image.size(), P0.colRange(0,3), cv::Mat(), cv::Mat(), P0,
+			image.size(), P1.colRange(0,3), cv::Mat(), cv::Mat(), P1,
+			cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat());
+	if(!model.save(output, true))
+	{
+		UERROR("Could not save calibration!");
+		return -1;
+	}
+	printf("Saved calibration \"%s\" to \"%s\"\n", ("rtabmap_calib"+seq).c_str(), output.c_str());
+
+	// We use CameraThread only to use postUpdate() method
+	Transform opticalRotation(0,0,1,0, -1,0,0,color?-0.06:0, 0,-1,0,0);
+	CameraThread cameraThread(new
+		CameraStereoImages(
+				pathLeftImages,
+				pathRightImages,
+				false, // assume that images are already rectified
+				0.0f,
+				opticalRotation), parameters);
+	((CameraStereoImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
+	if(!gtPath.empty())
+	{
+		((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(gtPath, 2);
+	}
+	if(!pathScan.empty())
+	{
+		((CameraStereoImages*)cameraThread.camera())->setScanPath(
+						pathScan,
+						130000,
+						scanStep,
+						scanVoxel,
+						scanNormalK,
+						Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
+	}
+
+	bool intermediateNodes = false;
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+	std::string databasePath = output+"/rtabmap" + seq + ".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(output, "rtabmap_calib"+seq))
+	{
+		int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
+
+		OdometryF2M odom(parameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		UTimer totalTime;
+		UTimer timer;
+		CameraInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
+		int iteration = 0;
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		while(data.isValid() && g_forever)
+		{
+			std::map<std::string, float> externalStats;
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			// save camera statistics to database
+			externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+			externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+			externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+			externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+			externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+			externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+			externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+			externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+
+			OdometryInfo odomInfo;
+			Transform pose = odom.process(data, &odomInfo);
+			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+			float speed = odomInfo.transform.x()/odomInfo.interval*3.6;
+			externalStats.insert(std::make_pair("Odometry/Speed/ms", speed));
+			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
+			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
+
+			bool processData = true;
+			if(iteration % mapUpdate != 0)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				rtabmap.process(data, pose, odomInfo.varianceLin, odomInfo.varianceAng, externalStats);
+			}
+			double slamTime = timer.ticks();
+
+			++iteration;
+			printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms\n",
+					iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+
+			cameraInfo = CameraInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeImage(&cameraInfo);
+		}
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving rtabmap_trajectory.txt ...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		rtabmap.getGraph(poses, links, true, true);
+		std::string pathTrajectory = output+"/rtabmap_poses"+seq+".txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!gtPath.empty())
+		{
+			// Log ground truth statistics (in TUM's RGBD-SLAM format)
+			std::map<int, Transform> groundTruth;
+			graph::importPoses(gtPath, 2, groundTruth);
+			if(poses.size() == groundTruth.size())
+			{
+				//align with ground truth for more meaningful results
+				pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+				cloud1.resize(poses.size());
+				cloud2.resize(poses.size());
+				int oi = 0;
+				int idFirst = 0;
+				for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
+				{
+					std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
+					if(iter2!=poses.end())
+					{
+						if(oi==0)
+						{
+							idFirst = iter->first;
+						}
+						cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+						cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
+					}
+				}
+
+				Transform t = Transform::getIdentity();
+				if(oi>5)
+				{
+					cloud1.resize(oi);
+					cloud2.resize(oi);
+
+					t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+				}
+				else if(idFirst)
+				{
+					t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
+				}
+				if(!t.isIdentity())
+				{
+					for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+					{
+						iter->second = t * iter->second;
+					}
+				}
+
+				std::vector<float> translationalErrors(poses.size());
+				std::vector<float> rotationalErrors(poses.size());
+				float sumTranslationalErrors = 0.0f;
+				float sumRotationalErrors = 0.0f;
+				float sumSqrdTranslationalErrors = 0.0f;
+				float sumSqrdRotationalErrors = 0.0f;
+				float radToDegree = 180.0f / M_PI;
+				float translational_min = 0.0f;
+				float translational_max = 0.0f;
+				float rotational_min = 0.0f;
+				float rotational_max = 0.0f;
+				oi=0;
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
+					if(jter!=groundTruth.end())
+					{
+						Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+						Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+						rotationalErrors[oi] = a*radToDegree;
+						translationalErrors[oi] = iter->second.getDistance(jter->second);
+
+						sumTranslationalErrors+=translationalErrors[oi];
+						sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
+						sumRotationalErrors+=rotationalErrors[oi];
+						sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
+
+						if(oi == 0)
+						{
+							translational_min = translational_max = translationalErrors[oi];
+							rotational_min = rotational_max = rotationalErrors[oi];
+						}
+						else
+						{
+							if(translationalErrors[oi] < translational_min)
+							{
+								translational_min = translationalErrors[oi];
+							}
+							else if(translationalErrors[oi] > translational_max)
+							{
+								translational_max = translationalErrors[oi];
+							}
+
+							if(rotationalErrors[oi] < rotational_min)
+							{
+								rotational_min = rotationalErrors[oi];
+							}
+							else if(rotationalErrors[oi] > rotational_max)
+							{
+								rotational_max = rotationalErrors[oi];
+							}
+						}
+
+						++oi;
+					}
+				}
+				translationalErrors.resize(oi);
+				rotationalErrors.resize(oi);
+				if(oi)
+				{
+					float total = float(oi);
+					float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
+					float translational_mean = sumTranslationalErrors/total;
+					float translational_median = translationalErrors[oi/2];
+					float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
+
+					float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
+					float rotational_mean = sumRotationalErrors/total;
+					float rotational_median = rotationalErrors[oi/2];
+					float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
+
+					printf("Ground truth comparison:\n");
+					printf("  translational_rmse=   %f\n", translational_rmse);
+					printf("  translational_mean=   %f\n", translational_mean);
+					printf("  translational_median= %f\n", translational_median);
+					printf("  translational_std=    %f\n", translational_std);
+					printf("  translational_min=    %f\n", translational_min);
+					printf("  translational_max=    %f\n", translational_max);
+					printf("  rotational_rmse=      %f\n", rotational_rmse);
+					printf("  rotational_mean=      %f\n", rotational_mean);
+					printf("  rotational_median=    %f\n", rotational_median);
+					printf("  rotational_std=       %f\n", rotational_std);
+					printf("  rotational_min=       %f\n", rotational_min);
+					printf("  rotational_max=       %f\n", rotational_max);
+
+					pFile = 0;
+					std::string pathErrors = output+"/rtabmap_rmse"+seq+".txt";
+					pFile = fopen(pathErrors.c_str(),"w");
+					if(!pFile)
+					{
+						UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+					}
+					fprintf(pFile, "Ground truth comparison:\n");
+					fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+					fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+					fprintf(pFile, "  translational_median= %f\n", translational_median);
+					fprintf(pFile, "  translational_std=    %f\n", translational_std);
+					fprintf(pFile, "  translational_min=    %f\n", translational_min);
+					fprintf(pFile, "  translational_max=    %f\n", translational_max);
+					fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+					fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+					fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+					fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+					fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+					fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+					fclose(pFile);
+				}
+			}
+			else
+			{
+				UWARN("Cannot compute ground truth statistics, the computed poses (%d) are not the same as the ground truth (%d). Make sure to use option \"--Rtabmap/CreateIntermediateNodes true\".");
+			}
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/rtabmap" + seq + ".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/rtabmap" + seq + ".db").c_str());
+
+	return 0;
+}