Added CalibrationDialog in Preferences->source for convenience. The database is also changed to handle new FX, FY, CX and CY intrinsic parameters instead of only depthConstant parameter. Added version of the database in a new table Admin. Updated to version 0.7.0.

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1613 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/CMakeLists.txt

@@ -80,6 +80,8 @@ ENDIF(OpenNI2_FOUND)
 ####################################
 # Generate resources files
 ####################################
+CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql)
+
 SET(R
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
 )

corelib/src/CameraRGBD.cpp

@@ -50,11 +50,14 @@
 namespace rtabmap
 {
 
-CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform, float focalLength) :
+CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
 	_imageRate(imageRate),
 	_localTransform(localTransform),
 	_frameRateTimer(new UTimer()),
-	_focalLength(focalLength)
+	_fx(fx),
+	_fy(fy),
+	_cx(cx),
+	_cy(cy)
 {
 }
 
@@ -66,7 +69,7 @@ CameraRGBD::~CameraRGBD()
 	}
 }
 
-void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
 	if(imageRate>0)
@@ -89,10 +92,22 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant
 	}
 
 	UTimer timer;
-	this->captureImage(rgb, depth, depthConstant);
-	if(_focalLength)
+	this->captureImage(rgb, depth, fx, fy, cx, cy);
+	if(_fx)
 	{
-		depthConstant = 1.0f/_focalLength; // override if set
+		fx = _fx; // override if set
+	}
+	if(_fy)
+	{
+		fy = _fy; // override if set
+	}
+	if(_cx)
+	{
+		cx = _cx; // override if set
+	}
+	if(_cy)
+	{
+		cy = _cy; // override if set
 	}
 	UDEBUG("Time capturing image = %fs", timer.ticks());
 }
@@ -100,8 +115,8 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant
 /////////////////////////
 // CameraOpenNIPCL
 /////////////////////////
-CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform, float focalLength) :
-		CameraRGBD(imageRate, localTransform, focalLength),
+CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
+		CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 		interface_(0),
 		deviceId_(deviceId),
 		depthConstant_(0.0f)
@@ -184,15 +199,21 @@ bool CameraOpenni::init()
 	return true;
 }
 
-void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 	if(interface_ && interface_->isRunning())
 	{
 		dataReady_.acquire();
 		UScopeMutex s(dataMutex_);
-		depth = depth_;
-		rgb = rgb_;
-		depthConstant = depthConstant_;
+		if(depthConstant_)
+		{
+			depth = depth_;
+			rgb = rgb_;
+			fx = 1.0f/depthConstant_;
+			fy = 1.0f/depthConstant_;
+			cx = float(depth_.cols/2) - 0.5f;
+			cy = float(depth_.rows/2) - 0.5f;
+		}
 
 		depth_ = cv::Mat();
 		rgb_ = cv::Mat();
@@ -210,8 +231,8 @@ bool CameraOpenNICV::available()
 	return cv::getBuildInformation().find("OpenNI:                      YES") != std::string::npos;
 }
 
-CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform, float focalLength) :
-	CameraRGBD(imageRate, localTransform, focalLength),
+CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
+	CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 	_asus(asus),
 	_depthFocal(0.0f)
 {
@@ -272,7 +293,7 @@ bool CameraOpenNICV::init()
 	return true;
 }
 
-void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 	if(_capture.isOpened())
 	{
@@ -283,7 +304,10 @@ void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthC
 		depth = depth.clone();
 		rgb = rgb.clone();
 		UASSERT(_depthFocal > 0.0f);
-		depthConstant = 1.0f/_depthFocal;
+		fx = _depthFocal;
+		fy = _depthFocal;
+		cx = float(depth.cols/2) - 0.5f;
+		cy = float(depth.rows/2) - 0.5f;
 	}
 	else
 	{
@@ -304,8 +328,8 @@ bool CameraOpenNI2::available()
 #endif
 }
 
-CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float focalLength) :
-	CameraRGBD(imageRate, localTransform, focalLength),
+CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
+	CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 #ifdef WITH_OPENNI2
 	_device(new openni::Device()),
 	_color(new openni::VideoStream()),
@@ -315,7 +339,8 @@ CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTr
 	_color(0),
 	_depth(0),
 #endif
-	_depthFocal(0.0f)
+	_depthFx(0.0f),
+	_depthFy(0.0f)
 {
 }
 
