Tango: Added Level of detail rendering depending on the distance from the camera

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

app/android/jni/RTABMapApp.cpp

@@ -56,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 
+#define LOW_RES_PIX 1
 
 const int g_exportedMeshId = -100;
 
@@ -169,6 +170,7 @@ RTABMapApp::RTABMapApp() :
 		totalPolygons_(0),
 		lastDrawnCloudsCount_(0),
 		renderingTime_(0.0f),
+		previousRenderingTime_(0.0f),
 		processMemoryUsedBytes(0),
 		processGPUMemoryUsedBytes(0),
 		visualizingMesh_(false),
@@ -209,6 +211,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	totalPolygons_ = 0;
 	lastDrawnCloudsCount_ = 0;
 	renderingTime_ = 0.0f;
+	previousRenderingTime_ = 0.0f;
 	processMemoryUsedBytes = 0;
 	processGPUMemoryUsedBytes = 0;
 	progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
@@ -622,6 +625,7 @@ int RTABMapApp::Render()
 				totalPolygons_ = 0;
 				lastDrawnCloudsCount_ = 0;
 				renderingTime_ = 0.0f;
+				previousRenderingTime_ = 0.0f;
 				processMemoryUsedBytes = 0;
 				processGPUMemoryUsedBytes = 0;
 			}
@@ -638,15 +642,17 @@ int RTABMapApp::Render()
 				}
 				if(added.size() != meshes)
 				{
+					LOGD("added (%d) != meshes (%d)", (int)added.size(), meshes);
 					processGPUMemoryUsedBytes = 0;
 					for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 					{
-						if(!main_scene_.hasCloud(iter->first))
+						if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull())
 						{
 							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_);
+								iter->second.polygonsLowRes = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
 							}
 
 							if(main_scene_.isMeshTexturing())
@@ -667,6 +673,7 @@ int RTABMapApp::Render()
 							estimateGPUMem += iter->second.cloud->size()*16; // 3*float + 1 float rgb
 							estimateGPUMem += iter->second.indices->size()*4; // int
 							estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
+							estimateGPUMem += iter->second.polygonsLowRes.size()*4*3; // 3 indices per polygon
 
 							processGPUMemoryUsedBytes += estimateGPUMem + (iter->second.texture.empty()?0:iter->second.polygons.size()*3*8+iter->second.texture.total());
 
@@ -821,10 +828,13 @@ int RTABMapApp::Render()
 										{
 											UTimer time;
 											std::vector<pcl::Vertices> polygons;
+											std::vector<pcl::Vertices> polygonsLowRes;
 											if(main_scene_.isMeshRendering())
 											{
 												polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 												LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
+												polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
+												LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
 											}
 
 											if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
@@ -834,6 +844,7 @@ int RTABMapApp::Render()
 												inserted.first->second.cloud = cloud;
 												inserted.first->second.indices = indices;
 												inserted.first->second.polygons = polygons;
+												inserted.first->second.polygonsLowRes = polygonsLowRes;
 												inserted.first->second.visible = true;
 												inserted.first->second.cameraModel = data.cameraModels()[0];
 												inserted.first->second.gain = 1.0f;
@@ -911,10 +922,10 @@ int RTABMapApp::Render()
 			}
 			else
 			{
-				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_);
+				main_scene_.setCloudVisible(-1, !(renderingTime_ > 0.05 || previousRenderingTime_>0.05) && odomCloudShown_ && !trajectoryMode_ && !paused_);
 
 				//just process the last one
-				if(!odomEvent.pose().isNull())
+				if(!odomEvent.pose().isNull() && !(renderingTime_ > 0.05 || previousRenderingTime_>0.05))
 				{
 					if(odomCloudShown_ && !trajectoryMode_)
 					{
@@ -1076,6 +1087,11 @@ int RTABMapApp::Render()
 			return notifyDataLoaded||notifyCameraStarted?1:0;
 		}
 	}
+	catch(const UException & e)
+	{
+		UERROR("Exception! msg=\"%s\"", e.what());
+		return -2;
+	}
 	catch(const std::exception & e)
 	{
 		UERROR("Exception! msg=\"%s\"", e.what());
@@ -1382,7 +1398,7 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string &
 			resetMapping();
 		}
 		uInsert(mappingParameters_, rtabmap::ParametersPair(compatibleKey, value));
-		UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
+		UEventsManager::post(new rtabmap::ParamEvent(this->getRtabmapParameters()));
 		return 0;
 	}
 	else
