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 vec3 aColor;\n"
174  "attribute vec2 aTexCoord;\n"
175 
176  "uniform mat4 uMVP;\n"
177 
178  "varying vec3 vColor;\n"
179  "varying vec2 vTexCoord;\n"
180  "varying float vLightWeighting;\n"
181 
182  "void main() {\n"
183  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
184 
185  " if(aTexCoord.x < 0.0) {\n"
186  " vTexCoord.x = 1.0;\n"
187  " vTexCoord.y = 1.0;\n" // bottom right corner
188  " } else {\n"
189  " vTexCoord = aTexCoord;\n"
190  " }\n"
191 
192  " vColor = aColor;\n"
193  " vLightWeighting = 1.0;\n"
194  "}\n";
195 const std::string kTextureMeshLightingVertexShader =
196  "precision highp float;\n"
197  "precision mediump int;\n"
198  "attribute vec3 aVertex;\n"
199  "attribute vec3 aColor;\n"
200  "attribute vec3 aNormal;\n"
201  "attribute vec2 aTexCoord;\n"
202 
203  "uniform mat4 uMVP;\n"
204  "uniform mat3 uN;\n"
205  "uniform vec3 uLightingDirection;\n"
206 
207  "varying vec3 vColor;\n"
208  "varying vec2 vTexCoord;\n"
209  "varying float vLightWeighting;\n"
210 
211  "void main() {\n"
212  " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
213 
214  " if(aTexCoord.x < 0.0) {\n"
215  " vTexCoord.x = 1.0;\n"
216  " vTexCoord.y = 1.0;\n" // bottom right corner
217  " } else {\n"
218  " vTexCoord = aTexCoord;\n"
219  " }\n"
220 
221  " vColor = aColor;\n"
222  " vec3 transformedNormal = uN * aNormal;\n"
223  " vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
224  " if(vLightWeighting<0.5) \n"
225  " vLightWeighting=0.5;\n"
226  "}\n";
227 const std::string kTextureMeshFragmentShader =
228  "precision highp float;\n"
229  "precision mediump int;\n"
230  "uniform sampler2D uTexture;\n"
231  "uniform float uGainR;\n"
232  "uniform float uGainG;\n"
233  "uniform float uGainB;\n"
234  "uniform int uHideSeams;\n"
235  "varying vec3 vColor;\n"
236  "varying vec2 vTexCoord;\n"
237  "varying float vLightWeighting;\n"
238  ""
239  "void main() {\n"
240  " vec4 textureColor;\n"
241  " if(uHideSeams==1) {\n"
242  " if(vTexCoord.x>0.99 && vTexCoord.y>0.99) {\n"
243  " textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
244  " } else {\n"
245  " textureColor = texture2D(uTexture, vTexCoord);\n"
246  " }\n"
247  " } else {\n"
248  " textureColor = texture2D(uTexture, vTexCoord) * vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
249  " }\n"
250  " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
251  "}\n";
252 const std::string kTextureMeshBlendingFragmentShader =
253  "precision highp float;\n"
254  "precision mediump int;\n"
255  "uniform sampler2D uTexture;\n"
256  "uniform sampler2D uDepthTexture;\n"
257  "uniform float uGainR;\n"
258  "uniform float uGainG;\n"
259  "uniform float uGainB;\n"
260  "uniform vec2 uScreenScale;\n"
261  "uniform float uNearZ;\n"
262  "uniform float uFarZ;\n"
263  "varying vec3 vColor;\n"
264  "varying vec2 vTexCoord;\n"
265  "varying float vLightWeighting;\n"
266  ""
267  "void main() {\n"
268  " vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
269  " float alpha = 1.0;\n"
270  " vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
271  " vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
272  " float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
273  " float num = (2.0 * uNearZ * uFarZ);\n"
274  " float diff = (uFarZ - uNearZ);\n"
275  " float add = (uFarZ + uNearZ);\n"
276  " float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
277  " float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
278  " float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
279  " float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
280  " if(linearFragz > linearDepth + 0.05)\n"
281  " alpha=0.0;\n"
282  " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
283  "}\n";
284 
285 std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
286 
287 void PointCloudDrawable::createShaderPrograms()
288 {
289  if(shaderPrograms_.empty())
290  {
291  shaderPrograms_.resize(9);
292 
293  shaderPrograms_[kPointCloud] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
294  UASSERT(shaderPrograms_[kPointCloud] != 0);
295  shaderPrograms_[kPointCloudBlending] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
296  UASSERT(shaderPrograms_[kPointCloudBlending] != 0);
297  shaderPrograms_[kPointCloudLighting] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudFragmentShader.c_str());
298  UASSERT(shaderPrograms_[kPointCloudLighting] != 0);
299  shaderPrograms_[kPointCloudLightingBlending] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
300  UASSERT(shaderPrograms_[kPointCloudLightingBlending] != 0);
301 
302  shaderPrograms_[kTexture] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
303  UASSERT(shaderPrograms_[kTexture] != 0);
304  shaderPrograms_[kTextureBlending] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
305  UASSERT(shaderPrograms_[kTextureBlending] != 0);
306  shaderPrograms_[kTextureLighting] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
307  UASSERT(shaderPrograms_[kTextureLighting] != 0);
308  shaderPrograms_[kTextureLightingBlending] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
309  UASSERT(shaderPrograms_[kTextureLightingBlending] != 0);
310 
311  shaderPrograms_[kDepthPacking] = tango_gl::util::CreateProgram(kPointCloudDepthPackingVertexShader.c_str(), kPointCloudDepthPackingFragmentShader.c_str());
312  UASSERT(shaderPrograms_[kDepthPacking] != 0);
313  }
314 }
315 void PointCloudDrawable::releaseShaderPrograms()
316 {
317  for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
318  {
319  glDeleteShader(shaderPrograms_[i]);
320  }
321  shaderPrograms_.clear();
322 }
323 
324 PointCloudDrawable::PointCloudDrawable(
325  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
326  const pcl::IndicesPtr & indices,
327  float gainR,
328  float gainG,
329  float gainB) :
330  vertex_buffer_(0),
331  texture_(0),
332  nPoints_(0),
333  pose_(rtabmap::Transform::getIdentity()),
334  poseGl_(1.0f),
335  visible_(true),
336  hasNormals_(false),
337  gainR_(gainR),
338  gainG_(gainG),
339  gainB_(gainB)
340 {
341  index_buffers_.resize(6, 0);
342  index_buffers_count_.resize(6, 0);
343  updateCloud(cloud, indices);
344 }
345 
346 PointCloudDrawable::PointCloudDrawable(
347  const rtabmap::Mesh & mesh,
348  bool createWireframe) :
349  vertex_buffer_(0),
350  texture_(0),
351  nPoints_(0),
352  pose_(rtabmap::Transform::getIdentity()),
353  poseGl_(1.0f),
354  visible_(true),
355  hasNormals_(false),
356  gainR_(1.0f),
357  gainG_(1.0f),
358  gainB_(1.0f)
359 {
360  index_buffers_.resize(6, 0);
361  index_buffers_count_.resize(6, 0);
362  updateMesh(mesh, createWireframe);
363 }
364 
365 PointCloudDrawable::~PointCloudDrawable()
366 {
367  LOGI("Freeing cloud buffer %d", vertex_buffer_);
368  if (vertex_buffer_)
369  {
370  glDeleteBuffers(1, &vertex_buffer_);
371  tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
372  vertex_buffer_ = 0;
373  }
374 
375  if (texture_)
376  {
377  glDeleteTextures(1, &texture_);
378  tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
379  texture_ = 0;
380  }
381 
382  for(size_t i=0; i<index_buffers_.size(); ++i)
383  {
384  if(index_buffers_[i])
385  {
386  glDeleteBuffers(1, &index_buffers_[i]);
387  index_buffers_[i] = 0;
388  tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
389  }
390  }
391 }
392 
393 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
394 {
395  for(int i=0; i<4; ++i)
396  {
397  if(index_buffers_[i])
398  {
399  glDeleteBuffers(1, &index_buffers_[i]);
400  index_buffers_[i] = 0;
401  tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
402  }
403  }
404 
405  //LOGD("Update polygons");
406  if(polygons.size() && organizedToDenseIndices_.size())
407  {
408  size_t polygonSize = polygons[0].vertices.size();
409  UASSERT(polygonSize == 3);
410  std::vector<std::vector<GLuint> > indexes(4);
411  indexes[0].resize(polygons.size() * polygonSize);
412  if(createWireframe)
413  indexes[2].resize(indexes[0].size()*2);
414  int oi = 0;
415  int li = 0;
416  for(size_t i=0; i<polygons.size(); ++i)
417  {
418  UASSERT(polygons[i].vertices.size() == polygonSize);
419  for(unsigned int j=0; j<polygonSize; ++j)
420  {
421  indexes[0][oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
422  if(createWireframe)
423  {
424  indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
425  indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
426  }
427  }
428  }
429 
430  if(polygonsLowRes.size())
431  {
432  size_t polygonSize = polygonsLowRes[0].vertices.size();
433  UASSERT(polygonSize == 3);
434  indexes[1].resize(polygonsLowRes.size() * polygonSize);
435  if(createWireframe)
436  indexes[3].resize(indexes[1].size()*2);
437  int oi = 0;
438  int li = 0;
439  for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
440  {
441  UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
442  for(unsigned int j=0; j<polygonSize; ++j)
443  {
444  indexes[1][oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
445  if(createWireframe)
446  {
447  indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
448  indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
449  }
450  }
451  }
452  }
453 
454  // Generate index buffers
455  for(size_t i=0; i<indexes.size(); ++i)
456  {
457  if(!indexes[i].empty())
458  {
459  glGenBuffers(1, &index_buffers_[i]);
460  if(!index_buffers_[i])
461  {
462  LOGE("OpenGL: could not generate index buffer %ld\n", i);
463  return;
464  }
465 
466  LOGD("Adding polygon index %ld size=%ld", i, indexes[i].size());
467 
468  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
469  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * indexes[i].size(), indexes[i].data(), GL_STATIC_DRAW);
470  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
471  index_buffers_count_[i] = (int)indexes[i].size();
472 
473  GLint error = glGetError();
474  if(error != GL_NO_ERROR)
475  {
476  LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
477  index_buffers_[i] = 0;
478  return;
479  }
480  }
481  }
482  }
483 }
484 
485 void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices)
486 {
487  UASSERT(cloud.get() && !cloud->empty());
488  nPoints_ = 0;
489  aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
490  aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
491 
492  if (vertex_buffer_)
493  {
494  glDeleteBuffers(1, &vertex_buffer_);
495  tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
496  vertex_buffer_ = 0;
497  }
498 
499  if (texture_)
500  {
501  glDeleteTextures(1, &texture_);
502  tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
503  texture_ = 0;
504  }
505 
506  for(size_t i=0; i<index_buffers_.size(); ++i)
507  {
508  if(index_buffers_[i])
509  {
510  glDeleteBuffers(1, &index_buffers_[i]);
511  index_buffers_[i] = 0;
512  tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear index buffer");
513  }
514  }
515 
516  glGenBuffers(1, &vertex_buffer_);
517  if(!vertex_buffer_)
518  {
519  LOGE("OpenGL: could not generate vertex buffers\n");
520  return;
521  }
522 
523  LOGI("Creating cloud buffer %d", vertex_buffer_);
524  std::vector<float> vertices;
525  size_t totalPoints = 0;
526  std::vector<GLuint> verticesLowRes;
527  std::vector<GLuint> verticesLowLowRes;
528  if(indices.get() && indices->size())
529  {
530  totalPoints = indices->size();
531  vertices.resize(indices->size()*4);
532  verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
533  verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
534  int oi_low = 0;
535  int oi_lowlow = 0;
536  for(unsigned int i=0; i<indices->size(); ++i)
537  {
538  const pcl::PointXYZRGB & pt = cloud->at(indices->at(i));
539  vertices[i*4] = pt.x;
540  vertices[i*4+1] = pt.y;
541  vertices[i*4+2] = pt.z;
542  vertices[i*4+3] = pt.rgb;
543 
544  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
545 
546  if(cloud->isOrganized())
547  {
548  if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
549  {
550  verticesLowRes[oi_low++] = i;
551  }
552  if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
553  {
554  verticesLowLowRes[oi_lowlow++] = i;
555  }
556  }
557  }
558  verticesLowRes.resize(oi_low);
559  verticesLowLowRes.resize(oi_lowlow);
560  }
561  else
562  {
563  totalPoints = cloud->size();
564  vertices.resize(cloud->size()*4);
565  verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
566  verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
567  int oi_low = 0;
568  int oi_lowlow = 0;
569  for(unsigned int i=0; i<cloud->size(); ++i)
570  {
571  const pcl::PointXYZRGB & pt = cloud->at(i);
572  vertices[i*4] = pt.x;
573  vertices[i*4+1] = pt.y;
574  vertices[i*4+2] = pt.z;
575  vertices[i*4+3] = pt.rgb;
576 
577  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
578 
579  if(cloud->isOrganized())
580  {
581  if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
582  {
583  verticesLowRes[oi_low++] = i;
584  }
585  if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
586  {
587  verticesLowLowRes[oi_lowlow++] = i;
588  }
589  }
590  }
591  verticesLowRes.resize(oi_low);
592  verticesLowLowRes.resize(oi_lowlow);
593  }
594 
595  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
596  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
597  glBindBuffer(GL_ARRAY_BUFFER, 0);
598 
599  GLint error = glGetError();
600  if(error != GL_NO_ERROR)
601  {
602  LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
603  vertex_buffer_ = 0;
604  return;
605  }
606 
607  // vertex index buffers
608  for(size_t i=4; i<5; ++i)
609  {
610  if((i==4 && !verticesLowRes.empty()) ||
611  (i==5 && !verticesLowLowRes.empty()))
612  {
613  glGenBuffers(1, &index_buffers_[i]);
614  if(!index_buffers_[i])
615  {
616  LOGE("OpenGL: could not generate index buffer %ld\n", i);
617  return;
618  }
619 
620  index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
621  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
622  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
623  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
624 
625 
626  GLint error = glGetError();
627  if(error != GL_NO_ERROR)
628  {
629  LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
630  index_buffers_[i] = 0;
631  return;
632  }
633  }
634  }
635 
636  nPoints_ = (int)totalPoints;
637 }
638 
639 void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
640 {
641  UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
642  nPoints_ = 0;
643  aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
644  aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
645 
646  if (vertex_buffer_)
647  {
648  glDeleteBuffers(1, &vertex_buffer_);
649  tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
650  vertex_buffer_ = 0;
651  }
652 
653  for(size_t i=0; i<index_buffers_.size(); ++i)
654  {
655  if(index_buffers_[i])
656  {
657  glDeleteBuffers(1, &index_buffers_[i]);
658  index_buffers_[i] = 0;
659  tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear index buffer");
660  }
661  }
662 
663  gainR_ = mesh.gains[0];
664  gainG_ = mesh.gains[1];
665  gainB_ = mesh.gains[2];
666 
667  bool textureUpdate = false;
668  if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
669  {
670  if (texture_)
671  {
672  glDeleteTextures(1, &texture_);
673  tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
674  texture_ = 0;
675  }
676  textureUpdate = true;
677  }
678 
679  glGenBuffers(1, &vertex_buffer_);
680  if(!vertex_buffer_)
681  {
682  LOGE("OpenGL: could not generate vertex buffers\n");
683  return;
684  }
685 
686  if(textureUpdate)
687  {
688  glGenTextures(1, &texture_);
689  if(!texture_)
690  {
691  vertex_buffer_ = 0;
692  LOGE("OpenGL: could not generate texture buffers\n");
693  return;
694  }
695  }
696 
697  //LOGD("Creating cloud buffer %d", vertex_buffers_);
698  std::vector<float> vertices;
699  int totalPoints = 0;
700  std::vector<pcl::Vertices> polygons = mesh.polygons;
701  std::vector<pcl::Vertices> polygonsLowRes;
702  hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
703  UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
704  if(mesh.cloud->isOrganized()) // assume organized mesh
705  {
706  polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
707  organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
708  totalPoints = (int)mesh.indices->size();
709  std::vector<GLuint> verticesLowRes;
710  std::vector<GLuint> verticesLowLowRes;
711  verticesLowRes.resize(totalPoints);
712  verticesLowLowRes.resize(totalPoints);
713  int oi_low = 0;
714  int oi_lowlow = 0;
715  if(texture_ && polygons.size())
716  {
717  int items = hasNormals_?9:6;
718  vertices = std::vector<float>(mesh.indices->size()*items);
719  for(unsigned int i=0; i<mesh.indices->size(); ++i)
720  {
721  const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
722  vertices[i*items] = pt.x;
723  vertices[i*items+1] = pt.y;
724  vertices[i*items+2] = pt.z;
725 
726  // rgb
727  vertices[i*items+3] = pt.rgb;
728 
729  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
730 
731  // texture uv
732  int index = mesh.indices->at(i);
733  vertices[i*items+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
734  vertices[i*items+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height); //v
735 
736  if(hasNormals_)
737  {
738  // normal
739  vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_x;
740  vertices[i*items+7] = mesh.normals->at(mesh.indices->at(i)).normal_y;
741  vertices[i*items+8] = mesh.normals->at(mesh.indices->at(i)).normal_z;
742  }
743 
744  organizedToDenseIndices_[mesh.indices->at(i)] = i;
745 
746  if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
747  {
748  verticesLowRes[oi_low++] = i;
749  }
750  if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
751  {
752  verticesLowLowRes[oi_lowlow++] = i;
753  }
754  }
755  }
756  else
757  {
758  //LOGD("Organized mesh");
759  int items = hasNormals_?7:4;
760  vertices = std::vector<float>(mesh.indices->size()*items);
761  for(unsigned int i=0; i<mesh.indices->size(); ++i)
762  {
763  const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
764  vertices[i*items] = pt.x;
765  vertices[i*items+1] = pt.y;
766  vertices[i*items+2] = pt.z;
767  vertices[i*items+3] = pt.rgb;
768 
769  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
770 
771  if(hasNormals_)
772  {
773  // normal
774  vertices[i*items+4] = mesh.normals->at(mesh.indices->at(i)).normal_x;
775  vertices[i*items+5] = mesh.normals->at(mesh.indices->at(i)).normal_y;
776  vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_z;
777  }
778 
779  organizedToDenseIndices_[mesh.indices->at(i)] = i;
780 
781  if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
782  {
783  verticesLowRes[oi_low++] = i;
784  }
785  if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
786  {
787  verticesLowLowRes[oi_lowlow++] = i;
788  }
789  }
790  }
791  verticesLowRes.resize(oi_low);
792  verticesLowLowRes.resize(oi_lowlow);
793 
794  // vertex index buffers
795  for(size_t i=4; i<5; ++i)
796  {
797  if((i==4 && !verticesLowRes.empty()) ||
798  (i==5 && !verticesLowLowRes.empty()))
799  {
800  glGenBuffers(1, &index_buffers_[i]);
801  if(!index_buffers_[i])
802  {
803  LOGE("OpenGL: could not generate index buffer %ld\n", i);
804  return;
805  }
806 
807  index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
808  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
809  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
810  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
811 
812  GLint error = glGetError();
813  if(error != GL_NO_ERROR)
814  {
815  LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
816  index_buffers_[i] = 0;
817  return;
818  }
819  }
820  }
821  }
822  else // assume dense mesh with texCoords set to polygons
823  {
824  if(texture_ && polygons.size())
825  {
826  //LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
827  // (int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
828 
829  // Texturing issue:
830  // tex_coordinates should be linked to points, not
831  // polygon vertices. Points linked to multiple different texCoords (different textures) should
832  // be duplicated.
833  totalPoints = (int)mesh.texCoords.size();
834  int items = hasNormals_?9:6;
835  vertices = std::vector<float>(mesh.texCoords.size()*items);
836  organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
837 
838  UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
839  uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
840 
841  unsigned int oi=0;
842  for(unsigned int i=0; i<polygons.size(); ++i)
843  {
844  pcl::Vertices & v = polygons[i];
845  for(unsigned int j=0; j<v.vertices.size(); ++j)
846  {
847  UASSERT(oi < mesh.texCoords.size());
848  UASSERT(v.vertices[j] < mesh.cloud->size());
849 
850  const pcl::PointXYZRGB & pt = mesh.cloud->at(v.vertices[j]);
851 
852  vertices[oi*items] = pt.x;
853  vertices[oi*items+1] = pt.y;
854  vertices[oi*items+2] = pt.z;
855 
856  // rgb
857  vertices[oi*items+3] = pt.rgb;
858 
859  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
860 
861  // texture uv
862  if(mesh.texCoords[oi][0]>=0.0f)
863  {
864  vertices[oi*items+4] = mesh.texCoords[oi][0]; //u
865  vertices[oi*items+5] = 1.0f-mesh.texCoords[oi][1]; //v
866  }
867  else
868  {
869  vertices[oi*items+4] = vertices[oi*items+5] = -1.0f;
870  }
871 
872  if(hasNormals_)
873  {
874  // normal
875  vertices[oi*items+6] = mesh.normals->at(v.vertices[j]).normal_x;
876  vertices[oi*items+7] = mesh.normals->at(v.vertices[j]).normal_y;
877  vertices[oi*items+8] = mesh.normals->at(v.vertices[j]).normal_z;
878  }
879 
880  v.vertices[j] = (int)oi; // new vertex index
881 
882  UASSERT(oi < organizedToDenseIndices_.size());
883  organizedToDenseIndices_[oi] = oi;
884 
885  ++oi;
886  }
887  }
888  }
889  else
890  {
891  totalPoints = (int)mesh.cloud->size();
892  //LOGD("Dense mesh");
893  int items = hasNormals_?7:4;
894  organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
895  vertices = std::vector<float>(mesh.cloud->size()*items);
896  for(unsigned int i=0; i<mesh.cloud->size(); ++i)
897  {
898  const pcl::PointXYZRGB & pt = mesh.cloud->at(i);
899 
900  vertices[i*items] =pt.x;
901  vertices[i*items+1] = pt.y;
902  vertices[i*items+2] = pt.z;
903  vertices[i*items+3] = pt.rgb;
904 
905  updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
906 
907  if(hasNormals_)
908  {
909  vertices[i*items+4] = mesh.normals->at(i).normal_x;
910  vertices[i*items+5] = mesh.normals->at(i).normal_y;
911  vertices[i*items+6] = mesh.normals->at(i).normal_z;
912  }
913 
914  organizedToDenseIndices_[i] = i;
915  }
916  }
917  }
918 
919  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
920  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
921  glBindBuffer(GL_ARRAY_BUFFER, 0);
922 
923  GLint error = glGetError();
924  if(error != GL_NO_ERROR)
925  {
926  LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
927  vertex_buffer_ = 0;
928  return;
929  }
930 
931  if(texture_ && textureUpdate)
932  {
933  //GLint maxTextureSize = 0;
934  //glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
935  //LOGI("maxTextureSize=%d", maxTextureSize);
936  //GLint maxTextureUnits = 0;
937  //glGetIntegerv(GL_MAX_TEXTURE_IMAGE_UNITS, &maxTextureUnits);
938  //LOGW("maxTextureUnits=%d", maxTextureUnits);
939 
940  // gen texture from image
941  glBindTexture(GL_TEXTURE_2D, texture_);
942  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
943  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
944  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
945  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
946  cv::Mat rgbImage;
947  cv::cvtColor(mesh.texture, rgbImage, cv::COLOR_BGR2RGBA);
948 
949  glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
950  //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
951  //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
952  //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
953  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
954 
955  GLint error = glGetError();
956  if(error != GL_NO_ERROR)
957  {
958  LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
959  texture_ = 0;
960 
961  glDeleteBuffers(1, &vertex_buffer_);
962  vertex_buffer_ = 0;
963  return;
964  }
965  }
966 
967  nPoints_ = totalPoints;
968 
969  updatePolygons(polygons, polygonsLowRes, createWireframe);
970 
971  if(!pose_.isNull())
972  {
973  updateAABBWorld(pose_);
974  }
975 }
976 
977 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
978 {
979  UASSERT(!pose.isNull());
980 
981  if(pose_ != pose)
982  {
983  updateAABBWorld(pose);
984  }
985 
986  pose_ = pose;
987  poseGl_ = glmFromTransform(pose);
988 }
989 
990 void PointCloudDrawable::updateAABBWorld(const rtabmap::Transform & pose)
991 {
992  pcl::PointCloud<pcl::PointXYZ> corners;
993  corners.resize(8);
994  corners.at(0) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMinModel_.z);
995  corners.at(1) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
996  corners.at(2) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
997  corners.at(3) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMinModel_.z);
998  corners.at(4) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
999  corners.at(5) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
1000  corners.at(6) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
1001  corners.at(7) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
1002 
1003  pcl::PointCloud<pcl::PointXYZ> cornersTransformed;
1004  pcl::transformPointCloud(corners, cornersTransformed, pose.toEigen3f());
1005 
1006  aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
1007  aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
1008  for(unsigned int i=0; i<cornersTransformed.size(); ++i)
1009  {
1010  updateAABBMinMax(cornersTransformed.at(i), aabbMinWorld_, aabbMaxWorld_);
1011  }
1012 }
1013 
1014 
1015 void PointCloudDrawable::Render(
1016  const glm::mat4 & projectionMatrix,
1017  const glm::mat4 & viewMatrix,
1018  bool meshRendering,
1019  float pointSize,
1020  bool textureRendering,
1021  bool lighting,
1022  float distanceToCameraSqr,
1023  const GLuint & depthTexture,
1024  int screenWidth,
1025  int screenHeight,
1026  float nearClipPlane,
1027  float farClipPlane,
1028  bool packDepthToColorChannel,
1029  bool wireFrame,
1030  bool hideSeams) const
1031 {
1032  if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
1033  {
1034  if(packDepthToColorChannel || !hasNormals_)
1035  {
1036  lighting = false;
1037  }
1038 
1039  if(packDepthToColorChannel || !(meshRendering && textureRendering && texture_))
1040  {
1041  textureRendering = false;
1042  }
1043 
1044  GLuint program;
1045  if(packDepthToColorChannel)
1046  {
1047  program = shaderPrograms_[kDepthPacking];
1048  }
1049  else if(textureRendering)
1050  {
1051  if(lighting)
1052  {
1053  program = shaderPrograms_[depthTexture>0?kTextureLightingBlending:kTextureLighting];
1054  }
1055  else
1056  {
1057  program = shaderPrograms_[depthTexture>0?kTextureBlending:kTexture];
1058  }
1059  }
1060  else
1061  {
1062  if(lighting)
1063  {
1064  program = shaderPrograms_[depthTexture>0?kPointCloudLightingBlending:kPointCloudLighting];
1065  }
1066  else
1067  {
1068  program = shaderPrograms_[depthTexture>0?kPointCloudBlending:kPointCloud];
1069  }
1070  }
1071 
1072  glUseProgram(program);
1073  tango_gl::util::CheckGlError("Pointcloud::Render() set program");
1074 
1075  GLuint mvp_handle = glGetUniformLocation(program, "uMVP");
1076  glm::mat4 mv_mat = viewMatrix * poseGl_;
1077  glm::mat4 mvp_mat = projectionMatrix * mv_mat;
1078  glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
1079 
1080  GLint attribute_vertex = glGetAttribLocation(program, "aVertex");
1081  glEnableVertexAttribArray(attribute_vertex);
1082  GLint attribute_color = 0;
1083  GLint attribute_texture = 0;
1084  GLint attribute_normal = 0;
1085 
1086  if(packDepthToColorChannel || !textureRendering)
1087  {
1088  GLuint point_size_handle_ = glGetUniformLocation(program, "uPointSize");
1089  glUniform1f(point_size_handle_, pointSize);
1090  }
1091  tango_gl::util::CheckGlError("Pointcloud::Render() vertex");
1092 
1093  if(!packDepthToColorChannel)
1094  {
1095  GLuint gainR_handle = glGetUniformLocation(program, "uGainR");
1096  GLuint gainG_handle = glGetUniformLocation(program, "uGainG");
1097  GLuint gainB_handle = glGetUniformLocation(program, "uGainB");
1098  glUniform1f(gainR_handle, gainR_);
1099  glUniform1f(gainG_handle, gainG_);
1100  glUniform1f(gainB_handle, gainB_);
1101 
1102  // blending
1103  if(depthTexture > 0)
1104  {
1105  // Texture activate unit 1
1106  glActiveTexture(GL_TEXTURE1);
1107  // Bind the texture to this unit.
1108  glBindTexture(GL_TEXTURE_2D, depthTexture);
1109  // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
1110  GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
1111  glUniform1i(depth_texture_handle, 1);
1112 
1113  GLuint zNear_handle = glGetUniformLocation(program, "uNearZ");
1114  GLuint zFar_handle = glGetUniformLocation(program, "uFarZ");
1115  glUniform1f(zNear_handle, nearClipPlane);
1116  glUniform1f(zFar_handle, farClipPlane);
1117 
1118  GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
1119  glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
1120  }
1121 
1122  if(lighting)
1123  {
1124  GLuint n_handle = glGetUniformLocation(program, "uN");
1125  glm::mat3 normalMatrix(mv_mat);
1126  normalMatrix = glm::inverse(normalMatrix);
1127  normalMatrix = glm::transpose(normalMatrix);
1128  glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
1129 
1130  GLuint lightingDirection_handle = glGetUniformLocation(program, "uLightingDirection");
1131  glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
1132 
1133  attribute_normal = glGetAttribLocation(program, "aNormal");
1134  glEnableVertexAttribArray(attribute_normal);
1135  }
1136 
1137  if(textureRendering)
1138  {
1139  // Texture activate unit 0
1140  glActiveTexture(GL_TEXTURE0);
1141  // Bind the texture to this unit.
1142  glBindTexture(GL_TEXTURE_2D, texture_);
1143  // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
1144  GLuint texture_handle = glGetUniformLocation(program, "uTexture");
1145  glUniform1i(texture_handle, 0);
1146 
1147  attribute_texture = glGetAttribLocation(program, "aTexCoord");
1148  glEnableVertexAttribArray(attribute_texture);
1149 
1150  if(depthTexture == 0)
1151  {
1152  GLuint hideSeams_handle = glGetUniformLocation(program, "uHideSeams");
1153  glUniform1i(hideSeams_handle, hideSeams?1:0);
1154  }
1155  }
1156 
1157  attribute_color = glGetAttribLocation(program, "aColor");
1158  glEnableVertexAttribArray(attribute_color);
1159  }
1160  tango_gl::util::CheckGlError("Pointcloud::Render() common");
1161 
1162  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
1163  if(texture_)
1164  {
1165  glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
1166  if(textureRendering)
1167  {
1168  glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
1169  }
1170  if(!packDepthToColorChannel)
1171  {
1172  glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
1173  }
1174  if(lighting && hasNormals_)
1175  {
1176  glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
1177  }
1178  }
1179  else
1180  {
1181  glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
1182  if(!packDepthToColorChannel)
1183  {
1184  glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
1185  }
1186  if(lighting && hasNormals_)
1187  {
1188  glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
1189  }
1190  }
1191  tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
1192 
1193  UTimer drawTime;
1194  if((textureRendering || meshRendering) && index_buffers_[0])
1195  {
1196  float dist = meshRendering?50.0f:16.0f;
1197  if(distanceToCameraSqr<dist || index_buffers_[1]==0)
1198  {
1199  wireFrame = wireFrame && index_buffers_[2];
1200  if(wireFrame)
1201  {
1202  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[2]);
1203  glDrawElements(GL_LINES, index_buffers_count_[2], GL_UNSIGNED_INT, 0);
1204  }
1205  else
1206  {
1207  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[0]);
1208  glDrawElements(GL_TRIANGLES, index_buffers_count_[0], GL_UNSIGNED_INT, 0);
1209  }
1210  }
1211  else
1212  {
1213  wireFrame = wireFrame && index_buffers_[3];
1214  if(wireFrame)
1215  {
1216  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[3]);
1217  glDrawElements(GL_LINES, index_buffers_count_[3], GL_UNSIGNED_INT, 0);
1218  }
1219  else
1220  {
1221  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[1]);
1222  glDrawElements(GL_TRIANGLES, index_buffers_count_[1], GL_UNSIGNED_INT, 0);
1223  }
1224  }
1225  }
1226  else if(index_buffers_[4])
1227  {
1228  if(distanceToCameraSqr>600.0f && index_buffers_[5])
1229  {
1230  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
1231  glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
1232  }
1233  else if(distanceToCameraSqr>150.0f)
1234  {
1235  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[4]);
1236  glDrawElements(GL_POINTS, index_buffers_count_[4], GL_UNSIGNED_INT, 0);
1237  }
1238  else
1239  {
1240  glDrawArrays(GL_POINTS, 0, nPoints_);
1241  }
1242  }
1243  else
1244  {
1245  glDrawArrays(GL_POINTS, 0, nPoints_);
1246  }
1247  //UERROR("drawTime=%fs", drawTime.ticks());
1248  tango_gl::util::CheckGlError("Pointcloud::Render() draw");
1249 
1250  glDisableVertexAttribArray(0);
1251  glBindBuffer(GL_ARRAY_BUFFER, 0);
1252  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
1253 
1254  glUseProgram(0);
1255  tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");
1256  }
1257 }
1258 
int
int
PointCloudDrawable::releaseShaderPrograms
static void releaseShaderPrograms()
Definition: point_cloud_drawable.cpp:315
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:106
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:977
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
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:127
PointCloudDrawable::aabbMaxModel_
pcl::PointXYZ aabbMaxModel_
Definition: point_cloud_drawable.h:136
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:128
PointCloudDrawable::pose_
rtabmap::Transform pose_
Definition: point_cloud_drawable.h:124
PointCloudDrawable::vertex_buffer_
GLuint vertex_buffer_
Definition: point_cloud_drawable.h:119
PointCloudDrawable::nPoints_
int nPoints_
Definition: point_cloud_drawable.h:123
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:137
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
vertices
static const float vertices[]
Definition: quad_color.cpp:20
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:131
PointCloudDrawable::gainG_
float gainG_
Definition: point_cloud_drawable.h:132
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:324
PointCloudDrawable::updateCloud
void updateCloud(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices)
Definition: point_cloud_drawable.cpp:485
PointCloudDrawable::aabbMaxWorld_
pcl::PointXYZ aabbMaxWorld_
Definition: point_cloud_drawable.h:138
PointCloudDrawable::createShaderPrograms
static void createShaderPrograms()
Definition: point_cloud_drawable.cpp:287
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:639
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:393
PointCloudDrawable::index_buffers_count_
std::vector< int > index_buffers_count_
Definition: point_cloud_drawable.h:122
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:125
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:227
PointCloudDrawable::visible_
bool visible_
Definition: point_cloud_drawable.h:126
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:252
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:135
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:195
rtabmap::Mesh::cloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud
Definition: util.h:177
float
float
PointCloudDrawable::~PointCloudDrawable
virtual ~PointCloudDrawable()
Definition: point_cloud_drawable.cpp:365
PointCloudDrawable::gainB_
float gainB_
Definition: point_cloud_drawable.h:133
glUniformMatrix4fv
void glUniformMatrix4fv(GLuint, int, int, float *)
Definition: dummy.cpp:80
PointCloudDrawable::updateAABBWorld
void updateAABBWorld(const rtabmap::Transform &pose)
Definition: point_cloud_drawable.cpp:990
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:120
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:121
rtabmap::Mesh::gains
double gains[3]
Definition: util.h:185
rtabmap::Mesh::texCoords
std::vector< Eigen::Vector2f > texCoords
Definition: util.h:189
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, bool hideSeams=false) const
Definition: point_cloud_drawable.cpp:1015
point_cloud_drawable.h

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:58