Tango: added online blending option, background color option, double tap, fixed meshes jiggering on touch, removed raw mesh option

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

app/android/jni/RTABMapApp.cpp

@@ -164,6 +164,7 @@ RTABMapApp::RTABMapApp() :
 		clusterRatio_(0.1),
 		maxGainRadius_(0.02f),
 		renderingTextureDecimation_(4),
+		backgroundColor_(0.2f),
 		paused_(false),
 		dataRecorderMode_(false),
 		clearSceneOnNextRender_(false),
@@ -237,6 +238,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	processGPUMemoryUsedBytes = 0;
 	bufferedStatsData_.clear();
 	progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
+	main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
 
 	if(camera_)
 	{
@@ -680,6 +682,9 @@ void RTABMapApp::InitializeGLContent()
 {
 	UINFO("");
 	main_scene_.InitGLContent();
+
+	float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_;
+	main_scene_.setGridColor(v, v, v);
 }
 
 // OpenGL thread
@@ -936,21 +941,18 @@ int RTABMapApp::Render()
 			//backup state
 			bool isMeshRendering = main_scene_.isMeshRendering();
 			bool isTextureRendering = main_scene_.isMeshTexturing();
-			bool isFrustumCulling = main_scene_.isFrustumCulling();
 
 			main_scene_.setMeshRendering(main_scene_.hasMesh(g_exportedMeshId), main_scene_.hasTexture(g_exportedMeshId));
-			main_scene_.setFrustumCulling(false);
 
+			fpsTime.restart();
 			lastDrawnCloudsCount_ = main_scene_.Render();
-
-			// revert state
-			main_scene_.setMeshRendering(isMeshRendering, isTextureRendering);
-			main_scene_.setFrustumCulling(isFrustumCulling);
-
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();
 			}
+
+			// revert state
+			main_scene_.setMeshRendering(isMeshRendering, isTextureRendering);
 		}
 		else
 		{
@@ -1143,7 +1145,7 @@ int RTABMapApp::Render()
 						}
 						else if(!paused_ && rejected>0)
 						{
-							main_scene_.setBackgroundColor(0, 0.1f, 0); // dark green
+							main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
 						}
 						else if(!paused_ && rehearsalMerged>0)
 						{
@@ -1151,7 +1153,7 @@ int RTABMapApp::Render()
 						}
 						else
 						{
-							main_scene_.setBackgroundColor(0, 0, 0);
+							main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
 						}
 					}
 				}
@@ -1299,10 +1301,6 @@ int RTABMapApp::Render()
 									processGPUMemoryUsedBytes += estimateCPUMem + (mesh.texture.empty()?0:mesh.polygons.size()*3*8+mesh.texture.total());
 									mesh.texture = cv::Mat(); // don't keep textures in memory
 								}
-								else if(id == poses.rbegin()->first)
-								{
-									UERROR("No mesh could be created for node %d", id);
-								}
 							}
 						}
 					}
@@ -1426,6 +1424,7 @@ int RTABMapApp::Render()
 				notifyDataLoaded = true;
 			}
 
+			fpsTime.restart();
 			lastDrawnCloudsCount_ = main_scene_.Render();
 			if(renderingTime_ < fpsTime.elapsed())
 			{
@@ -1533,7 +1532,7 @@ void RTABMapApp::setPausedMapping(bool paused)
 	{
 		boost::mutex::scoped_lock  lock(renderingMutex_);
 		visualizingMesh_ = false;
-		main_scene_.setBackgroundColor(0, 0, 0);
+		main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
 	}
 	paused_ = paused;
 	if(camera_)
@@ -1551,6 +1550,10 @@ void RTABMapApp::setPausedMapping(bool paused)
 		}
 	}
 }
+void RTABMapApp::setOnlineBlending(bool enabled)
+{
+	main_scene_.setBlending(enabled);
+}
 void RTABMapApp::setMapCloudShown(bool shown)
 {
 	main_scene_.setMapRendering(shown);
@@ -1745,6 +1748,13 @@ void RTABMapApp::setRenderingTextureDecimation(int value)
 	renderingTextureDecimation_ = value;
 }
 