@@ -2647,7 +2663,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 		if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
 		   event->getClassName().compare("RtabmapEvent") == 0)
 		{
-			LOGI("Received RtabmapEvent initialized event!");
+			LOGI("Received RtabmapEvent event!");
 			if(camera_->isRunning())
 			{
 				rtabmap::RtabmapEvent * rtabmapEvent = (rtabmap::RtabmapEvent*)event;
@@ -2660,7 +2676,8 @@ void RTABMapApp::handleEvent(UEvent * event)
 						smallMovement == 0 &&
 						rehearsalMerged == 0 &&
 						!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().imageRaw().empty() &&
-						!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().depthRaw().empty())
+						!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().depthRaw().empty() &&
+						rtabmapEvent->getStats().poses().find(rtabmapEvent->getStats().getSignatures().rbegin()->first) != rtabmapEvent->getStats().poses().end())
 				{
 					int id = rtabmapEvent->getStats().getSignatures().rbegin()->first;
 					const rtabmap::SensorData & data = rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData();
@@ -2675,10 +2692,13 @@ void RTABMapApp::handleEvent(UEvent * event)
 					{
 						UTimer time;
 						std::vector<pcl::Vertices> polygons;
+						std::vector<pcl::Vertices> polygonsLowRes;
 						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());
+							polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
+							LOGI("(EVENT) Creating mesh low res, %d polygons (%fs)", (int)polygonsLowRes.size(), time.ticks());
 						}
 
 						if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
@@ -2697,6 +2717,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 							inserted.first->second.cloud = cloud;
 							inserted.first->second.indices = indices;
 							inserted.first->second.polygons = polygons;
+							inserted.first->second.polygonsLowRes = polygonsLowRes;
 							inserted.first->second.visible = true;
 							inserted.first->second.cameraModel = data.cameraModels()[0];
 							inserted.first->second.gain = 1.0f;
@@ -2853,6 +2874,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 		{
 			UERROR("Failed to call RTABMapActivity::updateStatsCallback");
 		}
+		previousRenderingTime_ = renderingTime_;
 		renderingTime_ = 0.0f;
 	}
 }

app/android/jni/RTABMapApp.h

@@ -208,6 +208,7 @@ class RTABMapApp : public UEventsHandler {
   int totalPolygons_;
   int lastDrawnCloudsCount_;
   float renderingTime_;
+  float previousRenderingTime_;
   long processMemoryUsedBytes;
   long processGPUMemoryUsedBytes;
 

app/android/jni/point_cloud_drawable.cpp

@@ -35,6 +35,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <GLES2/gl2.h>
 
+#define LOW_DEC 2
+#define LOWLOW_DEC 4
+
 PointCloudDrawable::PointCloudDrawable(
 		GLuint cloudShaderProgram,
 		GLuint textureShaderProgram,
@@ -89,7 +92,7 @@ PointCloudDrawable::~PointCloudDrawable()
 	}
 }
 
-void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes)
 {
 	LOGD("Update polygons");
 	polygons_.clear();
@@ -107,6 +110,22 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
 			}
 		}
+
+		if(polygonsLowRes.size())
+		{
+			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
+			UASSERT(polygonSize == 3);
+			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+			int oi = 0;
+			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
+			{
+				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
+				for(unsigned int j=0; j<polygonSize; ++j)
+				{
+					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+				}
+			}
+		}
 	}
 }
 
@@ -115,7 +134,10 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	UASSERT(cloud.get() && !cloud->empty());
 	nPoints_ = 0;
 	polygons_.clear();
+	polygonsLowRes_.clear();
 	gain_ = gain;
+	verticesLowRes_.clear();
+	verticesLowLowRes_.clear();
 
 	if (vertex_buffers_)
 	{
@@ -145,25 +167,61 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	{
 		totalPoints = indices->size();
 		vertices.resize(indices->size()*4);
+		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		int oi_low = 0;
+		int oi_lowlow = 0;
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
 			vertices[i*4] = cloud->at(indices->at(i)).x;
 			vertices[i*4+1] = cloud->at(indices->at(i)).y;
 			vertices[i*4+2] = cloud->at(indices->at(i)).z;
 			vertices[i*4+3] = cloud->at(indices->at(i)).rgb;
+
+			if(cloud->isOrganized())
+			{
+				if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
+				{
+					verticesLowRes_[oi_low++] = i;
+				}
+				if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
+				{
+					verticesLowLowRes_[oi_lowlow++] = i;
+				}
+			}
 		}
+		verticesLowRes_.resize(oi_low);
+		verticesLowLowRes_.resize(oi_lowlow);
 	}
 	else
 	{
 		totalPoints = cloud->size();
 		vertices.resize(cloud->size()*4);
+		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		int oi_low = 0;
+		int oi_lowlow = 0;
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			vertices[i*4] = cloud->at(i).x;
 			vertices[i*4+1] = cloud->at(i).y;
 			vertices[i*4+2] = cloud->at(i).z;
 			vertices[i*4+3] = cloud->at(i).rgb;
+
+			if(cloud->isOrganized())
+			{
+				if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
+				{
+					verticesLowRes_[oi_low++] = i;
+				}
+				if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
+				{
+					verticesLowLowRes_[oi_lowlow++] = i;
+				}
+			}
 		}