@@ -435,13 +460,15 @@ bool CameraOpenNI2::init()
 	bool registered = true;
 	if(registered)
 	{
-		_depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
+		_depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
+		_depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
 	}
 	else
 	{
-		_depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+		_depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+		_depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
 	}
-	UINFO("depth focal = %f", _depthFocal);
+	UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
 
 	UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
 			_color->getVideoMode().getFps(),
@@ -473,7 +500,7 @@ bool CameraOpenNI2::init()
 #endif
 }
 
-void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 #ifdef WITH_OPENNI2
 	if(_device->isValid() &&
@@ -498,8 +525,11 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCo
 			cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
 			cv::cvtColor(tmp, rgb, CV_RGB2BGR);
 		}
-		UASSERT(_depthFocal != 0.0f);
-		depthConstant = 1.0f/_depthFocal;
+		UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
+		fx = _depthFx;
+		fy = _depthFy;
+		cx = float(depth.cols/2) - 0.5f;
+		cy = float(depth.rows/2) - 0.5f;
 	}
 	else
 	{
@@ -690,8 +720,8 @@ bool CameraFreenect::available()
 #endif
 }
 
-CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform, float focalLength) :
-		CameraRGBD(imageRate, localTransform, focalLength),
+CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
+		CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 		deviceId_(deviceId),
 		ctx_(0),
 		freenectDevice_(0)
@@ -756,7 +786,7 @@ bool CameraFreenect::init()
 	return false;
 }
 
-void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 #ifdef WITH_FREENECT
 	if(ctx_ && freenectDevice_)
