00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/CloudViewer.h"
00029 #include "rtabmap/gui/CloudViewerCellPicker.h"
00030
00031 #include <rtabmap/core/Version.h>
00032 #include <rtabmap/core/util3d_transforms.h>
00033 #include <rtabmap/utilite/ULogger.h>
00034 #include <rtabmap/utilite/UTimer.h>
00035 #include <rtabmap/utilite/UMath.h>
00036 #include <rtabmap/utilite/UConversion.h>
00037 #include <rtabmap/utilite/UStl.h>
00038 #include <pcl/visualization/pcl_visualizer.h>
00039 #include <pcl/common/transforms.h>
00040 #include <QMenu>
00041 #include <QAction>
00042 #include <QtGui/QContextMenuEvent>
00043 #include <QInputDialog>
00044 #include <QtGui/QWheelEvent>
00045 #include <QtGui/QKeyEvent>
00046 #include <QColorDialog>
00047 #include <QtGui/QVector3D>
00048 #include <QMainWindow>
00049 #include <set>
00050
00051 #include <vtkCamera.h>
00052 #include <vtkRenderWindow.h>
00053 #include <vtkCubeSource.h>
00054 #include <vtkGlyph3D.h>
00055 #include <vtkGlyph3DMapper.h>
00056 #include <vtkSmartVolumeMapper.h>
00057 #include <vtkVolumeProperty.h>
00058 #include <vtkColorTransferFunction.h>
00059 #include <vtkPiecewiseFunction.h>
00060 #include <vtkImageData.h>
00061 #include <vtkLookupTable.h>
00062 #include <vtkTextureUnitManager.h>
00063 #include <vtkJPEGReader.h>
00064 #include <vtkBMPReader.h>
00065 #include <vtkPNMReader.h>
00066 #include <vtkPNGReader.h>
00067 #include <vtkTIFFReader.h>
00068 #include <vtkOpenGLRenderWindow.h>
00069 #include <vtkPointPicker.h>
00070 #include <vtkTextActor.h>
00071 #include <vtkOBBTree.h>
00072 #include <vtkObjectFactory.h>
00073 #include <vtkQuad.h>
00074 #include <opencv/vtkImageMatSource.h>
00075
00076 #ifdef RTABMAP_OCTOMAP
00077 #include <rtabmap/core/OctoMap.h>
00078 #endif
00079
00080 namespace rtabmap {
00081
00082 CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
00083 QVTKWidget(parent),
00084 _aLockCamera(0),
00085 _aFollowCamera(0),
00086 _aResetCamera(0),
00087 _aLockViewZ(0),
00088 _aCameraOrtho(0),
00089 _aShowTrajectory(0),
00090 _aSetTrajectorySize(0),
00091 _aClearTrajectory(0),
00092 _aShowFrustum(0),
00093 _aSetFrustumScale(0),
00094 _aSetFrustumColor(0),
00095 _aShowGrid(0),
00096 _aSetGridCellCount(0),
00097 _aSetGridCellSize(0),
00098 _aShowNormals(0),
00099 _aSetNormalsStep(0),
00100 _aSetNormalsScale(0),
00101 _aSetBackgroundColor(0),
00102 _aSetRenderingRate(0),
00103 _aSetLighting(0),
00104 _aSetFlatShading(0),
00105 _aSetEdgeVisibility(0),
00106 _aBackfaceCulling(0),
00107 _menu(0),
00108 _trajectory(new pcl::PointCloud<pcl::PointXYZ>),
00109 _maxTrajectorySize(100),
00110 _frustumScale(0.5f),
00111 _frustumColor(Qt::gray),
00112 _gridCellCount(50),
00113 _gridCellSize(1),
00114 _normalsStep(1),
00115 _normalsScale(0.2),
00116 _buildLocator(false),
00117 _lastCameraOrientation(0,0,0),
00118 _lastCameraPose(0,0,0),
00119 _defaultBgColor(Qt::black),
00120 _currentBgColor(Qt::black),
00121 _frontfaceCulling(false),
00122 _renderingRate(5.0),
00123 _octomapActor(0)
00124 {
00125 UDEBUG("");
00126 this->setMinimumSize(200, 200);
00127
00128 int argc = 0;
00129 UASSERT(style!=0);
00130 style->setCloudViewer(this);
00131 _visualizer = new pcl::visualization::PCLVisualizer(
00132 argc,
00133 0,
00134 "PCLVisualizer",
00135 style,
00136 false);
00137
00138 _visualizer->setShowFPS(false);
00139
00140 int viewport;
00141 _visualizer->createViewPort (0,0,1.0, 1.0, viewport);
00142 _visualizer->createViewPort (0,0,1.0, 1.0, viewport);
00143 _visualizer->getRendererCollection()->InitTraversal ();
00144 vtkRenderer* renderer = NULL;
00145 int i =0;
00146 while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
00147 {
00148 renderer->SetLayer(i);
00149 if(i==1)
00150 {
00151 _visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
00152 }
00153 ++i;
00154 }
00155 _visualizer->getRenderWindow()->SetNumberOfLayers(3);
00156
00157 this->SetRenderWindow(_visualizer->getRenderWindow());
00158
00159
00160
00161
00162 this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
00163
00164 vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
00165 UDEBUG("pick tolerance=%f", pp->GetTolerance());
00166 pp->SetTolerance (pp->GetTolerance()/2.0);
00167 this->GetInteractor()->SetPicker (pp);
00168
00169 setRenderingRate(_renderingRate);
00170
00171 _visualizer->setCameraPosition(
00172 -1, 0, 0,
00173 0, 0, 0,
00174 0, 0, 1, 1);
00175 #ifndef _WIN32
00176
00177 this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
00178 #endif
00179
00180
00181 createMenu();
00182
00183 setMouseTracking(false);
00184 }
00185
00186 CloudViewer::~CloudViewer()
00187 {
00188 UDEBUG("");
00189 this->clear();
00190 delete _visualizer;
00191 UDEBUG("");
00192 }
00193
00194 void CloudViewer::clear()
00195 {
00196 this->removeAllClouds();
00197 this->removeAllGraphs();
00198 this->removeAllCoordinates();
00199 this->removeAllLines();
00200 this->removeAllFrustums();
00201 this->removeAllTexts();
00202 this->removeOccupancyGridMap();
00203 this->removeOctomap();
00204
00205 this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
00206 _lastPose.setNull();
00207 if(_aLockCamera->isChecked() || _aFollowCamera->isChecked())
00208 {
00209 resetCamera();
00210 }
00211 this->clearTrajectory();
00212 }
00213
00214 void CloudViewer::createMenu()
00215 {
00216 _aLockCamera = new QAction("Lock target", this);
00217 _aLockCamera->setCheckable(true);
00218 _aLockCamera->setChecked(false);
00219 _aFollowCamera = new QAction("Follow", this);
00220 _aFollowCamera->setCheckable(true);
00221 _aFollowCamera->setChecked(true);
00222 QAction * freeCamera = new QAction("Free", this);
00223 freeCamera->setCheckable(true);
00224 freeCamera->setChecked(false);
00225 _aLockViewZ = new QAction("Lock view Z", this);
00226 _aLockViewZ->setCheckable(true);
00227 _aLockViewZ->setChecked(true);
00228 _aCameraOrtho = new QAction("Ortho mode", this);
00229 _aCameraOrtho->setCheckable(true);
00230 _aCameraOrtho->setChecked(false);
00231 _aResetCamera = new QAction("Reset position", this);
00232 _aShowTrajectory= new QAction("Show trajectory", this);
00233 _aShowTrajectory->setCheckable(true);
00234 _aShowTrajectory->setChecked(true);
00235 _aSetTrajectorySize = new QAction("Set trajectory size...", this);
00236 _aClearTrajectory = new QAction("Clear trajectory", this);
00237 _aShowFrustum= new QAction("Show frustum", this);
00238 _aShowFrustum->setCheckable(true);
00239 _aShowFrustum->setChecked(false);
00240 _aSetFrustumScale = new QAction("Set frustum scale...", this);
00241 _aSetFrustumColor = new QAction("Set frustum color...", this);
00242 _aShowGrid = new QAction("Show grid", this);
00243 _aShowGrid->setCheckable(true);
00244 _aSetGridCellCount = new QAction("Set cell count...", this);
00245 _aSetGridCellSize = new QAction("Set cell size...", this);
00246 _aShowNormals = new QAction("Show normals", this);
00247 _aShowNormals->setCheckable(true);
00248 _aSetNormalsStep = new QAction("Set normals step...", this);
00249 _aSetNormalsScale = new QAction("Set normals scale...", this);
00250 _aSetBackgroundColor = new QAction("Set background color...", this);
00251 _aSetRenderingRate = new QAction("Set rendering rate...", this);
00252 _aSetLighting = new QAction("Lighting", this);
00253 _aSetLighting->setCheckable(true);
00254 _aSetLighting->setChecked(false);
00255 _aSetFlatShading = new QAction("Flat Shading", this);
00256 _aSetFlatShading->setCheckable(true);
00257 _aSetFlatShading->setChecked(false);
00258 _aSetEdgeVisibility = new QAction("Show edges", this);
00259 _aSetEdgeVisibility->setCheckable(true);
00260 _aSetEdgeVisibility->setChecked(false);
00261 _aBackfaceCulling = new QAction("Backface culling", this);
00262 _aBackfaceCulling->setCheckable(true);
00263 _aBackfaceCulling->setChecked(true);
00264 _aPolygonPicking = new QAction("Polygon picking", this);
00265 _aPolygonPicking->setCheckable(true);
00266 _aPolygonPicking->setChecked(false);
00267
00268 QMenu * cameraMenu = new QMenu("Camera", this);
00269 cameraMenu->addAction(_aLockCamera);
00270 cameraMenu->addAction(_aFollowCamera);
00271 cameraMenu->addAction(freeCamera);
00272 cameraMenu->addSeparator();
00273 cameraMenu->addAction(_aLockViewZ);
00274 cameraMenu->addAction(_aCameraOrtho);
00275 cameraMenu->addAction(_aResetCamera);
00276 QActionGroup * group = new QActionGroup(this);
00277 group->addAction(_aLockCamera);
00278 group->addAction(_aFollowCamera);
00279 group->addAction(freeCamera);
00280
00281 QMenu * trajectoryMenu = new QMenu("Trajectory", this);
00282 trajectoryMenu->addAction(_aShowTrajectory);
00283 trajectoryMenu->addAction(_aSetTrajectorySize);
00284 trajectoryMenu->addAction(_aClearTrajectory);
00285
00286 QMenu * frustumMenu = new QMenu("Frustum", this);
00287 frustumMenu->addAction(_aShowFrustum);
00288 frustumMenu->addAction(_aSetFrustumScale);
00289 frustumMenu->addAction(_aSetFrustumColor);
00290
00291 QMenu * gridMenu = new QMenu("Grid", this);
00292 gridMenu->addAction(_aShowGrid);
00293 gridMenu->addAction(_aSetGridCellCount);
00294 gridMenu->addAction(_aSetGridCellSize);
00295
00296 QMenu * normalsMenu = new QMenu("Normals", this);
00297 normalsMenu->addAction(_aShowNormals);
00298 normalsMenu->addAction(_aSetNormalsStep);
00299 normalsMenu->addAction(_aSetNormalsScale);
00300
00301
00302 _menu = new QMenu(this);
00303 _menu->addMenu(cameraMenu);
00304 _menu->addMenu(trajectoryMenu);
00305 _menu->addMenu(frustumMenu);
00306 _menu->addMenu(gridMenu);
00307 _menu->addMenu(normalsMenu);
00308 _menu->addAction(_aSetBackgroundColor);
00309 _menu->addAction(_aSetRenderingRate);
00310 _menu->addAction(_aSetLighting);
00311 _menu->addAction(_aSetFlatShading);
00312 _menu->addAction(_aSetEdgeVisibility);
00313 _menu->addAction(_aBackfaceCulling);
00314 _menu->addAction(_aPolygonPicking);
00315 }
00316
00317 void CloudViewer::saveSettings(QSettings & settings, const QString & group) const
00318 {
00319 if(!group.isEmpty())
00320 {
00321 settings.beginGroup(group);
00322 }
00323
00324 float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
00325 this->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
00326 QVector3D pose(poseX, poseY, poseZ);
00327 QVector3D focal(focalX, focalY, focalZ);
00328 if(!this->isCameraFree())
00329 {
00330
00331 Transform T = this->getTargetPose();
00332 if(this->isCameraTargetLocked())
00333 {
00334 T = Transform(T.x(), T.y(), T.z(), 0,0,0);
00335 }
00336 Transform F(focalX, focalY, focalZ, 0,0,0);
00337 Transform P(poseX, poseY, poseZ, 0,0,0);
00338 Transform newFocal = T.inverse() * F;
00339 Transform newPose = newFocal * F.inverse() * P;
00340 pose = QVector3D(newPose.x(), newPose.y(), newPose.z());
00341 focal = QVector3D(newFocal.x(), newFocal.y(), newFocal.z());
00342 }
00343 settings.setValue("camera_pose", pose);
00344 settings.setValue("camera_focal", focal);
00345 settings.setValue("camera_up", QVector3D(upX, upY, upZ));
00346
00347 settings.setValue("grid", this->isGridShown());
00348 settings.setValue("grid_cell_count", this->getGridCellCount());
00349 settings.setValue("grid_cell_size", (double)this->getGridCellSize());
00350
00351 settings.setValue("normals", this->isNormalsShown());
00352 settings.setValue("normals_step", this->getNormalsStep());
00353 settings.setValue("normals_scale", (double)this->getNormalsScale());
00354
00355 settings.setValue("trajectory_shown", this->isTrajectoryShown());
00356 settings.setValue("trajectory_size", this->getTrajectorySize());
00357
00358 settings.setValue("frustum_shown", this->isFrustumShown());
00359 settings.setValue("frustum_scale", this->getFrustumScale());
00360 settings.setValue("frustum_color", this->getFrustumColor());
00361
00362 settings.setValue("camera_target_locked", this->isCameraTargetLocked());
00363 settings.setValue("camera_target_follow", this->isCameraTargetFollow());
00364 settings.setValue("camera_free", this->isCameraFree());
00365 settings.setValue("camera_lockZ", this->isCameraLockZ());
00366 settings.setValue("camera_ortho", this->isCameraOrtho());
00367
00368 settings.setValue("bg_color", this->getDefaultBackgroundColor());
00369 settings.setValue("rendering_rate", this->getRenderingRate());
00370 if(!group.isEmpty())
00371 {
00372 settings.endGroup();
00373 }
00374 }
00375
00376 void CloudViewer::loadSettings(QSettings & settings, const QString & group)
00377 {
00378 if(!group.isEmpty())
00379 {
00380 settings.beginGroup(group);
00381 }
00382
00383 float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
00384 this->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
00385 QVector3D pose(poseX, poseY, poseZ), focal(focalX, focalY, focalZ), up(upX, upY, upZ);
00386 pose = settings.value("camera_pose", pose).value<QVector3D>();
00387 focal = settings.value("camera_focal", focal).value<QVector3D>();
00388 up = settings.value("camera_up", up).value<QVector3D>();
00389 this->setCameraPosition(pose.x(),pose.y(),pose.z(), focal.x(),focal.y(),focal.z(), up.x(),up.y(),up.z());
00390
00391 this->setGridShown(settings.value("grid", this->isGridShown()).toBool());
00392 this->setGridCellCount(settings.value("grid_cell_count", this->getGridCellCount()).toUInt());
00393 this->setGridCellSize(settings.value("grid_cell_size", this->getGridCellSize()).toFloat());
00394
00395 this->setNormalsShown(settings.value("normals", this->isNormalsShown()).toBool());
00396 this->setNormalsStep(settings.value("normals_step", this->getNormalsStep()).toInt());
00397 this->setNormalsScale(settings.value("normals_scale", this->getNormalsScale()).toFloat());
00398
00399 this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
00400 this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
00401
00402 this->setFrustumShown(settings.value("frustum_shown", this->isFrustumShown()).toBool());
00403 this->setFrustumScale(settings.value("frustum_scale", this->getFrustumScale()).toDouble());
00404 this->setFrustumColor(settings.value("frustum_color", this->getFrustumColor()).value<QColor>());
00405
00406 this->setCameraTargetLocked(settings.value("camera_target_locked", this->isCameraTargetLocked()).toBool());
00407 this->setCameraTargetFollow(settings.value("camera_target_follow", this->isCameraTargetFollow()).toBool());
00408 if(settings.value("camera_free", this->isCameraFree()).toBool())
00409 {
00410 this->setCameraFree();
00411 }
00412 this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());
00413 this->setCameraOrtho(settings.value("camera_ortho", this->isCameraOrtho()).toBool());
00414
00415 this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
00416
00417 this->setRenderingRate(settings.value("rendering_rate", this->getRenderingRate()).toDouble());
00418
00419 if(!group.isEmpty())
00420 {
00421 settings.endGroup();
00422 }
00423
00424 this->update();
00425 }
00426
00427 bool CloudViewer::updateCloudPose(
00428 const std::string & id,
00429 const Transform & pose)
00430 {
00431 if(_addedClouds.contains(id))
00432 {
00433
00434 bool samePose = _addedClouds.find(id).value() == pose;
00435 Eigen::Affine3f posef = pose.toEigen3f();
00436 if(samePose ||
00437 _visualizer->updatePointCloudPose(id, posef))
00438 {
00439 _addedClouds.find(id).value() = pose;
00440 if(!samePose)
00441 {
00442 std::string idNormals = id+"-normals";
00443 if(_addedClouds.find(idNormals)!=_addedClouds.end())
00444 {
00445 _visualizer->updatePointCloudPose(idNormals, posef);
00446 _addedClouds.find(idNormals).value() = pose;
00447 }
00448 }
00449 return true;
00450 }
00451 }
00452 return false;
00453 }
00454
00455 bool CloudViewer::addCloud(
00456 const std::string & id,
00457 const pcl::PCLPointCloud2Ptr & binaryCloud,
00458 const Transform & pose,
00459 bool rgb,
00460 bool hasNormals,
00461 bool hasIntensity,
00462 const QColor & color)
00463 {
00464 int previousColorIndex = -1;
00465 if(_addedClouds.contains(id))
00466 {
00467 previousColorIndex = _visualizer->getColorHandlerIndex(id);
00468 this->removeCloud(id);
00469 }
00470
00471 Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
00472 Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().linear());
00473
00474 if(hasNormals && _aShowNormals->isChecked())
00475 {
00476 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
00477 pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
00478 std::string idNormals = id + "-normals";
00479 if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, 0))
00480 {
00481 _visualizer->updatePointCloudPose(idNormals, pose.toEigen3f());
00482 _addedClouds.insert(idNormals, pose);
00483 }
00484 }
00485
00486
00487 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
00488 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
00489 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1))
00490 {
00491 QColor c = Qt::gray;
00492 if(color.isValid())
00493 {
00494 c = color;
00495 }
00496 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, c.red(), c.green(), c.blue()));
00497 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00498
00499
00500 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
00501 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00502 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
00503 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00504 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
00505 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00506
00507 if(rgb)
00508 {
00509
00510 colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
00511 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00512 }
00513 else if(hasIntensity)
00514 {
00515
00516 colorHandler.reset(new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2>(binaryCloud, "intensity"));
00517 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00518 }
00519 else if(previousColorIndex == 5)
00520 {
00521 previousColorIndex = -1;
00522 }
00523
00524 if(hasNormals)
00525 {
00526
00527 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
00528 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00529 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
00530 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00531 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
00532 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
00533 }
00534 else if(previousColorIndex > 5)
00535 {
00536 previousColorIndex = -1;
00537 }
00538
00539 if(previousColorIndex>=0)
00540 {
00541 _visualizer->updateColorHandlerIndex(id, previousColorIndex);
00542 }
00543 else if(rgb)
00544 {
00545 _visualizer->updateColorHandlerIndex(id, 5);
00546 }
00547 else if(hasNormals)
00548 {
00549 _visualizer->updateColorHandlerIndex(id, hasIntensity?8:7);
00550 }
00551 else if(hasIntensity)
00552 {
00553 _visualizer->updateColorHandlerIndex(id, 5);
00554 }
00555 else if(color.isValid())
00556 {
00557 _visualizer->updateColorHandlerIndex(id, 1);
00558 }
00559
00560 _addedClouds.insert(id, pose);
00561 return true;
00562 }
00563 return false;
00564 }
00565
00566 bool CloudViewer::addCloud(
00567 const std::string & id,
00568 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00569 const Transform & pose,
00570 const QColor & color)
00571 {
00572 pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00573 pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00574 return addCloud(id, binaryCloud, pose, true, true, false, color);
00575 }
00576
00577 bool CloudViewer::addCloud(
00578 const std::string & id,
00579 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00580 const Transform & pose,
00581 const QColor & color)
00582 {
00583 pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00584 pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00585 return addCloud(id, binaryCloud, pose, true, false, false, color);
00586 }
00587
00588 bool CloudViewer::addCloud(
00589 const std::string & id,
00590 const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
00591 const Transform & pose,
00592 const QColor & color)
00593 {
00594 pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00595 pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00596 return addCloud(id, binaryCloud, pose, false, true, true, color);
00597 }
00598
00599 bool CloudViewer::addCloud(
00600 const std::string & id,
00601 const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
00602 const Transform & pose,
00603 const QColor & color)
00604 {
00605 pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00606 pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00607 return addCloud(id, binaryCloud, pose, false, false, true, color);
00608 }
00609
00610 bool CloudViewer::addCloud(
00611 const std::string & id,
00612 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
00613 const Transform & pose,
00614 const QColor & color)
00615 {
00616 pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00617 pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00618 return addCloud(id, binaryCloud, pose, false, true, false, color);
00619 }
00620
00621 bool CloudViewer::addCloud(
00622 const std::string & id,
00623 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
00624 const Transform & pose,
00625 const QColor & color)
00626 {
00627 pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00628 pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00629 return addCloud(id, binaryCloud, pose, false, false, false, color);
00630 }
00631
00632 bool CloudViewer::addCloudMesh(
00633 const std::string & id,
00634 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
00635 const std::vector<pcl::Vertices> & polygons,
00636 const Transform & pose)
00637 {
00638 if(_addedClouds.contains(id))
00639 {
00640 this->removeCloud(id);
00641 }
00642
00643 UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
00644 if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id, 1))
00645 {
00646 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
00647 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
00648 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
00649 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
00650 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
00651 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
00652 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00653 if(_buildLocator)
00654 {
00655 vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
00656 tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
00657 tree->BuildLocator();
00658 _locators.insert(std::make_pair(id, tree));
00659 }
00660 _addedClouds.insert(id, pose);
00661 return true;
00662 }
00663 return false;
00664 }
00665
00666 bool CloudViewer::addCloudMesh(
00667 const std::string & id,
00668 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00669 const std::vector<pcl::Vertices> & polygons,
00670 const Transform & pose)
00671 {
00672 if(_addedClouds.contains(id))
00673 {
00674 this->removeCloud(id);
00675 }
00676
00677 UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
00678 if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id, 1))
00679 {
00680 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
00681 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
00682 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
00683 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
00684 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
00685 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
00686 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00687 if(_buildLocator)
00688 {
00689 vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
00690 tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
00691 tree->BuildLocator();
00692 _locators.insert(std::make_pair(id, tree));
00693 }
00694 _addedClouds.insert(id, pose);
00695 return true;
00696 }
00697 return false;
00698 }
00699
00700 bool CloudViewer::addCloudMesh(
00701 const std::string & id,
00702 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00703 const std::vector<pcl::Vertices> & polygons,
00704 const Transform & pose)
00705 {
00706 if(_addedClouds.contains(id))
00707 {
00708 this->removeCloud(id);
00709 }
00710
00711 UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
00712 if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id, 1))
00713 {
00714 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
00715 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
00716 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
00717 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
00718 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
00719 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
00720 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00721 if(_buildLocator)
00722 {
00723 vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
00724 tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
00725 tree->BuildLocator();
00726 _locators.insert(std::make_pair(id, tree));
00727 }
00728 _addedClouds.insert(id, pose);
00729 return true;
00730 }
00731 return false;
00732 }
00733
00734 bool CloudViewer::addCloudMesh(
00735 const std::string & id,
00736 const pcl::PolygonMesh::Ptr & mesh,
00737 const Transform & pose)
00738 {
00739 if(_addedClouds.contains(id))
00740 {
00741 this->removeCloud(id);
00742 }
00743
00744 UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
00745 if(_visualizer->addPolygonMesh(*mesh, id, 1))
00746 {
00747 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
00748 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
00749 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
00750 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
00751 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
00752 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00753 if(_buildLocator)
00754 {
00755 vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
00756 tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
00757 tree->BuildLocator();
00758 _locators.insert(std::make_pair(id, tree));
00759 }
00760 _addedClouds.insert(id, pose);
00761 return true;
00762 }
00763
00764 return false;
00765 }
00766
00767 bool CloudViewer::addCloudTextureMesh(
00768 const std::string & id,
00769 const pcl::TextureMesh::Ptr & textureMesh,
00770 const cv::Mat & texture,
00771 const Transform & pose)
00772 {
00773 if(_addedClouds.contains(id))
00774 {
00775 this->removeCloud(id);
00776 }
00777
00778 UDEBUG("Adding %s", id.c_str());
00779 if(this->addTextureMesh(*textureMesh, texture, id, 1))
00780 {
00781 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
00782 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
00783 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
00784 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
00785 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
00786 if(!textureMesh->cloud.is_dense)
00787 {
00788 _visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetInterpolate(1);
00789 _visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
00790 }
00791 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00792 if(_buildLocator)
00793 {
00794 vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
00795 tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
00796 tree->BuildLocator();
00797 _locators.insert(std::make_pair(id, tree));
00798 }
00799 _addedClouds.insert(id, pose);
00800 return true;
00801 }
00802 return false;
00803 }
00804
00805 bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool volumeRepresentation)
00806 {
00807 UDEBUG("");
00808 #ifdef RTABMAP_OCTOMAP
00809 UASSERT(octomap!=0);
00810
00811 pcl::IndicesPtr obstacles(new std::vector<int>);
00812
00813 if(treeDepth == 0 || treeDepth > octomap->octree()->getTreeDepth())
00814 {
00815 if(treeDepth>0)
00816 {
00817 UWARN("Tree depth requested (%d) is deeper than the "
00818 "actual maximum tree depth of %d. Using maximum depth.",
00819 (int)treeDepth, (int)octomap->octree()->getTreeDepth());
00820 }
00821 treeDepth = octomap->octree()->getTreeDepth();
00822 }
00823
00824 removeOctomap();
00825
00826 if(!volumeRepresentation)
00827 {
00828 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get(), 0, 0, false);
00829 if(obstacles->size())
00830 {
00831
00832
00833
00834 vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
00835 colors->SetName("colors");
00836 colors->SetNumberOfValues(obstacles->size());
00837
00838 vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
00839 lut->SetNumberOfTableValues(obstacles->size());
00840 lut->Build();
00841
00842
00843 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
00844 points->SetNumberOfPoints(obstacles->size());
00845 double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
00846 for (unsigned int i = 0; i < obstacles->size(); i++)
00847 {
00848 points->InsertPoint(i,
00849 cloud->at(obstacles->at(i)).x,
00850 cloud->at(obstacles->at(i)).y,
00851 cloud->at(obstacles->at(i)).z);
00852 colors->InsertValue(i,i);
00853
00854 lut->SetTableValue(i,
00855 double(cloud->at(obstacles->at(i)).r) / 255.0,
00856 double(cloud->at(obstacles->at(i)).g) / 255.0,
00857 double(cloud->at(obstacles->at(i)).b) / 255.0);
00858 }
00859
00860
00861 vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
00862 polydata->SetPoints(points);
00863 polydata->GetPointData()->SetScalars(colors);
00864
00865
00866 vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
00867 cubeSource->SetBounds(-s, s, -s, s, -s, s);
00868
00869 vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
00870 mapper->SetSourceConnection(cubeSource->GetOutputPort());
00871 #if VTK_MAJOR_VERSION <= 5
00872 mapper->SetInputConnection(polydata->GetProducerPort());
00873 #else
00874 mapper->SetInputData(polydata);
00875 #endif
00876 mapper->SetScalarRange(0, obstacles->size() - 1);
00877 mapper->SetLookupTable(lut);
00878 mapper->ScalingOff();
00879 mapper->Update();
00880
00881 vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
00882 octomapActor->SetMapper(mapper);
00883
00884 octomapActor->GetProperty()->SetRepresentationToSurface();
00885 octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
00886 octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
00887
00888 _visualizer->getRendererCollection()->InitTraversal ();
00889 vtkRenderer* renderer = NULL;
00890 renderer = _visualizer->getRendererCollection()->GetNextItem ();
00891 renderer = _visualizer->getRendererCollection()->GetNextItem ();
00892 UASSERT(renderer);
00893 renderer->AddActor(octomapActor);
00894
00895 _octomapActor = octomapActor.GetPointer();
00896 return true;
00897 }
00898 }
00899 else
00900 {
00901 if(octomap->octree()->size())
00902 {
00903
00904 vtkSmartPointer<vtkImageData> imageData =
00905 vtkSmartPointer<vtkImageData>::New();
00906
00907 double sizeX, sizeY, sizeZ;
00908 double minX, minY, minZ;
00909 double maxX, maxY, maxZ;
00910 octomap->getGridMin(minX, minY, minZ);
00911 octomap->getGridMax(maxX, maxY, maxZ);
00912 sizeX = maxX-minX;
00913 sizeY = maxY-minY;
00914 sizeZ = maxZ-minZ;
00915 double cellSize = octomap->octree()->getNodeSize(treeDepth);
00916
00917 UTimer t;
00918
00919 imageData->SetExtent(0, int(sizeX/cellSize+0.5), 0, int(sizeY/cellSize+0.5), 0, int(sizeZ/cellSize+0.5));
00920 #if VTK_MAJOR_VERSION <= 5
00921 imageData->SetNumberOfScalarComponents(4);
00922 imageData->SetScalarTypeToUnsignedChar();
00923 #else
00924 imageData->AllocateScalars(VTK_UNSIGNED_CHAR,4);
00925 #endif
00926
00927 int dims[3];
00928 imageData->GetDimensions(dims);
00929
00930 memset(imageData->GetScalarPointer(), 0, imageData->GetScalarSize()*imageData->GetNumberOfScalarComponents()*dims[0]*dims[1]*dims[2]);
00931
00932 for (RtabmapColorOcTree::iterator it = octomap->octree()->begin(treeDepth); it != octomap->octree()->end(); ++it)
00933 {
00934 if(octomap->octree()->isNodeOccupied(*it))
00935 {
00936 octomap::point3d pt = octomap->octree()->keyToCoord(it.getKey());
00937 int x = (pt.x()-minX) / cellSize;
00938 int y = (pt.y()-minY) / cellSize;
00939 int z = (pt.z()-minZ) / cellSize;
00940 if(x>=0 && x<dims[0] && y>=0 && y<dims[1] && z>=0 && z<dims[2])
00941 {
00942 unsigned char* pixel = static_cast<unsigned char*>(imageData->GetScalarPointer(x,y,z));
00943 if(octomap->octree()->getTreeDepth() == it.getDepth() && it->isColorSet())
00944 {
00945 pixel[0] = it->getColor().r;
00946 pixel[1] = it->getColor().g;
00947 pixel[2] = it->getColor().b;
00948 }
00949 else
00950 {
00951
00952 float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
00953 float r,g,b;
00954 OctoMap::HSVtoRGB(&r, &g, &b, H, 1, 1);
00955 pixel[0] = r*255.0f;
00956 pixel[1] = g*255.0f;
00957 pixel[2] = b*255.0f;
00958 }
00959 pixel[3] = 255;
00960 }
00961 }
00962 }
00963 vtkSmartPointer<vtkSmartVolumeMapper> volumeMapper =
00964 vtkSmartPointer<vtkSmartVolumeMapper>::New();
00965 volumeMapper->SetBlendModeToComposite();
00966 #if VTK_MAJOR_VERSION <= 5
00967 volumeMapper->SetInputConnection(imageData->GetProducerPort());
00968 #else
00969 volumeMapper->SetInputData(imageData);
00970 #endif
00971 vtkSmartPointer<vtkVolumeProperty> volumeProperty =
00972 vtkSmartPointer<vtkVolumeProperty>::New();
00973 volumeProperty->ShadeOff();
00974 volumeProperty->IndependentComponentsOff();
00975
00976 vtkSmartPointer<vtkPiecewiseFunction> compositeOpacity =
00977 vtkSmartPointer<vtkPiecewiseFunction>::New();
00978 compositeOpacity->AddPoint(0.0,0.0);
00979 compositeOpacity->AddPoint(255.0,1.0);
00980 volumeProperty->SetScalarOpacity(0, compositeOpacity);
00981
00982 vtkSmartPointer<vtkVolume> volume =
00983 vtkSmartPointer<vtkVolume>::New();
00984 volume->SetMapper(volumeMapper);
00985 volume->SetProperty(volumeProperty);
00986 volume->SetScale(cellSize);
00987 volume->SetPosition(minX, minY, minZ);
00988
00989 _visualizer->getRendererCollection()->InitTraversal ();
00990 vtkRenderer* renderer = NULL;
00991 renderer = _visualizer->getRendererCollection()->GetNextItem ();
00992 renderer = _visualizer->getRendererCollection()->GetNextItem ();
00993 UASSERT(renderer);
00994 renderer->AddViewProp(volume);
00995
00996
00997 #if !defined(VTK_LEGACY_REMOVE) && !defined(VTK_OPENGL2)
00998 volumeMapper->SetRequestedRenderModeToRayCastAndTexture();
00999 #endif // VTK_LEGACY_REMOVE
01000
01001
01002
01003 volumeMapper->SetRequestedRenderModeToRayCast();
01004 _octomapActor = volume.GetPointer();
01005
01006 return true;
01007 }
01008 }
01009 #endif
01010 return false;
01011 }
01012
01013 void CloudViewer::removeOctomap()
01014 {
01015 UDEBUG("");
01016 #ifdef RTABMAP_OCTOMAP
01017 if(_octomapActor)
01018 {
01019 _visualizer->getRendererCollection()->InitTraversal ();
01020 vtkRenderer* renderer = NULL;
01021 renderer = _visualizer->getRendererCollection()->GetNextItem ();
01022 renderer = _visualizer->getRendererCollection()->GetNextItem ();
01023 UASSERT(renderer);
01024 renderer->RemoveActor(_octomapActor);
01025 _octomapActor = 0;
01026 }
01027 #endif
01028 }
01029
01030 bool CloudViewer::addTextureMesh (
01031 const pcl::TextureMesh &mesh,
01032 const cv::Mat & image,
01033 const std::string &id,
01034 int viewport)
01035 {
01036
01037
01038 pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id);
01039 if (am_it != _visualizer->getCloudActorMap()->end ())
01040 {
01041 PCL_ERROR ("[PCLVisualizer::addTextureMesh] A shape with id <%s> already exists!"
01042 " Please choose a different id and retry.\n",
01043 id.c_str ());
01044 return (false);
01045 }
01046
01047 if (mesh.tex_materials.size () == 0)
01048 {
01049 PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures found!\n");
01050 return (false);
01051 }
01052 else if (mesh.tex_materials.size() > 1)
01053 {
01054 PCL_ERROR("[PCLVisualizer::addTextureMesh] only one material per mesh is supported!\n");
01055 return (false);
01056 }
01057
01058 if (mesh.tex_materials.size () != mesh.tex_polygons.size ())
01059 {
01060 PCL_ERROR("[PCLVisualizer::addTextureMesh] Materials number %lu differs from polygons number %lu!\n",
01061 mesh.tex_materials.size (), mesh.tex_polygons.size ());
01062 return (false);
01063 }
01064
01065 if (mesh.tex_materials.size () != mesh.tex_coordinates.size ())
01066 {
01067 PCL_ERROR("[PCLVisualizer::addTextureMesh] Coordinates number %lu differs from materials number %lu!\n",
01068 mesh.tex_coordinates.size (), mesh.tex_materials.size ());
01069 return (false);
01070 }
01071
01072 std::size_t nb_vertices = 0;
01073 for (std::size_t i = 0; i < mesh.tex_polygons.size (); ++i)
01074 nb_vertices+= mesh.tex_polygons[i].size ();
01075
01076 if (nb_vertices == 0)
01077 {
01078 PCL_ERROR("[PCLVisualizer::addTextureMesh] No vertices found!\n");
01079 return (false);
01080 }
01081
01082 std::size_t nb_coordinates = 0;
01083 for (std::size_t i = 0; i < mesh.tex_coordinates.size (); ++i)
01084 nb_coordinates+= mesh.tex_coordinates[i].size ();
01085
01086 if (nb_coordinates == 0)
01087 {
01088 PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures coordinates found!\n");
01089 return (false);
01090 }
01091
01092
01093 vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
01094 vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
01095 bool has_color = false;
01096 vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
01097
01098 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
01099 pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
01100
01101 if (cloud->points.size () == 0)
01102 {
01103 PCL_ERROR("[PCLVisualizer::addTextureMesh] Cloud is empty!\n");
01104 return (false);
01105 }
01106 pcl::visualization::PCLVisualizer::convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
01107 poly_points->SetNumberOfPoints (cloud->points.size ());
01108 for (std::size_t i = 0; i < cloud->points.size (); ++i)
01109 {
01110 const pcl::PointXYZ &p = cloud->points[i];
01111 poly_points->InsertPoint (i, p.x, p.y, p.z);
01112 }
01113
01114
01115 vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New ();
01116 for (std::size_t i = 0; i < mesh.tex_polygons.size (); i++)
01117 {
01118 for (std::size_t j = 0; j < mesh.tex_polygons[i].size (); j++)
01119 {
01120 std::size_t n_points = mesh.tex_polygons[i][j].vertices.size ();
01121 polys->InsertNextCell (int (n_points));
01122 for (std::size_t k = 0; k < n_points; k++)
01123 polys->InsertCellPoint (mesh.tex_polygons[i][j].vertices[k]);
01124 }
01125 }
01126
01127 vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
01128 polydata->SetPolys (polys);
01129 polydata->SetPoints (poly_points);
01130 if (has_color)
01131 polydata->GetPointData()->SetScalars(colors);
01132
01133 vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
01134 #if VTK_MAJOR_VERSION < 6
01135 mapper->SetInput (polydata);
01136 #else
01137 mapper->SetInputData (polydata);
01138 #endif
01139
01140 vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
01141 vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
01142 if (!tex_manager)
01143 return (false);
01144
01145 vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
01146
01147 vtkSmartPointer<vtkImageMatSource> cvImageToVtk = vtkSmartPointer<vtkImageMatSource>::New();
01148 cvImageToVtk->SetImage(image);
01149 cvImageToVtk->Update();
01150 texture->SetInputConnection(cvImageToVtk->GetOutputPort());
01151
01152
01153 vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New ();
01154 coordinates->SetNumberOfComponents (2);
01155 coordinates->SetNumberOfTuples (mesh.tex_coordinates[0].size ());
01156 for (std::size_t tc = 0; tc < mesh.tex_coordinates[0].size (); ++tc)
01157 {
01158 const Eigen::Vector2f &uv = mesh.tex_coordinates[0][tc];
01159 coordinates->SetTuple2 (tc, (double)uv[0], (double)uv[1]);
01160 }
01161 coordinates->SetName ("TCoords");
01162 polydata->GetPointData ()->SetTCoords(coordinates);
01163
01164 actor->SetTexture (texture);
01165
01166
01167 actor->SetMapper (mapper);
01168
01169
01170
01171 _visualizer->getRendererCollection()->InitTraversal ();
01172 vtkRenderer* renderer = NULL;
01173 int i = 0;
01174 while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
01175 {
01176
01177 if (viewport == 0)
01178 {
01179 renderer->AddActor (actor);
01180 }
01181 else if (viewport == i)
01182 {
01183 renderer->AddActor (actor);
01184 }
01185 ++i;
01186 }
01187
01188
01189 (*_visualizer->getCloudActorMap())[id].actor = actor;
01190
01191
01192 (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
01193
01194 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
01195 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
01196 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
01197 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
01198 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
01199 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
01200 return true;
01201 }
01202
01203 bool CloudViewer::addOccupancyGridMap(
01204 const cv::Mat & map8U,
01205 float resolution,
01206 float xMin,
01207 float yMin,
01208 float opacity)
01209 {
01210 UASSERT(map8U.channels() == 1 && map8U.type() == CV_8U);
01211
01212 float xSize = float(map8U.cols) * resolution;
01213 float ySize = float(map8U.rows) * resolution;
01214
01215 UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
01216 if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
01217 {
01218 _visualizer->removePointCloud("map");
01219 }
01220
01221 if(xSize > 0.0f && ySize > 0.0f)
01222 {
01223 pcl::TextureMeshPtr mesh(new pcl::TextureMesh());
01224 pcl::PointCloud<pcl::PointXYZ> cloud;
01225 cloud.push_back(pcl::PointXYZ(xMin, yMin, 0));
01226 cloud.push_back(pcl::PointXYZ(xSize+xMin, yMin, 0));
01227 cloud.push_back(pcl::PointXYZ(xSize+xMin, ySize+yMin, 0));
01228 cloud.push_back(pcl::PointXYZ(xMin, ySize+yMin, 0));
01229 pcl::toPCLPointCloud2(cloud, mesh->cloud);
01230
01231 std::vector<pcl::Vertices> polygons(1);
01232 polygons[0].vertices.push_back(0);
01233 polygons[0].vertices.push_back(1);
01234 polygons[0].vertices.push_back(2);
01235 polygons[0].vertices.push_back(3);
01236 polygons[0].vertices.push_back(0);
01237 mesh->tex_polygons.push_back(polygons);
01238
01239
01240 pcl::TexMaterial material;
01241 material.tex_file = "";
01242 mesh->tex_materials.push_back(material);
01243
01244 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
01245 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > coordinates;
01246 #else
01247 std::vector<Eigen::Vector2f> coordinates;
01248 #endif
01249 coordinates.push_back(Eigen::Vector2f(0,1));
01250 coordinates.push_back(Eigen::Vector2f(1,1));
01251 coordinates.push_back(Eigen::Vector2f(1,0));
01252 coordinates.push_back(Eigen::Vector2f(0,0));
01253 mesh->tex_coordinates.push_back(coordinates);
01254
01255 this->addTextureMesh(*mesh, map8U, "map", 1);
01256 setCloudOpacity("map", opacity);
01257 }
01258 return true;
01259 }
01260
01261 void CloudViewer::removeOccupancyGridMap()
01262 {
01263 if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
01264 {
01265 _visualizer->removePointCloud("map");
01266 }
01267 }
01268
01269 void CloudViewer::addOrUpdateCoordinate(
01270 const std::string & id,
01271 const Transform & transform,
01272 double scale,
01273 bool foreground)
01274 {
01275 if(id.empty())
01276 {
01277 UERROR("id should not be empty!");
01278 return;
01279 }
01280
01281 removeCoordinate(id);
01282
01283 if(!transform.isNull())
01284 {
01285 _coordinates.insert(id);
01286 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
01287 _visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id, foreground?2:1);
01288 #else
01289
01290 _visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0);
01291 #endif
01292 }
01293 }
01294
01295 bool CloudViewer::updateCoordinatePose(
01296 const std::string & id,
01297 const Transform & pose)
01298 {
01299 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
01300 if(_coordinates.find(id) != _coordinates.end() && !pose.isNull())
01301 {
01302 UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
01303 return _visualizer->updateCoordinateSystemPose(id, pose.toEigen3f());
01304 }
01305 #else
01306 UERROR("CloudViewer::updateCoordinatePose() is not available on PCL < 1.7.2");
01307 #endif
01308 return false;
01309 }
01310
01311 void CloudViewer::removeCoordinate(const std::string & id)
01312 {
01313 if(id.empty())
01314 {
01315 UERROR("id should not be empty!");
01316 return;
01317 }
01318
01319 if(_coordinates.find(id) != _coordinates.end())
01320 {
01321 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
01322 _visualizer->removeCoordinateSystem(id);
01323 #else
01324
01325 _visualizer->removeCoordinateSystem(0);
01326 #endif
01327 _coordinates.erase(id);
01328 }
01329 }
01330
01331 void CloudViewer::removeAllCoordinates()
01332 {
01333 std::set<std::string> coordinates = _coordinates;
01334 for(std::set<std::string>::iterator iter = coordinates.begin(); iter!=coordinates.end(); ++iter)
01335 {
01336 this->removeCoordinate(*iter);
01337 }
01338 UASSERT(_coordinates.empty());
01339 }
01340
01341 void CloudViewer::addOrUpdateLine(
01342 const std::string & id,
01343 const Transform & from,
01344 const Transform & to,
01345 const QColor & color,
01346 bool arrow,
01347 bool foreground)
01348 {
01349 if(id.empty())
01350 {
01351 UERROR("id should not be empty!");
01352 return;
01353 }
01354
01355 removeLine(id);
01356
01357 if(!from.isNull() && !to.isNull())
01358 {
01359 _lines.insert(id);
01360
01361 QColor c = Qt::gray;
01362 if(color.isValid())
01363 {
01364 c = color;
01365 }
01366
01367 pcl::PointXYZ pt1(from.x(), from.y(), from.z());
01368 pcl::PointXYZ pt2(to.x(), to.y(), to.z());
01369
01370 if(arrow)
01371 {
01372 _visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id, foreground?2:1);
01373 }
01374 else
01375 {
01376 _visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?2:1);
01377 }
01378 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
01379 }
01380 }
01381
01382 void CloudViewer::removeLine(const std::string & id)
01383 {
01384 if(id.empty())
01385 {
01386 UERROR("id should not be empty!");
01387 return;
01388 }
01389
01390 if(_lines.find(id) != _lines.end())
01391 {
01392 _visualizer->removeShape(id);
01393 _lines.erase(id);
01394 }
01395 }
01396
01397 void CloudViewer::removeAllLines()
01398 {
01399 std::set<std::string> arrows = _lines;
01400 for(std::set<std::string>::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter)
01401 {
01402 this->removeLine(*iter);
01403 }
01404 UASSERT(_lines.empty());
01405 }
01406
01407 void CloudViewer::addOrUpdateSphere(
01408 const std::string & id,
01409 const Transform & pose,
01410 float radius,
01411 const QColor & color,
01412 bool foreground)
01413 {
01414 if(id.empty())
01415 {
01416 UERROR("id should not be empty!");
01417 return;
01418 }
01419
01420 removeSphere(id);
01421
01422 if(!pose.isNull())
01423 {
01424 _spheres.insert(id);
01425
01426 QColor c = Qt::gray;
01427 if(color.isValid())
01428 {
01429 c = color;
01430 }
01431
01432 pcl::PointXYZ center(pose.x(), pose.y(), pose.z());
01433 _visualizer->addSphere(center, radius, c.redF(), c.greenF(), c.blueF(), id, foreground?2:1);
01434 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
01435 }
01436 }
01437
01438 void CloudViewer::removeSphere(const std::string & id)
01439 {
01440 if(id.empty())
01441 {
01442 UERROR("id should not be empty!");
01443 return;
01444 }
01445
01446 if(_spheres.find(id) != _spheres.end())
01447 {
01448 _visualizer->removeShape(id);
01449 _spheres.erase(id);
01450 }
01451 }
01452
01453 void CloudViewer::removeAllSpheres()
01454 {
01455 std::set<std::string> spheres = _spheres;
01456 for(std::set<std::string>::iterator iter = spheres.begin(); iter!=spheres.end(); ++iter)
01457 {
01458 this->removeSphere(*iter);
01459 }
01460 UASSERT(_spheres.empty());
01461 }
01462
01463 void CloudViewer::addOrUpdateCube(
01464 const std::string & id,
01465 const Transform & pose,
01466 float width,
01467 float height,
01468 float depth,
01469 const QColor & color,
01470 bool wireframe,
01471 bool foreground)
01472 {
01473 if(id.empty())
01474 {
01475 UERROR("id should not be empty!");
01476 return;
01477 }
01478
01479 removeCube(id);
01480
01481 if(!pose.isNull())
01482 {
01483 _cubes.insert(id);
01484
01485 QColor c = Qt::gray;
01486 if(color.isValid())
01487 {
01488 c = color;
01489 }
01490 _visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?2:1);
01491 if(wireframe)
01492 {
01493 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id);
01494 }
01495 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
01496 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
01497 }
01498 }
01499
01500 void CloudViewer::removeCube(const std::string & id)
01501 {
01502 if(id.empty())
01503 {
01504 UERROR("id should not be empty!");
01505 return;
01506 }
01507
01508 if(_cubes.find(id) != _cubes.end())
01509 {
01510 _visualizer->removeShape(id);
01511 _cubes.erase(id);
01512 }
01513 }
01514
01515 void CloudViewer::removeAllCubes()
01516 {
01517 std::set<std::string> cubes = _cubes;
01518 for(std::set<std::string>::iterator iter = cubes.begin(); iter!=cubes.end(); ++iter)
01519 {
01520 this->removeCube(*iter);
01521 }
01522 UASSERT(_cubes.empty());
01523 }
01524
01525 void CloudViewer::addOrUpdateQuad(
01526 const std::string & id,
01527 const Transform & pose,
01528 float width,
01529 float height,
01530 const QColor & color,
01531 bool foreground)
01532 {
01533 addOrUpdateQuad(id, pose, width/2.0f, width/2.0f, height/2.0f, height/2.0f, color, foreground);
01534 }
01535
01536 void CloudViewer::addOrUpdateQuad(
01537 const std::string & id,
01538 const Transform & pose,
01539 float widthLeft,
01540 float widthRight,
01541 float heightBottom,
01542 float heightTop,
01543 const QColor & color,
01544 bool foreground)
01545 {
01546 if(id.empty())
01547 {
01548 UERROR("id should not be empty!");
01549 return;
01550 }
01551
01552 removeQuad(id);
01553
01554 if(!pose.isNull())
01555 {
01556 _quads.insert(id);
01557
01558 QColor c = Qt::gray;
01559 if(color.isValid())
01560 {
01561 c = color;
01562 }
01563
01564
01565 double p0[3] = {0.0, -widthLeft, heightTop};
01566 double p1[3] = {0.0, -widthLeft, -heightBottom};
01567 double p2[3] = {0.0, widthRight, -heightBottom};
01568 double p3[3] = {0.0, widthRight, heightTop};
01569
01570
01571 vtkSmartPointer<vtkPoints> points =
01572 vtkSmartPointer<vtkPoints>::New();
01573 points->InsertNextPoint(p0);
01574 points->InsertNextPoint(p1);
01575 points->InsertNextPoint(p2);
01576 points->InsertNextPoint(p3);
01577
01578
01579 vtkSmartPointer<vtkQuad> quad =
01580 vtkSmartPointer<vtkQuad>::New();
01581 quad->GetPointIds()->SetId(0,0);
01582 quad->GetPointIds()->SetId(1,1);
01583 quad->GetPointIds()->SetId(2,2);
01584 quad->GetPointIds()->SetId(3,3);
01585
01586
01587 vtkSmartPointer<vtkCellArray> quads =
01588 vtkSmartPointer<vtkCellArray>::New();
01589 quads->InsertNextCell(quad);
01590
01591
01592 vtkSmartPointer<vtkPolyData> polydata =
01593 vtkSmartPointer<vtkPolyData>::New();
01594
01595
01596 polydata->SetPoints(points);
01597 polydata->SetPolys(quads);
01598
01599
01600 vtkSmartPointer<vtkPolyDataMapper> mapper =
01601 vtkSmartPointer<vtkPolyDataMapper>::New();
01602 #if VTK_MAJOR_VERSION <= 5
01603 mapper->SetInput(polydata);
01604 #else
01605 mapper->SetInputData(polydata);
01606 #endif
01607
01608 vtkSmartPointer<vtkLODActor> actor =
01609 vtkSmartPointer<vtkLODActor>::New();
01610 actor->SetMapper(mapper);
01611 actor->GetProperty()->SetColor(c.redF(), c.greenF(), c.blueF());
01612
01613
01614
01615 _visualizer->getRendererCollection()->InitTraversal ();
01616 vtkRenderer* renderer = NULL;
01617 int i = 0;
01618 while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
01619 {
01620 if ((foreground?2:1) == i)
01621 {
01622 renderer->AddActor (actor);
01623 }
01624 ++i;
01625 }
01626
01627
01628 (*_visualizer->getCloudActorMap())[id].actor = actor;
01629
01630
01631 vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
01632 pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), transformation);
01633 (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
01634 (*_visualizer->getCloudActorMap())[id].actor->SetUserMatrix (transformation);
01635 (*_visualizer->getCloudActorMap())[id].actor->Modified ();
01636
01637 (*_visualizer->getCloudActorMap())[id].actor->GetProperty()->SetLighting(false);
01638 _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
01639 }
01640 }
01641
01642 void CloudViewer::removeQuad(const std::string & id)
01643 {
01644 if(id.empty())
01645 {
01646 UERROR("id should not be empty!");
01647 return;
01648 }
01649
01650 if(_quads.find(id) != _quads.end())
01651 {
01652 _visualizer->removeShape(id);
01653 _quads.erase(id);
01654 }
01655 }
01656
01657 void CloudViewer::removeAllQuads()
01658 {
01659 std::set<std::string> quads = _quads;
01660 for(std::set<std::string>::iterator iter = quads.begin(); iter!=quads.end(); ++iter)
01661 {
01662 this->removeQuad(*iter);
01663 }
01664 UASSERT(_quads.empty());
01665 }
01666
01667 static const float frustum_vertices[] = {
01668 0.0f, 0.0f, 0.0f,
01669 1.0f, 1.0f, 1.0f,
01670 1.0f, -1.0f, 1.0f,
01671 1.0f, -1.0f, -1.0f,
01672 1.0f, 1.0f, -1.0f};
01673
01674 static const int frustum_indices[] = {
01675 1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4};
01676
01677 void CloudViewer::addOrUpdateFrustum(
01678 const std::string & id,
01679 const Transform & transform,
01680 const Transform & localTransform,
01681 double scale,
01682 const QColor & color)
01683 {
01684 if(id.empty())
01685 {
01686 UERROR("id should not be empty!");
01687 return;
01688 }
01689
01690 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
01691 this->removeFrustum(id);
01692 #endif
01693
01694 if(!transform.isNull())
01695 {
01696 if(_frustums.find(id)==_frustums.end())
01697 {
01698 _frustums.insert(id, Transform());
01699
01700 int frustumSize = sizeof(frustum_vertices)/sizeof(float);
01701 UASSERT(frustumSize>0 && frustumSize % 3 == 0);
01702 frustumSize/=3;
01703 pcl::PointCloud<pcl::PointXYZ> frustumPoints;
01704 frustumPoints.resize(frustumSize);
01705 float scaleX = 0.5f * scale;
01706 float scaleY = 0.4f * scale;
01707 float scaleZ = 0.3f * scale;
01708 QColor c = Qt::gray;
01709 if(color.isValid())
01710 {
01711 c = color;
01712 }
01713 Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
01714
01715 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
01716 Eigen::Affine3f t = (transform*localTransform*opticalRotInv).toEigen3f();
01717 #else
01718 Eigen::Affine3f t = (localTransform*opticalRotInv).toEigen3f();
01719 #endif
01720 for(int i=0; i<frustumSize; ++i)
01721 {
01722 frustumPoints[i].x = frustum_vertices[i*3]*scaleX;
01723 frustumPoints[i].y = frustum_vertices[i*3+1]*scaleY;
01724 frustumPoints[i].z = frustum_vertices[i*3+2]*scaleZ;
01725 frustumPoints[i] = pcl::transformPoint(frustumPoints[i], t);
01726 }
01727
01728 pcl::PolygonMesh mesh;
01729 pcl::Vertices vertices;
01730 vertices.vertices.resize(sizeof(frustum_indices)/sizeof(int));
01731 for(unsigned int i=0; i<vertices.vertices.size(); ++i)
01732 {
01733 vertices.vertices[i] = frustum_indices[i];
01734 }
01735 pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
01736 mesh.polygons.push_back(vertices);
01737 _visualizer->addPolylineFromPolygonMesh(mesh, id, 1);
01738 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
01739 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
01740 }
01741 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
01742 if(!this->updateFrustumPose(id, transform))
01743 {
01744 UERROR("Failed updating pose of frustum %s!?", id.c_str());
01745 }
01746 #endif
01747 }
01748 else
01749 {
01750 removeFrustum(id);
01751 }
01752 }
01753
01754 bool CloudViewer::updateFrustumPose(
01755 const std::string & id,
01756 const Transform & pose)
01757 {
01758 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
01759 QMap<std::string, Transform>::iterator iter=_frustums.find(id);
01760 if(iter != _frustums.end() && !pose.isNull())
01761 {
01762 if(iter.value() == pose)
01763 {
01764
01765 return true;
01766 }
01767
01768 pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
01769
01770 vtkActor* actor;
01771
01772 if (am_it == _visualizer->getShapeActorMap()->end ())
01773 return (false);
01774 else
01775 actor = vtkActor::SafeDownCast (am_it->second);
01776
01777 if (!actor)
01778 return (false);
01779
01780 vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
01781
01782 pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
01783
01784 actor->SetUserMatrix (matrix);
01785 actor->Modified ();
01786
01787 iter.value() = pose;
01788
01789 return true;
01790 }
01791 #else
01792 UERROR("updateFrustumPose() cannot be used with PCL<1.7.2. Use addOrUpdateFrustum() instead.");
01793 #endif
01794 return false;
01795 }
01796
01797 void CloudViewer::removeFrustum(const std::string & id)
01798 {
01799 if(id.empty())
01800 {
01801 UERROR("id should not be empty!");
01802 return;
01803 }
01804
01805 if(_frustums.find(id) != _frustums.end())
01806 {
01807 _visualizer->removeShape(id);
01808 _frustums.remove(id);
01809 }
01810 }
01811
01812 void CloudViewer::removeAllFrustums(bool exceptCameraReference)
01813 {
01814 QMap<std::string, Transform> frustums = _frustums;
01815 for(QMap<std::string, Transform>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
01816 {
01817 if(!exceptCameraReference || !uStrContains(iter.key(), "reference_frustum"))
01818 {
01819 this->removeFrustum(iter.key());
01820 }
01821 }
01822 UASSERT(exceptCameraReference || _frustums.empty());
01823 }
01824
01825 void CloudViewer::addOrUpdateGraph(
01826 const std::string & id,
01827 const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
01828 const QColor & color)
01829 {
01830 if(id.empty())
01831 {
01832 UERROR("id should not be empty!");
01833 return;
01834 }
01835
01836 removeGraph(id);
01837
01838 if(graph->size())
01839 {
01840 _graphes.insert(id);
01841
01842 pcl::PolygonMesh mesh;
01843 pcl::Vertices vertices;
01844 vertices.vertices.resize(graph->size());
01845 for(unsigned int i=0; i<vertices.vertices.size(); ++i)
01846 {
01847 vertices.vertices[i] = i;
01848 }
01849 pcl::toPCLPointCloud2(*graph, mesh.cloud);
01850 mesh.polygons.push_back(vertices);
01851 _visualizer->addPolylineFromPolygonMesh(mesh, id, 1);
01852 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
01853 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, color.alphaF(), id);
01854
01855 this->addCloud(id+"_nodes", graph, Transform::getIdentity(), color);
01856 this->setCloudPointSize(id+"_nodes", 5);
01857 }
01858 }
01859
01860 void CloudViewer::removeGraph(const std::string & id)
01861 {
01862 if(id.empty())
01863 {
01864 UERROR("id should not be empty!");
01865 return;
01866 }
01867
01868 if(_graphes.find(id) != _graphes.end())
01869 {
01870 _visualizer->removeShape(id);
01871 _graphes.erase(id);
01872 removeCloud(id+"_nodes");
01873 }
01874 }
01875
01876 void CloudViewer::removeAllGraphs()
01877 {
01878 std::set<std::string> graphes = _graphes;
01879 for(std::set<std::string>::iterator iter = graphes.begin(); iter!=graphes.end(); ++iter)
01880 {
01881 this->removeGraph(*iter);
01882 }
01883 UASSERT(_graphes.empty());
01884 }
01885
01886 void CloudViewer::addOrUpdateText(
01887 const std::string & id,
01888 const std::string & text,
01889 const Transform & position,
01890 double scale,
01891 const QColor & color,
01892 bool foreground)
01893 {
01894 if(id.empty())
01895 {
01896 UERROR("id should not be empty!");
01897 return;
01898 }
01899
01900 removeText(id);
01901
01902 if(!position.isNull())
01903 {
01904 _texts.insert(id);
01905 _visualizer->addText3D(
01906 text,
01907 pcl::PointXYZ(position.x(), position.y(), position.z()),
01908 scale,
01909 color.redF(),
01910 color.greenF(),
01911 color.blueF(),
01912 id,
01913 foreground?2:1);
01914 }
01915 }
01916
01917 void CloudViewer::removeText(const std::string & id)
01918 {
01919 if(id.empty())
01920 {
01921 UERROR("id should not be empty!");
01922 return;
01923 }
01924
01925 if(_texts.find(id) != _texts.end())
01926 {
01927 _visualizer->removeText3D(id);
01928 _texts.erase(id);
01929 }
01930 }
01931
01932 void CloudViewer::removeAllTexts()
01933 {
01934 std::set<std::string> texts = _texts;
01935 for(std::set<std::string>::iterator iter = texts.begin(); iter!=texts.end(); ++iter)
01936 {
01937 this->removeText(*iter);
01938 }
01939 UASSERT(_texts.empty());
01940 }
01941
01942 bool CloudViewer::isTrajectoryShown() const
01943 {
01944 return _aShowTrajectory->isChecked();
01945 }
01946
01947 unsigned int CloudViewer::getTrajectorySize() const
01948 {
01949 return _maxTrajectorySize;
01950 }
01951
01952 void CloudViewer::setTrajectoryShown(bool shown)
01953 {
01954 _aShowTrajectory->setChecked(shown);
01955 }
01956
01957 void CloudViewer::setTrajectorySize(unsigned int value)
01958 {
01959 _maxTrajectorySize = value;
01960 }
01961
01962 void CloudViewer::clearTrajectory()
01963 {
01964 _trajectory->clear();
01965 _visualizer->removeShape("trajectory");
01966 this->update();
01967 }
01968
01969 bool CloudViewer::isFrustumShown() const
01970 {
01971 return _aShowFrustum->isChecked();
01972 }
01973
01974 float CloudViewer::getFrustumScale() const
01975 {
01976 return _frustumScale;
01977 }
01978
01979 QColor CloudViewer::getFrustumColor() const
01980 {
01981 return _frustumColor;
01982 }
01983
01984 void CloudViewer::setFrustumShown(bool shown)
01985 {
01986 if(!shown)
01987 {
01988 QMap<std::string, Transform> frustumsCopy = _frustums;
01989 for(QMap<std::string, Transform>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
01990 {
01991 if(uStrContains(iter.key(), "reference_frustum"))
01992 {
01993 this->removeFrustum(iter.key());
01994 }
01995 }
01996 std::set<std::string> linesCopy = _lines;
01997 for(std::set<std::string>::iterator iter=linesCopy.begin(); iter!=linesCopy.end(); ++iter)
01998 {
01999 if(uStrContains(*iter, "reference_frustum_line"))
02000 {
02001 this->removeLine(*iter);
02002 }
02003 }
02004 this->update();
02005 }
02006 _aShowFrustum->setChecked(shown);
02007 }
02008
02009 void CloudViewer::setFrustumScale(float value)
02010 {
02011 _frustumScale = value;
02012 }
02013
02014 void CloudViewer::setFrustumColor(QColor value)
02015 {
02016 if(!value.isValid())
02017 {
02018 value = Qt::gray;
02019 }
02020 for(QMap<std::string, Transform>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
02021 {
02022 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), iter.key());
02023 }
02024 this->update();
02025 _frustumColor = value;
02026 }
02027
02028 void CloudViewer::resetCamera()
02029 {
02030 _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
02031 if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull())
02032 {
02033
02034 cv::Point3f pt = util3d::transformPoint(cv::Point3f(_lastPose.x(), _lastPose.y(), _lastPose.z()), ( _lastPose.rotation()*Transform(-1, 0, 0)).translation());
02035 if(_aCameraOrtho->isChecked())
02036 {
02037 _visualizer->setCameraPosition(
02038 _lastPose.x(), _lastPose.y(), _lastPose.z()+5,
02039 _lastPose.x(), _lastPose.y(), _lastPose.z(),
02040 1, 0, 0, 1);
02041 }
02042 else if(_aLockViewZ->isChecked())
02043 {
02044 _visualizer->setCameraPosition(
02045 pt.x, pt.y, pt.z,
02046 _lastPose.x(), _lastPose.y(), _lastPose.z(),
02047 0, 0, 1, 1);
02048 }
02049 else
02050 {
02051 _visualizer->setCameraPosition(
02052 pt.x, pt.y, pt.z,
02053 _lastPose.x(), _lastPose.y(), _lastPose.z(),
02054 _lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
02055 }
02056 }
02057 else if(_aCameraOrtho->isChecked())
02058 {
02059 _visualizer->setCameraPosition(
02060 0, 0, 5,
02061 0, 0, 0,
02062 1, 0, 0, 1);
02063 }
02064 else
02065 {
02066 _visualizer->setCameraPosition(
02067 -1, 0, 0,
02068 0, 0, 0,
02069 0, 0, 1, 1);
02070 }
02071 this->update();
02072 }
02073
02074 void CloudViewer::removeAllClouds()
02075 {
02076 _addedClouds.clear();
02077 _locators.clear();
02078 _visualizer->removeAllPointClouds();
02079 }
02080
02081
02082 bool CloudViewer::removeCloud(const std::string & id)
02083 {
02084 bool success = _visualizer->removePointCloud(id);
02085 _visualizer->removePointCloud(id+"-normals");
02086 _addedClouds.remove(id);
02087 _addedClouds.remove(id+"-normals");
02088 _locators.erase(id);
02089 return success;
02090 }
02091
02092 bool CloudViewer::getPose(const std::string & id, Transform & pose)
02093 {
02094 if(_addedClouds.contains(id))
02095 {
02096 pose = _addedClouds.value(id);
02097 return true;
02098 }
02099 return false;
02100 }
02101
02102 Transform CloudViewer::getTargetPose() const
02103 {
02104 if(_lastPose.isNull())
02105 {
02106 return Transform::getIdentity();
02107 }
02108 return _lastPose;
02109 }
02110
02111 std::string CloudViewer::getIdByActor(vtkProp * actor) const
02112 {
02113 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02114 for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
02115 {
02116 if(iter->second.actor.GetPointer() == actor)
02117 {
02118 return iter->first;
02119 }
02120 }
02121
02122 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
02123
02124 pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
02125 for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
02126 {
02127 if(iter->second.GetPointer() == actor)
02128 {
02129 std::string id = iter->first;
02130 while(id.back() == '*')
02131 {
02132 id.erase(id.size()-1);
02133 }
02134
02135 return id;
02136 }
02137 }
02138 #endif
02139 return std::string();
02140 }
02141
02142 QColor CloudViewer::getColor(const std::string & id)
02143 {
02144 QColor color;
02145 pcl::visualization::CloudActorMap::iterator iter = _visualizer->getCloudActorMap()->find(id);
02146 if(iter != _visualizer->getCloudActorMap()->end())
02147 {
02148 double r,g,b,a;
02149 iter->second.actor->GetProperty()->GetColor(r,g,b);
02150 a = iter->second.actor->GetProperty()->GetOpacity();
02151 color.setRgbF(r, g, b, a);
02152 }
02153 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
02154
02155 else
02156 {
02157 std::string idLayer1 = id+"*";
02158 std::string idLayer2 = id+"**";
02159 pcl::visualization::ShapeActorMap::iterator iter = _visualizer->getShapeActorMap()->find(id);
02160 if(iter == _visualizer->getShapeActorMap()->end())
02161 {
02162 iter = _visualizer->getShapeActorMap()->find(idLayer1);
02163 if(iter == _visualizer->getShapeActorMap()->end())
02164 {
02165 iter = _visualizer->getShapeActorMap()->find(idLayer2);
02166 }
02167 }
02168 if(iter != _visualizer->getShapeActorMap()->end())
02169 {
02170 vtkActor * actor = vtkActor::SafeDownCast(iter->second);
02171 if(actor)
02172 {
02173 double r,g,b,a;
02174 actor->GetProperty()->GetColor(r,g,b);
02175 a = actor->GetProperty()->GetOpacity();
02176 color.setRgbF(r, g, b, a);
02177 }
02178 }
02179 }
02180 #endif
02181 return color;
02182 }
02183
02184 void CloudViewer::setColor(const std::string & id, const QColor & color)
02185 {
02186 pcl::visualization::CloudActorMap::iterator iter = _visualizer->getCloudActorMap()->find(id);
02187 if(iter != _visualizer->getCloudActorMap()->end())
02188 {
02189 iter->second.actor->GetProperty()->SetColor(color.redF(),color.greenF(),color.blueF());
02190 iter->second.actor->GetProperty()->SetOpacity(color.alphaF());
02191 }
02192 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
02193
02194 else
02195 {
02196 std::string idLayer1 = id+"*";
02197 std::string idLayer2 = id+"**";
02198 pcl::visualization::ShapeActorMap::iterator iter = _visualizer->getShapeActorMap()->find(id);
02199 if(iter == _visualizer->getShapeActorMap()->end())
02200 {
02201 iter = _visualizer->getShapeActorMap()->find(idLayer1);
02202 if(iter == _visualizer->getShapeActorMap()->end())
02203 {
02204 iter = _visualizer->getShapeActorMap()->find(idLayer2);
02205 }
02206 }
02207 if(iter != _visualizer->getShapeActorMap()->end())
02208 {
02209 vtkActor * actor = vtkActor::SafeDownCast(iter->second);
02210 if(actor)
02211 {
02212 actor->GetProperty()->SetColor(color.redF(),color.greenF(),color.blueF());
02213 actor->GetProperty()->SetOpacity(color.alphaF());
02214 }
02215 }
02216 }
02217 #endif
02218 }
02219
02220 void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
02221 {
02222 _aBackfaceCulling->setChecked(enabled);
02223 _frontfaceCulling = frontfaceCulling;
02224
02225 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02226 for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
02227 {
02228 iter->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
02229 iter->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
02230 }
02231 this->update();
02232 }
02233
02234 void CloudViewer::setPolygonPicking(bool enabled)
02235 {
02236 _aPolygonPicking->setChecked(enabled);
02237
02238 if(!_aPolygonPicking->isChecked())
02239 {
02240 vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
02241 pp->SetTolerance (pp->GetTolerance());
02242 this->GetInteractor()->SetPicker (pp);
02243 setMouseTracking(false);
02244 }
02245 else
02246 {
02247 vtkSmartPointer<CloudViewerCellPicker> pp = vtkSmartPointer<CloudViewerCellPicker>::New ();
02248 pp->SetTolerance (pp->GetTolerance());
02249 this->GetInteractor()->SetPicker (pp);
02250 setMouseTracking(true);
02251 }
02252 }
02253
02254
02255
02256 void CloudViewer::setRenderingRate(double rate)
02257 {
02258 _renderingRate = rate;
02259 _visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(_renderingRate);
02260 }
02261
02262 void CloudViewer::setLighting(bool on)
02263 {
02264 _aSetLighting->setChecked(on);
02265 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02266 for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
02267 {
02268 iter->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
02269 }
02270 this->update();
02271 }
02272
02273 void CloudViewer::setShading(bool on)
02274 {
02275 _aSetFlatShading->setChecked(on);
02276 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02277 for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
02278 {
02279 iter->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
02280 }
02281 this->update();
02282 }
02283
02284 void CloudViewer::setEdgeVisibility(bool visible)
02285 {
02286 _aSetEdgeVisibility->setChecked(visible);
02287 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02288 for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
02289 {
02290 iter->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
02291 }
02292 this->update();
02293 }
02294
02295 void CloudViewer::setInteractorLayer(int layer)
02296 {
02297 _visualizer->getRendererCollection()->InitTraversal ();
02298 vtkRenderer* renderer = NULL;
02299 int i =0;
02300 while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
02301 {
02302 if(i==layer)
02303 {
02304 _visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
02305 _visualizer->getInteractorStyle()->SetCurrentRenderer(renderer);
02306 return;
02307 }
02308 ++i;
02309 }
02310 UWARN("Could not set layer %d to interactor (layers=%d).", layer, _visualizer->getRendererCollection()->GetNumberOfItems());
02311 }
02312
02313 void CloudViewer::getCameraPosition(
02314 float & x, float & y, float & z,
02315 float & focalX, float & focalY, float & focalZ,
02316 float & upX, float & upY, float & upZ) const
02317 {
02318 std::vector<pcl::visualization::Camera> cameras;
02319 _visualizer->getCameras(cameras);
02320 if(cameras.size())
02321 {
02322 x = cameras.begin()->pos[0];
02323 y = cameras.begin()->pos[1];
02324 z = cameras.begin()->pos[2];
02325 focalX = cameras.begin()->focal[0];
02326 focalY = cameras.begin()->focal[1];
02327 focalZ = cameras.begin()->focal[2];
02328 upX = cameras.begin()->view[0];
02329 upY = cameras.begin()->view[1];
02330 upZ = cameras.begin()->view[2];
02331 }
02332 else
02333 {
02334 UERROR("No camera set!?");
02335 }
02336 }
02337
02338 void CloudViewer::setCameraPosition(
02339 float x, float y, float z,
02340 float focalX, float focalY, float focalZ,
02341 float upX, float upY, float upZ)
02342 {
02343 _lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
02344 _visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ, 1);
02345 }
02346
02347 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
02348 {
02349 if(!pose.isNull())
02350 {
02351 Eigen::Affine3f m = pose.toEigen3f();
02352 Eigen::Vector3f pos = m.translation();
02353
02354 Eigen::Vector3f lastPos(0,0,0);
02355 if(_trajectory->size())
02356 {
02357 lastPos[0]=_trajectory->back().x;
02358 lastPos[1]=_trajectory->back().y;
02359 lastPos[2]=_trajectory->back().z;
02360 }
02361
02362 _trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2]));
02363 if(_maxTrajectorySize>0)
02364 {
02365 while(_trajectory->size() > _maxTrajectorySize)
02366 {
02367 _trajectory->erase(_trajectory->begin());
02368 }
02369 }
02370 if(_aShowTrajectory->isChecked())
02371 {
02372 _visualizer->removeShape("trajectory");
02373 pcl::PolygonMesh mesh;
02374 pcl::Vertices vertices;
02375 vertices.vertices.resize(_trajectory->size());
02376 for(unsigned int i=0; i<vertices.vertices.size(); ++i)
02377 {
02378 vertices.vertices[i] = i;
02379 }
02380 pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
02381 mesh.polygons.push_back(vertices);
02382 _visualizer->addPolylineFromPolygonMesh(mesh, "trajectory", 1);
02383 }
02384
02385 if(pose != _lastPose || _lastPose.isNull())
02386 {
02387 if(_lastPose.isNull())
02388 {
02389 _lastPose.setIdentity();
02390 }
02391
02392 std::vector<pcl::visualization::Camera> cameras;
02393 _visualizer->getCameras(cameras);
02394
02395 if(_aLockCamera->isChecked() || _aCameraOrtho->isChecked())
02396 {
02397
02398 Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
02399 cameras.front().pos[0] += diff[0];
02400 cameras.front().pos[1] += diff[1];
02401 cameras.front().pos[2] += diff[2];
02402 cameras.front().focal[0] += diff[0];
02403 cameras.front().focal[1] += diff[1];
02404 cameras.front().focal[2] += diff[2];
02405 }
02406 else if(_aFollowCamera->isChecked())
02407 {
02408 Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
02409 cameras.front().focal[1] - cameras.front().pos[1],
02410 cameras.front().focal[2] - cameras.front().pos[2]).normalized();
02411 Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
02412 Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
02413 Eigen::Vector3f xAxis = yAxis.cross(zAxis);
02414 Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
02415 yAxis[0], yAxis[1], yAxis[2],0,
02416 zAxis[0], zAxis[1], zAxis[2],0);
02417
02418 PR.normalizeRotation();
02419
02420 Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
02421 PR[4], PR[5], PR[6], cameras.front().pos[1],
02422 PR[8], PR[9], PR[10], cameras.front().pos[2]);
02423 Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
02424 PR[4], PR[5], PR[6], cameras.front().focal[1],
02425 PR[8], PR[9], PR[10], cameras.front().focal[2]);
02426 Transform N = pose;
02427 Transform O = _lastPose;
02428 Transform O2N = O.inverse()*N;
02429 Transform F2O = F.inverse()*O;
02430 Transform T = F2O * O2N * F2O.inverse();
02431 Transform Fp = F * T;
02432 Transform P2F = P.inverse()*F;
02433 Transform Pp = P * P2F * T * P2F.inverse();
02434
02435 cameras.front().pos[0] = Pp.x();
02436 cameras.front().pos[1] = Pp.y();
02437 cameras.front().pos[2] = Pp.z();
02438 cameras.front().focal[0] = Fp.x();
02439 cameras.front().focal[1] = Fp.y();
02440 cameras.front().focal[2] = Fp.z();
02441
02442 cameras.front().view[0] = _aLockViewZ->isChecked()?0:Fp[8];
02443 cameras.front().view[1] = _aLockViewZ->isChecked()?0:Fp[9];
02444 cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10];
02445 }
02446
02447 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
02448 if(_coordinates.find("reference") != _coordinates.end())
02449 {
02450 this->updateCoordinatePose("reference", pose);
02451 }
02452 else
02453 #endif
02454 {
02455 this->addOrUpdateCoordinate("reference", pose, 0.2);
02456 }
02457
02458 _visualizer->setCameraPosition(
02459 cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
02460 cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
02461 cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
02462 }
02463 }
02464
02465 _lastPose = pose;
02466 }
02467
02468 void CloudViewer::updateCameraFrustum(const Transform & pose, const StereoCameraModel & model)
02469 {
02470 std::vector<CameraModel> models;
02471 models.push_back(model.left());
02472 CameraModel right = model.right();
02473 if(!model.left().localTransform().isNull())
02474 {
02475 right.setLocalTransform(model.left().localTransform() * Transform(model.baseline(), 0, 0, 0, 0, 0));
02476 }
02477 models.push_back(right);
02478 updateCameraFrustums(pose, models);
02479 }
02480
02481 void CloudViewer::updateCameraFrustum(const Transform & pose, const CameraModel & model)
02482 {
02483 std::vector<CameraModel> models;
02484 models.push_back(model);
02485 updateCameraFrustums(pose, models);
02486 }
02487
02488 void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector<CameraModel> & models)
02489 {
02490 if(!pose.isNull())
02491 {
02492 if(_aShowFrustum->isChecked())
02493 {
02494 Transform baseToCamera;
02495 for(unsigned int i=0; i<models.size(); ++i)
02496 {
02497 baseToCamera = Transform::getIdentity();
02498 if(!models[i].localTransform().isNull() && !models[i].localTransform().isIdentity())
02499 {
02500 baseToCamera = models[i].localTransform();
02501 }
02502 std::string id = uFormat("reference_frustum_%d", i);
02503 this->removeFrustum(id);
02504 this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor);
02505 if(!baseToCamera.isIdentity())
02506 {
02507 this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);
02508 }
02509 }
02510 }
02511 }
02512 }
02513
02514 const QColor & CloudViewer::getDefaultBackgroundColor() const
02515 {
02516 return _defaultBgColor;
02517 }
02518
02519 void CloudViewer::setDefaultBackgroundColor(const QColor & color)
02520 {
02521 if(_currentBgColor == _defaultBgColor)
02522 {
02523 setBackgroundColor(color);
02524 }
02525 _defaultBgColor = color;
02526 }
02527
02528 const QColor & CloudViewer::getBackgroundColor() const
02529 {
02530 return _currentBgColor;
02531 }
02532
02533 void CloudViewer::setBackgroundColor(const QColor & color)
02534 {
02535 _currentBgColor = color;
02536 _visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
02537 }
02538
02539 void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
02540 {
02541 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02542 pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
02543 if(iter != cloudActorMap->end())
02544 {
02545 iter->second.actor->SetVisibility(isVisible?1:0);
02546
02547 iter = cloudActorMap->find(id+"-normals");
02548 if(iter != cloudActorMap->end())
02549 {
02550 iter->second.actor->SetVisibility(isVisible&&_aShowNormals->isChecked()?1:0);
02551 }
02552 }
02553 else
02554 {
02555 UERROR("Cannot find actor named \"%s\".", id.c_str());
02556 }
02557 }
02558
02559 bool CloudViewer::getCloudVisibility(const std::string & id)
02560 {
02561 pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
02562 pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
02563 if(iter != cloudActorMap->end())
02564 {
02565 return iter->second.actor->GetVisibility() != 0;
02566 }
02567 else
02568 {
02569 UERROR("Cannot find actor named \"%s\".", id.c_str());
02570 }
02571 return false;
02572 }
02573
02574 void CloudViewer::setCloudColorIndex(const std::string & id, int index)
02575 {
02576 if(index>0)
02577 {
02578 _visualizer->updateColorHandlerIndex(id, index-1);
02579 }
02580 }
02581
02582 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
02583 {
02584 double lastOpacity;
02585 _visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id);
02586 if(lastOpacity != opacity)
02587 {
02588 _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
02589 }
02590 }
02591
02592 void CloudViewer::setCloudPointSize(const std::string & id, int size)
02593 {
02594 double lastSize;
02595 _visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id);
02596 if((int)lastSize != size)
02597 {
02598 _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
02599 }
02600 }
02601
02602 void CloudViewer::setCameraTargetLocked(bool enabled)
02603 {
02604 _aLockCamera->setChecked(enabled);
02605 }
02606
02607 void CloudViewer::setCameraTargetFollow(bool enabled)
02608 {
02609 _aFollowCamera->setChecked(enabled);
02610 }
02611
02612 void CloudViewer::setCameraFree()
02613 {
02614 _aLockCamera->setChecked(false);
02615 _aFollowCamera->setChecked(false);
02616 }
02617
02618 void CloudViewer::setCameraLockZ(bool enabled)
02619 {
02620 _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
02621 _aLockViewZ->setChecked(enabled);
02622 }
02623 void CloudViewer::setCameraOrtho(bool enabled)
02624 {
02625 _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
02626 CloudViewerInteractorStyle * interactor = CloudViewerInteractorStyle::SafeDownCast(this->GetInteractor()->GetInteractorStyle());
02627 if(interactor)
02628 {
02629 interactor->setOrthoMode(enabled);
02630 this->update();
02631 }
02632 _aCameraOrtho->setChecked(enabled);
02633 }
02634 bool CloudViewer::isCameraTargetLocked() const
02635 {
02636 return _aLockCamera->isChecked();
02637 }
02638 bool CloudViewer::isCameraTargetFollow() const
02639 {
02640 return _aFollowCamera->isChecked();
02641 }
02642 bool CloudViewer::isCameraFree() const
02643 {
02644 return !_aFollowCamera->isChecked() && !_aLockCamera->isChecked();
02645 }
02646 bool CloudViewer::isCameraLockZ() const
02647 {
02648 return _aLockViewZ->isChecked();
02649 }
02650 bool CloudViewer::isCameraOrtho() const
02651 {
02652 return _aCameraOrtho->isChecked();
02653 }
02654 double CloudViewer::getRenderingRate() const
02655 {
02656 return _renderingRate;
02657 }
02658
02659 void CloudViewer::setGridShown(bool shown)
02660 {
02661 _aShowGrid->setChecked(shown);
02662 if(shown)
02663 {
02664 this->addGrid();
02665 }
02666 else
02667 {
02668 this->removeGrid();
02669 }
02670 }
02671 bool CloudViewer::isGridShown() const
02672 {
02673 return _aShowGrid->isChecked();
02674 }
02675 unsigned int CloudViewer::getGridCellCount() const
02676 {
02677 return _gridCellCount;
02678 }
02679 float CloudViewer::getGridCellSize() const
02680 {
02681 return _gridCellSize;
02682 }
02683 void CloudViewer::setGridCellCount(unsigned int count)
02684 {
02685 if(count > 0)
02686 {
02687 _gridCellCount = count;
02688 if(_aShowGrid->isChecked())
02689 {
02690 this->removeGrid();
02691 this->addGrid();
02692 }
02693 }
02694 else
02695 {
02696 UERROR("Cannot set grid cell count < 1, count=%d", count);
02697 }
02698 }
02699 void CloudViewer::setGridCellSize(float size)
02700 {
02701 if(size > 0)
02702 {
02703 _gridCellSize = size;
02704 if(_aShowGrid->isChecked())
02705 {
02706 this->removeGrid();
02707 this->addGrid();
02708 }
02709 }
02710 else
02711 {
02712 UERROR("Cannot set grid cell size <= 0, value=%f", size);
02713 }
02714 }
02715 void CloudViewer::addGrid()
02716 {
02717 if(_gridLines.empty())
02718 {
02719 float cellSize = _gridCellSize;
02720 int cellCount = _gridCellCount;
02721 double r=0.5;
02722 double g=0.5;
02723 double b=0.5;
02724 int id = 0;
02725 float min = -float(cellCount/2) * cellSize;
02726 float max = float(cellCount/2) * cellSize;
02727 std::string name;
02728 for(float i=min; i<=max; i += cellSize)
02729 {
02730
02731 name = uFormat("line%d", ++id);
02732 _visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name, 1);
02733 _gridLines.push_back(name);
02734
02735 name = uFormat("line%d", ++id);
02736 _visualizer->addLine(
02737 pcl::PointXYZ(min, i, 0),
02738 pcl::PointXYZ(max, i, 0),
02739 r, g, b, name, 1);
02740 _gridLines.push_back(name);
02741 }
02742 }
02743 }
02744
02745 void CloudViewer::removeGrid()
02746 {
02747 for(std::list<std::string>::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter)
02748 {
02749 _visualizer->removeShape(*iter);
02750 }
02751 _gridLines.clear();
02752 }
02753
02754 void CloudViewer::setNormalsShown(bool shown)
02755 {
02756 _aShowNormals->setChecked(shown);
02757 QList<std::string> ids = _addedClouds.keys();
02758 for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
02759 {
02760 std::string idNormals = *iter + "-normals";
02761 if(_addedClouds.find(idNormals) != _addedClouds.end())
02762 {
02763 this->setCloudVisibility(idNormals, this->getCloudVisibility(*iter) && shown);
02764 }
02765 }
02766 }
02767 bool CloudViewer::isNormalsShown() const
02768 {
02769 return _aShowNormals->isChecked();
02770 }
02771 int CloudViewer::getNormalsStep() const
02772 {
02773 return _normalsStep;
02774 }
02775 float CloudViewer::getNormalsScale() const
02776 {
02777 return _normalsScale;
02778 }
02779 void CloudViewer::setNormalsStep(int step)
02780 {
02781 if(step > 0)
02782 {
02783 _normalsStep = step;
02784 }
02785 else
02786 {
02787 UERROR("Cannot set normals step <= 0, step=%d", step);
02788 }
02789 }
02790 void CloudViewer::setNormalsScale(float scale)
02791 {
02792 if(scale > 0)
02793 {
02794 _normalsScale= scale;
02795 }
02796 else
02797 {
02798 UERROR("Cannot set normals scale <= 0, value=%f", scale);
02799 }
02800 }
02801
02802 void CloudViewer::buildPickingLocator(bool enable)
02803 {
02804 _buildLocator = enable;
02805 }
02806
02807 Eigen::Vector3f rotatePointAroundAxe(
02808 const Eigen::Vector3f & point,
02809 const Eigen::Vector3f & axis,
02810 float angle)
02811 {
02812 Eigen::Vector3f direction = point;
02813 Eigen::Vector3f zAxis = axis;
02814 float dotProdZ = zAxis.dot(direction);
02815 Eigen::Vector3f ptOnZaxis = zAxis * dotProdZ;
02816 direction -= ptOnZaxis;
02817 Eigen::Vector3f xAxis = direction.normalized();
02818 Eigen::Vector3f yAxis = zAxis.cross(xAxis);
02819
02820 Eigen::Matrix3f newFrame;
02821 newFrame << xAxis[0], yAxis[0], zAxis[0],
02822 xAxis[1], yAxis[1], zAxis[1],
02823 xAxis[2], yAxis[2], zAxis[2];
02824
02825
02826
02827 Eigen::Vector3f newDirection = newFrame.transpose() * direction;
02828
02829
02830 float cosTheta = cos(angle);
02831 float sinTheta = sin(angle);
02832 float magnitude = newDirection.norm();
02833 newDirection[0] = ( magnitude * cosTheta );
02834 newDirection[1] = ( magnitude * sinTheta );
02835
02836
02837 direction = newFrame * newDirection;
02838
02839 return direction + ptOnZaxis;
02840 }
02841
02842 void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
02843 if(event->key() == Qt::Key_Up ||
02844 event->key() == Qt::Key_Down ||
02845 event->key() == Qt::Key_Left ||
02846 event->key() == Qt::Key_Right)
02847 {
02848 _keysPressed -= (Qt::Key)event->key();
02849 }
02850 else
02851 {
02852 QVTKWidget::keyPressEvent(event);
02853 }
02854 }
02855
02856 void CloudViewer::keyPressEvent(QKeyEvent * event)
02857 {
02858 if(event->key() == Qt::Key_Up ||
02859 event->key() == Qt::Key_Down ||
02860 event->key() == Qt::Key_Left ||
02861 event->key() == Qt::Key_Right)
02862 {
02863 _keysPressed += (Qt::Key)event->key();
02864
02865 std::vector<pcl::visualization::Camera> cameras;
02866 _visualizer->getCameras(cameras);
02867
02868
02869 Eigen::Vector3f pos(cameras.front().pos[0], cameras.front().pos[1], _aLockViewZ->isChecked()?0:cameras.front().pos[2]);
02870 Eigen::Vector3f focal(cameras.front().focal[0], cameras.front().focal[1], _aLockViewZ->isChecked()?0:cameras.front().focal[2]);
02871 Eigen::Vector3f viewUp(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
02872 Eigen::Vector3f cummulatedDir(0,0,0);
02873 Eigen::Vector3f cummulatedFocalDir(0,0,0);
02874 float step = 0.2f;
02875 float stepRot = 0.02f;
02876 if(_keysPressed.contains(Qt::Key_Up))
02877 {
02878 Eigen::Vector3f dir;
02879 if(event->modifiers() & Qt::ShiftModifier)
02880 {
02881 dir = viewUp * step;
02882 }
02883 else
02884 {
02885 dir = (focal-pos).normalized() * step;
02886 }
02887 cummulatedDir += dir;
02888 }
02889 if(_keysPressed.contains(Qt::Key_Down))
02890 {
02891 Eigen::Vector3f dir;
02892 if(event->modifiers() & Qt::ShiftModifier)
02893 {
02894 dir = viewUp * -step;
02895 }
02896 else
02897 {
02898 dir = (focal-pos).normalized() * -step;
02899 }
02900 cummulatedDir += dir;
02901 }
02902 if(_keysPressed.contains(Qt::Key_Right))
02903 {
02904 if(event->modifiers() & Qt::ShiftModifier)
02905 {
02906
02907 Eigen::Vector3f point = (focal-pos);
02908 Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, -stepRot);
02909 Eigen::Vector3f diff = newPoint - point;
02910 cummulatedFocalDir += diff;
02911 }
02912 else
02913 {
02914 Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * step;
02915 cummulatedDir += dir;
02916 }
02917 }
02918 if(_keysPressed.contains(Qt::Key_Left))
02919 {
02920 if(event->modifiers() & Qt::ShiftModifier)
02921 {
02922
02923 Eigen::Vector3f point = (focal-pos);
02924 Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, stepRot);
02925 Eigen::Vector3f diff = newPoint - point;
02926 cummulatedFocalDir += diff;
02927 }
02928 else
02929 {
02930 Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * -step;
02931 cummulatedDir += dir;
02932 }
02933 }
02934
02935 cameras.front().pos[0] += cummulatedDir[0];
02936 cameras.front().pos[1] += cummulatedDir[1];
02937 cameras.front().pos[2] += cummulatedDir[2];
02938 cameras.front().focal[0] += cummulatedDir[0] + cummulatedFocalDir[0];
02939 cameras.front().focal[1] += cummulatedDir[1] + cummulatedFocalDir[1];
02940 cameras.front().focal[2] += cummulatedDir[2] + cummulatedFocalDir[2];
02941 _visualizer->setCameraPosition(
02942 cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
02943 cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
02944 cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
02945
02946 update();
02947
02948 Q_EMIT configChanged();
02949 }
02950 else
02951 {
02952 QVTKWidget::keyPressEvent(event);
02953 }
02954 }
02955
02956 void CloudViewer::mousePressEvent(QMouseEvent * event)
02957 {
02958 if(event->button() == Qt::RightButton)
02959 {
02960 event->accept();
02961 }
02962 else
02963 {
02964 QVTKWidget::mousePressEvent(event);
02965 }
02966 }
02967
02968 void CloudViewer::mouseMoveEvent(QMouseEvent * event)
02969 {
02970 QVTKWidget::mouseMoveEvent(event);
02971
02972
02973 if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
02974 {
02975 std::vector<pcl::visualization::Camera> cameras;
02976 _visualizer->getCameras(cameras);
02977
02978 cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
02979
02980 if( _lastCameraOrientation!=cv::Vec3d(0,0,0) &&
02981 _lastCameraPose!=cv::Vec3d(0,0,0) &&
02982 (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) &&
02983 uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1])))
02984 {
02985 cameras.front().pos[0] = _lastCameraPose[0];
02986 cameras.front().pos[1] = _lastCameraPose[1];
02987 cameras.front().pos[2] = _lastCameraPose[2];
02988 }
02989 else if(newCameraOrientation != cv::Vec3d(0,0,0))
02990 {
02991 _lastCameraOrientation = newCameraOrientation;
02992 _lastCameraPose = cv::Vec3d(cameras.front().pos);
02993 }
02994 else
02995 {
02996 if(cameras.front().view[2] == 0)
02997 {
02998 cameras.front().pos[0] -= 0.00001*cameras.front().view[0];
02999 cameras.front().pos[1] -= 0.00001*cameras.front().view[1];
03000 }
03001 else
03002 {
03003 cameras.front().pos[0] -= 0.00001;
03004 }
03005 }
03006 cameras.front().view[0] = 0;
03007 cameras.front().view[1] = 0;
03008 cameras.front().view[2] = 1;
03009
03010 _visualizer->setCameraPosition(
03011 cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
03012 cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
03013 cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
03014
03015 }
03016 this->update();
03017
03018 Q_EMIT configChanged();
03019 }
03020
03021 void CloudViewer::wheelEvent(QWheelEvent * event)
03022 {
03023 QVTKWidget::wheelEvent(event);
03024 if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
03025 {
03026 std::vector<pcl::visualization::Camera> cameras;
03027 _visualizer->getCameras(cameras);
03028 _lastCameraPose = cv::Vec3d(cameras.front().pos);
03029 }
03030 Q_EMIT configChanged();
03031 }
03032
03033 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
03034 {
03035 QAction * a = _menu->exec(event->globalPos());
03036 if(a)
03037 {
03038 handleAction(a);
03039 Q_EMIT configChanged();
03040 }
03041 }
03042
03043 void CloudViewer::handleAction(QAction * a)
03044 {
03045 if(a == _aSetTrajectorySize)
03046 {
03047 bool ok;
03048 int value = QInputDialog::getInt(this, tr("Set trajectory size"), tr("Size (0=infinite)"), _maxTrajectorySize, 0, 10000, 10, &ok);
03049 if(ok)
03050 {
03051 _maxTrajectorySize = value;
03052 }
03053 }
03054 else if(a == _aClearTrajectory)
03055 {
03056 this->clearTrajectory();
03057 }
03058 else if(a == _aShowFrustum)
03059 {
03060 this->setFrustumShown(a->isChecked());
03061 }
03062 else if(a == _aSetFrustumScale)
03063 {
03064 bool ok;
03065 double value = QInputDialog::getDouble(this, tr("Set frustum scale"), tr("Scale"), _frustumScale, 0.0, 999.0, 1, &ok);
03066 if(ok)
03067 {
03068 this->setFrustumScale(value);
03069 }
03070 }
03071 else if(a == _aSetFrustumColor)
03072 {
03073 QColor value = QColorDialog::getColor(_frustumColor, this);
03074 if(value.isValid())
03075 {
03076 this->setFrustumColor(value);
03077 }
03078 }
03079 else if(a == _aResetCamera)
03080 {
03081 this->resetCamera();
03082 }
03083 else if(a == _aShowGrid)
03084 {
03085 if(_aShowGrid->isChecked())
03086 {
03087 this->addGrid();
03088 }
03089 else
03090 {
03091 this->removeGrid();
03092 }
03093
03094 this->update();
03095 }
03096 else if(a == _aSetGridCellCount)
03097 {
03098 bool ok;
03099 int value = QInputDialog::getInt(this, tr("Set grid cell count"), tr("Count"), _gridCellCount, 1, 10000, 10, &ok);
03100 if(ok)
03101 {
03102 this->setGridCellCount(value);
03103 }
03104 }
03105 else if(a == _aSetGridCellSize)
03106 {
03107 bool ok;
03108 double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
03109 if(ok)
03110 {
03111 this->setGridCellSize(value);
03112 }
03113 }
03114 else if(a == _aShowNormals)
03115 {
03116 this->setNormalsShown(_aShowNormals->isChecked());
03117 this->update();
03118 }
03119 else if(a == _aSetNormalsStep)
03120 {
03121 bool ok;
03122 int value = QInputDialog::getInt(this, tr("Set normals step"), tr("Step"), _normalsStep, 1, 10000, 1, &ok);
03123 if(ok)
03124 {
03125 this->setNormalsStep(value);
03126 }
03127 }
03128 else if(a == _aSetNormalsScale)
03129 {
03130 bool ok;
03131 double value = QInputDialog::getDouble(this, tr("Set normals scale"), tr("Scale (m)"), _normalsScale, 0.01, 10, 2, &ok);
03132 if(ok)
03133 {
03134 this->setNormalsScale(value);
03135 }
03136 }
03137 else if(a == _aSetBackgroundColor)
03138 {
03139 QColor color = this->getDefaultBackgroundColor();
03140 color = QColorDialog::getColor(color, this);
03141 if(color.isValid())
03142 {
03143 this->setDefaultBackgroundColor(color);
03144 this->update();
03145 }
03146 }
03147 else if(a == _aSetRenderingRate)
03148 {
03149 bool ok;
03150 double value = QInputDialog::getDouble(this, tr("Rendering rate"), tr("Rate (hz)"), _renderingRate, 0, 60, 0, &ok);
03151 if(ok)
03152 {
03153 this->setRenderingRate(value);
03154 }
03155 }
03156 else if(a == _aLockViewZ)
03157 {
03158 if(_aLockViewZ->isChecked())
03159 {
03160 this->update();
03161 }
03162 }
03163 else if(a == _aCameraOrtho)
03164 {
03165 this->setCameraOrtho(_aCameraOrtho->isChecked());
03166 }
03167 else if(a == _aSetLighting)
03168 {
03169 this->setLighting(_aSetLighting->isChecked());
03170 }
03171 else if(a == _aSetFlatShading)
03172 {
03173 this->setShading(_aSetFlatShading->isChecked());
03174 }
03175 else if(a == _aSetEdgeVisibility)
03176 {
03177 this->setEdgeVisibility(_aSetEdgeVisibility->isChecked());
03178 }
03179 else if(a == _aBackfaceCulling)
03180 {
03181 this->setBackfaceCulling(_aBackfaceCulling->isChecked(), _frontfaceCulling);
03182 }
03183 else if(a == _aPolygonPicking)
03184 {
03185 this->setPolygonPicking(_aPolygonPicking->isChecked());
03186 }
03187 }
03188
03189 }