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

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