Database: added "opt_****" fields in Admin table. Tango: optimized mesh saved in database for quick open, open menu shows preview images. util3d::mergeTextures() return all textures in same cv::Mat.

2026-09-01 17:10:26 +08:00 · 2017-06-20 17:29:06 -04:00
parent 539d500528
commit ee8d48a915
25 changed files with 1376 additions and 100 deletions
--- a/corelib/src/DBDriver.cpp
+++ b/corelib/src/DBDriver.cpp
@@ -1024,6 +1024,45 @@ void DBDriver::addStatistics(const Statistics & statistics) const
 	_dbSafeAccessMutex.unlock();
 }

+void DBDriver::savePreviewImage(const cv::Mat & image) const
+{
+	_dbSafeAccessMutex.lock();
+	savePreviewImageQuery(image);
+	_dbSafeAccessMutex.unlock();
+}
+
+cv::Mat DBDriver::loadPreviewImage() const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat image = loadPreviewImageQuery();
+	_dbSafeAccessMutex.unlock();
+	return image;
+}
+
+void DBDriver::saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::map<int, Transform> & poses,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+			const cv::Mat & textures) const
+{
+	_dbSafeAccessMutex.lock();
+	saveOptimizedMeshQuery(cloud, poses, polygons, texCoords, textures);
+	_dbSafeAccessMutex.unlock();
+}
+
+cv::Mat DBDriver::loadOptimizedMesh(
+				std::map<int, Transform> * poses,
+				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+				cv::Mat * textures) const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
+	_dbSafeAccessMutex.unlock();
+	return cloud;
+}
+
 void DBDriver::generateGraph(
 		const std::string & fileName,
 		const std::set<int> & idsInput,
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -3728,6 +3728,477 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 	}
 }