@@ -765,7 +795,10 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthC
 		{
 			freenectDevice_->getData(rgb, depth);
 			UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
-			depthConstant = 1.0f/freenectDevice_->getDepthFocal();
+			fx = freenectDevice_->getDepthFocal();
+			fy = freenectDevice_->getDepthFocal();
+			cx = float(depth.cols/2) - 0.5f;
+			cy = float(depth.rows/2) - 0.5f;
 
 			if(depth.empty())
 			{
@@ -783,7 +816,10 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthC
 		{
 			rgb = cv::Mat();
 			depth = cv::Mat();
-			depthConstant = 0.0f;
+			fx = 0.0f;
+			fy = 0.0f;
+			cx = 0.0f;
+			cy = 0.0f;
 		}
 	}
 #else

corelib/src/CameraThread.cpp

@@ -99,10 +99,13 @@ void CameraThread::mainLoop()
 	cv::Mat descriptors;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat rgb, depth;
-	float depthConstant = 0.0f;
+	float fx = 0.0f;
+	float fy = 0.0f;
+	float cx = 0.0f;
+	float cy = 0.0f;
 	if(_cameraRGBD)
 	{
-		_cameraRGBD->takeImage(rgb, depth, depthConstant);
+		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy);
 	}
 	else
 	{
@@ -113,7 +116,7 @@ void CameraThread::mainLoop()
 	{
 		if(_cameraRGBD)
 		{
-			this->post(new CameraEvent(rgb, depth, depthConstant, _cameraRGBD->getLocalTransform(), ++_seq));
+			this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq));
 		}
 		else
 		{

corelib/src/DBDriver.cpp

@@ -393,11 +393,14 @@ void DBDriver::getNodeData(
 		std::vector<unsigned char> & image,
 		std::vector<unsigned char> & depth,
 		std::vector<unsigned char> & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getNodeDataQuery(signatureId, image, depth, depth2d, depthConstant, localTransform);
+	this->getNodeDataQuery(signatureId, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -33,6 +33,7 @@ namespace rtabmap {
 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
 	DBDriver(parameters),
 	_ppDb(0),
+	_version("0.0.0"),
 	_dbInMemory(Parameters::defaultDbSqlite3InMemory()),
 	_cacheSize(Parameters::defaultDbSqlite3CacheSize()),
 	_journalMode(Parameters::defaultDbSqlite3JournalMode()),
@@ -259,6 +260,44 @@ int DBDriverSqlite3::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileNa
   return rc;
 }
 
+bool DBDriverSqlite3::getVersion(std::string & version) const
+{
+	version = "0.0.0";
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT version FROM Admin;";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		if(rc == SQLITE_OK)
+		{
+			// Process the result if one
+			rc = sqlite3_step(ppStmt);
+			if(rc == SQLITE_ROW)
+			{
+				version = reinterpret_cast<const char*>(sqlite3_column_text(ppStmt, 0));
+				rc = sqlite3_step(ppStmt);
+			}
+			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());
+		}
+		//else
+		//{
+			// old version detected
+		//}
+		return true;
+	}
+	return false;
+}
+
 
 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
 {
@@ -322,6 +361,8 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 		schema = uHex2Str(schema);
 		this->executeNoResultQuery(schema.c_str());
 	}
+	UASSERT(this->getVersion(_version)); // must be true!
+	UINFO("Database version = %s", _version.c_str());
 
 	//Set database optimizations
 	this->setCacheSize(_cacheSize); // this will call the SQL
@@ -409,13 +450,26 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 		if(loadMetricData)
 		{
-			query << "SELECT Image.data, "
-					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
-				  << "FROM Image "
-				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
-				  << "ON Image.id = Depth.id "
-				  << "WHERE Image.id = ?"
-				  <<";";
+			if(uStrNumCmp(_version, "0.7.0") < 0)
+			{
+				query << "SELECT Image.data, "
+						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+					  << "FROM Image "
+					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+					  << "ON Image.id = Depth.id "
+					  << "WHERE Image.id = ?"
+					  <<";";
+			}
+			else
+			{
+				query << "SELECT Image.data, "
+						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+					  << "FROM Image "
+					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+					  << "ON Image.id = Depth.id "
+					  << "WHERE Image.id = ?"
+					  <<";";
+			}
 		}
 		else
 		{
@@ -471,8 +525,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						memcpy(depth.data(), data, dataSize);
 					}
 
-					float depthConstant = sqlite3_column_double(ppStmt, index++);
-					(*iter)->setDepth(depth, depthConstant); // depth constant
+					if(uStrNumCmp(_version, "0.7.0") < 0)
+					{
+						float depthConstant = sqlite3_column_double(ppStmt, index++);
+						(*iter)->setDepth(depth, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0);
+					}
+					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)->setDepth(depth, fx, fy, cx, cy);
+					}
 
 					data = sqlite3_column_blob(ppStmt, index); // local transform
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -516,7 +581,10 @@ void DBDriverSqlite3::getNodeDataQuery(
 		std::vector<unsigned char> & image,
 		std::vector<unsigned char> & depth,
 		std::vector<unsigned char> & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform) const
 {
 	if(_ppDb)
@@ -527,13 +595,26 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		query << "SELECT Image.data, "
-				 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
-			  << "FROM Image "
-			  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
-			  << "ON Image.id = Depth.id "
-			  << "WHERE Image.id = " << signatureId
-			  <<";";
+		if(uStrNumCmp(_version, "0.7.0") < 0)
+		{
+			query << "SELECT Image.data, "
+					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+				  << "FROM Image "
+				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+				  << "ON Image.id = Depth.id "
+				  << "WHERE Image.id = " << signatureId
+				  <<";";
+		}
+		else
+		{
+			query << "SELECT Image.data, "
+					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+				  << "FROM Image "
+				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+				  << "ON Image.id = Depth.id "
+				  << "WHERE Image.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());
@@ -570,7 +651,21 @@ void DBDriverSqlite3::getNodeDataQuery(
 				memcpy(depth.data(), data, dataSize);
 			}
 
-			depthConstant = sqlite3_column_double(ppStmt, index++);
+			if(uStrNumCmp(_version, "0.7.0") < 0)
+			{
+				float depthConstant = sqlite3_column_double(ppStmt, index++);
+				fx = 1.0f/depthConstant;
+				fy = 1.0f/depthConstant;
+				cx = 0.0f;
+				cy = 0.0f;
+			}
+			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++);
@@ -588,7 +683,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 				memcpy(depth2d.data(), data, dataSize);
 			}
 
-			if(depth.empty() || depthConstant <= 0)
+			if(depth.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
 			{
 				UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only.");
 			}
@@ -1707,7 +1802,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			//metric
 			if((*i)->getDepth().size() || (*i)->getDepth2D().size())
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepth(), (*i)->getDepth2D(), (*i)->getDepthConstant(), (*i)->getLocalTransform());
+				stepDepth(ppStmt, (*i)->id(), (*i)->getDepth(), (*i)->getDepth2D(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
 			}
 		}
 		// Finalize (delete) the statement
@@ -1836,13 +1931,23 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepDepth() const
 {
-	return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
+	if(uStrNumCmp(_version, "0.7.0") < 0)
+	{
+		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
+	}
+	else
+	{
+		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
+	}
 }
 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 		int id,
 		const std::vector<unsigned char> & depth,
 		const std::vector<unsigned char> & depth2d,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & localTransform) const
 {
 	UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depth.size(), (int)depth2d.size());
