Tango: added progression bar when exporting, averaging colors in color radius when exporting without texture

app/android/jni/RTABMapApp.cpp

@@ -211,6 +211,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	renderingTime_ = 0.0f;
 	processMemoryUsedBytes = 0;
 	processGPUMemoryUsedBytes = 0;
+	progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
 
 	if(camera_)
 	{
@@ -467,7 +468,7 @@ int RTABMapApp::Render()
 
 		bool notifyCameraStarted = false;
 
-		// process only pose events in vsualization mode
+		// process only pose events in visualization mode
 		rtabmap::Transform pose;
 		{
 			boost::mutex::scoped_lock lock(poseMutex_);
@@ -531,14 +532,13 @@ int RTABMapApp::Render()
 					pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.cloud);
 					pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.normals);
 					mesh.polygons = exportedMesh_->tex_polygons[0];
-					cv::Mat texture;
 					if(exportedMesh_->tex_coordinates.size())
 					{
 						mesh.texCoords = exportedMesh_->tex_coordinates[0];
-						texture = exportedTexture_;
+						mesh.texture = exportedTexture_;
 					}
 
-					main_scene_.addMesh(g_exportedMeshId, mesh, texture, opengl_world_T_rtabmap_world);
+					main_scene_.addMesh(g_exportedMeshId, mesh, opengl_world_T_rtabmap_world);
 				}
 				else
 				{
@@ -583,17 +583,21 @@ int RTABMapApp::Render()
 			// should be before clearSceneOnNextRender_ in case openDatabase is called
 			std::list<rtabmap::Statistics> rtabmapEvents;
 			{
-				boost::mutex::scoped_lock  lock(rtabmapMutex_);
+				rtabmapMutex_.lock();
 				rtabmapEvents = rtabmapEvents_;
 				rtabmapEvents_.clear();
+				rtabmapMutex_.unlock();
 
-				boost::mutex::scoped_lock  lockMesh(meshesMutex_);
-				if(!clearSceneOnNextRender_ && rtabmapEvents.size() && createdMeshes_.size())
+				if(!clearSceneOnNextRender_ && rtabmapEvents.size())
 				{
-					if(rtabmapEvents.front().refImageId()>0 && rtabmapEvents.front().refImageId() < createdMeshes_.rbegin()->first)
+					boost::mutex::scoped_lock  lockMesh(meshesMutex_);
+					if(createdMeshes_.size())
 					{
-						LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front().refImageId(), createdMeshes_.rbegin()->first);
-						clearSceneOnNextRender_ = true;
+						if(rtabmapEvents.front().refImageId()>0 && rtabmapEvents.front().refImageId() < createdMeshes_.rbegin()->first)
+						{
+							LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front().refImageId(), createdMeshes_.rbegin()->first);
+							clearSceneOnNextRender_ = true;
+						}
 					}
 				}
 			}
@@ -627,18 +631,24 @@ int RTABMapApp::Render()
 			added.erase(-1);
 			{
 				boost::mutex::scoped_lock  lock(meshesMutex_);
-				if(added.size() != createdMeshes_.size())
+				unsigned int meshes = createdMeshes_.size();
+				if(meshes && createdMeshes_.rbegin()->second.pose.isNull())
+				{
+					meshes -= 1; // buffered mesh
+				}
+				if(added.size() != meshes)
 				{
 					processGPUMemoryUsedBytes = 0;
 					for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 					{
 						if(!main_scene_.hasCloud(iter->first))
 						{
+							LOGI("Re-add mesh %d to OpenGL context", iter->first);
 							if(main_scene_.isMeshRendering() && iter->second.polygons.size() == 0)
 							{
 								iter->second.polygons = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 							}
-							cv::Mat texture;
+
 							if(main_scene_.isMeshTexturing())
 							{
 								cv::Mat textureRaw;
@@ -647,10 +657,10 @@ int RTABMapApp::Render()
 								{
 									cv::Size reducedSize(textureRaw.cols/(textureRaw.cols>1000?4:2), textureRaw.rows/(textureRaw.cols>1000?4:2));
 									LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
-									cv::resize(textureRaw, texture, reducedSize, 0, 0, CV_INTER_AREA);
+									cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_AREA);
 								}
 							}
-							main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
+							main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
 							main_scene_.setCloudVisible(iter->first, iter->second.visible);
 
 							long estimateGPUMem = 0;
@@ -658,7 +668,9 @@ int RTABMapApp::Render()
 							estimateGPUMem += iter->second.indices->size()*4; // int
 							estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
 
-							processGPUMemoryUsedBytes += estimateGPUMem + (texture.empty()?0:iter->second.polygons.size()*3*8+texture.total());
+							processGPUMemoryUsedBytes += estimateGPUMem + (iter->second.texture.empty()?0:iter->second.polygons.size()*3*8+iter->second.texture.total());
+
+							iter->second.texture = cv::Mat(); // don't keep textures in memory
 						}
 					}
 				}