+void RTABMapApp::setBackgroundColor(float gray)
+{
+	backgroundColor_ = gray;
+	float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_;
+	main_scene_.setGridColor(v, v, v);
+}
+
 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
 {
 	std::string compatibleKey = key;

app/android/jni/RTABMapApp.h

@@ -114,6 +114,7 @@ class RTABMapApp : public UEventsHandler {
                     float x0, float y0, float x1, float y1);
 
   void setPausedMapping(bool paused);
+  void setOnlineBlending(bool enabled);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
   void setMeshRendering(bool enabled, bool withTexture);
@@ -141,6 +142,7 @@ class RTABMapApp : public UEventsHandler {
   void setClusterRatio(float value);
   void setMaxGainRadius(float value);
   void setRenderingTextureDecimation(int value);
+  void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
@@ -204,6 +206,7 @@ class RTABMapApp : public UEventsHandler {
   float clusterRatio_;
   float maxGainRadius_;
   int renderingTextureDecimation_;
+  float backgroundColor_;
 
   rtabmap::ParametersMap mappingParameters_;
 

app/android/jni/jni_interface.cpp

@@ -127,6 +127,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setPausedMapping(
 	return app.setPausedMapping(paused);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOnlineBlending(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setOnlineBlending(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMapCloudShown(
 		JNIEnv*, jobject, bool shown)
 {
@@ -288,6 +294,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setRenderingTextureDecimation(
 {
 	return app.setRenderingTextureDecimation(value);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackgroundColor(
+		JNIEnv*, jobject, float value)
+{
+	return app.setBackgroundColor(value);
+}
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
     JNIEnv* env, jobject, jstring key, jstring value)

app/android/jni/point_cloud_drawable.cpp

@@ -600,7 +600,11 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 		float pointSize,
 		bool textureRendering,
 		bool lighting,
-		float distanceToCameraSqr) {
+		float distanceToCameraSqr,
+		const GLuint & depthTexture,
+		int screenWidth,
+		int screenHeight,
+		bool packDepthToColorChannel) {
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
@@ -645,6 +649,14 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture");
 			glUniform1i(texture_handle, 0);
 
+			// Texture activate unit 1
+			glActiveTexture(GL_TEXTURE1);
+			// Bind the texture to this unit.
+			glBindTexture(GL_TEXTURE_2D, depthTexture);
+			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
+			GLuint depth_texture_handle = glGetUniformLocation(texture_shader_program_, "uDepthTexture");
+			glUniform1i(depth_texture_handle, 1);
+
 			GLuint gainR_handle = glGetUniformLocation(texture_shader_program_, "uGainR");
 			GLuint gainG_handle = glGetUniformLocation(texture_shader_program_, "uGainG");
 			GLuint gainB_handle = glGetUniformLocation(texture_shader_program_, "uGainB");
@@ -652,6 +664,12 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			glUniform1f(gainG_handle, gainG_);
 			glUniform1f(gainB_handle, gainB_);
 
+			GLuint blending_handle = glGetUniformLocation(texture_shader_program_, "uBlending");
+			glUniform1i(blending_handle, depthTexture>0?1:0);
+
+			GLuint screenScale_handle = glGetUniformLocation(texture_shader_program_, "uScreenScale");
+			glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
+
 			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "aVertex");
 			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord");
 			GLint attribute_normal=0;
@@ -691,7 +709,7 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			glm::mat4 mvp_mat = projectionMatrix * mv_mat;
 			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
 
-			GLuint n_handle = glGetUniformLocation(texture_shader_program_, "uN");
+			GLuint n_handle = glGetUniformLocation(cloud_shader_program_, "uN");
 			glm::mat3 normalMatrix(mv_mat);
 			normalMatrix = glm::inverse(normalMatrix);
 			normalMatrix = glm::transpose(normalMatrix);
@@ -703,21 +721,29 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			}
 
 			//lighting
-			GLuint lighting_handle = glGetUniformLocation(texture_shader_program_, "uUseLighting");
+			GLuint lighting_handle = glGetUniformLocation(cloud_shader_program_, "uUseLighting");
 			glUniform1i(lighting_handle, lighting?1:0);
 
 			if(lighting)
 			{
-				GLuint ambiant_handle = glGetUniformLocation(texture_shader_program_, "uAmbientColor");
+				GLuint ambiant_handle = glGetUniformLocation(cloud_shader_program_, "uAmbientColor");
 				glUniform3f(ambiant_handle,0.6,0.6,0.6);
 
-				GLuint lightingDirection_handle = glGetUniformLocation(texture_shader_program_, "uLightingDirection");
+				GLuint lightingDirection_handle = glGetUniformLocation(cloud_shader_program_, "uLightingDirection");
 				glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
 			}
 
 			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "uPointSize");
 			glUniform1f(point_size_handle_, pointSize);
 
+			// Texture activate unit 1
+			glActiveTexture(GL_TEXTURE0);
+			// Bind the texture to this unit.
+			glBindTexture(GL_TEXTURE_2D, depthTexture);
+			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
+			GLuint depth_texture_handle = glGetUniformLocation(cloud_shader_program_, "uDepthTexture");
+			glUniform1i(depth_texture_handle, 0);
+
 			GLuint gainR_handle = glGetUniformLocation(cloud_shader_program_, "uGainR");
 			GLuint gainG_handle = glGetUniformLocation(cloud_shader_program_, "uGainG");
 			GLuint gainB_handle = glGetUniformLocation(cloud_shader_program_, "uGainB");
@@ -725,6 +751,15 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			glUniform1f(gainG_handle, gainG_);
 			glUniform1f(gainB_handle, gainB_);
 
+			GLuint packing_handle = glGetUniformLocation(cloud_shader_program_, "uPackDepthToColor");
+			glUniform1i(packing_handle, packDepthToColorChannel?1:0);
+
+			GLuint blending_handle = glGetUniformLocation(cloud_shader_program_, "uBlending");
+			glUniform1i(blending_handle, depthTexture>0?1:0);
+
+			GLuint screenScale_handle = glGetUniformLocation(cloud_shader_program_, "uScreenScale");
+			glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
+
 			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "aVertex");
 			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor");
 			GLint attribute_normal=0;

app/android/jni/point_cloud_drawable.h

@@ -83,7 +83,11 @@ class PointCloudDrawable {
 		  float pointSize = 3.0f,
 		  bool textureRendering = false,
 		  bool lighting = true,
-		  float distanceToCamSqr = 0.0f);
+		  float distanceToCamSqr = 0.0f,
+		  const GLuint & depthTexture = 0,
+		  int screenWidth = 0,
+		  int screenHeight = 0,
+		  bool packDepthToColorChannel = false);
 
  private:
   template<class PointT>

app/android/jni/scene.cpp

@@ -19,6 +19,7 @@
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <pcl/common/transforms.h>
 
@@ -72,16 +73,42 @@ const std::string kPointCloudVertexShader =
     "  vColor = aColor;\n"
     "}\n";
 const std::string kPointCloudFragmentShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
 	"uniform float uGainR;\n"
 	"uniform float uGainG;\n"
 	"uniform float uGainB;\n"
+	"uniform sampler2D uDepthTexture;\n"
+	"uniform bool uBlending;\n"
+	"uniform vec2 uScreenScale;\n"
+	"uniform bool uPackDepthToColor;\n"
     "varying vec3 vColor;\n"
 	"varying float vLightWeighting;\n"
     "void main() {\n"
     "  vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
-	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
+	"  float alpha = 1.0;\n"
+	"  if(uBlending) {\n"
+	"    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+	"    float depth = texture2D(uDepthTexture, coord).r;\n"
+//	"    alpha = 0.5;\n"
+	"    float zNear = 0.2;\n"
+	"    float zFar = 1000.0;\n"
+	"    float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+	" 	 float linearDepth = (2.0 * zNear * zFar) / (zFar + zNear - ndcDepth * (zFar - zNear));\n"
+	"    float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
+	" 	 float linearFragz = (2.0 * zNear * zFar) / (zFar + zNear - ndcFragz * (zFar - zNear));\n"
+	"	 if(linearFragz > linearDepth + 0.05)\n"
+	"      alpha=0.0;\n"
+	"  }\n"
+	"  if(uPackDepthToColor) {\n"
+	"    float toFixed = 255.0/256.0;\n"
+	"    vec4 enc = vec4(1.0, 255.0, 65025.0, 160581375.0) * toFixed * gl_FragCoord.z;\n"
+	"    enc = fract(enc);\n"
+	"    gl_FragColor = enc;\n"
+	"  }\n"
+	"  else {"
+	"    gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
+	"  }\n"
     "}\n";
 
 const std::string kTextureMeshVertexShader =
@@ -119,17 +146,36 @@ const std::string kTextureMeshVertexShader =
     "  }\n"
     "}\n";
 const std::string kTextureMeshFragmentShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
 	"uniform sampler2D uTexture;\n"
+    "uniform sampler2D uDepthTexture;\n"
 	"uniform float uGainR;\n"
 	"uniform float uGainG;\n"
 	"uniform float uGainB;\n"
+	"uniform bool uBlending;\n"
+	"uniform vec2 uScreenScale;\n"
     "varying vec2 vTexCoord;\n"
 	"varying float vLightWeighting;\n"
+	""
     "void main() {\n"
     "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
-	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
+    "  float alpha = 1.0;\n"
+    "  if(uBlending) {\n"
+	"    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    float depth = texture2D(uDepthTexture, coord).r;\n"
+ //   "    alpha = 0.5;\n"
+  //Linearize depth: http://stackoverflow.com/questions/6652253/getting-the-true-z-value-from-the-depth-buffer
+	"    float zNear = 0.2;\n"
+	"    float zFar = 1000.0;\n"
+    "    float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+    " 	 float linearDepth = (2.0 * zNear * zFar) / (zFar + zNear - ndcDepth * (zFar - zNear));\n"
+	"    float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
+	" 	 float linearFragz = (2.0 * zNear * zFar) / (zFar + zNear - ndcFragz * (zFar - zNear));\n"
+	"	 if(linearFragz > linearDepth + 0.05)\n"
+	"      alpha=0.0;\n"
+	"  }\n"
+	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
     "}\n";
 
 const std::string kGraphVertexShader =
@@ -169,17 +215,22 @@ Scene::Scene() :
 		cloud_shader_program_(0),
 		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
+		blending_(true),
 		mapRendering_(true),
 		meshRendering_(true),
 		meshRenderingTexture_(true),
 		pointSize_(5.0f),
-		frustumCulling_(true),
 		boundingBoxRendering_(false),
 		lighting_(false),
 		backfaceCulling_(true),
 		r_(0.0f),
 		g_(0.0f),
-		b_(0.0f)
+		b_(0.0f),
+		fboId_(0),
+		depthTexture_(0),
+		screenWidth_(0),
+		screenHeight_(0),
+		doubleTapOn_(false)
 {
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
@@ -242,27 +293,35 @@ void Scene::DeleteResources() {
 	LOGI("Scene::DeleteResources()");
 	if(axis_)
 	{
-	  delete axis_;
+		delete axis_;
-	  axis_ = 0;
+		axis_ = 0;
-	  delete frustum_;
+		delete frustum_;
-	  delete trace_;
+		delete trace_;
-	  delete grid_;
+		delete grid_;
-	  delete currentPose_;
+		delete currentPose_;
-	  delete box_;
+		delete box_;
 	}
 
 	if (cloud_shader_program_) {
 		glDeleteShader(cloud_shader_program_);
 		cloud_shader_program_ = 0;
-	  }
+	}
 	if (texture_mesh_shader_program_) {
 		glDeleteShader(texture_mesh_shader_program_);
 		texture_mesh_shader_program_ = 0;
-	  }
+	}
 	if (graph_shader_program_) {
 		glDeleteShader(graph_shader_program_);
 		graph_shader_program_ = 0;
-	  }
+	}
+
+	if(fboId_>0)
+	{
+		glDeleteFramebuffers(1, &fboId_);
+		fboId_ = 0;
+		glDeleteTextures(1, &depthTexture_);
+		depthTexture_ = 0;
+	}
 
 	clear();
 }
@@ -289,13 +348,49 @@ void Scene::clear()
 
 //Should only be called in OpenGL thread!
 void Scene::SetupViewPort(int w, int h) {
-  if (h == 0) {
+	if (h == 0) {
-    LOGE("Setup graphic height not valid");
+		LOGE("Setup graphic height not valid");
-  }
+	}
-  UASSERT(gesture_camera_ != 0);
+	UASSERT(gesture_camera_ != 0);
-  gesture_camera_->SetAspectRatio(static_cast<float>(w) /
+	gesture_camera_->SetAspectRatio(static_cast<float>(w) / static_cast<float>(h));
-                                  static_cast<float>(h));
+	glViewport(0, 0, w, h);
-  glViewport(0, 0, w, h);
+	if(screenWidth_ != w || fboId_ == 0)
+	{
+		if(fboId_>0)
+		{
+			glDeleteFramebuffers(1, &fboId_);
+			fboId_ = 0;
+			glDeleteTextures(1, &depthTexture_);
+			depthTexture_ = 0;
+		}
+
+		// Create depth texture
+		glGenTextures(1, &depthTexture_);
+		glBindTexture(GL_TEXTURE_2D, depthTexture_);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, w, h, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT, NULL);
+		glBindTexture(GL_TEXTURE_2D, 0);
+
+		// regenerate fbo texture
+		// create a framebuffer object, you need to delete them when program exits.
+		glGenFramebuffers(1, &fboId_);
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
+		// Set the texture to be at the depth attachment point of the FBO
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthTexture_, 0);
+
+		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
+		if ( status != GL_FRAMEBUFFER_COMPLETE)
+		{
+			LOGE("Frame buffer cannot be generated! Status: %in", status);
+		}
+		glBindFramebuffer(GL_FRAMEBUFFER,0);
+	}
+	screenWidth_ = w;
+	screenHeight_ = h;
 }
 
 std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normalize = true)