+void DBDriverSqlite3::savePreviewImageQuery(const cv::Mat & image) const
+{
+	UDEBUG("");
+	if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		// Update table Admin
+		query = uFormat("UPDATE Admin SET preview_image=? WHERE version='%s';", _version.c_str());
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		int index = 1;
+		cv::Mat compressedImage;
+		if(image.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			// compress
+			if(image.rows == 1 && image.type() == CV_8UC1)
+			{
+				// already compressed
+				compressedImage = image;
+			}
+			else
+			{
+				compressedImage	= compressImage2(image, ".jpg");
+			}
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedImage.data, compressedImage.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		//execute query
+		rc=sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
+{
+	UDEBUG("");
+	cv::Mat image;
+	if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT preview_image "
+			  << "FROM Admin "
+			  << "WHERE version='" << _version.c_str()
+			  <<"';";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
+		if(rc == SQLITE_ROW)
+		{
+			const void * data = 0;
+			int dataSize = 0;
+			int index = 0;
+
+			//opt_cloud
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				image = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+			}
+			UDEBUG("Image=%dx%d", image.cols, image.rows);
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+	}
+	return image;
+}
+
+void DBDriverSqlite3::saveOptimizedMeshQuery(
+			const cv::Mat & cloud,
+			const std::map<int, Transform> & poses,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+			const cv::Mat & textures) const
+{
+	UDEBUG("");
+	if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		// Update table Admin
+		query = uFormat("UPDATE Admin SET opt_cloud=?, opt_ids=?, opt_poses=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		if(cloud.empty())
+		{
+			// set all fields to null
+			for(int i=1; i<=7; ++i)
+			{
+				rc = sqlite3_bind_null(ppStmt, i);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+
+			//execute query
+			rc=sqlite3_step(ppStmt);
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			int index = 1;
+
+			// compress and save cloud
+			cv::Mat compressedCloud;
+			if(cloud.rows == 1 && cloud.type() == CV_8UC1)
+			{
+				// already compressed
+				compressedCloud = cloud;
+			}
+			else
+			{
+				compressedCloud	= compressData2(cloud);
+			}
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			// opt ids and poses
+			cv::Mat compressedIds;
+			cv::Mat compressedPoses;
+			cv::Mat compressedPolygons;
+			cv::Mat compressedTexCoords;
+			cv::Mat compressedTextures;
+			if(poses.empty())
+			{
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			else
+			{
+				std::vector<int> serializedIds(poses.size());
+				std::vector<float> serializedPoses(poses.size()*12);
+				int i=0;
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					serializedIds[i] = iter->first;
+					memcpy(serializedPoses.data()+(12*sizeof(float)*i), iter->second.data(), 12*sizeof(float));
+				}
+
+				compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
+				compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
+
+				rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+
+			// polygons
+			if(polygons.empty())
+			{
+				//polygon size
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				// polygons
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				// tex_coords
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				// materials
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			else
+			{
+				std::vector<int> serializedPolygons;
+				std::vector<float> serializedTexCoords;
+				int polygonSize = 0;
+				int totalPolygonIndices = 0;
+				UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
+				for(unsigned int t=0; t<polygons.size(); ++t)
+				{
+					unsigned int materialPolygonIndices = 0;
+					for(unsigned int p=0; p<polygons[t].size(); ++p)
+					{
+						if(polygonSize == 0)
+						{
+							UASSERT(polygons[t][p].size());
+							polygonSize = polygons[t][p].size();
+						}
+						else
+						{
+							UASSERT(polygonSize == (int)polygons[t][p].size());
+						}
+
+						materialPolygonIndices += polygons[t][p].size();
+					}
+					totalPolygonIndices += materialPolygonIndices;
+
+					if(!texCoords.empty())
+					{
+						UASSERT(materialPolygonIndices == texCoords[t].size());
+					}
+				}
+				UASSERT(totalPolygonIndices>0);
+				serializedPolygons.resize(totalPolygonIndices+polygons.size());
+				if(!texCoords.empty())
+				{
+					serializedTexCoords.resize(totalPolygonIndices*2+polygons.size());
+				}
+
+				int oi=0;
+				int ci=0;
+				for(unsigned int t=0; t<polygons.size(); ++t)
+				{
+					serializedPolygons[oi++] = polygons[t].size();
+					if(!texCoords.empty())
+					{
+						serializedTexCoords[ci++] = texCoords[t].size();
+					}
+					for(unsigned int p=0; p<polygons[t].size(); ++p)
+					{
+						int texIndex = p*polygonSize;
+						for(unsigned int i=0; i<polygons[t][p].size(); ++i)
+						{
+							serializedPolygons[oi++] = polygons[t][p][i];
+
+							if(!texCoords.empty())
+							{
+								serializedTexCoords[ci++] = texCoords[t][texIndex+i][0];
+								serializedTexCoords[ci++] = texCoords[t][texIndex+i][1];
+							}
+						}
+					}
+				}
+
+				compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
+
+				// polygon size
+				rc = sqlite3_bind_int(ppStmt, index++, polygonSize);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				rc = sqlite3_bind_blob(ppStmt, index++, compressedPolygons.data, compressedPolygons.cols, SQLITE_STATIC);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				// tex coords
+				if(texCoords.empty())
+				{
+					// tex coords
+					rc = sqlite3_bind_null(ppStmt, index++);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					// materials
+					rc = sqlite3_bind_null(ppStmt, index++);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				}
+				else
+				{
+					compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
+					rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+					UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)texCoords.size());
+					if(textures.rows == 1 && textures.type() == CV_8UC1)
+					{
+						//already compressed
+						compressedTextures = textures;
+					}
+					else
+					{
+						compressedTextures = compressImage2(textures, ".jpg");
+					}
+					rc = sqlite3_bind_blob(ppStmt, index++, compressedTextures.data, compressedTextures.cols, SQLITE_STATIC);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				}
+			}
+
+			//execute query
+			rc=sqlite3_step(ppStmt);
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
+cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
+			std::map<int, Transform> * poses,
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+			cv::Mat * textures) const
+{
+	UDEBUG("");
+	cv::Mat cloud;
+	if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT opt_cloud, opt_ids, opt_poses, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
+			  << "FROM Admin "
+			  << "WHERE version='" << _version.c_str()
+			  <<"';";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
+		if(rc == SQLITE_ROW)
+		{
+			const void * data = 0;
+			int dataSize = 0;
+			int index = 0;
+
+			//opt_cloud
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				cloud = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+			}
+			UDEBUG("Cloud=%d points", cloud.cols);
+
+			//opt_poses
+			cv::Mat serializedIds;
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("serializedIds=%d", serializedIds.cols);
+			}
+
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("serializedPoses=%d", serializedPoses.cols);
+
+				if(poses)
+				{
+					UASSERT(serializedIds.cols == serializedPoses.cols/12);
+				}
+			}
+
+			//opt_polygons_size
+			int polygonSize = sqlite3_column_int(ppStmt, index++);
+			UDEBUG("polygonSize=%d", polygonSize);
+
+			//opt_polygons
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				UASSERT(polygonSize > 0);
+				if(polygons)
+				{
+					cv::Mat serializedPolygons = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+					UDEBUG("serializedPolygons=%d", serializedPolygons.cols);
+					UASSERT(serializedPolygons.total());
+					for(int t=0; t<serializedPolygons.cols; ++t)
+					{
+						UASSERT(serializedPolygons.at<int>(t) > 0);
+						std::vector<std::vector<unsigned int> > materialPolygons(serializedPolygons.at<int>(t), std::vector<unsigned int>(polygonSize));
+						++t;
+						UASSERT(t < serializedPolygons.cols);
+						UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
+						for(int p=0; p<(int)materialPolygons.size(); ++p)
+						{
+							for(int i=0; i<polygonSize; ++i)
+							{
+								materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
+							}
+						}
+						t+=materialPolygons.size()*polygonSize;
+						polygons->push_back(materialPolygons);
+					}
+				}
+
+				//opt_tex_coords
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				if(dataSize>0 && data)
+				{
+					if(texCoords)
+					{
+						cv::Mat serializedTexCoords = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+						UDEBUG("serializedTexCoords=%d", serializedTexCoords.cols);
+						UASSERT(serializedTexCoords.total());
+						for(int t=0; t<serializedTexCoords.cols; ++t)
+						{
+							UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
+							std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
+							++t;
+							UASSERT(t < serializedTexCoords.cols);
+							UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());
+							for(int p=0; p<(int)materialtexCoords.size(); ++p)
+							{
+								materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
+								materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
+							}
+							t+=materialtexCoords.size()*2;
+							texCoords->push_back(materialtexCoords);
+						}
+					}
+
+					//opt_tex_materials
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize>0 && data)
+					{
+						if(textures)
+						{
+							*textures = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+							UDEBUG("textures=%dx%d", textures->cols, textures->rows);
+						}
+					}
+				}
+			}
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+	}
+	return cloud;
+}
+
 std::string DBDriverSqlite3::queryStepNode() const
 {
 	if(uStrNumCmp(_version, "0.13.0") >= 0)
--- a/corelib/src/DBDriverSqlite3.h
+++ b/corelib/src/DBDriverSqlite3.h
@@ -95,6 +95,19 @@ private:
 				const cv::Mat & image) const;

 	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const;