@@ -1867,8 +1972,22 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	rc = sqlite3_bind_double(ppStmt, index++, depthConstant);
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	if(uStrNumCmp(_version, "0.7.0") < 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fy);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else
+	{
+		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);
+		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());
+		rc = sqlite3_bind_double(ppStmt, index++, cy);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
 
 	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());

corelib/src/DBDriverSqlite3.h

@@ -72,7 +72,10 @@ private:
 			std::vector<unsigned char> & image,
 			std::vector<unsigned char> & depth,
 			std::vector<unsigned char> & depth2d,
-			float & depthConstant,
+			float & fx,
+			float & fy,
+			float & cx,
+			float & cy,
 			Transform & localTransform) const;
 	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const;
 	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
@@ -97,7 +100,10 @@ private:
 			int id,
 			const std::vector<unsigned char> & depth,
 			const std::vector<unsigned char> & depth2d,
-			float depthConstant,
+			float fx,
+			float fy,
+			float cx,
+			float cy,
 			const Transform & localTransform) const;
 	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
@@ -106,9 +112,11 @@ private:
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
+	bool getVersion(std::string &) const;
 
 private:
 	sqlite3 * _ppDb;
+	std::string _version;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/src/DBReader.cpp

@@ -112,9 +112,9 @@ void DBReader::mainLoopBegin()
 void DBReader::mainLoop()
 {
 	cv::Mat image, depth, depth2d;
-	float depthConstant;
+	float fx,fy,cx,cy;
 	Transform localTransform, pose;
-	this->getNextImage(image, depth, depth2d, depthConstant, localTransform, pose);
+	this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose);
 	if(!image.empty())
 	{
 		if(depth.empty())
@@ -125,7 +125,7 @@ void DBReader::mainLoop()
 		{
 			if(!_odometryIgnored)
 			{
-				Image data(image, depth, depth2d, depthConstant, pose, localTransform);
+				Image data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
 				this->post(new OdometryEvent(data));
 				if(pose.isNull())
 				{
@@ -137,7 +137,7 @@ void DBReader::mainLoop()
 			else
 			{
 				// without odometry
-				this->post(new CameraEvent(image, depth, depth2d, depthConstant, localTransform));
+				this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform));
 			}
 		}
 	}
