CloudViewer.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 "rtabmap/gui/CloudViewer.h"
29 #include "rtabmap/gui/CloudViewerCellPicker.h"
30 
31 #include <rtabmap/core/Version.h>
32 #include <rtabmap/core/util3d_transforms.h>
33 #include <rtabmap/utilite/ULogger.h>
34 #include <rtabmap/utilite/UTimer.h>
35 #include <rtabmap/utilite/UMath.h>
36 #include <rtabmap/utilite/UConversion.h>
37 #include <rtabmap/utilite/UStl.h>
38 #include <rtabmap/core/util2d.h>
39 #include <pcl/visualization/pcl_visualizer.h>
40 #include <pcl/common/transforms.h>
41 #include <QMenu>
42 #include <QAction>
43 #include <QActionGroup>
44 #include <QtGui/QContextMenuEvent>
45 #include <QInputDialog>
46 #include <QtGui/QWheelEvent>
47 #include <QtGui/QKeyEvent>
48 #include <QColorDialog>
49 #include <QtGui/QVector3D>
50 #include <QMainWindow>
51 #include <set>
52 
53 #include <vtkCamera.h>
54 #include <vtkRenderWindow.h>
55 #include <vtkCubeSource.h>
56 #include <vtkDataSetMapper.h>
57 #include <vtkDelaunay2D.h>
58 #include <vtkGlyph3D.h>
59 #include <vtkGlyph3DMapper.h>
60 #include <vtkSmartVolumeMapper.h>
61 #include <vtkVolumeProperty.h>
62 #include <vtkColorTransferFunction.h>
63 #include <vtkPiecewiseFunction.h>
64 #include <vtkImageData.h>
65 #include <vtkLookupTable.h>
66 #include <vtkTextureUnitManager.h>
67 #include <vtkJPEGReader.h>
68 #include <vtkBMPReader.h>
69 #include <vtkPNMReader.h>
70 #include <vtkPNGReader.h>
71 #include <vtkTIFFReader.h>
72 #include <vtkElevationFilter.h>
73 #include <vtkOpenGLRenderWindow.h>
74 #include <vtkPointPicker.h>
75 #include <vtkPointData.h>
76 #include <vtkTextActor.h>
77 #include <vtkTexture.h>
78 #include <vtkNamedColors.h>
79 #include <vtkOBBTree.h>
80 #include <vtkObjectFactory.h>
81 #include <vtkQuad.h>
82 #include <vtkWarpScalar.h>
83 #include <opencv/vtkImageMatSource.h>
84 
85 #if VTK_MAJOR_VERSION >= 7
86 #include <vtkEDLShading.h>
87 #include <vtkRenderStepsPass.h>
88 #include <vtkOpenGLRenderer.h>
89 #endif
90 
91 #if VTK_MAJOR_VERSION >= 8
92 #include <vtkGenericOpenGLRenderWindow.h>
93 #endif
94 
95 #ifdef RTABMAP_OCTOMAP
96 #include <rtabmap/core/global_map/OctoMap.h>
97 #endif
98 
99 namespace rtabmap {
100 
101 CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
102  PCLQVTKWidget(parent),
103  _aLockCamera(0),
104  _aFollowCamera(0),
105  _aResetCamera(0),
106  _aLockViewZ(0),
107  _aCameraOrtho(0),
108  _aShowTrajectory(0),
109  _aSetTrajectorySize(0),
110  _aClearTrajectory(0),
111  _aShowCameraAxis(0),
112  _aShowFrustum(0),
113  _aSetFrustumScale(0),
114  _aSetFrustumColor(0),
115  _aShowGrid(0),
116  _aSetGridCellCount(0),
117  _aSetGridCellSize(0),
118  _aShowNormals(0),
119  _aSetNormalsStep(0),
120  _aSetNormalsScale(0),
121  _aSetBackgroundColor(0),
122  _aSetRenderingRate(0),
123  _aSetEDLShading(0),
124  _aSetLighting(0),
125  _aSetFlatShading(0),
126  _aSetEdgeVisibility(0),
127  _aBackfaceCulling(0),
128  _menu(0),
129  _trajectory(new pcl::PointCloud<pcl::PointXYZ>),
130  _maxTrajectorySize(100),
131  _frustumScale(0.5f),
132  _frustumColor(Qt::gray),
133  _gridCellCount(50),
134  _gridCellSize(1),
135  _normalsStep(1),
136  _normalsScale(0.2),
137  _buildLocator(false),
138  _lastCameraOrientation(0,0,0),
139  _lastCameraPose(0,0,0),
140  _defaultBgColor(Qt::black),
141  _currentBgColor(Qt::black),
142  _frontfaceCulling(false),
143  _renderingRate(5.0),
144  _octomapActor(0),
145  _intensityAbsMax(100.0f),
146  _coordinateFrameScale(1.0)
147 {
148  this->setMinimumSize(200, 200);
149 
150  int argc = 0;
151  UASSERT(style!=0);
152  style->setCloudViewer(this);
153  style->AutoAdjustCameraClippingRangeOff();
154 #if VTK_MAJOR_VERSION > 8
155  auto renderer1 = vtkSmartPointer<vtkRenderer>::New();
156  auto renderWindow1 = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
157  renderWindow1->AddRenderer(renderer1);
158  _visualizer = new pcl::visualization::PCLVisualizer(
159  argc,
160  0,
161  renderer1,
162  renderWindow1,
163  "PCLVisualizer",
164  style,
165  false);
166 #else
167  _visualizer = new pcl::visualization::PCLVisualizer(
168  argc,
169  0,
170  "PCLVisualizer",
171  style,
172  false);
173 #endif
174 
175  _visualizer->setShowFPS(false);
176 
177  int viewport;
178  // Layer 0: unavailable layer, used as "all" by PCLVisualizer
179  _visualizer->createViewPort (0,0,1.0, 1.0, viewport); // Layer 1: all clouds here
180  _visualizer->createViewPort (0,0,1.0, 1.0, viewport); // Layer 2: all 3d objects here
181  _visualizer->createViewPort (0,0,1.0, 1.0, viewport); // Layer 3: text overlay
182  _visualizer->getRendererCollection()->InitTraversal ();
183  vtkRenderer* renderer = NULL;
184  int i =0;
185  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
186  {
187  renderer->SetLayer(i);
188  if(i==1)
189  {
190 #if VTK_MAJOR_VERSION >= 7
191  renderer->PreserveColorBufferOff();
192 #endif
193  renderer->PreserveDepthBufferOff();
194  _visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
195  }
196  else if(i==2)
197  {
198 #if VTK_MAJOR_VERSION >= 7
199  renderer->PreserveColorBufferOn();
200 #endif
201  renderer->PreserveDepthBufferOn();
202  }
203  ++i;
204  }
205  _visualizer->getRenderWindow()->SetNumberOfLayers(4);
206 
207 #ifdef VTK_GLOBAL_WARNING_DISPLAY_OFF
208  _visualizer->getRenderWindow()->GlobalWarningDisplayOff();
209 #endif
210 
211 #if VTK_MAJOR_VERSION > 8
212  this->setRenderWindow(_visualizer->getRenderWindow());
213 #else
214  this->SetRenderWindow(_visualizer->getRenderWindow());
215 #endif
216 
217  // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as
218  // the "Invalid drawable" warning when the view is not visible.
219 #if VTK_MAJOR_VERSION > 8
220  //_visualizer->setupInteractor(this->interactor(), this->renderWindow());
221  this->interactor()->SetInteractorStyle (_visualizer->getInteractorStyle());
222 #else
223  //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
224  this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
225 #endif
226  // setup a simple point picker
227  vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
228  UDEBUG("pick tolerance=%f", pp->GetTolerance());
229  pp->SetTolerance (pp->GetTolerance()/2.0);
230 #if VTK_MAJOR_VERSION > 8
231  this->interactor()->SetPicker (pp);
232 #else
233  this->GetInteractor()->SetPicker (pp);
234 #endif
235 
236  setRenderingRate(_renderingRate);
237 
238  this->setCameraPosition(
239  -1, 0, 0,
240  0, 0, 0,
241  0, 0, 1);
242 #ifndef _WIN32
243  // Crash on startup on Windows (vtk issue)
244  this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
245 #endif
246 
247  //setup menu/actions
248  createMenu();
249 
250  setMouseTracking(false);
251 }
252 
253 CloudViewer::~CloudViewer()
254 {
255  UDEBUG("");
256  this->clear();
257  delete _visualizer;
258  UDEBUG("");
259 }
260 
261 void CloudViewer::clear()
262 {
263  this->removeAllClouds();
264  this->removeAllGraphs();
265  this->removeAllCoordinates();
266  this->removeAllLines();
267  this->removeAllFrustums();
268  this->removeAllTexts();
269  this->removeOccupancyGridMap();
270  this->removeOctomap();
271  this->removeElevationMap();
272 
273  if(_aShowCameraAxis->isChecked())
274  {
275  this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
276  }
277  _lastPose.setNull();
278  if(_aLockCamera->isChecked() || _aFollowCamera->isChecked())
279  {
280  resetCamera();
281  }
282  this->clearTrajectory();
283 }
284 
285 void CloudViewer::createMenu()
286 {
287  _aLockCamera = new QAction("Lock target", this);
288  _aLockCamera->setCheckable(true);
289  _aLockCamera->setChecked(false);
290  _aFollowCamera = new QAction("Follow", this);
291  _aFollowCamera->setCheckable(true);
292  _aFollowCamera->setChecked(true);
293  QAction * freeCamera = new QAction("Free", this);
294  freeCamera->setCheckable(true);
295  freeCamera->setChecked(false);
296  _aLockViewZ = new QAction("Lock view Z", this);
297  _aLockViewZ->setCheckable(true);
298  _aLockViewZ->setChecked(true);
299  _aCameraOrtho = new QAction("Ortho mode", this);
300  _aCameraOrtho->setCheckable(true);
301  _aCameraOrtho->setChecked(false);
302  _aResetCamera = new QAction("Reset position", this);
303  _aShowTrajectory= new QAction("Show trajectory", this);
304  _aShowTrajectory->setCheckable(true);
305  _aShowTrajectory->setChecked(true);
306  _aSetTrajectorySize = new QAction("Set trajectory size...", this);
307  _aClearTrajectory = new QAction("Clear trajectory", this);
308  _aShowCameraAxis= new QAction("Show base frame", this);
309  _aShowCameraAxis->setCheckable(true);
310  _aShowCameraAxis->setChecked(true);
311  _aSetFrameScale= new QAction("Set frame scale...", this);
312  _aShowCameraAxis->setChecked(true);
313  _aShowFrustum= new QAction("Show frustum", this);
314  _aShowFrustum->setCheckable(true);
315  _aShowFrustum->setChecked(false);
316  _aSetFrustumScale = new QAction("Set frustum scale...", this);
317  _aSetFrustumColor = new QAction("Set frustum color...", this);
318  _aShowGrid = new QAction("Show grid", this);
319  _aShowGrid->setCheckable(true);
320  _aSetGridCellCount = new QAction("Set cell count...", this);
321  _aSetGridCellSize = new QAction("Set cell size...", this);
322  _aShowNormals = new QAction("Show normals", this);
323  _aShowNormals->setCheckable(true);
324  _aSetNormalsStep = new QAction("Set normals step...", this);
325  _aSetNormalsScale = new QAction("Set normals scale...", this);
326  _aSetIntensityRedColormap = new QAction("Red/Yellow Colormap", this);
327  _aSetIntensityRedColormap->setCheckable(true);
328  _aSetIntensityRedColormap->setChecked(true);
329  _aSetIntensityRainbowColormap = new QAction("Rainbow Colormap", this);
330  _aSetIntensityRainbowColormap->setCheckable(true);
331  _aSetIntensityRainbowColormap->setChecked(false);
332  _aSetIntensityMaximum = new QAction("Set maximum absolute intensity...", this);
333  _aSetBackgroundColor = new QAction("Set background color...", this);
334  _aSetRenderingRate = new QAction("Set rendering rate...", this);
335  _aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
336  _aSetEDLShading->setCheckable(true);
337  _aSetEDLShading->setChecked(false);
338 #if VTK_MAJOR_VERSION < 7
339  _aSetEDLShading->setEnabled(false);
340 #endif
341  _aSetLighting = new QAction("Lighting", this);
342  _aSetLighting->setCheckable(true);
343  _aSetLighting->setChecked(false);
344  _aSetFlatShading = new QAction("Flat Shading", this);
345  _aSetFlatShading->setCheckable(true);
346  _aSetFlatShading->setChecked(false);
347  _aSetEdgeVisibility = new QAction("Show edges", this);
348  _aSetEdgeVisibility->setCheckable(true);
349  _aSetEdgeVisibility->setChecked(false);
350  _aBackfaceCulling = new QAction("Backface culling", this);
351  _aBackfaceCulling->setCheckable(true);
352  _aBackfaceCulling->setChecked(true);
353  _aPolygonPicking = new QAction("Polygon picking", this);
354  _aPolygonPicking->setCheckable(true);
355  _aPolygonPicking->setChecked(false);
356 
357  QMenu * cameraMenu = new QMenu("Camera", this);
358  cameraMenu->addAction(_aLockCamera);
359  cameraMenu->addAction(_aFollowCamera);
360  cameraMenu->addAction(freeCamera);
361  cameraMenu->addSeparator();
362  cameraMenu->addAction(_aLockViewZ);
363  cameraMenu->addAction(_aCameraOrtho);
364  cameraMenu->addAction(_aResetCamera);
365  QActionGroup * group = new QActionGroup(this);
366  group->addAction(_aLockCamera);
367  group->addAction(_aFollowCamera);
368  group->addAction(freeCamera);
369 
370  QMenu * trajectoryMenu = new QMenu("Trajectory", this);
371  trajectoryMenu->addAction(_aShowTrajectory);
372  trajectoryMenu->addAction(_aSetTrajectorySize);
373  trajectoryMenu->addAction(_aClearTrajectory);
374 
375  QMenu * frustumMenu = new QMenu("Frustum", this);
376  frustumMenu->addAction(_aShowFrustum);
377  frustumMenu->addAction(_aSetFrustumScale);
378  frustumMenu->addAction(_aSetFrustumColor);
379 
380  QMenu * gridMenu = new QMenu("Grid", this);
381  gridMenu->addAction(_aShowGrid);
382  gridMenu->addAction(_aSetGridCellCount);
383  gridMenu->addAction(_aSetGridCellSize);
384 
385  QMenu * normalsMenu = new QMenu("Normals", this);
386  normalsMenu->addAction(_aShowNormals);
387  normalsMenu->addAction(_aSetNormalsStep);
388  normalsMenu->addAction(_aSetNormalsScale);
389 
390  QMenu * scanMenu = new QMenu("Scan color", this);
391  scanMenu->addAction(_aSetIntensityRedColormap);
392  scanMenu->addAction(_aSetIntensityRainbowColormap);
393  scanMenu->addAction(_aSetIntensityMaximum);
394 
395  //menus
396  _menu = new QMenu(this);
397  _menu->addMenu(cameraMenu);
398  _menu->addMenu(trajectoryMenu);
399  _menu->addAction(_aShowCameraAxis);
400  _menu->addAction(_aSetFrameScale);
401  _menu->addMenu(frustumMenu);
402  _menu->addMenu(gridMenu);
403  _menu->addMenu(normalsMenu);
404  _menu->addMenu(scanMenu);
405  _menu->addAction(_aSetBackgroundColor);
406  _menu->addAction(_aSetRenderingRate);
407  _menu->addAction(_aSetEDLShading);
408  _menu->addAction(_aSetLighting);
409  _menu->addAction(_aSetFlatShading);
410  _menu->addAction(_aSetEdgeVisibility);
411  _menu->addAction(_aBackfaceCulling);
412  _menu->addAction(_aPolygonPicking);
413 }
414 
415 void CloudViewer::saveSettings(QSettings & settings, const QString & group) const
416 {
417  if(!group.isEmpty())
418  {
419  settings.beginGroup(group);
420  }
421 
422  float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
423  this->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
424  QVector3D pose(poseX, poseY, poseZ);
425  QVector3D focal(focalX, focalY, focalZ);
426  if(!this->isCameraFree())
427  {
428  // make camera position relative to target
429  Transform T = this->getTargetPose();
430  if(this->isCameraTargetLocked())
431  {
432  T = Transform(T.x(), T.y(), T.z(), 0,0,0);
433  }
434  Transform F(focalX, focalY, focalZ, 0,0,0);
435  Transform P(poseX, poseY, poseZ, 0,0,0);
436  Transform newFocal = T.inverse() * F;
437  Transform newPose = newFocal * F.inverse() * P;
438  pose = QVector3D(newPose.x(), newPose.y(), newPose.z());
439  focal = QVector3D(newFocal.x(), newFocal.y(), newFocal.z());
440  }
441  settings.setValue("camera_pose", pose);
442  settings.setValue("camera_focal", focal);
443  settings.setValue("camera_up", QVector3D(upX, upY, upZ));
444 
445  settings.setValue("grid", this->isGridShown());
446  settings.setValue("grid_cell_count", this->getGridCellCount());
447  settings.setValue("grid_cell_size", (double)this->getGridCellSize());
448 
449  settings.setValue("normals", this->isNormalsShown());
450  settings.setValue("normals_step", this->getNormalsStep());
451  settings.setValue("normals_scale", (double)this->getNormalsScale());
452 
453  settings.setValue("intensity_red_colormap", this->isIntensityRedColormap());
454  settings.setValue("intensity_rainbow_colormap", this->isIntensityRainbowColormap());
455  settings.setValue("intensity_max", (double)this->getIntensityMax());
456 
457  settings.setValue("trajectory_shown", this->isTrajectoryShown());
458  settings.setValue("trajectory_size", this->getTrajectorySize());
459 
460  settings.setValue("camera_axis_shown", this->isCameraAxisShown());
461  settings.setValue("coordinate_frame_scale", this->getCoordinateFrameScale());
462 
463  settings.setValue("frustum_shown", this->isFrustumShown());
464  settings.setValue("frustum_scale", this->getFrustumScale());
465  settings.setValue("frustum_color", this->getFrustumColor());
466 
467  settings.setValue("camera_target_locked", this->isCameraTargetLocked());
468  settings.setValue("camera_target_follow", this->isCameraTargetFollow());
469  settings.setValue("camera_free", this->isCameraFree());
470  settings.setValue("camera_lockZ", this->isCameraLockZ());
471 
472  settings.setValue("bg_color", this->getDefaultBackgroundColor());
473  settings.setValue("rendering_rate", this->getRenderingRate());
474  if(!group.isEmpty())
475  {
476  settings.endGroup();
477  }
478 }
479 
480 void CloudViewer::loadSettings(QSettings & settings, const QString & group)
481 {
482  if(!group.isEmpty())
483  {
484  settings.beginGroup(group);
485  }
486 
487  float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
488  this->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
489  QVector3D pose(poseX, poseY, poseZ), focal(focalX, focalY, focalZ), up(upX, upY, upZ);
490  pose = settings.value("camera_pose", pose).value<QVector3D>();
491  focal = settings.value("camera_focal", focal).value<QVector3D>();
492  up = settings.value("camera_up", up).value<QVector3D>();
493  _lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
494  this->setCameraPosition(pose.x(),pose.y(),pose.z(), focal.x(),focal.y(),focal.z(), up.x(),up.y(),up.z());
495 
496  this->setGridShown(settings.value("grid", this->isGridShown()).toBool());
497  this->setGridCellCount(settings.value("grid_cell_count", this->getGridCellCount()).toUInt());
498  this->setGridCellSize(settings.value("grid_cell_size", this->getGridCellSize()).toFloat());
499 
500  this->setNormalsShown(settings.value("normals", this->isNormalsShown()).toBool());
501  this->setNormalsStep(settings.value("normals_step", this->getNormalsStep()).toInt());
502  this->setNormalsScale(settings.value("normals_scale", this->getNormalsScale()).toFloat());
503 
504  this->setIntensityRedColormap(settings.value("intensity_red_colormap", this->isIntensityRedColormap()).toBool());
505  this->setIntensityRainbowColormap(settings.value("intensity_rainbow_colormap", this->isIntensityRainbowColormap()).toBool());
506  this->setIntensityMax(settings.value("intensity_max", this->getIntensityMax()).toFloat());
507 
508  this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
509  this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
510 
511  this->setCameraAxisShown(settings.value("camera_axis_shown", this->isCameraAxisShown()).toBool());
512  this->setCoordinateFrameScale(settings.value("coordinate_frame_scale", this->getCoordinateFrameScale()).toDouble());
513 
514  this->setFrustumShown(settings.value("frustum_shown", this->isFrustumShown()).toBool());
515  this->setFrustumScale(settings.value("frustum_scale", this->getFrustumScale()).toDouble());
516  this->setFrustumColor(settings.value("frustum_color", this->getFrustumColor()).value<QColor>());
517 
518  this->setCameraTargetLocked(settings.value("camera_target_locked", this->isCameraTargetLocked()).toBool());
519  this->setCameraTargetFollow(settings.value("camera_target_follow", this->isCameraTargetFollow()).toBool());
520  if(settings.value("camera_free", this->isCameraFree()).toBool())
521  {
522  this->setCameraFree();
523  }
524  this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());
525 
526  this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
527 
528  this->setRenderingRate(settings.value("rendering_rate", this->getRenderingRate()).toDouble());
529 
530  if(!group.isEmpty())
531  {
532  settings.endGroup();
533  }
534 
535  this->refreshView();
536 }
537 
538 void CloudViewer::refreshView()
539 {
540 #if VTK_MAJOR_VERSION > 8
541  this->renderWindow()->Render();
542 #else
543  this->update();
544 #endif
545 }
546 
547 bool CloudViewer::updateCloudPose(
548  const std::string & id,
549  const Transform & pose)
550 {
551  if(_addedClouds.contains(id))
552  {
553  //UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
554  bool samePose = _addedClouds.find(id).value() == pose;
555  Eigen::Affine3f posef = pose.toEigen3f();
556  if(!samePose)
557  {
558  // PointCloud / Mesh
559  bool updated = _visualizer->updatePointCloudPose(id, posef);
560 
561 #if VTK_MAJOR_VERSION >= 7
562  if(!updated)
563  {
564  // TextureMesh, cannot use updateShapePose because it searches for vtkLODActor, not a vtkActor
565  pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
566  vtkActor* actor;
567  if (am_it != _visualizer->getShapeActorMap()->end ())
568  {
569  actor = vtkActor::SafeDownCast (am_it->second);
570  if (actor)
571  {
572  vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
573  pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
574  actor->SetUserMatrix (matrix);
575  actor->Modified ();
576  updated = true;
577  }
578  }
579  }
580 #endif
581 
582  if(updated)
583  {
584  _addedClouds.find(id).value() = pose;
585  std::string idNormals = id+"-normals";
586  if(_addedClouds.find(idNormals)!=_addedClouds.end())
587  {
588  _visualizer->updatePointCloudPose(idNormals, posef);
589  _addedClouds.find(idNormals).value() = pose;
590  }
591  return true;
592  }
593  }
594  }
595  return false;
596 }
597 
598 class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
599 {
600  typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
601  typedef PointCloud::Ptr PointCloudPtr;
602  typedef PointCloud::ConstPtr PointCloudConstPtr;
603 
604 public:
605  typedef boost::shared_ptr<PointCloudColorHandlerIntensityField > Ptr;
606  typedef boost::shared_ptr<const PointCloudColorHandlerIntensityField > ConstPtr;
607 
609  PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) :
610  pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud),
611  maxAbsIntensity_(maxAbsIntensity),
612  colormap_(colorMap)
613  {
614  field_idx_ = pcl::getFieldIndex (*cloud, "intensity");
615  if (field_idx_ != -1)
616  capable_ = true;
617  else
618  capable_ = false;
619  }
620 
622  virtual ~PointCloudColorHandlerIntensityField () {}
623 
629 #if PCL_VERSION_COMPARE(>, 1, 11, 1)
630  virtual vtkSmartPointer<vtkDataArray> getColor () const {
631  vtkSmartPointer<vtkDataArray> scalars;
632  if (!capable_ || !cloud_)
633  return scalars;
634 #else
635  virtual bool getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
636  if (!capable_ || !cloud_)
637  return (false);
638 #endif
639  if (!scalars)
640  scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
641  scalars->SetNumberOfComponents (3);
642 
643  vtkIdType nr_points = cloud_->width * cloud_->height;
644  // Allocate enough memory to hold all colors
645  float * intensities = new float[nr_points];
646  float intensity;
647  size_t point_offset = cloud_->fields[field_idx_].offset;
648  size_t j = 0;
649 
650  // If XYZ present, check if the points are invalid
651  int x_idx = pcl::getFieldIndex (*cloud_, "x");
652  if (x_idx != -1)
653  {
654  float x_data, y_data, z_data;
655  size_t x_point_offset = cloud_->fields[x_idx].offset;
656 
657  // Color every point
658  for (vtkIdType cp = 0; cp < nr_points; ++cp,
659  point_offset += cloud_->point_step,
660  x_point_offset += cloud_->point_step)
661  {
662  // Copy the value at the specified field
663  memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
664 
665  memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
666  memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float));
667  memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float));
668 
669  if (!std::isfinite (x_data) || !std::isfinite (y_data) || !std::isfinite (z_data))
670  continue;
671 
672  intensities[j++] = intensity;
673  }
674  }
675  // No XYZ data checks
676  else
677  {
678  // Color every point
679  for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
680  {
681  // Copy the value at the specified field
682  memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
683 
684  intensities[j++] = intensity;
685  }
686  }
687  if (j != 0)
688  {
689  // Allocate enough memory to hold all colors
690  unsigned char* colors = new unsigned char[j * 3];
691  float min, max;
692  if(maxAbsIntensity_>0.0f)
693  {
694  max = maxAbsIntensity_;
695  }
696  else
697  {
698  uMinMax(intensities, j, min, max);
699  }
700  for(size_t k=0; k<j; ++k)
701  {
702  colors[k*3+0] = colors[k*3+1] = colors[k*3+2] = max>0?(unsigned char)(std::min(intensities[k]/max*255.0f, 255.0f)):255;
703  if(colormap_ == 1)
704  {
705  colors[k*3+0] = 255;
706  colors[k*3+2] = 0;
707  }
708  else if(colormap_ == 2)
709  {
710  float r,g,b;
711  util2d::HSVtoRGB(&r, &g, &b, colors[k*3+0]*299.0f/255.0f, 1.0f, 1.0f);
712  colors[k*3+0] = r*255.0f;
713  colors[k*3+1] = g*255.0f;
714  colors[k*3+2] = b*255.0f;
715  }
716  }
717  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (j);
718  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
719  }
720  else
721  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
722  //delete [] colors;
723  delete [] intensities;
724 #if PCL_VERSION_COMPARE(>, 1, 11, 1)
725  return scalars;
726 #else
727  return (true);
728 #endif
729  }
730 
731 protected:
733  virtual std::string
734  getName () const { return ("PointCloudColorHandlerIntensityField"); }
735 
737  virtual std::string
738  getFieldName () const { return ("intensity"); }
739 
740 private:
741  float maxAbsIntensity_;
742  int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
743 };
744 
745 bool CloudViewer::addCloud(
746  const std::string & id,
747  const pcl::PCLPointCloud2Ptr & binaryCloud,
748  const Transform & pose,
749  bool rgb,
750  bool hasNormals,
751  bool hasIntensity,
752  const QColor & color,
753  int viewport)
754 {
755  int previousColorIndex = -1;
756  if(_addedClouds.contains(id))
757  {
758  previousColorIndex = _visualizer->getColorHandlerIndex(id);
759  this->removeCloud(id);
760  }
761 
762  Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
763  Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().linear());
764 
765  if(hasNormals && _aShowNormals->isChecked())
766  {
767  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
768  pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
769  std::string idNormals = id + "-normals";
770  if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, viewport))
771  {
772  _visualizer->updatePointCloudPose(idNormals, pose.toEigen3f());
773  _addedClouds.insert(idNormals, pose);
774  }
775  }
776 
777  // add random color channel
778  pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
779  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
780  if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport))
781  {
782  QColor c = Qt::gray;
783  if(color.isValid())
784  {
785  c = color;
786  }
787  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, c.red(), c.green(), c.blue()));
788  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
789 
790  // x,y,z
791  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
792  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
793  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
794  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
795  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
796  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
797 
798  if(rgb)
799  {
800  //rgb
801  colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
802  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
803  }
804  else if(hasIntensity)
805  {
806  //intensity
807  colorHandler.reset(new PointCloudColorHandlerIntensityField(binaryCloud, _intensityAbsMax, _aSetIntensityRedColormap->isChecked()?1:_aSetIntensityRainbowColormap->isChecked()?2:0));
808  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
809  }
810  else if(previousColorIndex == 5)
811  {
812  previousColorIndex = -1;
813  }
814 
815  if(hasNormals)
816  {
817  //normals
818  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
819  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
820  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
821  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
822  colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
823  _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
824  }
825  else if(previousColorIndex > 5)
826  {
827  previousColorIndex = -1;
828  }
829 
830  if(previousColorIndex>=0)
831  {
832  _visualizer->updateColorHandlerIndex(id, previousColorIndex);
833  }
834  else if(rgb)
835  {
836  _visualizer->updateColorHandlerIndex(id, 5);
837  }
838  else if(hasNormals)
839  {
840  _visualizer->updateColorHandlerIndex(id, hasIntensity?8:7);
841  }
842  else if(hasIntensity)
843  {
844  _visualizer->updateColorHandlerIndex(id, 5);
845  }
846  else if(color.isValid())
847  {
848  _visualizer->updateColorHandlerIndex(id, 1);
849  }
850 
851  _addedClouds.insert(id, pose);
852  return true;
853  }
854  return false;
855 }
856 
857 bool CloudViewer::addCloud(
858  const std::string & id,
859  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
860  const Transform & pose,
861  const QColor & color)
862 {
863  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
864  pcl::toPCLPointCloud2(*cloud, *binaryCloud);
865  return addCloud(id, binaryCloud, pose, true, true, false, color);
866 }
867 
868 bool CloudViewer::addCloud(
869  const std::string & id,
870  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
871  const Transform & pose,
872  const QColor & color)
873 {
874  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
875  pcl::toPCLPointCloud2(*cloud, *binaryCloud);
876  return addCloud(id, binaryCloud, pose, true, false, false, color);
877 }
878 
879 bool CloudViewer::addCloud(
880  const std::string & id,
881  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
882  const Transform & pose,
883  const QColor & color)
884 {
885  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
886  pcl::toPCLPointCloud2(*cloud, *binaryCloud);
887  return addCloud(id, binaryCloud, pose, false, true, true, color);
888 }
889 
890 bool CloudViewer::addCloud(
891  const std::string & id,
892  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
893  const Transform & pose,
894  const QColor & color)
895 {
896  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
897  pcl::toPCLPointCloud2(*cloud, *binaryCloud);
898  return addCloud(id, binaryCloud, pose, false, false, true, color);
899 }
900 
901 bool CloudViewer::addCloud(
902  const std::string & id,
903  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
904  const Transform & pose,
905  const QColor & color)
906 {
907  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
908  pcl::toPCLPointCloud2(*cloud, *binaryCloud);
909  return addCloud(id, binaryCloud, pose, false, true, false, color);
910 }
911 
912 bool CloudViewer::addCloud(
913  const std::string & id,
914  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
915  const Transform & pose,
916  const QColor & color)
917 {
918  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
919  pcl::toPCLPointCloud2(*cloud, *binaryCloud);
920  return addCloud(id, binaryCloud, pose, false, false, false, color);
921 }
922 
923 bool CloudViewer::addCloudMesh(
924  const std::string & id,
925  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
926  const std::vector<pcl::Vertices> & polygons,
927  const Transform & pose)
928 {
929  if(_addedClouds.contains(id))
930  {
931  this->removeCloud(id);
932  }
933 
934  UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
935  if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id, 1))
936  {
937 #if VTK_MAJOR_VERSION >= 7
938  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
939 #else
940  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
941 #endif
942  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
943  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
944  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
945  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
946  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
947  _visualizer->updatePointCloudPose(id, pose.toEigen3f());
948  if(_buildLocator)
949  {
950  vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
951  tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
952  tree->BuildLocator();
953  _locators.insert(std::make_pair(id, tree));
954  }
955  _addedClouds.insert(id, pose);
956  return true;
957  }
958  return false;
959 }
960 
961 bool CloudViewer::addCloudMesh(
962  const std::string & id,
963  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
964  const std::vector<pcl::Vertices> & polygons,
965  const Transform & pose)
966 {
967  if(_addedClouds.contains(id))
968  {
969  this->removeCloud(id);
970  }
971 
972  UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
973  if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id, 1))
974  {
975 #if VTK_MAJOR_VERSION >= 7
976  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
977 #else
978  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
979 #endif
980  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
981  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
982  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
983  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
984  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
985  _visualizer->updatePointCloudPose(id, pose.toEigen3f());
986  if(_buildLocator)
987  {
988  vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
989  tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
990  tree->BuildLocator();
991  _locators.insert(std::make_pair(id, tree));
992  }
993  _addedClouds.insert(id, pose);
994  return true;
995  }
996  return false;
997 }
998 
999 bool CloudViewer::addCloudMesh(
1000  const std::string & id,
1001  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1002  const std::vector<pcl::Vertices> & polygons,
1003  const Transform & pose)
1004 {
1005  if(_addedClouds.contains(id))
1006  {
1007  this->removeCloud(id);
1008  }
1009 
1010  UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
1011  if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id, 1))
1012  {
1013 #if VTK_MAJOR_VERSION >= 7
1014  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
1015 #else
1016  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
1017 #endif
1018  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
1019  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
1020  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
1021  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
1022  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
1023  _visualizer->updatePointCloudPose(id, pose.toEigen3f());
1024  if(_buildLocator)
1025  {
1026  vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
1027  tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
1028  tree->BuildLocator();
1029  _locators.insert(std::make_pair(id, tree));
1030  }
1031  _addedClouds.insert(id, pose);
1032  return true;
1033  }
1034  return false;
1035 }
1036 
1037 bool CloudViewer::addCloudMesh(
1038  const std::string & id,
1039  const pcl::PolygonMesh::Ptr & mesh,
1040  const Transform & pose)
1041 {
1042  if(_addedClouds.contains(id))
1043  {
1044  this->removeCloud(id);
1045  }
1046 
1047  UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
1048  if(_visualizer->addPolygonMesh(*mesh, id, 1))
1049  {
1050 #if VTK_MAJOR_VERSION >= 7
1051  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
1052 #else
1053  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
1054 #endif
1055  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
1056  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
1057  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
1058  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
1059  _visualizer->updatePointCloudPose(id, pose.toEigen3f());
1060  if(_buildLocator)
1061  {
1062  vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
1063  tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
1064  tree->BuildLocator();
1065  _locators.insert(std::make_pair(id, tree));
1066  }
1067  _addedClouds.insert(id, pose);
1068  return true;
1069  }
1070 
1071  return false;
1072 }
1073 
1074 bool CloudViewer::addCloudTextureMesh(
1075  const std::string & id,
1076  const pcl::TextureMesh::Ptr & textureMesh,
1077  const cv::Mat & texture,
1078  const Transform & pose)
1079 {
1080  if(_addedClouds.contains(id))
1081  {
1082  this->removeCloud(id);
1083  }
1084 
1085  UDEBUG("Adding %s", id.c_str());
1086  if(this->addTextureMesh(*textureMesh, texture, id, 1))
1087  {
1088 #if VTK_MAJOR_VERSION >= 7
1089  vtkActor* actor = vtkActor::SafeDownCast (_visualizer->getShapeActorMap()->find(id)->second);
1090 #else
1091  vtkActor* actor = vtkActor::SafeDownCast (_visualizer->getCloudActorMap()->find(id)->second.actor);
1092 #endif
1093  UASSERT(actor);
1094  if(!textureMesh->cloud.is_dense)
1095  {
1096  actor->GetTexture()->SetInterpolate(1);
1097  actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
1098  }
1099  if(_buildLocator)
1100  {
1101  vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
1102  tree->SetDataSet(actor->GetMapper()->GetInput());
1103  tree->BuildLocator();
1104  _locators.insert(std::make_pair(id, tree));
1105  }
1106  _addedClouds.insert(id, Transform::getIdentity());
1107  this->updateCloudPose(id, pose);
1108  return true;
1109  }
1110  return false;
1111 }
1112 
1113 bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool volumeRepresentation)
1114 {
1115  UDEBUG("");
1116 #ifdef RTABMAP_OCTOMAP
1117  UASSERT(octomap!=0);
1118 
1119  if(treeDepth == 0 || treeDepth > octomap->octree()->getTreeDepth())
1120  {
1121  if(treeDepth>0)
1122  {
1123  UWARN("Tree depth requested (%d) is deeper than the "
1124  "actual maximum tree depth of %d. Using maximum depth.",
1125  (int)treeDepth, (int)octomap->octree()->getTreeDepth());
1126  }
1127  treeDepth = octomap->octree()->getTreeDepth();
1128  }
1129 
1130  removeOctomap();
1131 
1132  if(!volumeRepresentation)
1133  {
1134  pcl::IndicesPtr obstacles(new std::vector<int>);
1135  pcl::IndicesPtr ground(new std::vector<int>);
1136 
1137  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(
1138  treeDepth, obstacles.get(), 0, ground.get(), false);
1139  obstacles->insert(obstacles->end(), ground->begin(), ground->end());
1140  if(obstacles->size())
1141  {
1142  //vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
1143  //colors->SetName("colors");
1144  //colors->SetNumberOfComponents(3);
1145  vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
1146  colors->SetName("colors");
1147  colors->SetNumberOfValues(obstacles->size());
1148 
1149  vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
1150  lut->SetNumberOfTableValues(obstacles->size());
1151  lut->Build();
1152 
1153  // Create points
1154  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
1155  points->SetNumberOfPoints(obstacles->size());
1156  double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
1157  for (unsigned int i = 0; i < obstacles->size(); i++)
1158  {
1159  points->InsertPoint(i,
1160  cloud->at(obstacles->at(i)).x,
1161  cloud->at(obstacles->at(i)).y,
1162  cloud->at(obstacles->at(i)).z);
1163  colors->InsertValue(i,i);
1164 
1165  lut->SetTableValue(i,
1166  double(cloud->at(obstacles->at(i)).r) / 255.0,
1167  double(cloud->at(obstacles->at(i)).g) / 255.0,
1168  double(cloud->at(obstacles->at(i)).b) / 255.0);
1169  }
1170 
1171  // Combine into a polydata
1172  vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
1173  polydata->SetPoints(points);
1174  polydata->GetPointData()->SetScalars(colors);
1175 
1176  // Create anything you want here, we will use a cube for the demo.
1177  vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
1178  cubeSource->SetBounds(-s, s, -s, s, -s, s);
1179 
1180  vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
1181  mapper->SetSourceConnection(cubeSource->GetOutputPort());
1182 #if VTK_MAJOR_VERSION <= 5
1183  mapper->SetInputConnection(polydata->GetProducerPort());
1184 #else
1185  mapper->SetInputData(polydata);
1186 #endif
1187  mapper->SetScalarRange(0, obstacles->size() - 1);
1188  mapper->SetLookupTable(lut);
1189  mapper->ScalingOff();
1190  mapper->Update();
1191 
1192  vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
1193  octomapActor->SetMapper(mapper);
1194 
1195  octomapActor->GetProperty()->SetRepresentationToSurface();
1196  octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
1197  octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
1198 
1199  _visualizer->getRendererCollection()->InitTraversal ();
1200  vtkRenderer* renderer = NULL;
1201  renderer = _visualizer->getRendererCollection()->GetNextItem ();
1202  renderer = _visualizer->getRendererCollection()->GetNextItem ();
1203  UASSERT(renderer);
1204  renderer->AddActor(octomapActor);
1205 
1206  _octomapActor = octomapActor.GetPointer();
1207  return true;
1208  }
1209  }
1210  else
1211  {
1212  if(octomap->octree()->size())
1213  {
1214  // Create an image data
1215  vtkSmartPointer<vtkImageData> imageData =
1216  vtkSmartPointer<vtkImageData>::New();
1217 
1218  double sizeX, sizeY, sizeZ;
1219  double minX, minY, minZ;
1220  double maxX, maxY, maxZ;
1221  octomap->getGridMin(minX, minY, minZ);
1222  octomap->getGridMax(maxX, maxY, maxZ);
1223  sizeX = maxX-minX;
1224  sizeY = maxY-minY;
1225  sizeZ = maxZ-minZ;
1226  double cellSize = octomap->octree()->getNodeSize(treeDepth);
1227 
1228  UTimer t;
1229  // Specify the size of the image data
1230  imageData->SetExtent(0, int(sizeX/cellSize+0.5), 0, int(sizeY/cellSize+0.5), 0, int(sizeZ/cellSize+0.5)); // 3D image
1231 #if VTK_MAJOR_VERSION <= 5
1232  imageData->SetNumberOfScalarComponents(4);
1233  imageData->SetScalarTypeToUnsignedChar();
1234 #else
1235  imageData->AllocateScalars(VTK_UNSIGNED_CHAR,4);
1236 #endif
1237 
1238  int dims[3];
1239  imageData->GetDimensions(dims);
1240 
1241  memset(imageData->GetScalarPointer(), 0, imageData->GetScalarSize()*imageData->GetNumberOfScalarComponents()*dims[0]*dims[1]*dims[2]);
1242 
1243  for (RtabmapColorOcTree::iterator it = octomap->octree()->begin(treeDepth); it != octomap->octree()->end(); ++it)
1244  {
1245  if(octomap->octree()->isNodeOccupied(*it))
1246  {
1247  octomap::point3d pt = octomap->octree()->keyToCoord(it.getKey());
1248  int x = (pt.x()-minX) / cellSize;
1249  int y = (pt.y()-minY) / cellSize;
1250  int z = (pt.z()-minZ) / cellSize;
1251  if(x>=0 && x<dims[0] && y>=0 && y<dims[1] && z>=0 && z<dims[2])
1252  {
1253  unsigned char* pixel = static_cast<unsigned char*>(imageData->GetScalarPointer(x,y,z));
1254  if(octomap->octree()->getTreeDepth() == it.getDepth() && it->isColorSet())
1255  {
1256  pixel[0] = it->getColor().r;
1257  pixel[1] = it->getColor().g;
1258  pixel[2] = it->getColor().b;
1259  }
1260  else
1261  {
1262  // Gradiant color on z axis
1263  float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
1264  float r,g,b;
1265  util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
1266  pixel[0] = r*255.0f;
1267  pixel[1] = g*255.0f;
1268  pixel[2] = b*255.0f;
1269  }
1270  pixel[3] = 255;
1271  }
1272  }
1273  }
1274  vtkSmartPointer<vtkSmartVolumeMapper> volumeMapper =
1275  vtkSmartPointer<vtkSmartVolumeMapper>::New();
1276  volumeMapper->SetBlendModeToComposite(); // composite first
1277 #if VTK_MAJOR_VERSION <= 5
1278  volumeMapper->SetInputConnection(imageData->GetProducerPort());
1279 #else
1280  volumeMapper->SetInputData(imageData);
1281 #endif
1282  vtkSmartPointer<vtkVolumeProperty> volumeProperty =
1283  vtkSmartPointer<vtkVolumeProperty>::New();
1284  volumeProperty->ShadeOff();
1285  volumeProperty->IndependentComponentsOff();
1286 
1287  vtkSmartPointer<vtkPiecewiseFunction> compositeOpacity =
1288  vtkSmartPointer<vtkPiecewiseFunction>::New();
1289  compositeOpacity->AddPoint(0.0,0.0);
1290  compositeOpacity->AddPoint(255.0,1.0);
1291  volumeProperty->SetScalarOpacity(0, compositeOpacity); // composite first.
1292 
1293  vtkSmartPointer<vtkVolume> volume =
1294  vtkSmartPointer<vtkVolume>::New();
1295  volume->SetMapper(volumeMapper);
1296  volume->SetProperty(volumeProperty);
1297  volume->SetScale(cellSize);
1298  volume->SetPosition(minX, minY, minZ);
1299 
1300  _visualizer->getRendererCollection()->InitTraversal ();
1301  vtkRenderer* renderer = NULL;
1302  renderer = _visualizer->getRendererCollection()->GetNextItem ();
1303  renderer = _visualizer->getRendererCollection()->GetNextItem ();
1304  UASSERT(renderer);
1305  renderer->AddViewProp(volume);
1306 
1307  // 3D texture mode. For coverage.
1308 #if !defined(VTK_LEGACY_REMOVE) && !defined(VTK_OPENGL2) && VTK_MAJOR_VERSION < 9
1309  volumeMapper->SetRequestedRenderModeToRayCastAndTexture();
1310 #endif // VTK_LEGACY_REMOVE
1311 
1312  // Software mode, for coverage. It also makes sure we will get the same
1313  // regression image on all platforms.
1314  volumeMapper->SetRequestedRenderModeToRayCast();
1315  _octomapActor = volume.GetPointer();
1316 
1317  return true;
1318  }
1319  }
1320 #endif
1321  return false;
1322 }
1323 
1324 void CloudViewer::removeOctomap()
1325 {
1326  UDEBUG("");
1327 #ifdef RTABMAP_OCTOMAP
1328  if(_octomapActor)
1329  {
1330  _visualizer->getRendererCollection()->InitTraversal ();
1331  vtkRenderer* renderer = NULL;
1332  renderer = _visualizer->getRendererCollection()->GetNextItem ();
1333  renderer = _visualizer->getRendererCollection()->GetNextItem ();
1334  UASSERT(renderer);
1335  renderer->RemoveActor(_octomapActor);
1336  _octomapActor = 0;
1337  }
1338 #endif
1339 }
1340 
1341 bool CloudViewer::addTextureMesh (
1342  const pcl::TextureMesh &mesh,
1343  const cv::Mat & image,
1344  const std::string &id,
1345  int viewport)
1346 {
1347  // Copied from PCL 1.8, modified to ignore vertex color and accept only one material (loaded from memory instead of file)
1348 
1349 #if VTK_MAJOR_VERSION >= 7
1350  pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
1351  if (am_it != _visualizer->getShapeActorMap()->end ())
1352 #else
1353  pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id);
1354  if (am_it != _visualizer->getCloudActorMap()->end ())
1355 #endif
1356  {
1357  PCL_ERROR ("[PCLVisualizer::addTextureMesh] A shape with id <%s> already exists!"
1358  " Please choose a different id and retry.\n",
1359  id.c_str ());
1360  return (false);
1361  }
1362  // no texture materials --> exit
1363  if (mesh.tex_materials.size () == 0)
1364  {
1365  PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures found!\n");
1366  return (false);
1367  }
1368  else if (mesh.tex_materials.size() > 1)
1369  {
1370  PCL_ERROR("[PCLVisualizer::addTextureMesh] only one material per mesh is supported!\n");
1371  return (false);
1372  }
1373  // polygons are mapped to texture materials
1374  if (mesh.tex_materials.size () != mesh.tex_polygons.size ())
1375  {
1376  PCL_ERROR("[PCLVisualizer::addTextureMesh] Materials number %lu differs from polygons number %lu!\n",
1377  mesh.tex_materials.size (), mesh.tex_polygons.size ());
1378  return (false);
1379  }
1380  // each texture material should have its coordinates set
1381  if (mesh.tex_materials.size () != mesh.tex_coordinates.size ())
1382  {
1383  PCL_ERROR("[PCLVisualizer::addTextureMesh] Coordinates number %lu differs from materials number %lu!\n",
1384  mesh.tex_coordinates.size (), mesh.tex_materials.size ());
1385  return (false);
1386  }
1387  // total number of vertices
1388  std::size_t nb_vertices = 0;
1389  for (std::size_t i = 0; i < mesh.tex_polygons.size (); ++i)
1390  nb_vertices+= mesh.tex_polygons[i].size ();
1391  // no vertices --> exit
1392  if (nb_vertices == 0)
1393  {
1394  PCL_ERROR("[PCLVisualizer::addTextureMesh] No vertices found!\n");
1395  return (false);
1396  }
1397  // total number of coordinates
1398  std::size_t nb_coordinates = 0;
1399  for (std::size_t i = 0; i < mesh.tex_coordinates.size (); ++i)
1400  nb_coordinates+= mesh.tex_coordinates[i].size ();
1401  // no texture coordinates --> exit
1402  if (nb_coordinates == 0)
1403  {
1404  PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures coordinates found!\n");
1405  return (false);
1406  }
1407 
1408  // Create points from mesh.cloud
1409  vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
1410  vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
1411  bool has_color = false;
1412  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
1413 
1414  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
1415  pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
1416  // no points --> exit
1417  if (cloud->points.size () == 0)
1418  {
1419  PCL_ERROR("[PCLVisualizer::addTextureMesh] Cloud is empty!\n");
1420  return (false);
1421  }
1422  pcl::visualization::PCLVisualizer::convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
1423  poly_points->SetNumberOfPoints (cloud->points.size ());
1424  for (std::size_t i = 0; i < cloud->points.size (); ++i)
1425  {
1426  const pcl::PointXYZ &p = cloud->points[i];
1427  poly_points->InsertPoint (i, p.x, p.y, p.z);
1428  }
1429 
1430  //create polys from polyMesh.tex_polygons
1431  vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New ();
1432  for (std::size_t i = 0; i < mesh.tex_polygons.size (); i++)
1433  {
1434  for (std::size_t j = 0; j < mesh.tex_polygons[i].size (); j++)
1435  {
1436  std::size_t n_points = mesh.tex_polygons[i][j].vertices.size ();
1437  polys->InsertNextCell (int (n_points));
1438  for (std::size_t k = 0; k < n_points; k++)
1439  polys->InsertCellPoint (mesh.tex_polygons[i][j].vertices[k]);
1440  }
1441  }
1442 
1443  vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
1444  polydata->SetPolys (polys);
1445  polydata->SetPoints (poly_points);
1446  if (has_color)
1447  polydata->GetPointData()->SetScalars(colors);
1448 
1449  vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
1450 #if VTK_MAJOR_VERSION < 6
1451  mapper->SetInput (polydata);
1452 #else
1453  mapper->SetInputData (polydata);
1454 #endif
1455 
1456 #if VTK_MAJOR_VERSION >= 7
1457  vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
1458 #else
1459  vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
1460 #endif
1461  vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
1462  if (!tex_manager)
1463  return (false);
1464 
1465  vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
1466  // fill vtkTexture from pcl::TexMaterial structure
1467  vtkSmartPointer<vtkImageMatSource> cvImageToVtk = vtkSmartPointer<vtkImageMatSource>::New();
1468  cvImageToVtk->SetImage(image);
1469  cvImageToVtk->Update();
1470  texture->SetInputConnection(cvImageToVtk->GetOutputPort());
1471 
1472  // set texture coordinates
1473  vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New ();
1474  coordinates->SetNumberOfComponents (2);
1475  coordinates->SetNumberOfTuples (mesh.tex_coordinates[0].size ());
1476  for (std::size_t tc = 0; tc < mesh.tex_coordinates[0].size (); ++tc)
1477  {
1478  const Eigen::Vector2f &uv = mesh.tex_coordinates[0][tc];
1479  coordinates->SetTuple2 (tc, (double)uv[0], (double)uv[1]);
1480  }
1481  coordinates->SetName ("TCoords");
1482  polydata->GetPointData ()->SetTCoords(coordinates);
1483  // apply texture
1484  actor->SetTexture (texture);
1485 
1486  // set mapper
1487  actor->SetMapper (mapper);
1488 
1489  //_visualizer->addActorToRenderer (actor, viewport);
1490  // Add it to all renderers
1491  _visualizer->getRendererCollection()->InitTraversal ();
1492  vtkRenderer* renderer = NULL;
1493  int i = 0;
1494  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
1495  {
1496  // Should we add the actor to all renderers?
1497  if (viewport == 0)
1498  {
1499  renderer->AddActor (actor);
1500  }
1501  else if (viewport == i) // add the actor only to the specified viewport
1502  {
1503  renderer->AddActor (actor);
1504  }
1505  ++i;
1506  }
1507 
1508  // Save the pointer/ID pair to the global actor map
1509 #if VTK_MAJOR_VERSION >= 7
1510  (*_visualizer->getShapeActorMap())[id] = actor;
1511 #else
1512  (*_visualizer->getCloudActorMap())[id].actor = actor;
1513  // Save the viewpoint transformation matrix to the global actor map
1514  (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
1515 #endif
1516 
1517 #if VTK_MAJOR_VERSION >= 7
1518  actor->GetProperty()->SetAmbient(0.1);
1519 #else
1520  actor->GetProperty()->SetAmbient(0.5);
1521 #endif
1522  actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
1523  actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
1524  actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
1525  actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
1526  actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
1527  return true;
1528 }
1529 
1530 bool CloudViewer::addOccupancyGridMap(
1531  const cv::Mat & map8U,
1532  float resolution, // cell size
1533  float xMin,
1534  float yMin,
1535  float opacity)
1536 {
1537  UASSERT(map8U.channels() == 1 && map8U.type() == CV_8U);
1538 
1539  float xSize = float(map8U.cols) * resolution;
1540  float ySize = float(map8U.rows) * resolution;
1541 
1542  UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
1543 #if VTK_MAJOR_VERSION >= 7
1544  if(_visualizer->getShapeActorMap()->find("map") != _visualizer->getShapeActorMap()->end())
1545  {
1546  _visualizer->removeShape("map");
1547  }
1548 #else
1549  if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
1550  {
1551  _visualizer->removePointCloud("map");
1552  }
1553 #endif
1554 
1555  if(xSize > 0.0f && ySize > 0.0f)
1556  {
1557  pcl::TextureMeshPtr mesh(new pcl::TextureMesh());
1558  pcl::PointCloud<pcl::PointXYZ> cloud;
1559  cloud.push_back(pcl::PointXYZ(xMin, yMin, 0));
1560  cloud.push_back(pcl::PointXYZ(xSize+xMin, yMin, 0));
1561  cloud.push_back(pcl::PointXYZ(xSize+xMin, ySize+yMin, 0));
1562  cloud.push_back(pcl::PointXYZ(xMin, ySize+yMin, 0));
1563  pcl::toPCLPointCloud2(cloud, mesh->cloud);
1564 
1565  std::vector<pcl::Vertices> polygons(1);
1566  polygons[0].vertices.push_back(0);
1567  polygons[0].vertices.push_back(1);
1568  polygons[0].vertices.push_back(2);
1569  polygons[0].vertices.push_back(3);
1570  polygons[0].vertices.push_back(0);
1571  mesh->tex_polygons.push_back(polygons);
1572 
1573  // default texture materials parameters
1574  pcl::TexMaterial material;
1575  material.tex_file = "";
1576  mesh->tex_materials.push_back(material);
1577 
1578 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1579  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > coordinates;
1580 #else
1581  std::vector<Eigen::Vector2f> coordinates;
1582 #endif
1583  coordinates.push_back(Eigen::Vector2f(0,1));
1584  coordinates.push_back(Eigen::Vector2f(1,1));
1585  coordinates.push_back(Eigen::Vector2f(1,0));
1586  coordinates.push_back(Eigen::Vector2f(0,0));
1587  mesh->tex_coordinates.push_back(coordinates);
1588 
1589  this->addTextureMesh(*mesh, map8U, "map", 1);
1590  setCloudOpacity("map", opacity);
1591  }
1592  return true;
1593 }
1594 
1595 void CloudViewer::removeOccupancyGridMap()
1596 {
1597 #if VTK_MAJOR_VERSION >= 7
1598  if(_visualizer->getShapeActorMap()->find("map") != _visualizer->getShapeActorMap()->end())
1599  {
1600  _visualizer->removeShape("map");
1601  }
1602 #else
1603  if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
1604  {
1605  _visualizer->removePointCloud("map");
1606  }
1607 #endif
1608 }
1609 
1610 bool CloudViewer::addElevationMap(
1611  const cv::Mat & map32FC1,
1612  float resolution, // cell size
1613  float xMin,
1614  float yMin,
1615  float opacity)
1616 {
1617  if(_visualizer->getShapeActorMap()->find("elevation_map") != _visualizer->getShapeActorMap()->end())
1618  {
1619  _visualizer->removeShape("elevation_map");
1620  }
1621 
1622  vtkSmartPointer<vtkPoints> gridPoints = vtkSmartPointer<vtkPoints>::New ();
1623  vtkSmartPointer<vtkCellArray> gridCells = vtkSmartPointer<vtkCellArray>::New ();
1624  for (int y = 0; y < map32FC1.rows; ++y)
1625  {
1626  const float * previousRow = y>0?map32FC1.ptr<float>(y-1):0;
1627  const float * rowPtr = map32FC1.ptr<float>(y);
1628  for (int x = 0; x < map32FC1.cols; ++x)
1629  {
1630  gridPoints->InsertNextPoint(xMin + x*resolution, yMin + y*resolution, rowPtr[x]);
1631  if(x>0 && y>0 &&
1632  rowPtr[x] != 0 &&
1633  rowPtr[x-1] != 0 &&
1634  previousRow[x] != 0 &&
1635  previousRow[x-1] != 0)
1636  {
1637  gridCells->InsertNextCell(4);
1638  gridCells->InsertCellPoint(x-1+y*map32FC1.cols);
1639  gridCells->InsertCellPoint(x+y*map32FC1.cols);
1640  gridCells->InsertCellPoint(x+(y-1)*map32FC1.cols);
1641  gridCells->InsertCellPoint(x-1+(y-1)*map32FC1.cols);
1642  }
1643  }
1644  }
1645 
1646  double bounds[6];
1647  gridPoints->GetBounds(bounds);
1648 
1649  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
1650  polyData->SetPoints(gridPoints);
1651  polyData->SetPolys(gridCells);
1652 
1653  vtkSmartPointer<vtkElevationFilter> elevationFilter = vtkSmartPointer<vtkElevationFilter>::New ();
1654  elevationFilter->SetInputData(polyData);
1655  elevationFilter->SetLowPoint(0.0, 0.0, bounds[4]);
1656  elevationFilter->SetHighPoint(0.0, 0.0, bounds[5]);
1657  elevationFilter->Update();
1658 
1659  vtkSmartPointer<vtkPolyData> output = vtkSmartPointer<vtkPolyData>::New ();
1660  output->ShallowCopy(dynamic_cast<vtkPolyData*>(elevationFilter->GetOutput()));
1661 
1662  vtkFloatArray* elevation = dynamic_cast<vtkFloatArray*>(
1663  output->GetPointData()->GetArray("Elevation"));
1664 
1665  // Create the color map
1666  vtkSmartPointer<vtkLookupTable> colorLookupTable = vtkSmartPointer<vtkLookupTable>::New ();
1667  colorLookupTable->SetTableRange(bounds[4], bounds[5]);
1668  colorLookupTable->Build();
1669 
1670  // Generate the colors for each point based on the color map
1671  vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
1672  colors->SetNumberOfComponents(3);
1673  colors->SetName("Colors");
1674 
1675  for (vtkIdType i = 0; i < output->GetNumberOfPoints(); i++)
1676  {
1677  double val = elevation->GetValue(i);
1678  double dcolor[3];
1679  colorLookupTable->GetColor(val, dcolor);
1680  unsigned char color[3];
1681  for (unsigned int j = 0; j < 3; j++)
1682  {
1683  color[j] = 255 * dcolor[j] / 1.0;
1684  }
1685 
1686 #if VTK_MAJOR_VERSION > 7 || (VTK_MAJOR_VERSION==7 && VTK_MINOR_VERSION >= 1)
1687  colors->InsertNextTypedTuple(color);
1688 #else
1689  colors->InsertNextTupleValue(color);
1690 #endif
1691 
1692  }
1693 
1694  output->GetPointData()->AddArray(colors);
1695 
1696  vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
1697  mapper->SetInputData(output);
1698 
1699  vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
1700  actor->SetMapper(mapper);
1701 
1702  // Add it to all renderers
1703  _visualizer->getRendererCollection()->InitTraversal ();
1704  vtkRenderer* renderer = NULL;
1705  int i = 0;
1706  int viewport = 1;
1707  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
1708  {
1709  // Should we add the actor to all renderers?
1710  if (viewport == 0)
1711  {
1712  renderer->AddActor (actor);
1713  }
1714  else if (viewport == i) // add the actor only to the specified viewport
1715  {
1716  renderer->AddActor (actor);
1717  }
1718  ++i;
1719  }
1720 
1721  (*_visualizer->getShapeActorMap())["elevation_map"] = actor;
1722 
1723  setCloudOpacity("elevation_map", opacity);
1724 
1725  return true;
1726 }
1727 void CloudViewer::removeElevationMap()
1728 {
1729  if(_visualizer->getShapeActorMap()->find("elevation_map") != _visualizer->getShapeActorMap()->end())
1730  {
1731  _visualizer->removeShape("elevation_map");
1732  }
1733 }
1734 
1735 void CloudViewer::addOrUpdateCoordinate(
1736  const std::string & id,
1737  const Transform & transform,
1738  double scale,
1739  bool foreground)
1740 {
1741  if(id.empty())
1742  {
1743  UERROR("id should not be empty!");
1744  return;
1745  }
1746 
1747  removeCoordinate(id);
1748 
1749  if(!transform.isNull())
1750  {
1751  _coordinates.insert(id);
1752 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
1753  _visualizer->addCoordinateSystem(scale*_coordinateFrameScale, transform.toEigen3f(), id, foreground?3:2);
1754 #else
1755  // Well, on older versions, just update the main coordinate
1756  _visualizer->addCoordinateSystem(scale*_coordinateFrameScale, transform.toEigen3f(), 0);
1757 #endif
1758  }
1759 }
1760 
1761 bool CloudViewer::updateCoordinatePose(
1762  const std::string & id,
1763  const Transform & pose)
1764 {
1765 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
1766  if(_coordinates.find(id) != _coordinates.end() && !pose.isNull())
1767  {
1768  UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
1769  return _visualizer->updateCoordinateSystemPose(id, pose.toEigen3f());
1770  }
1771 #else
1772  UERROR("CloudViewer::updateCoordinatePose() is not available on PCL < 1.7.2");
1773 #endif
1774  return false;
1775 }
1776 
1777 void CloudViewer::removeCoordinate(const std::string & id)
1778 {
1779  if(id.empty())
1780  {
1781  UERROR("id should not be empty!");
1782  return;
1783  }
1784 
1785  if(_coordinates.find(id) != _coordinates.end())
1786  {
1787 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
1788  _visualizer->removeCoordinateSystem(id);
1789 #else
1790  // Well, on older versions, just update the main coordinate
1791  _visualizer->removeCoordinateSystem(0);
1792 #endif
1793  _coordinates.erase(id);
1794  }
1795 }
1796 
1797 void CloudViewer::removeAllCoordinates(const std::string & prefix)
1798 {
1799  std::set<std::string> coordinates = _coordinates;
1800  for(std::set<std::string>::iterator iter = coordinates.begin(); iter!=coordinates.end(); ++iter)
1801  {
1802  if(prefix.empty() || iter->find(prefix) != std::string::npos)
1803  {
1804  this->removeCoordinate(*iter);
1805  }
1806  }
1807  UASSERT(!prefix.empty() || _coordinates.empty());
1808 }
1809 
1810 void CloudViewer::addOrUpdateLine(
1811  const std::string & id,
1812  const Transform & from,
1813  const Transform & to,
1814  const QColor & color,
1815  bool arrow,
1816  bool foreground)
1817 {
1818  if(id.empty())
1819  {
1820  UERROR("id should not be empty!");
1821  return;
1822  }
1823 
1824  removeLine(id);
1825 
1826  if(!from.isNull() && !to.isNull())
1827  {
1828  _lines.insert(id);
1829 
1830  QColor c = Qt::gray;
1831  if(color.isValid())
1832  {
1833  c = color;
1834  }
1835 
1836  pcl::PointXYZ pt1(from.x(), from.y(), from.z());
1837  pcl::PointXYZ pt2(to.x(), to.y(), to.z());
1838 
1839  if(arrow)
1840  {
1841  _visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id, foreground?3:2);
1842  }
1843  else
1844  {
1845  _visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2);
1846  }
1847  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
1848  }
1849 }
1850 
1851 void CloudViewer::removeLine(const std::string & id)
1852 {
1853  if(id.empty())
1854  {
1855  UERROR("id should not be empty!");
1856  return;
1857  }
1858 
1859  if(_lines.find(id) != _lines.end())
1860  {
1861  _visualizer->removeShape(id);
1862  _lines.erase(id);
1863  }
1864 }
1865 
1866 void CloudViewer::removeAllLines()
1867 {
1868  std::set<std::string> arrows = _lines;
1869  for(std::set<std::string>::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter)
1870  {
1871  this->removeLine(*iter);
1872  }
1873  UASSERT(_lines.empty());
1874 }
1875 
1876 void CloudViewer::addOrUpdateSphere(
1877  const std::string & id,
1878  const Transform & pose,
1879  float radius,
1880  const QColor & color,
1881  bool foreground)
1882 {
1883  if(id.empty())
1884  {
1885  UERROR("id should not be empty!");
1886  return;
1887  }
1888 
1889  removeSphere(id);
1890 
1891  if(!pose.isNull())
1892  {
1893  _spheres.insert(id);
1894 
1895  QColor c = Qt::gray;
1896  if(color.isValid())
1897  {
1898  c = color;
1899  }
1900 
1901  pcl::PointXYZ center(pose.x(), pose.y(), pose.z());
1902  _visualizer->addSphere(center, radius, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2);
1903  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
1904  }
1905 }
1906 
1907 void CloudViewer::removeSphere(const std::string & id)
1908 {
1909  if(id.empty())
1910  {
1911  UERROR("id should not be empty!");
1912  return;
1913  }
1914 
1915  if(_spheres.find(id) != _spheres.end())
1916  {
1917  _visualizer->removeShape(id);
1918  _spheres.erase(id);
1919  }
1920 }
1921 
1922 void CloudViewer::removeAllSpheres()
1923 {
1924  std::set<std::string> spheres = _spheres;
1925  for(std::set<std::string>::iterator iter = spheres.begin(); iter!=spheres.end(); ++iter)
1926  {
1927  this->removeSphere(*iter);
1928  }
1929  UASSERT(_spheres.empty());
1930 }
1931 
1932 void CloudViewer::addOrUpdateCube(
1933  const std::string & id,
1934  const Transform & pose,
1935  float width,
1936  float height,
1937  float depth,
1938  const QColor & color,
1939  bool wireframe,
1940  bool foreground)
1941 {
1942  if(id.empty())
1943  {
1944  UERROR("id should not be empty!");
1945  return;
1946  }
1947 
1948  removeCube(id);
1949 
1950  if(!pose.isNull())
1951  {
1952  _cubes.insert(id);
1953 
1954  QColor c = Qt::gray;
1955  if(color.isValid())
1956  {
1957  c = color;
1958  }
1959  _visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?3:2);
1960  if(wireframe)
1961  {
1962  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id);
1963  }
1964  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
1965  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
1966  }
1967 }
1968 
1969 void CloudViewer::removeCube(const std::string & id)
1970 {
1971  if(id.empty())
1972  {
1973  UERROR("id should not be empty!");
1974  return;
1975  }
1976 
1977  if(_cubes.find(id) != _cubes.end())
1978  {
1979  _visualizer->removeShape(id);
1980  _cubes.erase(id);
1981  }
1982 }
1983 
1984 void CloudViewer::removeAllCubes()
1985 {
1986  std::set<std::string> cubes = _cubes;
1987  for(std::set<std::string>::iterator iter = cubes.begin(); iter!=cubes.end(); ++iter)
1988  {
1989  this->removeCube(*iter);
1990  }
1991  UASSERT(_cubes.empty());
1992 }
1993 
1994 void CloudViewer::addOrUpdateQuad(
1995  const std::string & id,
1996  const Transform & pose,
1997  float width,
1998  float height,
1999  const QColor & color,
2000  bool foreground)
2001 {
2002  addOrUpdateQuad(id, pose, width/2.0f, width/2.0f, height/2.0f, height/2.0f, color, foreground);
2003 }
2004 
2005 void CloudViewer::addOrUpdateQuad(
2006  const std::string & id,
2007  const Transform & pose,
2008  float widthLeft,
2009  float widthRight,
2010  float heightBottom,
2011  float heightTop,
2012  const QColor & color,
2013  bool foreground)
2014 {
2015  if(id.empty())
2016  {
2017  UERROR("id should not be empty!");
2018  return;
2019  }
2020 
2021  removeQuad(id);
2022 
2023  if(!pose.isNull())
2024  {
2025  _quads.insert(id);
2026 
2027  QColor c = Qt::gray;
2028  if(color.isValid())
2029  {
2030  c = color;
2031  }
2032 
2033  // Create four points (must be in counter clockwise order)
2034  double p0[3] = {0.0, -widthLeft, heightTop};
2035  double p1[3] = {0.0, -widthLeft, -heightBottom};
2036  double p2[3] = {0.0, widthRight, -heightBottom};
2037  double p3[3] = {0.0, widthRight, heightTop};
2038 
2039  // Add the points to a vtkPoints object
2040  vtkSmartPointer<vtkPoints> points =
2041  vtkSmartPointer<vtkPoints>::New();
2042  points->InsertNextPoint(p0);
2043  points->InsertNextPoint(p1);
2044  points->InsertNextPoint(p2);
2045  points->InsertNextPoint(p3);
2046 
2047  // Create a quad on the four points
2048  vtkSmartPointer<vtkQuad> quad =
2049  vtkSmartPointer<vtkQuad>::New();
2050  quad->GetPointIds()->SetId(0,0);
2051  quad->GetPointIds()->SetId(1,1);
2052  quad->GetPointIds()->SetId(2,2);
2053  quad->GetPointIds()->SetId(3,3);
2054 
2055  // Create a cell array to store the quad in
2056  vtkSmartPointer<vtkCellArray> quads =
2057  vtkSmartPointer<vtkCellArray>::New();
2058  quads->InsertNextCell(quad);
2059 
2060  // Create a polydata to store everything in
2061  vtkSmartPointer<vtkPolyData> polydata =
2062  vtkSmartPointer<vtkPolyData>::New();
2063 
2064  // Add the points and quads to the dataset
2065  polydata->SetPoints(points);
2066  polydata->SetPolys(quads);
2067 
2068  // Setup actor and mapper
2069  vtkSmartPointer<vtkPolyDataMapper> mapper =
2070  vtkSmartPointer<vtkPolyDataMapper>::New();
2071 #if VTK_MAJOR_VERSION <= 5
2072  mapper->SetInput(polydata);
2073 #else
2074  mapper->SetInputData(polydata);
2075 #endif
2076 
2077  vtkSmartPointer<vtkLODActor> actor =
2078  vtkSmartPointer<vtkLODActor>::New();
2079  actor->SetMapper(mapper);
2080  actor->GetProperty()->SetColor(c.redF(), c.greenF(), c.blueF());
2081 
2082  //_visualizer->addActorToRenderer (actor, viewport);
2083  // Add it to all renderers
2084  _visualizer->getRendererCollection()->InitTraversal ();
2085  vtkRenderer* renderer = NULL;
2086  int i = 0;
2087  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
2088  {
2089  if ((foreground?3:2) == i) // add the actor only to the specified viewport
2090  {
2091  renderer->AddActor (actor);
2092  }
2093  ++i;
2094  }
2095 
2096  // Save the pointer/ID pair to the global actor map
2097  (*_visualizer->getCloudActorMap())[id].actor = actor;
2098 
2099  // Save the viewpoint transformation matrix to the global actor map
2100  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
2101  pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), transformation);
2102  (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
2103  (*_visualizer->getCloudActorMap())[id].actor->SetUserMatrix (transformation);
2104  (*_visualizer->getCloudActorMap())[id].actor->Modified ();
2105 
2106  (*_visualizer->getCloudActorMap())[id].actor->GetProperty()->SetLighting(false);
2107  _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
2108  }
2109 }
2110 
2111 void CloudViewer::removeQuad(const std::string & id)
2112 {
2113  if(id.empty())
2114  {
2115  UERROR("id should not be empty!");
2116  return;
2117  }
2118 
2119  if(_quads.find(id) != _quads.end())
2120  {
2121  _visualizer->removeShape(id);
2122  _quads.erase(id);
2123  }
2124 }
2125 
2126 void CloudViewer::removeAllQuads()
2127 {
2128  std::set<std::string> quads = _quads;
2129  for(std::set<std::string>::iterator iter = quads.begin(); iter!=quads.end(); ++iter)
2130  {
2131  this->removeQuad(*iter);
2132  }
2133  UASSERT(_quads.empty());
2134 }
2135 
2136 static const float frustum_vertices[] = {
2137  0.0f, 0.0f, 0.0f,
2138  1.0f, 1.0f, 1.0f,
2139  1.0f, -1.0f, 1.0f,
2140  -1.0f, -1.0f, 1.0f,
2141  -1.0f, 1.0f, 1.0f};
2142 
2143 static const int frustum_indices[] = {
2144  1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4};
2145 
2146 void CloudViewer::addOrUpdateFrustum(
2147  const std::string & id,
2148  const Transform & pose,
2149  const Transform & localTransform,
2150  double scale,
2151  const QColor & color,
2152  float fovX,
2153  float fovY)
2154 {
2155  if(id.empty())
2156  {
2157  UERROR("id should not be empty!");
2158  return;
2159  }
2160 
2161 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
2162  this->removeFrustum(id);
2163 #endif
2164 
2165  if(!pose.isNull())
2166  {
2167  if(_frustums.find(id)==_frustums.end())
2168  {
2169  _frustums.insert(id, Transform());
2170 
2171  int frustumSize = sizeof(frustum_vertices)/sizeof(float);
2172  UASSERT(frustumSize>0 && frustumSize % 3 == 0);
2173  frustumSize/=3;
2174  pcl::PointCloud<pcl::PointXYZ> frustumPoints;
2175  frustumPoints.resize(frustumSize);
2176  float scaleX = tan((fovX>0?fovX:1.1)/2.0f) * scale;
2177  float scaleY = tan((fovY>0?fovY:0.85)/2.0f) * scale;
2178  float scaleZ = scale;
2179  QColor c = Qt::gray;
2180  if(color.isValid())
2181  {
2182  c = color;
2183  }
2184  Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
2185 
2186 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
2187  Eigen::Affine3f t = (pose*localTransform).toEigen3f();
2188 #else
2189  Eigen::Affine3f t = (localTransform).toEigen3f();
2190 #endif
2191  for(int i=0; i<frustumSize; ++i)
2192  {
2193  frustumPoints[i].x = frustum_vertices[i*3]*scaleX;
2194  frustumPoints[i].y = frustum_vertices[i*3+1]*scaleY;
2195  frustumPoints[i].z = frustum_vertices[i*3+2]*scaleZ;
2196  frustumPoints[i] = pcl::transformPoint(frustumPoints[i], t);
2197  }
2198 
2199  pcl::PolygonMesh mesh;
2200  pcl::Vertices vertices;
2201  vertices.vertices.resize(sizeof(frustum_indices)/sizeof(int));
2202  for(unsigned int i=0; i<vertices.vertices.size(); ++i)
2203  {
2204  vertices.vertices[i] = frustum_indices[i];
2205  }
2206  pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
2207  mesh.polygons.push_back(vertices);
2208  _visualizer->addPolylineFromPolygonMesh(mesh, id, 2);
2209  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
2210  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
2211  }
2212 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2213  if(!this->updateFrustumPose(id, pose))
2214  {
2215  UERROR("Failed updating pose of frustum %s!?", id.c_str());
2216  }
2217 #endif
2218  }
2219  else
2220  {
2221  removeFrustum(id);
2222  }
2223 }
2224 
2225 bool CloudViewer::updateFrustumPose(
2226  const std::string & id,
2227  const Transform & pose)
2228 {
2229 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2230  QMap<std::string, Transform>::iterator iter=_frustums.find(id);
2231  if(iter != _frustums.end() && !pose.isNull())
2232  {
2233  if(iter.value() == pose)
2234  {
2235  // same pose, just return
2236  return true;
2237  }
2238 
2239  pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
2240 
2241  vtkActor* actor;
2242 
2243  if (am_it == _visualizer->getShapeActorMap()->end ())
2244  return (false);
2245  else
2246  actor = vtkActor::SafeDownCast (am_it->second);
2247 
2248  if (!actor)
2249  return (false);
2250 
2251  vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
2252 
2253  pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
2254 
2255  actor->SetUserMatrix (matrix);
2256  actor->Modified ();
2257 
2258  iter.value() = pose;
2259 
2260  return true;
2261  }
2262 #else
2263  UERROR("updateFrustumPose() cannot be used with PCL<1.7.2. Use addOrUpdateFrustum() instead.");
2264 #endif
2265  return false;
2266 }
2267 
2268 void CloudViewer::removeFrustum(const std::string & id)
2269 {
2270  if(id.empty())
2271  {
2272  UERROR("id should not be empty!");
2273  return;
2274  }
2275 
2276  if(_frustums.find(id) != _frustums.end())
2277  {
2278  _visualizer->removeShape(id);
2279  _frustums.remove(id);
2280  }
2281 }
2282 
2283 void CloudViewer::removeAllFrustums(bool exceptCameraReference)
2284 {
2285  QMap<std::string, Transform> frustums = _frustums;
2286  for(QMap<std::string, Transform>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
2287  {
2288  if(!exceptCameraReference || !uStrContains(iter.key(), "reference_frustum"))
2289  {
2290  this->removeFrustum(iter.key());
2291  }
2292  }
2293  UASSERT(exceptCameraReference || _frustums.empty());
2294 }
2295 
2296 void CloudViewer::addOrUpdateGraph(
2297  const std::string & id,
2298  const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
2299  const QColor & color)
2300 {
2301  if(id.empty())
2302  {
2303  UERROR("id should not be empty!");
2304  return;
2305  }
2306 
2307  removeGraph(id);
2308 
2309  if(graph->size())
2310  {
2311  _graphes.insert(id);
2312 
2313  pcl::PolygonMesh mesh;
2314  pcl::Vertices vertices;
2315  vertices.vertices.resize(graph->size());
2316  for(unsigned int i=0; i<vertices.vertices.size(); ++i)
2317  {
2318  vertices.vertices[i] = i;
2319  }
2320  pcl::toPCLPointCloud2(*graph, mesh.cloud);
2321  mesh.polygons.push_back(vertices);
2322  _visualizer->addPolylineFromPolygonMesh(mesh, id, 2);
2323  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
2324  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, color.alphaF(), id);
2325 
2326  pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
2327  pcl::toPCLPointCloud2(*graph, *binaryCloud);
2328  this->addCloud(id+"_nodes", binaryCloud, Transform::getIdentity(), false, false, false, color, 2);
2329  this->setCloudPointSize(id+"_nodes", 5);
2330  }
2331 }
2332 
2333 void CloudViewer::removeGraph(const std::string & id)
2334 {
2335  if(id.empty())
2336  {
2337  UERROR("id should not be empty!");
2338  return;
2339  }
2340 
2341  if(_graphes.find(id) != _graphes.end())
2342  {
2343  _visualizer->removeShape(id);
2344  _graphes.erase(id);
2345  removeCloud(id+"_nodes");
2346  }
2347 }
2348 
2349 void CloudViewer::removeAllGraphs()
2350 {
2351  std::set<std::string> graphes = _graphes;
2352  for(std::set<std::string>::iterator iter = graphes.begin(); iter!=graphes.end(); ++iter)
2353  {
2354  this->removeGraph(*iter);
2355  }
2356  UASSERT(_graphes.empty());
2357 }
2358 
2359 void CloudViewer::addOrUpdateText(
2360  const std::string & id,
2361  const std::string & text,
2362  const Transform & position,
2363  double scale,
2364  const QColor & color,
2365  bool foreground)
2366 {
2367  if(id.empty())
2368  {
2369  UERROR("id should not be empty!");
2370  return;
2371  }
2372 
2373  removeText(id);
2374 
2375  if(!position.isNull())
2376  {
2377  _texts.insert(id);
2378  _visualizer->addText3D(
2379  text,
2380  pcl::PointXYZ(position.x(), position.y(), position.z()),
2381  scale,
2382  color.redF(),
2383  color.greenF(),
2384  color.blueF(),
2385  id,
2386  foreground?3:2);
2387  }
2388 }
2389 
2390 void CloudViewer::removeText(const std::string & id)
2391 {
2392  if(id.empty())
2393  {
2394  UERROR("id should not be empty!");
2395  return;
2396  }
2397 
2398  if(_texts.find(id) != _texts.end())
2399  {
2400  _visualizer->removeText3D(id);
2401  _texts.erase(id);
2402  }
2403 }
2404 
2405 void CloudViewer::removeAllTexts()
2406 {
2407  std::set<std::string> texts = _texts;
2408  for(std::set<std::string>::iterator iter = texts.begin(); iter!=texts.end(); ++iter)
2409  {
2410  this->removeText(*iter);
2411  }
2412  UASSERT(_texts.empty());
2413 }
2414 
2415 bool CloudViewer::isTrajectoryShown() const
2416 {
2417  return _aShowTrajectory->isChecked();
2418 }
2419 
2420 unsigned int CloudViewer::getTrajectorySize() const
2421 {
2422  return _maxTrajectorySize;
2423 }
2424 
2425 void CloudViewer::setTrajectoryShown(bool shown)
2426 {
2427  _aShowTrajectory->setChecked(shown);
2428 }
2429 
2430 void CloudViewer::setTrajectorySize(unsigned int value)
2431 {
2432  _maxTrajectorySize = value;
2433 }
2434 
2435 void CloudViewer::clearTrajectory()
2436 {
2437  _trajectory->clear();
2438  _visualizer->removeShape("trajectory");
2439  this->refreshView();
2440 }
2441 
2442 bool CloudViewer::isCameraAxisShown() const
2443 {
2444  return _aShowCameraAxis->isChecked();
2445 }
2446 
2447 void CloudViewer::setCameraAxisShown(bool shown)
2448 {
2449  if(!shown)
2450  {
2451  this->removeCoordinate("reference");
2452  }
2453  else
2454  {
2455  this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
2456  }
2457  this->refreshView();
2458  _aShowCameraAxis->setChecked(shown);
2459 }
2460 
2461 double CloudViewer::getCoordinateFrameScale() const
2462 {
2463  return _coordinateFrameScale;
2464 }
2465 
2466 void CloudViewer::setCoordinateFrameScale(double scale)
2467 {
2468  _coordinateFrameScale = std::max(0.1, scale);
2469 }
2470 
2471 bool CloudViewer::isFrustumShown() const
2472 {
2473  return _aShowFrustum->isChecked();
2474 }
2475 
2476 float CloudViewer::getFrustumScale() const
2477 {
2478  return _frustumScale;
2479 }
2480 
2481 QColor CloudViewer::getFrustumColor() const
2482 {
2483  return _frustumColor;
2484 }
2485 
2486 void CloudViewer::setFrustumShown(bool shown)
2487 {
2488  if(!shown)
2489  {
2490  QMap<std::string, Transform> frustumsCopy = _frustums;
2491  for(QMap<std::string, Transform>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
2492  {
2493  if(uStrContains(iter.key(), "reference_frustum"))
2494  {
2495  this->removeFrustum(iter.key());
2496  }
2497  }
2498  std::set<std::string> linesCopy = _lines;
2499  for(std::set<std::string>::iterator iter=linesCopy.begin(); iter!=linesCopy.end(); ++iter)
2500  {
2501  if(uStrContains(*iter, "reference_frustum_line"))
2502  {
2503  this->removeLine(*iter);
2504  }
2505  }
2506  this->refreshView();
2507  }
2508  _aShowFrustum->setChecked(shown);
2509 }
2510 
2511 void CloudViewer::setFrustumScale(float value)
2512 {
2513  _frustumScale = value;
2514 }
2515 
2516 void CloudViewer::setFrustumColor(QColor value)
2517 {
2518  if(!value.isValid())
2519  {
2520  value = Qt::gray;
2521  }
2522  for(QMap<std::string, Transform>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
2523  {
2524  _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), iter.key());
2525  }
2526  this->refreshView();
2527  _frustumColor = value;
2528 }
2529 
2530 void CloudViewer::resetCamera()
2531 {
2532  _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
2533  if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull())
2534  {
2535  // reset relative to last current pose
2536  cv::Point3f pt = util3d::transformPoint(cv::Point3f(_lastPose.x(), _lastPose.y(), _lastPose.z()), ( _lastPose.rotation()*Transform(-1, 0, 0)).translation());
2537  if(_aCameraOrtho->isChecked())
2538  {
2539  this->setCameraPosition(
2540  _lastPose.x(), _lastPose.y(), _lastPose.z()+5,
2541  _lastPose.x(), _lastPose.y(), _lastPose.z(),
2542  1, 0, 0);
2543  }
2544  else if(_aLockViewZ->isChecked())
2545  {
2546  this->setCameraPosition(
2547  pt.x, pt.y, pt.z,
2548  _lastPose.x(), _lastPose.y(), _lastPose.z(),
2549  0, 0, 1);
2550  }
2551  else
2552  {
2553  this->setCameraPosition(
2554  pt.x, pt.y, pt.z,
2555  _lastPose.x(), _lastPose.y(), _lastPose.z(),
2556  _lastPose.r31(), _lastPose.r32(), _lastPose.r33());
2557  }
2558  }
2559  else if(_aCameraOrtho->isChecked())
2560  {
2561  this->setCameraPosition(
2562  0, 0, 5,
2563  0, 0, 0,
2564  1, 0, 0);
2565  }
2566  else
2567  {
2568  this->setCameraPosition(
2569  -1, 0, 0,
2570  0, 0, 0,
2571  0, 0, 1);
2572  }
2573 }
2574 
2575 void CloudViewer::removeAllClouds()
2576 {
2577  QMap<std::string, Transform> addedClouds = _addedClouds;
2578  QList<std::string> ids = _addedClouds.keys();
2579  for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
2580  {
2581  removeCloud(*iter);
2582  }
2583  UASSERT(_addedClouds.empty());
2584  UASSERT(_locators.empty());
2585 }
2586 
2587 
2588 bool CloudViewer::removeCloud(const std::string & id)
2589 {
2590  bool success = _visualizer->removePointCloud(id);
2591 #if VTK_MAJOR_VERSION >= 7
2592  if(!success)
2593  {
2594  success = _visualizer->removeShape(id);
2595  }
2596 #endif
2597  _visualizer->removePointCloud(id+"-normals");
2598  _addedClouds.remove(id); // remove after visualizer
2599  _addedClouds.remove(id+"-normals");
2600  _locators.erase(id);
2601  return success;
2602 }
2603 
2604 bool CloudViewer::getPose(const std::string & id, Transform & pose)
2605 {
2606  if(_addedClouds.contains(id))
2607  {
2608  pose = _addedClouds.value(id);
2609  return true;
2610  }
2611  return false;
2612 }
2613 
2614 Transform CloudViewer::getTargetPose() const
2615 {
2616  if(_lastPose.isNull())
2617  {
2618  return Transform::getIdentity();
2619  }
2620  return _lastPose;
2621 }
2622 
2623 std::string CloudViewer::getIdByActor(vtkProp * actor) const
2624 {
2625  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
2626  for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
2627  {
2628  if(iter->second.actor.GetPointer() == actor)
2629  {
2630  return iter->first;
2631  }
2632  }
2633 
2634 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2635  // getShapeActorMap() not available in version < 1.7.2
2636  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
2637  for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
2638  {
2639  if(iter->second.GetPointer() == actor)
2640  {
2641  std::string id = iter->first;
2642  while(id.size() && id.at(id.size()-1) == '*')
2643  {
2644  id.erase(id.size()-1);
2645  }
2646 
2647  return id;
2648  }
2649  }
2650 #endif
2651  return std::string();
2652 }
2653 
2654 QColor CloudViewer::getColor(const std::string & id)
2655 {
2656  QColor color;
2657  pcl::visualization::CloudActorMap::iterator iter = _visualizer->getCloudActorMap()->find(id);
2658  if(iter != _visualizer->getCloudActorMap()->end())
2659  {
2660  double r,g,b,a;
2661  iter->second.actor->GetProperty()->GetColor(r,g,b);
2662  a = iter->second.actor->GetProperty()->GetOpacity();
2663  color.setRgbF(r, g, b, a);
2664  }
2665 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2666  // getShapeActorMap() not available in version < 1.7.2
2667  else
2668  {
2669  std::string idLayer1 = id+"*";
2670  std::string idLayer2 = id+"**";
2671  pcl::visualization::ShapeActorMap::iterator iter = _visualizer->getShapeActorMap()->find(id);
2672  if(iter == _visualizer->getShapeActorMap()->end())
2673  {
2674  iter = _visualizer->getShapeActorMap()->find(idLayer1);
2675  if(iter == _visualizer->getShapeActorMap()->end())
2676  {
2677  iter = _visualizer->getShapeActorMap()->find(idLayer2);
2678  }
2679  }
2680  if(iter != _visualizer->getShapeActorMap()->end())
2681  {
2682  vtkActor * actor = vtkActor::SafeDownCast(iter->second);
2683  if(actor)
2684  {
2685  double r,g,b,a;
2686  actor->GetProperty()->GetColor(r,g,b);
2687  a = actor->GetProperty()->GetOpacity();
2688  color.setRgbF(r, g, b, a);
2689  }
2690  }
2691  }
2692 #endif
2693  return color;
2694 }
2695 
2696 void CloudViewer::setColor(const std::string & id, const QColor & color)
2697 {
2698  pcl::visualization::CloudActorMap::iterator iter = _visualizer->getCloudActorMap()->find(id);
2699  if(iter != _visualizer->getCloudActorMap()->end())
2700  {
2701  iter->second.actor->GetProperty()->SetColor(color.redF(),color.greenF(),color.blueF());
2702  iter->second.actor->GetProperty()->SetOpacity(color.alphaF());
2703  }
2704 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2705  // getShapeActorMap() not available in version < 1.7.2
2706  else
2707  {
2708  std::string idLayer1 = id+"*";
2709  std::string idLayer2 = id+"**";
2710  pcl::visualization::ShapeActorMap::iterator iter = _visualizer->getShapeActorMap()->find(id);
2711  if(iter == _visualizer->getShapeActorMap()->end())
2712  {
2713  iter = _visualizer->getShapeActorMap()->find(idLayer1);
2714  if(iter == _visualizer->getShapeActorMap()->end())
2715  {
2716  iter = _visualizer->getShapeActorMap()->find(idLayer2);
2717  }
2718  }
2719  if(iter != _visualizer->getShapeActorMap()->end())
2720  {
2721  vtkActor * actor = vtkActor::SafeDownCast(iter->second);
2722  if(actor)
2723  {
2724  actor->GetProperty()->SetColor(color.redF(),color.greenF(),color.blueF());
2725  actor->GetProperty()->SetOpacity(color.alphaF());
2726  }
2727  }
2728  }
2729 #endif
2730 }
2731 
2732 void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
2733 {
2734  _aBackfaceCulling->setChecked(enabled);
2735  _frontfaceCulling = frontfaceCulling;
2736 
2737  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
2738  for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
2739  {
2740  iter->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
2741  iter->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
2742  }
2743 #if VTK_MAJOR_VERSION >= 7
2744  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
2745  for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
2746  {
2747  vtkActor* actor = vtkActor::SafeDownCast (iter->second);
2748  if(actor)
2749  {
2750  actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
2751  actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
2752  }
2753  }
2754 #endif
2755  this->refreshView();
2756 }
2757 
2758 void CloudViewer::setPolygonPicking(bool enabled)
2759 {
2760  _aPolygonPicking->setChecked(enabled);
2761 
2762  if(!_aPolygonPicking->isChecked())
2763  {
2764  vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
2765  pp->SetTolerance (pp->GetTolerance());
2766  #if VTK_MAJOR_VERSION > 8
2767  this->interactor()->SetPicker (pp);
2768 #else
2769  this->GetInteractor()->SetPicker (pp);
2770 #endif
2771  setMouseTracking(false);
2772  }
2773  else
2774  {
2775  vtkSmartPointer<CloudViewerCellPicker> pp = vtkSmartPointer<CloudViewerCellPicker>::New ();
2776  pp->SetTolerance (pp->GetTolerance());
2777 #if VTK_MAJOR_VERSION > 8
2778  this->interactor()->SetPicker (pp);
2779 #else
2780  this->GetInteractor()->SetPicker (pp);
2781 #endif
2782  setMouseTracking(true);
2783  }
2784 }
2785 
2786 
2787 
2788 void CloudViewer::setRenderingRate(double rate)
2789 {
2790  _renderingRate = rate;
2791  _visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(_renderingRate);
2792 }
2793 
2794 void CloudViewer::setEDLShading(bool on)
2795 {
2796 #if VTK_MAJOR_VERSION >= 7
2797  _aSetEDLShading->setChecked(on);
2798  _visualizer->getRendererCollection()->InitTraversal ();
2799  vtkRenderer* renderer = NULL;
2800  renderer = _visualizer->getRendererCollection()->GetNextItem ();
2801  renderer = _visualizer->getRendererCollection()->GetNextItem (); // Get Layer 1
2802  UASSERT(renderer);
2803 
2804  vtkOpenGLRenderer* glrenderer = vtkOpenGLRenderer::SafeDownCast(renderer);
2805  UASSERT(glrenderer);
2806  if(on)
2807  {
2808  // EDL shader
2809  vtkSmartPointer<vtkRenderStepsPass> basicPasses = vtkSmartPointer<vtkRenderStepsPass>::New ();
2810  vtkSmartPointer<vtkEDLShading> edl = vtkSmartPointer<vtkEDLShading>::New ();
2811  edl->SetDelegatePass(basicPasses);
2812  glrenderer->SetPass(edl);
2813  }
2814  else if(glrenderer->GetPass())
2815  {
2816  glrenderer->GetPass()->ReleaseGraphicsResources(NULL);
2817  glrenderer->SetPass(NULL);
2818  }
2819 
2820  this->refreshView();
2821 #else
2822  if(on)
2823  {
2824  UERROR("RTAB-Map must be built with VTK>=7 to enable EDL shading!");
2825  }
2826 #endif
2827 }
2828 
2829 void CloudViewer::setLighting(bool on)
2830 {
2831  _aSetLighting->setChecked(on);
2832  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
2833  for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
2834  {
2835  iter->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
2836  }
2837 #if VTK_MAJOR_VERSION >= 7
2838  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
2839  for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
2840  {
2841  vtkActor* actor = vtkActor::SafeDownCast (iter->second);
2842  if(actor && _addedClouds.contains(iter->first))
2843  {
2844  actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
2845  }
2846  }
2847 #endif
2848  this->refreshView();
2849 }
2850 
2851 void CloudViewer::setShading(bool on)
2852 {
2853  _aSetFlatShading->setChecked(on);
2854  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
2855  for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
2856  {
2857  iter->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); // VTK_FLAT - VTK_GOURAUD - VTK_PHONG
2858  }
2859 #if VTK_MAJOR_VERSION >= 7
2860  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
2861  for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
2862  {
2863  vtkActor* actor = vtkActor::SafeDownCast (iter->second);
2864  if(actor && _addedClouds.contains(iter->first))
2865  {
2866  actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); // VTK_FLAT - VTK_GOURAUD - VTK_PHONG
2867  }
2868  }
2869 #endif
2870  this->refreshView();
2871 }
2872 
2873 void CloudViewer::setEdgeVisibility(bool visible)
2874 {
2875  _aSetEdgeVisibility->setChecked(visible);
2876  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
2877  for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
2878  {
2879  iter->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
2880  }
2881 #if VTK_MAJOR_VERSION >= 7
2882  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
2883  for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
2884  {
2885  vtkActor* actor = vtkActor::SafeDownCast (iter->second);
2886  if(actor && _addedClouds.contains(iter->first))
2887  {
2888  actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
2889  }
2890  }
2891 #endif
2892  this->refreshView();
2893 }
2894 
2895 void CloudViewer::setInteractorLayer(int layer)
2896 {
2897  _visualizer->getRendererCollection()->InitTraversal ();
2898  vtkRenderer* renderer = NULL;
2899  int i =0;
2900  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
2901  {
2902  if(i==layer)
2903  {
2904  _visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
2905  _visualizer->getInteractorStyle()->SetCurrentRenderer(renderer);
2906  return;
2907  }
2908  ++i;
2909  }
2910  UWARN("Could not set layer %d to interactor (layers=%d).", layer, _visualizer->getRendererCollection()->GetNumberOfItems());
2911 }
2912 
2913 void CloudViewer::getCameraPosition(
2914  float & x, float & y, float & z,
2915  float & focalX, float & focalY, float & focalZ,
2916  float & upX, float & upY, float & upZ) const
2917 {
2918  std::vector<pcl::visualization::Camera> cameras;
2919  _visualizer->getCameras(cameras);
2920  if(cameras.size())
2921  {
2922  x = cameras.begin()->pos[0];
2923  y = cameras.begin()->pos[1];
2924  z = cameras.begin()->pos[2];
2925  focalX = cameras.begin()->focal[0];
2926  focalY = cameras.begin()->focal[1];
2927  focalZ = cameras.begin()->focal[2];
2928  upX = cameras.begin()->view[0];
2929  upY = cameras.begin()->view[1];
2930  upZ = cameras.begin()->view[2];
2931  }
2932  else
2933  {
2934  UERROR("No camera set!?");
2935  }
2936 }
2937 
2938 void CloudViewer::setCameraPosition(
2939  float x, float y, float z,
2940  float focalX, float focalY, float focalZ,
2941  float upX, float upY, float upZ)
2942 {
2943  vtkRenderer* renderer = NULL;
2944  double boundingBox[6] = {1, -1, 1, -1, 1, -1};
2945 
2946  // compute global bounding box
2947  _visualizer->getRendererCollection()->InitTraversal ();
2948  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
2949  {
2950  vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
2951  cam->SetPosition (x, y, z);
2952  cam->SetFocalPoint (focalX, focalY, focalZ);
2953  cam->SetViewUp (upX, upY, upZ);
2954 
2955  double BB[6];
2956  renderer->ComputeVisiblePropBounds(BB);
2957  for (int i = 0; i < 6; i++) {
2958  if (i % 2 == 0) {
2959  // Even Index is Min
2960  if (BB[i] < boundingBox[i]) {
2961  boundingBox[i] = BB[i];
2962  }
2963  } else {
2964  // Odd Index is Max
2965  if (BB[i] > boundingBox[i]) {
2966  boundingBox[i] = BB[i];
2967  }
2968  }
2969  }
2970  }
2971 
2972  _visualizer->getRendererCollection()->InitTraversal ();
2973  while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
2974  {
2975  renderer->ResetCameraClippingRange(boundingBox);
2976  }
2977 }
2978 
2979 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
2980 {
2981  if(!pose.isNull())
2982  {
2983  Eigen::Affine3f m = pose.toEigen3f();
2984  Eigen::Vector3f pos = m.translation();
2985 
2986  _trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2]));
2987  if(_maxTrajectorySize>0)
2988  {
2989  while(_trajectory->size() > _maxTrajectorySize)
2990  {
2991  _trajectory->erase(_trajectory->begin());
2992  }
2993  }
2994  if(_aShowTrajectory->isChecked())
2995  {
2996  _visualizer->removeShape("trajectory");
2997  pcl::PolygonMesh mesh;
2998  pcl::Vertices vertices;
2999  vertices.vertices.resize(_trajectory->size());
3000  for(unsigned int i=0; i<vertices.vertices.size(); ++i)
3001  {
3002  vertices.vertices[i] = i;
3003  }
3004  pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
3005  mesh.polygons.push_back(vertices);
3006  _visualizer->addPolylineFromPolygonMesh(mesh, "trajectory", 2);
3007  }
3008 
3009  if(pose != _lastPose || _lastPose.isNull())
3010  {
3011  if(_lastPose.isNull())
3012  {
3013  _lastPose.setIdentity();
3014  }
3015 
3016  std::vector<pcl::visualization::Camera> cameras;
3017  _visualizer->getCameras(cameras);
3018 
3019  if(_aLockCamera->isChecked() || _aCameraOrtho->isChecked())
3020  {
3021  //update camera position
3022  Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
3023  cameras.front().pos[0] += diff[0];
3024  cameras.front().pos[1] += diff[1];
3025  cameras.front().pos[2] += diff[2];
3026  cameras.front().focal[0] += diff[0];
3027  cameras.front().focal[1] += diff[1];
3028  cameras.front().focal[2] += diff[2];
3029  }
3030  else if(_aFollowCamera->isChecked())
3031  {
3032  Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
3033  cameras.front().focal[1] - cameras.front().pos[1],
3034  cameras.front().focal[2] - cameras.front().pos[2]).normalized();
3035  Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3036  Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
3037  Eigen::Vector3f xAxis = yAxis.cross(zAxis);
3038  Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
3039  yAxis[0], yAxis[1], yAxis[2],0,
3040  zAxis[0], zAxis[1], zAxis[2],0);
3041 
3042  PR.normalizeRotation();
3043 
3044  Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
3045  PR[4], PR[5], PR[6], cameras.front().pos[1],
3046  PR[8], PR[9], PR[10], cameras.front().pos[2]);
3047  Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
3048  PR[4], PR[5], PR[6], cameras.front().focal[1],
3049  PR[8], PR[9], PR[10], cameras.front().focal[2]);
3050  Transform N = pose;
3051  Transform O = _lastPose;
3052  Transform O2N = O.inverse()*N;
3053  Transform F2O = F.inverse()*O;
3054  Transform T = F2O * O2N * F2O.inverse();
3055  Transform Fp = F * T;
3056  Transform P2F = P.inverse()*F;
3057  Transform Pp = P * P2F * T * P2F.inverse();
3058 
3059  cameras.front().pos[0] = Pp.x();
3060  cameras.front().pos[1] = Pp.y();
3061  cameras.front().pos[2] = Pp.z();
3062  cameras.front().focal[0] = Fp.x();
3063  cameras.front().focal[1] = Fp.y();
3064  cameras.front().focal[2] = Fp.z();
3065  //FIXME: the view up is not set properly...
3066  cameras.front().view[0] = _aLockViewZ->isChecked()?0:Fp[8];
3067  cameras.front().view[1] = _aLockViewZ->isChecked()?0:Fp[9];
3068  cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10];
3069  }
3070 
3071  if(_aShowCameraAxis->isChecked())
3072  {
3073 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
3074  if(_coordinates.find("reference") != _coordinates.end())
3075  {
3076  this->updateCoordinatePose("reference", pose);
3077  }
3078  else
3079 #endif
3080  {
3081  this->addOrUpdateCoordinate("reference", pose, 0.2);
3082  }
3083  }
3084 
3085  this->setCameraPosition(
3086  cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
3087  cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
3088  cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3089  }
3090  }
3091 
3092  _lastPose = pose;
3093 }
3094 
3095 void CloudViewer::updateCameraFrustum(const Transform & pose, const StereoCameraModel & model)
3096 {
3097  std::vector<CameraModel> models;
3098  models.push_back(model.left());
3099  CameraModel right = model.right();
3100  if(!model.left().localTransform().isNull())
3101  {
3102  right.setLocalTransform(model.left().localTransform() * Transform(model.baseline(), 0, 0, 0, 0, 0));
3103  }
3104  models.push_back(right);
3105  updateCameraFrustums(pose, models);
3106 }
3107 
3108 void CloudViewer::updateCameraFrustum(const Transform & pose, const CameraModel & model)
3109 {
3110  std::vector<CameraModel> models;
3111  models.push_back(model);
3112  updateCameraFrustums(pose, models);
3113 }
3114 
3115 void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector<CameraModel> & models)
3116 {
3117  if(!pose.isNull())
3118  {
3119  if(_aShowFrustum->isChecked())
3120  {
3121  Transform baseToCamera;
3122  for(unsigned int i=0; i<models.size(); ++i)
3123  {
3124  baseToCamera = Transform::getIdentity();
3125  if(!models[i].localTransform().isNull() && !models[i].localTransform().isIdentity())
3126  {
3127  baseToCamera = models[i].localTransform();
3128  }
3129  std::string id = uFormat("reference_frustum_%d", i);
3130  this->removeFrustum(id);
3131  this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor, models[i].fovX(), models[i].fovY());
3132  if(!baseToCamera.isIdentity())
3133  {
3134  this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);
3135  }
3136  }
3137  }
3138  }
3139 }
3140 void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector<StereoCameraModel> & stereoModels)
3141 {
3142  std::vector<CameraModel> models;
3143  for(size_t i=0; i<stereoModels.size(); ++i)
3144  {
3145  models.push_back(stereoModels[i].left());
3146  CameraModel right = stereoModels[i].right();
3147  if(!stereoModels[i].left().localTransform().isNull())
3148  {
3149  right.setLocalTransform(stereoModels[i].left().localTransform() * Transform(stereoModels[i].baseline(), 0, 0, 0, 0, 0));
3150  }
3151  models.push_back(right);
3152  updateCameraFrustums(pose, models);
3153  }
3154 }
3155 
3156 const QColor & CloudViewer::getDefaultBackgroundColor() const
3157 {
3158  return _defaultBgColor;
3159 }
3160 
3161 void CloudViewer::setDefaultBackgroundColor(const QColor & color)
3162 {
3163  if(_currentBgColor == _defaultBgColor)
3164  {
3165  setBackgroundColor(color);
3166  }
3167  _defaultBgColor = color;
3168 }
3169 
3170 const QColor & CloudViewer::getBackgroundColor() const
3171 {
3172  return _currentBgColor;
3173 }
3174 
3175 void CloudViewer::setBackgroundColor(const QColor & color)
3176 {
3177  _currentBgColor = color;
3178  _visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
3179 }
3180 
3181 void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
3182 {
3183  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
3184  pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
3185  if(iter != cloudActorMap->end())
3186  {
3187  iter->second.actor->SetVisibility(isVisible?1:0);
3188 
3189  iter = cloudActorMap->find(id+"-normals");
3190  if(iter != cloudActorMap->end())
3191  {
3192  iter->second.actor->SetVisibility(isVisible&&_aShowNormals->isChecked()?1:0);
3193  }
3194  }
3195  else
3196  {
3197 #if VTK_MAJOR_VERSION >= 7
3198  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
3199  pcl::visualization::ShapeActorMap::iterator iter = shapeActorMap->find(id);
3200  if(iter != shapeActorMap->end())
3201  {
3202  vtkActor* actor = vtkActor::SafeDownCast (iter->second);
3203  if(actor)
3204  {
3205  actor->SetVisibility(isVisible?1:0);
3206  return;
3207  }
3208  }
3209 #endif
3210  UERROR("Cannot find actor named \"%s\".", id.c_str());
3211  }
3212 }
3213 
3214 bool CloudViewer::getCloudVisibility(const std::string & id)
3215 {
3216  pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
3217  pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
3218  if(iter != cloudActorMap->end())
3219  {
3220  return iter->second.actor->GetVisibility() != 0;
3221  }
3222  else
3223  {
3224 #if VTK_MAJOR_VERSION >= 7
3225  pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
3226  pcl::visualization::ShapeActorMap::iterator iter = shapeActorMap->find(id);
3227  if(iter != shapeActorMap->end())
3228  {
3229  vtkActor* actor = vtkActor::SafeDownCast (iter->second);
3230  if(actor)
3231  {
3232  return actor->GetVisibility() != 0;
3233  }
3234  }
3235 #endif
3236  UERROR("Cannot find actor named \"%s\".", id.c_str());
3237  }
3238  return false;
3239 }
3240 
3241 void CloudViewer::setCloudColorIndex(const std::string & id, int index)
3242 {
3243  if(index>0)
3244  {
3245  _visualizer->updateColorHandlerIndex(id, index-1);
3246  }
3247 }
3248 
3249 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
3250 {
3251  double lastOpacity;
3252  if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id))
3253  {
3254  if(lastOpacity != opacity)
3255  {
3256  _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
3257  }
3258  }
3259 #if VTK_MAJOR_VERSION >= 7
3260  else
3261  {
3262  pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
3263  if (am_it != _visualizer->getShapeActorMap()->end ())
3264  {
3265  vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
3266  if(actor)
3267  {
3268  actor->GetProperty ()->SetOpacity (opacity);
3269  actor->Modified ();
3270  }
3271  }
3272  }
3273 #endif
3274 }
3275 
3276 void CloudViewer::setCloudPointSize(const std::string & id, int size)
3277 {
3278  double lastSize;
3279  if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id))
3280  {
3281  if((int)lastSize != size)
3282  {
3283  _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
3284  }
3285  }
3286 }
3287 
3288 void CloudViewer::setCameraTargetLocked(bool enabled)
3289 {
3290  _aLockCamera->setChecked(enabled);
3291 }
3292 
3293 void CloudViewer::setCameraTargetFollow(bool enabled)
3294 {
3295  _aFollowCamera->setChecked(enabled);
3296 }
3297 
3298 void CloudViewer::setCameraFree()
3299 {
3300  _aLockCamera->setChecked(false);
3301  _aFollowCamera->setChecked(false);
3302 }
3303 
3304 void CloudViewer::setCameraLockZ(bool enabled)
3305 {
3306  _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
3307  _aLockViewZ->setChecked(enabled);
3308 }
3309 void CloudViewer::setCameraOrtho(bool enabled)
3310 {
3311  _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
3312 #if VTK_MAJOR_VERSION > 8
3313  CloudViewerInteractorStyle * interactor = CloudViewerInteractorStyle::SafeDownCast(this->interactor()->GetInteractorStyle());
3314 #else
3315  CloudViewerInteractorStyle * interactor = CloudViewerInteractorStyle::SafeDownCast(this->GetInteractor()->GetInteractorStyle());
3316 #endif
3317  if(interactor)
3318  {
3319  interactor->setOrthoMode(enabled);
3320  this->refreshView();
3321  }
3322  _aCameraOrtho->setChecked(enabled);
3323 }
3324 bool CloudViewer::isCameraTargetLocked() const
3325 {
3326  return _aLockCamera->isChecked();
3327 }
3328 bool CloudViewer::isCameraTargetFollow() const
3329 {
3330  return _aFollowCamera->isChecked();
3331 }
3332 bool CloudViewer::isCameraFree() const
3333 {
3334  return !_aFollowCamera->isChecked() && !_aLockCamera->isChecked();
3335 }
3336 bool CloudViewer::isCameraLockZ() const
3337 {
3338  return _aLockViewZ->isChecked();
3339 }
3340 bool CloudViewer::isCameraOrtho() const
3341 {
3342  return _aCameraOrtho->isChecked();
3343 }
3344 bool CloudViewer::isBackfaceCulling() const
3345 {
3346  return _aBackfaceCulling->isChecked();
3347 }
3348 bool CloudViewer::isFrontfaceCulling() const
3349 {
3350  return _frontfaceCulling;
3351 }
3352 bool CloudViewer::isPolygonPicking() const
3353 {
3354  return _aPolygonPicking->isChecked();
3355 }
3356 bool CloudViewer::isEDLShadingOn() const
3357 {
3358  return _aSetEDLShading->isChecked();
3359 }
3360 bool CloudViewer::isLightingOn() const
3361 {
3362  return _aSetLighting->isChecked();
3363 }
3364 bool CloudViewer::isShadingOn() const
3365 {
3366  return _aSetFlatShading->isChecked();
3367 }
3368 bool CloudViewer::isEdgeVisible() const
3369 {
3370  return _aSetEdgeVisibility->isChecked();
3371 }
3372 double CloudViewer::getRenderingRate() const
3373 {
3374  return _renderingRate;
3375 }
3376 
3377 void CloudViewer::setGridShown(bool shown)
3378 {
3379  _aShowGrid->setChecked(shown);
3380  if(shown)
3381  {
3382  this->addGrid();
3383  }
3384  else
3385  {
3386  this->removeGrid();
3387  }
3388 }
3389 bool CloudViewer::isGridShown() const
3390 {
3391  return _aShowGrid->isChecked();
3392 }
3393 unsigned int CloudViewer::getGridCellCount() const
3394 {
3395  return _gridCellCount;
3396 }
3397 float CloudViewer::getGridCellSize() const
3398 {
3399  return _gridCellSize;
3400 }
3401 void CloudViewer::setGridCellCount(unsigned int count)
3402 {
3403  if(count > 0)
3404  {
3405  _gridCellCount = count;
3406  if(_aShowGrid->isChecked())
3407  {
3408  this->removeGrid();
3409  this->addGrid();
3410  }
3411  }
3412  else
3413  {
3414  UERROR("Cannot set grid cell count < 1, count=%d", count);
3415  }
3416 }
3417 void CloudViewer::setGridCellSize(float size)
3418 {
3419  if(size > 0)
3420  {
3421  _gridCellSize = size;
3422  if(_aShowGrid->isChecked())
3423  {
3424  this->removeGrid();
3425  this->addGrid();
3426  }
3427  }
3428  else
3429  {
3430  UERROR("Cannot set grid cell size <= 0, value=%f", size);
3431  }
3432 }
3433 void CloudViewer::addGrid()
3434 {
3435  if(_gridLines.empty())
3436  {
3437  float cellSize = _gridCellSize;
3438  int cellCount = _gridCellCount;
3439  double r=0.5;
3440  double g=0.5;
3441  double b=0.5;
3442  int id = 0;
3443  float min = -float(cellCount/2) * cellSize;
3444  float max = float(cellCount/2) * cellSize;
3445  std::string name;
3446  for(float i=min; i<=max; i += cellSize)
3447  {
3448  //over x
3449  name = uFormat("line%d", ++id);
3450  _visualizer->addLine(
3451  pcl::PointXYZ(i, min, 0.0f),
3452  pcl::PointXYZ(i, max, 0.0f),
3453  r, g, b, name, 2);
3454  _gridLines.push_back(name);
3455  //over y or z
3456  name = uFormat("line%d", ++id);
3457  _visualizer->addLine(
3458  pcl::PointXYZ(min, i, 0),
3459  pcl::PointXYZ(max, i, 0),
3460  r, g, b, name, 2);
3461  _gridLines.push_back(name);
3462  }
3463  // this will update clipping planes
3464  std::vector<pcl::visualization::Camera> cameras;
3465  _visualizer->getCameras(cameras);
3466  this->setCameraPosition(
3467  cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
3468  cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
3469  cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3470  }
3471 }
3472 
3473 void CloudViewer::removeGrid()
3474 {
3475  for(std::list<std::string>::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter)
3476  {
3477  _visualizer->removeShape(*iter);
3478  }
3479  _gridLines.clear();
3480 }
3481 
3482 void CloudViewer::setNormalsShown(bool shown)
3483 {
3484  _aShowNormals->setChecked(shown);
3485  QList<std::string> ids = _addedClouds.keys();
3486  for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
3487  {
3488  std::string idNormals = *iter + "-normals";
3489  if(_addedClouds.find(idNormals) != _addedClouds.end())
3490  {
3491  this->setCloudVisibility(idNormals, this->getCloudVisibility(*iter) && shown);
3492  }
3493  }
3494 }
3495 bool CloudViewer::isNormalsShown() const
3496 {
3497  return _aShowNormals->isChecked();
3498 }
3499 int CloudViewer::getNormalsStep() const
3500 {
3501  return _normalsStep;
3502 }
3503 float CloudViewer::getNormalsScale() const
3504 {
3505  return _normalsScale;
3506 }
3507 void CloudViewer::setNormalsStep(int step)
3508 {
3509  if(step > 0)
3510  {
3511  _normalsStep = step;
3512  }
3513  else
3514  {
3515  UERROR("Cannot set normals step <= 0, step=%d", step);
3516  }
3517 }
3518 void CloudViewer::setNormalsScale(float scale)
3519 {
3520  if(scale > 0)
3521  {
3522  _normalsScale= scale;
3523  }
3524  else
3525  {
3526  UERROR("Cannot set normals scale <= 0, value=%f", scale);
3527  }
3528 }
3529 
3530 bool CloudViewer::isIntensityRedColormap() const
3531 {
3532  return _aSetIntensityRedColormap->isChecked();
3533 }
3534 bool CloudViewer::isIntensityRainbowColormap() const
3535 {
3536  return _aSetIntensityRainbowColormap->isChecked();
3537 }
3538 float CloudViewer::getIntensityMax() const
3539 {
3540  return _intensityAbsMax;
3541 }
3542 
3543 void CloudViewer::setIntensityRedColormap(bool on)
3544 {
3545  _aSetIntensityRedColormap->setChecked(on);
3546  if(on)
3547  {
3548  _aSetIntensityRainbowColormap->setChecked(false);
3549  }
3550 }
3551 void CloudViewer::setIntensityRainbowColormap(bool on)
3552 {
3553  _aSetIntensityRainbowColormap->setChecked(on);
3554  if(on)
3555  {
3556  _aSetIntensityRedColormap->setChecked(false);
3557  }
3558 }
3559 void CloudViewer::setIntensityMax(float value)
3560 {
3561  if(value >= 0.0f)
3562  {
3563  _intensityAbsMax = value;
3564  }
3565  else
3566  {
3567  UERROR("Cannot set normals scale < 0, value=%f", value);
3568  }
3569 }
3570 
3571 void CloudViewer::buildPickingLocator(bool enable)
3572 {
3573  _buildLocator = enable;
3574 }
3575 
3576 Eigen::Vector3f rotatePointAroundAxe(
3577  const Eigen::Vector3f & point,
3578  const Eigen::Vector3f & axis,
3579  float angle)
3580 {
3581  Eigen::Vector3f direction = point;
3582  Eigen::Vector3f zAxis = axis;
3583  float dotProdZ = zAxis.dot(direction);
3584  Eigen::Vector3f ptOnZaxis = zAxis * dotProdZ;
3585  direction -= ptOnZaxis;
3586  Eigen::Vector3f xAxis = direction.normalized();
3587  Eigen::Vector3f yAxis = zAxis.cross(xAxis);
3588 
3589  Eigen::Matrix3f newFrame;
3590  newFrame << xAxis[0], yAxis[0], zAxis[0],
3591  xAxis[1], yAxis[1], zAxis[1],
3592  xAxis[2], yAxis[2], zAxis[2];
3593 
3594  // transform to axe frame
3595  // transpose=inverse for orthogonal matrices
3596  Eigen::Vector3f newDirection = newFrame.transpose() * direction;
3597 
3598  // rotate about z
3599  float cosTheta = cos(angle);
3600  float sinTheta = sin(angle);
3601  float magnitude = newDirection.norm();
3602  newDirection[0] = ( magnitude * cosTheta );
3603  newDirection[1] = ( magnitude * sinTheta );
3604 
3605  // transform back to global frame
3606  direction = newFrame * newDirection;
3607 
3608  return direction + ptOnZaxis;
3609 }
3610 
3611 void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
3612  if(event->key() == Qt::Key_Up ||
3613  event->key() == Qt::Key_Down ||
3614  event->key() == Qt::Key_Left ||
3615  event->key() == Qt::Key_Right)
3616  {
3617  _keysPressed -= (Qt::Key)event->key();
3618  }
3619  else
3620  {
3621  PCLQVTKWidget::keyPressEvent(event);
3622  }
3623 }
3624 
3625 void CloudViewer::keyPressEvent(QKeyEvent * event)
3626 {
3627  if(event->key() == Qt::Key_Up ||
3628  event->key() == Qt::Key_Down ||
3629  event->key() == Qt::Key_Left ||
3630  event->key() == Qt::Key_Right)
3631  {
3632  _keysPressed += (Qt::Key)event->key();
3633 
3634  std::vector<pcl::visualization::Camera> cameras;
3635  _visualizer->getCameras(cameras);
3636 
3637  //update camera position
3638  Eigen::Vector3f pos(cameras.front().pos[0], cameras.front().pos[1], _aLockViewZ->isChecked()?0:cameras.front().pos[2]);
3639  Eigen::Vector3f focal(cameras.front().focal[0], cameras.front().focal[1], _aLockViewZ->isChecked()?0:cameras.front().focal[2]);
3640  Eigen::Vector3f viewUp(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3641  Eigen::Vector3f cummulatedDir(0,0,0);
3642  Eigen::Vector3f cummulatedFocalDir(0,0,0);
3643  float step = 0.2f;
3644  float stepRot = 0.02f; // radian
3645  if(_keysPressed.contains(Qt::Key_Up))
3646  {
3647  Eigen::Vector3f dir;
3648  if(event->modifiers() & Qt::ShiftModifier)
3649  {
3650  dir = viewUp * step;// up
3651  }
3652  else
3653  {
3654  dir = (focal-pos).normalized() * step; // forward
3655  }
3656  cummulatedDir += dir;
3657  }
3658  if(_keysPressed.contains(Qt::Key_Down))
3659  {
3660  Eigen::Vector3f dir;
3661  if(event->modifiers() & Qt::ShiftModifier)
3662  {
3663  dir = viewUp * -step;// down
3664  }
3665  else
3666  {
3667  dir = (focal-pos).normalized() * -step; // backward
3668  }
3669  cummulatedDir += dir;
3670  }
3671  if(_keysPressed.contains(Qt::Key_Right))
3672  {
3673  if(event->modifiers() & Qt::ShiftModifier)
3674  {
3675  // rotate right
3676  Eigen::Vector3f point = (focal-pos);
3677  Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, -stepRot);
3678  Eigen::Vector3f diff = newPoint - point;
3679  cummulatedFocalDir += diff;
3680  }
3681  else
3682  {
3683  Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * step; // strafing right
3684  cummulatedDir += dir;
3685  }
3686  }
3687  if(_keysPressed.contains(Qt::Key_Left))
3688  {
3689  if(event->modifiers() & Qt::ShiftModifier)
3690  {
3691  // rotate left
3692  Eigen::Vector3f point = (focal-pos);
3693  Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, stepRot);
3694  Eigen::Vector3f diff = newPoint - point;
3695  cummulatedFocalDir += diff;
3696  }
3697  else
3698  {
3699  Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * -step; // strafing left
3700  cummulatedDir += dir;
3701  }
3702  }
3703 
3704  cameras.front().pos[0] += cummulatedDir[0];
3705  cameras.front().pos[1] += cummulatedDir[1];
3706  cameras.front().pos[2] += cummulatedDir[2];
3707  cameras.front().focal[0] += cummulatedDir[0] + cummulatedFocalDir[0];
3708  cameras.front().focal[1] += cummulatedDir[1] + cummulatedFocalDir[1];
3709  cameras.front().focal[2] += cummulatedDir[2] + cummulatedFocalDir[2];
3710  this->setCameraPosition(
3711  cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
3712  cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
3713  cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3714 
3715  Q_EMIT configChanged();
3716  }
3717  else
3718  {
3719  PCLQVTKWidget::keyPressEvent(event);
3720  }
3721 }
3722 
3723 void CloudViewer::mousePressEvent(QMouseEvent * event)
3724 {
3725  if(event->button() == Qt::RightButton)
3726  {
3727  event->accept();
3728  }
3729  else
3730  {
3731  PCLQVTKWidget::mousePressEvent(event);
3732  }
3733 }
3734 
3735 void CloudViewer::mouseMoveEvent(QMouseEvent * event)
3736 {
3737  PCLQVTKWidget::mouseMoveEvent(event);
3738 
3739  std::vector<pcl::visualization::Camera> cameras;
3740  _visualizer->getCameras(cameras);
3741 
3742  // camera view up z locked?
3743  if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
3744  {
3745  cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
3746 
3747  if( _lastCameraOrientation!=cv::Vec3d(0,0,0) &&
3748  _lastCameraPose!=cv::Vec3d(0,0,0) &&
3749  (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) &&
3750  uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1])))
3751  {
3752  cameras.front().pos[0] = _lastCameraPose[0];
3753  cameras.front().pos[1] = _lastCameraPose[1];
3754  cameras.front().pos[2] = _lastCameraPose[2];
3755  }
3756  else if(newCameraOrientation != cv::Vec3d(0,0,0))
3757  {
3758  _lastCameraOrientation = newCameraOrientation;
3759  _lastCameraPose = cv::Vec3d(cameras.front().pos);
3760  }
3761  else
3762  {
3763  if(cameras.front().view[2] == 0)
3764  {
3765  cameras.front().pos[0] -= 0.00001*cameras.front().view[0];
3766  cameras.front().pos[1] -= 0.00001*cameras.front().view[1];
3767  }
3768  else
3769  {
3770  cameras.front().pos[0] -= 0.00001;
3771  }
3772  }
3773  cameras.front().view[0] = 0;
3774  cameras.front().view[1] = 0;
3775  cameras.front().view[2] = 1;
3776  }
3777 
3778  this->setCameraPosition(
3779  cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
3780  cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
3781  cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3782 
3783  Q_EMIT configChanged();
3784 }
3785 
3786 void CloudViewer::wheelEvent(QWheelEvent * event)
3787 {
3788  PCLQVTKWidget::wheelEvent(event);
3789 
3790  std::vector<pcl::visualization::Camera> cameras;
3791  _visualizer->getCameras(cameras);
3792 
3793  if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
3794  {
3795  _lastCameraPose = cv::Vec3d(cameras.front().pos);
3796  }
3797 
3798  this->setCameraPosition(
3799  cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
3800  cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
3801  cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
3802 
3803  Q_EMIT configChanged();
3804 }
3805 
3806 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
3807 {
3808  QAction * a = _menu->exec(event->globalPos());
3809  if(a)
3810  {
3811  handleAction(a);
3812  Q_EMIT configChanged();
3813  }
3814 }
3815 
3816 void CloudViewer::handleAction(QAction * a)
3817 {
3818  if(a == _aSetTrajectorySize)
3819  {
3820  bool ok;
3821  int value = QInputDialog::getInt(this, tr("Set trajectory size"), tr("Size (0=infinite)"), _maxTrajectorySize, 0, 10000, 10, &ok);
3822  if(ok)
3823  {
3824  _maxTrajectorySize = value;
3825  }
3826  }
3827  else if(a == _aClearTrajectory)
3828  {
3829  this->clearTrajectory();
3830  }
3831  else if(a == _aShowCameraAxis)
3832  {
3833  this->setCameraAxisShown(a->isChecked());
3834  }
3835  else if(a == _aSetFrameScale)
3836  {
3837  bool ok;
3838  double value = QInputDialog::getDouble(this, tr("Set frame scale"), tr("Scale"), _coordinateFrameScale, 0.1, 999.0, 1, &ok);
3839  if(ok)
3840  {
3841  this->setCoordinateFrameScale(value);
3842  }
3843  }
3844  else if(a == _aShowFrustum)
3845  {
3846  this->setFrustumShown(a->isChecked());
3847  }
3848  else if(a == _aSetFrustumScale)
3849  {
3850  bool ok;
3851  double value = QInputDialog::getDouble(this, tr("Set frustum scale"), tr("Scale"), _frustumScale, 0.0, 999.0, 1, &ok);
3852  if(ok)
3853  {
3854  this->setFrustumScale(value);
3855  }
3856  }
3857  else if(a == _aSetFrustumColor)
3858  {
3859  QColor value = QColorDialog::getColor(_frustumColor, this);
3860  if(value.isValid())
3861  {
3862  this->setFrustumColor(value);
3863  }
3864  }
3865  else if(a == _aResetCamera)
3866  {
3867  this->resetCamera();
3868  }
3869  else if(a == _aShowGrid)
3870  {
3871  if(_aShowGrid->isChecked())
3872  {
3873  this->addGrid();
3874  }
3875  else
3876  {
3877  this->removeGrid();
3878  }
3879 
3880  this->refreshView();
3881  }
3882  else if(a == _aSetGridCellCount)
3883  {
3884  bool ok;
3885  int value = QInputDialog::getInt(this, tr("Set grid cell count"), tr("Count"), _gridCellCount, 1, 10000, 10, &ok);
3886  if(ok)
3887  {
3888  this->setGridCellCount(value);
3889  }
3890  }
3891  else if(a == _aSetGridCellSize)
3892  {
3893  bool ok;
3894  double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 1000, 2, &ok);
3895  if(ok)
3896  {
3897  this->setGridCellSize(value);
3898  }
3899  }
3900  else if(a == _aShowNormals)
3901  {
3902  this->setNormalsShown(_aShowNormals->isChecked());
3903  this->refreshView();
3904  }
3905  else if(a == _aSetNormalsStep)
3906  {
3907  bool ok;
3908  int value = QInputDialog::getInt(this, tr("Set normals step"), tr("Step"), _normalsStep, 1, 10000, 1, &ok);
3909  if(ok)
3910  {
3911  this->setNormalsStep(value);
3912  }
3913  }
3914  else if(a == _aSetNormalsScale)
3915  {
3916  bool ok;
3917  double value = QInputDialog::getDouble(this, tr("Set normals scale"), tr("Scale (m)"), _normalsScale, 0.01, 10, 2, &ok);
3918  if(ok)
3919  {
3920  this->setNormalsScale(value);
3921  }
3922  }
3923  else if(a == _aSetIntensityMaximum)
3924  {
3925  bool ok;
3926  double value = QInputDialog::getDouble(this, tr("Set maximum absolute intensity"), tr("Intensity (0=auto)"), _intensityAbsMax, 0.0, 99999, 2, &ok);
3927  if(ok)
3928  {
3929  this->setIntensityMax(value);
3930  }
3931  }
3932  else if(a == _aSetIntensityRedColormap)
3933  {
3934  this->setIntensityRedColormap(_aSetIntensityRedColormap->isChecked());
3935  }
3936  else if(a == _aSetIntensityRainbowColormap)
3937  {
3938  this->setIntensityRainbowColormap(_aSetIntensityRainbowColormap->isChecked());
3939  }
3940  else if(a == _aSetBackgroundColor)
3941  {
3942  QColor color = this->getDefaultBackgroundColor();
3943  color = QColorDialog::getColor(color, this);
3944  if(color.isValid())
3945  {
3946  this->setDefaultBackgroundColor(color);
3947  this->refreshView();
3948  }
3949  }
3950  else if(a == _aSetRenderingRate)
3951  {
3952  bool ok;
3953  double value = QInputDialog::getDouble(this, tr("Rendering rate"), tr("Rate (hz)"), _renderingRate, 0, 60, 0, &ok);
3954  if(ok)
3955  {
3956  this->setRenderingRate(value);
3957  }
3958  }
3959  else if(a == _aLockViewZ)
3960  {
3961  if(_aLockViewZ->isChecked())
3962  {
3963  this->refreshView();
3964  }
3965  }
3966  else if(a == _aCameraOrtho)
3967  {
3968  this->setCameraOrtho(_aCameraOrtho->isChecked());
3969  }
3970  else if(a == _aSetEDLShading)
3971  {
3972  this->setEDLShading(_aSetEDLShading->isChecked());
3973  }
3974  else if(a == _aSetLighting)
3975  {
3976  this->setLighting(_aSetLighting->isChecked());
3977  }
3978  else if(a == _aSetFlatShading)
3979  {
3980  this->setShading(_aSetFlatShading->isChecked());
3981  }
3982  else if(a == _aSetEdgeVisibility)
3983  {
3984  this->setEdgeVisibility(_aSetEdgeVisibility->isChecked());
3985  }
3986  else if(a == _aBackfaceCulling)
3987  {
3988  this->setBackfaceCulling(_aBackfaceCulling->isChecked(), _frontfaceCulling);
3989  }
3990  else if(a == _aPolygonPicking)
3991  {
3992  this->setPolygonPicking(_aPolygonPicking->isChecked());
3993  }
3994 }
3995 
3996 } /* namespace rtabmap */
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3304
rtabmap::CloudViewer::removeQuad
void removeQuad(const std::string &id)
Definition: CloudViewer.cpp:2111
rtabmap::CloudViewer::removeAllCoordinates
void removeAllCoordinates(const std::string &prefix="")
Definition: CloudViewer.cpp:1797
rtabmap::CloudViewer::addOrUpdateCube
void addOrUpdateCube(const std::string &id, const Transform &pose, float width, float height, float depth, const QColor &color, bool wireframe=false, bool foreground=false)
Definition: CloudViewer.cpp:1932
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[ *:*] noreverse nowriteback set trange[ *:*] noreverse nowriteback set urange[ *:*] noreverse nowriteback set vrange[ *:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
rtabmap::CloudViewer::setColor
void setColor(const std::string &id, const QColor &color)
Definition: CloudViewer.cpp:2696
pcl
Definition: CameraOpenni.h:47
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2979
update
def update(text)
rtabmap::CloudViewer::_lastPose
Transform _lastPose
Definition: CloudViewer.h:486
rtabmap::CloudViewer::_aResetCamera
QAction * _aResetCamera
Definition: CloudViewer.h:436
glm::tan
GLM_FUNC_DECL genType tan(genType const &angle)
rtabmap::CloudViewer::addOrUpdateQuad
void addOrUpdateQuad(const std::string &id, const Transform &pose, float width, float height, const QColor &color, bool foreground=false)
Definition: CloudViewer.cpp:1994
rtabmap::CloudViewer::keyReleaseEvent
virtual void keyReleaseEvent(QKeyEvent *event)
Definition: CloudViewer.cpp:3611
rtabmap::CloudViewer::_aSetFrustumColor
QAction * _aSetFrustumColor
Definition: CloudViewer.h:446
rtabmap::CloudViewerInteractorStyle::setOrthoMode
void setOrthoMode(bool enabled)
Definition: CloudViewerInteractorStyle.cpp:87
name
rtabmap::CloudViewer::isFrustumShown
bool isFrustumShown() const
Definition: CloudViewer.cpp:2471
rtabmap::CloudViewer::_menu
QMenu * _menu
Definition: CloudViewer.h:464
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::util2d::HSVtoRGB
void RTABMAP_CORE_EXPORT HSVtoRGB(float *r, float *g, float *b, float h, float s, float v)
Definition: util2d.cpp:2047
rtabmap::CloudViewer::removeLine
void removeLine(const std::string &id)
Definition: CloudViewer.cpp:1851
rtabmap::CloudViewer::_aSetIntensityMaximum
QAction * _aSetIntensityMaximum
Definition: CloudViewer.h:455
rtabmap::CloudViewer::setEDLShading
void setEDLShading(bool on)
Definition: CloudViewer.cpp:2794
rtabmap::CloudViewer::setLighting
void setLighting(bool on)
Definition: CloudViewer.cpp:2829
rtabmap::CloudViewer::removeOccupancyGridMap
void removeOccupancyGridMap()
Definition: CloudViewer.cpp:1595
rtabmap::CloudViewer::handleAction
virtual void handleAction(QAction *event)
Definition: CloudViewer.cpp:3816
rtabmap::CloudViewer::wheelEvent
virtual void wheelEvent(QWheelEvent *event)
Definition: CloudViewer.cpp:3786
rtabmap::CloudViewer::_aSetEDLShading
QAction * _aSetEDLShading
Definition: CloudViewer.h:458
rtabmap::CloudViewer::setBackgroundColor
void setBackgroundColor(const QColor &color)
Definition: CloudViewer.cpp:3175
Eigen::Transform
boost::shared_ptr
s
RealScalar s
rtabmap::CloudViewer::addCloudTextureMesh
bool addCloudTextureMesh(const std::string &id, const pcl::TextureMesh::Ptr &textureMesh, const cv::Mat &texture, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:1074
rtabmap::CloudViewer::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: CloudViewer.cpp:415
rtabmap::rotatePointAroundAxe
Eigen::Vector3f rotatePointAroundAxe(const Eigen::Vector3f &point, const Eigen::Vector3f &axis, float angle)
Definition: CloudViewer.cpp:3576
b
Array< int, 3, 1 > b
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
Definition: CloudViewer.cpp:745
glm::axis
GLM_FUNC_DECL detail::tvec3< T, P > axis(detail::tquat< T, P > const &x)
rtabmap::CloudViewer::addTextureMesh
bool addTextureMesh(const pcl::TextureMesh &mesh, const cv::Mat &texture, const std::string &id="texture", int viewport=0)
Definition: CloudViewer.cpp:1341
rtabmap::PointCloudColorHandlerIntensityField::getColor
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const
Obtain the actual color for the input dataset as vtk scalars.
Definition: CloudViewer.cpp:635
rtabmap::CloudViewer::setPolygonPicking
void setPolygonPicking(bool enabled)
Definition: CloudViewer.cpp:2758
rtabmap::CloudViewer::getRenderingRate
double getRenderingRate() const
Definition: CloudViewer.cpp:3372
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3249
rtabmap::PointCloudColorHandlerIntensityField::colormap_
int colormap_
Definition: CloudViewer.cpp:742
CloudViewerCellPicker.h
c
Scalar Scalar * c
tree
rtabmap::CloudViewer::_aSetIntensityRedColormap
QAction * _aSetIntensityRedColormap
Definition: CloudViewer.h:453
rtabmap::CloudViewer::removeAllLines
void removeAllLines()
Definition: CloudViewer.cpp:1866
rtabmap::CloudViewer::~CloudViewer
virtual ~CloudViewer()
Definition: CloudViewer.cpp:253
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:2296
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CloudViewer::isPolygonPicking
bool isPolygonPicking() const
Definition: CloudViewer.cpp:3352
rtabmap::CloudViewer::removeGraph
void removeGraph(const std::string &id)
Definition: CloudViewer.cpp:2333
rtabmap::CloudViewer::_aShowGrid
QAction * _aShowGrid
Definition: CloudViewer.h:447
rtabmap::CloudViewer::removeGrid
void removeGrid()
Definition: CloudViewer.cpp:3473
rtabmap::CloudViewer::updateFrustumPose
bool updateFrustumPose(const std::string &id, const Transform &pose)
Definition: CloudViewer.cpp:2225
rtabmap::CloudViewer::CloudViewer
CloudViewer(QWidget *parent=0, CloudViewerInteractorStyle *style=CloudViewerInteractorStyle::New())
Definition: CloudViewer.cpp:101
rtabmap::CloudViewer::_aSetGridCellSize
QAction * _aSetGridCellSize
Definition: CloudViewer.h:449
rtabmap::CloudViewer::isNormalsShown
bool isNormalsShown() const
Definition: CloudViewer.cpp:3495
tango_gl::vertices
static const float vertices[]
Definition: quad.cpp:39
rtabmap::CloudViewer::isIntensityRedColormap
bool isIntensityRedColormap() const
Definition: CloudViewer.cpp:3530
rtabmap::CloudViewer::getPose
bool getPose(const std::string &id, Transform &pose)
Definition: CloudViewer.cpp:2604
updated
updated
count
Index count
rtabmap::CloudViewer::removeAllTexts
void removeAllTexts()
Definition: CloudViewer.cpp:2405
rtabmap::CloudViewer::setIntensityRedColormap
void setIntensityRedColormap(bool value)
Definition: CloudViewer.cpp:3543
rtabmap::PointCloudColorHandlerIntensityField::maxAbsIntensity_
float maxAbsIntensity_
Definition: CloudViewer.cpp:741
rtabmap::CloudViewer::updateCloudPose
bool updateCloudPose(const std::string &id, const Transform &pose)
Definition: CloudViewer.cpp:547
rtabmap::CloudViewer::setShading
void setShading(bool on)
Definition: CloudViewer.cpp:2851
rtabmap::CloudViewer::isTrajectoryShown
bool isTrajectoryShown() const
Definition: CloudViewer.cpp:2415
vtkSmartPointer
Definition: CloudViewer.h:72
rtabmap::CloudViewer::setGridCellCount
void setGridCellCount(unsigned int count)
Definition: CloudViewer.cpp:3401
rtabmap::CloudViewer::setCameraAxisShown
void setCameraAxisShown(bool shown)
Definition: CloudViewer.cpp:2447
y
Matrix3f y
rtabmap::CloudViewer::_aShowNormals
QAction * _aShowNormals
Definition: CloudViewer.h:450
rtabmap::CloudViewer::getBackgroundColor
const QColor & getBackgroundColor() const
Definition: CloudViewer.cpp:3170
rtabmap::CloudViewer::setDefaultBackgroundColor
void setDefaultBackgroundColor(const QColor &color)
Definition: CloudViewer.cpp:3161
rtabmap::CloudViewer::mouseMoveEvent
virtual void mouseMoveEvent(QMouseEvent *event)
Definition: CloudViewer.cpp:3735
rtabmap::CloudViewer::_lastCameraPose
cv::Vec3d _lastCameraPose
Definition: CloudViewer.h:484
rtabmap::CloudViewer::removeAllSpheres
void removeAllSpheres()
Definition: CloudViewer.cpp:1922
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
rtabmap::CloudViewer::setNormalsShown
void setNormalsShown(bool shown)
Definition: CloudViewer.cpp:3482
rtabmap::PointCloudColorHandlerIntensityField::PointCloudConstPtr
PointCloud::ConstPtr PointCloudConstPtr
Definition: CloudViewer.cpp:602
rtabmap::CloudViewer::isBackfaceCulling
bool isBackfaceCulling() const
Definition: CloudViewer.cpp:3344
rtabmap::CloudViewer::removeSphere
void removeSphere(const std::string &id)
Definition: CloudViewer.cpp:1907
rtabmap::CloudViewer::removeAllFrustums
void removeAllFrustums(bool exceptCameraReference=false)
Definition: CloudViewer.cpp:2283
rtabmap::CloudViewer::setIntensityMax
void setIntensityMax(float value)
Definition: CloudViewer.cpp:3559
UTimer.h
rtabmap::CloudViewer::_aSetRenderingRate
QAction * _aSetRenderingRate
Definition: CloudViewer.h:457
rtabmap::PointCloudColorHandlerIntensityField::PointCloud
pcl::visualization::PointCloudColorHandler< pcl::PCLPointCloud2 >::PointCloud PointCloud
Definition: CloudViewer.cpp:600
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:1113
rtabmap::CloudViewer::setTrajectorySize
void setTrajectorySize(unsigned int value)
Definition: CloudViewer.cpp:2430
cam
rtabmap::SensorCaptureThread * cam
Definition: tools/DataRecorder/main.cpp:58
rtabmap::CloudViewer::addCloudMesh
bool addCloudMesh(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::vector< pcl::Vertices > &polygons, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:923
rtabmap::CloudViewer::getCloudVisibility
bool getCloudVisibility(const std::string &id)
Definition: CloudViewer.cpp:3214
rtabmap::CloudViewer::getDefaultBackgroundColor
const QColor & getDefaultBackgroundColor() const
Definition: CloudViewer.cpp:3156
rtabmap::CloudViewer::_renderingRate
double _renderingRate
Definition: CloudViewer.h:492
rtabmap::PointCloudColorHandlerIntensityField::getFieldName
virtual std::string getFieldName() const
Get the name of the field used.
Definition: CloudViewer.cpp:738
UMath.h
Basic mathematics functions.
rtabmap::CloudViewer::getNormalsScale
float getNormalsScale() const
Definition: CloudViewer.cpp:3503
rtabmap::CloudViewer::setRenderingRate
void setRenderingRate(double rate)
Definition: CloudViewer.cpp:2788
rtabmap::CloudViewer::getFrustumColor
QColor getFrustumColor() const
Definition: CloudViewer.cpp:2481
p3
p3
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:391
octomath::Vector3
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::CameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: CameraModel.h:115
rtabmap::CloudViewer::removeText
void removeText(const std::string &id)
Definition: CloudViewer.cpp:2390
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
point
point
rtabmap::CloudViewer::_aSetFrustumScale
QAction * _aSetFrustumScale
Definition: CloudViewer.h:445
Eigen::Transform::matrix
EIGEN_DEVICE_FUNC MatrixType & matrix()
rtabmap::CloudViewer::setCoordinateFrameScale
void setCoordinateFrameScale(double scale)
Definition: CloudViewer.cpp:2466
rtabmap::CloudViewer::_aBackfaceCulling
QAction * _aBackfaceCulling
Definition: CloudViewer.h:462
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::CloudViewer::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: CloudViewer.cpp:480
rtabmap::CloudViewer::_lastCameraOrientation
cv::Vec3d _lastCameraOrientation
Definition: CloudViewer.h:483
rtabmap::CloudViewer::setInteractorLayer
void setInteractorLayer(int layer)
Definition: CloudViewer.cpp:2895
rtabmap::CloudViewer::removeCube
void removeCube(const std::string &id)
Definition: CloudViewer.cpp:1969
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
translation
translation
rtabmap::CloudViewer::addElevationMap
bool addElevationMap(const cv::Mat &map32FC1, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1610
PCLQVTKWidget
QVTKWidget PCLQVTKWidget
Definition: CloudViewer.h:46
rtabmap::CloudViewer::addOrUpdateFrustum
void addOrUpdateFrustum(const std::string &id, const Transform &pose, const Transform &localTransform, double scale, const QColor &color=QColor(), float fovX=1.1, float fovY=0.85)
Definition: CloudViewer.cpp:2146
util3d_transforms.h
rtabmap::CloudViewer::isCameraTargetFollow
bool isCameraTargetFollow() const
Definition: CloudViewer.cpp:3328
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
g
float g
j
std::ptrdiff_t j
rtabmap::CloudViewer::setBackfaceCulling
void setBackfaceCulling(bool enabled, bool frontfaceCulling)
Definition: CloudViewer.cpp:2732
rtabmap::CloudViewer::_aShowTrajectory
QAction * _aShowTrajectory
Definition: CloudViewer.h:439
UConversion.h
Some conversion functions.
rtabmap::CloudViewer::isCameraAxisShown
bool isCameraAxisShown() const
Definition: CloudViewer.cpp:2442
rtabmap::CloudViewer::_aClearTrajectory
QAction * _aClearTrajectory
Definition: CloudViewer.h:441
rtabmap::CloudViewer::getFrustumScale
float getFrustumScale() const
Definition: CloudViewer.cpp:2476
CloudViewer.h
rtabmap::CloudViewer::getNormalsStep
int getNormalsStep() const
Definition: CloudViewer.cpp:3499
rtabmap::CloudViewer::setNormalsScale
void setNormalsScale(float scale)
Definition: CloudViewer.cpp:3518
rtabmap::CloudViewer::updateCameraFrustum
void updateCameraFrustum(const Transform &pose, const StereoCameraModel &model)
Definition: CloudViewer.cpp:3095
rtabmap::CloudViewer::_aSetIntensityRainbowColormap
QAction * _aSetIntensityRainbowColormap
Definition: CloudViewer.h:454
rtabmap::CloudViewer::buildPickingLocator
void buildPickingLocator(bool enable)
Definition: CloudViewer.cpp:3571
rtabmap::CloudViewer::removeCoordinate
void removeCoordinate(const std::string &id)
Definition: CloudViewer.cpp:1777
rtabmap::CloudViewer::removeAllGraphs
void removeAllGraphs()
Definition: CloudViewer.cpp:2349
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1735
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::CloudViewer::_aSetFlatShading
QAction * _aSetFlatShading
Definition: CloudViewer.h:460
p2
p2
cross
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE MatrixBase< Derived >::template cross_product_return_type< OtherDerived >::type cross(const MatrixBase< OtherDerived > &other) const
rtabmap::CloudViewer::updateCoordinatePose
bool updateCoordinatePose(const std::string &id, const Transform &transform)
Definition: CloudViewer.cpp:1761
rtabmap::CloudViewer::_aSetNormalsScale
QAction * _aSetNormalsScale
Definition: CloudViewer.h:452
vtkImageMatSource.h
step
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
Eigen::Quaternionf
Quaternion< float > Quaternionf
glm::orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
rtabmap::CloudViewer::_aSetFrameScale
QAction * _aSetFrameScale
Definition: CloudViewer.h:443
rtabmap::CloudViewer::setCameraTargetFollow
void setCameraTargetFollow(bool enabled=true)
Definition: CloudViewer.cpp:3293
material
Definition: dummy.cpp:36
rtabmap::CloudViewer::addOrUpdateLine
void addOrUpdateLine(const std::string &id, const Transform &from, const Transform &to, const QColor &color, bool arrow=false, bool foreground=false)
Definition: CloudViewer.cpp:1810
rtabmap::CloudViewer::setNormalsStep
void setNormalsStep(int step)
Definition: CloudViewer.cpp:3507
rtabmap::CloudViewer::_aShowFrustum
QAction * _aShowFrustum
Definition: CloudViewer.h:444
UASSERT
#define UASSERT(condition)
rtabmap::CloudViewer::setCameraTargetLocked
void setCameraTargetLocked(bool enabled=true)
Definition: CloudViewer.cpp:3288
rtabmap::CloudViewer::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: CloudViewer.cpp:3625
z
z
rtabmap::CloudViewer::mousePressEvent
virtual void mousePressEvent(QMouseEvent *event)
Definition: CloudViewer.cpp:3723
rtabmap::CloudViewer::setCameraOrtho
void setCameraOrtho(bool enabled=true)
Definition: CloudViewer.cpp:3309
rtabmap::CloudViewer::getTrajectorySize
unsigned int getTrajectorySize() const
Definition: CloudViewer.cpp:2420
rtabmap::CloudViewer::isIntensityRainbowColormap
bool isIntensityRainbowColormap() const
Definition: CloudViewer.cpp:3534
x
x
m
Matrix3f m
iterator::value
object value
p
Point3_ p(2)
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:3241
Eigen::Triplet< double >
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:3115
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::CloudViewer::setCloudVisibility
void setCloudVisibility(const std::string &id, bool isVisible)
Definition: CloudViewer.cpp:3181
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:785
F
Key F(std::uint64_t j)
rtabmap::PointCloudColorHandlerIntensityField::getName
virtual std::string getName() const
Get the name of the class.
Definition: CloudViewer.cpp:734
p1
Vector3f p1
rtabmap::CloudViewer::_aSetLighting
QAction * _aSetLighting
Definition: CloudViewer.h:459
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::CloudViewer::_visualizer
pcl::visualization::PCLVisualizer * _visualizer
Definition: CloudViewer.h:433
rtabmap::CloudViewer::removeFrustum
void removeFrustum(const std::string &id)
Definition: CloudViewer.cpp:2268
graph
FactorGraph< FACTOR > * graph
rtabmap::PointCloudColorHandlerIntensityField
Definition: CloudViewer.cpp:598
rtabmap::CloudViewer::resetCamera
void resetCamera()
Definition: CloudViewer.cpp:2530
rtabmap::CloudViewer::isShadingOn
bool isShadingOn() const
Definition: CloudViewer.cpp:3364
glm::isfinite
GLM_FUNC_DECL bool isfinite(genType const &x)
Test whether or not a scalar or each vector component is a finite value. (From GLM_GTX_compatibility)
rtabmap::CloudViewer::removeOctomap
void removeOctomap()
Definition: CloudViewer.cpp:1324
UWARN
#define UWARN(...)
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2588
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::CloudViewerInteractorStyle::setCloudViewer
void setCloudViewer(CloudViewer *cloudViewer)
Definition: CloudViewerInteractorStyle.h:38
rtabmap::CloudViewer::setCameraPosition
void setCameraPosition(float x, float y, float z, float focalX, float focalY, float focalZ, float upX, float upY, float upZ)
Definition: CloudViewer.cpp:2938
O
Key O(std::uint64_t j)
OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::iterator
leaf_iterator iterator
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::CloudViewer::_aSetEdgeVisibility
QAction * _aSetEdgeVisibility
Definition: CloudViewer.h:461
uMinMax
void uMinMax(const T *v, unsigned int size, T &min, T &max, unsigned int &indexMin, unsigned int &indexMax)
Definition: UMath.h:224
rtabmap::CloudViewer::isFrontfaceCulling
bool isFrontfaceCulling() const
Definition: CloudViewer.cpp:3348
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1530
empty
util2d.h
rtabmap::CloudViewer::createMenu
void createMenu()
Definition: CloudViewer.cpp:285
rtabmap::CloudViewer::_aSetGridCellCount
QAction * _aSetGridCellCount
Definition: CloudViewer.h:448
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:261
rtabmap::CloudViewerInteractorStyle
Definition: CloudViewerInteractorStyle.h:22
octomath::Vector3::y
float & y()
Eigen::Transform::linear
EIGEN_DEVICE_FUNC LinearPart linear()
rtabmap::CloudViewer::_aCameraOrtho
QAction * _aCameraOrtho
Definition: CloudViewer.h:438
rtabmap::CloudViewer::setIntensityRainbowColormap
void setIntensityRainbowColormap(bool value)
Definition: CloudViewer.cpp:3551
N
N
OctoMap.h
PointMatcherSupport::Parametrizable
rtabmap::CloudViewer::_aLockCamera
QAction * _aLockCamera
Definition: CloudViewer.h:434
iter
iterator iter(handle obj)
uSign
int uSign(const T &v)
Definition: UMath.h:300
p0
Vector3f p0
rtabmap::CloudViewer::isCameraFree
bool isCameraFree() const
Definition: CloudViewer.cpp:3332
rtabmap::CloudViewer::_aShowCameraAxis
QAction * _aShowCameraAxis
Definition: CloudViewer.h:442
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
id
id
rtabmap::CloudViewer::getColor
QColor getColor(const std::string &id)
Definition: CloudViewer.cpp:2654
c_str
const char * c_str(Args &&...args)
rtabmap::CloudViewer::setTrajectoryShown
void setTrajectoryShown(bool shown)
Definition: CloudViewer.cpp:2425
UStl.h
Wrappers of STL for convenient functions.
rtabmap::PointCloudColorHandlerIntensityField::PointCloudPtr
PointCloud::Ptr PointCloudPtr
Definition: CloudViewer.cpp:601
diff
diff
rtabmap::frustum_indices
static const int frustum_indices[]
Definition: CloudViewer.cpp:2143
rtabmap::PointCloudColorHandlerIntensityField::ConstPtr
boost::shared_ptr< const PointCloudColorHandlerIntensityField > ConstPtr
Definition: CloudViewer.cpp:606
position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
UDEBUG
#define UDEBUG(...)
transformation
transformation
rtabmap::Transform::normalizeRotation
void normalizeRotation()
Definition: Transform.cpp:316
rtabmap::CloudViewer::isCameraLockZ
bool isCameraLockZ() const
Definition: CloudViewer.cpp:3336
rtabmap::CloudViewer::setGridCellSize
void setGridCellSize(float size)
Definition: CloudViewer.cpp:3417
UTimer
Definition: UTimer.h:46
rtabmap::CloudViewer::removeAllCubes
void removeAllCubes()
Definition: CloudViewer.cpp:1984
rtabmap::CloudViewer::setCameraFree
void setCameraFree()
Definition: CloudViewer.cpp:3298
rtabmap::CloudViewer::removeAllQuads
void removeAllQuads()
Definition: CloudViewer.cpp:2126
rtabmap::CloudViewer::removeAllClouds
void removeAllClouds()
Definition: CloudViewer.cpp:2575
float
float
rtabmap::CloudViewer::removeElevationMap
void removeElevationMap()
Definition: CloudViewer.cpp:1727
rtabmap::CloudViewer::setFrustumColor
void setFrustumColor(QColor value)
Definition: CloudViewer.cpp:2516
rtabmap::CloudViewer::isLightingOn
bool isLightingOn() const
Definition: CloudViewer.cpp:3360
rtabmap::CloudViewer::getGridCellCount
unsigned int getGridCellCount() const
Definition: CloudViewer.cpp:3393
octomap
rtabmap::OctoMap
Definition: global_map/OctoMap.h:175
rtabmap::CloudViewer::getCameraPosition
void getCameraPosition(float &x, float &y, float &z, float &focalX, float &focalY, float &focalZ, float &upX, float &upY, float &upZ) const
Definition: CloudViewer.cpp:2913
rtabmap::CloudViewer::_aSetNormalsStep
QAction * _aSetNormalsStep
Definition: CloudViewer.h:451
rtabmap::CloudViewer::_addedClouds
QMap< std::string, Transform > _addedClouds
Definition: CloudViewer.h:485
Vector2::x
float x
rtabmap::CloudViewer::isCameraOrtho
bool isCameraOrtho() const
Definition: CloudViewer.cpp:3340
rtabmap::CloudViewer::_aSetTrajectorySize
QAction * _aSetTrajectorySize
Definition: CloudViewer.h:440
NULL
#define NULL
false
#define false
Definition: ConvertUTF.c:56
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
rtabmap::CloudViewer::_aPolygonPicking
QAction * _aPolygonPicking
Definition: CloudViewer.h:463
rtabmap::CloudViewer::_aLockViewZ
QAction * _aLockViewZ
Definition: CloudViewer.h:437
rtabmap::CloudViewer::_aSetBackgroundColor
QAction * _aSetBackgroundColor
Definition: CloudViewer.h:456
rtabmap::CloudViewer::setFrustumShown
void setFrustumShown(bool shown)
Definition: CloudViewer.cpp:2486
rtabmap::CloudViewer::isGridShown
bool isGridShown() const
Definition: CloudViewer.cpp:3389
rtabmap::CloudViewer::contextMenuEvent
virtual void contextMenuEvent(QContextMenuEvent *event)
Definition: CloudViewer.cpp:3806
rtabmap::CloudViewer::refreshView
void refreshView()
Definition: CloudViewer.cpp:538
pos
rtabmap::CloudViewer::addOrUpdateText
void addOrUpdateText(const std::string &id, const std::string &text, const Transform &position, double scale, const QColor &color, bool foreground=true)
Definition: CloudViewer.cpp:2359
matrix
Map< const Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(const T *data, int rows, int cols, int stride)
rtabmap::CloudViewer::setEdgeVisibility
void setEdgeVisibility(bool visible)
Definition: CloudViewer.cpp:2873
rtabmap::CloudViewer::setFrustumScale
void setFrustumScale(float value)
Definition: CloudViewer.cpp:2511
t
Point2 t(10, 10)
octomath::Vector3::z
float & z()
rtabmap::PointCloudColorHandlerIntensityField::~PointCloudColorHandlerIntensityField
virtual ~PointCloudColorHandlerIntensityField()
Empty destructor.
Definition: CloudViewer.cpp:622
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
rtabmap::frustum_vertices
static const float frustum_vertices[]
Definition: CloudViewer.cpp:2136
rtabmap::CloudViewer::getGridCellSize
float getGridCellSize() const
Definition: CloudViewer.cpp:3397
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::CloudViewer::getCoordinateFrameScale
double getCoordinateFrameScale() const
Definition: CloudViewer.cpp:2461
rtabmap::CloudViewer::isEDLShadingOn
bool isEDLShadingOn() const
Definition: CloudViewer.cpp:3356
octomath::Vector3::x
float & x()
rtabmap::CloudViewer::getIdByActor
std::string getIdByActor(vtkProp *actor) const
Definition: CloudViewer.cpp:2623
UERROR
#define UERROR(...)
rtabmap::CloudViewer::getIntensityMax
float getIntensityMax() const
Definition: CloudViewer.cpp:3538
trace.model
model
Definition: trace.py:4
rtabmap::CloudViewer::addOrUpdateSphere
void addOrUpdateSphere(const std::string &id, const Transform &pose, float radius, const QColor &color, bool foreground=false)
Definition: CloudViewer.cpp:1876
rtabmap::CloudViewer::addGrid
void addGrid()
Definition: CloudViewer.cpp:3433
rtabmap::CloudViewer::isCameraTargetLocked
bool isCameraTargetLocked() const
Definition: CloudViewer.cpp:3324
value
value
i
int i
rtabmap::CloudViewer::_aFollowCamera
QAction * _aFollowCamera
Definition: CloudViewer.h:435
rtabmap::Transform::getQuaternionf
Eigen::Quaternionf getQuaternionf() const
Definition: Transform.cpp:401
text
text
rtabmap::CloudViewer::setGridShown
void setGridShown(bool shown)
Definition: CloudViewer.cpp:3377
baseline
double baseline
rtabmap::CloudViewer::getTargetPose
Transform getTargetPose() const
Definition: CloudViewer.cpp:2614
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)
rtabmap::CloudViewer::isEdgeVisible
bool isEdgeVisible() const
Definition: CloudViewer.cpp:3368
rtabmap::CloudViewer::clearTrajectory
void clearTrajectory()
Definition: CloudViewer.cpp:2435
rtabmap::PointCloudColorHandlerIntensityField::Ptr
boost::shared_ptr< PointCloudColorHandlerIntensityField > Ptr
Definition: CloudViewer.cpp:605
rtabmap::PointCloudColorHandlerIntensityField::PointCloudColorHandlerIntensityField
PointCloudColorHandlerIntensityField(const PointCloudConstPtr &cloud, float maxAbsIntensity=0.0f, int colorMap=0)
Constructor.
Definition: CloudViewer.cpp:609

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:25