+	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedMeshQuery(
+			const cv::Mat & cloud,
+			const std::map<int, Transform> & poses,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+			const cv::Mat & textures) const;
+	virtual cv::Mat loadOptimizedMeshQuery(
+			std::map<int, Transform> * poses,
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+			cv::Mat * textures) const;

 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -1651,6 +1651,48 @@ void Memory::saveStatistics(const Statistics & statistics)
 	}
 }

+void Memory::savePreviewImage(const cv::Mat & image) const
+{
+	if(_dbDriver)
+	{
+		_dbDriver->savePreviewImage(image);
+	}
+}
+cv::Mat Memory::loadPreviewImage() const
+{
+	if(_dbDriver)
+	{
+		return _dbDriver->loadPreviewImage();
+	}
+	return cv::Mat();
+}
+
+void Memory::saveOptimizedMesh(
+		const cv::Mat & cloud,
+		const std::map<int, Transform> & poses,
+		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+		const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+		const cv::Mat & textures) const
+{
+	if(_dbDriver)
+	{
+		_dbDriver->saveOptimizedMesh(cloud, poses, polygons, texCoords, textures);
+	}
+}
+
+cv::Mat Memory::loadOptimizedMesh(
+			std::map<int, Transform> * poses,
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+			cv::Mat * textures) const
+{
+	if(_dbDriver)
+	{
+		return _dbDriver->loadOptimizedMesh(poses, polygons, texCoords, textures);
+	}
+	return cv::Mat();
+}
+
 void Memory::emptyTrash()
 {
 	if(_dbDriver)
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/filters/crop_box.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
+#include <pcl/TextureMesh.h>

 #include <stdlib.h>
 #include <set>
--- a/corelib/src/resources/DatabaseSchema.sql.in
+++ b/corelib/src/resources/DatabaseSchema.sql.in
@@ -104,7 +104,16 @@ CREATE TABLE Statistics (

 CREATE TABLE Admin (
 	version TEXT,
-	preview_image BLOB,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+
 	time_enter DATE
 );

--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -1299,6 +1299,38 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 	return laserScan;
 }

+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		float * ptr = laserScan.ptr<float>(0, i);
+		if(!nullTransform)
+		{
+			pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.z;
+			ptr[4] = pt.normal_x;
+			ptr[5] = pt.normal_y;
+			ptr[6] = pt.normal_z;
+		}
+		else
+		{
+			ptr[0] = cloud.at(i).x;
+			ptr[1] = cloud.at(i).y;
+			ptr[2] = cloud.at(i).z;
+			ptr[4] = cloud.at(i).normal_x;
+			ptr[5] = cloud.at(i).normal_y;
+			ptr[6] = cloud.at(i).normal_z;
+		}
+		int * ptrInt = (int*)ptr;
+		ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
+	}
+	return laserScan;
+}
+
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -1325,7 +1357,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,

 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)  || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
