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.

app/android/jni/RTABMapApp.cpp

@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/VWDictionary.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/GainCompensator.h>
+#include <rtabmap/core/DBDriver.h>
 #include <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -174,6 +175,7 @@ RTABMapApp::RTABMapApp() :
 		filterPolygonsOnNextRender_(false),
 		gainCompensationOnNextRender_(0),
 		bilateralFilteringOnNextRender_(false),
+		takeScreenshotOnNextRender_(false),
 		cameraJustInitialized_(false),
 		meshDecimation_(1),
 		totalPoints_(0),
@@ -271,14 +273,14 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	camera_->setScreenRotation(rotation);
 }
 
-int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize)
+int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource)
 {
 	LOGI("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0);
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
-	openingDatabase_ = true;
 	rtabmapMutex_.lock();
 	rtabmapEvents_.clear();
+	openingDatabase_ = true;
 	if(rtabmapThread_)
 	{
 		rtabmapThread_->close(false);
@@ -287,6 +289,165 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		rtabmap_ = 0;
 	}
 
+	int status = 0;
+
+	// Open visualization while we load (if there is an optimized mesh saved in database)
+	exportedMesh_.reset(new pcl::TextureMesh);
+	exportedTexture_ = cv::Mat();
+	cv::Mat cloudMat;
+	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	cv::Mat textures;
+	std::map<int, rtabmap::Transform> optPoses;
+	if(!databaseSource.empty())
+	{
+		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized mesh..."));
+		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
+		if(driver->openConnection(databaseSource))
+		{
+			cloudMat = driver->loadOptimizedMesh(&optPoses, &polygons, &exportedMesh_->tex_coordinates, &textures);
+			if(!cloudMat.empty())
+			{
+				LOGI("Open: Found optimized mesh! Visualizing it.");
+				if(cloudMat.channels() <= 3)
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(cloudMat);
+					pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
+				}
+				else if(cloudMat.channels() == 4)
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(cloudMat);
+					pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
+				}
+				else if(cloudMat.channels() == 6)
+				{
+					pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(cloudMat);
+					pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
+				}
+				else if(cloudMat.channels() == 7)
+				{
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(cloudMat);
+					pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
+				}
+
+				if(exportedMesh_->cloud.data.size())
+				{
+					status = 1;
+				}
+
+				if(exportedMesh_->cloud.data.size() && polygons.size())
+				{
+					status = 2;
+					exportedMesh_->tex_polygons.resize(polygons.size());
+					for(unsigned int t=0; t<polygons.size(); ++t)
+					{
+						exportedMesh_->tex_polygons[t].resize(polygons[t].size());
+						for(unsigned int p=0; p<polygons[t].size(); ++p)
+						{
+							exportedMesh_->tex_polygons[t][p].vertices = polygons[t][p];
+						}
+					}
+
+					if(!exportedMesh_->tex_coordinates.empty())
+					{
+						status = 3;
+						UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)exportedMesh_->tex_coordinates.size());
+						if(textures.cols/textures.rows == 1)
+						{
+							exportedTexture_ = textures;
+						}
+						else if(textures.cols/textures.rows > 1)
+						{
+							// Visualization doesn't support more than one material, so concatenate to 1
+							std::vector<bool> materialsKept;
+							float scale = 0.0f;
+							cv::Size imageSize(textures.rows, textures.rows);
+							int imageType = CV_8UC3;
+							rtabmap::util3d::concatenateTextureMaterials(*exportedMesh_, imageSize, textures.rows, 1, scale, &materialsKept);
+							if(scale && exportedMesh_->tex_materials.size() == 1)
+							{
+								int cols = float(textures.rows)/(scale*imageSize.width);
+								int rows = float(textures.rows)/(scale*imageSize.height);
+
+								exportedTexture_ = cv::Mat(textures.rows, textures.rows, imageType, cv::Scalar::all(255));
+
+								// make a blank texture
+								cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
+								int oi=0;
+								for(int i=0; i<(int)materialsKept.size(); ++i)
+								{
+									if(materialsKept.at(i))
+									{
+										int u = oi%cols * resizedImageSize.width;
+										int v = ((oi/cols) % rows ) * resizedImageSize.height;
+										UASSERT(u < textures.rows-resizedImageSize.width);
+										UASSERT(v < textures.rows-resizedImageSize.height);
+
+										cv::Mat resizedImage;
+										cv::resize(textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
+
+										UASSERT(resizedImage.type() == exportedTexture_.type());
+										resizedImage.copyTo(exportedTexture_(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
+
+										++oi;
+									}
+								}
+							}
+						}
+
+						exportedMesh_->tex_materials.resize (exportedMesh_->tex_coordinates.size () + 1);
+						for(unsigned int i = 0 ; i <= exportedMesh_->tex_coordinates.size() ; ++i)
+						{
+							pcl::TexMaterial mesh_material;
+							mesh_material.tex_Ka.r = 0.2f;
+							mesh_material.tex_Ka.g = 0.2f;
+							mesh_material.tex_Ka.b = 0.2f;
+
+							mesh_material.tex_Kd.r = 0.8f;
+							mesh_material.tex_Kd.g = 0.8f;
+							mesh_material.tex_Kd.b = 0.8f;
+
+							mesh_material.tex_Ks.r = 1.0f;
+							mesh_material.tex_Ks.g = 1.0f;
+							mesh_material.tex_Ks.b = 1.0f;
+
+							mesh_material.tex_d = 1.0f;
+							mesh_material.tex_Ns = 75.0f;
+							mesh_material.tex_illum = 2;
+
+							std::stringstream tex_name;
+							tex_name << "material_" << i;
+							tex_name >> mesh_material.tex_name;
+
+							mesh_material.tex_file = uFormat("%d", i);
+
+							exportedMesh_->tex_materials[i] = mesh_material;
+						}
+					}
+				}
+			}
+			else
+			{
+				LOGI("Open: No optimized mesh found.");
+			}
+			delete driver;
+		}
+	}
+
+	if(status > 0)
+	{
+		boost::mutex::scoped_lock  lockRender(renderingMutex_);
+		visualizingMesh_ = true;
+		exportedMeshUpdated_ = true;
+	}
+
+	LOGI("Erasing database \"%s\"...", databasePath.c_str());
+	UFile::erase(databasePath);
+	if(!databaseSource.empty())
+	{
+		LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str());
+		UFile::copy(databaseSource, databasePath);
+	}
+
 	this->registerToEventsManager();
 
 	//Rtabmap
