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

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="38"
+        android:versionCode="39"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />

app/android/jni/ProgressionStatus.h

@@ -0,0 +1,134 @@
+/*
+ * ProgressionStatus.h
+ *
+ *  Created on: Feb 28, 2017
+ *      Author: mathieu
+ */
+
+#ifndef APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+#define APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+
+#include <rtabmap/core/ProgressState.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <jni.h>
+
+namespace rtabmap {
+
+class ProgressEvent : public UEvent
+{
+public:
+	ProgressEvent(int count = 1) : count_(count){}
+	virtual std::string getClassName() const {return "ProgressEvent";}
+
+	int count_;
+};
+
+class ProgressionStatus: public ProgressState, public UEventsHandler
+{
+public:
+	ProgressionStatus() : count_(0), max_(100), canceled_(false), jvm_(0), rtabmap_(0)
+	{
+		registerToEventsManager();
+	}
+
+	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
+	{
+		jvm_ = jvm;
+		rtabmap_ = rtabmap;
+	}
+
+	void reset(int max)
+	{
+		count_=-1;
+		max_ = max;
+		canceled_ = false;
+
+		increment();
+	}
+
+	void setMax(int max)
+	{
+		max_ = max;
+	}
+	int getMax() const {return max_;}
+
+	void increment(int count = 1) const
+	{
+		UEventsManager::post(new ProgressEvent(count));
+	}
+
+	void finish()
+	{
+
+	}
+
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!canceled_)
+		{
+			increment();
+		}
+
+		return !canceled_;
+	}
+	virtual ~ProgressionStatus(){}
+
+	void cancel()
+	{
+		canceled_ = true;
+	}
+
+	bool isCanceled() const
+	{
+		return canceled_;
+	}
+
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("ProgressEvent") == 0)
+		{
+			count_ += ((ProgressEvent*)event)->count_;
+			// Call JAVA callback
+			bool success = false;
+			if(jvm_ && rtabmap_)
+			{
+				JNIEnv *env = 0;
+				jint rs = jvm_->AttachCurrentThread(&env, NULL);
+				if(rs == JNI_OK && env)
+				{
+					jclass clazz = env->GetObjectClass(rtabmap_);
+					if(clazz)
+					{
+						jmethodID methodID = env->GetMethodID(clazz, "updateProgressionCallback", "(II)V" );
+						if(methodID)
+						{
+							env->CallVoidMethod(rtabmap_, methodID,
+									count_,
+									max_);
+							success = true;
+						}
+					}
+				}
+				jvm_->DetachCurrentThread();
+			}
+			if(!success)
+			{
+				UERROR("Failed to call rtabmap::updateProgressionCallback");
+			}
+		}
+	}
+
+private:
+	int count_;
+	int max_;
+	bool canceled_;
+	JavaVM *jvm_;
+	jobject rtabmap_;
+};
+
+}
+
+
+#endif /* APP_ANDROID_JNI_PROGRESSIONSTATUS_H_ */

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())
-				if(!clearSceneOnNextRender_ && rtabmapEvents.size() && createdMeshes_.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);
+						if(rtabmapEvents.front().refImageId()>0 && rtabmapEvents.front().refImageId() < createdMeshes_.rbegin()->first)
-						clearSceneOnNextRender_ = true;
+						{
+							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);
+								if(createdMeshes_.find(id) == createdMeshes_.end())
-
-								cv::Mat tmpA, depth;
-								data.uncompressData(&tmpA, &depth);
-
-								if(!data.imageRaw().empty() && !data.depthRaw().empty())
 								{
-									// Voxelize and filter depending on the previous cloud?
+									rtabmap::SensorData data = bufferedSensorData.at(id);
-									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(cloud->size() && indices->size())
+									cv::Mat tmpA, depth;
+									data.uncompressData(&tmpA, &depth);
+
+									if(!data.imageRaw().empty() && !data.depthRaw().empty())
 									{
-										UTimer time;
+										// Voxelize and filter depending on the previous cloud?
-										std::vector<pcl::Vertices> polygons;
+										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-										if(main_scene_.isMeshRendering())
+										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);
-											polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+										cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
-											LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
-										}
 
-										if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
+										if(cloud->size() && indices->size())
 										{
-											totalPolygons_ += polygons.size();
+											UTimer time;
-
+											std::vector<pcl::Vertices> polygons;
-											std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
+											if(main_scene_.isMeshRendering())
-											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())
 											{
-												cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
+												polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
-												LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
+												LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
-												cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
 											}
-											main_scene_.addMesh(id, inserted.first->second, texture, iter->second);
 
