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

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