Tango: Added Rendering Options... -> Mesh Rendering -> Mesh Decimation

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="26"
+        android:versionCode="27"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />

app/android/jni/CameraTango.h

@@ -77,6 +77,7 @@ public:
 	void close(); // close Tango connection
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setDecimation(int value) {decimation_ = value;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}

app/android/jni/RTABMapApp.cpp

@@ -133,6 +133,7 @@ RTABMapApp::RTABMapApp() :
 		fullResolution_(false),
 		appendMode_(true),
 		maxCloudDepth_(0.0),
+		meshDecimation_(1),
 		meshTrianglePix_(1),
 		meshAngleToleranceDeg_(15.0),
 		paused_(false),
@@ -333,35 +334,58 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
 {
 	UTimer t;
 	// reconstruct depth image
-	cv::Mat depth = cv::Mat::zeros(mesh.height, mesh.width, CV_32FC1);
+	UASSERT(mesh.indices.get() && mesh.indices->size());
+	cv::Mat depth = cv::Mat::zeros(mesh.cloud->height, mesh.cloud->width, CV_32FC1);
 	rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse();
-	for(unsigned int i=0; i<mesh.denseToOrganizedIndices.size(); ++i)
+	for(unsigned int i=0; i<mesh.indices->size(); ++i)
 	{
+		int index = mesh.indices->at(i);
 		// FastBilateralFilter works in camera frame
-		pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(i), localTransformInv);
+		if(mesh.cloud->at(index).x > 0)
-		depth.at<float>(mesh.denseToOrganizedIndices[i]) = pt.z;
+		{
+			pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
+			depth.at<float>(index) = pt.z;
+		}
 	}
 
 	depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
 	LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks());
 
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOrganized = rtabmap::util3d::cloudFromDepthRGB(mesh.texture.rows>1?mesh.texture:rtabmap::uncompressImage(mesh.texture), depth, mesh.cameraModel, 1, maxCloudDepth_);
+	if(!depth.empty() && mesh.indices->size())
-	cloudOrganized = rtabmap::util3d::transformPointCloud(cloudOrganized, mesh.cameraModel.localTransform());
+	{
+		pcl::IndicesPtr newIndices(new std::vector<int>(mesh.indices->size()));
+		int oi = 0;
+		for(unsigned int i=0; i<mesh.indices->size(); ++i)
+		{
+			int index = mesh.indices->at(i);
+
+			 pcl::PointXYZRGB & pt = mesh.cloud->at(index);
+			 pcl::PointXYZRGB newPt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
+			 if(depth.at<float>(index) > 0)
+			 {
+				 newPt.z = depth.at<float>(index);
+				 newPt = rtabmap::util3d::transformPoint(newPt, mesh.cameraModel.localTransform());
+				 newIndices->at(oi++) = index;
+			 }
+			 else
+			 {
+				 newPt.x = newPt.y = newPt.z = std::numeric_limits<float>::quiet_NaN();
+			 }
+			 pt.x = newPt.x;
+			 pt.y = newPt.y;
+			 pt.z = newPt.z;
+		}
+		newIndices->resize(oi);
+		mesh.indices = newIndices;
 
 		//reconstruct the mesh with smoothed surfaces
-	std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloudOrganized, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+		std::vector<pcl::Vertices> polygons;
-
+		if(main_scene_.isMeshRendering())
-	// filter NaN points
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	std::vector<pcl::Vertices> outputPolygons;
-	std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloudOrganized, polygons, *outputCloud, outputPolygons);
-
-	LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
-	if(outputPolygons.size())
 		{
-		mesh.cloud = outputCloud;
+			polygons = rtabmap::util3d::organizedFastMesh(mesh.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
-		mesh.polygons = outputPolygons;
+		}
-		mesh.denseToOrganizedIndices = denseToOrganizedIndices;
+		LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
+		mesh.polygons = polygons;
 	}
 	else
 	{
@@ -414,11 +438,13 @@ int RTABMapApp::Render()
 		{
 			if(!main_scene_.hasCloud(iter->first))
 			{
-				cv::Mat compressed = iter->second.texture;
+				cv::Mat texture;
-				iter->second.texture = rtabmap::uncompressImage(iter->second.texture);
+				if(main_scene_.isMeshTexturing())
-				main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
+				{
+					texture = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
+				}
+				main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
 				main_scene_.setCloudVisible(iter->first, iter->second.visible);
-				iter->second.texture = compressed;
 			}
 		}
 	}
