point_cloud_drawable.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <sstream>
29 
30 #include "point_cloud_drawable.h"
31 #include "rtabmap/utilite/ULogger.h"
32 #include "rtabmap/utilite/UTimer.h"
33 #include "rtabmap/utilite/UConversion.h"
34 #include <opencv2/imgproc/imgproc.hpp>
35 #include "util.h"
36 #include "pcl/common/transforms.h"
37 
38 #ifdef __ANDROID__
39 #include <GLES2/gl2.h>
40 #else // __APPLE__
41 #include <OpenGLES/ES2/gl.h>
42 #endif
43 
44 #define LOW_DEC 2
45 #define LOWLOW_DEC 4
46 
47 enum PointCloudShaders
48 {
49  kPointCloud = 0,
50  kPointCloudBlending = 1,
51  kPointCloudLighting = 2,
52  kPointCloudLightingBlending = 3,
53 
54  kTexture = 4,
55  kTextureBlending = 5,
56  kTextureLighting = 6,
57  kTextureLightingBlending = 7,
58 
59  kDepthPacking = 8
60 };
61 
62 // PointCloud shaders
63 const std::string kPointCloudVertexShader =
64  "precision highp float;\n"
65  "precision mediump int;\n"
66  "attribute vec3 aVertex;\n"
67  "attribute vec3 aColor;\n"
68 
69  "uniform mat4 uMVP;\n"
70  "uniform float uPointSize;\n"
71 
72  "varying vec3 vColor;\n"
73  "varying float vLightWeighting;\n"
74 
75  "void main() {\n"
76  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
77  " gl_PointSize = uPointSize;\n"
78  " vLightWeighting = 1.0;\n"
79  " vColor = aColor;\n"
80  "}\n";
81 const std::string kPointCloudLightingVertexShader =
82  "precision highp float;\n"
83  "precision mediump int;\n"
84  "attribute vec3 aVertex;\n"
85  "attribute vec3 aNormal;\n"
86  "attribute vec3 aColor;\n"
87 
88  "uniform mat4 uMVP;\n"
89  "uniform mat3 uN;\n"
90  "uniform vec3 uLightingDirection;\n"
91  "uniform float uPointSize;\n"
92 
93  "varying vec3 vColor;\n"
94  "varying float vLightWeighting;\n"
95 
96  "void main() {\n"
97  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
98  " gl_PointSize = uPointSize;\n"
99  " vec3 transformedNormal = uN * aNormal;\n"
100  " vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
101  " if(vLightWeighting<0.5)"
102  " vLightWeighting=0.5;\n"
103  " vColor = aColor;\n"
104  "}\n";
105 
106 const std::string kPointCloudFragmentShader =
107  "precision highp float;\n"
108  "precision mediump int;\n"
109  "uniform float uGainR;\n"
110  "uniform float uGainG;\n"
111  "uniform float uGainB;\n"
112  "varying vec3 vColor;\n"
113  "varying float vLightWeighting;\n"
114  "void main() {\n"
115  " vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
116  " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
117  "}\n";
118 const std::string kPointCloudBlendingFragmentShader =
119  "precision highp float;\n"
120  "precision mediump int;\n"
121  "uniform float uGainR;\n"
122  "uniform float uGainG;\n"
123  "uniform float uGainB;\n"
124  "uniform float uNearZ;\n"
125  "uniform float uFarZ;\n"
126  "uniform sampler2D uDepthTexture;\n"
127  "uniform vec2 uScreenScale;\n"
128  "varying vec3 vColor;\n"
129  "varying float vLightWeighting;\n"
130  "void main() {\n"
131  " vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
132  " float alpha = 1.0;\n"
133  " vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
134  " vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
135  " float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
136  " float num = (2.0 * uNearZ * uFarZ);\n"
137  " float diff = (uFarZ - uNearZ);\n"
138  " float add = (uFarZ + uNearZ);\n"
139  " float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
140  " float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
141  " float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
142  " float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
143  " if(linearFragz > linearDepth + 0.05)\n"
144  " alpha=0.0;\n"
145  " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
146  "}\n";
147 
148 const std::string kPointCloudDepthPackingVertexShader =
149  "precision highp float;\n"
150  "precision mediump int;\n"
151  "attribute vec3 aVertex;\n"
152  "uniform mat4 uMVP;\n"
153  "uniform float uPointSize;\n"
154  "void main() {\n"
155  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
156  " gl_PointSize = uPointSize;\n"
157  "}\n";
158 const std::string kPointCloudDepthPackingFragmentShader =
159  "precision highp float;\n"
160  "precision mediump int;\n"
161  "void main() {\n"
162  " vec4 enc = vec4(1.,255.,65025.,16581375.) * gl_FragCoord.z;\n"
163  " enc = fract(enc);\n"
164  " enc -= enc.yzww * vec2(1./255., 0.).xxxy;\n"
165  " gl_FragColor = enc;\n"
166  "}\n";
167 
168 // Texture shaders
169 const std::string kTextureMeshVertexShader =
170  "precision highp float;\n"
171  "precision mediump int;\n"
172  "attribute vec3 aVertex;\n"
173  "attribute vec2 aTexCoord;\n"
174 
175  "uniform mat4 uMVP;\n"
176 
177  "varying vec2 vTexCoord;\n"
178  "varying float vLightWeighting;\n"
179 
180  "void main() {\n"
181  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
182 
183  " if(aTexCoord.x < 0.0) {\n"
184  " vTexCoord.x = 1.0;\n"
185  " vTexCoord.y = 1.0;\n" // bottom right corner
186  " } else {\n"
187  " vTexCoord = aTexCoord;\n"
188  " }\n"
189 
190  " vLightWeighting = 1.0;\n"
191  "}\n";
192 const std::string kTextureMeshLightingVertexShader =
193  "precision highp float;\n"
194  "precision mediump int;\n"
195  "attribute vec3 aVertex;\n"
196  "attribute vec3 aNormal;\n"
197  "attribute vec2 aTexCoord;\n"
198 
199  "uniform mat4 uMVP;\n"
200  "uniform mat3 uN;\n"
201  "uniform vec3 uLightingDirection;\n"
202 
203  "varying vec2 vTexCoord;\n"
204  "varying float vLightWeighting;\n"
205 
206  "void main() {\n"
207  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
208 
209  " if(aTexCoord.x < 0.0) {\n"
210  " vTexCoord.x = 1.0;\n"
211  " vTexCoord.y = 1.0;\n" // bottom right corner
212  " } else {\n"
213  " vTexCoord = aTexCoord;\n"
214  " }\n"
215 
216  " vec3 transformedNormal = uN * aNormal;\n"
217  " vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
218  " if(vLightWeighting<0.5) \n"
219  " vLightWeighting=0.5;\n"
220  "}\n";
221 const std::string kTextureMeshFragmentShader =
222  "precision highp float;\n"
223  "precision mediump int;\n"
224  "uniform sampler2D uTexture;\n"
225  "uniform float uGainR;\n"
226  "uniform float uGainG;\n"
227  "uniform float uGainB;\n"
228  "varying vec2 vTexCoord;\n"
229  "varying float vLightWeighting;\n"
230  ""
231  "void main() {\n"
232  " vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
233  " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
234  "}\n";
235 const std::string kTextureMeshBlendingFragmentShader =
236  "precision highp float;\n"
237  "precision mediump int;\n"
238  "uniform sampler2D uTexture;\n"
239  "uniform sampler2D uDepthTexture;\n"
240  "uniform float uGainR;\n"
241  "uniform float uGainG;\n"
242  "uniform float uGainB;\n"
243  "uniform vec2 uScreenScale;\n"
244  "uniform float uNearZ;\n"
245  "uniform float uFarZ;\n"
246  "varying vec2 vTexCoord;\n"
247  "varying float vLightWeighting;\n"
248  ""
249  "void main() {\n"
250  " vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
251  " float alpha = 1.0;\n"
252  " vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
253  " vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
254  " float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
255  " float num = (2.0 * uNearZ * uFarZ);\n"
256  " float diff = (uFarZ - uNearZ);\n"
257  " float add = (uFarZ + uNearZ);\n"
258  " float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
259  " float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
260  " float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
261  " float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
262  " if(linearFragz > linearDepth + 0.05)\n"
263  " alpha=0.0;\n"
264  " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
265  "}\n";
266 
267 std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
268 
269 void PointCloudDrawable::createShaderPrograms()
270 {
271  if(shaderPrograms_.empty())
272  {
273  shaderPrograms_.resize(9);
274 
275  shaderPrograms_[kPointCloud] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
276  UASSERT(shaderPrograms_[kPointCloud] != 0);
277  shaderPrograms_[kPointCloudBlending] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
278  UASSERT(shaderPrograms_[kPointCloudBlending] != 0);
279  shaderPrograms_[kPointCloudLighting] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudFragmentShader.c_str());
280  UASSERT(shaderPrograms_[kPointCloudLighting] != 0);
281  shaderPrograms_[kPointCloudLightingBlending] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
282  UASSERT(shaderPrograms_[kPointCloudLightingBlending] != 0);
283 
284  shaderPrograms_[kTexture] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
285  UASSERT(shaderPrograms_[kTexture] != 0);
286  shaderPrograms_[kTextureBlending] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
287  UASSERT(shaderPrograms_[kTextureBlending] != 0);
288  shaderPrograms_[kTextureLighting] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
289  UASSERT(shaderPrograms_[kTextureLighting] != 0);
290  shaderPrograms_[kTextureLightingBlending] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
291  UASSERT(shaderPrograms_[kTextureLightingBlending] != 0);
292 
293  shaderPrograms_[kDepthPacking] = tango_gl::util::CreateProgram(kPointCloudDepthPackingVertexShader.c_str(), kPointCloudDepthPackingFragmentShader.c_str());
294  UASSERT(shaderPrograms_[kDepthPacking] != 0);
295  }
296 }
297 void PointCloudDrawable::releaseShaderPrograms()
298 {
299  for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
300  {
301  glDeleteShader(shaderPrograms_[i]);
302  }
303  shaderPrograms_.clear();
304 }
305 
306 PointCloudDrawable::PointCloudDrawable(
307  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
308  const pcl::IndicesPtr & indices,
309  float gainR,
310  float gainG,
311  float gainB) :
312  vertex_buffer_(0),
313  texture_(0),
314  nPoints_(0),
315  pose_(rtabmap::Transform::getIdentity()),
316  poseGl_(1.0f),
317  visible_(true),
318  hasNormals_(false),
319  gainR_(gainR),
320  gainG_(gainG),
321  gainB_(gainB)
322 {
323  index_buffers_.resize(6, 0);
324  index_buffers_count_.resize(6, 0);
325  updateCloud(cloud, indices);
326 }
327 
328 PointCloudDrawable::PointCloudDrawable(
329  const rtabmap::Mesh & mesh,
330  bool createWireframe) :
331  vertex_buffer_(0),
332  texture_(0),
333  nPoints_(0),
334  pose_(rtabmap::Transform::getIdentity()),
335  poseGl_(1.0f),
336  visible_(true),
337  hasNormals_(false),
338  gainR_(1.0f),
339  gainG_(1.0f),
340  gainB_(1.0f)
341 {
342  index_buffers_.resize(6, 0);
343  index_buffers_count_.resize(6, 0);
344  updateMesh(mesh, createWireframe);
345 }
346 
347 PointCloudDrawable::~PointCloudDrawable()
348 {
349  LOGI("Freeing cloud buffer %d", vertex_buffer_);
350  if (vertex_buffer_)
351  {
352  glDeleteBuffers(1, &vertex_buffer_);
353  tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
354  vertex_buffer_ = 0;
355  }
356 
357  if (texture_)
358  {
359  glDeleteTextures(1, &texture_);
360  tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
361  texture_ = 0;
362  }
363 
364  for(size_t i=0; i<index_buffers_.size(); ++i)
365  {
366  if(index_buffers_[i])
367  {
368  glDeleteBuffers(1, &index_buffers_[i]);
369  index_buffers_[i] = 0;
370  tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
371  }
372  }
373 }
374 
375 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
376 {
377  for(int i=0; i<4; ++i)
378  {
379  if(index_buffers_[i])
380  {
381  glDeleteBuffers(1, &index_buffers_[i]);
382  index_buffers_[i] = 0;
383  tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
384  }
385  }
386 
387  //LOGD("Update polygons");
388  if(polygons.size() && organizedToDenseIndices_.size())
389  {
390  size_t polygonSize = polygons[0].vertices.size();
391  UASSERT(polygonSize == 3);
392  std::vector<std::vector<GLuint> > indexes(4);
393  indexes[0].resize(polygons.size() * polygonSize);
394  if(createWireframe)
395  indexes[2].resize(indexes[0].size()*2);
396  int oi = 0;
397  int li = 0;
398  for(size_t i=0; i<polygons.size(); ++i)
399  {
400  UASSERT(polygons[i].vertices.size() == polygonSize);
401  for(unsigned int j=0; j<polygonSize; ++j)
402  {
403  indexes[0][oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
404  if(createWireframe)
405  {
406  indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
407  indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
408  }
409  }
410  }
411 
412  if(polygonsLowRes.size())
413  {
414  size_t polygonSize = polygonsLowRes[0].vertices.size();
415  UASSERT(polygonSize == 3);
416  indexes[1].resize(polygonsLowRes.size() * polygonSize);
417  if(createWireframe)
418  indexes[3].resize(indexes[1].size()*2);
419  int oi = 0;
420  int li = 0;
421  for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
422  {
423  UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
424  for(unsigned int j=0; j<polygonSize; ++j)
425  {
426  indexes[1][oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
427  if(createWireframe)
428  {
429  indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
430  indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
431  }
432  }
433  }
434  }
435 
436  // Generate index buffers
437  for(size_t i=0; i<indexes.size(); ++i)
438  {
439  if(!indexes[i].empty())
440  {
441  glGenBuffers(1, &index_buffers_[i]);
442  if(!index_buffers_[i])
443  {
444  LOGE("OpenGL: could not generate index buffer %ld\n", i);
445  return;
446  }
447 
448  LOGD("Adding polygon index %ld size=%ld", i, indexes[i].size());
449 
450  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
451  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * indexes[i].size(), indexes[i].data(), GL_STATIC_DRAW);
452  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
453  index_buffers_count_[i] = (int)indexes[i].size();
454 
455  GLint error = glGetError();
456  if(error != GL_NO_ERROR)
457  {
458  LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
459  index_buffers_[i] = 0;
460  return;
461  }
462  }
463  }
464  }
465 }
466 
467 void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices)
468 {
469  UASSERT(cloud.get() && !cloud->empty());
470  nPoints_ = 0;
471  aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
472  aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
473 
474  if (vertex_buffer_)
475  {
476  glDeleteBuffers(1, &vertex_buffer_);
477  tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
478  vertex_buffer_ = 0;
479  }
480 
481  if (texture_)
482  {
483  glDeleteTextures(1, &texture_);
484  tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
485  texture_ = 0;
486  }
487 
488  for(size_t i=0; i<index_buffers_.size(); ++i)
489  {
490  if(index_buffers_[i])
491  {
492  glDeleteBuffers(1, &index_buffers_[i]);
493  index_buffers_[i] = 0;
494  tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear index buffer");
495  }
496  }
497 
498  glGenBuffers(1, &vertex_buffer_);
499  if(!vertex_buffer_)
500  {
501  LOGE("OpenGL: could not generate vertex buffers\n");
502  return;
503  }
504 
505  LOGI("Creating cloud buffer %d", vertex_buffer_);
506  std::vector<float> vertices;
507  size_t totalPoints = 0;
508  std::vector<GLuint> verticesLowRes;
509  std::vector<GLuint> verticesLowLowRes;
510  if(indices.get() && indices->size())
511  {
512  totalPoints = indices->size();
513  vertices.resize(indices->size()*4);
514  verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
515  verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
516  int oi_low = 0;
517  int oi_lowlow = 0;
518  for(unsigned int i=0; i<indices->size(); ++i)
519  {
520  const pcl::PointXYZRGB & pt = cloud->at(indices->at(i));
521  vertices[i*4] = pt.x;
522  vertices[i*4+1] = pt.y;
523  vertices[i*4+2] = pt.z;
524  vertices[i*4+3] = pt.rgb;
525 
526  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
527 
528  if(cloud->isOrganized())
529  {
530  if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
531  {
532  verticesLowRes[oi_low++] = i;
533  }
534  if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
535  {
536  verticesLowLowRes[oi_lowlow++] = i;
537  }
538  }
539  }
540  verticesLowRes.resize(oi_low);
541  verticesLowLowRes.resize(oi_lowlow);
542  }
543  else
544  {
545  totalPoints = cloud->size();
546  vertices.resize(cloud->size()*4);
547  verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
548  verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
549  int oi_low = 0;
550  int oi_lowlow = 0;
551  for(unsigned int i=0; i<cloud->size(); ++i)
552  {
553  const pcl::PointXYZRGB & pt = cloud->at(i);
554  vertices[i*4] = pt.x;
555  vertices[i*4+1] = pt.y;
556  vertices[i*4+2] = pt.z;
557  vertices[i*4+3] = pt.rgb;
558 
559  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
560 
561  if(cloud->isOrganized())
562  {
563  if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
564  {
565  verticesLowRes[oi_low++] = i;
566  }
567  if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
568  {
569  verticesLowLowRes[oi_lowlow++] = i;
570  }
571  }
572  }
573  verticesLowRes.resize(oi_low);
574  verticesLowLowRes.resize(oi_lowlow);
575  }
576 
577  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
578  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
579  glBindBuffer(GL_ARRAY_BUFFER, 0);
580 
581  GLint error = glGetError();
582  if(error != GL_NO_ERROR)
583  {
584  LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
585  vertex_buffer_ = 0;
586  return;
587  }
588 
589  // vertex index buffers
590  for(size_t i=4; i<5; ++i)
591  {
592  if((i==4 && !verticesLowRes.empty()) ||
593  (i==5 && !verticesLowLowRes.empty()))
594  {
595  glGenBuffers(1, &index_buffers_[i]);
596  if(!index_buffers_[i])
597  {
598  LOGE("OpenGL: could not generate index buffer %ld\n", i);
599  return;
600  }
601 
602  index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
603  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
604  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
605  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
606 
607 
608  GLint error = glGetError();
609  if(error != GL_NO_ERROR)
610  {
611  LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
612  index_buffers_[i] = 0;
613  return;
614  }
615  }
616  }
617 
618  nPoints_ = (int)totalPoints;
619 }
620 
621 void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
622 {
623  UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
624  nPoints_ = 0;
625  aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
626  aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
627 
628  if (vertex_buffer_)
629  {
630  glDeleteBuffers(1, &vertex_buffer_);
631  tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
632  vertex_buffer_ = 0;
633  }
634 
635  for(size_t i=0; i<index_buffers_.size(); ++i)
636  {
637  if(index_buffers_[i])
638  {
639  glDeleteBuffers(1, &index_buffers_[i]);
640  index_buffers_[i] = 0;
641  tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear index buffer");
642  }
643  }
644 
645  gainR_ = mesh.gains[0];
646  gainG_ = mesh.gains[1];
647  gainB_ = mesh.gains[2];
648 
649  bool textureUpdate = false;
650  if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
651  {
652  if (texture_)
653  {
654  glDeleteTextures(1, &texture_);
655  tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
656  texture_ = 0;
657  }
658  textureUpdate = true;
659  }
660 
661  glGenBuffers(1, &vertex_buffer_);
662  if(!vertex_buffer_)
663  {
664  LOGE("OpenGL: could not generate vertex buffers\n");
665  return;
666  }
667 
668  if(textureUpdate)
669  {
670  glGenTextures(1, &texture_);
671  if(!texture_)
672  {
673  vertex_buffer_ = 0;
674  LOGE("OpenGL: could not generate texture buffers\n");
675  return;
676  }
677  }
678 
679  //LOGD("Creating cloud buffer %d", vertex_buffers_);
680  std::vector<float> vertices;
681  int totalPoints = 0;
682  std::vector<pcl::Vertices> polygons = mesh.polygons;
683  std::vector<pcl::Vertices> polygonsLowRes;
684  hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
685  UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
686  if(mesh.cloud->isOrganized()) // assume organized mesh
687  {
688  polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
689  organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
690  totalPoints = (int)mesh.indices->size();
691  std::vector<GLuint> verticesLowRes;
692  std::vector<GLuint> verticesLowLowRes;
693  verticesLowRes.resize(totalPoints);
694  verticesLowLowRes.resize(totalPoints);
695  int oi_low = 0;
696  int oi_lowlow = 0;
697  if(texture_ && polygons.size())
698  {
699  int items = hasNormals_?9:6;
700  vertices = std::vector<float>(mesh.indices->size()*items);
701  for(unsigned int i=0; i<mesh.indices->size(); ++i)
702  {
703  const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
704  vertices[i*items] = pt.x;
705  vertices[i*items+1] = pt.y;
706  vertices[i*items+2] = pt.z;
707 
708  // rgb
709  vertices[i*items+3] = pt.rgb;
710 
711  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
712 
713  // texture uv
714  int index = mesh.indices->at(i);
715  vertices[i*items+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
716  vertices[i*items+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height); //v
717 
718  if(hasNormals_)
719  {
720  // normal
721  vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_x;
722  vertices[i*items+7] = mesh.normals->at(mesh.indices->at(i)).normal_y;
723  vertices[i*items+8] = mesh.normals->at(mesh.indices->at(i)).normal_z;
724  }
725 
726  organizedToDenseIndices_[mesh.indices->at(i)] = i;
727 
728  if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
729  {
730  verticesLowRes[oi_low++] = i;
731  }
732  if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
733  {
734  verticesLowLowRes[oi_lowlow++] = i;
735  }
736  }
737  }
738  else
739  {
740  //LOGD("Organized mesh");
741  int items = hasNormals_?7:4;
742  vertices = std::vector<float>(mesh.indices->size()*items);
743  for(unsigned int i=0; i<mesh.indices->size(); ++i)
744  {
745  const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
746  vertices[i*items] = pt.x;
747  vertices[i*items+1] = pt.y;
748  vertices[i*items+2] = pt.z;
749  vertices[i*items+3] = pt.rgb;
750 
751  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
752 
753  if(hasNormals_)
754  {
755  // normal
756  vertices[i*items+4] = mesh.normals->at(mesh.indices->at(i)).normal_x;
757  vertices[i*items+5] = mesh.normals->at(mesh.indices->at(i)).normal_y;
758  vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_z;
759  }
760 
761  organizedToDenseIndices_[mesh.indices->at(i)] = i;
762 
763  if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
764  {
765  verticesLowRes[oi_low++] = i;
766  }
767  if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
768  {
769  verticesLowLowRes[oi_lowlow++] = i;
770  }
771  }
772  }
773  verticesLowRes.resize(oi_low);
774  verticesLowLowRes.resize(oi_lowlow);
775 
776  // vertex index buffers
777  for(size_t i=4; i<5; ++i)
778  {
779  if((i==4 && !verticesLowRes.empty()) ||
780  (i==5 && !verticesLowLowRes.empty()))
781  {
782  glGenBuffers(1, &index_buffers_[i]);
783  if(!index_buffers_[i])
784  {
785  LOGE("OpenGL: could not generate index buffer %ld\n", i);
786  return;
787  }
788 
789  index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
790  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
791  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
792  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
793 
794  GLint error = glGetError();
795  if(error != GL_NO_ERROR)
796  {
797  LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
798  index_buffers_[i] = 0;
799  return;
800  }
801  }
802  }
803  }
804  else // assume dense mesh with texCoords set to polygons
805  {
806  if(texture_ && polygons.size())
807  {
808  //LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
809  // (int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
810 
811  // Texturing issue:
812  // tex_coordinates should be linked to points, not
813  // polygon vertices. Points linked to multiple different texCoords (different textures) should
814  // be duplicated.
815  totalPoints = (int)mesh.texCoords.size();
816  int items = hasNormals_?9:6;
817  vertices = std::vector<float>(mesh.texCoords.size()*items);
818  organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
819 
820  UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
821  uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
822 
823  unsigned int oi=0;
824  for(unsigned int i=0; i<polygons.size(); ++i)
825  {
826  pcl::Vertices & v = polygons[i];
827  for(unsigned int j=0; j<v.vertices.size(); ++j)
828  {
829  UASSERT(oi < mesh.texCoords.size());
830  UASSERT(v.vertices[j] < mesh.cloud->size());
831 
832  const pcl::PointXYZRGB & pt = mesh.cloud->at(v.vertices[j]);
833 
834  vertices[oi*items] = pt.x;
835  vertices[oi*items+1] = pt.y;
836  vertices[oi*items+2] = pt.z;
837 
838  // rgb
839  vertices[oi*items+3] = pt.rgb;
840 
841  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
842 
843  // texture uv
844  if(mesh.texCoords[oi][0]>=0.0f)
845  {
846  vertices[oi*items+4] = mesh.texCoords[oi][0]; //u
847  vertices[oi*items+5] = 1.0f-mesh.texCoords[oi][1]; //v
848  }
849  else
850  {
851  vertices[oi*items+4] = vertices[oi*items+5] = -1.0f;
852  }
853 
854  if(hasNormals_)
855  {
856  // normal
857  vertices[oi*items+6] = mesh.normals->at(v.vertices[j]).normal_x;
858  vertices[oi*items+7] = mesh.normals->at(v.vertices[j]).normal_y;
859  vertices[oi*items+8] = mesh.normals->at(v.vertices[j]).normal_z;
860  }
861 
862  v.vertices[j] = (int)oi; // new vertex index
863 
864  UASSERT(oi < organizedToDenseIndices_.size());
865  organizedToDenseIndices_[oi] = oi;
866 
867  ++oi;
868  }
869  }
870  }
871  else
872  {
873  totalPoints = (int)mesh.cloud->size();
874  //LOGD("Dense mesh");
875  int items = hasNormals_?7:4;
876  organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
877  vertices = std::vector<float>(mesh.cloud->size()*items);
878  for(unsigned int i=0; i<mesh.cloud->size(); ++i)
879  {
880  const pcl::PointXYZRGB & pt = mesh.cloud->at(i);
881 
882  vertices[i*items] =pt.x;
883  vertices[i*items+1] = pt.y;
884  vertices[i*items+2] = pt.z;
885  vertices[i*items+3] = pt.rgb;
886 
887  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
888 
889  if(hasNormals_)
890  {
891  vertices[i*items+4] = mesh.normals->at(i).normal_x;
892  vertices[i*items+5] = mesh.normals->at(i).normal_y;
893  vertices[i*items+6] = mesh.normals->at(i).normal_z;
894  }
895 
896  organizedToDenseIndices_[i] = i;
897  }
898  }
899  }
900 
901  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
902  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
903  glBindBuffer(GL_ARRAY_BUFFER, 0);
904 
905  GLint error = glGetError();
906  if(error != GL_NO_ERROR)
907  {
908  LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
909  vertex_buffer_ = 0;
910  return;
911  }
912 
913  if(texture_ && textureUpdate)
914  {
915  //GLint maxTextureSize = 0;
916  //glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
917  //LOGI("maxTextureSize=%d", maxTextureSize);
918  //GLint maxTextureUnits = 0;
919  //glGetIntegerv(GL_MAX_TEXTURE_IMAGE_UNITS, &maxTextureUnits);
920  //LOGW("maxTextureUnits=%d", maxTextureUnits);
921 
922  // gen texture from image
923  glBindTexture(GL_TEXTURE_2D, texture_);
924  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
925  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
926  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
927  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
928  cv::Mat rgbImage;
929  cv::cvtColor(mesh.texture, rgbImage, cv::COLOR_BGR2RGBA);
930 
931  glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
932  //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
933  //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
934  //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
935  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
936 
937  GLint error = glGetError();
938  if(error != GL_NO_ERROR)
939  {
940  LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
941  texture_ = 0;
942 
943  glDeleteBuffers(1, &vertex_buffer_);
944  vertex_buffer_ = 0;
945  return;
946  }
947  }
948 
949  nPoints_ = totalPoints;
950 
951  updatePolygons(polygons, polygonsLowRes, createWireframe);
952 
953  if(!pose_.isNull())
954  {
955  updateAABBWorld(pose_);
956  }
957 }
958 
959 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
960 {
961  UASSERT(!pose.isNull());
962 
963  if(pose_ != pose)
964  {
965  updateAABBWorld(pose);
966  }
967 
968  pose_ = pose;
969  poseGl_ = glmFromTransform(pose);
970 }
971 
972 void PointCloudDrawable::updateAABBWorld(const rtabmap::Transform & pose)
973 {
974  pcl::PointCloud<pcl::PointXYZ> corners;
975  corners.resize(8);
976  corners.at(0) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMinModel_.z);
977  corners.at(1) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
978  corners.at(2) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
979  corners.at(3) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMinModel_.z);
980  corners.at(4) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
981  corners.at(5) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
982  corners.at(6) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
983  corners.at(7) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
984 
985  pcl::PointCloud<pcl::PointXYZ> cornersTransformed;
986  pcl::transformPointCloud(corners, cornersTransformed, pose.toEigen3f());
987 
988  aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
989  aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
990  for(unsigned int i=0; i<cornersTransformed.size(); ++i)
991  {
992  updateAABBMinMax(cornersTransformed.at(i), aabbMinWorld_, aabbMaxWorld_);
993  }
994 }
995 
996 
997 void PointCloudDrawable::Render(
998  const glm::mat4 & projectionMatrix,
999  const glm::mat4 & viewMatrix,
1000  bool meshRendering,
1001  float pointSize,
1002  bool textureRendering,
1003  bool lighting,
1004  float distanceToCameraSqr,
1005  const GLuint & depthTexture,
1006  int screenWidth,
1007  int screenHeight,
1008  float nearClipPlane,
1009  float farClipPlane,
1010  bool packDepthToColorChannel,
1011  bool wireFrame) const
1012 {
1013  if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
1014  {
1015  if(packDepthToColorChannel || !hasNormals_)
1016  {
1017  lighting = false;
1018  }
1019 
1020  if(packDepthToColorChannel || !(meshRendering && textureRendering && texture_))
1021  {
1022  textureRendering = false;
1023  }
1024 
1025  GLuint program;
1026  if(packDepthToColorChannel)
1027  {
1028  program = shaderPrograms_[kDepthPacking];
1029  }
1030  else if(textureRendering)
1031  {
1032  if(lighting)
1033  {
1034  program = shaderPrograms_[depthTexture>0?kTextureLightingBlending:kTextureLighting];
1035  }
1036  else
1037  {
1038  program = shaderPrograms_[depthTexture>0?kTextureBlending:kTexture];
1039  }
1040  }
1041  else
1042  {
1043  if(lighting)
1044  {
1045  program = shaderPrograms_[depthTexture>0?kPointCloudLightingBlending:kPointCloudLighting];
1046  }
1047  else
1048  {
1049  program = shaderPrograms_[depthTexture>0?kPointCloudBlending:kPointCloud];
1050  }
1051  }
1052 
1053  glUseProgram(program);
1054  tango_gl::util::CheckGlError("Pointcloud::Render() set program");
1055 
1056  GLuint mvp_handle = glGetUniformLocation(program, "uMVP");
1057  glm::mat4 mv_mat = viewMatrix * poseGl_;
1058  glm::mat4 mvp_mat = projectionMatrix * mv_mat;
1059  glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
1060 
1061  GLint attribute_vertex = glGetAttribLocation(program, "aVertex");
1062  glEnableVertexAttribArray(attribute_vertex);
1063  GLint attribute_color = 0;
1064  GLint attribute_texture = 0;
1065  GLint attribute_normal = 0;
1066 
1067  if(packDepthToColorChannel || !textureRendering)
1068  {
1069  GLuint point_size_handle_ = glGetUniformLocation(program, "uPointSize");
1070  glUniform1f(point_size_handle_, pointSize);
1071  }
1072  tango_gl::util::CheckGlError("Pointcloud::Render() vertex");
1073 
1074  if(!packDepthToColorChannel)
1075  {
1076  GLuint gainR_handle = glGetUniformLocation(program, "uGainR");
1077  GLuint gainG_handle = glGetUniformLocation(program, "uGainG");
1078  GLuint gainB_handle = glGetUniformLocation(program, "uGainB");
1079  glUniform1f(gainR_handle, gainR_);
1080  glUniform1f(gainG_handle, gainG_);
1081  glUniform1f(gainB_handle, gainB_);
1082 
1083  // blending
1084  if(depthTexture > 0)
1085  {
1086  // Texture activate unit 1
1087  glActiveTexture(GL_TEXTURE1);
1088  // Bind the texture to this unit.
1089  glBindTexture(GL_TEXTURE_2D, depthTexture);
1090  // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
1091  GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
1092  glUniform1i(depth_texture_handle, 1);
1093 
1094  GLuint zNear_handle = glGetUniformLocation(program, "uNearZ");
1095  GLuint zFar_handle = glGetUniformLocation(program, "uFarZ");
1096  glUniform1f(zNear_handle, nearClipPlane);
1097  glUniform1f(zFar_handle, farClipPlane);
1098 
1099  GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
1100  glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
1101  }
1102 
1103  if(lighting)
1104  {
1105  GLuint n_handle = glGetUniformLocation(program, "uN");
1106  glm::mat3 normalMatrix(mv_mat);
1107  normalMatrix = glm::inverse(normalMatrix);
1108  normalMatrix = glm::transpose(normalMatrix);
1109  glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
1110 
1111  GLuint lightingDirection_handle = glGetUniformLocation(program, "uLightingDirection");
1112  glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
1113 
1114  attribute_normal = glGetAttribLocation(program, "aNormal");
1115  glEnableVertexAttribArray(attribute_normal);
1116  }
1117 
1118  if(textureRendering)
1119  {
1120  // Texture activate unit 0
1121  glActiveTexture(GL_TEXTURE0);
1122  // Bind the texture to this unit.
1123  glBindTexture(GL_TEXTURE_2D, texture_);
1124  // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
1125  GLuint texture_handle = glGetUniformLocation(program, "uTexture");
1126  glUniform1i(texture_handle, 0);
1127 
1128  attribute_texture = glGetAttribLocation(program, "aTexCoord");
1129  glEnableVertexAttribArray(attribute_texture);
1130  }
1131  else
1132  {
1133  attribute_color = glGetAttribLocation(program, "aColor");
1134  glEnableVertexAttribArray(attribute_color);
1135  }
1136  }
1137  tango_gl::util::CheckGlError("Pointcloud::Render() common");
1138 
1139  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
1140  if(texture_)
1141  {
1142  glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
1143  if(textureRendering)
1144  {
1145  glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
1146  }
1147  else if(!packDepthToColorChannel)
1148  {
1149  glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
1150  }
1151  if(lighting && hasNormals_)
1152  {
1153  glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
1154  }
1155  }
1156  else
1157  {
1158  glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
1159  if(!packDepthToColorChannel)
1160  {
1161  glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
1162  }
1163  if(lighting && hasNormals_)
1164  {
1165  glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
1166  }
1167  }
1168  tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
1169 
1170  UTimer drawTime;
1171  if((textureRendering || meshRendering) && index_buffers_[0])
1172  {
1173  float dist = meshRendering?50.0f:16.0f;
1174  if(distanceToCameraSqr<dist || index_buffers_[1]==0)
1175  {
1176  wireFrame = wireFrame && index_buffers_[2];
1177  if(wireFrame)
1178  {
1179  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[2]);
1180  glDrawElements(GL_LINES, index_buffers_count_[2], GL_UNSIGNED_INT, 0);
1181  }
1182  else
1183  {
1184  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[0]);
1185  glDrawElements(GL_TRIANGLES, index_buffers_count_[0], GL_UNSIGNED_INT, 0);
1186  }
1187  }
1188  else
1189  {
1190  wireFrame = wireFrame && index_buffers_[3];
1191  if(wireFrame)
1192  {
1193  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[3]);
1194  glDrawElements(GL_LINES, index_buffers_count_[3], GL_UNSIGNED_INT, 0);
1195  }
1196  else
1197  {
1198  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[1]);
1199  glDrawElements(GL_TRIANGLES, index_buffers_count_[1], GL_UNSIGNED_INT, 0);
1200  }
1201  }
1202  }
1203  else if(index_buffers_[4])
1204  {
1205  if(distanceToCameraSqr>600.0f && index_buffers_[5])
1206  {
1207  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
1208  glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
1209  }
1210  else if(distanceToCameraSqr>150.0f)
1211  {
1212  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[4]);
1213  glDrawElements(GL_POINTS, index_buffers_count_[4], GL_UNSIGNED_INT, 0);
1214  }
1215  else
1216  {
1217  glDrawArrays(GL_POINTS, 0, nPoints_);
1218  }
1219  }
1220  else
1221  {
1222  glDrawArrays(GL_POINTS, 0, nPoints_);
1223  }
1224  //UERROR("drawTime=%fs", drawTime.ticks());
1225  tango_gl::util::CheckGlError("Pointcloud::Render() draw");
1226 
1227  glDisableVertexAttribArray(0);
1228  glBindBuffer(GL_ARRAY_BUFFER, 0);
1229  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
1230 
1231  glUseProgram(0);
1232  tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");
1233  }
1234 }
1235 
int
int
PointCloudDrawable::releaseShaderPrograms
static void releaseShaderPrograms()
Definition: point_cloud_drawable.cpp:297
kTextureMeshVertexShader
const std::string kTextureMeshVertexShader
Definition: point_cloud_drawable.cpp:169
PointCloudDrawable::shaderPrograms_
static std::vector< GLuint > shaderPrograms_
Definition: point_cloud_drawable.h:50
kPointCloudLightingBlending
@ kPointCloudLightingBlending
Definition: point_cloud_drawable.cpp:52
PointCloudDrawable::updateAABBMinMax
void updateAABBMinMax(const PointT &pt, pcl::PointXYZ &min, pcl::PointXYZ &max)
Definition: point_cloud_drawable.h:105
glm::detail::tmat3x3
Definition: type_mat.hpp:43
kTextureLighting
@ kTextureLighting
Definition: point_cloud_drawable.cpp:56
rtabmap::Mesh::polygonsLowRes
std::vector< pcl::Vertices > polygonsLowRes
Definition: util.h:181
PointCloudDrawable::setPose
void setPose(const rtabmap::Transform &pose)
Definition: point_cloud_drawable.cpp:959
uint32_t
::uint32_t uint32_t
PointCloudShaders
PointCloudShaders
Definition: point_cloud_drawable.cpp:47
size
Index size
tango_gl::util::CheckGlError
void CheckGlError(const char *operation)
Definition: util.cpp:43
kTextureBlending
@ kTextureBlending
Definition: point_cloud_drawable.cpp:55
tango_gl::vertices
static const float vertices[]
Definition: quad.cpp:39
rtabmap::Mesh::normals
pcl::PointCloud< pcl::Normal >::Ptr normals
Definition: util.h:178
kPointCloudDepthPackingFragmentShader
const std::string kPointCloudDepthPackingFragmentShader
Definition: point_cloud_drawable.cpp:158
PointCloudDrawable::hasNormals_
bool hasNormals_
Definition: point_cloud_drawable.h:126
PointCloudDrawable::aabbMaxModel_
pcl::PointXYZ aabbMaxModel_
Definition: point_cloud_drawable.h:135
glm::value_ptr
GLM_FUNC_DECL const genType::value_type * value_ptr(genType const &vec)
true
#define true
Definition: ConvertUTF.c:57
PointCloudDrawable::organizedToDenseIndices_
std::vector< unsigned int > organizedToDenseIndices_
Definition: point_cloud_drawable.h:127
PointCloudDrawable::pose_
rtabmap::Transform pose_
Definition: point_cloud_drawable.h:123
PointCloudDrawable::vertex_buffer_
GLuint vertex_buffer_
Definition: point_cloud_drawable.h:118
PointCloudDrawable::nPoints_
int nPoints_
Definition: point_cloud_drawable.h:122
kPointCloudFragmentShader
const std::string kPointCloudFragmentShader
Definition: point_cloud_drawable.cpp:106
corners
void corners(const MatrixType &m)
kPointCloudBlendingFragmentShader
const std::string kPointCloudBlendingFragmentShader
Definition: point_cloud_drawable.cpp:118
UTimer.h
glm::detail::tmat4x4
Definition: type_mat.hpp:47
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
kDepthPacking
@ kDepthPacking
Definition: point_cloud_drawable.cpp:59
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
indices
indices
PointCloudDrawable::aabbMinWorld_
pcl::PointXYZ aabbMinWorld_
Definition: point_cloud_drawable.h:136
rtabmap::Mesh
Definition: util.h:165
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:391
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
GL_FALSE
#define GL_FALSE
Definition: dummy.cpp:79
data
int data[]
kPointCloudLighting
@ kPointCloudLighting
Definition: point_cloud_drawable.cpp:51
GLuint
unsigned int GLuint
Definition: dummy.cpp:78
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
tango_gl::util::CreateProgram
GLuint CreateProgram(const char *vertex_source, const char *fragment_source)
Definition: util.cpp:75
rtabmap::Mesh::texture
cv::Mat texture
Definition: util.h:191
PointCloudDrawable::gainR_
float gainR_
Definition: point_cloud_drawable.h:130
PointCloudDrawable::gainG_
float gainG_
Definition: point_cloud_drawable.h:131
PointCloudDrawable::PointCloudDrawable
PointCloudDrawable(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float gainR=1.0f, float gainG=1.0f, float gainB=1.0f)
Definition: point_cloud_drawable.cpp:306
PointCloudDrawable::updateCloud
void updateCloud(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices)
Definition: point_cloud_drawable.cpp:467
PointCloudDrawable::Render
void Render(const glm::mat4 &projectionMatrix, const glm::mat4 &viewMatrix, bool meshRendering=true, float pointSize=3.0f, bool textureRendering=false, bool lighting=true, float distanceToCamSqr=0.0f, const GLuint &depthTexture=0, int screenWidth=0, int screenHeight=0, float nearClipPlane=0, float farClipPlane=0, bool packDepthToColorChannel=false, bool wireFrame=false) const
Definition: point_cloud_drawable.cpp:997
PointCloudDrawable::aabbMaxWorld_
pcl::PointXYZ aabbMaxWorld_
Definition: point_cloud_drawable.h:137
PointCloudDrawable::createShaderPrograms
static void createShaderPrograms()
Definition: point_cloud_drawable.cpp:269
UASSERT
#define UASSERT(condition)
error
void error(const char *str)
PointCloudDrawable::updateMesh
void updateMesh(const rtabmap::Mesh &mesh, bool createWireframe=false)
Definition: point_cloud_drawable.cpp:621
kPointCloudBlending
@ kPointCloudBlending
Definition: point_cloud_drawable.cpp:50
PointCloudDrawable::updatePolygons
void updatePolygons(const std::vector< pcl::Vertices > &polygons, const std::vector< pcl::Vertices > &polygonsLowRes=std::vector< pcl::Vertices >(), bool createWireframe=false)
Definition: point_cloud_drawable.cpp:375
PointCloudDrawable::index_buffers_count_
std::vector< int > index_buffers_count_
Definition: point_cloud_drawable.h:121
kPointCloud
@ kPointCloud
Definition: point_cloud_drawable.cpp:49
kPointCloudLightingVertexShader
const std::string kPointCloudLightingVertexShader
Definition: point_cloud_drawable.cpp:81
rtabmap::Mesh::polygons
std::vector< pcl::Vertices > polygons
Definition: util.h:180
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
LOGI
#define LOGI(...)
Definition: tango-gl/include/tango-gl/util.h:53
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
PointCloudDrawable::poseGl_
glm::mat4 poseGl_
Definition: point_cloud_drawable.h:124
kPointCloudVertexShader
const std::string kPointCloudVertexShader
Definition: point_cloud_drawable.cpp:63
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
kTextureMeshFragmentShader
const std::string kTextureMeshFragmentShader
Definition: point_cloud_drawable.cpp:221
PointCloudDrawable::visible_
bool visible_
Definition: point_cloud_drawable.h:125
ULogger.h
ULogger class and convenient macros.
kPointCloudDepthPackingVertexShader
const std::string kPointCloudDepthPackingVertexShader
Definition: point_cloud_drawable.cpp:148
rtabmap::Transform
Definition: Transform.h:41
empty
kTextureMeshBlendingFragmentShader
const std::string kTextureMeshBlendingFragmentShader
Definition: point_cloud_drawable.cpp:235
LOGD
#define LOGD(...)
Definition: tango-gl/include/tango-gl/util.h:52
LOWLOW_DEC
#define LOWLOW_DEC
Definition: point_cloud_drawable.cpp:45
PointCloudDrawable::aabbMinModel_
pcl::PointXYZ aabbMinModel_
Definition: point_cloud_drawable.h:134
rtabmap::Mesh::indices
pcl::IndicesPtr indices
Definition: util.h:179
v
Array< int, Dynamic, 1 > v
LOW_DEC
#define LOW_DEC
Definition: point_cloud_drawable.cpp:44
UTimer
Definition: UTimer.h:46
kTextureMeshLightingVertexShader
const std::string kTextureMeshLightingVertexShader
Definition: point_cloud_drawable.cpp:192
rtabmap::Mesh::cloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud
Definition: util.h:177
float
float
PointCloudDrawable::~PointCloudDrawable
virtual ~PointCloudDrawable()
Definition: point_cloud_drawable.cpp:347
PointCloudDrawable::gainB_
float gainB_
Definition: point_cloud_drawable.h:132
glUniformMatrix4fv
void glUniformMatrix4fv(GLuint, int, int, float *)
Definition: dummy.cpp:80
PointCloudDrawable::updateAABBWorld
void updateAABBWorld(const rtabmap::Transform &pose)
Definition: point_cloud_drawable.cpp:972
false
#define false
Definition: ConvertUTF.c:56
rtabmap::glmFromTransform
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:124
dist
dist
kTextureLightingBlending
@ kTextureLightingBlending
Definition: point_cloud_drawable.cpp:57
kTexture
@ kTexture
Definition: point_cloud_drawable.cpp:54
rtabmap
Definition: CameraARCore.cpp:35
PointCloudDrawable::texture_
GLuint texture_
Definition: point_cloud_drawable.h:119
util.h
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
i
int i
PointCloudDrawable::index_buffers_
std::vector< GLuint > index_buffers_
Definition: point_cloud_drawable.h:120
rtabmap::Mesh::gains
double gains[3]
Definition: util.h:185
rtabmap::Mesh::texCoords
std::vector< Eigen::Vector2f > texCoords
Definition: util.h:189
point_cloud_drawable.h

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:14