@@ -1344,7 +1376,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS

 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	output->resize(laserScan.cols);
@@ -1362,7 +1394,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat

 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	output->resize(laserScan.cols);
@@ -1379,10 +1411,28 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
 	return output;
 }

+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	output->resize(laserScan.cols);
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
+		if(!nullTransform)
+		{
+			output->at(i) = util3d::transformPoint(output->at(i), transform);
+		}
+	}
+	return output;
+}
+
 pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZ output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
@@ -1397,7 +1447,7 @@ pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
 pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointNormal output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
@@ -1412,13 +1462,19 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
 		output.normal_y = ptr[4];
 		output.normal_z = ptr[5];
 	}
+	else if(laserScan.channels() == 7)
+	{
+		output.normal_x = ptr[4];
+		output.normal_y = ptr[5];
+		output.normal_z = ptr[6];
+	}
 	return output;
 }

 pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZRGB output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
@@ -1427,7 +1483,47 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
 	{
 		output.z = ptr[2];
 	}
-	if(laserScan.channels() == 4)
+	if(laserScan.channels() == 4 || laserScan.channels() == 7)
+	{
+		int * ptrInt = (int*)ptr;
+		output.b = (unsigned char)(ptrInt[3] & 0xFF);
+		output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF);
+		output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF);
+	}
+	else
+	{
+		output.r = r;
+		output.g = g;
+		output.b = b;
+	}
+	return output;
+}
+
+pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
+{
+	UASSERT(!laserScan.empty() && index < laserScan.cols);
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	pcl::PointXYZRGBNormal output;
+	const float * ptr = laserScan.ptr<float>(0, index);
+	output.x = ptr[0];
+	output.y = ptr[1];
+	if(laserScan.channels() >= 3)
+	{
+		output.z = ptr[2];
+	}
+	if(laserScan.channels() == 6)
+	{
+		output.normal_x = ptr[3];
+		output.normal_y = ptr[4];
+		output.normal_z = ptr[5];
+	}
+	else if(laserScan.channels() == 7)
+	{
+		output.normal_x = ptr[4];
+		output.normal_y = ptr[5];
+		output.normal_z = ptr[6];
+	}
+	if(laserScan.channels() == 4 || laserScan.channels() == 7)
 	{
 		int * ptrInt = (int*)ptr;
 		output.b = (unsigned char)(ptrInt[3] & 0xFF);
@@ -1446,7 +1542,7 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
 void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
 {
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	const float * ptr = laserScan.ptr<float>(0, 0);
 	min.x = max.x = ptr[0];
@@ -1529,7 +1625,7 @@ cv::Mat projectCloudToCamera(
 {
 	UASSERT(!cameraTransform.isNull());
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);

 	float fx = cameraMatrixK.at<double>(0,0);
@@ -1542,7 +1638,9 @@ cv::Mat projectCloudToCamera(

 	const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
 	const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
+	const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
 	const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
+	const float* vec7Ptr = laserScan.ptr<float>();

 	int count = 0;
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1561,12 +1659,24 @@ cv::Mat projectCloudToCamera(
 			ptScan.y = vec3Ptr[i][1];
 			ptScan.z = vec3Ptr[i][2];
 		}
-		else
+		else if(laserScan.type() == CV_32FC(4))
+		{
+			ptScan.x = vec4Ptr[i][0];
+			ptScan.y = vec4Ptr[i][1];
+			ptScan.z = vec4Ptr[i][2];
+		}
+		else if(laserScan.type() == CV_32FC(6))
 		{
 			ptScan.x = vec6Ptr[i][0];
 			ptScan.y = vec6Ptr[i][1];
 			ptScan.z = vec6Ptr[i][2];
 		}
+		else // 7f
+		{
+			ptScan.x = (vec7Ptr+i*7)[0];
+			ptScan.y = (vec7Ptr+i*7)[1];
+			ptScan.z = (vec7Ptr+i*7)[2];
+		}
 		ptScan = util3d::transformPoint(ptScan, t);

 		// re-project in camera frame
--- a/corelib/src/util3d_surface.cpp
+++ b/corelib/src/util3d_surface.cpp
@@ -1194,7 +1194,8 @@ double sqr(uchar v)
 {
 	return double(v)*double(v);
 }
-std::vector<cv::Mat> mergeTextures(
+
+cv::Mat mergeTextures(
 		pcl::TextureMesh & mesh,
 		const std::map<int, cv::Mat> & images,
 		const std::map<int, std::vector<CameraModel> > & calibrations,
@@ -1216,7 +1217,7 @@ std::vector<cv::Mat> mergeTextures(
 	//get texture size, if disabled use default 1024
 	UASSERT(textureSize%256 == 0);
 	UDEBUG("textureSize = %d", textureSize);
-	std::vector<cv::Mat> globalTextures;
+	cv::Mat globalTextures;
 	if(mesh.tex_materials.size() > 1)
 	{
 		std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,-1));
@@ -1346,13 +1347,8 @@ std::vector<cv::Mat> mergeTextures(
 				int cols = float(textureSize)/(scale*imageSize.width);
 				int rows = float(textureSize)/(scale*imageSize.height);

-				std::vector<cv::Mat> globalTextureMasks(materials);
-				globalTextures.resize(materials);
-				for(int i=0; i<materials; ++i)
-				{
-					globalTextures[i] = cv::Mat(textureSize, textureSize, imageType, cv::Scalar::all(255));
-					globalTextureMasks[i] = cv::Mat(textureSize, textureSize, CV_8UC1, cv::Scalar::all(0));
-				}
+				globalTextures = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(255));
+				cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));

 				// used for multi camera texturing, to avoid reloading same texture for sub cameras
 				cv::Mat previousImage;
@@ -1440,13 +1436,13 @@ std::vector<cv::Mat> mergeTextures(
 								cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
 								resizedImage = resizedImageColor;
 							}
-							UASSERT(resizedImage.type() == globalTextures[indexMaterial].type());
-							resizedImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
-							emptyImageMask.copyTo(globalTextureMasks[indexMaterial](cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
+							UASSERT(resizedImage.type() == globalTextures.type());
+							resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
+							emptyImageMask.copyTo(globalTextureMasks(cv::Rect(u+indexMaterial*globalTextureMasks.rows, v, resizedImage.cols, resizedImage.rows)));
 						}
 						else
 						{
-							emptyImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, emptyImage.cols, emptyImage.rows)));
+							emptyImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows)));
 						}
 						++oi;
 					}
