merged multicamera branch into devel branch

corelib/src/CMakeLists.txt

@@ -35,7 +35,6 @@ SET(SRC_FILES
 	util3d_surface.cpp
 	util3d_features.cpp
 	util3d_correspondences.cpp
-	util3d_conversions.cpp
 	
 	SensorData.cpp
 	Graph.cpp

corelib/src/CameraModel.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap {
@@ -39,13 +40,21 @@ CameraModel::CameraModel() :
 
 }
 
-CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P) :
+CameraModel::CameraModel(
+		const std::string & cameraName,
+		const cv::Size & imageSize,
+		const cv::Mat & K,
+		const cv::Mat & D,
+		const cv::Mat & R,
+		const cv::Mat & P,
+		const Transform & localTransform) :
 		name_(cameraName),
 		imageSize_(imageSize),
 		K_(K),
 		D_(D),
 		R_(R),
-		P_(P)
+		P_(P),
+		localTransform_(localTransform)
 {
 	UASSERT(!name_.empty());
 	UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
@@ -59,6 +68,35 @@ CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageS
 	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
 }
 
+CameraModel::CameraModel(
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		const Transform & localTransform,
+		double Tx) :
+		K_(cv::Mat::eye(3, 3, CV_64FC1)),
+		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
+		R_(cv::Mat::eye(3, 3, CV_64FC1)),
+		P_(cv::Mat::eye(3, 4, CV_64FC1)),
+		localTransform_(localTransform)
+{
+	UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
+	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	P_.at<double>(0,0) = fx;
+	P_.at<double>(1,1) = fy;
+	P_.at<double>(0,2) = cx;
+	P_.at<double>(1,2) = cy;
+	P_.at<double>(0,3) = Tx;
+
+	K_.at<double>(0,0) = fx;
+	K_.at<double>(1,1) = fy;
+	K_.at<double>(0,2) = cx;
+	K_.at<double>(1,2) = cy;
+}
+
 bool CameraModel::load(const std::string & filePath)
 {
 	K_ = cv::Mat();
@@ -176,6 +214,22 @@ bool CameraModel::save(const std::string & filePath)
 	return false;
 }
 
+void CameraModel::scale(double scale)
+{
+	UASSERT(scale > 0.0);
+	// has only effect on K and P
+	imageSize_.width *= scale;
+	imageSize_.height *= scale;
+	K_.at<double>(0,0) *= scale;
+	K_.at<double>(1,1) *= scale;
+	K_.at<double>(0,2) *= scale;
+	K_.at<double>(1,2) *= scale;
+	P_.at<double>(0,0) *= scale;
+	P_.at<double>(1,1) *= scale;
+	P_.at<double>(0,2) *= scale;
+	P_.at<double>(1,2) *= scale;
+}
+
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 {
 	if(!mapX_.empty() && !mapY_.empty())
@@ -364,7 +418,13 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
 	return false;
 }
 
-Transform StereoCameraModel::transform() const
+void StereoCameraModel::scale(double scale)
+{
+	left_.scale(scale);
+	right_.scale(scale);
+}
+
+Transform StereoCameraModel::stereoTransform() const
 {
 	if(!R_.empty() && !T_.empty())
 	{

corelib/src/CameraThread.cpp

@@ -109,13 +109,13 @@ void CameraThread::mainLoop()
 	UDEBUG("");
 	cv::Mat rgb, depth;
 	float fx = 0.0f;
-	float fy = 0.0f;
+	float fyOrBaseline = 0.0f;
 	float cx = 0.0f;
 	float cy = 0.0f;
 	double stamp = UTimer::now();
 	if(_cameraRGBD)
 	{
-		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
+		_cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp);
 	}
 	else
 	{
@@ -125,8 +125,19 @@ void CameraThread::mainLoop()
 	if(!rgb.empty())
 	{
 		if(_cameraRGBD)
-		{
-			SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, stamp);
+		{
+			SensorData data;
+			if(dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) || dynamic_cast<CameraStereoDC1394*>(_cameraRGBD))
+			{
+				//stereo
+				data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
+				UASSERT(data.stereoCameraModel().isValid());
+			}
+			else
+			{
+				data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
+				UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid());
+			}
 			this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
 		}
 		else

corelib/src/DBDriver.cpp

@@ -412,15 +412,7 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetric
 
 void DBDriver::getNodeData(
 		int signatureId,
-		cv::Mat & imageCompressed,
-		cv::Mat & depthCompressed,
-		cv::Mat & laserScanCompressed,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		int & laserScanMaxPts) const
+		SensorData & data) const
 {
 	bool found = false;
 	// look in the trash
@@ -428,17 +420,9 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->getImageCompressed().empty() || !s->isSaved())
+		if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
 		{
-			imageCompressed = s->getImageCompressed();
-			depthCompressed = s->getDepthCompressed();
-			laserScanCompressed = s->getLaserScanCompressed();
-			fx = s->getFx();
-			fy = s->getFy();
-			cx = s->getCx();
-			cy = s->getCy();
-			localTransform = s->getLocalTransform();
-			laserScanMaxPts = s->getLaserScanMaxPts();
+			data = (SensorData)s->sensorData();
 			found = true;
 		}
 	}
@@ -447,31 +431,7 @@ void DBDriver::getNodeData(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
-		_dbSafeAccessMutex.unlock();
-	}
-}
-
-void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
-{
-	bool found = false;
-	// look in the trash
-	_trashesMutex.lock();
-	if(uContains(_trashSignatures, signatureId))
-	{
-		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->getImageCompressed().empty() || !s->isSaved())
-		{
-			imageCompressed = s->getImageCompressed();
-			found = true;
-		}
-	}
-	_trashesMutex.unlock();
-
-	if(!found)
-	{
-		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed);
+		this->getNodeDataQuery(signatureId, data);
 		_dbSafeAccessMutex.unlock();
 	}
 }

corelib/src/DBDriverSqlite3.cpp

@@ -458,10 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 		if(loadMetricData)
 		{
-			if(uStrNumCmp(_version, "0.8.11") >= 0)
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				query << "SELECT image, depth, calibration, scan_max_pts, scan "
+					  << "FROM Data "
+					  << "WHERE id = ?"
+					  <<";";
+			}
+			else if(uStrNumCmp(_version, "0.8.11") >= 0)
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+						 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -471,7 +478,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			else if(uStrNumCmp(_version, "0.7.0") >= 0)
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -481,7 +488,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			else
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -491,10 +498,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 		}
 		else
 		{
-			query << "SELECT data "
-				  << "FROM Image "
-				  << "WHERE id = ?"
-				  <<";";
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				query << "SELECT image "
+					  << "FROM Data "
+					  << "WHERE id = ?"
+					  <<";";
+			}
+			else
+			{
+				query << "SELECT data "
+					  << "FROM Image "
+					  << "WHERE id = ?"
+					  <<";";
+			}
 		}
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
@@ -519,13 +536,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			{
 				index = 0;
 
+				cv::Mat imageCompressed;
+				cv::Mat depthOrRightCompressed;
+				std::vector<CameraModel> models;
+				StereoCameraModel stereoModel;
+				Transform localTransform = Transform::getIdentity();
+				cv::Mat scanCompressed;
+
 				data = sqlite3_column_blob(ppStmt, index);
 				dataSize = sqlite3_column_bytes(ppStmt, index++);
 
 				//Create the image
 				if(dataSize>4 && data)
 				{
-					(*iter)->setImageCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 				}
 
 				if(loadMetricData)
@@ -534,35 +558,92 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
 
 					//Create the depth image
-					cv::Mat depthCompressed;
 					if(dataSize>4 && data)
 					{
-						depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+						depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 					}
 
-					if(uStrNumCmp(_version, "0.7.0") < 0)
+					if(uStrNumCmp(_version, "0.10.0") < 0)
 					{
-						float depthConstant = sqlite3_column_double(ppStmt, index++);
-						(*iter)->setDepthCompressed(depthCompressed, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0);
+						data = sqlite3_column_blob(ppStmt, index); // local transform
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
+						{
+							memcpy(localTransform.data(), data, dataSize);
+						}
+					}
+
+					// calibration
+					if(uStrNumCmp(_version, "0.10.0") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+						// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+						if(dataSize > 0 && data)
+						{
+							float * dataFloat = (float*)data;
+							if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+							{
+								int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+								int max = cameraCount*(4+localTransform.size());
+								for(int i=0; i<max; i+=4+localTransform.size())
+								{
+									memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+									models.push_back(CameraModel(
+											(double)dataFloat[i],
+											(double)dataFloat[i+1],
+											(double)dataFloat[i+2],
+											(double)dataFloat[i+3],
+											localTransform));
+								}
+							}
+							else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+							{
+								UDEBUG("Loading calibration of a stereo camera");
+								memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+								stereoModel = StereoCameraModel(
+										dataFloat[0],  // fx
+										dataFloat[1],  // fy
+										dataFloat[2],  // cx
+										dataFloat[3],  // cy
+										dataFloat[4], // baseline
+										localTransform);
+							}
+							else
+							{
+								UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+							}
+						}
+
+					}
+					else if(uStrNumCmp(_version, "0.7.0") >= 0)
+					{
+						double fx = sqlite3_column_double(ppStmt, index++);
+						double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+						double cx = sqlite3_column_double(ppStmt, index++);
+						double cy = sqlite3_column_double(ppStmt, index++);
+						if(fyOrBaseline < 1.0)
+						{
+							//it is a baseline
+							stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+						}
+						else
+						{
+							models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+						}
 					}
 					else
 					{
-						float fx = sqlite3_column_double(ppStmt, index++);
-						float fy = sqlite3_column_double(ppStmt, index++);
-						float cx = sqlite3_column_double(ppStmt, index++);
-						float cy = sqlite3_column_double(ppStmt, index++);
-						(*iter)->setDepthCompressed(depthCompressed, fx, fy, cx, cy);
+						float depthConstant = sqlite3_column_double(ppStmt, index++);
+						float fx = 1.0f/depthConstant;
+						float fy = 1.0f/depthConstant;
+						float cx = 0.0f;
+						float cy = 0.0f;
+						models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
 					}
 
-					data = sqlite3_column_blob(ppStmt, index); // local transform
-					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					Transform localTransform;
-					if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
-					{
-						memcpy(localTransform.data(), data, dataSize);
-					}
-					(*iter)->setLocalTransform(localTransform);
-
 					int laserScanMaxPts = 0;
 					if(uStrNumCmp(_version, "0.8.11") >= 0)
 					{
@@ -574,8 +655,30 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					//Create the laserScan
 					if(dataSize>4 && data)
 					{
-						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
+						scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
 					}
+
+					if(models.size())
+					{
+						(*iter)->sensorData() = SensorData(
+								scanCompressed,
+								laserScanMaxPts,
+								imageCompressed,
+								depthOrRightCompressed,
+								models,
+								(*iter)->id());
+					}
+					else
+					{
+						(*iter)->sensorData() = SensorData(
+								scanCompressed,
+								laserScanMaxPts,
+								imageCompressed,
+								depthOrRightCompressed,
+								stereoModel,
+								(*iter)->id());
+					}
+
 				}
 
 				rc = sqlite3_step(ppStmt); // next result...
@@ -596,15 +699,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 void DBDriverSqlite3::getNodeDataQuery(
 		int signatureId,
-		cv::Mat & imageCompressed,
-		cv::Mat & depthCompressed,
-		cv::Mat & laserScanCompressed,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		int & laserScanMaxPts) const
+		 SensorData & sensorData) const
 {
 	if(_ppDb)
 	{
@@ -614,10 +709,17 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.8.11") >= 0)
+		if(uStrNumCmp(_version, "0.10.0") >= 0)
+		{
+			query << "SELECT image, depth, calibration, scan_max_pts, scan "
+				  << "FROM Data "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.8.11") >= 0)
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+					 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -627,7 +729,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		else if(uStrNumCmp(_version, "0.7.0") >= 0)
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -637,7 +739,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		else
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -650,7 +752,15 @@ void DBDriverSqlite3::getNodeDataQuery(
 
 		const void * data = 0;
 		int dataSize = 0;
-		int index = 0;;
+		int index = 0;
+
+		cv::Mat imageCompressed;
+		cv::Mat depthOrRightCompressed;
+		std::vector<CameraModel> models;
+		StereoCameraModel stereoModel;
+		Transform localTransform = Transform::getIdentity();
+		int laserScanMaxPts;
+		cv::Mat scanCompressed;
 
 		ULOGGER_DEBUG("Loading data for %d...", signatureId);
 
@@ -675,30 +785,88 @@ void DBDriverSqlite3::getNodeDataQuery(
 			//Create the depth image
 			if(dataSize>4 && data)
 			{
-				depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 			}
 
-			if(uStrNumCmp(_version, "0.7.0") < 0)
+			if(uStrNumCmp(_version, "0.10.0") < 0)
 			{
-				float depthConstant = sqlite3_column_double(ppStmt, index++);
-				fx = 1.0f/depthConstant;
-				fy = 1.0f/depthConstant;
-				cx = 0.0f;
-				cy = 0.0f;
+				data = sqlite3_column_blob(ppStmt, index); // local transform
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
+				{
+					memcpy(localTransform.data(), data, dataSize);
+				}
+			}
+
+			// calibration
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+				// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+				if(dataSize > 0 && data)
+				{
+					float * dataFloat = (float*)data;
+					if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras", cameraCount);
+						int max = cameraCount*(4+localTransform.size());
+						for(int i=0; i<max; i+=4+localTransform.size())
+						{
+							memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									dataFloat[i],
+									dataFloat[i+1],
+									dataFloat[i+2],
+									dataFloat[i+3],
+									localTransform));
+						}
+					}
+					else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+					{
+						UDEBUG("Loading calibration for a stereo camera");
+						memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+						stereoModel = StereoCameraModel(
+								dataFloat[0],  // fx
+								dataFloat[1],  // fy
+								dataFloat[2],  // cx
+								dataFloat[3],  // cy
+								dataFloat[4], // baseline
+								localTransform);
+					}
+					else
+					{
+						UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+					}
+				}
+
+			}
+			else if(uStrNumCmp(_version, "0.7.0") >= 0)
+			{
+				double fx = sqlite3_column_double(ppStmt, index++);
+				double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+				double cx = sqlite3_column_double(ppStmt, index++);
+				double cy = sqlite3_column_double(ppStmt, index++);
+				if(fyOrBaseline < 1.0)
+				{
+					//it is a baseline
+					stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+				}
+				else
+				{
+					models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+				}
 			}
 			else
 			{
-				fx = sqlite3_column_double(ppStmt, index++);
-				fy = sqlite3_column_double(ppStmt, index++);
-				cx = sqlite3_column_double(ppStmt, index++);
-				cy = sqlite3_column_double(ppStmt, index++);
-			}
-
-			data = sqlite3_column_blob(ppStmt, index); // local transform
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-			if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
-			{
-				memcpy(localTransform.data(), data, dataSize);
+				float depthConstant = sqlite3_column_double(ppStmt, index++);
+				float fx = 1.0f/depthConstant;
+				float fy = 1.0f/depthConstant;
+				float cx = 0.0f;
+				float cy = 0.0f;
+				models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
 			}
 
 			laserScanMaxPts = 0;
@@ -712,63 +880,28 @@ void DBDriverSqlite3::getNodeDataQuery(
 			//Create the depth2d
 			if(dataSize>4 && data)
 			{
-				laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 			}
 
-			if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
+			if(models.size())
 			{
-				UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only.");
+				sensorData = SensorData(
+						scanCompressed,
+						laserScanMaxPts,
+						imageCompressed,
+						depthOrRightCompressed,
+						models,
+						signatureId);
 			}
-
-			rc = sqlite3_step(ppStmt); // next result...
-		}
-		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		ULOGGER_DEBUG("Time=%fs", timer.ticks());
-	}
-}
-
-void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const
-{
-	if(_ppDb)
-	{
-		UTimer timer;
-		timer.start();
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
-
-		query << "SELECT data "
-			  << "FROM Image "
-			  << "WHERE id = " << signatureId
-			  <<";";
-
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-
-		const void * data = 0;
-		int dataSize = 0;
-		int index = 0;;
-
-		ULOGGER_DEBUG("Loading data for %d...", signatureId);
-
-		// Process the result if one
-		rc = sqlite3_step(ppStmt);
-		if(rc == SQLITE_ROW)
-		{
-			index = 0;
-
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-
-			//Create the image
-			if(dataSize>4 && data)
+			else
 			{
-				imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				sensorData = SensorData(
+						scanCompressed,
+						laserScanMaxPts,
+						imageCompressed,
+						depthOrRightCompressed,
+						stereoModel,
+						signatureId);
 			}
 
 			rc = sqlite3_step(ppStmt); // next result...
@@ -1216,8 +1349,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						weight,
 						stamp,
 						label,
-						std::multimap<int, cv::KeyPoint>(),
-						std::multimap<int, pcl::PointXYZ>(),
 						pose,
 						userData);
 				s->setSaved(true);
@@ -1900,7 +2031,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				const std::map<int, Link> & links = (*j)->getLinks();
 				for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 				{
-					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
+					stepLink(ppStmt, i->second);
 				}
 			}
 		}