@@ -409,26 +504,12 @@ bool intersectFrustumAABB(
 int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
-	glEnable(GL_DEPTH_TEST);
+	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
-	if(backfaceCulling_)
+	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
-	{
+	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
-		glEnable(GL_CULL_FACE);
+	glm::mat4 rotateM;
-	}
-	else
-	{
-		glDisable(GL_CULL_FACE);
-	}
-
-	glClearColor(r_, g_, b_, 1.0f);
-	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
 	if(!currentPose_->isNull())
 	{
-		glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
-		Eigen::Quaternionf quat = currentPose_->getQuaternionf();
-		glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
-
-		glm::mat4 rotateM;
 		rotateM = glm::rotate<float>(float(color_camera_to_display_rotation_)*-1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f));
 
 		if (gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
@@ -441,47 +522,52 @@ int Scene::Render() {
 		{
 			// In third person or top down mode, we follow the camera movement.
 			gesture_camera_->SetAnchorPosition(position, rotation*glm::quat(rotateM));
-
-			frustum_->SetPosition(position);
-			frustum_->SetRotation(rotation);
-			// Set the frustum scale to 4:3, this doesn't necessarily match the physical
-			// camera's aspect ratio, this is just for visualization purposes.
-			frustum_->SetScale(kFrustumScale);
-			frustum_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
-
-			axis_->SetPosition(position);
-			axis_->SetRotation(rotation);
-			axis_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
-		}
-
-		trace_->UpdateVertexArray(position);
-		if(traceVisible_)
-		{
-			trace_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
-		}
-
-		if(gridVisible_)
-		{
-			grid_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
 		}
 	}
 
-	float fov = 45.0f;
+	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
-	rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov);//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
+	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
+
+	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*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;
+	std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
-	if(mapRendering_ && frustumCulling_)
+
+	// First rendering to get depth texture
+	glEnable(GL_DEPTH_TEST);
+	glDepthFunc(GL_LESS);
+	glDepthMask(GL_TRUE);
+	glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
+	glDisable (GL_BLEND);
+	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+	if(backfaceCulling_)
 	{
-		std::vector<glm::vec4> planes = computeFrustumPlanes(gesture_camera_->GetProjectionMatrix()*gesture_camera_->GetViewMatrix(), true);
+		glEnable(GL_CULL_FACE);
+	}
+	else
+	{
+		glDisable(GL_CULL_FACE);
+	}
+
+	UTimer timer;
+
+	bool onlineBlending = blending_ && mapRendering_ && meshRendering_ && pointClouds_.size()>1;
+	std::set<int> usedForDepth;
+	if(onlineBlending && fboId_)
+	{
+		// set the rendering destination to FBO
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
+		glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
+		glClearColor(1, 1, 1, 1);
+		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+		// Draw scene
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
 			if(iter->second->isVisible())
@@ -490,29 +576,136 @@ int Scene::Render() {
 						iter->second->aabbMinWorld(),
 						iter->second->aabbMaxWorld()))
 				{
-					if(boundingBoxRendering_)
+					usedForDepth.insert(iter->first);
-					{
-						box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
-						box_->Render(gesture_camera_->GetProjectionMatrix(),
-											gesture_camera_->GetViewMatrix());
-					}
-
-					++cloudDrawn;
 					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];
+					float distanceToCameraSqr = 999.0f; // set it large to use low res polygons for fast processing
-					iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
+					iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr);
 				}
 			}
 		}