@@ -554,56 +580,33 @@ int RTABMapApp::Render()
 							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, 1, maxCloudDepth_, 0, indices.get());
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
 
 							if(cloud->size() && indices->size())
 							{
 								UTimer time;
-
+								std::vector<pcl::Vertices> polygons;
-								// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
+								if(main_scene_.isMeshRendering())
-								pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-								pcl::ExtractIndices<pcl::PointXYZRGB> filter;
-								filter.setIndices(indices);
-								filter.setKeepOrganized(true);
-								filter.setInputCloud(cloud);
-								filter.filter(*output);
-
-								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
-								pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-								std::vector<pcl::Vertices> outputPolygons;
-
-								std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*output, polygons, *outputCloud, outputPolygons);
-
-								LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
-
-								if(outputCloud->size() && outputPolygons.size())
 								{
-									totalPolygons_ += outputPolygons.size();
+									polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+									LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
+								}
+
+								if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
+								{
+									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 = outputCloud;
+									inserted.first->second.cloud = cloud;
-									inserted.first->second.denseToOrganizedIndices = denseToOrganizedIndices;
+									inserted.first->second.indices = indices;
-									inserted.first->second.width = cloud->width;
+									inserted.first->second.polygons = polygons;
-									inserted.first->second.height = cloud->height;
-									inserted.first->second.polygons = outputPolygons;
 									inserted.first->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
 									inserted.first->second.visible = true;
-									inserted.first->second.texture = data.imageRaw();
 									inserted.first->second.cameraModel = data.cameraModels()[0];
+									inserted.first->second.gain = 1.0f;
 
-									if(notifyDataLoaded)
+									main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second);
-									{
-										// gain compensation is done, so don't compress the texture yet
-										inserted.first->second.texture = data.imageRaw(); // keep raw
-										// mesh will be added in gain compensation below
-									}
-									else
-									{
-										main_scene_.addMesh(id, inserted.first->second, iter->second);
-										inserted.first->second.texture = data.imageCompressed(); // keep compressed
-									}
-
 								}
 								else
 								{
@@ -646,7 +649,7 @@ int RTABMapApp::Render()
 					main_scene_.setCloudVisible(*iter, false);
 					std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
 					UASSERT(meshIter!=createdMeshes_.end());
-					meshIter->second.visible = true;
+					meshIter->second.visible = false;
 				}
 			}
 		}
@@ -663,14 +666,15 @@ int RTABMapApp::Render()
 				if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), 1, maxCloudDepth_);
+					pcl::IndicesPtr indices(new std::vector<int>);
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get());
 					if(cloud->size())
 					{
 						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
 								odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
 								odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
 							   (int)cloud->width, (int)cloud->height);
-						main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*odomEvent.pose());
+						main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*odomEvent.pose());
 						main_scene_.setCloudVisible(-1, true);
 					}
 					else
@@ -692,11 +696,12 @@ int RTABMapApp::Render()
 		LOGI("Gain compensation...");
 		boost::mutex::scoped_lock  lock(meshesMutex_);
 
-		rtabmap::GainCompensator compensator;
 		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
+		std::map<int, pcl::IndicesPtr> indices;
 		for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 		{
 			clouds.insert(std::make_pair(iter->first, iter->second.cloud));
+			indices.insert(std::make_pair(iter->first, iter->second.indices));
 		}
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
@@ -718,38 +723,24 @@ int RTABMapApp::Render()
 			}
 		}
 
+		rtabmap::GainCompensator compensator;
 		if(clouds.size() > 1 && links.size())
 		{
-			compensator.feed(clouds, links);
+			compensator.feed(clouds, indices, links);
 			LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
 		}
 
 		for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 		{
-			cv::Mat compressedImage = iter->second.texture;
-			iter->second.texture = compressedImage.rows == 1 ? rtabmap::uncompressImage(compressedImage) : compressedImage;
 			if(!iter->second.cloud->empty())
 			{
 				if(clouds.size() > 1 && links.size())
 				{
-					compensator.apply(iter->first, iter->second.cloud);
+					iter->second.gain = compensator.getGain(iter->first);
-					if(!iter->second.texture.empty())
-					{
-						compensator.apply(iter->first, iter->second.texture);
-					}
 				}
 			}
 
-			if(notifyDataLoaded)
+			main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
-			{
-				main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world* poses.at(iter->first));
-			}
-			else
-			{
-				main_scene_.updateMesh(iter->first, iter->second);
-			}
-
-			iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
 		}
 		LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
 
