Tango: Added texture to meshes #57

app/android/.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,11 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.6
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.6

app/android/jni/RTABMapApp.cpp

@@ -54,6 +54,7 @@ const int meshTrianglePixels = 1;
 const bool substractFiltering = false;
 const float subtractRadius = 0.02;
 const float subtractMaxAngle = M_PI/4.0f;
+const bool textureMeshing = true;
 const int minNeighborsInRadius = 5;
 const float closeVerticesDistance = 0.02f;
 
@@ -90,7 +91,6 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerVarianceIgnored(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("700")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemUseDepthAsMask(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisRefineIterations(), std::string("5")));
 
@@ -458,17 +458,34 @@ int RTABMapApp::Render()
 								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels);
 								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 								std::vector<pcl::Vertices> outputPolygons;
-								rtabmap::util3d::filterNotUsedVerticesFromMesh(
+
-										*output,
+								if(!textureMeshing)
-										polygons,
+								{
-										*outputCloud,
+									rtabmap::util3d::filterNotUsedVerticesFromMesh(
-										outputPolygons);
+											*output,
+											polygons,
+											*outputCloud,
+											outputPolygons);
+								}
+								else
+								{
+									outputCloud = output;
+									outputPolygons = polygons;
+								}
 								LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
 
-								if(outputCloud->size())
+								if(outputCloud->size() && (!textureMeshing || outputPolygons.size()))
 								{
 									totalPolygons_ += outputPolygons.size();
-									main_scene_.addOrUpdateCloud(id, outputCloud, outputPolygons, iter->second);
+									if(textureMeshing)
+									{
+										main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
+									}
+									else
+									{
+										main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second);
+									}
+
 
 									// protect createdMeshes_ used also by exportMesh() method
 									boost::mutex::scoped_lock  lock(meshesMutex_);
@@ -529,7 +546,7 @@ int RTABMapApp::Render()
 					if(cloud->size())
 					{
 						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleTolerance, false, meshTrianglePixels);
-						main_scene_.addOrUpdateCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose());
+						main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), textureMeshing?event.data().imageRaw():cv::Mat());
 						main_scene_.setCloudVisible(-1, true);
 					}
 					else
@@ -751,7 +768,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 							uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
 					words = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
 					updateTime = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
-					loopClosureId = rtabmapEvents_.back().loopClosureId()>0?rtabmapEvents_.back().loopClosureId():rtabmapEvents_.back().localLoopClosureId()>0?rtabmapEvents_.back().localLoopClosureId():0;
+					loopClosureId = rtabmapEvents_.back().loopClosureId()>0?rtabmapEvents_.back().loopClosureId():rtabmapEvents_.back().proximityDetectionId()>0?rtabmapEvents_.back().proximityDetectionId():0;
 					databaseMemoryUsed = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
 					inliers = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
 					featuresExtracted = rtabmapEvents_.back().getSignatures().size()?rtabmapEvents_.back().getSignatures().rbegin()->second.getWords().size():0;

app/android/jni/point_cloud_drawable.cpp

@@ -1,46 +1,95 @@
-
 /*
- * Copyright 2014 Google Inc. All Rights Reserved.
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- * Licensed under the Apache License, Version 2.0 (the "License");
+
- * you may not use this file except in compliance with the License.
+Redistribution and use in source and binary forms, with or without
- * You may obtain a copy of the License at
+modification, are permitted provided that the following conditions are met:
- *
+    * Redistributions of source code must retain the above copyright
- *      http://www.apache.org/licenses/LICENSE-2.0
+      notice, this list of conditions and the following disclaimer.
- *
+    * Redistributions in binary form must reproduce the above copyright
- * Unless required by applicable law or agreed to in writing, software
+      notice, this list of conditions and the following disclaimer in the
- * distributed under the License is distributed on an "AS IS" BASIS,
+      documentation and/or other materials provided with the distribution.
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    * Neither the name of the Universite de Sherbrooke nor the
- * See the License for the specific language governing permissions and
+      names of its contributors may be used to endorse or promote products
- * limitations under the License.
+      derived from this software without specific prior written permission.
- */
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #include <sstream>
 
 #include "point_cloud_drawable.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc.hpp>
 #include "util.h"
 
 #include <GLES2/gl2.h>
 
 PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
+		GLuint cloudShaderProgram,
+		GLuint textureShaderProgram,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & indices) :
+		const std::vector<pcl::Vertices> & polygons,
+		const cv::Mat & image) :
 		vertex_buffers_(0),