+		verticesLowRes_.resize(oi_low);
+		verticesLowLowRes_.resize(oi_lowlow);
 	}
 
 	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
@@ -229,12 +287,18 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 	std::vector<float> vertices;
 	int totalPoints = 0;
 	std::vector<pcl::Vertices> polygons = mesh.polygons;
+	std::vector<pcl::Vertices> polygonsLowRes;
 	hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
 	UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
 	if(mesh.cloud->isOrganized()) // assume organized mesh
 	{
+		polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
 		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
 		totalPoints = mesh.indices->size();
+		verticesLowRes_.resize(totalPoints);
+		verticesLowLowRes_.resize(totalPoints);
+		int oi_low = 0;
+		int oi_lowlow = 0;
 		if(textures_ && polygons.size())
 		{
 			//LOGD("Organized mesh with texture");
@@ -263,6 +327,15 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 				}
 
 				organizedToDenseIndices_[mesh.indices->at(i)] = i;
+
+				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
+				{
+					verticesLowRes_[oi_low++] = i;
+				}
+				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
+				{
+					verticesLowLowRes_[oi_lowlow++] = i;
+				}
 			}
 		}
 		else
@@ -286,8 +359,19 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 				}
 
 				organizedToDenseIndices_[mesh.indices->at(i)] = i;
+
+				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
+				{
+					verticesLowRes_[oi_low++] = i;
+				}
+				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
+				{
+					verticesLowLowRes_[oi_lowlow++] = i;
+				}
 			}
 		}
+		verticesLowRes_.resize(oi_low);
+		verticesLowLowRes_.resize(oi_lowlow);
 	}
 	else // assume dense mesh with texCoords set to polygons
 	{
@@ -427,7 +511,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 
 	if(polygons_.size() != polygons.size())
 	{
-		updatePolygons(polygons);
+		updatePolygons(polygons, polygonsLowRes);
 	}
 }
 
@@ -443,11 +527,12 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 		bool meshRendering,
 		float pointSize,
 		bool textureRendering,
-		bool lighting) {
+		bool lighting,
+		float distanceToCameraSqr) {
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		if(meshRendering && textureRendering && textures_)
+		if(meshRendering && textureRendering && textures_ && (verticesLowRes_.empty() || distanceToCameraSqr<50.0f))
 		{
 			glUseProgram(texture_shader_program_);
 
@@ -512,7 +597,14 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			{
 				glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
 			}
-			glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+			if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
+			{
+				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+			}
+			else
+			{
+				glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+			}
 		}
 		else // point cloud or colored mesh
 		{
@@ -588,7 +680,29 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			}
 			if(meshRendering && polygons_.size())
 			{
-				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+				if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
+				{
+					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+				}
+				else
+				{
+					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+				}
+			}
+			else if(!verticesLowRes_.empty())
+			{
+				if(distanceToCameraSqr>600.0f)
+				{
+					glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
+				}
+				else if(distanceToCameraSqr>150.0f)
+				{
+					glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
+				}
+				else
+				{
+					glDrawArrays(GL_POINTS, 0, nPoints_);
+				}
 			}
 			else
 			{

app/android/jni/point_cloud_drawable.h

@@ -53,7 +53,7 @@ class PointCloudDrawable {
     	  const Mesh & mesh);
   virtual ~PointCloudDrawable();
 
-  void updatePolygons(const std::vector<pcl::Vertices> & polygons);
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>());
   void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
   void updateMesh(const Mesh & mesh);
   void setPose(const rtabmap::Transform & pose);
@@ -75,13 +75,17 @@ class PointCloudDrawable {
 		  bool meshRendering = true,
 		  float pointSize = 3.0f,
 		  bool textureRendering = false,
-		  bool lighting = true);
+		  bool lighting = true,
+		  float distanceToCamSqr = 0.0f);
 
  private:
   // Vertex buffer of the point cloud geometry.
   GLuint vertex_buffers_;
   GLuint textures_;
   std::vector<GLuint> polygons_;