@@ -154,7 +154,10 @@ void DBReader::getNextImage(
 		cv::Mat & image,
 		cv::Mat & depth,
 		cv::Mat & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform,
 		Transform & pose)
 {
@@ -186,7 +189,7 @@ void DBReader::getNextImage(
 			std::vector<unsigned char> depthBytes;
 			std::vector<unsigned char> depth2dBytes;
 			int mapId;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, depthConstant, localTransform);
+			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
 			_dbDriver->getPose(*_currentId, pose, mapId);
 			++_currentId;
 			if(imageBytes.empty())

corelib/src/Features2d.cpp

@@ -41,28 +41,34 @@ namespace rtabmap {
 void filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		float maxDepth)
 {
 	cv::Mat descriptors;
-	filterKeypointsByDepth(keypoints, descriptors, depth, depthConstant, maxDepth);
+	filterKeypointsByDepth(keypoints, descriptors, depth, fx, fy, cx, cy, maxDepth);
 }
 
 void filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		float maxDepth)
 {
-	if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
+	if(!depth.empty() && fx > 0.0f && fy > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
 	{
 		std::vector<cv::KeyPoint> output(keypoints.size());
 		std::vector<int> indexes(keypoints.size(), 0);
 		int oi=0;
 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
-			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, depthConstant);
+			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy);
 			if(uIsFinite(pt.z) && pt.z < maxDepth)
 			{
 				output[oi++] = keypoints[i];

corelib/src/Memory.cpp

@@ -1759,7 +1759,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
 					oldDepth,
-					oldS.getDepthConstant(),
+					oldS.getDepthFx(),
+					oldS.getDepthFy(),
+					oldS.getDepthCx(),
+					oldS.getDepthCy(),
 					_icpDecimation,
 					_icpMaxDepth,
 					_icpVoxelSize,
@@ -1767,7 +1770,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 					oldS.getLocalTransform());
 			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 					newDepth,
-					newS.getDepthConstant(),
+					newS.getDepthFx(),
+					newS.getDepthFy(),
+					newS.getDepthCx(),
+					newS.getDepthCy(),
 					_icpDecimation,
 					_icpMaxDepth,
 					_icpVoxelSize,
@@ -2400,7 +2406,10 @@ void Memory::getImageDepth(
 		std::vector<unsigned char> & rgb,
 		std::vector<unsigned char> & depth,
 		std::vector<unsigned char> & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform) const
 {
 	const Signature * s = this->getSignature(locationId);
@@ -2409,12 +2418,15 @@ void Memory::getImageDepth(
 		rgb = s->getImage();
 		depth = s->getDepth();
 		depth2d = s->getDepth2D();
-		depthConstant = s->getDepthConstant();
+		fx = s->getDepthFx();
+		fy = s->getDepthFy();
+		cx = s->getDepthCx();
+		cy = s->getDepthCy();
 		localTransform = s->getLocalTransform();
 	}
 	if(rgb.empty() && this->isRawDataKept() && _dbDriver)
 	{
-		_dbDriver->getNodeData(locationId, rgb, depth, depth2d, depthConstant, localTransform);
+		_dbDriver->getNodeData(locationId, rgb, depth, depth2d, fx, fy, cx, cy, localTransform);
 	}
 }
 
@@ -2782,12 +2794,12 @@ void Memory::copyData(const Signature * from, Signature * to)
 			std::vector<unsigned char> image;
 			std::vector<unsigned char> depth;
 			std::vector<unsigned char> depth2d;
-			float depthConstant;
+			float fx, fy, cx, cy;
 			Transform localTransform;
-			_dbDriver->getNodeData(from->id(), image, depth, depth2d, depthConstant, localTransform);
+			_dbDriver->getNodeData(from->id(), image, depth, depth2d, fx, fy, cx, cy, localTransform);
 
 			to->setImage(image);
-			to->setDepth(depth, depthConstant);
+			to->setDepth(depth, fx, fy, cx, cy);
 			to->setDepth2D(depth2d);
 			to->setLocalTransform(localTransform);
 
@@ -2796,7 +2808,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 		else
 		{
 			to->setImage(from->getImage());
-			to->setDepth(from->getDepth(), from->getDepthConstant());
+			to->setDepth(from->getDepth(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy());
 			to->setDepth2D(from->getDepth2D());
 			to->setLocalTransform(from->getLocalTransform());
 		}
@@ -2814,7 +2826,10 @@ void Memory::copyData(const Signature * from, Signature * to)
 void Memory::extractKeypointsAndDescriptors(
 		const cv::Mat & image,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors)
 {
@@ -2827,7 +2842,7 @@ void Memory::extractKeypointsAndDescriptors(
 			keypoints = _feature2D->generateKeypoints(image, 0, roi);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
 
-			filterKeypointsByDepth(keypoints, depth, depthConstant, _wordsMaxDepth);
+			filterKeypointsByDepth(keypoints, depth, fx, fy, cx, cy, _wordsMaxDepth);
 			limitKeypoints(keypoints, _wordsPerImageTarget);
 		}
 
@@ -2923,7 +2938,7 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			descriptors = image.descriptors();
 			keypoints = image.keypoints();
 		}
-		filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthConstant(), _wordsMaxDepth);
+		filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
 		limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 	}
 	else
@@ -2940,7 +2955,7 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			imageMono = image.image();
 		}
 
-		this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
+		this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);
 
 		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
@@ -2998,9 +3013,9 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 
 	//3d words
 	std::multimap<int, pcl::PointXYZ> words3;
-	if(!image.depth().empty() && image.depthConstant())
+	if(!image.depth().empty() && image.depthFx() && image.depthFy())
 	{
-		words3 = util3d::generateWords3(words, image.depth(), image.depthConstant(), image.localTransform());
+		words3 = util3d::generateWords3(words, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), image.localTransform());
 	}
 
 	Signature * s;
@@ -3025,7 +3040,10 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			util3d::compressData(image.depth2d()),
 			imageBytes,
 			depthBytes,
-			image.depthConstant(),
+			image.depthFx(),
+			image.depthFy(),
+			image.depthCx(),
+			image.depthCy(),
 			image.localTransform());
 	}
 	else

corelib/src/Odometry.cpp

@@ -173,7 +173,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
+	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);
 
 	image.setDescriptors(descriptors, _memory->getFeatureType());
 	image.setKeypoints(keypoints);