+		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
-		shader_program_(shaderProgram)
+		shader_program_(cloudShaderProgram!=0?cloudShaderProgram:textureShaderProgram)
 {
 	UASSERT(!cloud->empty());
 
 	glGenBuffers(1, &vertex_buffers_);
-
+	if(!vertex_buffers_)
-	if(vertex_buffers_)
 	{
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
+		LOGE("OpenGL: could not generate vertex buffers\n");
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+		return;
+	}
+
+	if(textureShaderProgram)
+	{
+		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
+		UASSERT_MSG(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3,
+				uFormat("polygons=%d dense=%d image=%d", (int)polygons.size(), cloud->is_dense?1:0, image.type()).c_str());
+		glGenTextures(1, &textures_);
+		if(!textures_)
+		{
+			vertex_buffers_ = 0;
+			LOGE("OpenGL: could not generate vertex buffers\n");
+			return;
+		}
+	}
+
+	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices;
+	if(textures_)
+	{
+		vertices = std::vector<float>(cloud->size()*5);
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			vertices[i*5] = cloud->at(i).x;
+			vertices[i*5+1] = cloud->at(i).y;
+			vertices[i*5+2] = cloud->at(i).z;
+
+			// texture uv
+			vertices[i*5+3] = float(i % cloud->width)/float(cloud->width); //u
+			vertices[i*5+4] = float(i/cloud->width)/float(cloud->height);  //v
+		}
+	}
+	else
+	{
+		vertices = std::vector<float>(cloud->size()*4);
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			vertices[i*4] = cloud->at(i).x;
@@ -48,57 +97,116 @@ PointCloudDrawable::PointCloudDrawable(
 			vertices[i*4+2] = cloud->at(i).z;
 			vertices[i*4+3] = cloud->at(i).rgb;
 		}
+	}
 
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	if(textures_)
+	{
+		// gen texture from image
+		glBindTexture(GL_TEXTURE_2D, textures_);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		cv::Mat rgbImage;
+		cv::cvtColor(image, rgbImage, CV_BGR2RGB);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, (const void *)rgbImage.data);
+		glBindTexture(GL_TEXTURE_2D, 0);
 
 		GLint error = glGetError();
 		if(error != GL_NO_ERROR)
 		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			vertex_buffers_ = 0;
+			textures_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
 
-			if(indices.size())
+			glDeleteBuffers(1, &vertex_buffers_);
+			vertex_buffers_ = 0;
+			return;
+		}
+	}
+
+	nPoints_ = cloud->size();
+
+	if(polygons.size())
+	{
+		int polygonSize = polygons[0].vertices.size();
+		UASSERT(polygonSize == 3);
+		polygons_.resize(polygons.size() * polygonSize);
+		int oi = 0;
+		for(unsigned int i=0; i<polygons.size(); ++i)
+		{
+			UASSERT((int)polygons[i].vertices.size() == polygonSize);
+			for(int j=0; j<polygonSize; ++j)
 			{
-				int polygonSize = indices[0].vertices.size();
+				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
 			}
 		}
 	}
 }
+
 PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
+		GLuint cloudShaderProgram,
+		GLuint textureShaderProgram,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & indices) :
+		const std::vector<pcl::Vertices> & polygons,
+		const cv::Mat & image) :
 		vertex_buffers_(0),
+		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
-		shader_program_(shaderProgram)
+		shader_program_(cloudShaderProgram!=0?cloudShaderProgram:textureShaderProgram)
 {
 	UASSERT(!cloud->empty());
 
 	glGenBuffers(1, &vertex_buffers_);
-
+	if(!vertex_buffers_)
-	if(vertex_buffers_)
 	{
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
+		LOGE("OpenGL: could not generate vertex buffers\n");
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+		return;
+	}
+
+	if(textureShaderProgram)
+	{
+		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
+		UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
+		glGenTextures(1, &textures_);
+		if(!textures_)
+		{
+			vertex_buffers_ = 0;
+			LOGE("OpenGL: could not generate vertex buffers\n");
+			return;
+		}
+	}
+
+	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices;
+	if(textures_)
+	{
+		vertices = std::vector<float>(cloud->size()*5);
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			vertices[i*5] = cloud->at(i).x;
+			vertices[i*5+1] = cloud->at(i).y;
+			vertices[i*5+2] = cloud->at(i).z;
+
+			// texture uv
+			vertices[i*5+3] = float(i % cloud->width)/float(cloud->width); //u
+			vertices[i*5+4] = float(i/cloud->width)/float(cloud->height);  //v
+		}
+	}
+	else
+	{
+		vertices = std::vector<float>(cloud->size()*4);
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			vertices[i*4] = cloud->at(i).x;
@@ -106,35 +214,56 @@ PointCloudDrawable::PointCloudDrawable(
 			vertices[i*4+2] = cloud->at(i).z;
 			vertices[i*4+3] = cloud->at(i).rgb;
 		}
+	}
 
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	if(textures_)
+	{
+		// gen texture from image
+		glBindTexture(GL_TEXTURE_2D, textures_);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		cv::Mat rgbImage;
+		cv::cvtColor(image, rgbImage, CV_BGR2RGB);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
 
 		GLint error = glGetError();
 		if(error != GL_NO_ERROR)
 		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			vertex_buffers_ = 0;
+			textures_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
 
-			if(indices.size())
+			glDeleteBuffers(1, &vertex_buffers_);
+			vertex_buffers_ = 0;
+			return;
+		}
+	}
+
+	nPoints_ = cloud->size();
+
+	if(polygons.size())
+	{
+		int polygonSize = polygons[0].vertices.size();
+		UASSERT(polygonSize == 3);
+		polygons_.resize(polygons.size() * polygonSize);
+		int oi = 0;
+		for(unsigned int i=0; i<polygons.size(); ++i)
+		{
+			UASSERT((int)polygons[i].vertices.size() == polygonSize);
+			for(int j=0; j<polygonSize; ++j)
 			{
-				int polygonSize = indices[0].vertices.size();
+				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
 			}
 		}
 	}