-											long estimateCPUMem = 0;
+											if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
-											estimateCPUMem += inserted.first->second.cloud->size()*16; // 3*float + 1 float rgb
+											{
-											estimateCPUMem += inserted.first->second.indices->size()*4; // int
+												std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
-											estimateCPUMem += inserted.first->second.polygons.size()*4*3; // 3 indices per polygon
+												UASSERT(inserted.second);
-
+												inserted.first->second.cloud = cloud;
-											processMemoryUsedBytes += estimateCPUMem;
+												inserted.first->second.indices = indices;
-											processGPUMemoryUsedBytes += estimateCPUMem + (texture.empty()?0:inserted.first->second.polygons.size()*3*8+texture.total());
+												inserted.first->second.polygons = polygons;
-										}
+												inserted.first->second.visible = true;
-										else
+												inserted.first->second.cameraModel = data.cameraModels()[0];
-										{
+												inserted.first->second.gain = 1.0f;
-											UERROR("No mesh could be created for node %d", id);
+												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;
+											//compute average color
-											coloredCloud->at(i).g = mergedClouds->at(kIndices[0]).g;
+											int r=0;
-											coloredCloud->at(i).b = mergedClouds->at(kIndices[0]).b;
+											int g=0;
-											coloredCloud->at(i).a = mergedClouds->at(kIndices[0]).a;
+											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());
 			}

app/android/jni/RTABMapApp.h

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "scene.h"
 #include "CameraTango.h"
 #include "util.h"
+#include "ProgressionStatus.h"
 
 #include <rtabmap/core/RtabmapThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
