X-lanni
1259 lines
41 KiB
C++
1259 lines
41 KiB
C++
/*
|
|
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/UTimer.h"
|
|
#include "rtabmap/utilite/UConversion.h"
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
#include "util.h"
|
|
#include "pcl/common/transforms.h"
|
|
|
|
#ifdef __ANDROID__
|
|
#include <GLES2/gl2.h>
|
|
#else // __APPLE__
|
|
#include <OpenGLES/ES2/gl.h>
|
|
#endif
|
|
|
|
#define LOW_DEC 2
|
|
#define LOWLOW_DEC 4
|
|
|
|
enum PointCloudShaders
|
|
{
|
|
kPointCloud = 0,
|
|
kPointCloudBlending = 1,
|
|
kPointCloudLighting = 2,
|
|
kPointCloudLightingBlending = 3,
|
|
|
|
kTexture = 4,
|
|
kTextureBlending = 5,
|
|
kTextureLighting = 6,
|
|
kTextureLightingBlending = 7,
|
|
|
|
kDepthPacking = 8
|
|
};
|
|
|
|
// PointCloud shaders
|
|
const std::string kPointCloudVertexShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"attribute vec3 aVertex;\n"
|
|
"attribute vec3 aColor;\n"
|
|
|
|
"uniform mat4 uMVP;\n"
|
|
"uniform float uPointSize;\n"
|
|
|
|
"varying vec3 vColor;\n"
|
|
"varying float vLightWeighting;\n"
|
|
|
|
"void main() {\n"
|
|
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
|
|
" gl_PointSize = uPointSize;\n"
|
|
" vLightWeighting = 1.0;\n"
|
|
" vColor = aColor;\n"
|
|
"}\n";
|
|
const std::string kPointCloudLightingVertexShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"attribute vec3 aVertex;\n"
|
|
"attribute vec3 aNormal;\n"
|
|
"attribute vec3 aColor;\n"
|
|
|
|
"uniform mat4 uMVP;\n"
|
|
"uniform mat3 uN;\n"
|
|
"uniform vec3 uLightingDirection;\n"
|
|
"uniform float uPointSize;\n"
|
|
|
|
"varying vec3 vColor;\n"
|
|
"varying float vLightWeighting;\n"
|
|
|
|
"void main() {\n"
|
|
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
|
|
" gl_PointSize = uPointSize;\n"
|
|
" vec3 transformedNormal = uN * aNormal;\n"
|
|
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
|
|
" if(vLightWeighting<0.5)"
|
|
" vLightWeighting=0.5;\n"
|
|
" vColor = aColor;\n"
|
|
"}\n";
|
|
|
|
const std::string kPointCloudFragmentShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"uniform float uGainR;\n"
|
|
"uniform float uGainG;\n"
|
|
"uniform float uGainB;\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"
|
|
"}\n";
|
|
const std::string kPointCloudBlendingFragmentShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"uniform float uGainR;\n"
|
|
"uniform float uGainG;\n"
|
|
"uniform float uGainB;\n"
|
|
"uniform float uNearZ;\n"
|
|
"uniform float uFarZ;\n"
|
|
"uniform sampler2D uDepthTexture;\n"
|
|
"uniform vec2 uScreenScale;\n"
|
|
"varying vec3 vColor;\n"
|
|
"varying float vLightWeighting;\n"
|
|
"void main() {\n"
|
|
" vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
|
|
" float alpha = 1.0;\n"
|
|
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
|
|
" vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
|
|
" float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
|
|
" float num = (2.0 * uNearZ * uFarZ);\n"
|
|
" float diff = (uFarZ - uNearZ);\n"
|
|
" float add = (uFarZ + uNearZ);\n"
|
|
" float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
|
|
" float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
|
|
" float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
|
|
" float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
|
|
" if(linearFragz > linearDepth + 0.05)\n"
|
|
" alpha=0.0;\n"
|
|
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
|
|
"}\n";
|
|
|
|
const std::string kPointCloudDepthPackingVertexShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"attribute vec3 aVertex;\n"
|
|
"uniform mat4 uMVP;\n"
|
|
"uniform float uPointSize;\n"
|
|
"void main() {\n"
|
|
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
|
|
" gl_PointSize = uPointSize;\n"
|
|
"}\n";
|
|
const std::string kPointCloudDepthPackingFragmentShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"void main() {\n"
|
|
" vec4 enc = vec4(1.,255.,65025.,16581375.) * gl_FragCoord.z;\n"
|
|
" enc = fract(enc);\n"
|
|
" enc -= enc.yzww * vec2(1./255., 0.).xxxy;\n"
|
|
" gl_FragColor = enc;\n"
|
|
"}\n";
|
|
|
|
// Texture shaders
|
|
const std::string kTextureMeshVertexShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"attribute vec3 aVertex;\n"
|
|
"attribute vec3 aColor;\n"
|
|
"attribute vec2 aTexCoord;\n"
|
|
|
|
"uniform mat4 uMVP;\n"
|
|
|
|
"varying vec3 vColor;\n"
|
|
"varying vec2 vTexCoord;\n"
|
|
"varying float vLightWeighting;\n"
|
|
|
|
"void main() {\n"
|
|
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
|
|
|
|
" if(aTexCoord.x < 0.0) {\n"
|
|
" vTexCoord.x = 1.0;\n"
|
|
" vTexCoord.y = 1.0;\n" // bottom right corner
|
|
" } else {\n"
|
|
" vTexCoord = aTexCoord;\n"
|
|
" }\n"
|
|
|
|
" vColor = aColor;\n"
|
|
" vLightWeighting = 1.0;\n"
|
|
"}\n";
|
|
const std::string kTextureMeshLightingVertexShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"attribute vec3 aVertex;\n"
|
|
"attribute vec3 aColor;\n"
|
|
"attribute vec3 aNormal;\n"
|
|
"attribute vec2 aTexCoord;\n"
|
|
|
|
"uniform mat4 uMVP;\n"
|
|
"uniform mat3 uN;\n"
|
|
"uniform vec3 uLightingDirection;\n"
|
|
|
|
"varying vec3 vColor;\n"
|
|
"varying vec2 vTexCoord;\n"
|
|
"varying float vLightWeighting;\n"
|
|
|
|
"void main() {\n"
|
|
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
|
|
|
|
" if(aTexCoord.x < 0.0) {\n"
|
|
" vTexCoord.x = 1.0;\n"
|
|
" vTexCoord.y = 1.0;\n" // bottom right corner
|
|
" } else {\n"
|
|
" vTexCoord = aTexCoord;\n"
|
|
" }\n"
|
|
|
|
" vColor = aColor;\n"
|
|
" vec3 transformedNormal = uN * aNormal;\n"
|
|
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
|
|
" if(vLightWeighting<0.5) \n"
|
|
" vLightWeighting=0.5;\n"
|
|
"}\n";
|
|
const std::string kTextureMeshFragmentShader =
|
|
"precision highp float;\n"
|
|
"precision mediump int;\n"
|
|
"uniform sampler2D uTexture;\n"
|
|
"uniform float uGainR;\n"
|
|
"uniform float uGainG;\n"
|
|
"uniform float uGainB;\n"
|
|
"uniform int uHideSeams;\n"
|
|
"varying vec3 vColor;\n"
|
|
"varying vec2 vTexCoord;\n"
|
|
"varying float vLightWeighting;\n"
|
|
""
|
|
"void main() {\n"
|
|
" vec4 textureColor;\n"
|
|
" if(uHideSeams==1) {\n"
|
|
" if(vTexCoord.x>0.99 && vTexCoord.y>0.99) {\n"
|
|
" textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
|
|
" } else {\n"
|
|
" textureColor = texture2D(uTexture, vTexCoord);\n"
|
|
" }\n"
|
|
" } else {\n"
|
|
" textureColor = texture2D(uTexture, vTexCoord) * vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
|
|
" }\n"
|
|
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
|
|
"}\n";
|
|
const std::string kTextureMeshBlendingFragmentShader =
|
|
"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 vec2 uScreenScale;\n"
|
|
"uniform float uNearZ;\n"
|
|
"uniform float uFarZ;\n"
|
|
"varying vec3 vColor;\n"
|
|
"varying vec2 vTexCoord;\n"
|
|
"varying float vLightWeighting;\n"
|
|
""
|
|
"void main() {\n"
|
|
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
|
|
" float alpha = 1.0;\n"
|
|
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
|
|
" vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
|
|
" float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
|
|
" float num = (2.0 * uNearZ * uFarZ);\n"
|
|
" float diff = (uFarZ - uNearZ);\n"
|
|
" float add = (uFarZ + uNearZ);\n"
|
|
" float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
|
|
" float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
|
|
" float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
|
|
" float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
|
|
" if(linearFragz > linearDepth + 0.05)\n"
|
|
" alpha=0.0;\n"
|
|
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
|
|
"}\n";
|
|
|
|
std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
|
|
|
|
void PointCloudDrawable::createShaderPrograms()
|
|
{
|
|
if(shaderPrograms_.empty())
|
|
{
|
|
shaderPrograms_.resize(9);
|
|
|
|
shaderPrograms_[kPointCloud] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kPointCloud] != 0);
|
|
shaderPrograms_[kPointCloudBlending] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kPointCloudBlending] != 0);
|
|
shaderPrograms_[kPointCloudLighting] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kPointCloudLighting] != 0);
|
|
shaderPrograms_[kPointCloudLightingBlending] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kPointCloudLightingBlending] != 0);
|
|
|
|
shaderPrograms_[kTexture] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kTexture] != 0);
|
|
shaderPrograms_[kTextureBlending] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kTextureBlending] != 0);
|
|
shaderPrograms_[kTextureLighting] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kTextureLighting] != 0);
|
|
shaderPrograms_[kTextureLightingBlending] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kTextureLightingBlending] != 0);
|
|
|
|
shaderPrograms_[kDepthPacking] = tango_gl::util::CreateProgram(kPointCloudDepthPackingVertexShader.c_str(), kPointCloudDepthPackingFragmentShader.c_str());
|
|
UASSERT(shaderPrograms_[kDepthPacking] != 0);
|
|
}
|
|
}
|
|
void PointCloudDrawable::releaseShaderPrograms()
|
|
{
|
|
for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
|
|
{
|
|
glDeleteShader(shaderPrograms_[i]);
|
|
}
|
|
shaderPrograms_.clear();
|
|
}
|
|
|
|
PointCloudDrawable::PointCloudDrawable(
|
|
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
|
|
const pcl::IndicesPtr & indices,
|
|
float gainR,
|
|
float gainG,
|
|
float gainB) :
|
|
vertex_buffer_(0),
|
|
texture_(0),
|
|
nPoints_(0),
|
|
pose_(rtabmap::Transform::getIdentity()),
|
|
poseGl_(1.0f),
|
|
visible_(true),
|
|
hasNormals_(false),
|
|
gainR_(gainR),
|
|
gainG_(gainG),
|
|
gainB_(gainB)
|
|
{
|
|
index_buffers_.resize(6, 0);
|
|
index_buffers_count_.resize(6, 0);
|
|
updateCloud(cloud, indices);
|
|
}
|
|
|
|
PointCloudDrawable::PointCloudDrawable(
|
|
const rtabmap::Mesh & mesh,
|
|
bool createWireframe) :
|
|
vertex_buffer_(0),
|
|
texture_(0),
|
|
nPoints_(0),
|
|
pose_(rtabmap::Transform::getIdentity()),
|
|
poseGl_(1.0f),
|
|
visible_(true),
|
|
hasNormals_(false),
|
|
gainR_(1.0f),
|
|
gainG_(1.0f),
|
|
gainB_(1.0f)
|
|
{
|
|
index_buffers_.resize(6, 0);
|
|
index_buffers_count_.resize(6, 0);
|
|
updateMesh(mesh, createWireframe);
|
|
}
|
|
|
|
PointCloudDrawable::~PointCloudDrawable()
|
|
{
|
|
LOGI("Freeing cloud buffer %d", vertex_buffer_);
|
|
if (vertex_buffer_)
|
|
{
|
|
glDeleteBuffers(1, &vertex_buffer_);
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
|
|
vertex_buffer_ = 0;
|
|
}
|
|
|
|
if (texture_)
|
|
{
|
|
glDeleteTextures(1, &texture_);
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
|
|
texture_ = 0;
|
|
}
|
|
|
|
for(size_t i=0; i<index_buffers_.size(); ++i)
|
|
{
|
|
if(index_buffers_[i])
|
|
{
|
|
glDeleteBuffers(1, &index_buffers_[i]);
|
|
index_buffers_[i] = 0;
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
|
|
}
|
|
}
|
|
}
|
|
|
|
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
|
|
{
|
|
for(int i=0; i<4; ++i)
|
|
{
|
|
if(index_buffers_[i])
|
|
{
|
|
glDeleteBuffers(1, &index_buffers_[i]);
|
|
index_buffers_[i] = 0;
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
|
|
}
|
|
}
|
|
|
|
//LOGD("Update polygons");
|
|
if(polygons.size() && organizedToDenseIndices_.size())
|
|
{
|
|
size_t polygonSize = polygons[0].vertices.size();
|
|
UASSERT(polygonSize == 3);
|
|
std::vector<std::vector<GLuint> > indexes(4);
|
|
indexes[0].resize(polygons.size() * polygonSize);
|
|
if(createWireframe)
|
|
indexes[2].resize(indexes[0].size()*2);
|
|
int oi = 0;
|
|
int li = 0;
|
|
for(size_t i=0; i<polygons.size(); ++i)
|
|
{
|
|
UASSERT(polygons[i].vertices.size() == polygonSize);
|
|
for(unsigned int j=0; j<polygonSize; ++j)
|
|
{
|
|
indexes[0][oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
|
|
if(createWireframe)
|
|
{
|
|
indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
|
|
indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(polygonsLowRes.size())
|
|
{
|
|
size_t polygonSize = polygonsLowRes[0].vertices.size();
|
|
UASSERT(polygonSize == 3);
|
|
indexes[1].resize(polygonsLowRes.size() * polygonSize);
|
|
if(createWireframe)
|
|
indexes[3].resize(indexes[1].size()*2);
|
|
int oi = 0;
|
|
int li = 0;
|
|
for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
|
|
{
|
|
UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
|
|
for(unsigned int j=0; j<polygonSize; ++j)
|
|
{
|
|
indexes[1][oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
|
|
if(createWireframe)
|
|
{
|
|
indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
|
|
indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Generate index buffers
|
|
for(size_t i=0; i<indexes.size(); ++i)
|
|
{
|
|
if(!indexes[i].empty())
|
|
{
|
|
glGenBuffers(1, &index_buffers_[i]);
|
|
if(!index_buffers_[i])
|
|
{
|
|
LOGE("OpenGL: could not generate index buffer %ld\n", i);
|
|
return;
|
|
}
|
|
|
|
LOGD("Adding polygon index %ld size=%ld", i, indexes[i].size());
|
|
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
|
|
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * indexes[i].size(), indexes[i].data(), GL_STATIC_DRAW);
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
|
index_buffers_count_[i] = (int)indexes[i].size();
|
|
|
|
GLint error = glGetError();
|
|
if(error != GL_NO_ERROR)
|
|
{
|
|
LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
|
|
index_buffers_[i] = 0;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices)
|
|
{
|
|
UASSERT(cloud.get() && !cloud->empty());
|
|
nPoints_ = 0;
|
|
aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
|
|
aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
|
|
|
|
if (vertex_buffer_)
|
|
{
|
|
glDeleteBuffers(1, &vertex_buffer_);
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
|
|
vertex_buffer_ = 0;
|
|
}
|
|
|
|
if (texture_)
|
|
{
|
|
glDeleteTextures(1, &texture_);
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
|
|
texture_ = 0;
|
|
}
|
|
|
|
for(size_t i=0; i<index_buffers_.size(); ++i)
|
|
{
|
|
if(index_buffers_[i])
|
|
{
|
|
glDeleteBuffers(1, &index_buffers_[i]);
|
|
index_buffers_[i] = 0;
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear index buffer");
|
|
}
|
|
}
|
|
|
|
glGenBuffers(1, &vertex_buffer_);
|
|
if(!vertex_buffer_)
|
|
{
|
|
LOGE("OpenGL: could not generate vertex buffers\n");
|
|
return;
|
|
}
|
|
|
|
LOGI("Creating cloud buffer %d", vertex_buffer_);
|
|
std::vector<float> vertices;
|
|
size_t totalPoints = 0;
|
|
std::vector<GLuint> verticesLowRes;
|
|
std::vector<GLuint> verticesLowLowRes;
|
|
if(indices.get() && indices->size())
|
|
{
|
|
totalPoints = indices->size();
|
|
vertices.resize(indices->size()*4);
|
|
verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
|
|
verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
|
|
int oi_low = 0;
|
|
int oi_lowlow = 0;
|
|
for(unsigned int i=0; i<indices->size(); ++i)
|
|
{
|
|
const pcl::PointXYZRGB & pt = cloud->at(indices->at(i));
|
|
vertices[i*4] = pt.x;
|
|
vertices[i*4+1] = pt.y;
|
|
vertices[i*4+2] = pt.z;
|
|
vertices[i*4+3] = pt.rgb;
|
|
|
|
updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
|
|
|
|
if(cloud->isOrganized())
|
|
{
|
|
if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
|
|
{
|
|
verticesLowRes[oi_low++] = i;
|
|
}
|
|
if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
|
|
{
|
|
verticesLowLowRes[oi_lowlow++] = i;
|
|
}
|
|
}
|
|
}
|
|
verticesLowRes.resize(oi_low);
|
|
verticesLowLowRes.resize(oi_lowlow);
|
|
}
|
|
else
|
|
{
|
|
totalPoints = cloud->size();
|
|
vertices.resize(cloud->size()*4);
|
|
verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
|
|
verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
|
|
int oi_low = 0;
|
|
int oi_lowlow = 0;
|
|
for(unsigned int i=0; i<cloud->size(); ++i)
|
|
{
|
|
const pcl::PointXYZRGB & pt = cloud->at(i);
|
|
vertices[i*4] = pt.x;
|
|
vertices[i*4+1] = pt.y;
|
|
vertices[i*4+2] = pt.z;
|
|
vertices[i*4+3] = pt.rgb;
|
|
|
|
updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
|
|
|
|
if(cloud->isOrganized())
|
|
{
|
|
if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
|
|
{
|
|
verticesLowRes[oi_low++] = i;
|
|
}
|
|
if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
|
|
{
|
|
verticesLowLowRes[oi_lowlow++] = i;
|
|
}
|
|
}
|
|
}
|
|
verticesLowRes.resize(oi_low);
|
|
verticesLowLowRes.resize(oi_lowlow);
|
|
}
|
|
|
|
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
|
|
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_buffer_ = 0;
|
|
return;
|
|
}
|
|
|
|
// vertex index buffers
|
|
for(size_t i=4; i<5; ++i)
|
|
{
|
|
if((i==4 && !verticesLowRes.empty()) ||
|
|
(i==5 && !verticesLowLowRes.empty()))
|
|
{
|
|
glGenBuffers(1, &index_buffers_[i]);
|
|
if(!index_buffers_[i])
|
|
{
|
|
LOGE("OpenGL: could not generate index buffer %ld\n", i);
|
|
return;
|
|
}
|
|
|
|
index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
|
|
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
|
|
|
|
|
GLint error = glGetError();
|
|
if(error != GL_NO_ERROR)
|
|
{
|
|
LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
|
|
index_buffers_[i] = 0;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
nPoints_ = (int)totalPoints;
|
|
}
|
|
|
|
void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
|
|
{
|
|
UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
|
|
nPoints_ = 0;
|
|
aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
|
|
aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
|
|
|
|
if (vertex_buffer_)
|
|
{
|
|
glDeleteBuffers(1, &vertex_buffer_);
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
|
|
vertex_buffer_ = 0;
|
|
}
|
|
|
|
for(size_t i=0; i<index_buffers_.size(); ++i)
|
|
{
|
|
if(index_buffers_[i])
|
|
{
|
|
glDeleteBuffers(1, &index_buffers_[i]);
|
|
index_buffers_[i] = 0;
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear index buffer");
|
|
}
|
|
}
|
|
|
|
gainR_ = mesh.gains[0];
|
|
gainG_ = mesh.gains[1];
|
|
gainB_ = mesh.gains[2];
|
|
|
|
bool textureUpdate = false;
|
|
if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
|
|
{
|
|
if (texture_)
|
|
{
|
|
glDeleteTextures(1, &texture_);
|
|
tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
|
|
texture_ = 0;
|
|
}
|
|
textureUpdate = true;
|
|
}
|
|
|
|
glGenBuffers(1, &vertex_buffer_);
|
|
if(!vertex_buffer_)
|
|
{
|
|
LOGE("OpenGL: could not generate vertex buffers\n");
|
|
return;
|
|
}
|
|
|
|
if(textureUpdate)
|
|
{
|
|
glGenTextures(1, &texture_);
|
|
if(!texture_)
|
|
{
|
|
vertex_buffer_ = 0;
|
|
LOGE("OpenGL: could not generate texture buffers\n");
|
|
return;
|
|
}
|
|
}
|
|
|
|
//LOGD("Creating cloud buffer %d", vertex_buffers_);
|
|
std::vector<float> vertices;
|
|
int totalPoints = 0;
|
|
std::vector<pcl::Vertices> polygons = mesh.polygons;
|
|
std::vector<pcl::Vertices> polygonsLowRes;
|
|
hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
|
|
UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
|
|
if(mesh.cloud->isOrganized()) // assume organized mesh
|
|
{
|
|
polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
|
|
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
|
|
totalPoints = (int)mesh.indices->size();
|
|
std::vector<GLuint> verticesLowRes;
|
|
std::vector<GLuint> verticesLowLowRes;
|
|
verticesLowRes.resize(totalPoints);
|
|
verticesLowLowRes.resize(totalPoints);
|
|
int oi_low = 0;
|
|
int oi_lowlow = 0;
|
|
if(texture_ && polygons.size())
|
|
{
|
|
int items = hasNormals_?9:6;
|
|
vertices = std::vector<float>(mesh.indices->size()*items);
|
|
for(unsigned int i=0; i<mesh.indices->size(); ++i)
|
|
{
|
|
const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
|
|
vertices[i*items] = pt.x;
|
|
vertices[i*items+1] = pt.y;
|
|
vertices[i*items+2] = pt.z;
|
|
|
|
// rgb
|
|
vertices[i*items+3] = pt.rgb;
|
|
|
|
updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
|
|
|
|
// texture uv
|
|
int index = mesh.indices->at(i);
|
|
vertices[i*items+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
|
|
vertices[i*items+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height); //v
|
|
|
|
if(hasNormals_)
|
|
{
|
|
// normal
|
|
vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_x;
|
|
vertices[i*items+7] = mesh.normals->at(mesh.indices->at(i)).normal_y;
|
|
vertices[i*items+8] = mesh.normals->at(mesh.indices->at(i)).normal_z;
|
|
}
|
|
|
|
organizedToDenseIndices_[mesh.indices->at(i)] = i;
|
|
|
|
if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
|
|
{
|
|
verticesLowRes[oi_low++] = i;
|
|
}
|
|
if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
|
|
{
|
|
verticesLowLowRes[oi_lowlow++] = i;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//LOGD("Organized mesh");
|
|
int items = hasNormals_?7:4;
|
|
vertices = std::vector<float>(mesh.indices->size()*items);
|
|
for(unsigned int i=0; i<mesh.indices->size(); ++i)
|
|
{
|
|
const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
|
|
vertices[i*items] = pt.x;
|
|
vertices[i*items+1] = pt.y;
|
|
vertices[i*items+2] = pt.z;
|
|
vertices[i*items+3] = pt.rgb;
|
|
|
|
updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
|
|
|
|
if(hasNormals_)
|
|
{
|
|
// normal
|
|
vertices[i*items+4] = mesh.normals->at(mesh.indices->at(i)).normal_x;
|
|
vertices[i*items+5] = mesh.normals->at(mesh.indices->at(i)).normal_y;
|
|
vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_z;
|
|
}
|
|
|
|
organizedToDenseIndices_[mesh.indices->at(i)] = i;
|
|
|
|
if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
|
|
{
|
|
verticesLowRes[oi_low++] = i;
|
|
}
|
|
if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
|
|
{
|
|
verticesLowLowRes[oi_lowlow++] = i;
|
|
}
|
|
}
|
|
}
|
|
verticesLowRes.resize(oi_low);
|
|
verticesLowLowRes.resize(oi_lowlow);
|
|
|
|
// vertex index buffers
|
|
for(size_t i=4; i<5; ++i)
|
|
{
|
|
if((i==4 && !verticesLowRes.empty()) ||
|
|
(i==5 && !verticesLowLowRes.empty()))
|
|
{
|
|
glGenBuffers(1, &index_buffers_[i]);
|
|
if(!index_buffers_[i])
|
|
{
|
|
LOGE("OpenGL: could not generate index buffer %ld\n", i);
|
|
return;
|
|
}
|
|
|
|
index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
|
|
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
|
|
|
GLint error = glGetError();
|
|
if(error != GL_NO_ERROR)
|
|
{
|
|
LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
|
|
index_buffers_[i] = 0;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else // assume dense mesh with texCoords set to polygons
|
|
{
|
|
if(texture_ && polygons.size())
|
|
{
|
|
//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
|
|
// (int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
|
|
|
|
// Texturing issue:
|
|
// tex_coordinates should be linked to points, not
|
|
// polygon vertices. Points linked to multiple different texCoords (different textures) should
|
|
// be duplicated.
|
|
totalPoints = (int)mesh.texCoords.size();
|
|
int items = hasNormals_?9:6;
|
|
vertices = std::vector<float>(mesh.texCoords.size()*items);
|
|
organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
|
|
|
|
UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
|
|
uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
|
|
|
|
unsigned int oi=0;
|
|
for(unsigned int i=0; i<polygons.size(); ++i)
|
|
{
|
|
pcl::Vertices & v = polygons[i];
|
|
for(unsigned int j=0; j<v.vertices.size(); ++j)
|
|
{
|
|
UASSERT(oi < mesh.texCoords.size());
|
|
UASSERT(v.vertices[j] < mesh.cloud->size());
|
|
|
|
const pcl::PointXYZRGB & pt = mesh.cloud->at(v.vertices[j]);
|
|
|
|
vertices[oi*items] = pt.x;
|
|
vertices[oi*items+1] = pt.y;
|
|
vertices[oi*items+2] = pt.z;
|
|
|
|
// rgb
|
|
vertices[oi*items+3] = pt.rgb;
|
|
|
|
updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
|
|
|
|
// texture uv
|
|
if(mesh.texCoords[oi][0]>=0.0f)
|
|
{
|
|
vertices[oi*items+4] = mesh.texCoords[oi][0]; //u
|
|
vertices[oi*items+5] = 1.0f-mesh.texCoords[oi][1]; //v
|
|
}
|
|
else
|
|
{
|
|
vertices[oi*items+4] = vertices[oi*items+5] = -1.0f;
|
|
}
|
|
|
|
if(hasNormals_)
|
|
{
|
|
// normal
|
|
vertices[oi*items+6] = mesh.normals->at(v.vertices[j]).normal_x;
|
|
vertices[oi*items+7] = mesh.normals->at(v.vertices[j]).normal_y;
|
|
vertices[oi*items+8] = mesh.normals->at(v.vertices[j]).normal_z;
|
|
}
|
|
|
|
v.vertices[j] = (int)oi; // new vertex index
|
|
|
|
UASSERT(oi < organizedToDenseIndices_.size());
|
|
organizedToDenseIndices_[oi] = oi;
|
|
|
|
++oi;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
totalPoints = (int)mesh.cloud->size();
|
|
//LOGD("Dense mesh");
|
|
int items = hasNormals_?7:4;
|
|
organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
|
|
vertices = std::vector<float>(mesh.cloud->size()*items);
|
|
for(unsigned int i=0; i<mesh.cloud->size(); ++i)
|
|
{
|
|
const pcl::PointXYZRGB & pt = mesh.cloud->at(i);
|
|
|
|
vertices[i*items] =pt.x;
|
|
vertices[i*items+1] = pt.y;
|
|
vertices[i*items+2] = pt.z;
|
|
vertices[i*items+3] = pt.rgb;
|
|
|
|
updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
|
|
|
|
if(hasNormals_)
|
|
{
|
|
vertices[i*items+4] = mesh.normals->at(i).normal_x;
|
|
vertices[i*items+5] = mesh.normals->at(i).normal_y;
|
|
vertices[i*items+6] = mesh.normals->at(i).normal_z;
|
|
}
|
|
|
|
organizedToDenseIndices_[i] = i;
|
|
}
|
|
}
|
|
}
|
|
|
|
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
|
|
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_buffer_ = 0;
|
|
return;
|
|
}
|
|
|
|
if(texture_ && textureUpdate)
|
|
{
|
|
//GLint maxTextureSize = 0;
|
|
//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
|
|
//LOGI("maxTextureSize=%d", maxTextureSize);
|
|
//GLint maxTextureUnits = 0;
|
|
//glGetIntegerv(GL_MAX_TEXTURE_IMAGE_UNITS, &maxTextureUnits);
|
|
//LOGW("maxTextureUnits=%d", maxTextureUnits);
|
|
|
|
// gen texture from image
|
|
glBindTexture(GL_TEXTURE_2D, texture_);
|
|
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
|
|
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
|
|
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
|
|
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
|
|
cv::Mat rgbImage;
|
|
cv::cvtColor(mesh.texture, rgbImage, cv::COLOR_BGR2RGBA);
|
|
|
|
glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
|
|
//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
|
|
//glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
|
|
//glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
|
|
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
|
|
|
|
GLint error = glGetError();
|
|
if(error != GL_NO_ERROR)
|
|
{
|
|
LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
|
|
texture_ = 0;
|
|
|
|
glDeleteBuffers(1, &vertex_buffer_);
|
|
vertex_buffer_ = 0;
|
|
return;
|
|
}
|
|
}
|
|
|
|
nPoints_ = totalPoints;
|
|
|
|
updatePolygons(polygons, polygonsLowRes, createWireframe);
|
|
|
|
if(!pose_.isNull())
|
|
{
|
|
updateAABBWorld(pose_);
|
|
}
|
|
}
|
|
|
|
void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
|
|
{
|
|
UASSERT(!pose.isNull());
|
|
|
|
if(pose_ != pose)
|
|
{
|
|
updateAABBWorld(pose);
|
|
}
|
|
|
|
pose_ = pose;
|
|
poseGl_ = glmFromTransform(pose);
|
|
}
|
|
|
|
void PointCloudDrawable::updateAABBWorld(const rtabmap::Transform & pose)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ> corners;
|
|
corners.resize(8);
|
|
corners.at(0) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMinModel_.z);
|
|
corners.at(1) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
|
|
corners.at(2) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
|
|
corners.at(3) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMinModel_.z);
|
|
corners.at(4) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
|
|
corners.at(5) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
|
|
corners.at(6) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
|
|
corners.at(7) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
|
|
|
|
pcl::PointCloud<pcl::PointXYZ> cornersTransformed;
|
|
pcl::transformPointCloud(corners, cornersTransformed, pose.toEigen3f());
|
|
|
|
aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
|
|
aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
|
|
for(unsigned int i=0; i<cornersTransformed.size(); ++i)
|
|
{
|
|
updateAABBMinMax(cornersTransformed.at(i), aabbMinWorld_, aabbMaxWorld_);
|
|
}
|
|
}
|
|
|
|
|
|
void PointCloudDrawable::Render(
|
|
const glm::mat4 & projectionMatrix,
|
|
const glm::mat4 & viewMatrix,
|
|
bool meshRendering,
|
|
float pointSize,
|
|
bool textureRendering,
|
|
bool lighting,
|
|
float distanceToCameraSqr,
|
|
const GLuint & depthTexture,
|
|
int screenWidth,
|
|
int screenHeight,
|
|
float nearClipPlane,
|
|
float farClipPlane,
|
|
bool packDepthToColorChannel,
|
|
bool wireFrame,
|
|
bool hideSeams) const
|
|
{
|
|
if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
|
|
{
|
|
if(packDepthToColorChannel || !hasNormals_)
|
|
{
|
|
lighting = false;
|
|
}
|
|
|
|
if(packDepthToColorChannel || !(meshRendering && textureRendering && texture_))
|
|
{
|
|
textureRendering = false;
|
|
}
|
|
|
|
GLuint program;
|
|
if(packDepthToColorChannel)
|
|
{
|
|
program = shaderPrograms_[kDepthPacking];
|
|
}
|
|
else if(textureRendering)
|
|
{
|
|
if(lighting)
|
|
{
|
|
program = shaderPrograms_[depthTexture>0?kTextureLightingBlending:kTextureLighting];
|
|
}
|
|
else
|
|
{
|
|
program = shaderPrograms_[depthTexture>0?kTextureBlending:kTexture];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(lighting)
|
|
{
|
|
program = shaderPrograms_[depthTexture>0?kPointCloudLightingBlending:kPointCloudLighting];
|
|
}
|
|
else
|
|
{
|
|
program = shaderPrograms_[depthTexture>0?kPointCloudBlending:kPointCloud];
|
|
}
|
|
}
|
|
|
|
glUseProgram(program);
|
|
tango_gl::util::CheckGlError("Pointcloud::Render() set program");
|
|
|
|
GLuint mvp_handle = glGetUniformLocation(program, "uMVP");
|
|
glm::mat4 mv_mat = viewMatrix * poseGl_;
|
|
glm::mat4 mvp_mat = projectionMatrix * mv_mat;
|
|
glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
|
|
|
|
GLint attribute_vertex = glGetAttribLocation(program, "aVertex");
|
|
glEnableVertexAttribArray(attribute_vertex);
|
|
GLint attribute_color = 0;
|
|
GLint attribute_texture = 0;
|
|
GLint attribute_normal = 0;
|
|
|
|
if(packDepthToColorChannel || !textureRendering)
|
|
{
|
|
GLuint point_size_handle_ = glGetUniformLocation(program, "uPointSize");
|
|
glUniform1f(point_size_handle_, pointSize);
|
|
}
|
|
tango_gl::util::CheckGlError("Pointcloud::Render() vertex");
|
|
|
|
if(!packDepthToColorChannel)
|
|
{
|
|
GLuint gainR_handle = glGetUniformLocation(program, "uGainR");
|
|
GLuint gainG_handle = glGetUniformLocation(program, "uGainG");
|
|
GLuint gainB_handle = glGetUniformLocation(program, "uGainB");
|
|
glUniform1f(gainR_handle, gainR_);
|
|
glUniform1f(gainG_handle, gainG_);
|
|
glUniform1f(gainB_handle, gainB_);
|
|
|
|
// blending
|
|
if(depthTexture > 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(program, "uDepthTexture");
|
|
glUniform1i(depth_texture_handle, 1);
|
|
|
|
GLuint zNear_handle = glGetUniformLocation(program, "uNearZ");
|
|
GLuint zFar_handle = glGetUniformLocation(program, "uFarZ");
|
|
glUniform1f(zNear_handle, nearClipPlane);
|
|
glUniform1f(zFar_handle, farClipPlane);
|
|
|
|
GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
|
|
glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
|
|
}
|
|
|
|
if(lighting)
|
|
{
|
|
GLuint n_handle = glGetUniformLocation(program, "uN");
|
|
glm::mat3 normalMatrix(mv_mat);
|
|
normalMatrix = glm::inverse(normalMatrix);
|
|
normalMatrix = glm::transpose(normalMatrix);
|
|
glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
|
|
|
|
GLuint lightingDirection_handle = glGetUniformLocation(program, "uLightingDirection");
|
|
glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
|
|
|
|
attribute_normal = glGetAttribLocation(program, "aNormal");
|
|
glEnableVertexAttribArray(attribute_normal);
|
|
}
|
|
|
|
if(textureRendering)
|
|
{
|
|
// Texture activate unit 0
|
|
glActiveTexture(GL_TEXTURE0);
|
|
// Bind the texture to this unit.
|
|
glBindTexture(GL_TEXTURE_2D, texture_);
|
|
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
|
|
GLuint texture_handle = glGetUniformLocation(program, "uTexture");
|
|
glUniform1i(texture_handle, 0);
|
|
|
|
attribute_texture = glGetAttribLocation(program, "aTexCoord");
|
|
glEnableVertexAttribArray(attribute_texture);
|
|
|
|
if(depthTexture == 0)
|
|
{
|
|
GLuint hideSeams_handle = glGetUniformLocation(program, "uHideSeams");
|
|
glUniform1i(hideSeams_handle, hideSeams?1:0);
|
|
}
|
|
}
|
|
|
|
attribute_color = glGetAttribLocation(program, "aColor");
|
|
glEnableVertexAttribArray(attribute_color);
|
|
}
|
|
tango_gl::util::CheckGlError("Pointcloud::Render() common");
|
|
|
|
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
|
|
if(texture_)
|
|
{
|
|
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
|
|
if(textureRendering)
|
|
{
|
|
glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
|
|
}
|
|
if(!packDepthToColorChannel)
|
|
{
|
|
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
|
|
}
|
|
if(lighting && hasNormals_)
|
|
{
|
|
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
|
|
if(!packDepthToColorChannel)
|
|
{
|
|
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
|
|
}
|
|
if(lighting && hasNormals_)
|
|
{
|
|
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
|
|
}
|
|
}
|
|
tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
|
|
|
|
UTimer drawTime;
|
|
if((textureRendering || meshRendering) && index_buffers_[0])
|
|
{
|
|
float dist = meshRendering?50.0f:16.0f;
|
|
if(distanceToCameraSqr<dist || index_buffers_[1]==0)
|
|
{
|
|
wireFrame = wireFrame && index_buffers_[2];
|
|
if(wireFrame)
|
|
{
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[2]);
|
|
glDrawElements(GL_LINES, index_buffers_count_[2], GL_UNSIGNED_INT, 0);
|
|
}
|
|
else
|
|
{
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[0]);
|
|
glDrawElements(GL_TRIANGLES, index_buffers_count_[0], GL_UNSIGNED_INT, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
wireFrame = wireFrame && index_buffers_[3];
|
|
if(wireFrame)
|
|
{
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[3]);
|
|
glDrawElements(GL_LINES, index_buffers_count_[3], GL_UNSIGNED_INT, 0);
|
|
}
|
|
else
|
|
{
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[1]);
|
|
glDrawElements(GL_TRIANGLES, index_buffers_count_[1], GL_UNSIGNED_INT, 0);
|
|
}
|
|
}
|
|
}
|
|
else if(index_buffers_[4])
|
|
{
|
|
if(distanceToCameraSqr>600.0f && index_buffers_[5])
|
|
{
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
|
|
glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
|
|
}
|
|
else if(distanceToCameraSqr>150.0f)
|
|
{
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[4]);
|
|
glDrawElements(GL_POINTS, index_buffers_count_[4], GL_UNSIGNED_INT, 0);
|
|
}
|
|
else
|
|
{
|
|
glDrawArrays(GL_POINTS, 0, nPoints_);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
glDrawArrays(GL_POINTS, 0, nPoints_);
|
|
}
|
|
//UERROR("drawTime=%fs", drawTime.ticks());
|
|
tango_gl::util::CheckGlError("Pointcloud::Render() draw");
|
|
|
|
glDisableVertexAttribArray(0);
|
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
|
|
|
glUseProgram(0);
|
|
tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");
|
|
}
|
|
}
|
|
|