@@ -764,11 +755,11 @@ int RTABMapApp::Render()
 		boost::mutex::scoped_lock  lock(meshesMutex_);
 		for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 		{
-			if(iter->second.cloud->size())
+			if(iter->second.cloud->size() && iter->second.indices->size())
 			{
 				if(smoothMesh(iter->first, iter->second))
 				{
-					main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
+					main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
 				}
 			}
 		}
@@ -980,6 +971,45 @@ void RTABMapApp::setMaxCloudDepth(float value)
 	maxCloudDepth_ = value;
 }
 
+void RTABMapApp::setMeshDecimation(int value)
+{
+	LOGE("Set decimation to level %d", value);
+	meshDecimation_ = 1;
+	if(camera_)
+	{
+		// Google Tango Tablet 160x90
+		// Phab2Pro 240x135
+		int width = camera_->getCameraModel().imageWidth()/8;
+		if(value == 2) // high
+		{
+			if(width % 10 == 0)
+			{
+				meshDecimation_ = 10;
+			}
+			else if(width % 15 == 0)
+			{
+				meshDecimation_ = 15;
+			}
+			else
+			{
+				LOGE("Could not set decimation to high (width=%d)", width);
+			}
+		}
+		else if(value == 1) // medium
+		{
+			if(width % 5 == 0)
+			{
+				meshDecimation_ = 5;
+			}
+			else
+			{
+				LOGE("Could not set decimation to medium (width=%d)", width);
+			}
+		}
+	}
+	LOGE("Set decimation to %d", meshDecimation_);
+}
+
 void RTABMapApp::setMeshAngleTolerance(float value)
 {
 	meshAngleToleranceDeg_ = value;
@@ -1049,13 +1079,26 @@ void RTABMapApp::save(const std::string & databasePath)
 {
 	rtabmapThread_->join(true);
 
+	// save mapping parameters in the database
+
+	bool appendModeBackup = appendMode_;
+	if(appendMode_)
+	{
+		appendMode_ = false;
+	}
+
+	bool dataRecorderModeBackup = dataRecorderMode_;
 	if(dataRecorderMode_)
 	{
-		// to save mapping parameters in the database
 		dataRecorderMode_ = false;
+	}
+
+	if(appendModeBackup || dataRecorderModeBackup)
+	{
 		rtabmap::ParametersMap parameters = getRtabmapParameters();
 		rtabmap_->parseParameters(parameters);
-		dataRecorderMode_ = true;
+		appendMode_ = appendModeBackup;
+		dataRecorderMode_ = dataRecorderModeBackup;
 	}
 
 	rtabmap_->close(true, databasePath);
@@ -1076,7 +1119,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	if(UFile::getExtension(filePath).compare("obj") == 0)
 	{
 		pcl::TextureMesh textureMesh;
-		std::vector<cv::Mat> textures;
+		std::vector<int> textures;
 		int totalPolygons = 0;
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		{
@@ -1093,42 +1136,57 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 				iter!= createdMeshes_.end();
 				++iter)
 			{
-				if(!iter->second.texture.empty() &&
+				if(!rtabmap_->getMemory()->getImageCompressed(iter->first).empty() &&
 					iter->second.cloud->size() &&
-					iter->second.polygons.size() &&
+					iter->second.polygons.size())
-					(!iter->second.cloud->is_dense || (iter->second.cloud->is_dense && iter->second.denseToOrganizedIndices.size() == iter->second.cloud->size())))
 				{
+					// Convert organized to dense cloud
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+					std::vector<pcl::Vertices> outputPolygons;
+					std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*iter->second.cloud, iter->second.polygons, *outputCloud, outputPolygons);
+
+					if(iter->second.gain != 1.0f)
+					{
+						for(unsigned int i=0; i<outputCloud->size(); ++i)
+						{
+							pcl::PointXYZRGB & pt = outputCloud->at(i);
+							pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * iter->second.gain)));
+							pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * iter->second.gain)));
+							pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * iter->second.gain)));
+						}
+					}
+
 					// OBJ format requires normals
-					pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 6);
+					pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, 6);
 
 					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-					pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals);
+					pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals);
 
 					// polygons