@@ -788,67 +800,74 @@ int RTABMapApp::Render()
 								meshIter->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
 								meshIter->second.visible = true;
 							}
-							else if(uContains(bufferedSensorData, id))
+							else if(uContains(bufferedSensorData, id) || createdMeshes_.find(id) != createdMeshes_.end())
 							{
-								rtabmap::SensorData data = bufferedSensorData.at(id);
-
-								cv::Mat tmpA, depth;
-								data.uncompressData(&tmpA, &depth);
-
-								if(!data.imageRaw().empty() && !data.depthRaw().empty())
+								if(createdMeshes_.find(id) == createdMeshes_.end())
 								{
-									// Voxelize and filter depending on the previous cloud?
-									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-									pcl::IndicesPtr indices(new std::vector<int>);
-									LOGI("Creating node cloud %d (depth=%dx%d rgb=%dx%d)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows);
-									cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
+									rtabmap::SensorData data = bufferedSensorData.at(id);
 
-									if(cloud->size() && indices->size())
+									cv::Mat tmpA, depth;
+									data.uncompressData(&tmpA, &depth);
+
+									if(!data.imageRaw().empty() && !data.depthRaw().empty())
 									{
-										UTimer time;
-										std::vector<pcl::Vertices> polygons;
-										if(main_scene_.isMeshRendering())
-										{
-											polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
-											LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
-										}
+										// Voxelize and filter depending on the previous cloud?
+										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+										pcl::IndicesPtr indices(new std::vector<int>);
+										LOGI("Creating node cloud %d (depth=%dx%d rgb=%dx%d)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows);
+										cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
 
-										if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
+										if(cloud->size() && indices->size())
 										{
-											totalPolygons_ += polygons.size();
-
-											std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
-											UASSERT(inserted.second);
-											inserted.first->second.cloud = cloud;
-											inserted.first->second.indices = indices;
-											inserted.first->second.polygons = polygons;
-											inserted.first->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
-											inserted.first->second.visible = true;
-											inserted.first->second.cameraModel = data.cameraModels()[0];
-											inserted.first->second.gain = 1.0f;
-											cv::Mat texture;
-											if(main_scene_.isMeshTexturing())
+											UTimer time;
+											std::vector<pcl::Vertices> polygons;
+											if(main_scene_.isMeshRendering())
 											{
-												cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
-												LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
-												cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
+												polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+												LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
 											}
-											main_scene_.addMesh(id, inserted.first->second, texture, iter->second);
 
-											long estimateCPUMem = 0;
-											estimateCPUMem += inserted.first->second.cloud->size()*16; // 3*float + 1 float rgb
-											estimateCPUMem += inserted.first->second.indices->size()*4; // int
-											estimateCPUMem += inserted.first->second.polygons.size()*4*3; // 3 indices per polygon
-
-											processMemoryUsedBytes += estimateCPUMem;
-											processGPUMemoryUsedBytes += estimateCPUMem + (texture.empty()?0:inserted.first->second.polygons.size()*3*8+texture.total());
-										}
-										else
-										{
-											UERROR("No mesh could be created for node %d", id);
+											if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
+											{
+												std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
+												UASSERT(inserted.second);
+												inserted.first->second.cloud = cloud;
+												inserted.first->second.indices = indices;
+												inserted.first->second.polygons = polygons;
+												inserted.first->second.visible = true;
+												inserted.first->second.cameraModel = data.cameraModels()[0];
+												inserted.first->second.gain = 1.0f;
+												if(main_scene_.isMeshTexturing())
+												{
+													cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
+													LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
+													cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_AREA);
+												}
+											}
+											else
+											{
+												UERROR("No mesh could be created for node %d", id);
+											}
 										}
 									}
-									totalPoints_+=indices->size();
+								}
+
+								if(createdMeshes_.find(id) != createdMeshes_.end())
+								{
+									Mesh & mesh = createdMeshes_.at(id);
+									totalPoints_+=mesh.indices->size();
+									totalPolygons_ += mesh.polygons.size();
+									mesh.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
+									main_scene_.addMesh(id, mesh, iter->second);
+
+									long estimateCPUMem = 0;
+									estimateCPUMem += mesh.cloud->size()*16; // 3*float + 1 float rgb
+									estimateCPUMem += mesh.indices->size()*4; // int
+									estimateCPUMem += mesh.polygons.size()*4*3; // 3 indices per polygon
+
+									processMemoryUsedBytes += estimateCPUMem;
+									processGPUMemoryUsedBytes += estimateCPUMem + (mesh.texture.empty()?0:mesh.polygons.size()*3*8+mesh.texture.total());
+									mesh.texture = cv::Mat(); // don't keep textures in memory
 								}
 							}
 						}