@@ -2008,7 +2139,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			const std::map<int, Link> & links = (*jter)->getLinks();
 			for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 			{
-				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
+				stepLink(ppStmt, i->second);
 			}
 		}
 		// Finalize (delete) the statement
@@ -2048,40 +2179,66 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		UDEBUG("Time=%fs", timer.ticks());
 
-		// Add images
-		query = queryStepImage();
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		UDEBUG("Saving %d images", signatures.size());
-
-		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+		if(uStrNumCmp(_version, "0.10.0") >= 0)
 		{
-			if(!(*i)->getImageCompressed().empty())
+			// Add SensorData
+			query = queryStepSensorData();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Saving %d images", signatures.size());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				stepImage(ppStmt, (*i)->id(), (*i)->getImageCompressed());
+				if(!(*i)->sensorData().imageCompressed().empty())
+				{
+					UASSERT((*i)->id() == (*i)->sensorData().id());
+					stepSensorData(ppStmt, (*i)->sensorData());
+				}
 			}
+
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Time=%fs", timer.ticks());
 		}
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		UDEBUG("Time=%fs", timer.ticks());
-
-		// Add depths
-		query = queryStepDepth();
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+		else
 		{
-			//metric
-			if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
+			// Add images
+			query = queryStepImage();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Saving %d images", signatures.size());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
+				if(!(*i)->sensorData().imageCompressed().empty())
+				{
+					stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
+				}
 			}
+
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Time=%fs", timer.ticks());
+
+			// Add depths
+			query = queryStepDepth();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+			{
+				//metric
+				if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().empty())
+				{
+					UASSERT((*i)->id() == (*i)->sensorData().id());
+					stepDepth(ppStmt, (*i)->sensorData());
+				}
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		}
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 		UDEBUG("Time=%fs", timer.ticks());
 	}