-					UASSERT(iter->second.polygons.size());
+					UASSERT(outputPolygons.size());
-					unsigned int polygonSize = iter->second.polygons.front().vertices.size();
+					unsigned int polygonSize = outputPolygons.front().vertices.size();
-					textureMesh.tex_polygons[oi].resize(iter->second.polygons.size());
+					textureMesh.tex_polygons[oi].resize(outputPolygons.size());
-					textureMesh.tex_coordinates[oi].resize(iter->second.polygons.size() * polygonSize);
+					textureMesh.tex_coordinates[oi].resize(outputPolygons.size() * polygonSize);
-					for(unsigned int j=0; j<iter->second.polygons.size(); ++j)
+					for(unsigned int j=0; j<outputPolygons.size(); ++j)
 					{
-						pcl::Vertices vertices = iter->second.polygons[j];
+						pcl::Vertices vertices = outputPolygons[j];
 						UASSERT(polygonSize == vertices.vertices.size());
 						for(unsigned int k=0; k<vertices.vertices.size(); ++k)
 						{
 							//uv
-							UASSERT(vertices.vertices[k] < iter->second.denseToOrganizedIndices.size());
+							UASSERT(vertices.vertices[k] < denseToOrganizedIndices.size());
-							int originalVertex = iter->second.denseToOrganizedIndices[vertices.vertices[k]];
+							int originalVertex = denseToOrganizedIndices[vertices.vertices[k]];
 							textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
-									float(originalVertex % iter->second.width) / float(iter->second.width),   // u
+									float(originalVertex % iter->second.cloud->width) / float(iter->second.cloud->width),   // u
-									float(iter->second.height - originalVertex / iter->second.width) / float(iter->second.height)); // v
+									float(iter->second.cloud->height - originalVertex / iter->second.cloud->width) / float(iter->second.cloud->height)); // v
 
 							vertices.vertices[k] += polygonsStep;
 						}
 						textureMesh.tex_polygons[oi][j] = vertices;
 
 					}
-					totalPolygons += iter->second.polygons.size();
+					totalPolygons += outputPolygons.size();
-					polygonsStep += iter->second.cloud->size();
+					polygonsStep += outputCloud->size();
 
 					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second.pose);
 					if(mergedClouds->size() == 0)
@@ -1140,7 +1198,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 						*mergedClouds += *transformedCloud;
 					}
 
-					textures[oi] = iter->second.texture;
+					textures[oi] = iter->first;
 					textureMesh.tex_materials[oi].tex_illum = 1;
 					textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
 					++oi;
@@ -1163,7 +1221,11 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 				UDirectory::makeDir(textureDirectory);
 				for(unsigned int i=0;i<textures.size(); ++i)
 				{
-					cv::Mat rawImage = textures[i].rows>1?textures[i]:rtabmap::uncompressImage(textures[i]);
+					cv::Mat rawImage = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(textures[i]));
+					if(createdMeshes_.at(textures[i]).gain != 1.0f)
+					{
+						cv::multiply(rawImage, createdMeshes_.at(textures[i]).gain, rawImage);
+					}
 					std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
 					cv::imwrite(texFile, rawImage);
 
@@ -1198,20 +1260,36 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 				iter!= createdMeshes_.end();
 				++iter)
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(iter->second.cloud, iter->second.pose);
+				// Convert organized to dense cloud
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				std::vector<pcl::Vertices> outputPolygons;
+				rtabmap::util3d::filterNaNPointsFromMesh(*iter->second.cloud, iter->second.polygons, *outputCloud, outputPolygons);
+
+				if(iter->second.gain != 1.0f)
+				{
+					for(unsigned int i=0; i<outputCloud->size(); ++i)
+					{
+						pcl::PointXYZRGB & pt = outputCloud->at(i);
+						pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * iter->second.gain)));
+						pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * iter->second.gain)));
+						pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * iter->second.gain)));
+					}
+				}
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(outputCloud, iter->second.pose);
 				if(mergedClouds->size() == 0)
 				{
 					*mergedClouds = *transformedCloud;
-					mergedPolygons = iter->second.polygons;
+					mergedPolygons = outputPolygons;
 				}
 				else
 				{
-					rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, iter->second.polygons);
+					rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, outputPolygons);
 				}
 			}
 		}
 
-		if(mergedClouds->size() && mergedPolygons.size())
+		if(mergedClouds->size())
 		{
 			pcl::PolygonMesh mesh;
 			pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);

app/android/jni/RTABMapApp.h

