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

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_;
-	  axis_ = 0;
-	  delete frustum_;
-	  delete trace_;
-	  delete grid_;
-	  delete currentPose_;
-	  delete box_;
+		delete axis_;
+		axis_ = 0;
+		delete frustum_;
+		delete trace_;
+		delete grid_;
+		delete currentPose_;
+		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) {
-    LOGE("Setup graphic height not valid");
-  }
-  UASSERT(gesture_camera_ != 0);
-  gesture_camera_->SetAspectRatio(static_cast<float>(w) /
-                                  static_cast<float>(h));
-  glViewport(0, 0, w, h);
+	if (h == 0) {
+		LOGE("Setup graphic height not valid");
+	}
+	UASSERT(gesture_camera_ != 0);
+	gesture_camera_->SetAspectRatio(static_cast<float>(w) / static_cast<float>(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);
-	if(backfaceCulling_)
-	{
-		glEnable(GL_CULL_FACE);
-	}
-	else
-	{
-		glDisable(GL_CULL_FACE);
-	}
-
-	glClearColor(r_, g_, b_, 1.0f);
-	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
+	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;
 	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;
-	rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov);//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
+	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
+	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;
-	if(mapRendering_ && frustumCulling_)
+	std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
+
+	// 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_)
-					{
-						box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
-						box_->Render(gesture_camera_->GetProjectionMatrix(),
-											gesture_camera_->GetViewMatrix());
-					}
-
-					++cloudDrawn;
+					usedForDepth.insert(iter->first);
 					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);
+					float distanceToCameraSqr = 999.0f; // set it large to use low res polygons for fast processing
+					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(),
-										gesture_camera_->GetViewMatrix());
+					box_->Render(projectionMatrix, viewMatrix);
 				}
 
 				++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_)
-	{
-		graph_->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix());
-	}
-
-	return cloudDrawn;
+	return 1;
 }
 
 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;
-  far_clip_plane_ = 100.0f;
+  near_clip_plane_ = 0.2f;
+  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);