@@ -2216,12 +2373,14 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 
 std::string DBDriverSqlite3::queryStepImage() const
 {
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
 	return "INSERT INTO Image(id, data) VALUES(?,?);";
 }
 void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 		int id,
 		const cv::Mat & imageBytes) const
 {
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
 	UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
 	if(!ppStmt)
 	{
@@ -2254,6 +2413,7 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepDepth() const
 {
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
 	if(uStrNumCmp(_version, "0.8.11") >= 0)
 	{
 		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
@@ -2267,18 +2427,13 @@ std::string DBDriverSqlite3::queryStepDepth() const
 		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
 	}
 }
-void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
-		int id,
-		const cv::Mat & depthBytes,
-		const cv::Mat & depth2dBytes,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
-		int depth2dMaxPts) const
+void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
 {
-	UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
+	UDEBUG("Save depth %d (size=%d) depth2d = %d",
+			sensorData.id(),
+			(int)sensorData.depthOrRightCompressed().cols,
+			(int)sensorData.laserScanCompressed().cols);
 	if(!ppStmt)
 	{
 		UFATAL("");
@@ -2287,12 +2442,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	int rc = SQLITE_OK;
 	int index = 1;
 
-	rc = sqlite3_bind_int(ppStmt, index++, id);
+	rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(!depthBytes.empty())
+	if(!sensorData.depthOrRightCompressed().empty())
 	{
-		rc = sqlite3_bind_blob(ppStmt, index++, depthBytes.data, (int)depthBytes.cols, SQLITE_STATIC);
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
 	}
 	else
 	{
@@ -2300,11 +2455,33 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	float fx=0, fyOrBaseline=0, cx=0, cy=0;
+	Transform localTransform = Transform::getIdentity();
+	if(sensorData.cameraModels().size())
+	{
+		UASSERT_MSG(sensorData.cameraModels().size() == 1,
+				uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
+
+		fx = sensorData.cameraModels()[0].fx();
+		fyOrBaseline = sensorData.cameraModels()[0].fy();
+		cx = sensorData.cameraModels()[0].cx();
+		cy = sensorData.cameraModels()[0].cy();
+		localTransform = sensorData.cameraModels()[0].localTransform();
+	}
+	else if(sensorData.stereoCameraModel().isValid())
+	{
+		fx = sensorData.stereoCameraModel().left().fx();
+		fyOrBaseline = sensorData.stereoCameraModel().baseline();
+		cx = sensorData.stereoCameraModel().left().cx();
+		cy = sensorData.stereoCameraModel().left().cy();
+		localTransform = sensorData.stereoCameraModel().left().localTransform();
+	}
+
 	if(uStrNumCmp(_version, "0.7.0") >= 0)
 	{
 		rc = sqlite3_bind_double(ppStmt, index++, fx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		rc = sqlite3_bind_double(ppStmt, index++, fy);
+		rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		rc = sqlite3_bind_double(ppStmt, index++, cx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2320,9 +2497,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(!depth2dBytes.empty())
+	if(!sensorData.laserScanCompressed().empty())
 	{
-		rc = sqlite3_bind_blob(ppStmt, index++, depth2dBytes.data, (int)depth2dBytes.cols, SQLITE_STATIC);
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
 	}
 	else
 	{
@@ -2332,7 +2509,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 
 	if(uStrNumCmp(_version, "0.8.11") >= 0)
 	{
-		rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
+		rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 
@@ -2344,6 +2521,116 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepSensorData() const
+{
+	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
+	return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
+}
+void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
+		const SensorData & sensorData) const
+{
+	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
+	UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
+			sensorData.id(),
+			(int)sensorData.imageCompressed().cols,
+			(int)sensorData.depthOrRightCompressed().cols,
+			(int)sensorData.laserScanCompressed().cols);
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	// id
+	rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// image
+	if(!sensorData.imageCompressed().empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// depth or right image
+	if(!sensorData.depthOrRightCompressed().empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// calibration
+	std::vector<float> calibration;
+	// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+	// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+	if(sensorData.cameraModels().size())
+	{
+		calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+		{
+			const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
+			calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
+			calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
+			calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
+			calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
+			memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
+		}
+	}
+	else if(sensorData.stereoCameraModel().isValid())
+	{
+		const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
+		calibration.resize(5+localTransform.size());
+		calibration[0] = sensorData.stereoCameraModel().left().fx();
+		calibration[1] = sensorData.stereoCameraModel().left().fy();
+		calibration[2] = sensorData.stereoCameraModel().left().cx();
+		calibration[3] = sensorData.stereoCameraModel().left().cy();
+		calibration[4] = sensorData.stereoCameraModel().baseline();
+		memcpy(calibration.data()+5, localTransform.data(), localTransform.size()*sizeof(float));
+	}
+
+	if(calibration.size())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// scan_max_pts
+	rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// scan
+	if(!sensorData.laserScanCompressed().empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	//step
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+}
+
 std::string DBDriverSqlite3::queryStepLink() const
 {
 	if(uStrNumCmp(_version, "0.8.4") >= 0)
@@ -2361,21 +2648,16 @@ std::string DBDriverSqlite3::queryStepLink() const
 }
 void DBDriverSqlite3::stepLink(
 		sqlite3_stmt * ppStmt,
-		int fromId,
-		int toId,
-		Link::Type type,
-		float rotVariance,
-		float transVariance,
-		const Transform & transform) const
+		const Link & link) const
 {
 	if(!ppStmt)
 	{
 		UFATAL("");
 	}
-	UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type);
+	UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
 
 	// Don't save virtual links
-	if(type==Link::kVirtualClosure)
+	if(link.type()==Link::kVirtualClosure)
 	{
 		UDEBUG("Virtual link ignored....");
 		return;
@@ -2383,27 +2665,27 @@ void DBDriverSqlite3::stepLink(
 
 	int rc = SQLITE_OK;
 	int index = 1;
-	rc = sqlite3_bind_int(ppStmt, index++, fromId);
+	rc = sqlite3_bind_int(ppStmt, index++, link.from());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	rc = sqlite3_bind_int(ppStmt, index++, toId);
+	rc = sqlite3_bind_int(ppStmt, index++, link.to());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	rc = sqlite3_bind_int(ppStmt, index++, type);
+	rc = sqlite3_bind_int(ppStmt, index++, link.type());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 	if(uStrNumCmp(_version, "0.8.4") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, rotVariance);
+		rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		rc = sqlite3_bind_double(ppStmt, index++, transVariance);
+		rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 	else if(uStrNumCmp(_version, "0.7.4") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, rotVariance<transVariance?rotVariance:transVariance);
+		rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 
-	rc = sqlite3_bind_blob(ppStmt, index++, transform.data(), transform.size()*sizeof(float), SQLITE_STATIC);
+	rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 	rc=sqlite3_step(ppStmt);

corelib/src/DBDriverSqlite3.h

@@ -71,18 +71,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
-	virtual void getNodeDataQuery(
-			int signatureId,
-			cv::Mat & imageCompressed,
-			cv::Mat & depthCompressed,
-			cv::Mat & laserScanCompressed,
-			float & fx,
-			float & fy,
-			float & cx,
-			float & cy,
-			Transform & localTransform,
-			int & laserScanMaxPts) const;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
+	virtual void getNodeDataQuery(int signatureId, SensorData & data) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -94,6 +83,7 @@ private:
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
+	std::string queryStepSensorData() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
@@ -102,18 +92,9 @@ private:
 			sqlite3_stmt * ppStmt,
 			int id,
 			const cv::Mat & imageBytes) const;
-	void stepDepth(
-			sqlite3_stmt * ppStmt,
-			int id,
-			const cv::Mat & depthBytes,
-			const cv::Mat & depth2dBytes,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
-			const Transform & localTransform,
-			int depth2dMaxPts) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
+	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 

corelib/src/DBReader.cpp

@@ -148,39 +148,39 @@ void DBReader::mainLoopBegin()
 
 void DBReader::mainLoop()
 {
-	SensorData data = this->getNextData();
-	if(data.isValid())
+	OdometryEvent odom = this->getNextData();
+	if(odom.data().id())
 	{
 		int goalId = 0;
-		double previousStamp = data.stamp();
-		data.setStamp(UTimer::now());
-		if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
+		double previousStamp = odom.data().stamp();
+		odom.data().setStamp(UTimer::now());
+		if(odom.data().userData().size() >= 6 && memcmp(odom.data().userData().data(), "GOAL:", 5) == 0)
 		{
 			//GOAL format detected, remove it from the user data and send it as goal event
-			std::string goalStr = uBytes2Str(data.userData());
+			std::string goalStr = uBytes2Str(odom.data().userData());
 			if(!goalStr.empty())
 			{
 				std::list<std::string> strs = uSplit(goalStr, ':');
 				if(strs.size() == 2)
 				{
 					goalId = atoi(strs.rbegin()->c_str());
-					data.setUserData(std::vector<unsigned char>());
+					odom.data().setUserData(std::vector<unsigned char>());
 				}
 			}
 		}
 		if(!_odometryIgnored)
 		{
-			if(data.pose().isNull())
+			if(odom.pose().isNull())
 			{
 				UWARN("Reading the database: odometry is null! "
 					  "Please set \"Ignore odometry = true\" if there is "
 					  "no odometry in the database.");
 			}
-			this->post(new OdometryEvent(data));
+			this->post(new OdometryEvent(odom));
 		}
 		else
 		{
-			this->post(new CameraEvent(data));
+			this->post(new CameraEvent(odom.data()));
 		}
 
 		if(goalId > 0)
@@ -242,31 +242,26 @@ void DBReader::mainLoop()
 
 }
 
-SensorData DBReader::getNextData()
+OdometryEvent DBReader::getNextData()
 {
-	SensorData data;
+	OdometryEvent odom;
 	if(_dbDriver)
 	{
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
-			cv::Mat imageBytes;
-			cv::Mat depthBytes;
-			cv::Mat laserScanBytes;
 			int mapId;
-			float fx,fy,cx,cy;
-			Transform localTransform, pose;
-			float rotVariance = 1.0f;
-			float transVariance = 1.0f;
 			std::vector<unsigned char> userData;
-			int laserScanMaxPts = 0;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
+			SensorData data;
+			_dbDriver->getNodeData(*_currentId, data);
 
 			// info
+			Transform pose;
 			int weight;
 			std::string label;
 			double stamp;
 			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
 
+			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
 			{
 				std::map<int, Link> links;
@@ -274,8 +269,7 @@ SensorData DBReader::getNextData()
 				if(links.size())
 				{
 					// assume the first is the backward neighbor, take its variance
-					rotVariance = links.begin()->second.rotVariance();
-					transVariance = links.begin()->second.transVariance();
+					infMatrix = links.begin()->second.infMatrix();
 				}
 			}
 			else
@@ -285,7 +279,7 @@ SensorData DBReader::getNextData()
 
 			int seq = *_currentId;
 			++_currentId;
-			if(imageBytes.empty())
+			if(data.imageCompressed().empty())
 			{
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
@@ -339,33 +333,16 @@ SensorData DBReader::getNextData()
 
 			if(!this->isKilled())
 			{
-				rtabmap::CompressionThread ctImage(imageBytes, true);
-				rtabmap::CompressionThread ctDepth(depthBytes, true);
-				rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
-				ctImage.start();
-				ctDepth.start();
-				ctLaserScan.start();
-				ctImage.join();
-				ctDepth.join();
-				ctLaserScan.join();
-				data = SensorData(
-						ctLaserScan.getUncompressedData(),
-						laserScanMaxPts,
-						ctImage.getUncompressedData(),
-						ctDepth.getUncompressedData(),
-						fx,fy,cx,cy,
-						localTransform,
-						pose,
-						rotVariance,
-						transVariance,
-						seq,
-						stamp,
-						userData);
-				UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
-						data.laserScan().empty()?0:1,
-						data.image().empty()?0:1,
-						data.depth().empty()?0:1,
-						data.rightImage().empty()?0:1);
+				data.uncompressData();
+				data.setId(seq);
+				data.setStamp(stamp);
+				data.setUserData(userData);
+				UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d",
+						data.laserScanRaw().empty()?0:1,
+						data.imageRaw().empty()?0:1,
+						data.depthOrRightRaw().empty()?0:1);
+
+				odom = OdometryEvent(data, pose, infMatrix.inv());
 			}
 		}
 	}
@@ -373,7 +350,7 @@ SensorData DBReader::getNextData()
 	{
 		UERROR("Not initialized...");
 	}
-	return data;
+	return odom;
 }
 
 } /* namespace rtabmap */

corelib/src/Graph.cpp

@@ -263,20 +263,23 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
 				AISNavigation::TreePoseGraph2::InformationMatrix inf;
 				//Identity:
-				inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x
-				inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y
-				inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta
-				if(!isCovarianceIgnored())
+				if(isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
-					{
-						inf.values[0][0] = 1.0f/iter->second.transVariance(); // x
-						inf.values[1][1] = 1.0f/iter->second.transVariance(); // y
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta
-					}
+					inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
+					inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
+					inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
+				}
+				else
+				{
+					inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
+					inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
+					inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
+					inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
+					inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
+					inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
+					inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
+					inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
+					inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
 				int id1 = iter->first;
@@ -304,18 +307,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.rotVariance()>0)
-					{
-						inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
-						inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
-						inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
-					}
-					if(iter->second.transVariance()>0)
-					{
-						inf[3][3] = 1.0f/iter->second.transVariance(); // x
-						inf[4][4] = 1.0f/iter->second.transVariance(); // y
-						inf[5][5] = 1.0f/iter->second.transVariance(); // z
-					}
+					memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
 				}
 
 				int id1 = iter->first;
@@ -491,7 +483,7 @@ bool TOROOptimizer::saveGraph(
 		{
 			float x,y,z, yaw,pitch,roll;
 			pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw);
-			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
+			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 					iter->first,
 					iter->second.to(),
 					x,
@@ -500,12 +492,27 @@ bool TOROOptimizer::saveGraph(
 					roll,
 					pitch,
 					yaw,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f);
+					iter->second.infMatrix().at<double>(0,0),
+					iter->second.infMatrix().at<double>(0,1),
+					iter->second.infMatrix().at<double>(0,2),
+					iter->second.infMatrix().at<double>(0,3),
+					iter->second.infMatrix().at<double>(0,4),
+					iter->second.infMatrix().at<double>(0,5),
+					iter->second.infMatrix().at<double>(1,1),
+					iter->second.infMatrix().at<double>(1,2),
+					iter->second.infMatrix().at<double>(1,3),
+					iter->second.infMatrix().at<double>(1,4),
+					iter->second.infMatrix().at<double>(1,5),
+					iter->second.infMatrix().at<double>(2,2),
+					iter->second.infMatrix().at<double>(2,3),
+					iter->second.infMatrix().at<double>(2,4),
+					iter->second.infMatrix().at<double>(2,5),
+					iter->second.infMatrix().at<double>(3,3),
+					iter->second.infMatrix().at<double>(3,4),
+					iter->second.infMatrix().at<double>(3,5),
+					iter->second.infMatrix().at<double>(4,4),
+					iter->second.infMatrix().at<double>(4,5),
+					iter->second.infMatrix().at<double>(5,5));
 		}
 		UINFO("Graph saved to %s", fileName.c_str());
 		fclose(file);
@@ -689,15 +696,15 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
-					{
-						information(0,0) = 1.0f/iter->second.transVariance(); // x
-						information(1,1) = 1.0f/iter->second.transVariance(); // y
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						information(2,2) = 1.0f/iter->second.rotVariance(); // theta
-					}
+					information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
+					information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
+					information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
+					information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
+					information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
+					information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
+					information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
+					information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
+					information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
 				g2o::EdgeSE2 * e = new g2o::EdgeSE2();
@@ -716,18 +723,7 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
-					{
-						information(0,0) = 1.0f/iter->second.transVariance(); // x
-						information(1,1) = 1.0f/iter->second.transVariance(); // y
-						information(2,2) = 1.0f/iter->second.transVariance(); // z
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						information(3,3) = 1.0f/iter->second.rotVariance(); // roll
-						information(4,4) = 1.0f/iter->second.rotVariance(); // pitch
-						information(5,5) = 1.0f/iter->second.rotVariance(); // yaw
-					}
+					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
 				}
 
 				Eigen::Affine3d a = iter->second.transform().toEigen3d();

corelib/src/Odometry.cpp

@@ -161,16 +161,16 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		_pose.setIdentity(); // initialized
 	}
 
-	UASSERT(!data.image().empty());
+	UASSERT(!data.imageRaw().empty());
 	if(dynamic_cast<OdometryMono*>(this) == 0)
 	{
-		UASSERT(!data.depthOrRightImage().empty());
+		UASSERT(!data.depthOrRightRaw().empty());
 	}
 
-	if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
+	if(!data.stereoCameraModel().isValid() &&
+	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
 	{
-		UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
-				data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
+		UERROR("Rectified images required! Calibrate your camera.");
 		return Transform();
 	}
 

corelib/src/OdometryBOW.cpp

@@ -160,8 +160,15 @@ Transform OdometryBOW::computeTransform(
 			{
 				if(this->isPnPEstimationUsed())
 				{
-					if((int)newSignature->getWords().size() >= this->getMinInliers())
+					if(data.cameraModels().size() > 1)
 					{
+						UERROR("PnP cannot be used on multi-cameras setup.");
+					}
+					else if((int)newSignature->getWords().size() >= this->getMinInliers())
+					{
+						UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
+						const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
+
 						// find correspondences
 						std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
 						std::vector<cv::Point3f> objectPoints(ids.size());
@@ -194,11 +201,8 @@ Transform OdometryBOW::computeTransform(
 						if((int)matches.size() >= this->getMinInliers())
 						{
 							//PnPRansac
-							cv::Mat K = (cv::Mat_<double>(3,3) <<
-								data.fx(), 0, data.cx(),
-								0, data.fy()>0?data.fy():data.fx(), data.cy(),
-								0, 0, 1);
-							Transform guess = (this->getPose() * data.localTransform()).inverse();
+							cv::Mat K = cameraModel.K();
+							Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
 							cv::Mat R = (cv::Mat_<double>(3,3) <<
 									(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 									(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -229,7 +233,7 @@ Transform OdometryBOW::computeTransform(
 											   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
 								// make it incremental
-								transform = (data.localTransform() * pnp * this->getPose()).inverse();
+								transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse();
 
 								UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
 

corelib/src/OdometryICP.cpp

@@ -72,25 +72,32 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 	bool hasConverged = false;
 	double variance = 0;
 	unsigned int minPoints = 100;
-	if(!data.depth().empty())
+	if(!data.depthOrRightRaw().empty())
 	{
-		if(data.depth().type() == CV_8UC1)
+		if(data.depthOrRightRaw().type() == CV_8UC1)
 		{
 			UERROR("ICP 3D cannot be done on stereo images!");
 			return output;
 		}
 
+		if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
+		{
+			UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
+			return output;
+		}
+		const CameraModel & cameraModel = data.cameraModels()[0];
+
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
-						data.depth(),
-						data.fx(),
-						data.fy(),
-						data.cx(),
-						data.cy(),
+						data.depthOrRightRaw(),
+						cameraModel.fx(),
+						cameraModel.fy(),
+						cameraModel.cx(),
+						cameraModel.cy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
 						_samples,
-						data.localTransform());
+						cameraModel.localTransform());
 
 		if(_pointToPlane)
 		{

corelib/src/OdometryMono.cpp

@@ -147,11 +147,24 @@ void OdometryMono::reset(const Transform & initialPose)
 
 Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
 {
-	UASSERT(!data.image().empty());
-	UASSERT(data.fx());
+	Transform output;
+
+	if(data.imageRaw().empty())
+	{
+		UERROR("Image empty! Cannot compute odometry...");
+		return output;
+	}
+
+	if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid())))
+	{
+		UERROR("Odometry cannot be done without calibration or on multi-camera!");
+		return output;
+	}
+
+
+	const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
 
 	UTimer timer;
-	Transform output;
 
 	int inliers = 0;
 	int correspondences = 0;
@@ -159,13 +172,13 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 
 	cv::Mat newFrame;
 	// convert to grayscale
-	if(data.image().channels() > 1)
+	if(data.imageRaw().channels() > 1)
 	{
-		cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		newFrame = data.image().clone();
+		newFrame = data.imageRaw().clone();
 	}
 
 	if(memory_->getStMem().size() >= 1)
@@ -190,11 +203,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 				nFeatures = (int)newS->getWords().size();
 				if((int)newS->getWords().size() > this->getMinInliers())
 				{
-					cv::Mat K = (cv::Mat_<double>(3,3) <<
-						data.fx(), 0, data.cx(),
-						0, data.fy()==0?data.fx():data.fy(), data.cy(),
-						0, 0, 1);
-					Transform guess = (this->getPose() * data.localTransform()).inverse();
+					cv::Mat K = cameraModel.K();
+					Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
 					cv::Mat R = (cv::Mat_<double>(3,3) <<
 							(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 							(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -216,7 +226,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					UDEBUG("project points to previous image");
 					std::vector<cv::Point2f> prevImagePoints;
 					const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
-					Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse();
+					Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse();
 					cv::Mat prevR = (cv::Mat_<double>(3,3) <<
 							(double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
 							(double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
@@ -240,8 +250,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					{
 						if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) &&
 						   uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) &&
-						   uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) &&
-						   uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows))
+						   uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) &&
+						   uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows))
 						{
 							refCorners[oi] = prevImagePoints[i];
 							newCorners[oi] = imagePoints[i];
@@ -273,7 +283,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					std::vector<float> err;
 					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 					cv::calcOpticalFlowPyrLK(
-							prevS->getImageRaw(),
+							prevS->sensorData().imageRaw(),
 							newFrame,
 							refCorners,
 							newCorners,
@@ -357,7 +367,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							Transform pnp = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
 													  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 													  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
-							output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse();
+							output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse();
 
 							if(this->isInfoDataFilled() && info && inliersV.size())
 							{
@@ -402,9 +412,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
 										previousS->getWords(),
 										newS->getWords(),
-										data.fx(), data.fy()?data.fy():data.fx(),
-										data.cx(), data.cy(),
-										data.localTransform(),
+										cameraModel,
 										cameraTransform,
 										this->getIterations(),
 										this->getPnPReprojError(),
@@ -515,7 +523,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 			std::vector<float> err;
 			UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 			cv::calcOpticalFlowPyrLK(
-					refS->getImageRaw(),
+					refS->sensorData().imageRaw(),
 					newFrame,
 					refCorners,
 					refCornersGuess,
@@ -652,10 +660,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 						//UDEBUG("Correcting matches...done!");
 
 						UDEBUG("Computing P...");
-						cv::Mat K = (cv::Mat_<double>(3,3) <<
-									data.fx(), 0, data.cx(),
-									0, data.fy()==0?data.fx():data.fy(), data.cy(),
-									0, 0, 1);
+						cv::Mat K = cameraModel.K();
 
 						cv::Mat Kinv = K.inv();
 						cv::Mat E = K.t()*F*K;
@@ -716,7 +721,15 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									(*inliersRef)[oi] = cloud->at(i);
 									if(!refDepth_.empty())
 									{
-										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
+										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(
+												refDepth_,
+												refCorners[i].x,
+												refCorners[i].y,
+												cameraModel.cx(),
+												cameraModel.cy(),
+												cameraModel.fx(),
+												cameraModel.fy(),
+												true);
 									}
 									++oi;
 								}
@@ -824,7 +837,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 											  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 											  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
-								output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
+								output = cameraModel.localTransform() * pnp.inverse() * cameraModel.localTransform().inverse();
 								if(output.getNorm() < minTranslation_*5)
 								{
 									reject = true;
@@ -844,7 +857,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 										int index  =inliersPnP.at(i);
 										int id = cornerIds[index];
 										UASSERT(id > 0 && id <= *wordsId.rbegin());
-										pcl::PointXYZ pt =  util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform());
+										pcl::PointXYZ pt =  util3d::transformPoint(
+												pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z),
+												this->getPose()*cameraModel.localTransform());
 										localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
 										keyFrameWords3D.insert(std::make_pair(id, pt));
 									}
@@ -890,7 +905,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 				{
 					cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
 				}
-				refDepth_ = data.depth().clone();
+				refDepth_ = data.depthOrRightRaw().clone();
 				keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
 			}
 			else

corelib/src/OdometryOpticalFlow.cpp

@@ -124,6 +124,17 @@ Transform OdometryOpticalFlow::computeTransform(
 {
 	UTimer timer;
 	Transform output;
+	if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
+	{
+		UERROR("Calibrated stereo camera required");
+		return output;
+	}
+	if(!data.depthRaw().empty() &&
+		(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
+	{
+		UERROR("Calibrated camera required (multi-cameras not supported).");
+		return output;
+	}
 
 	double variance = 0;
 	int inliers = 0;
@@ -136,20 +147,20 @@ Transform OdometryOpticalFlow::computeTransform(
 
 	cv::Mat newLeftFrame;
 	// convert to grayscale
-	if(data.image().channels() > 1)
+	if(data.imageRaw().channels() > 1)
 	{
-		cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		newLeftFrame = data.image().clone();
+		newLeftFrame = data.imageRaw().clone();
 	}
 
 	std::vector<cv::Point2f> newCorners;
 	UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
-			(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightImage().empty()?0:1);
+			(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightRaw().empty()?0:1);
 	if(!refFrame_.empty() &&
-	   !data.depthOrRightImage().empty() &&
+	   ((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()) &&
 	   refCorners_.size() &&
 	   refCorners3D_->size())
 	{
@@ -158,11 +169,9 @@ Transform OdometryOpticalFlow::computeTransform(
 
 		// make guess
 		bool flowGuessByMotion = true;
-		cv::Mat K = (cv::Mat_<double>(3,3) <<
-			data.fx(), 0, data.cx(),
-			0, data.fx(), data.cy(),
-			0, 0, 1);
-		Transform guess = (this->previousTransform() * data.localTransform()).inverse();
+		cv::Mat K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K();
+		Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform();
+		Transform guess = (this->previousTransform() * localTransform).inverse();
 		cv::Mat R = (cv::Mat_<double>(3,3) <<
 				(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 				(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -263,7 +272,7 @@ Transform OdometryOpticalFlow::computeTransform(
 					if((int)inliersV.size() >= this->getMinInliers())
 					{
 						// make it incremental
-						output = (data.localTransform() * pnp).inverse();
+						output = (localTransform * pnp).inverse();
 						variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
 					}
 					else
@@ -294,17 +303,17 @@ Transform OdometryOpticalFlow::computeTransform(
 					info->newCorners.resize(newCornersKept.size());
 				}
 				int oi = 0;
-				if(!data.rightImage().empty())
+				if(!data.rightRaw().empty())
 				{
 					// stereo
 					pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D = util3d::generateKeypoints3DStereo(
 							newCornersKept,
 							newLeftFrame,
-							data.rightImage(),
-							data.fx(),
-							data.baseline(),
-							data.cx(),
-							data.cy(),
+							data.rightRaw(),
+							data.stereoCameraModel().left().fx(),
+							data.stereoCameraModel().baseline(),
+							data.stereoCameraModel().left().cx(),
+							data.stereoCameraModel().left().cy(),
 							Transform::getIdentity(),
 							stereoWinSize_,
 							stereoMaxLevel_,
@@ -319,7 +328,7 @@ Transform OdometryOpticalFlow::computeTransform(
 						{
 							//Add 3D correspondences!
 							correspondencesRef->at(oi) = refCorners3DKept->at(i);
-							correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), data.localTransform());
+							correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), localTransform);
 							if(this->isInfoDataFilled() && info)
 							{
 								info->refCorners[oi] = refCornersKept[i];
@@ -334,17 +343,18 @@ Transform OdometryOpticalFlow::computeTransform(
 					//depth
 					for(unsigned int i=0; i<newCornersKept.size(); ++i)
 					{
-						if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depth().cols)) &&
-						   uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depth().rows)))
+						if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depthRaw().cols)) &&
+						   uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depthRaw().rows)))
 						{
-							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCornersKept[i].x, newCorners[i].y,
-									data.cx(), data.cy(), data.fx(), data.fy(), true);
+							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depthRaw(), newCornersKept[i].x, newCorners[i].y,
+									data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].fx(), data.cameraModels()[0].fy(), true);
 							if(pcl::isFinite(pt) &&
 								(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
 							{
 								//Add 3D correspondences!
 								correspondencesRef->at(oi) = refCorners3DKept->at(i);
-								correspondencesNew->at(oi) = util3d::transformPoint(pt, data.localTransform());
+								correspondencesNew->at(oi) = util3d::transformPoint(pt, localTransform);
+
 								if(this->isInfoDataFilled() && info)
 								{
 									info->refCorners[oi] = refCornersKept[i];
@@ -444,17 +454,17 @@ Transform OdometryOpticalFlow::computeTransform(
 			newCorners3D->resize(newCorners.size());
 			std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
 			int oi=0;
-			if(!data.rightImage().empty())
+			if(!data.rightRaw().empty())
 			{
 				/// stereo
 				pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
 						newCorners,
 						newLeftFrame,
-						data.rightImage(),
-						data.fx(),
-						data.baseline(),
-						data.cx(),
-						data.cy(),
+						data.rightRaw(),
+						data.stereoCameraModel().left().fx(),
+						data.stereoCameraModel().baseline(),
+						data.stereoCameraModel().left().cx(),
+						data.stereoCameraModel().left().cy(),
 						Transform::getIdentity(),
 						stereoWinSize_,
 						stereoMaxLevel_,
@@ -467,7 +477,7 @@ Transform OdometryOpticalFlow::computeTransform(
 					if(pcl::isFinite(refCorners3DTmp->at(i)) &&
 						(this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth()))
 					{
-						newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.localTransform());
+						newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.stereoCameraModel().left().localTransform());
 						newCornersFiltered[oi] = newCorners[i];
 						++oi;
 					}
@@ -478,15 +488,22 @@ Transform OdometryOpticalFlow::computeTransform(
 				// depth
 				for(unsigned int i=0; i<newCorners.size(); ++i)
 				{
-					if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
-					   uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
+					if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
+					   uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
 					{
-						pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
-								data.cx(), data.cy(), data.fx(), data.fy(), true);
+						pcl::PointXYZ pt = util3d::projectDepthTo3D(
+								data.depthRaw(),
+								newCorners[i].x,
+								newCorners[i].y,
+								data.cameraModels()[0].cx(),
+								data.cameraModels()[0].cy(),
+								data.cameraModels()[0].fx(),
+								data.cameraModels()[0].fy(),
+								true);
 						if(pcl::isFinite(pt) &&
 							(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
 						{
-							newCorners3D->at(oi) = util3d::transformPoint(pt, data.localTransform());
+							newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
 							newCornersFiltered[oi] = newCorners[i];
 							++oi;
 						}

corelib/src/OdometryThread.cpp

@@ -97,8 +97,9 @@ void OdometryThread::mainLoop()
 	{
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
-		data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(data, info));
+		// a null pose notify that odometry could not be computed
+		double variance = info.variance>0?info.variance:1;
+		this->post(new OdometryEvent(data, pose, variance, variance, info));
 	}
 }
 
@@ -106,7 +107,7 @@ void OdometryThread::addData(const SensorData & data)
 {
 	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 	{
-		if(data.image().empty() || data.depthOrRightImage().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
+		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
 			ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
 			return;
@@ -114,7 +115,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
-		if(data.image().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
+		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
 			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
 			return;

corelib/src/Rtabmap.cpp

@@ -73,7 +73,7 @@ namespace rtabmap
 
 Rtabmap::Rtabmap() :
 	_publishStats(Parameters::defaultRtabmapPublishStats()),
-	_publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()),
+	_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
 	_publishPdf(Parameters::defaultRtabmapPublishPdf()),
 	_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
 	_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
@@ -372,7 +372,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 
 	Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
-	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
+	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
 	Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
@@ -792,7 +792,10 @@ void Rtabmap::resetMemory()
 //============================================================
 // MAIN LOOP
 //============================================================
-bool Rtabmap::process(const SensorData & data)
+bool Rtabmap::process(
+		const SensorData & data,
+		const Transform & odomPose,
+		const cv::Mat & covariance)
 {
 	UDEBUG("");
 
@@ -863,7 +866,7 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	if(_rgbdSlamMode)
 	{
-		if(data.pose().isNull())
+		if(odomPose.isNull())
 		{
 			UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
 				   "Image %d is ignored!", data.id());
@@ -877,7 +880,7 @@ bool Rtabmap::process(const SensorData & data)
 				const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
 
 				// look for identity
-				if(!lastPose.isIdentity() && data.pose().isIdentity())
+				if(!lastPose.isIdentity() && odomPose.isIdentity())
 				{
 					int mapId = triggerNewMap();
 					UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
@@ -885,7 +888,7 @@ bool Rtabmap::process(const SensorData & data)
 				else if(_newMapOdomChangeDistance > 0.0)
 				{
 					// look for large change
-					Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
+					Transform lastPoseToNewPose = lastPose.inverse() * odomPose;
 					float x,y,z, roll,pitch,yaw;
 					lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
 					if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
@@ -895,7 +898,7 @@ bool Rtabmap::process(const SensorData & data)
 								_newMapOdomChangeDistance,
 								mapId,
 								lastPose.prettyPrint().c_str(),
-								data.pose().prettyPrint().c_str());
+								odomPose.prettyPrint().c_str());
 					}
 				}
 			}
@@ -908,16 +911,14 @@ bool Rtabmap::process(const SensorData & data)
 	ULOGGER_INFO("Updating memory...");
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->update(data, &statistics_))
+		if(!_memory->update(data, odomPose, covariance, &statistics_))
 		{
 			return false;
 		}
 	}
 	else
 	{
-		SensorData dataWithoutOdom = data;
-		dataWithoutOdom.setPose(Transform(), 1, 1);
-		if(!_memory->update(dataWithoutOdom, &statistics_))
+		if(!_memory->update(data, Transform(), cv::Mat(), &statistics_))
 		{
 			return false;
 		}
@@ -929,6 +930,7 @@ bool Rtabmap::process(const SensorData & data)
 	{
 		UFATAL("Not supposed to be here...last signature is null?!?");
 	}
+
 	ULOGGER_INFO("Processing signature %d", signature->id());
 	timeMemoryUpdate = timer.ticks();
 	ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
@@ -980,7 +982,7 @@ bool Rtabmap::process(const SensorData & data)
 		//============================================================
 		if(_poseScanMatching &&
 			signature->getLinks().size() == 1 &&
-			!signature->getLaserScanCompressed().empty() &&
+			!signature->sensorData().laserScanCompressed().empty() &&
 			rehearsedId == 0) // don't do it if rehearsal happened
 		{
 			UINFO("Odometry correction by scan matching");
@@ -1023,13 +1025,13 @@ bool Rtabmap::process(const SensorData & data)
 
 			Link tmp = signature->getLinks().begin()->second.inverse();
 
-			// if the previous signature is a bad signature, remove it from the local graph
+			// if the previous node is an intermediate node, remove it from the local graph
 			if(_constraints.size() &&
 			   _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
 			{
 				const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to());
 				UASSERT(s!=0);
-				if(s->isBadSignature())
+				if(s->getWeight() == -1)
 				{
 					tmp = _constraints.rbegin()->second.merge(tmp);
 					_optimizedPoses.erase(s->id());
@@ -1070,7 +1072,7 @@ bool Rtabmap::process(const SensorData & data)
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance))
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1470,17 +1472,21 @@ bool Rtabmap::process(const SensorData & data)
 					{
 						if(immunizedLocally >= maxLocalLocationsImmunized)
 						{
-							UWARN("Could not immunize the whole local path (%d) between "
-								  "%d and %d (max location immunized=%d). You may want "
-								  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
-								  "to be able to immunize longer paths.",
-									(int)path.size(),
-									nearestId,
-									signature->id(),
-									maxLocalLocationsImmunized,
-									_localImmunizationRatio,
-									maxLocalLocationsImmunized,
-									(int)_memory->getWorkingMem().size());
+							// set 20 to avoid this warning when starting mapping
+							if(maxLocalLocationsImmunized > 20)
+							{
+								UWARN("Could not immunize the whole local path (%d) between "
+									  "%d and %d (max location immunized=%d). You may want "
+									  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
+									  "to be able to immunize longer paths.",
+										(int)path.size(),
+										nearestId,
+										signature->id(),
+										maxLocalLocationsImmunized,
+										_localImmunizationRatio,
+										maxLocalLocationsImmunized,
+										(int)_memory->getWorkingMem().size());
+							}
 							break;
 						}
 						else if(!_memory->isInSTM(iter->first))
@@ -1638,16 +1644,13 @@ bool Rtabmap::process(const SensorData & data)
 				// Add signatures
 				SensorData dataFrom = data;
 				dataFrom.setId(signature->id());
-				Signature tmpTo = _memory->getSignatureData(_loopClosureHypothesis.first, true);
-				SensorData dataTo = tmpTo.toSensorData();
+				SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true);
 				UDEBUG("timeTo = %fs", timeT.ticks());
 
-				if(dataFrom.isValid() &&
-				   dataFrom.isMetric() &&
-				   dataTo.isValid() &&
-				   dataTo.isMetric() &&
+				if(!dataFrom.depthOrRightRaw().empty() &&
+				   !dataTo.depthOrRightRaw().empty() &&
 				   dataFrom.id() != Memory::kIdInvalid &&
-				   tmpTo.id() != Memory::kIdInvalid)
+				   dataTo.id() != Memory::kIdInvalid)
 				{
 					memory.update(dataTo);
 					UDEBUG("timeUpTo = %fs", timeT.ticks());
@@ -1683,7 +1686,7 @@ bool Rtabmap::process(const SensorData & data)
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance);
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
 		}
 
 		if(rejectedHypothesis)
@@ -1797,16 +1800,13 @@ bool Rtabmap::process(const SensorData & data)
 							// Add signatures
 							SensorData dataFrom = data;
 							dataFrom.setId(signature->id());
-							Signature tmpTo = _memory->getSignatureData(nearestId, true);
-							SensorData dataTo = tmpTo.toSensorData();
+							SensorData dataTo = _memory->getNodeData(nearestId, true);
 							UDEBUG("timeTo = %fs", timeT.ticks());
 
-							if(dataFrom.isValid() &&
-							   dataFrom.isMetric() &&
-							   dataTo.isValid() &&
-							   dataTo.isMetric() &&
+							if(!dataFrom.depthOrRightRaw().empty() &&
+							   !dataTo.depthOrRightRaw().empty() &&
 							   dataFrom.id() != Memory::kIdInvalid &&
-							   tmpTo.id() != Memory::kIdInvalid)
+							   dataTo.id() != Memory::kIdInvalid)
 							{
 								memory.update(dataTo);
 								UDEBUG("timeUpTo = %fs", timeT.ticks());
@@ -1838,7 +1838,7 @@ bool Rtabmap::process(const SensorData & data)
 									signature->id(),
 									nearestId,
 									transform.prettyPrint().c_str());
-							_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
+							_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
 
 							if(_loopClosureHypothesis.first == 0)
 							{
@@ -1856,7 +1856,7 @@ bool Rtabmap::process(const SensorData & data)
 				//
 				// 2) compare locally with nearest locations by scan matching
 				//
-				if( !signature->getLaserScanCompressed().empty() &&
+				if( !signature->sensorData().laserScanCompressed().empty() &&
 					(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
 				{
 					// In localization mode, no need to check local loop
@@ -1927,7 +1927,7 @@ bool Rtabmap::process(const SensorData & data)
 												nearestId,
 												transform.prettyPrint().c_str());
 										// set Identify covariance for laser scan matching only
-										_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1);
+										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
 
 										++localSpaceClosuresAddedByICPOnly;
 
@@ -1967,6 +1967,7 @@ bool Rtabmap::process(const SensorData & data)
 			UINFO("Update map correction: SLAM mode");
 			// SLAM mode!
 			optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
+			UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
 
 			// Update map correction, it should be identify when optimizing from the last node
 			_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
@@ -2015,7 +2016,7 @@ bool Rtabmap::process(const SensorData & data)
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
 			if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
 			{
-				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100); // set high variance
+				_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance
 			}
 		}
 	}
@@ -2085,44 +2086,6 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
 
-			// Set local graph
-			if(!_rgbdSlamMode)
-			{
-				// no optimization on appearance-only mode, create a local graph
-				std::map<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
-				std::map<int, Transform> poses;
-				std::map<int, int> mapIds;
-				std::map<int, std::string> labels;
-				std::map<int, double> stamps;
-				std::map<int, std::vector<unsigned char> > userDatas;
-				std::multimap<int, Link> constraints;
-				_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
-				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					Transform odomPose;
-					int weight = -1;
-					int mapId = -1;
-					std::string label;
-					double stamp = 0;
-					std::vector<unsigned char> userData;
-					_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
-					mapIds.insert(std::make_pair(iter->first, mapId));
-					labels.insert(std::make_pair(iter->first, label));
-					stamps.insert(std::make_pair(iter->first, stamp));
-					userDatas.insert(std::make_pair(iter->first, userData));
-				}
-				statistics_.setPoses(poses);
-				statistics_.setConstraints(constraints);
-				statistics_.setMapIds(mapIds);
-				statistics_.setLabels(labels);
-				statistics_.setStamps(stamps);
-				statistics_.setUserDatas(userDatas);
-			}
-			else // RGBD-SLAM mode
-			{
-				//see after transfer below
-			}
-
 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
 			statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
@@ -2146,11 +2109,6 @@ bool Rtabmap::process(const SensorData & data)
 			//Epipolar geometry constraint
 			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 
-			if(_publishLastSignature)
-			{
-				statistics_.setSignature(*signature);
-			}
-
 			if(_publishLikelihood || _publishPdf)
 			{
 				// Child count by parent signature on the root of the memory ... for statistics
@@ -2178,6 +2136,12 @@ bool Rtabmap::process(const SensorData & data)
 		ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
 	}
 
+	Signature lastSignatureData(signature->id());
+	if(_publishLastSignatureData)
+	{
+		lastSignatureData = *signature;
+	}
+
 	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
 	//This will also remove rehearsed signatures
 	std::list<int> signaturesRemoved = _memory->cleanup();
@@ -2206,11 +2170,13 @@ bool Rtabmap::process(const SensorData & data)
 		_memory->deleteLocation(signature->id());
 	}
 
-	timeMemoryCleanup = timer.ticks();
-	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
-
 	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;
+
+	timeMemoryCleanup = timer.ticks();
+	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
+
+
 
 	//============================================================
 	// TRANSFER
@@ -2275,6 +2241,7 @@ bool Rtabmap::process(const SensorData & data)
 	//==============================================================
 	// Finalize statistics and log files
 	//==============================================================
+	int localGraphSize = 0;
 	if(_publishStats)
 	{
 		statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
@@ -2293,36 +2260,49 @@ bool Rtabmap::process(const SensorData & data)
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
 		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
-		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size());
 
-		if(_rgbdSlamMode)
+		std::map<int, Signature> signatures;
+		if(_publishLastSignatureData)
 		{
-			std::map<int, int> mapIds;
-			std::map<int, std::string> labels;
-			std::map<int, double> stamps;
-			std::map<int, std::vector<unsigned char> > userDatas;
-			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-			{
-				Transform odomPose;
-				int weight = -1;
-				int mapId = -1;
-				std::string label;
-				double stamp = 0;
-				std::vector<unsigned char> userData;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
-				mapIds.insert(std::make_pair(iter->first, mapId));
-				labels.insert(std::make_pair(iter->first, label));
-				stamps.insert(std::make_pair(iter->first, stamp));
-				userDatas.insert(std::make_pair(iter->first, userData));
-			}
-			statistics_.setPoses(_optimizedPoses);
-			statistics_.setConstraints(_constraints);
-			statistics_.setMapIds(mapIds);
-			statistics_.setLabels(labels);
-			statistics_.setStamps(stamps);
-			statistics_.setUserDatas(userDatas);
+			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
 		}
-
+		// Set local graph
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+		if(!_rgbdSlamMode)
+		{
+			// no optimization on appearance-only mode, create a local graph
+			std::map<int, int> ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
+		}
+		else // RGBD-SLAM mode
+		{
+			poses = _optimizedPoses;
+			constraints = _constraints;
+		}
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			Transform odomPose;
+			int weight = -1;
+			int mapId = -1;
+			std::string label;
+			double stamp = 0;
+			std::vector<unsigned char> userData;
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
+			signatures.insert(std::make_pair(iter->first,
+					Signature(iter->first,
+							mapId,
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData)));
+		}
+		statistics_.setPoses(poses);
+		statistics_.setConstraints(constraints);
+		statistics_.setSignatures(signatures);
+		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
+		localGraphSize = poses.size();
 	}
 
 	//Start trashing
@@ -2359,7 +2339,7 @@ bool Rtabmap::process(const SensorData & data)
 									timeLocalTimeDetection,
 									timeLocalSpaceDetection,
 									timeMapOptimization);
-		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
+		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
 									_loopClosureHypothesis.first,
 									_highestHypothesis.first,
 									(int)signaturesRemoved.size(),
@@ -2374,9 +2354,11 @@ bool Rtabmap::process(const SensorData & data)
 									lcHypothesisReactivated,
 									refUniqueWordsCount,
 									retrievalId,
-									0.0f,
+									0,
 									rehearsalMaxId,
-									rehearsalMaxId>0?1:0);
+									rehearsalMaxId>0?1:0,
+									localGraphSize,
+									data.id());
 		if(_statisticLogsBufferedInRAM)
 		{
 			_bufferedLogsF.push_back(logF);
@@ -2403,7 +2385,7 @@ bool Rtabmap::process(const SensorData & data)
 
 bool Rtabmap::process(const cv::Mat & image, int id)
 {
-	return this->process(SensorData(image, id));
+	return this->process(SensorData(image, id), Transform());
 }
 
 // SETTERS
@@ -2838,13 +2820,10 @@ void Rtabmap::dumpPrediction() const
 	}
 }
 
-void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
+void Rtabmap::get3DMap(
+		std::map<int, Signature> & signatures,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
 		bool global) const
 {
@@ -2870,22 +2849,6 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-		{
-			Transform odomPose;
-			int weight = -1;
-			int mapId = -1;
-			std::string label;
-			double stamp = 0;
-			std::vector<unsigned char> userData;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
-			mapIds.insert(std::make_pair(iter->first, mapId));
-			stamps.insert(std::make_pair(iter->first, stamp));
-			labels.insert(std::make_pair(iter->first, label));
-			userDatas.insert(std::make_pair(iter->first, userData));
-		}
-
-
 		// Get data
 		std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
 
@@ -2900,11 +2863,24 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			Signature data = _memory->getSignatureData(*iter);
-			if(data.id() != Memory::kIdInvalid)
-			{
-				signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
-			}
+			Transform odomPose;
+			int weight = -1;
+			int mapId = -1;
+			std::string label;
+			double stamp = 0;
+			std::vector<unsigned char> userData;
+			_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, userData, true);
+			SensorData data = _memory->getNodeData(*iter);
+			data.setId(*iter);
+			signatures.insert(std::make_pair(*iter,
+					Signature(*iter,
+							mapId,
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData,
+							data)));
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -2920,13 +2896,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
-		bool global,
-		bool posesConstraintsOnly)
+		bool global,
+		std::map<int, Signature> * signatures)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -2948,8 +2920,8 @@ void Rtabmap::getGraph(
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
-
-		if(!posesConstraintsOnly)
+
+		if(signatures)
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -2958,12 +2930,16 @@ void Rtabmap::getGraph(
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
-				std::vector<unsigned char> userData;
+				std::vector<unsigned char> userData;
 				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
-				mapIds.insert(std::make_pair(iter->first, mapId));
-				stamps.insert(std::make_pair(iter->first, stamp));
-				labels.insert(std::make_pair(iter->first, label));
-				userDatas.insert(std::make_pair(iter->first, userData));
+				signatures->insert(std::make_pair(iter->first,
+						Signature(iter->first,
+							mapId,
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData)));
 			}
 		}
 	}
@@ -3115,12 +3091,8 @@ bool Rtabmap::computePath(int targetNode, bool global)
 	UTimer totalTimer;
 	UTimer timer;
 	std::map<int, Transform> nodes;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-	this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global, true);
+	std::multimap<int, Link> constraints;
+	this->getGraph(nodes, constraints, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	if(computePath(targetNode, nodes, constraints))
@@ -3153,8 +3125,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	std::map<int, int> mapIds;
 	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-	this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global, true);
+	std::map<int, std::vector<unsigned char> > userDatas;
+	this->getGraph(nodes, constraints, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
@@ -3306,7 +3278,7 @@ void Rtabmap::updateGoalIndex()
 						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 						{
 							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1); // on the optimized path, set Identity variance
+							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance
 							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 						}
 					}

corelib/src/RtabmapThread.cpp

@@ -130,20 +130,13 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	_rtabmap->get3DMap(signatures,
 			poses,
 			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas,
 			optimized,
 			full);
 
-	this->post(new RtabmapEvent3DMap(signatures,
+	this->post(new RtabmapEvent3DMap(
+			signatures,
 			poses,
-			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas));
+			constraints));
 }
 
 void RtabmapThread::publishGraph(bool optimized, bool full) const
@@ -158,20 +151,14 @@ void RtabmapThread::publishGraph(bool optimized, bool full) const
 
 	_rtabmap->getGraph(poses,
 			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas,
 			optimized,
-			full);
+			full,
+			&signatures);
 
-	this->post(new RtabmapEvent3DMap(signatures,
+	this->post(new RtabmapEvent3DMap(
+			signatures,
 			poses,
-			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas));
+			constraints));
 }
 
 
@@ -314,16 +301,16 @@ void RtabmapThread::handleEvent(UEvent* event)
 		CameraEvent * e = (CameraEvent*)event;
 		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
 		{
-			this->addData(e->data());
+			this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
 		}
 	}
 	else if(event->getClassName().compare("OdometryEvent") == 0)
 	{
 		UDEBUG("OdometryEvent");
 		OdometryEvent * e = (OdometryEvent*)event;
-		if(e->isValid())
+		if(!e->pose().isNull())
 		{
-			this->addData(e->data());
+			this->addData(*e);
 		}
 		else
 		{
@@ -522,12 +509,12 @@ void RtabmapThread::handleEvent(UEvent* event)
 //============================================================
 void RtabmapThread::process()
 {
-	SensorData data;
+	OdometryEvent data;
 	if(_state.empty() && getData(data))
 	{
 		if(_rtabmap->getMemory())
 		{
-			if(_rtabmap->process(data))
+			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
@@ -542,16 +529,10 @@ void RtabmapThread::process()
 	}
 }
 
-void RtabmapThread::addData(const SensorData & sensorData)
+void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
-		if(!sensorData.isValid())
-		{
-			ULOGGER_ERROR("data not valid !?");
-			return;
-		}
-
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
@@ -559,9 +540,8 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			{
 				ignoreFrame = true;
 			}
-
 		}
-		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity())
+		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity())
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
@@ -578,48 +558,45 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			_frameRateTimer->start();
 		}
 
-		lastPose_ = sensorData.pose();
-		if(sensorData.poseRotVariance() > _rotVariance)
+		lastPose_ = odomEvent.pose();
+		double maxRotVar = odomEvent.rotVariance();
+		double maxTransVar = odomEvent.transVariance();
+		if(maxRotVar > _rotVariance)
 		{
-			_rotVariance = sensorData.poseRotVariance();
+			_rotVariance = maxRotVar;
 		}
-		if(sensorData.poseTransVariance() > _transVariance)
+		if(maxTransVar > _transVariance)
 		{
-			_transVariance = sensorData.poseTransVariance();
+			_transVariance = maxTransVar;
 		}
 
 		bool notify = true;
 		_dataMutex.lock();
 		{
+			if(_rotVariance <= 0)
+			{
+				_rotVariance = 1.0;
+			}
+			if(_transVariance <= 0)
+			{
+				_transVariance = 1.0;
+			}
 			if(ignoreFrame)
 			{
 				// remove data from the frame, keeping only constraints
 				SensorData tmp(
 						cv::Mat(),
-						cv::Mat(),
-						0,0,0,0,
-						sensorData.localTransform(),
-						sensorData.pose(),
-						sensorData.poseRotVariance(),
-						sensorData.poseTransVariance(),
-						sensorData.id(),
-						sensorData.stamp(),
-						sensorData.userData());
-				_dataBuffer.push_back(tmp);
+						odomEvent.data().id(),
+						odomEvent.data().stamp(),
+						odomEvent.data().userData());
+				_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
 			}
 			else
 			{
-				_dataBuffer.push_back(sensorData);
+				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
 			}
-			if(_rotVariance <= 0)
-			{
-				_rotVariance = 1.0f;
-			}
-			if(_transVariance <= 0)
-			{
-				_transVariance = 1.0f;
-			}
-			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
+			UDEBUG("Added data %d", odomEvent.data().id());
+
 			_rotVariance = 0;
 			_transVariance = 0;
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
@@ -638,7 +615,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
 	}
 }
 
-bool RtabmapThread::getData(SensorData & image)
+bool RtabmapThread::getData(OdometryEvent & data)
 {
 	ULOGGER_DEBUG("");
 
@@ -651,7 +628,7 @@ bool RtabmapThread::getData(SensorData & image)
 	{
 		if(!_dataBuffer.empty())
 		{
-			image = _dataBuffer.front();
+			data = _dataBuffer.front();
 			_dataBuffer.pop_front();
 			dataFilled = true;
 		}

corelib/src/SensorData.cpp

@@ -27,138 +27,430 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/ULogger.h"
 #include <rtabmap/utilite/UMath.h>
 
 namespace rtabmap
 {
 
-/**
- * An id is automatically generated if id=0.
- */
+// empty constructor
 SensorData::SensorData() :
-	_id(0),
-	_stamp(0.0),
-	_fx(0.0f),
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity()),
-	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f),
-	_laserScanMaxPts(0)
+		_id(0),
+		_stamp(0.0),
+		_laserScanMaxPts(0)
 {
 }
 
-SensorData::SensorData(const cv::Mat & image,
-	  int id,
-	  double stamp,
-	  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_fx(0.0f),
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity()),
-	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f),
-	_laserScanMaxPts(0),
-	_userData(userData)
+// Appearance-only constructor
+SensorData::SensorData(
+		const cv::Mat & image,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_userData(userData)
 {
-	UASSERT(image.empty() ||
-			image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
+	if(image.rows == 1)
+	{
+		UASSERT(image.type() == CV_8UC1); // Bytes
+		_imageCompressed = image;
+	}
+	else if(!image.empty())
+	{
+		UASSERT(image.type() == CV_8UC1 || // Mono
+				image.type() == CV_8UC3);  // RGB
+		_imageRaw = image;
+	}
 }
 
-	// Metric constructor
-SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_depthOrRightImage(depthOrRightImage),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform),
-	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance),
-	_laserScanMaxPts(0),
-	_userData(userData)
+// Mono constructor
+SensorData::SensorData(
+		const cv::Mat & image,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_userData(userData)
 {
-	UASSERT(image.empty() ||
-			image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.empty() ||
-			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!_localTransform.isNull());
-	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+	if(image.rows == 1)
+	{
+		UASSERT(image.type() == CV_8UC1); // Bytes
+		_imageCompressed = image;
+	}
+	else if(!image.empty())
+	{
+		UASSERT(image.type() == CV_8UC1 || // Mono
+				image.type() == CV_8UC3);  // RGB
+		_imageRaw = image;
+	}
 }
 
-	// Metric constructor + 2d depth
-SensorData::SensorData(const cv::Mat & laserScan,
-		  int laserScanMaxPts,
-		  const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_depthOrRightImage(depthOrRightImage),
-	_laserScan(laserScan),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform),
-	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance),
-	_laserScanMaxPts(laserScanMaxPts),
-	_userData(userData)
+// RGB-D constructor
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_userData(userData)
 {
-	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
-	UASSERT(image.empty() ||
-			image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.empty() ||
-			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!_localTransform.isNull());
-	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
 }
 
-bool SensorData::empty() const
+// RGB-D constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_userData(userData)
 {
-	return _image.empty();
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+}
+
+// Multi-cameras RGB-D constructor
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_cameraModels(cameraModels),
+		_userData(userData)
+{
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+	for(unsigned int i=0; i<cameraModels.size(); ++i)
+	{
+		UASSERT(cameraModels[i].isValid());
+	}
+}
+
+// Multi-cameras RGB-D constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_cameraModels(cameraModels),
+		_userData(userData)
+{
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+
+	for(unsigned int i=0; i<cameraModels.size(); ++i)
+	{
+		UASSERT(cameraModels[i].isValid());
+	}
+}
+
+// Stereo constructor
+SensorData::SensorData(
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const StereoCameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData):
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_stereoCameraModel(cameraModel),
+		_userData(userData)
+{
+	if(left.rows == 1)
+	{
+		UASSERT(left.type() == CV_8UC1); // Bytes
+		_imageCompressed = left;
+	}
+	else if(!left.empty())
+	{
+		UASSERT(left.type() == CV_8UC1 || // Mono
+				left.type() == CV_8UC3);  // RGB
+		_imageRaw = left;
+	}
+	if(right.rows == 1)
+	{
+		UASSERT(right.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = right;
+	}
+	else if(!right.empty())
+	{
+		UASSERT(right.type() == CV_8UC1); // Mono
+		_depthOrRightRaw = right;
+	}
+
+}
+
+// Stereo constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const StereoCameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_stereoCameraModel(cameraModel),
+		_userData(userData)
+{
+	if(left.rows == 1)
+	{
+		UASSERT(left.type() == CV_8UC1); // Bytes
+		_imageCompressed = left;
+	}
+	else if(!left.empty())
+	{
+		UASSERT(left.type() == CV_8UC1 || // Mono
+				left.type() == CV_8UC3);  // RGB
+		_imageRaw = left;
+	}
+	if(right.rows == 1)
+	{
+		UASSERT(right.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = right;
+	}
+	else if(!right.empty())
+	{
+		UASSERT(right.type() == CV_8UC1); // Mono
+		_depthOrRightRaw = right;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+}
+
+void SensorData::uncompressData()
+{
+	uncompressData(_imageCompressed.empty()?0:&_imageRaw,
+				_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
+				_laserScanCompressed.empty()?0:&_laserScanRaw);
+}
+
+void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
+{
+	uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
+	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
+	{
+		_imageRaw = *imageRaw;
+	}
+	if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
+	{
+		_depthOrRightRaw = *depthRaw;
+	}
+	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
+	{
+		_laserScanRaw = *laserScanRaw;
+	}
+}
+
+void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
+{
+	if(imageRaw)
+	{
+		*imageRaw = _imageRaw;
+	}
+	if(depthRaw)
+	{
+		*depthRaw = _depthOrRightRaw;
+	}
+	if(laserScanRaw)
+	{
+		*laserScanRaw = _laserScanRaw;
+	}
+	if( (imageRaw && imageRaw->empty()) ||
+		(depthRaw && depthRaw->empty()) ||
+		(laserScanRaw && laserScanRaw->empty()))
+	{
+		rtabmap::CompressionThread ctImage(_imageCompressed, true);
+		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
+		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
+		if(imageRaw && imageRaw->empty())
+		{
+			ctImage.start();
+		}
+		if(depthRaw && depthRaw->empty())
+		{
+			ctDepth.start();
+		}
+		if(laserScanRaw && laserScanRaw->empty())
+		{
+			ctLaserScan.start();
+		}
+		ctImage.join();
+		ctDepth.join();
+		ctLaserScan.join();
+		if(imageRaw && imageRaw->empty())
+		{
+			*imageRaw = ctImage.getUncompressedData();
+			if(imageRaw->empty())
+			{
+				UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id());
+			}
+		}
+		if(depthRaw && depthRaw->empty())
+		{
+			*depthRaw = ctDepth.getUncompressedData();
+			if(depthRaw->empty())
+			{
+				UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id());
+			}
+		}
+		if(laserScanRaw && laserScanRaw->empty())
+		{
+			*laserScanRaw = ctLaserScan.getUncompressedData();
+
+			if(laserScanRaw->empty())
+			{
+				UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
+			}
+		}
+	}
 }
 
 } // namespace rtabmap

corelib/src/Signature.cpp

@@ -39,17 +39,11 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
-	_stamp(0.0),
-	_weight(-1),
+	_weight(0),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_enabled(false),
-	_fx(0.0f),
-	_fy(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_laserScanMaxPts(0)
+	_enabled(false)
 {
 }
 
@@ -59,19 +53,9 @@ Signature::Signature(
 		int weight,
 		double stamp,
 		const std::string & label,
-		const std::multimap<int, cv::KeyPoint> & words,
-		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
 		const std::vector<unsigned char> & userData,
-		const cv::Mat & laserScanCompressed, // in base_link frame
-		const cv::Mat & imageCompressed, // in camera_link frame
-		const cv::Mat & depthCompressed, // in camera_link frame
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
-		int laserScanMaxPts) :
+		const SensorData & sensorData):
 	_id(id),
 	_mapId(mapId),
 	_stamp(stamp),
@@ -81,20 +65,15 @@ Signature::Signature(
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_words(words),
 	_enabled(false),
-	_imageCompressed(imageCompressed),
-	_depthCompressed(depthCompressed),
-	_laserScanCompressed(laserScanCompressed),
-	_fx(fx),
-	_fy(fy),
-	_cx(cx),
-	_cy(cy),
 	_pose(pose),
-	_localTransform(localTransform),
-	_words3(words3),
-	_laserScanMaxPts(laserScanMaxPts)
+	_sensorData(sensorData)
 {
+	if(_sensorData.id() == 0)
+	{
+		_sensorData.setId(id);
+	}
+	UASSERT(_sensorData.id() == _id);
 }
 
 Signature::~Signature()
@@ -239,25 +218,9 @@ void Signature::removeWord(int wordId)
 	_words3.erase(wordId);
 }
 
-void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy)
+cv::Mat Signature::getPoseCovariance() const
 {
-	UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
-	_depthCompressed = bytes;
-	_fx=fx;
-	_fy=fy;
-	_cx=cx;
-	_cy=cy;
-}
-
-float Signature::getDepthFx() const {return getFx();}
-float Signature::getDepthFy() const {return getFy();}
-float Signature::getDepthCx() const {return getCx();}
-float Signature::getDepthCy() const {return getCy();}
-
-void Signature::getPoseVariance(float & rotVariance, float & transVariance) const
-{
-	rotVariance = 1.0f;
-	transVariance = 1.0f;
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	if(_links.size())
 	{
 		for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -267,110 +230,13 @@ void Signature::getPoseVariance(float & rotVariance, float & transVariance) cons
 				//Assume the first neighbor to be the backward neighbor link
 				if(iter->second.to() < iter->second.from())
 				{
-					rotVariance = iter->second.rotVariance();
-					transVariance = iter->second.transVariance();
+					covariance = iter->second.infMatrix().inv();
 					break;
 				}
 			}
 		}
 	}
-}
-
-SensorData Signature::toSensorData()
-{
-	this->uncompressData();
-	float rotVariance = 1.0f;
-	float transVariance = 1.0f;
-	this->getPoseVariance(rotVariance, transVariance);
-
-	return SensorData(_laserScanRaw,
-			_laserScanMaxPts,
-			_imageRaw,
-			_depthRaw,
-			_fx,
-			_fy,
-			_cx,
-			_cy,
-			_localTransform,
-			_pose,
-			rotVariance,
-			transVariance,
-			_id,
-			_stamp,
-			_userData);
-}
-
-void Signature::uncompressData()
-{
-	uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
-}
-
-void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
-{
-	uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
-	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
-	{
-		_imageRaw = *imageRaw;
-	}
-	if(depthRaw && !depthRaw->empty() && _depthRaw.empty())
-	{
-		_depthRaw = *depthRaw;
-	}
-	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
-	{
-		_laserScanRaw = *laserScanRaw;
-	}
-}
-
-void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
-{
-	if(imageRaw)
-	{
-		*imageRaw = _imageRaw;
-	}
-	if(depthRaw)
-	{
-		*depthRaw = _depthRaw;
-	}
-	if(laserScanRaw)
-	{
-		*laserScanRaw = _laserScanRaw;
-	}
-	if( (imageRaw && imageRaw->empty()) ||
-		(depthRaw && depthRaw->empty()) ||
-		(laserScanRaw && laserScanRaw->empty()))
-	{
-		rtabmap::CompressionThread ctImage(_imageCompressed, true);
-		rtabmap::CompressionThread ctDepth(_depthCompressed, true);
-		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
-		if(imageRaw && imageRaw->empty())
-		{
-			ctImage.start();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			ctDepth.start();
-		}
-		if(laserScanRaw && laserScanRaw->empty())
-		{
-			ctLaserScan.start();
-		}
-		ctImage.join();
-		ctDepth.join();
-		ctLaserScan.join();
-		if(imageRaw && imageRaw->empty())
-		{
-			*imageRaw = ctImage.getUncompressedData();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			*depthRaw = ctDepth.getUncompressedData();
-		}
-		if(laserScanRaw && laserScanRaw->empty())
-		{
-			*laserScanRaw = ctLaserScan.getUncompressedData();
-		}
-	}
+	return covariance;
 }
 
 } //namespace rtabmap

corelib/src/Transform.cpp

@@ -31,44 +31,33 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
 #include <iomanip>
 
 namespace rtabmap {
 
-Transform::Transform() : data_(12)
+Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1))
 {
-	data_[0] = 0.0f;
-	data_[1] = 0.0f;
-	data_[2] = 0.0f;
-	data_[3] = 0.0f;
-	data_[4] = 0.0f;
-	data_[5] = 0.0f;
-	data_[6] = 0.0f;
-	data_[7] = 0.0f;
-	data_[8] = 0.0f;
-	data_[9] = 0.0f;
-	data_[10] = 0.0f;
-	data_[11] = 0.0f;
 }
 
 // rotation matrix r## and origin o##