@@ -142,6 +143,7 @@ class RTABMapApp : public UEventsHandler {
   void resetMapping();
   void save(const std::string & databasePath);
   cv::Mat mergeTextures(pcl::TextureMesh & mesh, int textureSize) const;
+  void cancelProcessing();
   bool exportMesh(
 		  const std::string & filePath,
 		  float cloudVoxelSize,
@@ -153,7 +155,7 @@ class RTABMapApp : public UEventsHandler {
 		  bool optimized,
 		  float optimizedVoxelSize,
 		  int optimizedDepth,
-		  float optimizedDecimationFactor,
+		  int optimizedMaxPolygons,
 		  float optimizedColorRadius,
 		  bool optimizedCleanWhitePolygons,
 		  bool optimizedColorWhitePolygons,
@@ -233,6 +235,8 @@ class RTABMapApp : public UEventsHandler {
 	std::map<int, rtabmap::Transform> rawPoses_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
+
+	rtabmap::ProgressionStatus progressionStatus_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

app/android/jni/jni_interface.cpp

@@ -296,6 +296,13 @@ Java_com_introlab_rtabmap_RTABMapLib_save(
 	return app.save(databasePathC);
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
+		JNIEnv* env, jobject)
+{
+	return app.cancelProcessing();
+}
+
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		JNIEnv* env, jobject,
@@ -309,7 +316,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		bool optimized,
 		float optimizedVoxelSize,
 		int optimizedDepth,
-		float optimizedDecimationFactor,
+		int optimizedMaxPolygons,
 		float optimizedColorRadius,
 		bool optimizedCleanWhitePolygons,
 		bool optimizedColorWhitePolygons,
@@ -328,7 +335,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 			optimized,
 			optimizedVoxelSize,
 			optimizedDepth,
-			optimizedDecimationFactor,
+			optimizedMaxPolygons,
 			optimizedColorRadius,
 			optimizedCleanWhitePolygons,
 			optimizedColorWhitePolygons,

app/android/jni/point_cloud_drawable.cpp

@@ -57,8 +57,7 @@ PointCloudDrawable::PointCloudDrawable(
 PointCloudDrawable::PointCloudDrawable(
 		GLuint cloudShaderProgram,
 		GLuint textureShaderProgram,
-		const Mesh & mesh,
+		const Mesh & mesh) :
-		const cv::Mat & texture) :
 		vertex_buffers_(0),
 		textures_(0),
 		nPoints_(0),
@@ -69,7 +68,7 @@ PointCloudDrawable::PointCloudDrawable(
 		texture_shader_program_(textureShaderProgram),
 		gain_(1.0f)
 {
-	updateMesh(mesh, texture);
+	updateMesh(mesh);
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -182,7 +181,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	nPoints_ = totalPoints;
 }
 
-void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
+void PointCloudDrawable::updateMesh(const Mesh & mesh)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
@@ -197,7 +196,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 	gain_ = mesh.gain;
 
 	bool textureUpdate = false;
-	if(!texture.empty() && texture.type() == CV_8UC3)
+	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
 	{
 		if (textures_)
 		{
@@ -404,7 +403,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 		cv::Mat rgbImage;
-		cv::cvtColor(texture, rgbImage, CV_BGR2RGB);
+		cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGB);
 
 		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
 		//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);

app/android/jni/point_cloud_drawable.h

@@ -50,13 +50,12 @@ class PointCloudDrawable {
   PointCloudDrawable(
   		  GLuint cloudShaderProgram,
   		  GLuint textureShaderProgram,
-    	  const Mesh & mesh,
+    	  const Mesh & mesh);
-		  const cv::Mat & texture);
   virtual ~PointCloudDrawable();
 
   void updatePolygons(const std::vector<pcl::Vertices> & polygons);
   void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
-  void updateMesh(const Mesh & mesh, const cv::Mat & texture);
+  void updateMesh(const Mesh & mesh);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   void setGain(float gain) {gain_ = gain;}

app/android/jni/scene.cpp

@@ -512,7 +512,6 @@ void Scene::addCloud(
 void Scene::addMesh(
 		int id,
 		const Mesh & mesh,
-		const cv::Mat & texture,
 		const rtabmap::Transform & pose)
 {
 	LOGI("add mesh %d", id);
@@ -528,8 +527,7 @@ void Scene::addMesh(
 	PointCloudDrawable * drawable = new PointCloudDrawable(
 			cloud_shader_program_,
 			texture_mesh_shader_program_,
-			mesh,
+			mesh);
-			texture);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
@@ -583,12 +581,12 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
 	}
 }
 
-void Scene::updateMesh(int id, const Mesh & mesh, const cv::Mat & texture)
+void Scene::updateMesh(int id, const Mesh & mesh)
 {
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())
 	{
-		iter->second->updateMesh(mesh, texture);
+		iter->second->updateMesh(mesh);
 	}
 }
 

app/android/jni/scene.h

@@ -107,7 +107,6 @@ class Scene {
   void addMesh(
   		int id,
   		const Mesh & mesh,
-		const cv::Mat & texture,
   		const rtabmap::Transform & pose);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
@@ -117,7 +116,7 @@ class Scene {
   bool hasTexture(int id) const;
   std::set<int> getAddedClouds() const;
   void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
-  void updateMesh(int id, const Mesh & mesh, const cv::Mat & texture);
+  void updateMesh(int id, const Mesh & mesh);
   void updateGain(int id, float gain);
 
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}

app/android/jni/util.h

@@ -163,6 +163,7 @@ public:
 	rtabmap::CameraModel cameraModel;
 	float gain;
 	std::vector<Eigen::Vector2f> texCoords;
+	cv::Mat texture;
 };
 
 #endif /* UTIL_H_ */

app/android/res/layout/activity_settings.xml

@@ -235,14 +235,7 @@
 		   			android:entryValues="@array/pref_opt_depth_values"
 		   			android:defaultValue="@string/pref_default_opt_depth"/>
 		        
-		        <ListPreference 
+
-		            android:key="@string/pref_key_opt_decimation_factor"
-		    		android:title="@string/pref_title_opt_decimation_factor"
-		    		android:summary="@string/pref_summary_opt_decimation_factor"
-		   			android:entries="@array/pref_opt_decimation_factor_keys"
-		   			android:entryValues="@array/pref_opt_decimation_factor_values"
-		   			android:defaultValue="@string/pref_default_opt_decimation_factor"/>
-		        
 		        <ListPreference 
 		            android:key="@string/pref_key_opt_color_radius"
 		    		android:title="@string/pref_title_opt_color_radius"

app/android/res/values/strings.xml

@@ -101,15 +101,13 @@
     <string name="pref_key_normal_k">pref_key_normal_k</string>
     <string name="pref_default_normal_k">6</string>
     <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
-    <string name="pref_default_max_texture_distance">0</string>
+    <string name="pref_default_max_texture_distance">3</string>
     <string name="pref_key_block_render">pref_key_block_render</string>
     <string name="pref_default_block_render">false</string>
     <string name="pref_key_opt_depth">pref_key_opt_depth</string>
     <string name="pref_default_opt_depth">8</string>
-    <string name="pref_key_opt_decimation_factor">pref_key_opt_decimation_factor</string>
-    <string name="pref_default_opt_decimation_factor">60</string>
     <string name="pref_key_opt_color_radius">pref_key_opt_color_radius</string>
-    <string name="pref_default_opt_color_radius">0.05</string>
+    <string name="pref_default_opt_color_radius">0.025</string>
     <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
     <string name="pref_default_opt_clean_white">true</string>
     <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
@@ -551,8 +549,6 @@
     <string name="pref_summary_opt_voxel">Increasing this can reduce reconstruction time at the cost of less geometry precision.</string>
     <string name="pref_title_opt_depth">Reconstruction Depth</string>
     <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower.</string>
-    <string name="pref_title_opt_decimation_factor">Mesh Decimation Factor</string>
-    <string name="pref_summary_opt_decimation_factor">Reduce the number of the output polygons on plane areas by this factor. Only used when exporting with texture. This also reduces texture projection time.</string>
     <string name="pref_title_opt_color_radius">Color Radius</string>
     <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
     <string name="pref_title_opt_clean_white">Clean Mesh</string>
@@ -567,30 +563,6 @@
         <item>"7"</item>
         <item>"6"</item>
     </string-array>
-    <string-array name="pref_opt_decimation_factor_keys">
-        <item>"90%"</item>
-        <item>"80%"</item>
-        <item>"70%"</item>
-        <item>"60%"</item>
-        <item>"50%"</item>
-        <item>"40%"</item>
-        <item>"30%"</item>
-        <item>"20%"</item>
-        <item>"10%"</item>
-        <item>"Disabled"</item>
-    </string-array>
-    <string-array name="pref_opt_decimation_factor_values">
-        <item>"90"</item>
-        <item>"80"</item>
-        <item>"70"</item>
-        <item>"60"</item>
-        <item>"50"</item>
-        <item>"40"</item>
-        <item>"30"</item>
-        <item>"20"</item>
-        <item>"20"</item>
-        <item>"0"</item>
-    </string-array>
     <string-array name="pref_opt_color_radius_keys">
         <item>"Nearest"</item>
         <item>"2 m"</item>

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

@@ -9,6 +9,8 @@ import java.io.FilenameFilter;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.OutputStream;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
 import java.text.SimpleDateFormat;
 import java.util.Arrays;
 import java.util.Date;
@@ -77,6 +79,8 @@ import android.webkit.WebViewClient;
 import android.widget.Button;
 import android.widget.EditText;
 import android.widget.LinearLayout;
+import android.widget.NumberPicker;
+import android.widget.RelativeLayout;
 import android.widget.TextView;
 import android.widget.Toast;
 import android.widget.ToggleButton;
@@ -126,6 +130,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private GLSurfaceView mGLView;
 
 	ProgressDialog mProgressDialog;
+	ProgressDialog mExportProgressDialog;
 
 	// Screen size for normalizing the touch input for orbiting the render camera.
 	private Point mScreenSize = new Point();
@@ -273,6 +278,19 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mProgressDialog.setCanceledOnTouchOutside(false);
 		mRenderer.setProgressDialog(mProgressDialog);
 		mRenderer.setToast(mToast);
+		
+		mExportProgressDialog = new ProgressDialog(this);
+		mExportProgressDialog.setCanceledOnTouchOutside(false);
+		mExportProgressDialog.setCancelable(false);
+		mExportProgressDialog.setProgressStyle(ProgressDialog.STYLE_HORIZONTAL);
+		mExportProgressDialog.setProgressNumberFormat(null);
+		mExportProgressDialog.setProgressPercentFormat(null);
+		mExportProgressDialog.setButton(DialogInterface.BUTTON_NEGATIVE, "Cancel", new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		    	RTABMapLib.cancelProcessing();
+		    }
+		});
 
 		// Check if the Tango Core is out dated.
 		if (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION)) {
@@ -738,6 +756,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							mMemoryWarningDialog = null;
 						}
 					})