@@ -997,7 +1016,7 @@ int RTABMapApp::Render()
 					{
 						if(smoothMesh(iter->first, iter->second))
 						{
-							main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
+							main_scene_.updateMesh(iter->first, iter->second);
 						}
 					}
 				}
@@ -1500,6 +1519,13 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
 						}
 						++oi;
 					}
+
+					if(progressionStatus_.isCanceled())
+					{
+						return cv::Mat();
+					}
+
+					progressionStatus_.increment();
 				}
 			}
 			else
@@ -1519,6 +1545,12 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
 	return globalTexture;
 }
 
+void RTABMapApp::cancelProcessing()
+{
+	UWARN("Processing canceled!");
+	progressionStatus_.cancel();
+}
+
 bool RTABMapApp::exportMesh(
 		const std::string & filePath,
 		float cloudVoxelSize,
@@ -1530,7 +1562,7 @@ bool RTABMapApp::exportMesh(
 		bool optimized,
 		float optimizedVoxelSize,
 		int optimizedDepth,
-		float optimizedDecimationFactor,
+		int optimizedMaxPolygons,
 		float optimizedColorRadius,
 		bool optimizedCleanWhitePolygons,
 		bool optimizedColorWhitePolygons, // not yet used
@@ -1553,6 +1585,34 @@ bool RTABMapApp::exportMesh(
 		std::multimap<int, rtabmap::Link> links;
 		rtabmap_->getGraph(poses, links, true, true);
 
+		int totalSteps = 0;
+		totalSteps+=poses.size(); // assemble
+		if(meshing)
+		{
+			if(optimized)
+			{
+				totalSteps += poses.size(); // meshing
+				if(textureSize > 0 && optimizedMaxPolygons > 0)
+				{
+					totalSteps += 1; // decimation
+				}
+
+				totalSteps += 1; // texture/coloring
+
+				if(textureSize > 0)
+				{
+					totalSteps+=poses.size()+1; // texture cameras + apply polygons
+				}
+			}
+			if(textureSize>0)
+			{
+				totalSteps += poses.size()+1; // uncompress and merge textures
+			}
+		}
+		totalSteps += 1; // save file
+
+		progressionStatus_.reset(totalSteps);
+
 		//Assemble the meshes
 		if(meshing) // Mesh or  Texture Mesh
 		{
@@ -1647,6 +1707,16 @@ bool RTABMapApp::exportMesh(
 						{
 							UERROR("Cloud %d not found or empty", iter->first);
 						}
+
+						if(progressionStatus_.isCanceled())
+						{
+							if(blockRendering)
+							{
+								renderingMutex_.unlock();
+							}
+							return false;
+						}
+						progressionStatus_.increment();
 					}
 					LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size());
 
@@ -1661,17 +1731,31 @@ bool RTABMapApp::exportMesh(
 						poisson.reconstruct(*mesh);
 						LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size());
 
+						if(progressionStatus_.isCanceled())
+						{
+							if(blockRendering)
+							{
+								renderingMutex_.unlock();
+							}
+							return false;
+						}
+
+						progressionStatus_.increment(poses.size());
+
 						if(mesh->polygons.size())
 						{
-							if(textureSize > 0 && optimizedDecimationFactor > 0.0f)
+							if(textureSize > 0 && optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size())
 							{
 	#ifndef DISABLE_VTK
 								unsigned int count = mesh->polygons.size();
-								LOGI("Mesh decimation (factor=%f) from %d polygons...", optimizedDecimationFactor, (int)count);
+								float factor = 1.0f-float(optimizedMaxPolygons)/float(count);
+								LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor);
+
+								progressionStatus_.setMax(progressionStatus_.getMax() + optimizedMaxPolygons/10000);
 
 								pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
 								pcl::MeshQuadricDecimationVTK mqd;
-								mqd.setTargetReductionFactor(optimizedDecimationFactor);
+								mqd.setTargetReductionFactor(factor);
 								mqd.setInputMesh(mesh);
 								mqd.process (*output);
 								mesh = output;
@@ -1681,7 +1765,7 @@ bool RTABMapApp::exportMesh(
 								// pcl::MeshQuadricDecimationVTK::performProcessing(pcl::PolygonMesh&): error: undefined reference to 'vtkQuadricDecimation::New()'
 								// pcl::VTKUtils::mesh2vtk(pcl::PolygonMesh const&, vtkSmartPointer<vtkPolyData>&): error: undefined reference to 'vtkFloatArray::New()'
 
-								LOGI("Mesh decimated (factor=%f) from %d to %d polygons (%fs)", optimizedDecimationFactor, count, (int)mesh->polygons.size(), timer.ticks());
+								LOGI("Mesh decimated (factor=%f) from %d to %d polygons (%fs)", factor, count, (int)mesh->polygons.size(), timer.ticks());
 								if(count < mesh->polygons.size())
 								{
 									UWARN("Decimated mesh has more polygons than before!");
@@ -1691,6 +1775,17 @@ bool RTABMapApp::exportMesh(
 	#endif
 							}
 
+							if(progressionStatus_.isCanceled())
+							{
+								if(blockRendering)
+								{
+									renderingMutex_.unlock();
+								}
+								return false;
+							}
+
+							progressionStatus_.increment();
+
 							if(textureSize == 0)
 							{
 								// colored polygon mesh
@@ -1721,10 +1816,22 @@ bool RTABMapApp::exportMesh(
 										}
 										if(kIndices.size())
 										{
-											coloredCloud->at(i).r = mergedClouds->at(kIndices[0]).r;
-											coloredCloud->at(i).g = mergedClouds->at(kIndices[0]).g;
-											coloredCloud->at(i).b = mergedClouds->at(kIndices[0]).b;
-											coloredCloud->at(i).a = mergedClouds->at(kIndices[0]).a;
+											//compute average color
+											int r=0;
+											int g=0;
+											int b=0;
+											int a=0;
+											for(unsigned int j=0; j<kIndices.size(); ++j)
+											{
+												r+=(int)mergedClouds->at(kIndices[j]).r;
+												g+=(int)mergedClouds->at(kIndices[j]).g;
+												b+=(int)mergedClouds->at(kIndices[j]).b;
+												a+=(int)mergedClouds->at(kIndices[j]).a;
+											}
+											coloredCloud->at(i).r = r/kIndices.size();
+											coloredCloud->at(i).g = g/kIndices.size();
+											coloredCloud->at(i).b = b/kIndices.size();
+											coloredCloud->at(i).a = a/kIndices.size();
 											coloredPts.at(i) = true;
 										}
 										else
@@ -1814,6 +1921,9 @@ bool RTABMapApp::exportMesh(
 											cloud->at(v.vertices[j]).normal_x = normal[0];
 											cloud->at(v.vertices[j]).normal_y = normal[1];
 											cloud->at(v.vertices[j]).normal_z = normal[2];
+											cloud->at(v.vertices[j]).r = 255;
+											cloud->at(v.vertices[j]).g = 255;
+											cloud->at(v.vertices[j]).b = 255;
 										}
 									}
 									pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
@@ -1881,9 +1991,19 @@ bool RTABMapApp::exportMesh(
 										mesh,
 										cameraPoses,
 										cameraModels,
-										maxTextureDistance);
+										maxTextureDistance,
+										&progressionStatus_);
 								LOGI("Texturing... done! %fs", timer.ticks());
 
+								if(progressionStatus_.isCanceled())
+								{
+									if(blockRendering)
+									{
+										renderingMutex_.unlock();
+									}
+									return false;
+								}
+
 								// Remove occluded polygons (polygons with no texture)
 								if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons)
 								{
@@ -2125,6 +2245,16 @@ bool RTABMapApp::exportMesh(
 						{
 							UERROR("Mesh not found for mesh %d", iter->first);
 						}
+
+						if(progressionStatus_.isCanceled())
+						{
+							if(blockRendering)
+							{
+								renderingMutex_.unlock();
+							}
+							return false;
+						}
+						progressionStatus_.increment();
 					}
 					if(textureSize == 0)
 					{
@@ -2156,6 +2286,15 @@ bool RTABMapApp::exportMesh(
 					LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size());
 					globalTexture = mergeTextures(*textureMesh, textureSize);
 
+					if(progressionStatus_.isCanceled())
+					{
+						if(blockRendering)
+						{
+							renderingMutex_.unlock();
+						}
+						return false;
+					}
+
 					std::string baseName = uSplit(UFile::getName(filePath), '.').front();
 					std::string textureDirectory = UDirectory::getDir(filePath);
 					std::string fullPath = textureDirectory+UDirectory::separator()+baseName+".jpg";
@@ -2170,6 +2309,16 @@ bool RTABMapApp::exportMesh(
 						LOGI("Saved %s (%d bytes).", fullPath.c_str(), globalTexture.total()*globalTexture.channels());
 					}
 				}
+				if(progressionStatus_.isCanceled())
+				{
+					if(blockRendering)
+					{
+						renderingMutex_.unlock();
+					}
+					return false;
+				}
+
+				progressionStatus_.increment();
 			}
 			if(totalPolygons)
 			{
@@ -2300,6 +2449,16 @@ bool RTABMapApp::exportMesh(
 						*mergedClouds += *transformedCloud;
 					}
 				}
+
+				if(progressionStatus_.isCanceled())
+				{
+					if(blockRendering)
+					{
+						renderingMutex_.unlock();
+					}
+					return false;
+				}
+				progressionStatus_.increment();
 			}
 
 			if(mergedClouds->size())
@@ -2328,6 +2487,8 @@ bool RTABMapApp::exportMesh(
 			}
 		}
 
+		progressionStatus_.finish();
+
 		if(blockRendering)
 		{
 			renderingMutex_.unlock();
@@ -2380,7 +2541,15 @@ int RTABMapApp::postProcessing(int approach)
 		// detect more loop closures
 		if(approach == -1 || approach == 2)
 		{
-			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1);
+			if(approach == -1)
+			{
+				progressionStatus_.reset(6);
+			}
+			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, approach==-1?&progressionStatus_:0);
+			if(approach == -1 && progressionStatus_.isCanceled())
+			{
+				return -1;
+			}
 		}
 
 		// graph optimization
@@ -2479,6 +2648,61 @@ void RTABMapApp::handleEvent(UEvent * event)
 			LOGI("Received RtabmapEvent initialized event!");
 			if(camera_->isRunning())
 			{
+				rtabmap::RtabmapEvent * rtabmapEvent = (rtabmap::RtabmapEvent*)event;
+				int smallMovement = (int)uValue(rtabmapEvent->getStats().data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
+				int rehearsalMerged = (int)uValue(rtabmapEvent->getStats().data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
+				if(rtabmapEvent->getStats().getSignatures().size() &&
+						!trajectoryMode_ &&
+						!dataRecorderMode_ &&
+						!localizationMode_ &&
+						smallMovement == 0 &&
+						rehearsalMerged == 0 &&
+						!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().imageRaw().empty() &&
+						!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().depthRaw().empty())
+				{
+					int id = rtabmapEvent->getStats().getSignatures().rbegin()->first;
+					const rtabmap::SensorData & data = rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData();
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+					pcl::IndicesPtr indices(new std::vector<int>);
+					LOGI("(EVENT) Creating node cloud %d (depth=%dx%d rgb=%dx%d)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows);
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(
+							rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData(),
+							meshDecimation_, maxCloudDepth_, 0, indices.get());
+
+					if(cloud->size() && indices->size())
+					{
+						UTimer time;
+						std::vector<pcl::Vertices> polygons;
+						if(main_scene_.isMeshRendering())
+						{
+							polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+							LOGI("(EVENT) Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
+						}
+
+						if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
+						{
+							cv::Mat texture;
+							if(main_scene_.isMeshTexturing())
+							{
+								cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
+								LOGD("(EVENT) resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
+								cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
+							}
+
+							boost::mutex::scoped_lock  lockMesh(meshesMutex_);
+							std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
+							UASSERT(inserted.second);
+							inserted.first->second.cloud = cloud;
+							inserted.first->second.indices = indices;
+							inserted.first->second.polygons = polygons;
+							inserted.first->second.visible = true;
+							inserted.first->second.cameraModel = data.cameraModels()[0];
+							inserted.first->second.gain = 1.0f;
+							inserted.first->second.texture = texture;
+						}
+					}
+				}
+
 				boost::mutex::scoped_lock lock(rtabmapMutex_);
 				rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
 			}