Tango: Added texture to meshes #57

2026-10-04 08:47:45 +08:00 · 2016-03-21 15:45:26 -04:00
parent 1b4539a0df
commit bc78f789eb
6 changed files with 381 additions and 133 deletions
@@ -1,46 +1,95 @@
-
 /*
- * Copyright 2014 Google Inc. 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.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      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_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		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_);
-		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, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	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);
-			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+			textures_ = 0;

-			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();
-				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];
-					}
-				}
+				polygons_[oi++] = (unsigned short)polygons[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_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		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_);
-		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, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	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);
-			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+			textures_ = 0;

-			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();
-				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];
-					}
-				}
+				polygons_[oi++] = (unsigned short)polygons[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");
-		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())
+		if(textures_)
 		{
-			glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_SHORT, indices_.data());
-		}
-		else
-		{
-			glDrawArrays(GL_POINTS, 0, nPoints_);
-		}
+			// Texture activate unit 0
+			glActiveTexture(GL_TEXTURE0);
+			// Bind the texture to this unit.
+			glBindTexture(GL_TEXTURE_2D, textures_);
+			// 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);