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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59