+					.setNeutralButton("Save", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							saveOnDevice();
+							mMemoryWarningDialog = null;
+						}
+					})
 					.create();
 					mMemoryWarningDialog.show();
 				}
@@ -903,6 +927,30 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			} 
 		});
 	}
+	
+	private void updateProgressionUI(
+			int count, 
+			int max)
+	{
+		Log.i(TAG, String.format("updateProgressionUI() count=%d max=%s", count, max));
+
+		mExportProgressDialog.setMax(max);
+		mExportProgressDialog.setProgress(count);
+	}
+
+	//called from jni
+	public void updateProgressionCallback(
+			final int count, 
+			final int max)
+	{
+		Log.i(TAG, String.format("updateProgressionCallback()"));
+
+		runOnUiThread(new Runnable() {
+			public void run() {
+				updateProgressionUI(count, max);
+			} 
+		});
+	}
 
 	private void tangoEventUI(
 			int type, 
@@ -1011,24 +1059,35 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	}
 
 	private void standardOptimization() {
-		mProgressDialog.setTitle("Post-Processing");
+		mExportProgressDialog.setTitle("Post-Processing");
-		mProgressDialog.setMessage(String.format("Please wait while optimizing..."));
+		mExportProgressDialog.setMessage(String.format("Please wait while optimizing..."));
-		mProgressDialog.show();
+		mExportProgressDialog.setProgress(0);
-
+		mExportProgressDialog.show();
+		
 		updateState(State.STATE_PROCESSING);
 		Thread workingThread = new Thread(new Runnable() {
 			public void run() {
 				final int loopDetected = RTABMapLib.postProcessing(-1);
 				runOnUiThread(new Runnable() {
 					public void run() {
-						if(loopDetected >= 0)
+						if(mExportProgressDialog.isShowing())
 						{
-							mTotalLoopClosures+=loopDetected;
+							mExportProgressDialog.dismiss();
-							mProgressDialog.setMessage(String.format("Optimization done! Increasing visual appeal..."));
+							if(loopDetected >= 0)
+							{
+								mTotalLoopClosures+=loopDetected;
+								mProgressDialog.setTitle("Post-Processing");
+								mProgressDialog.setMessage(String.format("Optimization done! Increasing visual appeal..."));
+								mProgressDialog.show();
+							}
+							else if(loopDetected < 0)
+							{
+								mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_LONG).show();
+							}
 						}
-						else if(loopDetected < 0)
+						else
 						{
-							mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_SHORT).show();
+							mToast.makeText(getActivity(), String.format("Optimization canceled"), mToast.LENGTH_LONG).show();
 						}
 						updateState(State.STATE_IDLE);
 					}
@@ -1521,63 +1580,191 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		else if(itemId == R.id.export_point_cloud ||
 				itemId == R.id.export_point_cloud_highrez ||
 				itemId == R.id.export_mesh ||
-				itemId == R.id.export_mesh_texture ||
+				itemId == R.id.export_mesh_texture)
-				itemId == R.id.export_optimized_mesh ||
-				itemId == R.id.export_optimized_mesh_texture)
 		{
 			final boolean isOBJ = itemId == R.id.export_mesh_texture || itemId == R.id.export_optimized_mesh_texture;
-			final String extension = isOBJ? ".obj" : ".ply";
-
 			final boolean meshing = itemId != R.id.export_point_cloud && itemId != R.id.export_point_cloud_highrez;
 			final boolean regenerateCloud = itemId == R.id.export_point_cloud_highrez;
-			final boolean optimized = itemId == R.id.export_optimized_mesh || itemId == R.id.export_optimized_mesh_texture;
 
-			// get Export settings
+			export(isOBJ, meshing, regenerateCloud, false, 0);
-			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		}
-			final String cloudVoxelSizeStr = sharedPref.getString(getString(R.string.pref_key_cloud_voxel), getString(R.string.pref_default_cloud_voxel));
+		else if(itemId == R.id.export_optimized_mesh ||
-			final float cloudVoxelSize = Float.parseFloat(cloudVoxelSizeStr);
+				itemId == R.id.export_optimized_mesh_texture)
-			final int textureSize = isOBJ?Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_texture_size), getString(R.string.pref_default_texture_size))):0;
+		{
-			final int normalK = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_normal_k), getString(R.string.pref_default_normal_k)));
+			final boolean isOBJ = itemId == R.id.export_optimized_mesh_texture;
-			final float maxTextureDistance = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_max_texture_distance), getString(R.string.pref_default_max_texture_distance)));
-			final float optimizedVoxelSize = cloudVoxelSize;
-			final int optimizedDepth = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_opt_depth), getString(R.string.pref_default_opt_depth)));
-			final float optimizedDecimationFactor = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_opt_decimation_factor), getString(R.string.pref_default_opt_decimation_factor)))/100.0f;
-			final float optimizedColorRadius = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_opt_color_radius), getString(R.string.pref_default_opt_color_radius)));
-			final boolean optimizedCleanWhitePolygons = sharedPref.getBoolean(getString(R.string.pref_key_opt_clean_white), Boolean.parseBoolean(getString(R.string.pref_default_opt_clean_white)));
-			final boolean optimizedColorWhitePolygons = false;//sharedPref.getBoolean("pref_key_opt_color_white", false); // not used
-			final boolean blockRendering = sharedPref.getBoolean(getString(R.string.pref_key_block_render), Boolean.parseBoolean(getString(R.string.pref_default_block_render)));
-
-			mProgressDialog.setTitle("Exporting");
-			mProgressDialog.setMessage(String.format("Please wait while preparing data to export..."));
-
-			mProgressDialog.show();
-			updateState(State.STATE_PROCESSING);
-			final String tmpPath = mWorkingDirectory + RTABMAP_TMP_DIR + RTABMAP_TMP_FILENAME + extension;
-		
-			File tmpDir = new File(mWorkingDirectory + RTABMAP_TMP_DIR);
-			tmpDir.mkdirs();
 			