-Transform::Transform(float r11, float r12, float r13, float o14,
-				     float r21, float r22, float r23, float o24,
-				     float r31, float r32, float r33, float o34) :
-	data_(12)
+Transform::Transform(
+		float r11, float r12, float r13, float o14,
+		float r21, float r22, float r23, float o24,
+		float r31, float r32, float r33, float o34)
 {
-	data_[0] = r11;
-	data_[1] = r12;
-	data_[2] = r13;
-	data_[3] = o14;
-	data_[4] = r21;
-	data_[5] = r22;
-	data_[6] = r23;
-	data_[7] = o24;
-	data_[8] = r31;
-	data_[9] = r32;
-	data_[10] = r33;
-	data_[11] = o34;
+	data_ = (cv::Mat_<float>(3,4) <<
+			r11, r12, r13, o14,
+			r21, r22, r23, o24,
+			r31, r32, r33, o34);
+}
+
+Transform::Transform(const cv::Mat & transformationMatrix)
+{
+	UASSERT(transformationMatrix.cols == 4 &&
+			transformationMatrix.rows == 3 &&
+			transformationMatrix.type() == CV_32FC1);
+	data_ = transformationMatrix;
 }
 
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
@@ -79,46 +68,46 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 
 bool Transform::isNull() const
 {
-	return (data_[0] == 0.0f &&
-			data_[1] == 0.0f &&
-			data_[2] == 0.0f &&
-			data_[3] == 0.0f &&
-			data_[4] == 0.0f &&
-			data_[5] == 0.0f &&
-			data_[6] == 0.0f &&
-			data_[7] == 0.0f &&
-			data_[8] == 0.0f &&
-			data_[9] == 0.0f &&
-			data_[10] == 0.0f &&
-			data_[11] == 0.0f) ||
-			uIsNan(data_[0]) ||
-			uIsNan(data_[1]) ||
-			uIsNan(data_[2]) ||
-			uIsNan(data_[3]) ||
-			uIsNan(data_[4]) ||
-			uIsNan(data_[5]) ||
-			uIsNan(data_[6]) ||
-			uIsNan(data_[7]) ||
-			uIsNan(data_[8]) ||
-			uIsNan(data_[9]) ||
-			uIsNan(data_[10]) ||
-			uIsNan(data_[11]);
+	return (data()[0] == 0.0f &&
+			data()[1] == 0.0f &&
+			data()[2] == 0.0f &&
+			data()[3] == 0.0f &&
+			data()[4] == 0.0f &&
+			data()[5] == 0.0f &&
+			data()[6] == 0.0f &&
+			data()[7] == 0.0f &&
+			data()[8] == 0.0f &&
+			data()[9] == 0.0f &&
+			data()[10] == 0.0f &&
+			data()[11] == 0.0f) ||
+			uIsNan(data()[0]) ||
+			uIsNan(data()[1]) ||
+			uIsNan(data()[2]) ||
+			uIsNan(data()[3]) ||
+			uIsNan(data()[4]) ||
+			uIsNan(data()[5]) ||
+			uIsNan(data()[6]) ||
+			uIsNan(data()[7]) ||
+			uIsNan(data()[8]) ||
+			uIsNan(data()[9]) ||
+			uIsNan(data()[10]) ||
+			uIsNan(data()[11]);
 }
 
 bool Transform::isIdentity() const
 {
-	return data_[0] == 1.0f &&
-			data_[1] == 0.0f &&
-			data_[2] == 0.0f &&
-			data_[3] == 0.0f &&
-			data_[4] == 0.0f &&
-			data_[5] == 1.0f &&
-			data_[6] == 0.0f &&
-			data_[7] == 0.0f &&
-			data_[8] == 0.0f &&
-			data_[9] == 0.0f &&
-			data_[10] == 1.0f &&
-			data_[11] == 0.0f;
+	return data()[0] == 1.0f &&
+			data()[1] == 0.0f &&
+			data()[2] == 0.0f &&
+			data()[3] == 0.0f &&
+			data()[4] == 0.0f &&
+			data()[5] == 1.0f &&
+			data()[6] == 0.0f &&
+			data()[7] == 0.0f &&
+			data()[8] == 0.0f &&
+			data()[9] == 0.0f &&
+			data()[10] == 1.0f &&
+			data()[11] == 0.0f;
 }
 
 void Transform::setNull()