+
+		// back to normal window-system-provided framebuffer
+		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
+		glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
+	}
+
+	if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+	{
+		glClearColor(0, 0, 0, 0);
+		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		{
+			if((onlineBlending && usedForDepth.find(iter->first) != usedForDepth.end()) ||
+				(!onlineBlending && iter->second->isVisible() &&
+						intersectFrustumAABB(planes,
+							iter->second->aabbMinWorld(),
+							iter->second->aabbMaxWorld())))
+			{
+				float distanceToCameraSqr = 999.0f; // set it large to use low res polygons for fast processing
+				iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, distanceToCameraSqr, 0, 0, 0, true);
+			}
+		}
+
+		GLubyte zValue[4];
+		glReadPixels(doubleTapPos_.x*screenWidth_, screenHeight_-doubleTapPos_.y*screenHeight_, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, zValue);
+		float fromFixed = 256.0f/255.0f;
+		float zValueF = float(zValue[0]/255.0f)*fromFixed + float(zValue[1]/255.0f)*fromFixed/255.0f + float(zValue[2]/255.0f)*fromFixed/65025.0f + float(zValue[3]/255.0f)*fromFixed/160581375.0f;
+
+		if(zValueF != 0.0f)
+		{
+			zValueF = zValueF*2.0-1.0;//NDC
+			glm::vec4 point = glm::inverse(projectionMatrix*viewMatrix)*glm::vec4(doubleTapPos_.x*2.0f-1.0f, (1.0f-doubleTapPos_.y)*2.0f-1.0f, zValueF, 1.0f);
+			point /= point.w;
+			gesture_camera_->SetAnchorOffset(glm::vec3(point.x, point.y, point.z) - position);
+		}
+	}
+	doubleTapOn_ = false;
+
+	glClearColor(r_, g_, b_, 1.0f);
+	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+	if(!currentPose_->isNull())
+	{
+		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		{
+			frustum_->SetPosition(position);
+			frustum_->SetRotation(rotation);
+			// Set the frustum scale to 4:3, this doesn't necessarily match the physical
+			// camera's aspect ratio, this is just for visualization purposes.
+			frustum_->SetScale(kFrustumScale);
+			frustum_->Render(projectionMatrix, viewMatrix);
+
+			axis_->SetPosition(position);
+			axis_->SetRotation(rotation);
+			axis_->Render(projectionMatrix, viewMatrix);
+		}
+
+		trace_->UpdateVertexArray(position);
+		if(traceVisible_)
+		{
+			trace_->Render(projectionMatrix, viewMatrix);
+		}
+
+		if(gridVisible_)
+		{
+			grid_->Render(projectionMatrix, viewMatrix);
+		}
+	}
+
+	if(graphVisible_ && graph_)
+	{
+		graph_->Render(projectionMatrix, viewMatrix);
+	}
+
+	int cloudDrawn=0;
+	if(mapRendering_)
+	{
+		if(onlineBlending)
+		{
+			glEnable (GL_BLEND);
+			glDepthMask(GL_FALSE);
+		}
+
+		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		{
+			if((onlineBlending && usedForDepth.find(iter->first) != usedForDepth.end()) ||
+				(!onlineBlending && iter->second->isVisible() &&
+						intersectFrustumAABB(planes,
+							iter->second->aabbMinWorld(),
+							iter->second->aabbMaxWorld())))
+			{
+				if(boundingBoxRendering_)
+				{
+					box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
+					box_->Render(projectionMatrix, viewMatrix);
+				}
+
+				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(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_);
+				++cloudDrawn;
+			}
+		}
+
+		if(onlineBlending)
+		{
+			glDisable (GL_BLEND);
+			glDepthMask(GL_TRUE);
+		}
 	}
 	else
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			if(!mapRendering_ && iter->first > 0)
+			if(iter->first > 0)
 			{
 				break;
 			}
@@ -522,8 +715,7 @@ int Scene::Render() {
 				if(boundingBoxRendering_)
 				{
 					box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
-					box_->Render(gesture_camera_->GetProjectionMatrix(),
+					box_->Render(projectionMatrix, viewMatrix);
-										gesture_camera_->GetViewMatrix());
 				}
 
 				++cloudDrawn;
@@ -532,17 +724,12 @@ int Scene::Render() {
 						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);
+				iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_);
 			}
 		}
 	}
 
-	if(graphVisible_ && graph_)
+	return 1;
-	{
-		graph_->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix());
-	}
-
-	return cloudDrawn;
 }
 
 void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