@@ -149,6 +278,13 @@ PointCloudDrawable::~PointCloudDrawable()
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
 		vertex_buffers_ = 0;
 	}
+
+	if (textures_)
+	{
+		glDeleteTextures(1, &textures_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		textures_ = 0;
+	}
 }
 
 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
@@ -168,27 +304,50 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 		glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
 		glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
 
-		GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
+		if(textures_)
-		glUniform1f(point_size_handle_, pointSize);
-
-		GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
-		GLint attribute_color = glGetAttribLocation(shader_program_, "color");
-
-		glEnableVertexAttribArray(attribute_vertex);
-		glEnableVertexAttribArray(attribute_color);
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
-		glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
-
-		if(meshRendering && indices_.size())
 		{
-			glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_SHORT, indices_.data());
+			// Texture activate unit 0
-		}
+			glActiveTexture(GL_TEXTURE0);
-		else
+			// Bind the texture to this unit.
-		{
+			glBindTexture(GL_TEXTURE_2D, textures_);
-			glDrawArrays(GL_POINTS, 0, nPoints_);
+			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
-		}
+			GLuint texture_handle = glGetUniformLocation(shader_program_, "u_Texture");
+			glUniform1i(texture_handle, 0);
 
+			GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
+			GLint attribute_texture = glGetAttribLocation(shader_program_, "a_TexCoordinate");
+
+			glEnableVertexAttribArray(attribute_vertex);
+			glEnableVertexAttribArray(attribute_texture);
+			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 5*sizeof(GLfloat), 0);
+			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 5*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+
+			glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
+		}
+		else // point cloud or colored mesh
+		{
+			GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
+			glUniform1f(point_size_handle_, pointSize);
+
+			GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
+			GLint attribute_color = glGetAttribLocation(shader_program_, "color");
+
+			glEnableVertexAttribArray(attribute_vertex);
+			glEnableVertexAttribArray(attribute_color);
+			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
+			glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+
+			if(meshRendering && polygons_.size())
+			{
+				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
+			}
+			else
+			{
+				glDrawArrays(GL_POINTS, 0, nPoints_);
+			}
+		}
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
 

app/android/jni/point_cloud_drawable.h