@@ -145,16 +134,17 @@ Transform Transform::inverse() const
 
 Transform Transform::rotation() const
 {
-	return Transform(data_[0], data_[1], data_[2], 0,
-					 data_[4], data_[5], data_[6], 0,
-					 data_[8], data_[9], data_[10], 0);
+	return Transform(
+			data()[0], data()[1], data()[2], 0,
+			data()[4], data()[5], data()[6], 0,
+			data()[8], data()[9], data()[10], 0);
 }
 
 Transform Transform::translation() const
 {
-	return Transform(1,0,0, data_[3],
-					 0,1,0, data_[7],
-					 0,0,1, data_[11]);
+	return Transform(1,0,0, data()[3],
+					 0,1,0, data()[7],
+					 0,0,1, data()[11]);
 }
 
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
@@ -215,7 +205,7 @@ Transform & Transform::operator*=(const Transform & t)
 
 bool Transform::operator==(const Transform & t) const
 {
-	return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
+	return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0;
 }
 
 bool Transform::operator!=(const Transform & t) const
@@ -239,18 +229,18 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 Eigen::Matrix4f Transform::toEigen4f() const
 {
 	Eigen::Matrix4f m;
-	m << data_[0], data_[1], data_[2], data_[3],
-		 data_[4], data_[5], data_[6], data_[7],
-		 data_[8], data_[9], data_[10], data_[11],
+	m << data()[0], data()[1], data()[2], data()[3],
+		 data()[4], data()[5], data()[6], data()[7],
+		 data()[8], data()[9], data()[10], data()[11],
 		 0,0,0,1;
 	return m;
 }
 Eigen::Matrix4d Transform::toEigen4d() const
 {
 	Eigen::Matrix4d m;
-	m << data_[0], data_[1], data_[2], data_[3],
-		 data_[4], data_[5], data_[6], data_[7],
-		 data_[8], data_[9], data_[10], data_[11],
+	m << data()[0], data()[1], data()[2], data()[3],
+		 data()[4], data()[5], data()[6], data()[7],
+		 data()[8], data()[9], data()[10], data()[11],
 		 0,0,0,1;
 	return m;
 }