@@ -570,7 +757,21 @@ void Scene::OnTouchEvent(int touch_count,
                          tango_gl::GestureCamera::TouchEvent event, float x0,
                          float y0, float x1, float y1) {
 	UASSERT(gesture_camera_ != 0);
-  gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
+	if(touch_count == 3)
+	{
+		//doubletap
+		if(!doubleTapOn_)
+		{
+			doubleTapPos_.x = x0;
+			doubleTapPos_.y = y0;
+			doubleTapOn_ = true;
+		}
+	}
+	else
+	{
+		// rotate/translate/zoom
+		gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
+	}
 }
 
 void Scene::updateGraph(
@@ -722,3 +923,11 @@ void Scene::updateGains(int id, float gainR, float gainG, float gainB)
 		iter->second->setGains(gainR, gainG, gainB);
 	}
 }
+
+void Scene::setGridColor(float r, float g, float b)
+{
+	if(grid_)
+	{
+		grid_->SetColor(r, g, b);
+	}
+}

app/android/jni/scene.h

@@ -78,7 +78,7 @@ class Scene {
 
   void SetCameraPose(const rtabmap::Transform & pose);
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
-  rtabmap::Transform GetOpenGLCameraPose(float * fov) const;
+  rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
 
   // Touch event passed from android activity. This function only support two
   // touches.
@@ -120,19 +120,20 @@ class Scene {
   void updateMesh(int id, const Mesh & mesh);
   void updateGains(int id, float gainR, float gainG, float gainB);
 
+  void setBlending(bool enabled) {blending_ = enabled;}
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
-  void setFrustumCulling(bool enabled) {frustumCulling_ = enabled;}
   void setLighting(bool enabled) {lighting_ = enabled;}
   void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
   void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
+  void setGridColor(float r, float g, float b);
 
+  bool isBlending() const {return blending_;}
   bool isMapRendering() const {return mapRendering_;}
   bool isMeshRendering() const {return meshRendering_;}
   bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
   float getPointSize() const {return pointSize_;}
-  bool isFrustumCulling() const {return frustumCulling_;}
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
 
@@ -170,17 +171,23 @@ class Scene {
   GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
+  bool blending_;
   bool mapRendering_;
   bool meshRendering_;
   bool meshRenderingTexture_;
   float pointSize_;
-  bool frustumCulling_;
   bool boundingBoxRendering_;
   bool lighting_;
   bool backfaceCulling_;
   float r_;
   float g_;
   float b_;
+  GLuint fboId_;
+  GLuint depthTexture_;
+  GLsizei screenWidth_;
+  GLsizei screenHeight_;
+  bool doubleTapOn_;
+  cv::Point2f doubleTapPos_;
 };
 
 #endif  // TANGO_POINT_CLOUD_SCENE_H_

app/android/jni/tango-gl/camera.cpp

@@ -22,8 +22,8 @@ namespace tango_gl {
 Camera::Camera() {
   field_of_view_ = 45.0f * DEGREE_2_RADIANS;
   aspect_ratio_ = 4.0f / 3.0f;
-  near_clip_plane_ = 0.1f;
+  near_clip_plane_ = 0.2f;
-  far_clip_plane_ = 100.0f;
+  far_clip_plane_ = 1000.0f;
 }
 
 glm::mat4 Camera::GetViewMatrix() {
@@ -43,6 +43,12 @@ void Camera::SetFieldOfView(float fov) {
   field_of_view_ = fov * DEGREE_2_RADIANS;
 }
 
+void Camera::SetNearFarClipPlanes(const float near, const float far)
+{
+	near_clip_plane_ = near;
+	far_clip_plane_ = far;
+}
+
 Camera::~Camera() {
 }
 

app/android/jni/tango-gl/gesture_camera.cpp

@@ -182,7 +182,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 6.0f;
       cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();

app/android/jni/tango-gl/include/tango-gl/camera.h

@@ -29,6 +29,7 @@ class Camera : public Transform {
 
   void SetAspectRatio(const float aspect_ratio);
   void SetFieldOfView(const float fov);
+  void SetNearFarClipPlanes(const float near, const float far);
 
   glm::mat4 GetViewMatrix();
   glm::mat4 GetProjectionMatrix();

app/android/jni/tango-gl/include/tango-gl/gesture_camera.h

@@ -55,6 +55,11 @@ class GestureCamera : public Camera {
                               float touch_range);
 
   void SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation);
+  void SetAnchorOffset(const glm::vec3& pos) {anchor_offset_ = pos;}
+  const glm::vec3& GetAnchorOffset() const {return anchor_offset_;}
+
+  void SetCameraDistance(float cameraDistance) {cam_cur_dist_ = cameraDistance;}
+  float GetCameraDistance() const {return cam_cur_dist_;}
 
   // Set camera type, set render camera's parent position and rotation.
   void SetCameraType(CameraType camera_index);

app/android/res/layout/activity_rtabmap.xml

@@ -23,32 +23,32 @@
         android:layout_height="fill_parent"
         android:layout_gravity="top" />
     
-    <ToggleButton
-        android:id="@+id/backface_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/light_button"
-        android:layout_alignLeft="@+id/light_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/backface_off"
-        android:textOn="@string/backface_on" />
-   
     <ToggleButton
         android:id="@+id/light_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/first_person_button"
+        android:layout_above="@+id/backface_button"
-        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_alignLeft="@+id/backface_button"
         android:layout_alignParentRight="true"
         android:layout_marginBottom="15dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/light_off"
         android:textOn="@string/light_on" />
-   
+    
+    <ToggleButton
+        android:id="@+id/backface_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/first_person_button"
+        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/backface_off"
+        android:textOn="@string/backface_on" />
+      
     <ToggleButton
         android:id="@+id/first_person_button"
         android:layout_width="100dp"

app/android/res/layout/activity_settings.xml

@@ -51,6 +51,18 @@
    			android:entries="@array/pref_rendering_texture_decimation_keys"
    			android:entryValues="@array/pref_rendering_texture_decimation_values"
    			android:defaultValue="@string/pref_default_rendering_texture_decimation"/>
+        <ListPreference 
+            android:key="@string/pref_key_background_color"
+    		android:title="@string/pref_title_background_color"
+    		android:summary="@string/pref_summary_background_color"
+   			android:entries="@array/pref_background_color_keys"
+   			android:entryValues="@array/pref_background_color_values"
+   			android:defaultValue="@string/pref_default_background_color"/>
+        <SwitchPreference
+            android:key="@string/pref_key_blending"
+            android:title="@string/pref_title_blending"
+            android:summary="@string/pref_summary_blending"
+            android:defaultValue="@string/pref_default_blending"/>
         <SwitchPreference
             android:key="@string/pref_key_nodes_filtering"
             android:title="@string/pref_title_nodes_filtering"

app/android/res/menu/optionmenu.xml

@@ -17,12 +17,6 @@
                   	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
                   </menu>
               </item>
-              <item android:id="@+id/export_mesh_menu" android:title="Raw Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_mesh" android:title="Colored Mesh" />
-			          <item android:id="@+id/export_mesh_texture" android:title="Textured Mesh" />
-		          </menu>
-	          </item>
 	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
 	              <menu>
 		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />

app/android/res/values/strings.xml

@@ -56,6 +56,10 @@
     <string name="pref_default_triangle">2</string>
     <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
     <string name="pref_default_rendering_texture_decimation">4</string>
+    <string name="pref_key_blending">pref_key_blending</string>
+    <string name="pref_default_blending">true</string>
+    <string name="pref_key_background_color">pref_key_background_color</string>
+    <string name="pref_default_background_color">0.2</string>
     <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
     <string name="pref_default_nodes_filtering">false</string>
     <string name="pref_key_append">pref_key_append</string>
@@ -101,7 +105,7 @@
     <string name="pref_default_db_in_memory">true</string>
     
     <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
-    <string name="pref_default_cloud_voxel">0</string>
+    <string name="pref_default_cloud_voxel">0.01</string>
     <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>
@@ -139,6 +143,10 @@
     <string name="pref_summary_depth">Points over the maximum depth are not rendered.</string>
     <string name="pref_title_point_size">Point Size</string>
     <string name="pref_summary_point_size">Size of the points when rendering only the point cloud.</string>
+    <string name="pref_title_blending">Blending</string>
+    <string name="pref_summary_blending">Blend close surfaces together to get more smooth colors on overlapping surfaces. May decrease rendering frame rate.</string>
+    <string name="pref_title_background_color">Background Color</string>
+    <string name="pref_summary_background_color"></string>
     <string name="pref_title_nodes_filtering">Nodes Filtering</string>
     <string name="pref_summary_nodes_filtering">Render only the newest point cloud of a loop closure.</string>
 
@@ -257,6 +265,32 @@
         <item>"4"</item>
         <item>"8"</item>
     </string-array>
+    <string-array name="pref_background_color_keys">
+        <item>"White"</item>
+        <item>"0.9"</item>
+        <item>"Light Gray"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"Gray"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"Dark Gray"</item>
+        <item>"0.1"</item>
+        <item>"Black"</item>
+    </string-array>
+    <string-array name="pref_background_color_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.0"</item>
+    </string-array>
 
     <string name="pref_title_mapping_sub">Mapping&#8230;</string>
     <string name="pref_title_mapping">Mapping</string>

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

@@ -66,6 +66,7 @@ import android.text.method.LinkMovementMethod;
 import android.text.util.Linkify;
 import android.util.Log;
 import android.view.Display;
+import android.view.GestureDetector;
 import android.view.Menu;
 import android.view.MenuItem;
 import android.view.MenuInflater;
@@ -73,6 +74,7 @@ import android.view.MotionEvent;
 import android.view.Surface;
 import android.view.View;
 import android.view.View.OnClickListener;
+import android.view.View.OnTouchListener;
 import android.view.WindowManager;
 import android.view.inputmethod.EditorInfo;
 import android.webkit.WebView;
@@ -189,6 +191,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private AlertDialog mMemoryWarningDialog = null;
 	
 	private String[] mStatusTexts = new String[16];
+	
+	GestureDetector mGesDetect = null;
 
 	//Tango Service connection.
 	ServiceConnection mTangoServiceConnection = new ServiceConnection() {
@@ -270,8 +274,45 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		// OpenGL view where all of the graphics are drawn.
 		mGLView = (GLSurfaceView) findViewById(R.id.gl_surface_view);
 
+		mGesDetect = new GestureDetector(this, new DoubleTapGestureDetector());
+		
 		// Configure OpenGL renderer
 		mGLView.setEGLContextClientVersion(2);
+		mGLView.setEGLConfigChooser(8, 8, 8, 8, 24, 0);
+		mGLView.setOnTouchListener(new OnTouchListener() {
+	        @Override
+	        public boolean onTouch(View v, MotionEvent event) {
+	            mGesDetect.onTouchEvent(event);
+	            
+	            // Pass the touch event to the native layer for camera control.
+	    		// Single touch to rotate the camera around the device.
+	    		// Two fingers to zoom in and out.
+	    		int pointCount = event.getPointerCount();
+	    		if (pointCount == 1) {
+	    			float normalizedX = event.getX(0) / mScreenSize.x;
+	    			float normalizedY = event.getY(0) / mScreenSize.y;
+	    			RTABMapLib.onTouchEvent(1, 
+	    					event.getActionMasked(), normalizedX, normalizedY, 0.0f, 0.0f);
+	    		}
+	    		if (pointCount == 2) {
+	    			if (event.getActionMasked() == MotionEvent.ACTION_POINTER_UP) {
+	    				int index = event.getActionIndex() == 0 ? 1 : 0;
+	    				float normalizedX = event.getX(index) / mScreenSize.x;
+	    				float normalizedY = event.getY(index) / mScreenSize.y;
+	    				RTABMapLib.onTouchEvent(1, 
+	    						MotionEvent.ACTION_DOWN, normalizedX, normalizedY, 0.0f, 0.0f);
+	    			} else {
+	    				float normalizedX0 = event.getX(0) / mScreenSize.x;
+	    				float normalizedY0 = event.getY(0) / mScreenSize.y;
+	    				float normalizedX1 = event.getX(1) / mScreenSize.x;
+	    				float normalizedY1 = event.getY(1) / mScreenSize.y;
+	    				RTABMapLib.onTouchEvent(2, event.getActionMasked(),
+	    						normalizedX0, normalizedY0, normalizedX1, normalizedY1);
+	    			}
+	    		}
+	    		return true;
+	        }
+	    });
 
 		// Configure the OpenGL renderer.
 		mRenderer = new Renderer(this);
@@ -342,6 +383,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
                 public void onDisplayChanged(int displayId) {
                     synchronized (this) {
                         setAndroidOrientation();
+                        Display display = getWindowManager().getDefaultDisplay();
+                        display.getSize(mScreenSize);
                     }
                 }
 
@@ -455,6 +498,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			String optimizer = sharedPref.getString(getString(R.string.pref_key_optimizer), getString(R.string.pref_default_optimizer));
 			
 			if(!DISABLE_LOG) Log.d(TAG, "set mapping parameters");
+			RTABMapLib.setOnlineBlending(sharedPref.getBoolean(getString(R.string.pref_key_blending), Boolean.parseBoolean(getString(R.string.pref_default_blending))));
 			RTABMapLib.setNodesFiltering(sharedPref.getBoolean(getString(R.string.pref_key_nodes_filtering), Boolean.parseBoolean(getString(R.string.pref_default_nodes_filtering))));
 			RTABMapLib.setAutoExposure(sharedPref.getBoolean(getString(R.string.pref_key_auto_exposure), Boolean.parseBoolean(getString(R.string.pref_default_auto_exposure))));
 			RTABMapLib.setRawScanSaved(sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
@@ -484,6 +528,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			RTABMapLib.setPointSize(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_point_size), getString(R.string.pref_default_point_size))));
 			RTABMapLib.setMeshAngleTolerance(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_angle), getString(R.string.pref_default_angle))));
 			RTABMapLib.setMeshTriangleSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_triangle), getString(R.string.pref_default_triangle))));
