scene.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014 Google Inc. All Rights Reserved.
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <tango-gl/conversions.h>
19 #include <tango-gl/util.h>
20 
22 #include <rtabmap/utilite/UStl.h>
23 #include <rtabmap/utilite/UTimer.h>
27 #include <pcl/common/transforms.h>
28 #include <pcl/common/common.h>
29 
30 #include <glm/gtx/transform.hpp>
31 
32 #include "scene.h"
33 #include "util.h"
34 
35 // We want to represent the device properly with respect to the ground so we'll
36 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
37 // on the average height of a human standing with a Tango device. This allows us
38 // to place a grid roughly on the ground for most users.
39 const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
40 
41 // Color of the motion tracking trajectory.
42 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
43 
44 // Color of the ground grid.
45 const tango_gl::Color kGridColor(0.85f, 0.85f, 0.85f);
46 
47 // Frustum scale.
48 const glm::vec3 kFrustumScale = glm::vec3(0.4f, 0.3f, 0.5f);
49 
50 const std::string kGraphVertexShader =
51  "precision mediump float;\n"
52  "precision mediump int;\n"
53  "attribute vec3 vertex;\n"
54  "uniform vec3 color;\n"
55  "uniform mat4 mvp;\n"
56  "varying vec3 v_color;\n"
57  "void main() {\n"
58  " gl_Position = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
59  " v_color = color;\n"
60  "}\n";
61 const std::string kGraphFragmentShader =
62  "precision mediump float;\n"
63  "precision mediump int;\n"
64  "varying vec3 v_color;\n"
65  "void main() {\n"
66  " gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
67  "}\n";
68 
69 
70 
72  background_renderer_(0),
73  gesture_camera_(0),
74  axis_(0),
75  frustum_(0),
76  grid_(0),
77  box_(0),
78  trace_(0),
79  graph_(0),
80  graphVisible_(true),
81  gridVisible_(true),
82  traceVisible_(true),
83  frustumVisible_(true),
84  color_camera_to_display_rotation_(rtabmap::ROTATION_0),
85  currentPose_(0),
86  graph_shader_program_(0),
87  blending_(true),
88  mapRendering_(true),
89  meshRendering_(true),
90  meshRenderingTexture_(true),
91  pointSize_(10.0f),
92  boundingBoxRendering_(false),
93  lighting_(false),
94  backfaceCulling_(true),
95  wireFrame_(false),
96  r_(0.0f),
97  g_(0.0f),
98  b_(0.0f),
99  fboId_(0),
100  rboId_(0),
101  screenWidth_(0),
102  screenHeight_(0),
103  doubleTapOn_(false)
104 {
105  depthTexture_ = 0;
109 }
110 
112  DeleteResources();
113  delete gesture_camera_;
114  delete currentPose_;
115 }
116 
117 //Should only be called in OpenGL thread!
119 {
120  if(axis_ != 0)
121  {
122  DeleteResources();
123  }
124 
125  UASSERT(axis_ == 0);
126 
127 
128  axis_ = new tango_gl::Axis();
129  frustum_ = new tango_gl::Frustum();
130  trace_ = new tango_gl::Trace();
131  grid_ = new tango_gl::Grid();
132  box_ = new BoundingBoxDrawable();
133 
134 
135  axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
140  grid_->SetPosition(kHeightOffset);
141  box_->SetShader();
142  box_->SetColor(1,0,0);
143 
145 
146  if(graph_shader_program_ == 0)
147  {
150  }
151 }
152 
153 //Should only be called in OpenGL thread!
155 
156  LOGI("Scene::DeleteResources()");
157  if(axis_)
158  {
159  delete axis_;
160  axis_ = 0;
161  delete frustum_;
162  delete trace_;
163  delete grid_;
164  delete box_;
165  delete background_renderer_;
167  }
168 
170 
171  if (graph_shader_program_) {
172  glDeleteShader(graph_shader_program_);
174  }
175 
176  if(fboId_>0)
177  {
178  glDeleteFramebuffers(1, &fboId_);
179  fboId_ = 0;
180  glDeleteRenderbuffers(1, &rboId_);
181  rboId_ = 0;
182  glDeleteTextures(1, &depthTexture_);
183  depthTexture_ = 0;
184  }
185 
186  clear();
187 }
188 
189 //Should only be called in OpenGL thread!
191 {
192  LOGI("Scene::clear()");
193  for(std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
194  {
195  delete iter->second;
196  }
197  for(std::map<int, tango_gl::Axis*>::iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
198  {
199  delete iter->second;
200  }
201  if(trace_)
202  {
204  }
205  if(graph_)
206  {
207  delete graph_;
208  graph_ = 0;
209  }
210  pointClouds_.clear();
211  markers_.clear();
212  if(grid_)
213  {
214  grid_->SetPosition(kHeightOffset);
215  }
216 }
217 
218 //Should only be called in OpenGL thread!
219 void Scene::SetupViewPort(int w, int h) {
220  if (h == 0) {
221  LOGE("Setup graphic height not valid");
222  }
223 
224  UASSERT(gesture_camera_ != 0);
225  gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
226  glViewport(0, 0, w, h);
227  if(screenWidth_ != w || screenHeight_ != h || fboId_ == 0)
228  {
229  UINFO("Setup viewport OpenGL: %dx%d", w, h);
230 
231  if(fboId_>0)
232  {
233  glDeleteFramebuffers(1, &fboId_);
234  fboId_ = 0;
235  glDeleteRenderbuffers(1, &rboId_);
236  rboId_ = 0;
237  glDeleteTextures(1, &depthTexture_);
238  depthTexture_ = 0;
239  }
240 
241  GLint originid = 0;
242  glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
243 
244  // regenerate fbo texture
245  // create a framebuffer object, you need to delete them when program exits.
246  glGenFramebuffers(1, &fboId_);
247  glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
248 
249  // Create depth texture
250  glGenTextures(1, &depthTexture_);
251  glBindTexture(GL_TEXTURE_2D, depthTexture_);
252  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
253  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
254  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
255  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
256  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
257  glBindTexture(GL_TEXTURE_2D, 0);
258 
259  glGenRenderbuffers(1, &rboId_);
260  glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
261  glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
262  glBindRenderbuffer(GL_RENDERBUFFER, 0);
263 
264  // Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
265  glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
266  glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
267 
268  GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
269  UASSERT ( status == GL_FRAMEBUFFER_COMPLETE);
270  glBindFramebuffer(GL_FRAMEBUFFER, originid);
271  }
272  screenWidth_ = w;
273  screenHeight_ = h;
274 }
275 
276 std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normalize = true)
277 {
278  // http://www.txutxi.com/?p=444
279  std::vector<glm::vec4> planes(6);
280 
281  // Left Plane
282  // col4 + col1
283  planes[0].x = mat[0][3] + mat[0][0];
284  planes[0].y = mat[1][3] + mat[1][0];
285  planes[0].z = mat[2][3] + mat[2][0];
286  planes[0].w = mat[3][3] + mat[3][0];
287 
288  // Right Plane
289  // col4 - col1
290  planes[1].x = mat[0][3] - mat[0][0];
291  planes[1].y = mat[1][3] - mat[1][0];
292  planes[1].z = mat[2][3] - mat[2][0];
293  planes[1].w = mat[3][3] - mat[3][0];
294 
295  // Bottom Plane
296  // col4 + col2
297  planes[2].x = mat[0][3] + mat[0][1];
298  planes[2].y = mat[1][3] + mat[1][1];
299  planes[2].z = mat[2][3] + mat[2][1];
300  planes[2].w = mat[3][3] + mat[3][1];
301 
302  // Top Plane
303  // col4 - col2
304  planes[3].x = mat[0][3] - mat[0][1];
305  planes[3].y = mat[1][3] - mat[1][1];
306  planes[3].z = mat[2][3] - mat[2][1];
307  planes[3].w = mat[3][3] - mat[3][1];
308 
309  // Near Plane
310  // col4 + col3
311  planes[4].x = mat[0][3] + mat[0][2];
312  planes[4].y = mat[1][3] + mat[1][2];
313  planes[4].z = mat[2][3] + mat[2][2];
314  planes[4].w = mat[3][3] + mat[3][2];
315 
316  // Far Plane
317  // col4 - col3
318  planes[5].x = mat[0][3] - mat[0][2];
319  planes[5].y = mat[1][3] - mat[1][2];
320  planes[5].z = mat[2][3] - mat[2][2];
321  planes[5].w = mat[3][3] - mat[3][2];
322 
323  //if(normalize)
324  {
325  for(unsigned int i=0;i<planes.size(); ++i)
326  {
327  if(normalize)
328  {
329  float d = std::sqrt(planes[i].x * planes[i].x + planes[i].y * planes[i].y + planes[i].z * planes[i].z); // for normalizing the coordinates
330  planes[i].x/=d;
331  planes[i].y/=d;
332  planes[i].z/=d;
333  planes[i].w/=d;
334  }
335  }
336  }
337 
338  return planes;
339 }
340 
350  const std::vector<glm::vec4> &planes,
351  const pcl::PointXYZ &boxMin,
352  const pcl::PointXYZ &boxMax)
353 {
354  // Indexed for the 'index trick' later
355  const pcl::PointXYZ * box[] = {&boxMin, &boxMax};
356 
357  // We only need to do 6 point-plane tests
358  for (unsigned int i = 0; i < planes.size(); ++i)
359  {
360  // This is the current plane
361  const glm::vec4 &p = planes[i];
362 
363  // p-vertex selection (with the index trick)
364  // According to the plane normal we can know the
365  // indices of the positive vertex
366  const int px = p.x > 0.0f?1:0;
367  const int py = p.y > 0.0f?1:0;
368  const int pz = p.z > 0.0f?1:0;
369 
370  // Dot product
371  // project p-vertex on plane normal
372  // (How far is p-vertex from the origin)
373  const float dp =
374  (p.x*box[px]->x) +
375  (p.y*box[py]->y) +
376  (p.z*box[pz]->z) + p.w;
377 
378  // Doesn't intersect if it is behind the plane
379  if (dp < 0) {return false; }
380  }
381  return true;
382 }
383 
384 //Should only be called in OpenGL thread!
385 int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping)
386 {
387  UASSERT(gesture_camera_ != 0);
388 
389  if(currentPose_ == 0)
390  {
391  currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
392  }
393  glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
394  Eigen::Quaternionf quat = currentPose_->getQuaternionf();
395  glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
396  glm::mat4 rotateM;
397  if(!currentPose_->isNull())
398  {
399  rotateM = glm::rotate<float>(float(color_camera_to_display_rotation_)*-1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f));
400 
402  {
403  // In first person mode, we directly control camera's motion.
404  gesture_camera_->SetPosition(position);
406  }
407  else
408  {
409  // In third person or top down mode, we follow the camera movement.
411  }
412  }
413 
414  glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
415  glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
416 
417  bool renderBackgroundCamera =
420  !rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
421  uvsTransformed;
422 
423  if(renderBackgroundCamera)
424  {
425  if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.3)
426  {
427  projectionMatrix = arProjectionMatrix;
428  viewMatrix = arViewMatrix;
429  }
430  else
431  {
432  renderBackgroundCamera = false;
433  }
434  }
435 
436  rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
437  // transform in same coordinate as frustum filtering
438  openglCamera *= rtabmap::Transform(
439  0.0f, 0.0f, 1.0f, 0.0f,
440  0.0f, 1.0f, 0.0f, 0.0f,
441  -1.0f, 0.0f, 0.0f, 0.0f);
442 
443  //Culling
444  std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
445  std::vector<PointCloudDrawable*> cloudsToDraw(pointClouds_.size());
446  int oi=0;
447  for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
448  {
449  if(!mapRendering_ && iter->first > 0)
450  {
451  break;
452  }
453 
454  if(iter->second->isVisible())
455  {
456  if(intersectFrustumAABB(planes,
457  iter->second->aabbMinWorld(),
458  iter->second->aabbMaxWorld()))
459  {
460  cloudsToDraw[oi++] = iter->second;
461  }
462  }
463  }
464  cloudsToDraw.resize(oi);
465 
466  // First rendering to get depth texture
467  glEnable(GL_DEPTH_TEST);
468  glDepthFunc(GL_LESS);
469  glDepthMask(GL_TRUE);
470  glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
471  glDisable (GL_BLEND);
472  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
473 
474  if(backfaceCulling_)
475  {
476  glEnable(GL_CULL_FACE);
477  }
478  else
479  {
480  glDisable(GL_CULL_FACE);
481  }
482 
483  bool onlineBlending =
484  (!meshRendering_ &&
485  occlusionMesh.cloud.get() &&
486  occlusionMesh.cloud->size()) ||
487  (blending_ &&
490  (cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));
491 
492  if(onlineBlending && fboId_)
493  {
494  GLint originid = 0;
495  glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
496 
497  // set the rendering destination to FBO
498  glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
499 
500  glClearColor(0, 0, 0, 0);
501  glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
502 
503  // Draw scene
504  for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
505  {
506  Eigen::Vector3f cloudToCamera(
507  (*iter)->getPose().x() - openglCamera.x(),
508  (*iter)->getPose().y() - openglCamera.y(),
509  (*iter)->getPose().z() - openglCamera.z());
510  float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
511  (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
512  }
513 
514  if(!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
515  {
516  PointCloudDrawable drawable(occlusionMesh);
517  drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 0, 0, 0, 0, 0, 0, true);
518  }
519 
520  // back to normal window-system-provided framebuffer
521  glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
522  }
523 
525  {
526  glClearColor(0, 0, 0, 0);
527  glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
528 
529  // FIXME: we could use the depthTexture if already computed!
530  for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
531  {
532  Eigen::Vector3f cloudToCamera(
533  (*iter)->getPose().x() - openglCamera.x(),
534  (*iter)->getPose().y() - openglCamera.y(),
535  (*iter)->getPose().z() - openglCamera.z());
536  float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
537 
538  (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
539  }
540 
541  GLubyte zValue[4];
542  glReadPixels(doubleTapPos_.x*screenWidth_, screenHeight_-doubleTapPos_.y*screenHeight_, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, zValue);
543  float zValueF = float(zValue[0]/255.0f) + float(zValue[1]/255.0f)/255.0f + float(zValue[2]/255.0f)/65025.0f + float(zValue[3]/255.0f)/160581375.0f;
544 
545  if(zValueF != 0.0f)
546  {
547  zValueF = zValueF*2.0-1.0;//NDC
548  glm::vec4 point = glm::inverse(projectionMatrix*viewMatrix)*glm::vec4(doubleTapPos_.x*2.0f-1.0f, (1.0f-doubleTapPos_.y)*2.0f-1.0f, zValueF, 1.0f);
549  point /= point.w;
550  gesture_camera_->SetAnchorOffset(glm::vec3(point.x, point.y, point.z) - position);
551  }
552  }
553  doubleTapOn_ = false;
554 
555  glClearColor(r_, g_, b_, 1.0f);
556  glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
557 
558  if(renderBackgroundCamera && (!onlineBlending || !meshRendering_))
559  {
560  background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);
561 
562  //To debug occlusion image:
563  //PointCloudDrawable drawable(occlusionMesh);
564  //drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
565  }
566 
567  if(!currentPose_->isNull())
568  {
570  {
571  frustum_->SetPosition(position);
573  // Set the frustum scale to 4:3, this doesn't necessarily match the physical
574  // camera's aspect ratio, this is just for visualization purposes.
575  frustum_->SetScale(kFrustumScale);
576  frustum_->Render(projectionMatrix, viewMatrix);
577 
579  glm::vec3 positionCamera(cameraFrame.x(), cameraFrame.y(), cameraFrame.z());
580  Eigen::Quaternionf quatCamera = cameraFrame.getQuaternionf();
581  glm::quat rotationCamera(quatCamera.w(), quatCamera.x(), quatCamera.y(), quatCamera.z());
582 
583  axis_->SetPosition(positionCamera);
584  axis_->SetRotation(rotationCamera);
585  axis_->Render(projectionMatrix, viewMatrix);
586  }
587 
588  trace_->UpdateVertexArray(position);
589  if(traceVisible_)
590  {
591  trace_->Render(projectionMatrix, viewMatrix);
592  }
593  else
594  {
596  }
597  }
598 
599  if(gridVisible_ && !renderBackgroundCamera)
600  {
601  grid_->Render(projectionMatrix, viewMatrix);
602  }
603 
604  if(graphVisible_ && graph_)
605  {
606  graph_->Render(projectionMatrix, viewMatrix);
607  }
608 
609 
610  if(onlineBlending)
611  {
612  glEnable (GL_BLEND);
613  glDepthMask(GL_FALSE);
614  }
615 
616  for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
617  {
618  PointCloudDrawable * cloud = *iter;
619 
621  {
622  box_->updateVertices(cloud->aabbMinWorld(), cloud->aabbMaxWorld());
623  box_->Render(projectionMatrix, viewMatrix);
624  }
625 
626  Eigen::Vector3f cloudToCamera(
627  cloud->getPose().x() - openglCamera.x(),
628  cloud->getPose().y() - openglCamera.y(),
629  cloud->getPose().z() - openglCamera.z());
630  float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
631 
632  cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
633  }
634 
635  if(onlineBlending)
636  {
637  if(renderBackgroundCamera && meshRendering_)
638  {
640  }
641 
642  glDisable (GL_BLEND);
643  glDepthMask(GL_TRUE);
644  }
645 
646  //draw markers on foreground
647  for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
648  {
649  iter->second->Render(projectionMatrix, viewMatrix);
650  }
651 
652  return (int)cloudsToDraw.size();
653 }
654 
656  gesture_camera_->SetCameraType(camera_type);
657 }
658 
660 {
661  UASSERT(!pose.isNull());
662  if(currentPose_ ==0)
663  {
664  currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
665  }
666  *currentPose_ = pose;
667 }
668 
669 void Scene::setFOV(float angle)
670 {
672 }
673 void Scene::setOrthoCropFactor(float value)
674 {
676 }
677 void Scene::setGridRotation(float angleDeg)
678 {
679  float angleRad = angleDeg * DEGREE_2_RADIANS;
680  if(grid_)
681  {
682  glm::quat rot = glm::rotate(glm::quat(1,0,0,0), angleRad, glm::vec3(0, 1, 0));
683  grid_->SetRotation(rot);
684  }
685 }
686 
688 {
689  if(fov)
690  {
691  *fov = gesture_camera_->getFOV();
692  }
694 }
695 
696 void Scene::OnTouchEvent(int touch_count,
697  tango_gl::GestureCamera::TouchEvent event, float x0,
698  float y0, float x1, float y1) {
699  UASSERT(gesture_camera_ != 0);
700  if(touch_count == 3)
701  {
702  //doubletap
703  if(!doubleTapOn_)
704  {
705  doubleTapPos_.x = x0;
706  doubleTapPos_.y = y0;
707  doubleTapOn_ = true;
708  }
709  }
710  else
711  {
712  // rotate/translate/zoom
713  gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
714  }
715 }
716 
718  const std::map<int, rtabmap::Transform> & poses,
719  const std::multimap<int, rtabmap::Link> & links)
720 {
721  LOGI("updateGraph");
722  //create
724  delete graph_;
725  graph_ = new GraphDrawable(graph_shader_program_, poses, links);
726 }
727 
728 void Scene::setGraphVisible(bool visible)
729 {
730  graphVisible_ = visible;
731 }
732 
733 void Scene::setGridVisible(bool visible)
734 {
735  gridVisible_ = visible;
736 }
737 
738 void Scene::setTraceVisible(bool visible)
739 {
740  traceVisible_ = visible;
741 }
742 
743 void Scene::setFrustumVisible(bool visible)
744 {
745  frustumVisible_ = visible;
746 }
747 
748 //Should only be called in OpenGL thread!
750  int id,
751  const rtabmap::Transform & pose)
752 {
753  LOGI("add marker %d", id);
754  std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
755  if(iter == markers_.end())
756  {
757  //create
758  tango_gl::Axis * drawable = new tango_gl::Axis();
759  drawable->SetScale(glm::vec3(0.05f,0.05f,0.05f));
760  drawable->SetLineWidth(5);
761  markers_.insert(std::make_pair(id, drawable));
762  }
763  setMarkerPose(id, pose);
764 }
765 void Scene::setMarkerPose(int id, const rtabmap::Transform & pose)
766 {
767  UASSERT(!pose.isNull());
768  std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
769  if(iter != markers_.end())
770  {
771  glm::vec3 position(pose.x(), pose.y(), pose.z());
772  Eigen::Quaternionf quat = pose.getQuaternionf();
773  glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
774  iter->second->SetPosition(position);
775  iter->second->SetRotation(rotation);
776  }
777 }
778 bool Scene::hasMarker(int id) const
779 {
780  return markers_.find(id) != markers_.end();
781 }
783 {
784  std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
785  if(iter != markers_.end())
786  {
787  delete iter->second;
788  markers_.erase(iter);
789  }
790 }
791 std::set<int> Scene::getAddedMarkers() const
792 {
793  return uKeysSet(markers_);
794 }
795 
797  int id,
798  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
799  const pcl::IndicesPtr & indices,
800  const rtabmap::Transform & pose)
801 {
802  LOGI("add cloud %d (%d points %d indices)", id, (int)cloud->size(), indices.get()?(int)indices->size():0);
803  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
804  if(iter != pointClouds_.end())
805  {
806  delete iter->second;
807  pointClouds_.erase(iter);
808  }
809 
810  //create
811  PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
812  drawable->setPose(pose);
813  pointClouds_.insert(std::make_pair(id, drawable));
814 }
815 
817  int id,
818  const rtabmap::Mesh & mesh,
819  const rtabmap::Transform & pose,
820  bool createWireframe)
821 {
822  LOGI("add mesh %d", id);
823  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
824  if(iter != pointClouds_.end())
825  {
826  delete iter->second;
827  pointClouds_.erase(iter);
828  }
829 
830  //create
831  PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
832  drawable->setPose(pose);
833  pointClouds_.insert(std::make_pair(id, drawable));
834 
835  if(!mesh.pose.isNull() && mesh.cloud->size() && (!mesh.cloud->isOrganized() || mesh.indices->size()))
836  {
837  UTimer time;
838  float height = 0.0f;
839  Eigen::Affine3f affinePose = mesh.pose.toEigen3f();
840  if(mesh.polygons.size())
841  {
842  for(unsigned int i=0; i<mesh.polygons.size(); ++i)
843  {
844  for(unsigned int j=0; j<mesh.polygons[i].vertices.size(); ++j)
845  {
846  pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.polygons[i].vertices[j]), affinePose);
847  if(pt.z < height)
848  {
849  height = pt.z;
850  }
851  }
852  }
853  }
854  else
855  {
856  if(mesh.cloud->isOrganized())
857  {
858  for(unsigned int i=0; i<mesh.indices->size(); ++i)
859  {
860  pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.indices->at(i)), affinePose);
861  if(pt.z < height)
862  {
863  height = pt.z;
864  }
865  }
866  }
867  else
868  {
869  for(unsigned int i=0; i<mesh.cloud->size(); ++i)
870  {
871  pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(i), affinePose);
872  if(pt.z < height)
873  {
874  height = pt.z;
875  }
876  }
877  }
878  }
879 
880  if(grid_->GetPosition().y == kHeightOffset.y || grid_->GetPosition().y > height)
881  {
882  grid_->SetPosition(glm::vec3(0,height,0));
883  }
884  LOGD("compute min height %f s", time.ticks());
885  }
886 }
887 
888 
889 void Scene::setCloudPose(int id, const rtabmap::Transform & pose)
890 {
891  UASSERT(!pose.isNull());
892  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
893  if(iter != pointClouds_.end())
894  {
895  iter->second->setPose(pose);
896  }
897 }
898 
899 void Scene::setCloudVisible(int id, bool visible)
900 {
901  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
902  if(iter != pointClouds_.end())
903  {
904  iter->second->setVisible(visible);
905  }
906 }
907 
908 bool Scene::hasCloud(int id) const
909 {
910  return pointClouds_.find(id) != pointClouds_.end();
911 }
912 
913 bool Scene::hasMesh(int id) const
914 {
915  return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasMesh();
916 }
917 
918 bool Scene::hasTexture(int id) const
919 {
920  return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasTexture();
921 }
922 
923 std::set<int> Scene::getAddedClouds() const
924 {
925  return uKeysSet(pointClouds_);
926 }
927 
928 void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons)
929 {
930  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
931  if(iter != pointClouds_.end())
932  {
933  iter->second->updatePolygons(polygons);
934  }
935 }
936 
937 void Scene::updateMesh(int id, const rtabmap::Mesh & mesh)
938 {
939  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
940  if(iter != pointClouds_.end())
941  {
942  iter->second->updateMesh(mesh);
943  }
944 }
945 
946 void Scene::updateGains(int id, float gainR, float gainG, float gainB)
947 {
948  std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
949  if(iter != pointClouds_.end())
950  {
951  iter->second->setGains(gainR, gainG, gainB);
952  }
953 }
954 
955 void Scene::setGridColor(float r, float g, float b)
956 {
957  if(grid_)
958  {
959  grid_->SetColor(r, g, b);
960  }
961 }
float b_
Definition: scene.h:208
d
void Render(const glm::mat4 &projectionMatrix, const glm::mat4 &viewMatrix)
#define NULL
highp_vec4 vec4
Definition: type_vec.hpp:397
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
static void releaseShaderPrograms()
const glm::vec3 kHeightOffset
Definition: scene.cpp:39
Definition: UTimer.h:46
void addMesh(int id, const rtabmap::Mesh &mesh, const rtabmap::Transform &pose, bool createWireframe=false)
Definition: scene.cpp:816
float g_
Definition: scene.h:207
void setGridVisible(bool visible)
Definition: scene.cpp:733
highp_vec3 vec3
Definition: type_vec.hpp:392
void SetRotation(const glm::quat &rotation)
Definition: transform.cpp:43
BoundingBoxDrawable * box_
Definition: scene.h:176
bool gridVisible_
Definition: scene.h:182
0 degree rotation (natural orientation)
Definition: util.h:199
pcl::IndicesPtr indices
Definition: util.h:179
rtabmap::Transform glmToTransform(const glm::mat4 &mat)
Definition: util.h:144
void updateCloudPolygons(int id, const std::vector< pcl::Vertices > &polygons)
Definition: scene.cpp:928
void OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event, float x0, float y0, float x1, float y1)
Definition: scene.cpp:696
void SetLineWidth(const float pixels)
Definition: line.cpp:24
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
bool wireFrame_
Definition: scene.h:205
void SetupViewPort(int w, int h)
Definition: scene.cpp:219
bool hasMarker(int id) const
Definition: scene.cpp:778
std::set< int > getAddedMarkers() const
Definition: scene.cpp:791
CameraType GetCameraType() const
void setCloudPose(int id, const rtabmap::Transform &pose)
Definition: scene.cpp:889
f
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud
Definition: util.h:177
void SetCameraType(CameraType camera_index)
x
void Render(const glm::mat4 &projection_mat, const glm::mat4 &view_mat) const
Definition: axis.cpp:53
void setFOV(float angle)
Definition: scene.cpp:669
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
std::map< int, PointCloudDrawable * > pointClouds_
Definition: scene.h:190
bool backfaceCulling_
Definition: scene.h:204
void Render(const glm::mat4 &projection_mat, const glm::mat4 &view_mat) const
Definition: line.cpp:25
void setPose(const rtabmap::Transform &pose)
bool hasTexture(int id) const
Definition: scene.cpp:918
void setGridColor(float r, float g, float b)
Definition: scene.cpp:955
const glm::vec3 kFrustumScale
Definition: scene.cpp:48
float getNearClipPlane() const
Definition: camera.h:39
GLuint fboId_
Definition: scene.h:209
int Render(const float *uvsTransformed=0, glm::mat4 arViewMatrix=glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh &occlusionMesh=rtabmap::Mesh(), bool mapping=false)
Definition: scene.cpp:385
highp_quat quat
Quaternion of default single-precision floating-point numbers.
Definition: fwd.hpp:68
std::set< int > getAddedClouds() const
Definition: scene.cpp:923
std::set< K > uKeysSet(const std::map< K, V > &m)
Definition: UStl.h:186
void Draw(const float *transformed_uvs, const GLuint &depthTexture, int screenWidth, int screenHeight, bool redUnknown)
void SetWindowSize(const float width, const float height)
Definition: camera.cpp:46
float r_
Definition: scene.h:206
~Scene()
Definition: scene.cpp:111
GLsizei screenWidth_
Definition: scene.h:212
bool graphVisible_
Definition: scene.h:181
void clear()
Definition: scene.cpp:190
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
glm::mat4 GetTransformationMatrix() const
Definition: transform.cpp:67
BackgroundRenderer * background_renderer_
Definition: scene.h:160
GLuint CreateProgram(const char *vertex_source, const char *fragment_source)
Definition: util.cpp:75
#define LOGE(...)
GLsizei screenHeight_
Definition: scene.h:213
bool meshRenderingTexture_
Definition: scene.h:200
std::vector< pcl::Vertices > polygons
Definition: util.h:180
bool frustumVisible_
Definition: scene.h:184
tango_gl::Frustum * frustum_
Definition: scene.h:170
void removeMarker(int id)
Definition: scene.cpp:782
unsigned int GLuint
Definition: dummy.cpp:78
glm::vec3 GetPosition() const
Definition: transform.cpp:39
const std::string kGraphVertexShader
Definition: scene.cpp:50
void DeleteResources()
Definition: scene.cpp:154
void setOrthoCropFactor(float value)
Definition: scene.cpp:673
bool lighting_
Definition: scene.h:203
#define UASSERT(condition)
Wrappers of STL for convenient functions.
void updateGains(int id, float gainR, float gainG, float gainB)
Definition: scene.cpp:946
GLM_FUNC_DECL genType normalize(genType const &x)
std::map< int, tango_gl::Axis * > markers_
Definition: scene.h:186
void InitGLContent()
Definition: scene.cpp:118
#define LOGD(...)
void SetFieldOfView(const float fov)
Definition: camera.cpp:52
static const rtabmap::Transform optical_T_opengl(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f)
#define true
Definition: ConvertUTF.c:57
#define GL_FALSE
Definition: dummy.cpp:79
const pcl::PointXYZ & aabbMinWorld() const
Scene()
Definition: scene.cpp:71
GraphDrawable * graph_
Definition: scene.h:180
void SetCameraPose(const rtabmap::Transform &pose)
Definition: scene.cpp:659
GLuint depthTexture_
Definition: scene.h:211
bool mapRendering_
Definition: scene.h:198
const std::string kGraphFragmentShader
Definition: scene.cpp:61
void addCloud(int id, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const rtabmap::Transform &pose)
Definition: scene.cpp:796
bool blending_
Definition: scene.h:197
const pcl::PointXYZ & aabbMaxWorld() const
bool doubleTapOn_
Definition: scene.h:214
void setCloudVisible(int id, bool visible)
Definition: scene.cpp:899
std::vector< glm::vec4 > computeFrustumPlanes(const glm::mat4 &mat, bool normalize=true)
Definition: scene.cpp:276
void updateGraph(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links)
Definition: scene.cpp:717
void SetColor(const Color &color)
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:381
bool hasCloud(int id) const
Definition: scene.cpp:908
cv::Point2f doubleTapPos_
Definition: scene.h:215
void OnTouchEvent(int touch_count, TouchEvent event, float x0, float y0, float x1, float y1)
tango_gl::Trace * trace_
Definition: scene.h:179
rtabmap::ScreenRotation color_camera_to_display_rotation_
Definition: scene.h:188
void addMarker(int id, const rtabmap::Transform &pose)
Definition: scene.cpp:749
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
#define LOGI(...)
bool isNull() const
Definition: Transform.cpp:107
void setFrustumVisible(bool visible)
Definition: scene.cpp:743
#define false
Definition: ConvertUTF.c:56
float getFarClipPlane() const
Definition: camera.h:40
rtabmap::Transform getPose() const
tango_gl::Grid * grid_
Definition: scene.h:173
Eigen::Quaternionf getQuaternionf() const
Definition: Transform.cpp:391
void SetAnchorPosition(const glm::vec3 &pos, const glm::quat &rotation)
void SetOrthoCropFactor(float value)
Definition: camera.h:34
bool intersectFrustumAABB(const std::vector< glm::vec4 > &planes, const pcl::PointXYZ &boxMin, const pcl::PointXYZ &boxMax)
Definition: scene.cpp:349
tango_gl::Axis * axis_
Definition: scene.h:167
void UpdateVertexArray(const glm::vec3 &v)
Definition: trace.cpp:26
void setTraceVisible(bool visible)
Definition: scene.cpp:738
void SetCameraType(tango_gl::GestureCamera::CameraType camera_type)
Definition: scene.cpp:655
void SetAnchorOffset(const glm::vec3 &pos)
ULogger class and convenient macros.
#define DEGREE_2_RADIANS
bool meshRendering_
Definition: scene.h:199
double ticks()
Definition: UTimer.cpp:117
void updateMesh(int id, const rtabmap::Mesh &mesh)
Definition: scene.cpp:937
void ClearVertexArray()
Definition: trace.cpp:37
void setGraphVisible(bool visible)
Definition: scene.cpp:728
bool hasMesh(int id) const
Definition: scene.cpp:913
void setMarkerPose(int id, const rtabmap::Transform &pose)
Definition: scene.cpp:765
static void createShaderPrograms()
static const rtabmap::Transform opticalRotationInv
Definition: CameraMobile.h:77
bool boundingBoxRendering_
Definition: scene.h:202
void setGridRotation(float angleDeg)
Definition: scene.cpp:677
void SetPosition(const glm::vec3 &position)
Definition: transform.cpp:35
void updateVertices(const pcl::PointXYZ &min, const pcl::PointXYZ &max)
glm::mat4 GetProjectionMatrix()
Definition: camera.cpp:38
tango_gl::GestureCamera * gesture_camera_
Definition: scene.h:164
void SetScale(const glm::vec3 &scale)
Definition: transform.cpp:51
rtabmap::Transform GetOpenGLCameraPose(float *fov=0) const
Definition: scene.cpp:687
const tango_gl::Color kGridColor(0.85f, 0.85f, 0.85f)
GLuint rboId_
Definition: scene.h:210
float pointSize_
Definition: scene.h:201
bool traceVisible_
Definition: scene.h:183
const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f)
rtabmap::Transform * currentPose_
Definition: scene.h:192
GLM_FUNC_DECL detail::tmat4x4< T, P > rotate(detail::tmat4x4< T, P > const &m, T const &angle, detail::tvec3< T, P > const &axis)
glm::mat4 GetViewMatrix()
Definition: camera.cpp:34
rtabmap::Transform pose
Definition: util.h:182
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
#define UINFO(...)
GLuint graph_shader_program_
Definition: scene.h:195


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:30