corelib/src/resources/DatabaseSchema.sql.in

@@ -25,24 +25,13 @@ CREATE TABLE Node (
 	PRIMARY KEY (id)
 );
 
-CREATE TABLE Image (
+CREATE TABLE Data (
 	id INTEGER NOT NULL,
-	data BLOB,       -- compressed image (RGB)
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
--- TODO: Merge "Image" and "Depth" tables to "Data" table.
-CREATE TABLE Depth (
-	id INTEGER NOT NULL,
-	data BLOB,       -- compressed image (Depth or Right image)
-	fx FLOAT,
-	fy FLOAT,        -- baseline if stereo
-	cx FLOAT,
-	cy FLOAT,
-	local_transform BLOB,
-	data2d BLOB,     -- compressed data (Laser scan)
-	data2d_max_pts INTEGER,   -- Laser scan max points
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy [,baseline] local_transform
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_max_pts INTEGER,     -- Laser scan max points
 	time_enter DATE,
 	PRIMARY KEY (id)
 );

corelib/src/util3d.cpp

@@ -27,10 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap
@@ -494,6 +496,383 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 			decimation);
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float voxelSize,
+		int samples)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+	{
+		//depth
+		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
+		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
+		cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+		{
+			if(sensorData.cameraModels()[i].isValid())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
+						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i].cy(),
+						sensorData.cameraModels()[i].fx(),
+						sensorData.cameraModels()[i].fy(),
+						decimation);
+
+				if(tmp->size())
+				{
+					bool filtered = false;
+					if(tmp->size() && maxDepth)
+					{
+						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+						filtered = true;
+					}
+
+					if(tmp->size() && voxelSize)
+					{
+						tmp = util3d::voxelize(tmp, voxelSize);
+						filtered = true;
+					}
+
+					if(tmp->size() && samples)
+					{
+						tmp = util3d::sampling(tmp, samples);
+						filtered = true;
+					}
+
+					if(tmp->size() && !filtered)
+					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
+					}
+
+					if(tmp->size())
+					{
+						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+					}
+
+					*cloud += *tmp;
+				}
+			}
+			else
+			{
+				UERROR("Camera model %d is invalid", i);
+			}
+		}
+
+		if(cloud->size() && voxelSize)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize);
+		}
+	}
+	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+	{
+		//stereo
+		UASSERT(sensorData.rightRaw().type() == CV_8UC1);
+
+		cv::Mat leftMono;
+		if(sensorData.imageRaw().channels() == 3)
+		{
+			cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftMono = sensorData.imageRaw();
+		}
+		cloud = cloudFromDisparity(
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				sensorData.stereoCameraModel().left().cx(),
+				sensorData.stereoCameraModel().left().cy(),
+				sensorData.stereoCameraModel().left().fx(),
+				sensorData.stereoCameraModel().baseline(),
+				decimation);
+
+		if(cloud->size())
+		{
+			bool filtered = false;
+			if(cloud->size() && maxDepth)
+			{
+				cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+				filtered = true;
+			}
+
+			if(cloud->size() && voxelSize)
+			{
+				cloud = util3d::voxelize(cloud, voxelSize);
+				filtered = true;
+			}
+
+			if(cloud->size() && !filtered)
+			{
+				cloud = util3d::removeNaNFromPointCloud(cloud);
+			}
+
+			if(cloud->size())
+			{
+				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float voxelSize,
+		int samples)
+{
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+
+	if(!sensorData.imageRaw().empty())
+	{
+		if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+		{
+			//depth
+			UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
+			UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
+			int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
+			cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+			{
+				if(sensorData.cameraModels()[i].isValid())
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
+							cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
+							cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+							sensorData.cameraModels()[i].cx(),
+							sensorData.cameraModels()[i].cy(),
+							sensorData.cameraModels()[i].fx(),
+							sensorData.cameraModels()[i].fy(),
+							decimation);
+
+					if(tmp->size())
+					{
+						bool filtered = false;
+						if(tmp->size() && maxDepth)
+						{
+							tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+							filtered = true;
+						}
+
+						if(tmp->size() && voxelSize)
+						{
+							tmp = util3d::voxelize(tmp, voxelSize);
+							filtered = true;
+						}
+
+						if(tmp->size() && samples)
+						{
+							tmp = util3d::sampling(tmp, samples);
+							filtered = true;
+						}
+
+						if(tmp->size() && !filtered)
+						{
+							tmp = util3d::removeNaNFromPointCloud(tmp);
+						}
+
+						if(tmp->size())
+						{
+							tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+						}
+
+						*cloud += *tmp;
+					}
+				}
+				else
+				{
+					UERROR("Camera model %d is invalid", i);
+				}
+			}
+
+			if(cloud->size() && voxelSize)
+			{
+				cloud = util3d::voxelize(cloud, voxelSize);
+			}
+		}
+		else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+		{
+			//stereo
+			cloud = cloudFromStereoImages(sensorData.imageRaw(),
+					sensorData.rightRaw(),
+					sensorData.stereoCameraModel().left().cx(),
+					sensorData.stereoCameraModel().left().cy(),
+					sensorData.stereoCameraModel().left().fx(),
+					sensorData.stereoCameraModel().baseline(),
+					decimation);
+
+			if(cloud->size())
+			{
+				bool filtered = false;
+				if(cloud->size() && maxDepth)
+				{
+					cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+					filtered = true;
+				}
+
+				if(cloud->size() && voxelSize)
+				{
+					cloud = util3d::voxelize(cloud, voxelSize);
+					filtered = true;
+				}
+
+				if(cloud->size() && !filtered)
+				{
+					cloud = util3d::removeNaNFromPointCloud(cloud);
+				}
+
+				if(cloud->size())
+				{
+					cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
+				}
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth,
+					const Transform & localTransform)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+	UASSERT(!localTransform.isNull());
+
+	pcl::PointCloud<pcl::PointXYZ> scan;
+	int middle = depthImage.rows/2;
+	if(middle)
+	{
+		scan.resize(depthImage.cols);
+		int oi = 0;
+		for(int i=0; i<depthImage.cols; ++i)
+		{
+			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
+			if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
+			{
+				if(!localTransform.isIdentity())
+				{
+					pt = util3d::transformPoint(pt, localTransform);
+				}
+				scan[oi++] = pt;
+			}
+		}
+		scan.resize(oi);
+	}
+	return scan;
+}
+
+
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+	}
+	return laserScan;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	output->resize(laserScan.cols);
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+	}
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform)
+{
+	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
+	UASSERT(matrix.rows == 1);
+
+	Eigen::Affine3f t = tranform.toEigen3f();
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(matrix.cols);
+	if(matrix.channels() == 2)
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
+			cloud->at(i).z = 0.0f;
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	else // channels=3
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
+			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	return cloud;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/src/util3d_conversions.cpp

@@ -1,142 +0,0 @@
-/*
-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/util3d_conversions.h"
-
-#include "rtabmap/utilite/ULogger.h"
-#include <pcl/common/transforms.h>
-
-namespace rtabmap
-{
-
-namespace util3d
-{
-
-cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
-{
-	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
-	cv::Mat depth16U;
-	if(!depth32F.empty())
-	{
-		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
-		for(int i=0; i<depth32F.rows; ++i)
-		{
-			for(int j=0; j<depth32F.cols; ++j)
-			{
-				float depth = (depth32F.at<float>(i,j)*1000.0f);
-				unsigned short depthMM = 0;
-				if(depth <= (float)USHRT_MAX)
-				{
-					depthMM = (unsigned short)depth;
-				}
-				depth16U.at<unsigned short>(i, j) = depthMM;
-			}
-		}
-	}
-	return depth16U;
-}
-
-cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
-{
-	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
-	cv::Mat depth32F;
-	if(!depth16U.empty())
-	{
-		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
-		for(int i=0; i<depth16U.rows; ++i)
-		{
-			for(int j=0; j<depth16U.cols; ++j)
-			{
-				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
-				depth32F.at<float>(i, j) = depth;
-			}
-		}
-	}
-	return depth32F;
-}
-
-cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
-{
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
-	for(unsigned int i=0; i<cloud.size(); ++i)
-	{
-		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
-		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
-	}
-	return laserScan;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
-{
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	output->resize(laserScan.cols);
-	for(int i=0; i<laserScan.cols; ++i)
-	{
-		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
-	}
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform)
-{
-	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
-	UASSERT(matrix.rows == 1);
-
-	Eigen::Affine3f t = tranform.toEigen3f();
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(matrix.cols);
-	if(matrix.channels() == 2)
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
-			cloud->at(i).z = 0.0f;
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	else // channels=3
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
-			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	return cloud;
-}
-
-}
-
-}

corelib/src/util3d_features.cpp

@@ -44,36 +44,49 @@ namespace rtabmap
 namespace util3d
 {
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel)
+{
+	UASSERT(cameraModel.isValid());
+	std::vector<CameraModel> models;
+	models.push_back(cameraModel);
+	return generateKeypoints3DDepth(keypoints, depth, models);
+}
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & transform)
+		const std::vector<CameraModel> & cameraModels)
 {
 	UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
+	UASSERT(cameraModels.size());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	if(!depth.empty())
 	{
+		UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
+		float subImageWidth = depth.cols/cameraModels.size();
 		keypoints3d->resize(keypoints.size());
 		for(unsigned int i=0; i!=keypoints.size(); ++i)
 		{
+			int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
+			UASSERT(cameraIndex < (int)cameraModels.size());
 			pcl::PointXYZ pt = util3d::projectDepthTo3D(
 					depth,
-					keypoints[i].pt.x,
+					keypoints[i].pt.x-subImageWidth*cameraIndex,
 					keypoints[i].pt.y,
-					cx,
-					cy,
-					fx,
-					fy,
+					cameraModels.at(cameraIndex).cx(),
+					cameraModels.at(cameraIndex).cy(),
+					cameraModels.at(cameraIndex).fx(),
+					cameraModels.at(cameraIndex).fy(),
 					true);
 
-			if(!transform.isNull() && !transform.isIdentity())
+			if(pcl::isFinite(pt) &&
+				!cameraModels.at(cameraIndex).localTransform().isNull() &&
+			    !cameraModels.at(cameraIndex).localTransform().isIdentity())
 			{
-				pt = util3d::transformPoint(pt, transform);
+				pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
 			}
 			keypoints3d->at(i) = pt;
 		}
@@ -84,13 +97,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform)
+		const StereoCameraModel & stereoCameraModel)
 {
 	UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
+	UASSERT(stereoCameraModel.isValid());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	keypoints3d->resize(keypoints.size());
 	for(unsigned int i=0; i!=keypoints.size(); ++i)
@@ -98,14 +108,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		pcl::PointXYZ pt = util3d::projectDisparityTo3D(
 				keypoints[i].pt,
 				disparity,
-				cx,
-				cy,
-				fx,
-				baseline);
+				stereoCameraModel.left().cx(),
+				stereoCameraModel.left().cy(),
+				stereoCameraModel.left().fx(),
+				stereoCameraModel.baseline());
 
-		if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
+		if(pcl::isFinite(pt) &&
+			!stereoCameraModel.left().localTransform().isNull() &&
+			!stereoCameraModel.left().localTransform().isIdentity())
 		{
-			pt = util3d::transformPoint(pt, transform);
+			pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
 		}
 		keypoints3d->at(i) = pt;
 	}
@@ -120,7 +132,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform,
+		Transform localTransform,
 		int flowWinSize,
 		int flowMaxLevel,
 		int flowIterations,
@@ -137,7 +149,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 			baseline,
 			cx,
 			cy,
-			transform,
+			localTransform,
 			flowWinSize,
 			flowMaxLevel,
 			flowIterations,
@@ -153,7 +165,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform,
+		Transform localTransform,
 		int flowWinSize,
 		int flowMaxLevel,
 		int flowIterations,
@@ -163,6 +175,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 	UASSERT(!leftImage.empty() && !rightImage.empty() &&
 			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
 			leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
+	UASSERT(fx > 0.0f && baseline > 0.0f);
 
 	// Find features in the new left image
 	std::vector<unsigned char> status;
@@ -198,14 +211,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 				pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
 						leftCorners[i],
 						disparity,
-						cx, cy, fx, baseline);
+						cx,
+						cy,
+						fx,
+						baseline);
 
 				if(pcl::isFinite(tmpPt))
 				{
 					pt = tmpPt;
-					if(!transform.isNull() && !transform.isIdentity())
+					if(!localTransform.isNull() &&
+					   !localTransform.isIdentity())
 					{
-						pt = util3d::transformPoint(pt, transform);
+						pt = util3d::transformPoint(pt, localTransform);
 					}
 				}
 			}
@@ -223,11 +240,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 		const std::multimap<int, cv::KeyPoint> & refWords,
 		const std::multimap<int, cv::KeyPoint> & nextWords,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
+		const CameraModel & cameraModel,
 		Transform & cameraTransform,
 		int pnpIterations,
 		float pnpReprojError,
@@ -237,6 +250,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 		const std::multimap<int, pcl::PointXYZ> & refGuess3D,
 		double * varianceOut)
 {
+	UASSERT(cameraModel.isValid());
 	std::multimap<int, pcl::PointXYZ> words3D;
 	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 	if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
@@ -290,10 +304,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 					xp.at<double>(2, i) = 1;
 				}
 
-				cv::Mat K = (cv::Mat_<double>(3,3) <<
-					fx, 0, cx,
-					0, fy, cy,
-					0, 0, 1);
+				cv::Mat K = cameraModel.K();
 				cv::Mat Kinv = K.inv();
 				cv::Mat E = K.t()*F*K;
 				cv::Mat x_norm = Kinv * x;
@@ -313,7 +324,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 					//if camera transform is set, use it instead of the computed one from epipolar geometry
 					if(useCameraTransformGuess)
 					{
-						Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
+						Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse();
 						P = (cv::Mat_<double>(3,4) <<
 								(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
 								(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
@@ -336,7 +347,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 						pts4D.col(i) /= pts4D.at<double>(3,i);
 						if(pts4D.at<double>(2,i) > 0)
 						{
-							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
+							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
 						}
 					}
 
@@ -349,7 +360,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 									R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
 									R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
 
-						cameraTransform = (localTransform * t).inverse() * localTransform;
+						cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
 					}
 
 					if(refGuess3D.size())
@@ -441,7 +452,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 								imagePoints.resize(oi);
 
 								//PnPRansac
-								Transform guess = localTransform.inverse();
+								Transform guess = cameraModel.localTransform().inverse();
 								cv::Mat R = (cv::Mat_<double>(3,3) <<
 										(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 										(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -473,7 +484,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 												   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 												   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
-									cameraTransform = (localTransform * pnp).inverse();
+									cameraTransform = (cameraModel.localTransform() * pnp).inverse();
 								}
 								else
 								{

corelib/src/util3d_mapping.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_mapping.h"
 
-#include <rtabmap/core/util3d_conversions.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d.h>