+			float bgColor = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_background_color), getString(R.string.pref_default_background_color)));
+			RTABMapLib.setBackgroundColor(bgColor);
+			mRenderer.setTextColor(bgColor==0.5f?0.4f:1.0f-bgColor);
 			
 			if(!DISABLE_LOG) Log.d(TAG, "set rendering parameters...");
 			RTABMapLib.setClusterRatio(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_cluster_ratio), getString(R.string.pref_default_cluster_ratio))));
@@ -592,37 +639,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
         Camera.getCameraInfo(fisheye?1:0, colorCameraInfo);
         RTABMapLib.setScreenRotation(display.getRotation(), colorCameraInfo.orientation);
     }
+	
+	class DoubleTapGestureDetector extends GestureDetector.SimpleOnGestureListener {
 
-	@Override
+        @Override
-	public boolean onTouchEvent(MotionEvent event) {
+        public boolean onDoubleTap(MotionEvent event) {
-		// Pass the touch event to the native layer for camera control.
+        	float normalizedX = event.getX(0) / mScreenSize.x;
-		// Single touch to rotate the camera around the device.
-		// Two fingers to zoom in and out.
-		int pointCount = event.getPointerCount();
-		if (pointCount == 1) {
-			float normalizedX = event.getX(0) / mScreenSize.x;
 			float normalizedY = event.getY(0) / mScreenSize.y;
-			RTABMapLib.onTouchEvent(1, 
+        	RTABMapLib.onTouchEvent(3, event.getActionMasked(), normalizedX, normalizedY, 0.0f, 0.0f);
-					event.getActionMasked(), normalizedX, normalizedY, 0.0f, 0.0f);
+            return true;
-		}
+        }
-		if (pointCount == 2) {
+    }
-			if (event.getActionMasked() == MotionEvent.ACTION_POINTER_UP) {
-				int index = event.getActionIndex() == 0 ? 1 : 0;
-				float normalizedX = event.getX(index) / mScreenSize.x;
-				float normalizedY = event.getY(index) / mScreenSize.y;
-				RTABMapLib.onTouchEvent(1, 
-						MotionEvent.ACTION_DOWN, normalizedX, normalizedY, 0.0f, 0.0f);
-			} else {
-				float normalizedX0 = event.getX(0) / mScreenSize.x;
-				float normalizedY0 = event.getY(0) / mScreenSize.y;
-				float normalizedX1 = event.getX(1) / mScreenSize.x;
-				float normalizedY1 = event.getY(1) / mScreenSize.y;
-				RTABMapLib.onTouchEvent(2, event.getActionMasked(),
-						normalizedX0, normalizedY0, normalizedX1, normalizedY1);
-			}
-		}
-		return true;
-	}
 
 	@Override
 	public boolean onCreateOptionsMenu(Menu menu) {
@@ -1568,15 +1595,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			.show();
 		}
 		else if(itemId == R.id.export_point_cloud ||
-				itemId == R.id.export_point_cloud_highrez ||
+				itemId == R.id.export_point_cloud_highrez)
-				itemId == R.id.export_mesh ||
-				itemId == R.id.export_mesh_texture)
 		{
-			final boolean isOBJ = itemId == R.id.export_mesh_texture || itemId == R.id.export_optimized_mesh_texture;
-			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;
 
-			export(isOBJ, meshing, regenerateCloud, false, 0);
+			export(false, false, regenerateCloud, false, 0);
 		}
 		else if(itemId == R.id.export_optimized_mesh ||
 				itemId == R.id.export_optimized_mesh_texture)

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