@@ -1455,7 +1451,7 @@ std::vector<cv::Mat> mergeTextures(
 					{
 						if(state->isCanceled())
 						{
-							return std::vector<cv::Mat>();
+							return cv::Mat();
 						}
 						state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
 					}
@@ -1510,8 +1506,8 @@ std::vector<cv::Mat> mergeTextures(
 											int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
 											int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
 											int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
-											cv::Vec3b * pt1 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vi,ui);
-											cv::Vec3b * pt2 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vj,uj);
+											cv::Vec3b * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
+											cv::Vec3b * pt2 = globalTextures.ptr<cv::Vec3b>(vj,uj+indexMaterial*globalTextures.rows);

 											I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
 											I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));
@@ -1609,7 +1605,7 @@ std::vector<cv::Mat> mergeTextures(
 								UDEBUG("Gain cam%d = %f", newCamIndex[t], gainsGray(newCamIndex[t], 0));

 								int indexMaterial = newCamIndex[t] / (cols*rows);
-								cv::Mat roi = globalTextures[indexMaterial](cv::Rect(u, v, emptyImage.cols, emptyImage.rows));
+								cv::Mat roi = globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows));

 								std::vector<cv::Mat> channels;
 								cv::split(roi, channels);
@@ -1689,7 +1685,7 @@ std::vector<cv::Mat> mergeTextures(
 						std::vector<cv::Mat> blendGains(materials);
 						for(int i=0; i<materials;++i)
 						{
-							blendGains[i] = cv::Mat(globalTextures[i].rows/decimation, globalTextures[i].cols/decimation, CV_32FC3, cv::Scalar::all(1.0f));
+							blendGains[i] = cv::Mat(globalTextures.rows/decimation, globalTextures.rows/decimation, CV_32FC3, cv::Scalar::all(1.0f));
 						}

 						for(unsigned int p=0; p<vertexToPixels.size(); ++p)
@@ -1715,7 +1711,7 @@ std::vector<cv::Mat> mergeTextures(
 											weight = 0.0f;
 										}
 										int indexMaterial = newCamIndex[iter->first] / (cols*rows);
-										cv::Vec3b * pt = globalTextures[indexMaterial].ptr<cv::Vec3b>(v,u);
+										cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
 										gainsB[k] = static_cast<double>(pt->val[0]) * weight;
 										gainsG[k] = static_cast<double>(pt->val[1]) * weight;
 										gainsR[k] = static_cast<double>(pt->val[2]) * weight;
@@ -1740,7 +1736,7 @@ std::vector<cv::Mat> mergeTextures(
 											int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
 											int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
 											int indexMaterial = newCamIndex[iter->first] / (cols*rows);
-											cv::Vec3b * pt = globalTextures[indexMaterial].ptr<cv::Vec3b>(v,u);
+											cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
 											float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
 											float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
 											float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
@@ -1766,9 +1762,10 @@ std::vector<cv::Mat> mergeTextures(
 							channels[2].convertTo(img,CV_8U,128.0,0);
 							cv::imwrite("blendSmallR.png", img);*/

+							cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
 							cv::Mat dst;
 							cv::blur(blendGains[i], dst, cv::Size(3,3));
-							cv::resize(dst, blendGains[i], globalTextures[i].size(), 0, 0, cv::INTER_LINEAR);
+							cv::resize(dst, blendGains[i], globalTexturesROI.size(), 0, 0, cv::INTER_LINEAR);

 							/*cv::split(blendGains, channels);
 							channels[0].convertTo(img,CV_8U,128.0,0);
@@ -1778,7 +1775,7 @@ std::vector<cv::Mat> mergeTextures(
 							channels[2].convertTo(img,CV_8U,128.0,0);
 							cv::imwrite("blendFullR.png", img);*/

-							cv::multiply(globalTextures[i], blendGains[i], globalTextures[i], 1.0, CV_8UC3);
+							cv::multiply(globalTexturesROI, blendGains[i], globalTexturesROI, 1.0, CV_8UC3);

 							//UWARN("Saving blending.png", globalTexture);
 							//cv::imwrite("blending.png", globalTexture);
@@ -1792,36 +1789,38 @@ std::vector<cv::Mat> mergeTextures(
 				{
 					for(int i=0; i<materials; ++i)
 					{
+						cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
+						cv::Mat globalTextureMasksROI = globalTextureMasks(cv::Range::all(), cv::Range(i*globalTextureMasks.rows, (i+1)*globalTextureMasks.rows));
 						if(exposureFusion)
 						{
 							std::vector<cv::Mat> images;
-							images.push_back(globalTextures[i]);
+							images.push_back(globalTexturesROI);
 							if (brightnessContrastRatioLow > 0)
 							{
 								images.push_back(util2d::brightnessAndContrastAuto(
-									globalTextures[i],
-									globalTextureMasks[i],
+									globalTexturesROI,
+									globalTextureMasksROI,
 									(float)brightnessContrastRatioLow,
 									0.0f));
 							}
 							if (brightnessContrastRatioHigh > 0)
 							{
 								images.push_back(util2d::brightnessAndContrastAuto(
-									globalTextures[i],
-									globalTextureMasks[i],
+									globalTexturesROI,
+									globalTextureMasksROI,
 									0.0f,
 									(float)brightnessContrastRatioHigh));
 							}

-							globalTextures[i] = util2d::exposureFusion(images);
+							util2d::exposureFusion(images).copyTo(globalTexturesROI);
 						}
 						else
 						{
-							globalTextures[i] = util2d::brightnessAndContrastAuto(
-								globalTextures[i],
-								globalTextureMasks[i],
+							util2d::brightnessAndContrastAuto(
+								globalTexturesROI,
+								globalTextureMasksROI,
 								(float)brightnessContrastRatioLow,
-								(float)brightnessContrastRatioHigh);
+								(float)brightnessContrastRatioHigh).copyTo(globalTexturesROI);
 						}
 					}
 					if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
@@ -1829,7 +1828,7 @@ std::vector<cv::Mat> mergeTextures(
 			}
 		}
 	}
-	UDEBUG("globalTextures=%d", (int)globalTextures.size());
+	UDEBUG("globalTextures=%d", globalTextures.cols / globalTextures.rows);
 	return globalTextures;
 }

--- a/corelib/src/util3d_transforms.cpp
+++ b/corelib/src/util3d_transforms.cpp
@@ -208,6 +208,23 @@ pcl::PointNormal transformPoint(
 	ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
 	return ret;
 }
+pcl::PointXYZRGBNormal transformPoint(
+		const pcl::PointXYZRGBNormal & point,
+		const Transform & transform)
+{
+	pcl::PointXYZRGBNormal ret;
+	Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
+	ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
+	ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
+	ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
+
+	// Rotate normals
+	Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
+	ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
+	ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
+	ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
+	return ret;
+}

 }