@@ -125,6 +125,7 @@ class RTABMapApp : public UEventsHandler {
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
+  void setMeshDecimation(int value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
   int setMappingParameter(const std::string & key, const std::string & value);
@@ -157,6 +158,7 @@ class RTABMapApp : public UEventsHandler {
   bool fullResolution_;
   bool appendMode_;
   float maxCloudDepth_;
+  int meshDecimation_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
 

app/android/jni/jni_interface.cpp

@@ -217,6 +217,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
 	return app.setMaxCloudDepth(value);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshDecimation(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshDecimation(value);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
 		JNIEnv*, jobject, float value)
 {

app/android/jni/point_cloud_drawable.cpp

@@ -38,31 +38,36 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 PointCloudDrawable::PointCloudDrawable(
 		GLuint cloudShaderProgram,
 		GLuint textureShaderProgram,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) :
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float gain) :
 		vertex_buffers_(0),
 		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
 		cloud_shader_program_(cloudShaderProgram),
-		texture_shader_program_(textureShaderProgram)
+		texture_shader_program_(textureShaderProgram),
+		gain_(1.0f)
 {
-	updateCloud(cloud);
+	updateCloud(cloud, indices, gain);
 }
 
 PointCloudDrawable::PointCloudDrawable(
 		GLuint cloudShaderProgram,
 		GLuint textureShaderProgram,
-		const Mesh & mesh) :
+		const Mesh & mesh,
+		const cv::Mat & texture) :
 		vertex_buffers_(0),
 		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
 		cloud_shader_program_(cloudShaderProgram),
-		texture_shader_program_(textureShaderProgram)
+		texture_shader_program_(textureShaderProgram),
+		gain_(1.0f)
 {
-	updateMesh(mesh);
+	updateMesh(mesh, texture);
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -86,7 +91,7 @@ PointCloudDrawable::~PointCloudDrawable()
 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
 {
 	polygons_.clear();
-	if(polygons.size())
+	if(polygons.size() && organizedToDenseIndices_.size())
 	{
 		int polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
@@ -97,21 +102,18 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 			UASSERT((int)polygons[i].vertices.size() == polygonSize);
 			for(int j=0; j<polygonSize; ++j)
 			{
-				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
+				polygons_[oi++] = organizedToDenseIndices_.at((unsigned short)polygons[i].vertices[j]);
 			}
 		}
 	}
 }
 
-void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
+void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain)
 {
-	UASSERT(!cloud->empty());
+	UASSERT(cloud.get() && !cloud->empty() && indices.get() && !indices->empty());
-	if(nPoints_)
-	{
-		UASSERT((int)cloud->size() == nPoints_);
-	}
 	nPoints_ = 0;
 	polygons_.clear();
+	gain_ = gain;
 
 	if (vertex_buffers_)
 	{
@@ -135,13 +137,13 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	}
 
 	LOGI("Creating cloud buffer %d", vertex_buffers_);
-	std::vector<float> vertices(cloud->size()*4);
+	std::vector<float> vertices(indices->size()*4);
-	for(unsigned int i=0; i<cloud->size(); ++i)
+	for(unsigned int i=0; i<indices->size(); ++i)
 	{
-		vertices[i*4] = cloud->at(i).x;
+		vertices[i*4] = cloud->at(indices->at(i)).x;
-		vertices[i*4+1] = cloud->at(i).y;
+		vertices[i*4+1] = cloud->at(indices->at(i)).y;
-		vertices[i*4+2] = cloud->at(i).z;
+		vertices[i*4+2] = cloud->at(indices->at(i)).z;
-		vertices[i*4+3] = cloud->at(i).rgb;
+		vertices[i*4+3] = cloud->at(indices->at(i)).rgb;
 	}
 
 	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
@@ -156,16 +158,12 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 		return;
 	}
 
-	nPoints_ = cloud->size();
+	nPoints_ = indices->size();
 }
 
-void PointCloudDrawable::updateMesh(const Mesh & mesh)
+void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 {
-	UASSERT(!mesh.cloud->empty());
+	UASSERT(mesh.cloud.get() && !mesh.cloud->empty() && mesh.indices.get() && !mesh.indices->empty());
-	if(nPoints_)
-	{
-		UASSERT((int)mesh.cloud->size() == nPoints_);
-	}
 	nPoints_ = 0;
 
 	if (vertex_buffers_)
@@ -175,8 +173,10 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 		vertex_buffers_ = 0;
 	}
 