@@ -58,6 +58,7 @@ public class RTABMapLib
     
 
     public static native void setPausedMapping(boolean paused);
+    public static native void setOnlineBlending(boolean enabled);
     public static native void setMapCloudShown(boolean shown);
     public static native void setOdomCloudShown(boolean shown);
     public static native void setMeshRendering(boolean enabled, boolean withTexture);
@@ -85,6 +86,7 @@ public class RTABMapLib
     public static native void setClusterRatio(float value);
     public static native void setMaxGainRadius(float value);
     public static native void setRenderingTextureDecimation(int value);
+    public static native void setBackgroundColor(float gray);
     public static native int setMappingParameter(String key, String value);
 
     public static native void resetMapping();

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

@@ -41,6 +41,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 
 	private TextManager mTextManager = null;
 	private float mSurfaceHeight = 0.0f;
+	private float mTextColor = 1.0f;
 	
 	private Vector<TextObject> mTexts;
 
@@ -174,9 +175,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 		
 		// Create our text manager
 		mTextManager = new TextManager(mActivity);
-		
+		mTextManager.setColor(mTextColor);
-		GLES20.glEnable(GLES20.GL_BLEND);
-	    GLES20.glBlendFunc(GLES20.GL_ONE, GLES20.GL_ONE_MINUS_SRC_ALPHA);
 	}
 	
 	public void updateTexts(String[] texts)
@@ -208,4 +207,13 @@ public class Renderer implements GLSurfaceView.Renderer {
 			mTextChanged = true;
 		}
 	}
+	
+	public void setTextColor(float color) 
+	{
+		mTextColor = color;
+		if(mTextManager != null)
+		{
+			mTextManager.setColor(mTextColor);
+		}
+	}
 }

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

@@ -34,6 +34,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_point_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_point_size))).getEntry() + ") "+getString(R.string.pref_summary_point_size));
         ((Preference)findPreference(getString(R.string.pref_key_angle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_angle))).getEntry() + ") "+getString(R.string.pref_summary_angle));
         ((Preference)findPreference(getString(R.string.pref_key_triangle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_triangle))).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