-			Thread exportThread = new Thread(new Runnable() {
+	        RelativeLayout linearLayout = new RelativeLayout(this);
-				public void run() {
+	        final NumberPicker aNumberPicker = new NumberPicker(this);
+	        aNumberPicker.setMaxValue(9);
+	        aNumberPicker.setMinValue(0);
+	        aNumberPicker.setWrapSelectorWheel(false);
+	        aNumberPicker.setDescendantFocusability(NumberPicker.FOCUS_BLOCK_DESCENDANTS);
+	        aNumberPicker.setFormatter(new NumberPicker.Formatter() {
+	            @Override
+	            public String format(int i) {
+	            	if(i==0)
+	            	{
+	            		return "No Limit";
+	            	}
+	            	return String.format("%d00 000", i);
+	            }
+	        });
+	        aNumberPicker.setValue(1);
 
-					final boolean success = RTABMapLib.exportMesh(
+	        // Fix to correctly show value on first render
-							tmpPath,
+	        try {
-							cloudVoxelSize,
+	        	Method method = aNumberPicker.getClass().getDeclaredMethod("changeValueByOne", boolean.class);
-							regenerateCloud,
+	        	method.setAccessible(true);
-							meshing,
+	        	method.invoke(aNumberPicker, true);
-							textureSize,
+	        } catch (NoSuchMethodException e) {
-							normalK,
+	        	e.printStackTrace();
-							maxTextureDistance,
+	        } catch (IllegalArgumentException e) {
-							optimized,
+	        	e.printStackTrace();
-							optimizedVoxelSize,
+	        } catch (IllegalAccessException e) {
-							optimizedDepth,
+	        	e.printStackTrace();
-							optimizedDecimationFactor,
+	        } catch (InvocationTargetException e) {
-							optimizedColorRadius,
+	        	e.printStackTrace();
-							optimizedCleanWhitePolygons,
+	        }
-							optimizedColorWhitePolygons,
+
-							blockRendering);
+
-					runOnUiThread(new Runnable() {
+	        RelativeLayout.LayoutParams params = new RelativeLayout.LayoutParams(50, 50);
-						public void run() {
+	        RelativeLayout.LayoutParams numPicerParams = new RelativeLayout.LayoutParams(RelativeLayout.LayoutParams.WRAP_CONTENT, RelativeLayout.LayoutParams.WRAP_CONTENT);
+	        numPicerParams.addRule(RelativeLayout.CENTER_HORIZONTAL);
+
+	        linearLayout.setLayoutParams(params);
+	        linearLayout.addView(aNumberPicker,numPicerParams);
+
+	        AlertDialog.Builder alertDialogBuilder = new AlertDialog.Builder(this);
+	        alertDialogBuilder.setTitle("Maximum polygons");
+	        alertDialogBuilder.setView(linearLayout);
+	        alertDialogBuilder
+	                .setCancelable(false)
+	                .setPositiveButton("Ok",
+	                        new DialogInterface.OnClickListener() {
+	                            public void onClick(DialogInterface dialog,
+	                                                int id) {
+	                                export(isOBJ, true, false, true, aNumberPicker.getValue()*100000);
+	                            }
+	                        })
+	                .setNegativeButton("Cancel",
+	                        new DialogInterface.OnClickListener() {
+	                            public void onClick(DialogInterface dialog,
+	                                                int id) {
+	                                dialog.cancel();
+	                            }
+	                        });
+	        AlertDialog alertDialog = alertDialogBuilder.create();
+	        alertDialog.show();
+		}
+		else if(itemId == R.id.open)
+		{
+			final String[] files = loadFileList(mWorkingDirectory);
+			if(files.length > 0)
+			{
+				String[] filesWithSize = new String[files.length];
+				for(int i = 0; i<filesWithSize.length; ++i)
+				{
+					File filePath = new File(mWorkingDirectory+files[i]);
+					long mb = filePath.length()/(1024*1024);
+					filesWithSize[i] = files[i] + " ("+mb+" MB)";
+				}
+				AlertDialog.Builder builder = new AlertDialog.Builder(this);
+				builder.setTitle("Choose Your File (*.db)");
+				builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
+					public void onClick(DialogInterface dialog, final int which) {
+												
+						// Adjust color now?
+						new AlertDialog.Builder(getActivity())
+						.setTitle("Opening database...")
+						.setMessage("Do you want to adjust colors now?\nThis can be done later under Optimize menu.")
+						.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+							public void onClick(DialogInterface dialog, int whichIn) {
+								openDatabase(files[which], true);
+							}
+						})
+						.setNeutralButton("No", new DialogInterface.OnClickListener() {
+							public void onClick(DialogInterface dialog, int whichIn) {
+								openDatabase(files[which], false);
+							}
+						})
+						.show();
+						return;
+					}
+				});
+				builder.show();
+			}   	
+		}
+		else if(itemId == R.id.settings)
+		{
+			Intent intent = new Intent(getActivity(), SettingsActivity.class);
+			startActivity(intent);
+			mBlockBack = true;
+		}
+		else if(itemId == R.id.about)
+		{
+			AboutDialog about = new AboutDialog(this);
+			about.setTitle("About RTAB-Map");
+			about.show();
+		}
+
+		return true;
+	}
+	
+	private void export(final boolean isOBJ, final boolean meshing, final boolean regenerateCloud, final boolean optimized, final int optimizedMaxPolygons)
+	{
+		final String extension = isOBJ? ".obj" : ".ply";
+		
+		// get Export settings
+		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		final String cloudVoxelSizeStr = sharedPref.getString(getString(R.string.pref_key_cloud_voxel), getString(R.string.pref_default_cloud_voxel));
+		final float cloudVoxelSize = Float.parseFloat(cloudVoxelSizeStr);
+		final int textureSize = isOBJ?Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_texture_size), getString(R.string.pref_default_texture_size))):0;
+		final int normalK = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_normal_k), getString(R.string.pref_default_normal_k)));
+		final float maxTextureDistance = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_max_texture_distance), getString(R.string.pref_default_max_texture_distance)));
+		final float optimizedVoxelSize = cloudVoxelSize;
+		final int optimizedDepth = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_opt_depth), getString(R.string.pref_default_opt_depth)));
+		final float optimizedColorRadius = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_opt_color_radius), getString(R.string.pref_default_opt_color_radius)));
+		final boolean optimizedCleanWhitePolygons = sharedPref.getBoolean(getString(R.string.pref_key_opt_clean_white), Boolean.parseBoolean(getString(R.string.pref_default_opt_clean_white)));
+		final boolean optimizedColorWhitePolygons = false;//sharedPref.getBoolean("pref_key_opt_color_white", false); // not used
+		final boolean blockRendering = sharedPref.getBoolean(getString(R.string.pref_key_block_render), Boolean.parseBoolean(getString(R.string.pref_default_block_render)));
+
+		
+		mExportProgressDialog.setTitle("Exporting");
+		mExportProgressDialog.setMessage(String.format("Please wait while preparing data to export..."));
+		mExportProgressDialog.setProgress(0);
+		
+		final State previousState = mState;
+		
+		mExportProgressDialog.show();
+		updateState(State.STATE_PROCESSING);
+		final String tmpPath = mWorkingDirectory + RTABMAP_TMP_DIR + RTABMAP_TMP_FILENAME + extension;
+	
+		File tmpDir = new File(mWorkingDirectory + RTABMAP_TMP_DIR);
+		tmpDir.mkdirs();
+		
+		Thread exportThread = new Thread(new Runnable() {
+			public void run() {
+
+				final boolean success = RTABMapLib.exportMesh(
+						tmpPath,
+						cloudVoxelSize,
+						regenerateCloud,
+						meshing,
+						textureSize,
+						normalK,
+						maxTextureDistance,
+						optimized,
+						optimizedVoxelSize,
+						optimizedDepth,
+						optimizedMaxPolygons,
+						optimizedColorRadius,
+						optimizedCleanWhitePolygons,
+						optimizedColorWhitePolygons,
+						blockRendering);
+				runOnUiThread(new Runnable() {
+					public void run() {
+						if(mExportProgressDialog.isShowing())
+						{
 							if(success)
 							{
 								if(!meshing && cloudVoxelSize>0.0f)
@@ -1641,65 +1828,18 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 								updateState(State.STATE_IDLE);
 								mToast.makeText(getActivity(), String.format("Exporting map failed!"), mToast.LENGTH_LONG).show();
 							}
-							mProgressDialog.dismiss();
+							mExportProgressDialog.dismiss();
+						}
+						else
+						{
+							mToast.makeText(getActivity(), String.format("Export canceled"), mToast.LENGTH_LONG).show();
+							updateState(previousState);
 						}
-					});
-				} 
-			});
-			exportThread.start();
-		}
-		else if(itemId == R.id.open)
-		{
-			final String[] files = loadFileList(mWorkingDirectory);
-			if(files.length > 0)
-			{
-				String[] filesWithSize = new String[files.length];
-				for(int i = 0; i<filesWithSize.length; ++i)
-				{
-					File filePath = new File(mWorkingDirectory+files[i]);
-					long mb = filePath.length()/(1024*1024);
-					filesWithSize[i] = files[i] + " ("+mb+" MB)";
-				}
-				AlertDialog.Builder builder = new AlertDialog.Builder(this);
-				builder.setTitle("Choose Your File (*.db)");
-				builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
-					public void onClick(DialogInterface dialog, final int which) {
-												
-						// Adjust color now?
-						new AlertDialog.Builder(getActivity())
-						.setTitle("Opening database...")
-						.setMessage("Do you want to adjust colors now?\nThis can be done later under Optimize menu.")
-						.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-							public void onClick(DialogInterface dialog, int whichIn) {
-								openDatabase(files[which], true);
-							}
-						})
-						.setNeutralButton("No", new DialogInterface.OnClickListener() {
-							public void onClick(DialogInterface dialog, int whichIn) {
-								openDatabase(files[which], false);
-							}
-						})
-						.show();
-						return;
 					}
 				});
