i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tango: Added texture to meshes #57

Browse Source
This commit is contained in:
matlabbe
2016-03-21 15:45:26 -04:00
parent 1b4539a0df
commit bc78f789eb
6 changed files with 381 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
app/android/.settings/org.eclipse.jdt.core.prefs Normal file
View File

@@ -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

37
app/android/jni/RTABMapApp.cpp
View File

@@ -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,
polygons,
*outputCloud,
outputPolygons);
if(!textureMeshing)
{
rtabmap::util3d::filterNotUsedVerticesFromMesh(
*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;

343
app/android/jni/point_cloud_drawable.cpp
View File

@@ -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);

54
app/android/jni/point_cloud_drawable.h
View File

@@ -1,18 +1,29 @@
/*
* 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.
*/
#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_;

58
app/android/jni/scene.cpp
View File

@@ -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);
PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
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);
PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
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));
}

11
app/android/jni/scene.h
View File

@@ -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);
void addOrUpdateCloud(
const rtabmap::Transform & pose,
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_;