28 #ifndef RTABMAP_CLOUDVIEWER_H_
29 #define RTABMAP_CLOUDVIEWER_H_
31 #include "rtabmap/gui/rtabmap_gui_export.h"
36 #include <vtkVersionMacros.h>
38 #if VTK_MAJOR_VERSION > 8
42 #include <QVTKOpenGLNativeWidget.h>
45 #include <QVTKWidget.h>
48 #include <pcl/pcl_base.h>
49 #include <pcl/point_types.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/PolygonMesh.h>
52 #include <pcl/TextureMesh.h>
54 #include <QtCore/QMap>
55 #include <QtCore/QSet>
56 #include <QtCore/qnamespace.h>
57 #include <QtCore/QSettings>
59 #include <opencv2/opencv.hpp>
62 #include <pcl/PCLPointCloud2.h>
65 namespace visualization {
87 void saveSettings(QSettings & settings,
const QString & group =
"")
const;
88 void loadSettings(QSettings & settings,
const QString & group =
"");
93 const std::string &
id,
97 const std::string &
id,
98 const pcl::PCLPointCloud2Ptr & binaryCloud,
103 const QColor & color = QColor(),
107 const std::string &
id,
108 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
110 const QColor & color = QColor());
113 const std::string &
id,
114 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
116 const QColor & color = QColor());
119 const std::string &
id,
120 const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
122 const QColor & color = QColor());
125 const std::string &
id,
126 const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
128 const QColor & color = QColor());
131 const std::string &
id,
132 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
134 const QColor & color = QColor());
137 const std::string &
id,
138 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
140 const QColor & color = QColor());
143 const std::string &
id,
144 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
145 const std::vector<pcl::Vertices> & polygons,
149 const std::string &
id,
150 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
151 const std::vector<pcl::Vertices> & polygons,
155 const std::string &
id,
156 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
157 const std::vector<pcl::Vertices> & polygons,
161 const std::string &
id,
162 const pcl::PolygonMesh::Ptr & mesh,
166 bool addCloudTextureMesh(
167 const std::string &
id,
168 const pcl::TextureMesh::Ptr & textureMesh,
169 const cv::Mat & texture,
172 bool addOctomap(
const OctoMap * octomap,
unsigned int treeDepth = 0,
bool volumeRepresentation =
true);
173 void removeOctomap();
176 bool addTextureMesh (
177 const pcl::TextureMesh &mesh,
178 const cv::Mat & texture,
179 const std::string &
id =
"texture",
181 bool addOccupancyGridMap(
182 const cv::Mat & map8U,
187 void removeOccupancyGridMap();
189 bool addElevationMap(
190 const cv::Mat & map32FC1,
195 void removeElevationMap();
197 void updateCameraTargetPosition(
200 void updateCameraFrustum(
204 void updateCameraFrustum(
208 void updateCameraFrustums(
210 const std::vector<CameraModel> & models);
212 void updateCameraFrustums(
214 const std::vector<StereoCameraModel> & models);
216 void addOrUpdateCoordinate(
217 const std::string &
id,
220 bool foreground=
false);
221 bool updateCoordinatePose(
222 const std::string &
id,
224 void removeCoordinate(
const std::string &
id);
225 void removeAllCoordinates(
const std::string & prefix =
"");
228 void addOrUpdateLine(
229 const std::string &
id,
232 const QColor & color,
234 bool foreground =
false);
235 void removeLine(
const std::string &
id);
236 void removeAllLines();
239 void addOrUpdateSphere(
240 const std::string &
id,
243 const QColor & color,
244 bool foreground =
false);
245 void removeSphere(
const std::string &
id);
246 void removeAllSpheres();
249 void addOrUpdateCube(
250 const std::string &
id,
255 const QColor & color,
256 bool wireframe =
false,
257 bool foreground =
false);
258 void removeCube(
const std::string &
id);
259 void removeAllCubes();
262 void addOrUpdateQuad(
263 const std::string &
id,
267 const QColor & color,
268 bool foreground =
false);
269 void addOrUpdateQuad(
270 const std::string &
id,
276 const QColor & color,
277 bool foreground =
false);
278 void removeQuad(
const std::string &
id);
279 void removeAllQuads();
282 void addOrUpdateFrustum(
283 const std::string &
id,
287 const QColor & color = QColor(),
290 bool updateFrustumPose(
291 const std::string &
id,
293 void removeFrustum(
const std::string &
id);
294 void removeAllFrustums(
bool exceptCameraReference =
false);
297 void addOrUpdateGraph(
298 const std::string &
id,
299 const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
300 const QColor & color = Qt::gray);
301 void removeGraph(
const std::string &
id);
302 void removeAllGraphs();
304 void addOrUpdateText(
305 const std::string &
id,
306 const std::string & text,
309 const QColor & color,
310 bool foreground =
true);
311 void removeText(
const std::string &
id);
312 void removeAllTexts();
315 bool isTrajectoryShown()
const;
316 unsigned int getTrajectorySize()
const;
317 void setTrajectoryShown(
bool shown);
318 void setTrajectorySize(
unsigned int value);
319 void clearTrajectory();
320 bool isCameraAxisShown()
const;
321 void setCameraAxisShown(
bool shown);
322 double getCoordinateFrameScale()
const;
323 void setCoordinateFrameScale(
double scale);
324 bool isFrustumShown()
const;
325 float getFrustumScale()
const;
326 QColor getFrustumColor()
const;
327 void setFrustumShown(
bool shown);
328 void setFrustumScale(
float value);
329 void setFrustumColor(QColor value);
332 void removeAllClouds();
333 bool removeCloud(
const std::string &
id);
336 bool getCloudVisibility(
const std::string &
id);
338 const QMap<std::string, Transform> &
getAddedClouds()
const {
return _addedClouds;}
339 const QColor & getDefaultBackgroundColor()
const;
340 const QColor & getBackgroundColor()
const;
342 std::string getIdByActor(vtkProp * actor)
const;
343 QColor getColor(
const std::string &
id);
344 void setColor(
const std::string &
id,
const QColor & color);
346 void setBackfaceCulling(
bool enabled,
bool frontfaceCulling);
347 void setPolygonPicking(
bool enabled);
348 void setRenderingRate(
double rate);
349 void setEDLShading(
bool on);
350 void setLighting(
bool on);
351 void setShading(
bool on);
352 void setEdgeVisibility(
bool visible);
353 void setInteractorLayer(
int layer);
355 bool isBackfaceCulling()
const;
356 bool isFrontfaceCulling()
const;
357 bool isPolygonPicking()
const;
358 bool isEDLShadingOn()
const;
359 bool isLightingOn()
const;
360 bool isShadingOn()
const;
361 bool isEdgeVisible()
const;
362 double getRenderingRate()
const;
364 void getCameraPosition(
365 float & x,
float & y,
float & z,
366 float & focalX,
float & focalY,
float & focalZ,
367 float & upX,
float & upY,
float & upZ)
const;
368 bool isCameraTargetLocked()
const;
369 bool isCameraTargetFollow()
const;
370 bool isCameraFree()
const;
371 bool isCameraLockZ()
const;
372 bool isCameraOrtho()
const;
373 bool isGridShown()
const;
374 unsigned int getGridCellCount()
const;
375 float getGridCellSize()
const;
377 void setCameraPosition(
378 float x,
float y,
float z,
379 float focalX,
float focalY,
float focalZ,
380 float upX,
float upY,
float upZ);
381 void setCameraTargetLocked(
bool enabled =
true);
382 void setCameraTargetFollow(
bool enabled =
true);
383 void setCameraFree();
384 void setCameraLockZ(
bool enabled =
true);
385 void setCameraOrtho(
bool enabled =
true);
386 void setGridShown(
bool shown);
387 void setNormalsShown(
bool shown);
388 void setGridCellCount(
unsigned int count);
389 void setGridCellSize(
float size);
390 bool isNormalsShown()
const;
391 int getNormalsStep()
const;
392 float getNormalsScale()
const;
393 void setNormalsStep(
int step);
394 void setNormalsScale(
float scale);
395 bool isIntensityRedColormap()
const;
396 bool isIntensityRainbowColormap()
const;
397 float getIntensityMax()
const;
398 void setIntensityRedColormap(
bool value);
399 void setIntensityRainbowColormap(
bool value);
400 void setIntensityMax(
float value);
401 void buildPickingLocator(
bool enable);
402 const std::map<std::string, vtkSmartPointer<vtkOBBTree> > &
getLocators()
const {
return _locators;}
405 void setDefaultBackgroundColor(
const QColor & color);
406 void setBackgroundColor(
const QColor & color);
407 void setCloudVisibility(
const std::string &
id,
bool isVisible);
408 void setCloudColorIndex(
const std::string &
id,
int index);
409 void setCloudOpacity(
const std::string &
id,
double opacity = 1.0);
410 void setCloudPointSize(
const std::string &
id,
int size);
411 virtual void clear();
414 void configChanged();
417 virtual void keyReleaseEvent(QKeyEvent * event);
418 virtual void keyPressEvent(QKeyEvent * event);
419 virtual void mousePressEvent(QMouseEvent * event);
420 virtual void mouseMoveEvent(QMouseEvent * event);
421 virtual void wheelEvent(QWheelEvent * event);
422 virtual void contextMenuEvent(QContextMenuEvent * event);
423 virtual void handleAction(QAction * event);
425 pcl::visualization::PCLVisualizer *
visualizer() {
return _visualizer;}
482 std::map<std::string, vtkSmartPointer<vtkOBBTree> >
_locators;