+  std::vector<GLuint> polygonsLowRes_;
+  std::vector<GLuint> verticesLowRes_;
+  std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
   glm::mat4 pose_;
   bool visible_;

app/android/jni/scene.cpp

@@ -352,6 +352,14 @@ int Scene::Render() {
 		}
 	}
 
+	float fov = 45.0f;
+	rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov)*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
+	// transform in same coordinate as frustum filtering
+	openglCamera *= rtabmap::Transform(
+		 0.0f,  0.0f,  1.0f, 0.0f,
+		 0.0f,  1.0f,  0.0f, 0.0f,
+		-1.0f,  0.0f,  0.0f, 0.0f);
+
 	int cloudDrawn=0;
 	if(mapRendering_ && frustumCulling_)
 	{
@@ -375,13 +383,6 @@ int Scene::Render() {
 
 		if(oi)
 		{
-			float fov = 45.0f;
-			rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov)*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
-			// transform in same coordinate as frustum filtering
-			openglCamera *= rtabmap::Transform(
-				 0.0f,  0.0f,  1.0f, 0.0f,
-				 0.0f,  1.0f,  0.0f, 0.0f,
-				-1.0f,  0.0f,  0.0f, 0.0f);
 			pcl::IndicesPtr indices = rtabmap::util3d::frustumFiltering(
 					cloud,
 					pcl::IndicesPtr(new std::vector<int>),
@@ -399,7 +400,13 @@ int Scene::Render() {
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				++cloudDrawn;
-				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
+				std::map<int, PointCloudDrawable*>::const_iterator iter = pointClouds_.find(ids[indices->at(i)]);
+				Eigen::Vector3f cloudToCamera(
+						iter->second->getPose().x() - openglCamera.x(),
+						iter->second->getPose().y() - openglCamera.y(),
+						iter->second->getPose().z() - openglCamera.z());
+				float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
 			}
 		}
 	}
@@ -410,7 +417,12 @@ int Scene::Render() {
 			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
+				Eigen::Vector3f cloudToCamera(
+						iter->second->getPose().x() - openglCamera.x(),
+						iter->second->getPose().y() - openglCamera.y(),
+						iter->second->getPose().z() - openglCamera.z());
+				float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
 			}
 		}
 	}

app/android/jni/util.h

@@ -158,6 +158,7 @@ public:
 	pcl::PointCloud<pcl::Normal>::Ptr normals;
 	pcl::IndicesPtr indices;
 	std::vector<pcl::Vertices> polygons;
+	std::vector<pcl::Vertices> polygonsLowRes;
 	rtabmap::Transform pose; // in rtabmap coordinates
 	bool visible;
 	rtabmap::CameraModel cameraModel;

app/android/res/values/strings.xml

@@ -99,7 +99,7 @@
     <string name="pref_key_texture_size">pref_key_texture_size</string>
     <string name="pref_default_texture_size">4096</string>
     <string name="pref_key_normal_k">pref_key_normal_k</string>
-    <string name="pref_default_normal_k">6</string>
+    <string name="pref_default_normal_k">18</string>
     <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
     <string name="pref_default_max_texture_distance">3</string>
     <string name="pref_key_min_texture_cluster_size">pref_key_min_texture_cluster_size</string>
@@ -173,6 +173,7 @@
         <item>"30"</item>
         <item>"25"</item>
         <item>"15"</item>
+        <item>"10"</item>
         <item>"5"</item>
         <item>"1"</item>
     </string-array>

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

@@ -804,7 +804,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			{
 				if(inliers >= Integer.parseInt(mMinInliers))
 				{
-					mToast.setText(String.format("Loop closure rejected, too high graph optimization error (%.3fm > %sm).", optimizationMaxError, mMaxOptimizationError));
+					if(optimizationMaxError > 0.0f)
+					{
+						mToast.setText(String.format("Loop closure rejected, too high graph optimization error (%.3fm > %sm).", optimizationMaxError, mMaxOptimizationError));
+					}
+					else
+					{
+						mToast.setText(String.format("Loop closure rejected, graph optimization failed! You may try a different Graph Optimizer (see Mapping options)."));
+					}
 				}
 				else
 				{

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

@@ -108,6 +108,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 					{
 						mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
 					}
+					else if(value==-2 && mToast!=null)
+					{
+						mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+					}
 				} 
 			});
 		}