-				builder.show();
+			} 
-			}   	
+		});
-		}
+		exportThread.start();
-		else if(itemId == R.id.settings)
-		{
-			Intent intent = new Intent(getActivity(), SettingsActivity.class);
-			startActivity(intent);
-			mBlockBack = true;
-		}
-		else if(itemId == R.id.about)
-		{
-			AboutDialog about = new AboutDialog(this);
-			about.setTitle("About RTAB-Map");
-			about.show();
-		}
-
-		return true;
 	}
 	
 	private void saveDatabase(String fileName)

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

@@ -86,6 +86,7 @@ public class RTABMapLib
 
     public static native void resetMapping();
     public static native void save(String outputDatabasePath);
+    public static native void cancelProcessing();
     public static native boolean exportMesh(
     		String filePath,
     		float cloudVoxelSize,
@@ -97,7 +98,7 @@ public class RTABMapLib
     		boolean optimized,
     		float optimizedVoxelSize,
     		int optimizedDepth,
-    		float optimizedDecimationFactor,
+    		int optimizedMaxPolygons,
     		float optimizedColorRadius,
     		boolean optimizedCleanWhitePolygons,
     		boolean optimizedColorWhitePolygons,

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

@@ -106,7 +106,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 					}
 					if(value==-1 && mToast!=null)
 					{
-						mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_LONG).show();
+						mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
 					}
 				} 
 			});