+	gain_ = mesh.gain;
+
 	bool textureUpdate = false;
-	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
+	if(!texture.empty() && texture.type() == CV_8UC3)
 	{
 		if (textures_)
 		{
@@ -196,8 +196,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 
 	if(textureUpdate)
 	{
-		UASSERT((!mesh.cloud->is_dense && mesh.cloud->width==mesh.width && mesh.cloud->height==mesh.height) ||
+		UASSERT(!mesh.cloud->is_dense);
-				(mesh.cloud->is_dense && mesh.width>1 && mesh.height>1 && mesh.denseToOrganizedIndices.size() == mesh.cloud->size()));
 		glGenTextures(1, &textures_);
 		if(!textures_)
 		{
@@ -209,37 +208,37 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 
 	LOGI("Creating cloud buffer %d", vertex_buffers_);
 	std::vector<float> vertices;
+	organizedToDenseIndices_ = std::vector<int>(mesh.cloud->width*mesh.cloud->height, -1);
 	if(textures_)
 	{
-		vertices = std::vector<float>(mesh.cloud->size()*6);
+		vertices = std::vector<float>(mesh.indices->size()*6);
-		for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+		for(unsigned int i=0; i<mesh.indices->size(); ++i)
 		{
-			vertices[i*6] = mesh.cloud->at(i).x;
+			vertices[i*6] = mesh.cloud->at(mesh.indices->at(i)).x;
-			vertices[i*6+1] = mesh.cloud->at(i).y;
+			vertices[i*6+1] = mesh.cloud->at(mesh.indices->at(i)).y;
-			vertices[i*6+2] = mesh.cloud->at(i).z;
+			vertices[i*6+2] = mesh.cloud->at(mesh.indices->at(i)).z;
 
 			// rgb
-			vertices[i*6+3] = mesh.cloud->at(i).rgb;
+			vertices[i*6+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
 
 			// texture uv
-			int index = i;
+			int index = mesh.indices->at(i);
-			if(mesh.cloud->is_dense)
+			vertices[i*6+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
-			{
+			vertices[i*6+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height);  //v
-				index = mesh.denseToOrganizedIndices[i];
+
-			}
+			organizedToDenseIndices_[mesh.indices->at(i)] = i;
-			vertices[i*6+4] = float(index % mesh.width)/float(mesh.width); //u
-			vertices[i*6+5] = float(index / mesh.width)/float(mesh.height);  //v
 		}
 	}
 	else
 	{
-		vertices = std::vector<float>(mesh.cloud->size()*4);
+		vertices = std::vector<float>(mesh.indices->size()*4);
-		for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+		for(unsigned int i=0; i<mesh.indices->size(); ++i)
 		{
-			vertices[i*4] = mesh.cloud->at(i).x;
+			vertices[i*4] = mesh.cloud->at(mesh.indices->at(i)).x;
-			vertices[i*4+1] = mesh.cloud->at(i).y;
+			vertices[i*4+1] = mesh.cloud->at(mesh.indices->at(i)).y;
-			vertices[i*4+2] = mesh.cloud->at(i).z;
+			vertices[i*4+2] = mesh.cloud->at(mesh.indices->at(i)).z;
-			vertices[i*4+3] = mesh.cloud->at(i).rgb;
+			vertices[i*4+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+			organizedToDenseIndices_[mesh.indices->at(i)] = i;
 		}
 	}
 
@@ -262,7 +261,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 		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(mesh.texture, rgbImage, CV_BGR2RGB);
+		cv::cvtColor(texture, rgbImage, CV_BGR2RGB);
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
 
 		GLint error = glGetError();
@@ -277,7 +276,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 		}
 	}
 
-	nPoints_ = mesh.cloud->size();
+	nPoints_ = mesh.indices->size();
 
 	if(polygons_.size() != mesh.polygons.size())
 	{
@@ -312,6 +311,9 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
 			glUniform1i(texture_handle, 0);
 
+			GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain");
+			glUniform1f(gain_handle, gain_);
+
 			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
 			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
 
@@ -334,6 +336,9 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
 			glUniform1f(point_size_handle_, pointSize);
 
+			GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain");
+			glUniform1f(gain_handle, gain_);
+
 			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
 			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");
 

app/android/jni/point_cloud_drawable.h

@@ -44,20 +44,24 @@ class PointCloudDrawable {
   PointCloudDrawable(
 		  GLuint cloudShaderProgram,
 		  GLuint textureShaderProgram,
-  		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
+  		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		  const pcl::IndicesPtr & indices,
+		  float gain);
   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);
+  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
-  void updateMesh(const Mesh & mesh);
+  void updateMesh(const Mesh & mesh, const cv::Mat & texture);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   rtabmap::Transform getPose() const {return glmToTransform(pose_);}
   bool isVisible() const {return visible_;}
+  bool hasTexture() const {return textures_ != 0;}
 
   // Update current point cloud data.
   //
@@ -75,9 +79,12 @@ class PointCloudDrawable {
   int nPoints_;
   glm::mat4 pose_;
   bool visible_;
+  std::vector<int> organizedToDenseIndices_;
 
   GLuint cloud_shader_program_;
   GLuint texture_shader_program_;
+
+  float gain_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -55,9 +55,10 @@ const std::string kPointCloudVertexShader =
 const std::string kPointCloudFragmentShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
+	"uniform float u_gain;\n"
     "varying vec3 v_color;\n"
     "void main() {\n"
-    "  gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
+    "  gl_FragColor = vec4(v_color.z*u_gain, v_color.y*u_gain, v_color.x*u_gain, 1.0);\n"
     "}\n";
 
 const std::string kTextureMeshVertexShader =
@@ -75,9 +76,13 @@ const std::string kTextureMeshFragmentShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
 	"uniform sampler2D u_Texture;\n"
+	"uniform float u_gain;\n"
     "varying vec2 v_TexCoordinate;\n"
     "void main() {\n"
     "  gl_FragColor = texture2D(u_Texture, v_TexCoordinate);\n"
+    "  gl_FragColor.x *= u_gain;\n"
+	"  gl_FragColor.y *= u_gain;\n"
+	"  gl_FragColor.z *= u_gain;\n"
     "}\n";
 
 const std::string kGraphVertexShader =
@@ -418,6 +423,7 @@ void Scene::setTraceVisible(bool visible)
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
 		const rtabmap::Transform & pose)
 {
 	LOGI("add cloud %d", id);
@@ -433,7 +439,9 @@ void Scene::addCloud(
 	PointCloudDrawable * drawable = new PointCloudDrawable(
 			cloud_shader_program_,
 			texture_mesh_shader_program_,
-			cloud);
+			cloud,
+			indices,
+			1.0f);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
@@ -441,6 +449,7 @@ void Scene::addCloud(
 void Scene::addMesh(
 		int id,
 		const Mesh & mesh,
+		const cv::Mat & texture,
 		const rtabmap::Transform & pose)
 {
 	LOGI("add mesh %d", id);
@@ -456,7 +465,8 @@ 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));
 }
@@ -486,6 +496,11 @@ bool Scene::hasCloud(int id) const
 	return pointClouds_.find(id) != pointClouds_.end();
 }
 
+bool Scene::hasTexture(int id) const
+{
+	return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasTexture();
+}
+
 std::set<int> Scene::getAddedClouds() const
 {
 	return uKeysSet(pointClouds_);
@@ -500,11 +515,11 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
 	}
 }
 
-void Scene::updateMesh(int id, const Mesh & mesh)
+void Scene::updateMesh(int id, const Mesh & mesh, const cv::Mat & texture)
 {
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())
 	{
-		iter->second->updateMesh(mesh);
+		iter->second->updateMesh(mesh, texture);
 	}
 }

app/android/jni/scene.h

@@ -100,23 +100,29 @@ class Scene {
   void addCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		  const pcl::IndicesPtr & indices,
   		  const rtabmap::Transform & pose);
   void addMesh(
   		int id,
   		const Mesh & mesh,
+		const cv::Mat & texture,
   		const rtabmap::Transform & pose);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
   bool hasCloud(int id) const;
+  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);
+  void updateMesh(int id, const Mesh & mesh, const cv::Mat & texture);
 
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
 
+  bool isMeshRendering() const {return meshRendering_;}
+  bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
+
  private:
   // Camera object that allows user to use touch input to interact with.
   tango_gl::GestureCamera* gesture_camera_;

app/android/jni/util.h

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/Vertices.h>
+#include <pcl/pcl_base.h>
 
 class LogHandler : public UEventsHandler
 {
@@ -144,15 +145,13 @@ inline rtabmap::Transform glmToTransform(const glm::mat4 & mat)
 
 struct Mesh
 {
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // dense or organized cloud
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
+	pcl::IndicesPtr indices;
 	std::vector<pcl::Vertices> polygons;
-	std::vector<int> denseToOrganizedIndices; // should be set if cloud is dense, used for texturing
-	unsigned int width;  // width of the organized cloud
-	unsigned int height; // height of the organized cloud
 	rtabmap::Transform pose; // in rtabmap coordinates
 	bool visible;
-	cv::Mat texture;
 	rtabmap::CameraModel cameraModel;
+	float gain;
 };
 
 #endif /* UTIL_H_ */

app/android/res/menu/optionmenu.xml

@@ -46,6 +46,7 @@
 				           <item android:id="@+id/mesh" android:title="Mesh" />
 				           <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
 			           </group>
+			           <item android:id="@+id/mesh_decimation" android:checkable="false" android:title="Mesh Decimation..." />
 			           <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
             		   <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
             		   <item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />

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

@@ -89,6 +89,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private String mWorkingDirectory = "";
   
   private int mMaxDepthIndex = 5;
+  private int mMeshDecimationIndex = 0;
   private int mMeshAngleToleranceIndex = 2;
   private int mMeshTriangleSizeIndex = 0;
   
@@ -558,7 +559,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  mItemOpen.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
     	  mItemPostProcessing.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
     	  mItemDataRecorderMode.setEnabled(item.isChecked());
-    	 // mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
+    	 
     	  if(item.isChecked())
     	  {
     		  RTABMapLib.setPausedMapping(true);
@@ -849,6 +850,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  });
 		  builder.show();
       }
+      else if(itemId == R.id.mesh_decimation)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Mesh Decimation");
+		  final String[] values = {"Disabled", "Medium", "High"};
+		  builder.setSingleChoiceItems(values, mMeshDecimationIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which <= 2)
+		        {
+		        	mMeshDecimationIndex = which;
+			        RTABMapLib.setMeshDecimation(which);
+		        }
+		    }
+		  });
+		  builder.show();
+      }
       else if(itemId == R.id.mesh_angle_tolerance)
       {
     	  // get double

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

@@ -72,6 +72,7 @@ public class RTABMapLib
     public static native void setAppendMode(boolean enabled);
     public static native void setDataRecorderMode(boolean enabled);
     public static native void setMaxCloudDepth(float value);
+    public static native void setMeshDecimation(int value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);
     public static native int setMappingParameter(String key, String value);

corelib/include/rtabmap/core/util3d_surface.h

@@ -65,6 +65,12 @@ std::list<std::list<int> > RTABMAP_EXP clusterPolygons(
 		const std::vector<std::set<int> > & neighborPolygons,
 		int minClusterSize = 0);
 
+std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		double angleTolerance,
+		bool quad,
+		int trianglePixelSize,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0));
 std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		double angleTolerance = M_PI/16,