@@ -316,7 +477,6 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			true,
 			true);
 
-	int status = 0;
 	if(signatures.size() && poses.empty())
 	{
 		LOGE("Failed to optimize the graph!");
@@ -329,7 +489,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		createdMeshes_.clear();
 		int i=0;
 		UTimer addTime;
-		for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status==0; ++iter)
+		for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status>=0; ++iter)
 		{
 			try
 			{
@@ -386,6 +546,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 									}
 									LOGI("Created cloud %d (%fs)", id, timer.ticks());
 								}
+								else
+								{
+									LOGI("Cloud %d not added to created meshes", id);
+								}
+							}
+							else
+							{
+								UWARN("Cloud %d is empty", id);
 							}
 						}
 						else
@@ -404,6 +572,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							processMemoryUsedBytes +=s.getWordsDescriptors().size()*(4+s.getWordsDescriptors().begin()->second.total());
 						}
 					}
+					else
+					{
+						UWARN("Data for node %d not found", id);
+					}
+				}
+				else
+				{
+					UWARN("Pose %d is null !?", id);
 				}
 				++i;
 				if(addTime.elapsed() >= 4.0f)
@@ -428,14 +604,19 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 				status = -2;
 			}
 		}
+		if(status < 0)
+		{
+			createdMeshes_.clear();
+		}
+		else
+		{
+			LOGI("Created %d meshes...", (int)createdMeshes_.size());
+		}
 	}
 
-	if(status < 0)
-	{
-		createdMeshes_.clear();
-	}
 
-	if(optimize && status==0)
+
+	if(optimize && status>=0)
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Visual optimization..."));
 		gainCompensation();
@@ -477,11 +658,12 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	rtabmapMutex_.unlock();
 
 	boost::mutex::scoped_lock  lockRender(renderingMutex_);
-	if(poses.empty())
+	if(poses.empty() || status>0)
 	{
 		openingDatabase_ = false;
 	}
-	clearSceneOnNextRender_ = true;
+
+	clearSceneOnNextRender_ = status<=0;
 
 	return status;
 }
@@ -868,7 +1050,7 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-		UASSERT(camera_!=0 && rtabmap_!=0);
+		UASSERT(camera_!=0);
 
 		UTimer fpsTime;
 #ifdef DEBUG_RENDERING_PERFORMANCE
@@ -876,14 +1058,14 @@ int RTABMapApp::Render()
 #endif
 		boost::mutex::scoped_lock  lock(renderingMutex_);
 
+		bool notifyDataLoaded = false;
+		bool notifyCameraStarted = false;
+
 		if(clearSceneOnNextRender_)
 		{
 			visualizingMesh_ = false;
 		}
 
