diff --git a/app/android/.settings/org.eclipse.jdt.core.prefs b/app/android/.settings/org.eclipse.jdt.core.prefs new file mode 100644 index 00000000..8000cd6c --- /dev/null +++ b/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 diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index a7a986f7..776140bc 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/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 polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels); pcl::PointCloud::Ptr outputCloud(new pcl::PointCloud); std::vector 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 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; diff --git a/app/android/jni/point_cloud_drawable.cpp b/app/android/jni/point_cloud_drawable.cpp index d8668ebf..42626011 100644 --- a/app/android/jni/point_cloud_drawable.cpp +++ b/app/android/jni/point_cloud_drawable.cpp @@ -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 #include "point_cloud_drawable.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UConversion.h" +#include #include "util.h" #include PointCloudDrawable::PointCloudDrawable( - GLuint shaderProgram, + GLuint cloudShaderProgram, + GLuint textureShaderProgram, const pcl::PointCloud::Ptr & cloud, - const std::vector & indices) : + const std::vector & 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 vertices = std::vector(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 vertices; + if(textures_) + { + vertices = std::vector(cloud->size()*5); + for(unsigned int i=0; isize(); ++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(cloud->size()*4); for(unsigned int i=0; isize(); ++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::Ptr & cloud, - const std::vector & indices) : + const std::vector & 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 vertices = std::vector(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 vertices; + if(textures_) + { + vertices = std::vector(cloud->size()*5); + for(unsigned int i=0; isize(); ++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(cloud->size()*4); for(unsigned int i=0; isize(); ++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::Ptr & cloud, - const std::vector & indices = std::vector()); + const std::vector & polygons = std::vector(), + const cv::Mat & image = cv::Mat()); PointCloudDrawable( - GLuint shaderProgram, + GLuint cloudShaderProgram, + GLuint textureShaderProgram, const pcl::PointCloud::Ptr & cloud, - const std::vector & indices = std::vector()); + const std::vector & polygons = std::vector(), + 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 indices_; + GLuint textures_; + std::vector polygons_; int nPoints_; glm::mat4 pose_; bool visible_; diff --git a/app/android/jni/scene.cpp b/app/android/jni/scene.cpp index aafd5bad..53259d01 100644 --- a/app/android/jni/scene.cpp +++ b/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::Ptr & cloud, const std::vector & polygons, - const rtabmap::Transform & pose) + const rtabmap::Transform & pose, + const cv::Mat & image) { LOGI("addOrUpdateCloud cloud %d", id); std::map::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::Ptr & cloud, const std::vector & polygons, - const rtabmap::Transform & pose) + const rtabmap::Transform & pose, + const cv::Mat & image) { LOGI("addOrUpdateCloud cloud %d", id); std::map::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)); } diff --git a/app/android/jni/scene.h b/app/android/jni/scene.h index 89f1e3d6..a58494bc 100644 --- a/app/android/jni/scene.h +++ b/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::Ptr & cloud, const std::vector & polygons, - const rtabmap::Transform & pose); - void addOrUpdateCloud( + const rtabmap::Transform & pose, + const cv::Mat & image = cv::Mat()); + void addCloud( int id, const pcl::PointCloud::Ptr & cloud, const std::vector & 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_;