corelib/src/util3d_surface.cpp

@@ -137,6 +137,44 @@ std::list<std::list<int> > clusterPolygons(
 	return clusters;
 }
 
+std::vector<pcl::Vertices> organizedFastMesh(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		double angleTolerance,
+		bool quad,
+		int trianglePixelSize,
+		const Eigen::Vector3f & viewpoint)
+{
+	UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
+	UASSERT(cloud->is_dense == false);
+	UASSERT(cloud->width > 1 && cloud->height > 1);
+
+	pcl::OrganizedFastMesh<pcl::PointXYZ> ofm;
+	ofm.setTrianglePixelSize (trianglePixelSize);
+	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZ>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_RIGHT_CUT);
+	ofm.setInputCloud (cloud);
+	ofm.setAngleTolerance(angleTolerance);
+	ofm.setViewpoint(viewpoint);
+
+	std::vector<pcl::Vertices> vertices;
+	ofm.reconstruct (vertices);
+
+	if(quad)
+	{
+		//flip all polygons (right handed)
+		std::vector<pcl::Vertices> output(vertices.size());
+		for(unsigned int i=0; i<vertices.size(); ++i)
+		{
+			output[i].vertices.resize(4);
+			output[i].vertices[0] = vertices[i].vertices[0];
+			output[i].vertices[3] = vertices[i].vertices[1];
+			output[i].vertices[2] = vertices[i].vertices[2];
+			output[i].vertices[1] = vertices[i].vertices[3];
+		}
+		return output;
+	}
+
+	return vertices;
+}
 std::vector<pcl::Vertices> organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		double angleTolerance,