-		bool notifyDataLoaded = false;
-		bool notifyCameraStarted = false;
-
 		// process only pose events in visualization mode
 		rtabmap::Transform pose;
 		{
@@ -939,6 +1121,7 @@ int RTABMapApp::Render()
 			}
 			if(!main_scene_.hasCloud(g_exportedMeshId))
 			{
+				LOGI("Adding optimized mesh to opengl...");
 				if(exportedMesh_->tex_polygons.size() && exportedMesh_->tex_polygons[0].size())
 				{
 					Mesh mesh;
@@ -1020,6 +1203,7 @@ int RTABMapApp::Render()
 
 			if(clearSceneOnNextRender_)
 			{
+				LOGI("Clearing all rendering data...");
 				odomMutex_.lock();
 				odomEvents_.clear();
 				odomMutex_.unlock();
@@ -1033,6 +1217,7 @@ int RTABMapApp::Render()
 				if(!openingDatabase_)
 				{
 					boost::mutex::scoped_lock  lock(meshesMutex_);
+					LOGI("Clearing  meshes...");
 					createdMeshes_.clear();
 				}
 				else
@@ -1061,6 +1246,8 @@ int RTABMapApp::Render()
 				{
 					LOGI("added (%d) != meshes (%d)", (int)added.size(), meshes);
 					processGPUMemoryUsedBytes = 0;
+					boost::mutex::scoped_lock  lockRtabmap(rtabmapMutex_);
+					UASSERT(rtabmap_!=0);
 					for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 					{
 						if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull())
@@ -1475,7 +1662,34 @@ int RTABMapApp::Render()
 			}
 		}
 
-		if(openingDatabase_ || exporting_ || postProcessing_)
+		if(takeScreenshotOnNextRender_)
+		{
+			takeScreenshotOnNextRender_ = false;
+			int w = main_scene_.getViewPortWidth();
+			int h = main_scene_.getViewPortHeight();
+			cv::Mat image(h, w, CV_8UC4);
+			glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
+			cv::flip(image, image, 0);
+			cv::cvtColor(image, image, CV_RGBA2BGRA);
+			cv::Mat roi;
+			if(w>h)
+			{
+				int offset = (w-h)/2;
+				roi = image(cv::Range::all(), cv::Range(offset,offset+h));
+			}
+			else
+			{
+				int offset = (h-w)/2;
+				roi = image(cv::Range(offset,offset+w), cv::Range::all());
+			}
+			rtabmapMutex_.lock();
+			LOGI("Saving screenshot %dx%d...", roi.cols, roi.rows);
+			rtabmap_->getMemory()->savePreviewImage(roi);
+			rtabmapMutex_.unlock();
+			screenshotReady_.release();
+		}
+
+		if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_)
 		{
 			// throttle rendering max 5Hz if we are doing some processing
 			double renderTime = fpsTime.elapsed();
@@ -1846,10 +2060,17 @@ void RTABMapApp::resetMapping()
 
 void RTABMapApp::save(const std::string & databasePath)
 {
+	LOGI("Saving database to %s", databasePath.c_str());
 	rtabmapThread_->join(true);
 
-	// save mapping parameters in the database
+	LOGI("Taking screenshot...");
+	takeScreenshotOnNextRender_ = true;
+	if(!screenshotReady_.acquire(1, 2000))
+	{
+		UERROR("Failed to take a screenshot after 2 sec!");
+	}
 
+	// save mapping parameters in the database
 	bool appendModeBackup = appendMode_;
 	if(appendMode_)
 	{
@@ -1962,7 +2183,7 @@ bool RTABMapApp::exportMesh(
 			pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
 			pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
 			std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
-			std::vector<cv::Mat> globalTextures;
+			cv::Mat globalTextures;
 			int totalPolygons = 0;
 			{
 				if(optimized)
@@ -2420,10 +2641,11 @@ bool RTABMapApp::exportMesh(
 						return false;
 					}
 
-					LOGD("Saving texture(s) (%d)", (int)globalTextures.size());
+					LOGD("Saving texture(s) (%d)", globalTextures.empty()?0:globalTextures.cols/globalTextures.rows);
 					std::string baseName = uSplit(UFile::getName(filePath), '.').front();
 					std::string textureDirectory = UDirectory::getDir(filePath);
-					UASSERT(textureMesh->tex_materials.size() == globalTextures.size());
+					UASSERT(globalTextures.empty() || globalTextures.cols % globalTextures.rows == 0);
+					UASSERT((int)textureMesh->tex_materials.size() == globalTextures.cols/globalTextures.rows);
 					for(unsigned int i=0; i<textureMesh->tex_materials.size(); ++i)
 					{
 						std::string baseNameNum = baseName;
@@ -2434,13 +2656,13 @@ bool RTABMapApp::exportMesh(
 						std::string fullPath = textureDirectory+UDirectory::separator()+baseNameNum+".jpg";
 						textureMesh->tex_materials[i].tex_file = baseNameNum+".jpg";
 						LOGI("Saving texture to %s.", fullPath.c_str());
-						if(!cv::imwrite(fullPath, globalTextures[i]))
+						if(!cv::imwrite(fullPath, globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows))))
 						{
 							LOGI("Failed saving %s!", fullPath.c_str());
 						}
 						else
 						{
-							LOGI("Saved %s (%d bytes).", fullPath.c_str(), globalTextures[i].total()*globalTextures[i].channels());
+							LOGI("Saved %s.", fullPath.c_str());
 						}
 					}
 				}
@@ -2471,6 +2693,22 @@ bool RTABMapApp::exportMesh(
 							exportedMesh_.reset(new pcl::TextureMesh);
 							exportedMesh_->cloud = polygonMesh->cloud;
 							exportedMesh_->tex_polygons.push_back(polygonMesh->polygons);
+
+							// save in database
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+							pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
+							cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database
+							std::vector<std::vector<std::vector<unsigned int> > > polygons(exportedMesh_->tex_polygons.size());
+							for(unsigned int t=0; t<exportedMesh_->tex_polygons.size(); ++t)
+							{
+								polygons[t].resize(exportedMesh_->tex_polygons[t].size());
+								for(unsigned int p=0; p<exportedMesh_->tex_polygons[t].size(); ++p)
+								{
+									polygons[t][p] = exportedMesh_->tex_polygons[t][p].vertices;
+								}
+							}
+							boost::mutex::scoped_lock  lock(rtabmapMutex_);
+							rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
 						}
 						else
 						{
@@ -2483,6 +2721,7 @@ bool RTABMapApp::exportMesh(
 					// With Sketchfab, the OBJ models are rotated 90 degrees on x axis, so rotate -90 to have model in right position
 					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database
 					pcl::PCLPointCloud2 tmp = textureMesh->cloud;
 					cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform(1,0,0,0, 0,0,1,0, 0,-1,0,0));
 					pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
@@ -2493,16 +2732,32 @@ bool RTABMapApp::exportMesh(
 					{
 						LOGI("Saved obj to %s!", filePath.c_str());
 						exportedMesh_ = textureMesh;
-						if(globalTextures.size() == 1)
+
+						// save in database
 						{
-							exportedTexture_ = globalTextures[0];
+							std::vector<std::vector<std::vector<unsigned int> > > polygons(exportedMesh_->tex_polygons.size());
+							for(unsigned int t=0; t<exportedMesh_->tex_polygons.size(); ++t)
+							{
+								polygons[t].resize(exportedMesh_->tex_polygons[t].size());
+								for(unsigned int p=0; p<exportedMesh_->tex_polygons[t].size(); ++p)
+								{
+									polygons[t][p] = exportedMesh_->tex_polygons[t][p].vertices;
+								}
+							}
+							boost::mutex::scoped_lock  lock(rtabmapMutex_);
+							rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons, exportedMesh_->tex_coordinates, globalTextures);
 						}
-						else if(globalTextures.size() > 1)
+
+						if(globalTextures.cols/globalTextures.rows == 1)
+						{
+							exportedTexture_ = globalTextures;
+						}
+						else if(globalTextures.cols/globalTextures.rows > 1)
 						{
 							// Visualization doesn't support more than one material, so concatenate to 1
 							std::vector<bool> materialsKept;
 							float scale = 0.0f;
-							cv::Size imageSize = globalTextures[0].size();
+							cv::Size imageSize(globalTextures.rows, globalTextures.rows);
 							int imageType = CV_8UC3;
 							rtabmap::util3d::concatenateTextureMaterials(*exportedMesh_, imageSize, textureSize, 1, scale, &materialsKept);
 							if(scale && exportedMesh_->tex_materials.size() == 1)
@@ -2515,7 +2770,7 @@ bool RTABMapApp::exportMesh(
 								// make a blank texture
 								cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
 								int oi=0;
-								for(int i=0; i<(int)globalTextures.size(); ++i)
+								for(int i=0; i<(int)materialsKept.size(); ++i)
 								{
 									if(materialsKept.at(i))
 									{
@@ -2525,7 +2780,7 @@ bool RTABMapApp::exportMesh(
 										UASSERT(v < textureSize-resizedImageSize.height);
 
 										cv::Mat resizedImage;
-										cv::resize(globalTextures[i], resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
+										cv::resize(globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
 
 										UASSERT(resizedImage.type() == exportedTexture_.type());
 										resizedImage.copyTo(exportedTexture_(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
@@ -2664,6 +2919,14 @@ bool RTABMapApp::exportMesh(
 				if(success)
 				{
 					LOGI("Saved ply to %s!", filePath.c_str());
+
+					// save in database
+					{
+						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database
+						boost::mutex::scoped_lock  lock(rtabmapMutex_);
+						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses);
+					}
+
 					mergedClouds->clear();
 					exportedMesh_.reset(new pcl::TextureMesh);
 					exportedMesh_->cloud = mesh.cloud;

app/android/jni/RTABMapApp.h

@@ -56,7 +56,7 @@ class RTABMapApp : public UEventsHandler {
 
   void setScreenRotation(int displayRotation, int cameraRotation);
 
-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
 
   bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
@@ -218,6 +218,7 @@ class RTABMapApp : public UEventsHandler {
   bool filterPolygonsOnNextRender_;
   int gainCompensationOnNextRender_;
   bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
   bool cameraJustInitialized_;
   int meshDecimation_;
   int totalPoints_;
@@ -233,6 +234,7 @@ class RTABMapApp : public UEventsHandler {
   bool exportedMeshUpdated_;
   pcl::TextureMesh::Ptr exportedMesh_;
   cv::Mat exportedTexture_;
+  std::map<int, rtabmap::Transform> exportedPoses_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
@@ -250,6 +252,8 @@ class RTABMapApp : public UEventsHandler {
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;
 
+	USemaphore screenshotReady_;
+
 	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 

app/android/jni/jni_interface.cpp

@@ -71,6 +71,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC, databaseInMemory, optimize);
 }
 
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
+    JNIEnv* env, jobject, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+{
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	std::string databaseSourceC;
+	GetJStringContent(env,databaseSource,databaseSourceC);
+	return app.openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+}
+
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
 		JNIEnv* env, jobject, jobject iBinder) {

app/android/jni/scene.h

@@ -57,6 +57,8 @@ class Scene {
 
   // Setup GL view port.
   void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}
 
   void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 

app/android/jni/util.h

@@ -44,8 +44,8 @@ class LogHandler : public UEventsHandler
 public:
 	LogHandler()
 	{
-		ULogger::setLevel(ULogger::kWarning);
+		ULogger::setLevel(ULogger::kDebug);
-		ULogger::setEventLevel(ULogger::kWarning);
+		ULogger::setEventLevel(ULogger::kDebug);
 		ULogger::setPrintThreadId(true);
 
 		registerToEventsManager();

app/android/res/layout/database_list.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+android:layout_width="match_parent"
+android:layout_height="wrap_content"
+android:background="#fff">
+
+<ImageView
+android:id="@+id/imageView"
+android:layout_width="150dp"
+android:layout_height="150dp"
+android:layout_marginLeft="10dp"
+android:padding="5dp"
+android:src="@drawable/ic_launcher" />
+
+<TextView
+android:id="@+id/textView"
+android:layout_width="wrap_content"
+android:layout_height="wrap_content"
+android:layout_gravity="center"
+android:text="Demo"
+android:textColor="#000" />
+</LinearLayout>

app/android/src/com/introlab/rtabmap/DatabaseListArrayAdapter.java

@@ -0,0 +1,89 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.database.Cursor;
+import android.database.sqlite.SQLiteDatabase;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.util.Log;
+import android.view.LayoutInflater;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.BaseAdapter;
+import android.widget.ImageView;
+import android.widget.SimpleAdapter;
+
+import java.io.ByteArrayInputStream;
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+public class DatabaseListArrayAdapter extends SimpleAdapter {
+	LayoutInflater inflater;
+	Context context;
+	ArrayList<HashMap<String, String>> arrayList;
+
+	public DatabaseListArrayAdapter(Context context, ArrayList<HashMap<String, String>> data, int resource, String[] from, int[] to) {
+		super(context, data, resource, from, to);
+		this.context = context;
+		this.arrayList = data;
+		inflater.from(context);
+	}
+
+	@Override
+	public View getView(final int position, View convertView, ViewGroup parent) {
+		View view = super.getView(position, convertView, parent);
+		ImageView imageView = (ImageView) view.findViewById(R.id.imageView);
+		
+		String path = this.arrayList.get(position).get("path");
+		if(!path.isEmpty())
+		{
+			SQLiteDatabase db = null;
+			try {
+				db = SQLiteDatabase.openDatabase(path, null, SQLiteDatabase.OPEN_READONLY);
+				
+				// get version
+				Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);    
+		        if(c1.moveToFirst()) {
+		        	String version = c1.getString(c1.getColumnIndex("version"));
+		        	Log.i(RTABMapActivity.TAG, "Version="+version);
+		        	if(Util.versionCompare(version, "0.12.0") >= 0) {
+		        		Cursor c2 = db.rawQuery("SELECT preview_image FROM Admin WHERE preview_image is not null", null); 
+		        		if(c2.moveToFirst()) {
+		        			Log.i(RTABMapActivity.TAG, "Found image preview for db " + path);
+
+		        			byte[] bytes = c2.getBlob(c2.getColumnIndex("preview_image"));
+		        			ByteArrayInputStream inputStream = new ByteArrayInputStream(bytes);
+		        			Bitmap bitmap = BitmapFactory.decodeStream(inputStream);
+		        			imageView.setImageBitmap(bitmap);
+		                    
+		        		}
+		        		else {
+		        			Log.i(RTABMapActivity.TAG, "Not found image preview for db " + path);
+		        		}
+		        	}
+		        	else {
+		        		Log.i(RTABMapActivity.TAG, "Too old database for preview image, path = " + path);
+		        	}
+		        }
+		        else {
+		        	Log.e(RTABMapActivity.TAG, "Failed getting version from database");
+		        }
+	
+	        } catch (Exception e) {
+	            Log.e(RTABMapActivity.TAG, e.getMessage());
+	        }
+			finally {
+				if(db != null && db.isOpen()) {
+	            	db.close();
+	            }
+			}
+		}
+		else
+		{
+			Log.e(RTABMapActivity.TAG, "Database path empty for item " + position);
+		}
+		return view;
+	}
+
+}

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -12,8 +12,10 @@ import java.io.OutputStream;
 import java.lang.reflect.InvocationTargetException;
 import java.lang.reflect.Method;
 import java.text.SimpleDateFormat;
+import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Date;
+import java.util.HashMap;
 import java.util.List;
 import java.util.zip.ZipEntry;
 import java.util.zip.ZipOutputStream;
@@ -38,6 +40,8 @@ import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
 import android.content.res.Configuration;
+import android.database.Cursor;
+import android.database.sqlite.SQLiteDatabase;
 import android.graphics.Bitmap;
 import android.graphics.Canvas;
 import android.graphics.Color;
@@ -133,7 +137,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private static enum State {
 		STATE_IDLE,
 		STATE_PROCESSING,
-		STATE_VISUALIZING
+		STATE_VISUALIZING,
+		STATE_VISUALIZING_WHILE_LOADING
 	}
 	State mState = State.STATE_IDLE;
 
@@ -1264,6 +1269,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mButtonLighting.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
 			mButtonWireframe.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
 			mButtonCloseVisualization.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
+			mButtonCloseVisualization.setEnabled(true);
 			mButtonSaveOnDevice.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
 			mButtonShareOnSketchfab.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
 			mItemSave.setEnabled(mButtonPause.isChecked());
@@ -1276,6 +1282,22 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mButtonPause.setVisibility(View.INVISIBLE);
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
 			break;
+		case STATE_VISUALIZING_WHILE_LOADING:
+			mButtonLighting.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
+			mButtonWireframe.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
+			mButtonCloseVisualization.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
+			mButtonCloseVisualization.setEnabled(false);
+			mButtonSaveOnDevice.setVisibility(View.INVISIBLE);
+			mButtonShareOnSketchfab.setVisibility(View.INVISIBLE);
+			mItemSave.setEnabled(false);
+			mItemExport.setEnabled(false);
+			mItemOpen.setEnabled(false);
+			mItemPostProcessing.setEnabled(false);
+			mItemSettings.setEnabled(false);
+			mItemReset.setEnabled(false);
+			mItemModes.setEnabled(false);
+			mButtonPause.setVisibility(View.INVISIBLE);
+			break;
 		default:
 			mButtonLighting.setVisibility(View.INVISIBLE);
 			mButtonWireframe.setVisibility(View.INVISIBLE);
@@ -1357,6 +1379,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	}
 
 	public boolean onOptionsItemSelected(MenuItem item) {
+		resetNoTouchTimer();
 		if(!DISABLE_LOG) Log.i(TAG, "called onOptionsItemSelected; selected item: " + item);
 		int itemId = item.getItemId();
 		if (itemId == R.id.post_processing_standard)
@@ -1775,9 +1798,22 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					long mb = filePath.length()/(1024*1024);
 					filesWithSize[i] = files[i] + " ("+mb+" MB)";
 				}
+				
+				ArrayList<HashMap<String, String> > arrayList = new ArrayList<HashMap<String, String> >();
+		        for (int i = 0; i < filesWithSize.length; i++) {
+		            HashMap<String, String> hashMap = new HashMap<String, String>();//create a hashmap to store the data in key value pair
+		            hashMap.put("name", filesWithSize[i]);
+		            hashMap.put("path", mWorkingDirectory + files[i]);
+		            arrayList.add(hashMap);//add the hashmap into arrayList
+		        }
+		        String[] from = {"name", "path"};//string array
+		        int[] to = {R.id.textView, R.id.imageView};//int array of views id's
+		        DatabaseListArrayAdapter simpleAdapter = new DatabaseListArrayAdapter(this, arrayList, R.layout.database_list, from, to);//Create object and set the parameters for simpleAdapter
+
 				AlertDialog.Builder builder = new AlertDialog.Builder(this);
 				builder.setTitle("Choose Your File (*.db)");
-				builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
+				builder.setAdapter(simpleAdapter, new DialogInterface.OnClickListener() {
+				//builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
 					public void onClick(DialogInterface dialog, final int which) {
 												
 						// Adjust color now?
@@ -1912,7 +1948,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 									public void onClick(DialogInterface dialog, int which) {
 										mExportedOBJ = isOBJ;
 										resetNoTouchTimer();
-										mSavedRenderingType = !meshing?0:!isOBJ?1:2;
+										mSavedRenderingType = mItemRenderingPointCloud.isChecked()?0:mItemRenderingMesh.isChecked()?1:2;
 										if(!meshing)
 										{
 											mItemRenderingPointCloud.setChecked(true);
@@ -2206,6 +2242,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	{
 		mOpenedDatabasePath = mWorkingDirectory + fileName;
 		
+		Log.i(TAG, "Open database " + mOpenedDatabasePath);
+		
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
 		final boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));		
 		
@@ -2218,27 +2256,55 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		Thread openThread = new Thread(new Runnable() {
 			public void run() {
 
-				final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
+				SQLiteDatabase db = null;
-				(new File(tmpDatabase)).delete();
+				try {
-				try{
+					db = SQLiteDatabase.openDatabase(mOpenedDatabasePath, null, SQLiteDatabase.OPEN_READONLY);
-					copy(new File(mOpenedDatabasePath), new File(tmpDatabase));
+					
-				}
+					// get version
-				catch(IOException e)
+					Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);    
-				{
+			        if(c1.moveToFirst()) {
-					mToast.makeText(getActivity(), String.format("Failed to create temp database from %s!", mOpenedDatabasePath), mToast.LENGTH_LONG).show();
+			        	String version = c1.getString(c1.getColumnIndex("version"));
-					updateState(State.STATE_IDLE);
+			        	Log.i(TAG, "Version="+version);
-					mProgressDialog.dismiss();
+			        	if(Util.versionCompare(version, "0.13.0") >= 0) {
-					return;
+			        		Cursor c2 = db.rawQuery("SELECT version FROM Admin WHERE opt_cloud is not null", null); 
+			        		if(c2.moveToFirst()) {
+			        			Log.i(TAG, "Found optimized mesh");
+			        			runOnUiThread(new Runnable() {
+			    					public void run() {
+			    						mProgressDialog.dismiss();
+			    						updateState(State.STATE_VISUALIZING_WHILE_LOADING);
+			    						mToast.makeText(getActivity(), String.format("Optimized mesh detected in the database. It will be shown while the database is loading..."), mToast.LENGTH_LONG).show();
+			    					}
+			        			});
+			        		}
+			        		else
+			        		{
+			        			Log.i(TAG, "Not found optimized mesh");
+			        		}
+			        	}
+			        }
+			        else
+			        {
+			        	Log.e(TAG, "Failed getting version from database");
+			        }
+
+	            } catch (Exception e) {
+	                Log.e(TAG, e.getMessage());
+	            }
+				finally {
+					if(db != null && db.isOpen()) {
+	                	db.close();
+	                }
 				}
 				
-				final int status = RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, optimize);
+				final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;				
+				final int status = RTABMapLib.openDatabase2(mOpenedDatabasePath, tmpDatabase, databaseInMemory, optimize);
 				
 				runOnUiThread(new Runnable() {
 					public void run() {
-						setCamera(1);
-						updateState(State.STATE_IDLE);
 						if(status == -1)
 						{
+							updateState(State.STATE_IDLE);
 							mProgressDialog.dismiss();
 							new AlertDialog.Builder(getActivity())
 							.setCancelable(false)
@@ -2262,6 +2328,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						}
 						else if(status == -2)
 						{
+							updateState(State.STATE_IDLE);
 							mProgressDialog.dismiss();
 							new AlertDialog.Builder(getActivity())
 							.setCancelable(false)
@@ -2283,12 +2350,35 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						}
 						else
 						{
-							// creating meshes...
+							if(status >= 1 && status<=3)
-							if(!mItemTrajectoryMode.isChecked())
 							{
+								mProgressDialog.dismiss();
+								resetNoTouchTimer();
+								mSavedRenderingType = mItemRenderingPointCloud.isChecked()?0:mItemRenderingMesh.isChecked()?1:2;
+								if(status==1)
+								{
+									mItemRenderingPointCloud.setChecked(true);
+								}
+								else if(status==2)
+								{
+									mItemRenderingMesh.setChecked(true);
+								}
+								else // isOBJ
+								{
+									mItemRenderingTextureMesh.setChecked(true);
+								}
+								updateState(State.STATE_VISUALIZING);
+								mToast.makeText(getActivity(), String.format("Database loaded!"), mToast.LENGTH_LONG).show();
+							}
+							else if(!mItemTrajectoryMode.isChecked())
+							{
+								setCamera(1);
+								// creating meshes...
+								updateState(State.STATE_IDLE);
 								mProgressDialog.setTitle("Loading");
 								mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while rendering point clouds and meshes...", fileName));
 							}
+							
 						}
 					}
 				});

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -28,6 +28,7 @@ public class RTABMapLib
     public static native void setScreenRotation(int displayRotation, int cameraRotation);
     
     public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase2(String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
     
     /*
      * Called when the Tango service is connected.

app/android/src/com/introlab/rtabmap/Util.java

@@ -90,4 +90,37 @@ public class Util {
 		return fileList;
 	}
 	
+	/**
+	 * https://stackoverflow.com/questions/6701948/efficient-way-to-compare-version-strings-in-java
+	 * Compares two version strings. 
+	 * 
+	 * Use this instead of String.compareTo() for a non-lexicographical 
+	 * comparison that works for version strings. e.g. "1.10".compareTo("1.6").
+	 * 
+	 * @note It does not work if "1.10" is supposed to be equal to "1.10.0".
+	 * 
+	 * @param str1 a string of ordinal numbers separated by decimal points. 
+	 * @param str2 a string of ordinal numbers separated by decimal points.
+	 * @return The result is a negative integer if str1 is _numerically_ less than str2. 
+	 *         The result is a positive integer if str1 is _numerically_ greater than str2. 
+	 *         The result is zero if the strings are _numerically_ equal.
+	 */
+	public static int versionCompare(String str1, String str2) {
+	    String[] vals1 = str1.split("\\.");
+	    String[] vals2 = str2.split("\\.");
+	    int i = 0;
+	    // set index to first non-equal ordinal or length of shortest version string
+	    while (i < vals1.length && i < vals2.length && vals1[i].equals(vals2[i])) {
+	      i++;
+	    }
+	    // compare first non-equal ordinal number
+	    if (i < vals1.length && i < vals2.length) {
+	        int diff = Integer.valueOf(vals1[i]).compareTo(Integer.valueOf(vals2[i]));
+	        return Integer.signum(diff);
+	    }
+	    // the strings are equal or one string is a substring of the other
+	    // e.g. "1.2.3" = "1.2.3" or "1.2.3" < "1.2.3.4"
+	    return Integer.signum(vals1.length - vals2.length);
+	}
+	
 }

corelib/include/rtabmap/core/DBDriver.h

@@ -98,6 +98,19 @@ public:
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
 	void addStatistics(const Statistics & statistics) const;
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
+	cv::Mat loadOptimizedMesh(
+			std::map<int, Transform> * poses = 0,
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+			cv::Mat * textures = 0) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
@@ -200,6 +213,19 @@ private:
 					const cv::Mat & image) const = 0;
 
 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
+	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	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 = 0;
+	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 = 0;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;

corelib/include/rtabmap/core/Memory.h

@@ -43,6 +43,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 
+namespace pcl
+{
+class TextureMesh;
+}
+
 namespace rtabmap {
 
 class Signature;
@@ -92,6 +97,19 @@ public:
 
 	int cleanup();
 	void saveStatistics(const Statistics & statistics);
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
+	cv::Mat loadOptimizedMesh(
+			std::map<int, Transform> * poses = 0,
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+			cv::Mat * textures = 0) const;
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link, bool addInDatabase = false);

corelib/include/rtabmap/core/Rtabmap.h

@@ -41,6 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <stack>
 #include <set>
 
+namespace pcl
+{
+class TextureMesh;
+}
+
 namespace rtabmap
 {
 

corelib/include/rtabmap/core/util3d.h

@@ -190,14 +190,16 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 		float maxDepth,
 		float minDepth);
 
-// return CV_32FC3
+// return CV_32FC3  (x,y,z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
-// return CV_32FC6
+// return CV_32FC6 (x,y,z,normal_z,normal_y,normalz)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
-// return CV_32FC4
+// return CV_32FC4 (x,y,z,rgb)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
-// return CV_32FC2
+// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
+// return CV_32FC2  (x,y)
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 // For laserScan of type CV_32FC2, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
@@ -205,6 +207,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
 // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
 
 // For laserScan of type CV_32FC2, z is set to null.
 pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
@@ -212,6 +217,9 @@ pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index)
 pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
 // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
 pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
+pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
 
 void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max);
 void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max);

corelib/include/rtabmap/core/util3d_surface.h

@@ -177,9 +177,9 @@ void RTABMAP_EXP concatenateTextureMaterials(
 
 /**
  * Merge all textures in the mesh into "textureCount" textures of size "textureSize".
- * @return merged textures corresponding to new materials set in TextureMesh
+ * @return merged textures corresponding to new materials set in TextureMesh (height=textureSize, width=textureSize*materials)
  */
-std::vector<cv::Mat> RTABMAP_EXP mergeTextures(
+cv::Mat RTABMAP_EXP mergeTextures(
 		pcl::TextureMesh & mesh,
 		const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
 		const std::map<int, std::vector<CameraModel> > & calibrations, // Should match images

corelib/include/rtabmap/core/util3d_transforms.h

@@ -94,6 +94,9 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 pcl::PointNormal RTABMAP_EXP transformPoint(
 		const pcl::PointNormal & point,
 		const Transform & transform);
+pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint(
+		const pcl::PointXYZRGBNormal & point,
+		const Transform & transform);
 
 } // namespace util3d
 } // namespace rtabmap

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,

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)

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;

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)

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>

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
 );
 

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

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 = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(255));
-				globalTextures.resize(materials);
+				cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));
-				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));
-				}
 
 				// 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());
+							UASSERT(resizedImage.type() == globalTextures.type());
-							resizedImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
+							resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
-							emptyImageMask.copyTo(globalTextureMasks[indexMaterial](cv::Rect(u, 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 * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
-											cv::Vec3b * pt2 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vj,uj);
+											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],
+									globalTexturesROI,
-									globalTextureMasks[i],
+									globalTextureMasksROI,
 									(float)brightnessContrastRatioLow,
 									0.0f));
 							}
 							if (brightnessContrastRatioHigh > 0)
 							{
 								images.push_back(util2d::brightnessAndContrastAuto(
-									globalTextures[i],
+									globalTexturesROI,
-									globalTextureMasks[i],
+									globalTextureMasksROI,
 									0.0f,
 									(float)brightnessContrastRatioHigh));
 							}
 
-							globalTextures[i] = util2d::exposureFusion(images);
+							util2d::exposureFusion(images).copyTo(globalTexturesROI);
 						}
 						else
 						{
-							globalTextures[i] = util2d::brightnessAndContrastAuto(
+							util2d::brightnessAndContrastAuto(
-								globalTextures[i],
+								globalTexturesROI,
-								globalTextureMasks[i],
+								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;
 }
 

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;
+}
 
 }