@@ -117,7 +117,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 			{
 				mActivity.runOnUiThread(new Runnable() {
 					public void run() {
-						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_LONG).show();
+						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
 					}
 
 				});

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

@@ -52,7 +52,6 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_max_texture_distance))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_texture_distance))).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
 
         ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
-        ((Preference)findPreference(getString(R.string.pref_key_opt_decimation_factor))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_decimation_factor))).getValue() + "%%) "+getString(R.string.pref_summary_opt_decimation_factor));
         ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
     
         ((Preference)findPreference(getString(R.string.pref_key_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));
@@ -87,7 +86,6 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_max_texture_distance))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
 
         	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
-        	if(key.compareTo(getString(R.string.pref_key_opt_decimation_factor))==0) pref.setSummary("("+((ListPreference)pref).getValue() + "%%) "+getString(R.string.pref_summary_opt_decimation_factor));
         	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
         	
         	if(key.compareTo(getString(R.string.pref_key_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));

corelib/include/rtabmap/core/Rtabmap.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Link.h"
+#include "rtabmap/core/ProgressState.h"
 
 #include <opencv2/core/core.hpp>
 #include <list>
@@ -141,7 +142,7 @@ public:
 			bool optimized,
 			bool global,
 			std::map<int, Signature> * signatures = 0);