@@ -389,7 +389,10 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 						image.depth(),
-						image.depthConstant(),
+						image.depthFx(),
+						image.depthFy(),
+						image.depthCx(),
+						image.depthCy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
@@ -520,7 +523,7 @@ void OdometryThread::mainLoop()
 
 void OdometryThread::addImage(const Image & image)
 {
-	if(image.empty() || image.depth().empty() || image.depthConstant() == 0.0f)
+	if(image.empty() || image.depth().empty() || image.depthFx() == 0.0f || image.depthFy() == 0.0f)
 	{
 		ULOGGER_ERROR("image empty !?");
 		return;

corelib/src/Rtabmap.cpp

@@ -1482,7 +1482,10 @@ bool Rtabmap::process(const Image & image)
 				std::map<int, std::vector<unsigned char> > images;
 				std::map<int, std::vector<unsigned char> > depths;
 				std::map<int, std::vector<unsigned char> > depth2ds;
-				std::map<int, float> depthConstants;
+				std::map<int, float> depthFxs;
+				std::map<int, float> depthFys;
+				std::map<int, float> depthCxs;
+				std::map<int, float> depthCys;
 				std::map<int, Transform> localTransforms;
 
 				std::vector<int> ids(signaturesRetrieved.begin(), signaturesRetrieved.end());
@@ -1500,14 +1503,17 @@ bool Rtabmap::process(const Image & image)
 					if(_rgbdSlamMode)
 					{
 						std::vector<unsigned char> depth, depth2d;
-						float depthConstant;
+						float fx, fy, cx, cy;
 						Transform localTransform;
-						_memory->getImageDepth(ids[i], im, depth, depth2d, depthConstant, localTransform);
+						_memory->getImageDepth(ids[i], im, depth, depth2d, fx, fy, cx, cy, localTransform);
 
 						if(!depth.empty())
 						{
 							depths.insert(std::make_pair(ids[i], depth));
-							depthConstants.insert(std::make_pair(ids[i], depthConstant));
+							depthFxs.insert(std::make_pair(ids[i], fx));
+							depthFys.insert(std::make_pair(ids[i], fy));
+							depthCxs.insert(std::make_pair(ids[i], cx));
+							depthCys.insert(std::make_pair(ids[i], cy));
 							localTransforms.insert(std::make_pair(ids[i], localTransform));
 						}
 						if(!depth2d.empty())
@@ -1536,7 +1542,10 @@ bool Rtabmap::process(const Image & image)
 				statistics_.setImages(images);
 				statistics_.setDepths(depths);
 				statistics_.setDepth2ds(depth2ds);
-				statistics_.setDepthConstants(depthConstants);
+				statistics_.setDepthFxs(depthFxs);
+				statistics_.setDepthFys(depthFys);
+				statistics_.setDepthCxs(depthCxs);
+				statistics_.setDepthCys(depthCys);
 				statistics_.setLocalTransforms(localTransforms);
 			}
 
@@ -2153,7 +2162,10 @@ void Rtabmap::dumpPrediction() const
 void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 		std::map<int, std::vector<unsigned char> > & depths,
 		std::map<int, std::vector<unsigned char> > & depths2d,
-		std::map<int, float> & depthConstants,
+		std::map<int, float> & depthFxs,
+		std::map<int, float> & depthFys,
+		std::map<int, float> & depthCxs,
+		std::map<int, float> & depthCys,
 		std::map<int, Transform> & localTransforms,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
@@ -2182,9 +2194,9 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
 			std::vector<unsigned char> image, depth, depth2d;
-			float depthConstant;
+			float fx, fy, cx, cy;
 			Transform localTransform;
-			_memory->getImageDepth(*iter, image, depth, depth2d, depthConstant, localTransform);
+			_memory->getImageDepth(*iter, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 
 			if(image.size())
 			{
@@ -2198,9 +2210,12 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 			{
 				depths2d.insert(std::make_pair(*iter, depth2d));
 			}
-			if(depthConstant > 0)
+			if(fx > 0 && fy > 0)
 			{
-				depthConstants.insert(std::make_pair(*iter, depthConstant));
+				depthFxs.insert(std::make_pair(*iter, fx));
+				depthFys.insert(std::make_pair(*iter, fy));
+				depthCxs.insert(std::make_pair(*iter, cx));
+				depthCys.insert(std::make_pair(*iter, cy));
 			}
 			if(!localTransform.isNull())
 			{

corelib/src/RtabmapThread.cpp

@@ -95,7 +95,10 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthConstants;
+	std::map<int, float> depthFxs;
+	std::map<int, float> depthFys;
+	std::map<int, float> depthCxs;
+	std::map<int, float> depthCys;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
@@ -103,7 +106,10 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	_rtabmap->get3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints,
@@ -113,7 +119,10 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints));
@@ -124,7 +133,10 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthConstants;
+	std::map<int, float> depthFxs;
+	std::map<int, float> depthFys;
+	std::map<int, float> depthCxs;
+	std::map<int, float> depthCys;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
@@ -137,7 +149,10 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints));

corelib/src/Signature.cpp

@@ -42,7 +42,10 @@ Signature::Signature(
 		const std::vector<unsigned char> & depth2D, // in base_link frame
 		const std::vector<unsigned char> & image, // in camera_link frame
 		const std::vector<unsigned char> & depth, // in camera_link frame
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & localTransform) :
 	_id(id),
 	_mapId(mapId),
@@ -55,7 +58,10 @@ Signature::Signature(
 	_image(image),
 	_depth(depth),
 	_depth2D(depth2D),
-	_depthConstant(depthConstant),
+	_fx(fx),
+	_fy(fy),
+	_cx(cx),
+	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
 	_words3(words3)
@@ -188,11 +194,14 @@ void Signature::removeWord(int wordId)
 	_words3.erase(wordId);
 }
 
-void Signature::setDepth(const std::vector<unsigned char> & depth, float depthConstant)
+void Signature::setDepth(const std::vector<unsigned char> & depth, float fx, float fy, float cx, float cy)
 {
-	UASSERT_MSG(depth.empty() || (!depth.empty() && depthConstant > 0.0f), uFormat("depthConstant=%f",depthConstant).c_str());
+	UASSERT_MSG(depth.empty() || (!depth.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());
 	_depth = depth;
-	_depthConstant=depthConstant;
+	_fx=fx;
+	_fy=fy;
+	_cx=cx;
+	_cy=cy;
 }
 
 } //namespace rtabmap

corelib/src/resources/DatabaseSchema.sql.in

@@ -1,115 +1,128 @@
--- *******************************************************************
---  DatabaseSchema: Script for creating the database
---   Usage:
---       $ sqlite3 LTM.db < DatabaseSchema.sql
---
--- *******************************************************************
-
--- *******************************************************************
--- CLEAN
--- *******************************************************************
-/*DROP TABLE Node;*/
-
--- *******************************************************************
--- CREATE
--- *******************************************************************
-CREATE TABLE Node (
-	id INTEGER NOT NULL,
-	map_id INTEGER NOT NULL,
-	weight INTEGER,
-	pose BLOB,
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Image (
-	id INTEGER NOT NULL,
-	data BLOB,
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Depth (
-	id INTEGER NOT NULL,
-	data BLOB,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
-	constant FLOAT,
-	local_transform BLOB,
-	data2d BLOB,   -- CV_32FC2, Example: Laser scan
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Link (
-	from_id INTEGER NOT NULL,
-	to_id INTEGER NOT NULL,
-	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
-	transform BLOB,
-	FOREIGN KEY (from_id) REFERENCES Node(id),
-	FOREIGN KEY (to_id) REFERENCES Node(id)
-);
-
--- 
-CREATE TABLE Word (
-	id INTEGER NOT NULL,
-	descriptor_size INTEGER NOT NULL,
-	descriptor BLOB NOT NULL,
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Map_Node_Word (
-	node_id INTEGER NOT NULL,
-	word_id INTEGER NOT NULL,
-	pos_x FLOAT NOT NULL,
-	pos_y FLOAT NOT NULL,
-	size INTEGER NOT NULL,
-	dir FLOAT NOT NULL,
-	response FLOAT NOT NULL,
-	depth_x FLOAT,
-	depth_y FLOAT,
-	depth_z FLOAT,
-	FOREIGN KEY (node_id) REFERENCES Node(id),
-	FOREIGN KEY (word_id) REFERENCES Word(id)
-);
-
-CREATE TABLE Statistics (
-	STM_size INTEGER,
-	last_sign_added INTEGER,
-	process_mem_used INTEGER,
-	database_mem_used INTEGER,
-	dictionary_size INTEGER,
-	time_enter DATE
-);
-
--- *******************************************************************
--- TRIGGERS
--- *******************************************************************
-CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word 
-WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
-BEGIN
- SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
-END;
-
- --   Creating a trigger for time_enter
-CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
-BEGIN
- UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
-END;
-
-CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
-BEGIN
- UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
-END;
-
-CREATE TRIGGER insert_Statistics_timeEnter AFTER INSERT ON Statistics
-BEGIN
- UPDATE Statistics SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
-END;
-
-
--- *******************************************************************
--- INDEXES
--- *******************************************************************
-CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
-CREATE INDEX IDX_Link_from_id on Link (from_id);
-
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	pose BLOB,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Image (
+	id INTEGER NOT NULL,
+	data BLOB,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Depth (
+	id INTEGER NOT NULL,
+	data BLOB,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
+	fx FLOAT,
+	fy FLOAT,
+	cx FLOAT,
+	cy FLOAT,
+	local_transform BLOB,
+	data2d BLOB,   -- CV_32FC2, Example: Laser scan
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
+	transform BLOB,
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Map_Node_Word (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	FOREIGN KEY (node_id) REFERENCES Node(id),
+	FOREIGN KEY (word_id) REFERENCES Word(id)
+);
+
+CREATE TABLE Statistics (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	time_enter DATE
+);
+
+CREATE TABLE Admin (
+	version INTEGER,
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Statistics_timeEnter AFTER INSERT ON Statistics
+BEGIN
+ UPDATE Statistics SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('@PROJECT_VERSION@');
+

corelib/src/util3d.cpp

@@ -301,7 +301,10 @@ cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
 std::multimap<int, pcl::PointXYZ> generateWords3(
 		const std::multimap<int, cv::KeyPoint> & words,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & transform)
 {
 	std::multimap<int, pcl::PointXYZ> words3;
@@ -311,10 +314,10 @@ std::multimap<int, pcl::PointXYZ> generateWords3(
 				depth,
 				iter->second.pt.x+0.5f,
 				iter->second.pt.y+0.5f,
-				depth.cols/2,
-				depth.rows/2,
-				1.0f/depthConstant,
-				1.0f/depthConstant);
+				cx,
+				cy,
+				fx,
+				fy);
 
 		if(!transform.isNull() && !transform.isIdentity())
 		{
@@ -411,18 +414,6 @@ void findCorrespondences(
 	inliers2.resize(oi);
 }
 
-pcl::PointXYZ getDepth(
-		const cv::Mat & depthImage,
-		int x, int y,
-		float depthConstant)
-{
-	return getDepth(depthImage, x, y,
-			(float)depthImage.cols/2,
-			(float)depthImage.rows/2,
-			1.0f/depthConstant,
-			1.0f/depthConstant);
-}
-
 pcl::PointXYZ getDepth(const cv::Mat & depthImage,
 					   int x, int y,
 			       float cx, float cy,
@@ -433,8 +424,8 @@ pcl::PointXYZ getDepth(const cv::Mat & depthImage,
 	pcl::PointXYZ pt;
 
 	// Use correct principal point from calibration
-	float center_x = cx; //cameraInfo.K.at(2)
-	float center_y = cy; //cameraInfo.K.at(5)
+	float center_x = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
+	float center_y = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
 
 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
 
@@ -624,19 +615,6 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 	return pcl::transformPoint(pt, transformToEigen3f(transform));
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
-		const cv::Mat & imageDepth,
-		float depthConstant,
-		int decimation)
-{
-	return cloudFromDepth(
-			imageDepth,
-			float(imageDepth.cols/2),
-			float(imageDepth.rows/2),
-			1.0f/depthConstant,
-			1.0f/depthConstant,
-			decimation);
-}
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -675,21 +653,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	return cloud;
 }
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
-		const cv::Mat & imageRgb,
-		const cv::Mat & imageDepth,
-		float depthConstant,
-		int decimation)
-{
-	return cloudFromDepthRGB(
-			imageRgb,
-			imageDepth,
-			float(imageDepth.cols/2),
-			float(imageDepth.rows/2),
-			1.0f/depthConstant,
-			1.0f/depthConstant,
-			decimation);
-}
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
@@ -1421,7 +1384,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
 // If "voxel" > 0, "samples" is ignored
 pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		int decimation,
 		double maxDepth,
 		float voxel,
@@ -1431,10 +1397,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 	cloud = cloudFromDepth(
 			depth,
-			depth.cols/2,
-			depth.rows/2,
-			1.0f/depthConstant,
-			1.0f/depthConstant,
+			cx,
+			cy,
+			fx,
+			fy,
 			decimation);
 
 	if(cloud->size())