Added rtabmap-kitti_dataset tool

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())
-		{
-			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.");
-			}
-		}
+		postUpdate(&data, &info);
 
 		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_ang(), covariance.empty()?1.0f:(float)covariance.at<double>(5,5));
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
+		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 ||
-	   leftColor.cols % decimation != 0)
+	if(decimation>1.0f)
 	{
-		leftColor = util2d::decimate(leftColor, decimation);
-		rightMono = util2d::decimate(rightMono, decimation);
+		cv::Mat resized;
+		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);