-	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);
+	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1, const ProgressState * state = 0);
 	int refineLinks();
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success

corelib/src/Rtabmap.cpp

@@ -3404,7 +3404,7 @@ void Rtabmap::getGraph(
 	}
 }
 
-int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations)
+int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations, const ProgressState * processState)
 {
 	UASSERT(iterations>0);
 
@@ -3492,7 +3492,21 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 				}
 			}
 		}
-		UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+
+		if(processState)
+		{
+			std::string msg = uFormat("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+			UINFO(msg.c_str());
+			if(!processState->callback(msg))
+			{
+				return -1;
+			}
+		}
+		else
+		{
+			UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+		}
+
 		if(addedLinks.size() == 0)
 		{
 			break;

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -1256,7 +1256,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 	}
 	for(unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
 	{
-		if((idx_face+1)%1000 == 0)
+		if((idx_face+1)%10000 == 0)
 		{
 			UDEBUG("face %d/%d", idx_face+1, (int)faces.size());
 			if(state && !state->callback(""))

guilib/src/ExportCloudsDialog.cpp

@@ -437,7 +437,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_gp3Radius->setValue(0.2);
 	_ui->doubleSpinBox_gp3Mu->setValue(2.5);
 	_ui->doubleSpinBox_meshDecimationFactor->setValue(0.0);
-	_ui->doubleSpinBox_transferColorRadius->setValue(0.05);
+	_ui->doubleSpinBox_transferColorRadius->setValue(0.025);
 	_ui->checkBox_cleanMesh->setChecked(true);
 	_ui->spinBox_mesh_minClusterSize->setValue(0);
 
@@ -1408,10 +1408,22 @@ bool ExportCloudsDialog::getExportedClouds(
 								}
 								if(kIndices.size())
 								{
-									coloredCloud->at(i).r = mergedClouds->at(kIndices[0]).r;
+									//compute average color
-									coloredCloud->at(i).g = mergedClouds->at(kIndices[0]).g;
+									int r=0;
-									coloredCloud->at(i).b = mergedClouds->at(kIndices[0]).b;
+									int g=0;
-									coloredCloud->at(i).a = mergedClouds->at(kIndices[0]).a;
+									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();
 								}
 								else
 								{
@@ -1529,10 +1541,22 @@ bool ExportCloudsDialog::getExportedClouds(
 							}
 							if(kIndices.size())
 							{
-								coloredCloud->at(i).r = iter->second->at(kIndices[0]).r;
+								//compute average color
-								coloredCloud->at(i).g = iter->second->at(kIndices[0]).g;
+								int r=0;
-								coloredCloud->at(i).b = iter->second->at(kIndices[0]).b;
+								int g=0;
-								coloredCloud->at(i).a = iter->second->at(kIndices[0]).a;
+								int b=0;
+								int a=0;
+								for(unsigned int j=0; j<kIndices.size(); ++j)
+								{
+									r+=(int)iter->second->at(kIndices[j]).r;
+									g+=(int)iter->second->at(kIndices[j]).g;
+									b+=(int)iter->second->at(kIndices[j]).b;
+									a+=(int)iter->second->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