@@ -89,6 +90,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_point_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_point_size));
         	if(key.compareTo(getString(R.string.pref_key_angle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_angle));
         	if(key.compareTo(getString(R.string.pref_key_triangle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));

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

@@ -29,23 +29,20 @@ public class TextManager {
 	public static final String vs_Text =
 		"uniform mat4 uMVPMatrix;" +
 		"attribute vec4 vPosition;" +
-		"attribute vec4 a_Color;" +
 		"attribute vec2 a_texCoord;" +
-		"varying vec4 v_Color;" + 
 		"varying vec2 v_texCoord;" +
 	    "void main() {" +
 	    "  gl_Position = uMVPMatrix * vPosition;" +
 	    "  v_texCoord = a_texCoord;" +
-	    "  v_Color = a_Color;" + 
 	    "}";
 	public static final String fs_Text =
 	    "precision mediump float;" +
-	    "varying vec4 v_Color;" +
+	    "uniform float uColor;" +
 	    "varying vec2 v_texCoord;" +
         "uniform sampler2D s_texture;" +
 	    "void main() {" +
-	    "  gl_FragColor = texture2D( s_texture, v_texCoord ) * v_Color;" +
+	    "  gl_FragColor = texture2D( s_texture, v_texCoord );" +
-	    "  gl_FragColor.rgb *= v_Color.a;" +
+	    "  gl_FragColor.rgb *= uColor;" +
 	    "}";
 	
 	public static int sp_Text;
@@ -60,21 +57,19 @@ public class TextManager {
 	private float mUVWidth;
 	private float mUVHeight;
 	private float mTextHeight;
+	private float mColor;
 	
 	private FloatBuffer vertexBuffer;
 	private FloatBuffer textureBuffer;
-	private FloatBuffer colorBuffer;
 	private ShortBuffer drawListBuffer;
 	
 	private float[] vecs;
 	private float[] uvs;
 	private short[] indices;
-	private float[] colors;
 	
 	private int index_vecs;
 	private int index_indices;
 	private int index_uvs;
-	private int index_colors;
 
 	private int texturenr;
 	private int[] mTextures;
@@ -87,7 +82,6 @@ public class TextManager {
 	{
 		// Create the arrays
 		vecs = new float[3 * 10];
-		colors = new float[4 * 10];
 		uvs = new float[2 * 10];
 		indices = new short[10];
 		
@@ -133,6 +127,7 @@ public class TextManager {
 		}
 		mUVWidth = (float)RI_TEXT_HEIGHT_BASE/(float)RI_TEXT_TEXTURE_SIZE;
 		mUVHeight = mTextHeight/(float)RI_TEXT_TEXTURE_SIZE;
+		mColor = 1.0f;
 		
 		int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
 		mCharacterWidth = new float[RI_TEXT_STOP-RI_TEXT_START];
@@ -190,13 +185,6 @@ public class TextManager {
 			index_vecs++;
 		}
 		
-		// We should add the colors, so we can use the same texture for multiple effects.
-		for(int i=0;i<cs.length;i++)
-		{
-			colors[index_colors] = cs[i];
-			index_colors++;
-		}
-		
 		// We should add the uvs
 		for(int i=0;i<uv.length;i++)
 		{
@@ -218,7 +206,6 @@ public class TextManager {
 		index_vecs = 0;
 		index_indices = 0;
 		index_uvs = 0;
-		index_colors = 0;
 		
 		// Get the total amount of characters
 		int charcount = 0;
@@ -234,12 +221,10 @@ public class TextManager {
 		
 		// Create the arrays we need with the correct size.
 		vecs = null;
-		colors = null;
 		uvs = null;
 		indices = null;
 		
 		vecs = new float[charcount * 12];
-		colors = new float[charcount * 16];
 		uvs = new float[charcount * 8];
 		indices = new short[charcount * 6];
 
@@ -269,6 +254,7 @@ public class TextManager {
 		if(vecs.length > 0)
 		{
 			GLES20.glDisable(GLES20.GL_DEPTH_TEST);
+			GLES20.glEnable(GLES20.GL_BLEND);
 			
 			// Set the correct shader for our grid object.
 			GLES20.glUseProgram(sp_Text);
@@ -280,13 +266,6 @@ public class TextManager {
 			vertexBuffer.put(vecs);
 			vertexBuffer.position(0);
 			
-			// The vertex buffer.
-			ByteBuffer bb3 = ByteBuffer.allocateDirect(colors.length * 4);
-			bb3.order(ByteOrder.nativeOrder());
-			colorBuffer = bb3.asFloatBuffer();
-			colorBuffer.put(colors);
-			colorBuffer.position(0);
-			
 			// The texture buffer
 			ByteBuffer bb2 = ByteBuffer.allocateDirect(uvs.length * 4);
 			bb2.order(ByteOrder.nativeOrder());
@@ -322,22 +301,16 @@ public class TextManager {
 		    GLES20.glEnableVertexAttribArray ( mPositionHandle );
 	        GLES20.glEnableVertexAttribArray ( mTexCoordLoc );
 	
-	        int mColorHandle = GLES20.glGetAttribLocation(sp_Text, "a_Color");
-	
-		    // Enable a handle to the triangle vertices
-		    GLES20.glEnableVertexAttribArray(mColorHandle);
-	
-		    // Prepare the background coordinate data
-		    GLES20.glVertexAttribPointer(mColorHandle, 4,
-		                                 GLES20.GL_FLOAT, false,
-		                                 0, colorBuffer);
-	
 		    // get handle to shape's transformation matrix
 	        int mtrxhandle = GLES20.glGetUniformLocation(sp_Text, "uMVPMatrix");
 	        
 	    	// Apply the projection and view transformation
 	        GLES20.glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
 	        
+	        // get handle to color value
+	        int colorhandle = GLES20.glGetUniformLocation(sp_Text, "uColor");
+	        GLES20.glUniform1f(colorhandle, mColor);
+	        
 	        int mSamplerLoc = GLES20.glGetUniformLocation (sp_Text, "s_texture" );
 	        
 	        // Texture activate unit 0
@@ -353,7 +326,6 @@ public class TextManager {
 	        // Disable vertex array
 	        GLES20.glDisableVertexAttribArray(mPositionHandle);
 	        GLES20.glDisableVertexAttribArray(mTexCoordLoc);
-	        GLES20.glDisableVertexAttribArray(mColorHandle);
 		}
 	}
 	
@@ -440,4 +412,8 @@ public class TextManager {
 	public void setUniformscale(float uniformscale) {
 		this.uniformscale = uniformscale;
 	}
+	
+	public void setColor(float color) {
+		mColor = color;
+	}
 }