@@ -1,18 +1,29 @@
 /*
- * Copyright 2014 Google Inc. All Rights Reserved.
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- * Licensed under the Apache License, Version 2.0 (the "License");
+
- * you may not use this file except in compliance with the License.
+Redistribution and use in source and binary forms, with or without
- * You may obtain a copy of the License at
+modification, are permitted provided that the following conditions are met:
- *
+    * Redistributions of source code must retain the above copyright
- *      http://www.apache.org/licenses/LICENSE-2.0
+      notice, this list of conditions and the following disclaimer.
- *
+    * Redistributions in binary form must reproduce the above copyright
- * Unless required by applicable law or agreed to in writing, software
+      notice, this list of conditions and the following disclaimer in the
- * distributed under the License is distributed on an "AS IS" BASIS,
+      documentation and/or other materials provided with the distribution.
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    * Neither the name of the Universite de Sherbrooke nor the
- * See the License for the specific language governing permissions and
+      names of its contributors may be used to endorse or promote products
- * limitations under the License.
+      derived from this software without specific prior written permission.
- */
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 #define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
@@ -31,13 +42,17 @@
 class PointCloudDrawable {
  public:
   PointCloudDrawable(
-		  GLuint shaderProgram,
+		  GLuint cloudShaderProgram,
+		  GLuint textureShaderProgram,
 		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
+		  const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
+		  const cv::Mat & image = cv::Mat());
   PointCloudDrawable(
-  		  GLuint shaderProgram,
+		  GLuint cloudShaderProgram,
+		  GLuint textureShaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
+  		  const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
+		  const cv::Mat & image = cv::Mat());
   virtual ~PointCloudDrawable();
 
   void setPose(const rtabmap::Transform & pose);
@@ -56,7 +71,8 @@ class PointCloudDrawable {
  private:
   // Vertex buffer of the point cloud geometry.
   GLuint vertex_buffers_;
-  std::vector<GLushort> indices_;
+  GLuint textures_;
+  std::vector<GLushort> polygons_;
   int nPoints_;
   glm::mat4 pose_;
   bool visible_;

app/android/jni/scene.cpp

@@ -60,6 +60,26 @@ const std::string kPointCloudFragmentShader =
     "  gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
     "}\n";
 
+const std::string kTextureMeshVertexShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+    "attribute vec3 vertex;\n"
+    "attribute vec2 a_TexCoordinate;\n"
+    "uniform mat4 mvp;\n"
+    "varying vec2 v_TexCoordinate;\n"
+    "void main() {\n"
+    "  gl_Position = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
+    "  v_TexCoordinate = a_TexCoordinate;\n"
+    "}\n";
+const std::string kTextureMeshFragmentShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+	"uniform sampler2D u_Texture;\n"
+    "varying vec2 v_TexCoordinate;\n"
+    "void main() {\n"
+    "  gl_FragColor = texture2D(u_Texture, v_TexCoordinate);\n"
+    "}\n";
+
 const std::string kGraphVertexShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
@@ -92,6 +112,7 @@ Scene::Scene() :
 		traceVisible_(true),
 		currentPose_(0),
 		cloud_shader_program_(0),
+		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
 		meshRendering_(true),
 		pointSize_(3.0f) {}
@@ -130,6 +151,11 @@ void Scene::InitGLContent()
 	  cloud_shader_program_ = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
 	  UASSERT(cloud_shader_program_ != 0);
   }
+  if(texture_mesh_shader_program_ == 0)
+  {
+	  texture_mesh_shader_program_ = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
+	  UASSERT(texture_mesh_shader_program_ != 0);
+  }
   if(graph_shader_program_ == 0)
   {
 	  graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
@@ -156,6 +182,10 @@ void Scene::DeleteResources() {
 		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;
@@ -372,11 +402,12 @@ void Scene::setTraceVisible(bool visible)
 }
 
 //Should only be called in OpenGL thread!
-void Scene::addOrUpdateCloud(
+void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const std::vector<pcl::Vertices> & polygons,
-		const rtabmap::Transform & pose)
+		const rtabmap::Transform & pose,
+		const cv::Mat & image)
 {
 	LOGI("addOrUpdateCloud cloud %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
@@ -387,16 +418,22 @@ void Scene::addOrUpdateCloud(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0);
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			image.empty()?cloud_shader_program_:0,
+			image.empty()?0:texture_mesh_shader_program_,
+			cloud,
+			polygons,
+			image);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
-void Scene::addOrUpdateCloud(
+void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const std::vector<pcl::Vertices> & polygons,
-		const rtabmap::Transform & pose)
+		const rtabmap::Transform & pose,
+		const cv::Mat & image)
 {
 	LOGI("addOrUpdateCloud cloud %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
@@ -407,8 +444,13 @@ void Scene::addOrUpdateCloud(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0);
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			image.empty()?cloud_shader_program_:0,
+			image.empty()?0:texture_mesh_shader_program_,
+			cloud,
+			polygons,
+			image);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }

app/android/jni/scene.h

@@ -96,16 +96,18 @@ class Scene {
   void setGraphVisible(bool visible);
   void setTraceVisible(bool visible);
 
-  void addOrUpdateCloud(
+  void addCloud(
 		  int id,
 		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		  const std::vector<pcl::Vertices> & polygons,
-		  const rtabmap::Transform & pose);
+		  const rtabmap::Transform & pose,
-  void addOrUpdateCloud(
+		  const cv::Mat & image = cv::Mat());
+  void addCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
   		  const std::vector<pcl::Vertices> & polygons,
-  		  const rtabmap::Transform & pose);
+  		  const rtabmap::Transform & pose,
+		  const cv::Mat & image = cv::Mat());
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
@@ -140,6 +142,7 @@ class Scene {
 
   // Shader to display point cloud.
   GLuint cloud_shader_program_;
+  GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
   bool meshRendering_;