CloudViewer.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/gui/CloudViewer.h"
00029 
00030 #include <rtabmap/core/Version.h>
00031 #include <rtabmap/utilite/ULogger.h>
00032 #include <rtabmap/utilite/UTimer.h>
00033 #include <rtabmap/utilite/UMath.h>
00034 #include <rtabmap/utilite/UConversion.h>
00035 #include <rtabmap/utilite/UStl.h>
00036 #include <pcl/visualization/pcl_visualizer.h>
00037 #include <pcl/common/transforms.h>
00038 #include <QMenu>
00039 #include <QAction>
00040 #include <QtGui/QContextMenuEvent>
00041 #include <QInputDialog>
00042 #include <QtGui/QWheelEvent>
00043 #include <QtGui/QKeyEvent>
00044 #include <QColorDialog>
00045 #include <QtGui/QVector3D>
00046 #include <QMainWindow>
00047 #include <set>
00048 
00049 #include <vtkCamera.h>
00050 #include <vtkRenderWindow.h>
00051 #include <vtkCubeSource.h>
00052 #include <vtkGlyph3D.h>
00053 #include <vtkGlyph3DMapper.h>
00054 #include <vtkLookupTable.h>
00055 
00056 #ifdef RTABMAP_OCTOMAP
00057 #include <rtabmap/core/OctoMap.h>
00058 #endif
00059 
00060 namespace rtabmap {
00061 
00062 class MyInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
00063 {
00064 public:
00065         virtual void Rotate()
00066         {
00067                 if (this->CurrentRenderer == NULL)
00068                 {
00069                         return;
00070                 }
00071 
00072                 vtkRenderWindowInteractor *rwi = this->Interactor;
00073 
00074                 int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
00075                 int dy = rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
00076 
00077                 int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
00078 
00079                 double delta_elevation = -20.0 / size[1];
00080                 double delta_azimuth = -20.0 / size[0];
00081 
00082                 double rxf = dx * delta_azimuth * this->MotionFactor;
00083                 double ryf = dy * delta_elevation * this->MotionFactor;
00084 
00085                 vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
00086                 camera->Azimuth(rxf);
00087                 camera->Elevation(ryf);
00088                 camera->OrthogonalizeViewUp();
00089 
00090                 if (this->AutoAdjustCameraClippingRange)
00091                 {
00092                         this->CurrentRenderer->ResetCameraClippingRange();
00093                 }
00094 
00095                 if (rwi->GetLightFollowCamera())
00096                 {
00097                         this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
00098                 }
00099 
00100                 //rwi->Render();
00101         }
00102 };
00103 
00104 
00105 CloudViewer::CloudViewer(QWidget *parent) :
00106                 QVTKWidget(parent),
00107                 _aLockCamera(0),
00108                 _aFollowCamera(0),
00109                 _aResetCamera(0),
00110                 _aLockViewZ(0),
00111                 _aShowTrajectory(0),
00112                 _aSetTrajectorySize(0),
00113                 _aClearTrajectory(0),
00114                 _aShowFrustum(0),
00115                 _aSetFrustumScale(0),
00116                 _aSetFrustumColor(0),
00117                 _aShowGrid(0),
00118                 _aSetGridCellCount(0),
00119                 _aSetGridCellSize(0),
00120                 _aSetBackgroundColor(0),
00121                 _menu(0),
00122                 _trajectory(new pcl::PointCloud<pcl::PointXYZ>),
00123                 _maxTrajectorySize(100),
00124                 _frustumScale(0.5f),
00125                 _frustumColor(Qt::gray),
00126                 _gridCellCount(50),
00127                 _gridCellSize(1),
00128                 _lastCameraOrientation(0,0,0),
00129                 _lastCameraPose(0,0,0),
00130                 _workingDirectory("."),
00131                 _defaultBgColor(Qt::black),
00132                 _currentBgColor(Qt::black),
00133                 _backfaceCulling(false),
00134                 _frontfaceCulling(false),
00135                 _renderingRate(5.0),
00136                 _octomapActor(0)
00137 {
00138         UDEBUG("");
00139         this->setMinimumSize(200, 200);
00140 
00141         int argc = 0;
00142         _visualizer = new pcl::visualization::PCLVisualizer(
00143                 argc, 
00144                 0, 
00145                 "PCLVisualizer", 
00146                 vtkSmartPointer<MyInteractorStyle>(new MyInteractorStyle()), 
00147                 false);
00148 
00149         _visualizer->setShowFPS(false);
00150         
00151         this->SetRenderWindow(_visualizer->getRenderWindow());
00152 
00153         // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as
00154         // the "Invalid drawable" warning when the view is not visible.
00155         //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
00156         this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
00157         setRenderingRate(_renderingRate);
00158 
00159         _visualizer->setCameraPosition(
00160                                 -1, 0, 0,
00161                                 0, 0, 0,
00162                                 0, 0, 1);
00163 #ifndef _WIN32
00164         // Crash on startup on Windows (vtk issue)
00165         this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
00166 #endif
00167 
00168         //setup menu/actions
00169         createMenu();
00170 
00171         setMouseTracking(false);
00172 }
00173 
00174 CloudViewer::~CloudViewer()
00175 {
00176         UDEBUG("");
00177         this->clear();
00178         delete _visualizer;
00179         UDEBUG("");
00180 }
00181 
00182 void CloudViewer::clear()
00183 {
00184         this->removeAllClouds();
00185         this->removeAllGraphs();
00186         this->removeAllCoordinates();
00187         this->removeAllLines();
00188         this->removeAllFrustums();
00189         this->removeAllTexts();
00190         this->clearTrajectory();
00191         this->removeOccupancyGridMap();
00192         this->removeOctomap();
00193 
00194         this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
00195 }
00196 
00197 void CloudViewer::createMenu()
00198 {
00199         _aLockCamera = new QAction("Lock target", this);
00200         _aLockCamera->setCheckable(true);
00201         _aLockCamera->setChecked(false);
00202         _aFollowCamera = new QAction("Follow", this);
00203         _aFollowCamera->setCheckable(true);
00204         _aFollowCamera->setChecked(true);
00205         QAction * freeCamera = new QAction("Free", this);
00206         freeCamera->setCheckable(true);
00207         freeCamera->setChecked(false);
00208         _aLockViewZ = new QAction("Lock view Z", this);
00209         _aLockViewZ->setCheckable(true);
00210         _aLockViewZ->setChecked(true);
00211         _aResetCamera = new QAction("Reset position", this);
00212         _aShowTrajectory= new QAction("Show trajectory", this);
00213         _aShowTrajectory->setCheckable(true);
00214         _aShowTrajectory->setChecked(true);
00215         _aSetTrajectorySize = new QAction("Set trajectory size...", this);
00216         _aClearTrajectory = new QAction("Clear trajectory", this);
00217         _aShowFrustum= new QAction("Show frustum", this);
00218         _aShowFrustum->setCheckable(true);
00219         _aShowFrustum->setChecked(false);
00220         _aSetFrustumScale = new QAction("Set frustum scale...", this);
00221         _aSetFrustumColor = new QAction("Set frustum color...", this);
00222         _aShowGrid = new QAction("Show grid", this);
00223         _aShowGrid->setCheckable(true);
00224         _aSetGridCellCount = new QAction("Set cell count...", this);
00225         _aSetGridCellSize = new QAction("Set cell size...", this);
00226         _aSetBackgroundColor = new QAction("Set background color...", this);    
00227         _aSetRenderingRate = new QAction("Set rendering rate...", this);
00228 
00229         QMenu * cameraMenu = new QMenu("Camera", this);
00230         cameraMenu->addAction(_aLockCamera);
00231         cameraMenu->addAction(_aFollowCamera);
00232         cameraMenu->addAction(freeCamera);
00233         cameraMenu->addSeparator();
00234         cameraMenu->addAction(_aLockViewZ);
00235         cameraMenu->addAction(_aResetCamera);
00236         QActionGroup * group = new QActionGroup(this);
00237         group->addAction(_aLockCamera);
00238         group->addAction(_aFollowCamera);
00239         group->addAction(freeCamera);
00240 
00241         QMenu * trajectoryMenu = new QMenu("Trajectory", this);
00242         trajectoryMenu->addAction(_aShowTrajectory);
00243         trajectoryMenu->addAction(_aSetTrajectorySize);
00244         trajectoryMenu->addAction(_aClearTrajectory);
00245 
00246         QMenu * frustumMenu = new QMenu("Frustum", this);
00247         frustumMenu->addAction(_aShowFrustum);
00248         frustumMenu->addAction(_aSetFrustumScale);
00249         frustumMenu->addAction(_aSetFrustumColor);
00250 
00251         QMenu * gridMenu = new QMenu("Grid", this);
00252         gridMenu->addAction(_aShowGrid);
00253         gridMenu->addAction(_aSetGridCellCount);
00254         gridMenu->addAction(_aSetGridCellSize);
00255 
00256         //menus
00257         _menu = new QMenu(this);
00258         _menu->addMenu(cameraMenu);
00259         _menu->addMenu(trajectoryMenu);
00260         _menu->addMenu(frustumMenu);
00261         _menu->addMenu(gridMenu);
00262         _menu->addAction(_aSetBackgroundColor);
00263         _menu->addAction(_aSetRenderingRate);
00264 }
00265 
00266 void CloudViewer::saveSettings(QSettings & settings, const QString & group) const
00267 {
00268         if(!group.isEmpty())
00269         {
00270                 settings.beginGroup(group);
00271         }
00272 
00273         float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
00274         this->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
00275         QVector3D pose(poseX, poseY, poseZ);
00276         QVector3D focal(focalX, focalY, focalZ);
00277         if(!this->isCameraFree())
00278         {
00279                 // make camera position relative to target
00280                 Transform T = this->getTargetPose();
00281                 if(this->isCameraTargetLocked())
00282                 {
00283                         T = Transform(T.x(), T.y(), T.z(), 0,0,0);
00284                 }
00285                 Transform F(focalX, focalY, focalZ, 0,0,0);
00286                 Transform P(poseX, poseY, poseZ, 0,0,0);
00287                 Transform newFocal = T.inverse() * F;
00288                 Transform newPose = newFocal * F.inverse() * P;
00289                 pose = QVector3D(newPose.x(), newPose.y(), newPose.z());
00290                 focal = QVector3D(newFocal.x(), newFocal.y(), newFocal.z());
00291         }
00292         settings.setValue("camera_pose", pose);
00293         settings.setValue("camera_focal", focal);
00294         settings.setValue("camera_up", QVector3D(upX, upY, upZ));
00295 
00296         settings.setValue("grid", this->isGridShown());
00297         settings.setValue("grid_cell_count", this->getGridCellCount());
00298         settings.setValue("grid_cell_size", (double)this->getGridCellSize());
00299 
00300         settings.setValue("trajectory_shown", this->isTrajectoryShown());
00301         settings.setValue("trajectory_size", this->getTrajectorySize());
00302 
00303         settings.setValue("frustum_shown", this->isFrustumShown());
00304         settings.setValue("frustum_scale", this->getFrustumScale());
00305         settings.setValue("frustum_color", this->getFrustumColor());
00306 
00307         settings.setValue("camera_target_locked", this->isCameraTargetLocked());
00308         settings.setValue("camera_target_follow", this->isCameraTargetFollow());
00309         settings.setValue("camera_free", this->isCameraFree());
00310         settings.setValue("camera_lockZ", this->isCameraLockZ());
00311 
00312         settings.setValue("bg_color", this->getDefaultBackgroundColor());
00313         settings.setValue("rendering_rate", this->getRenderingRate());
00314         if(!group.isEmpty())
00315         {
00316                 settings.endGroup();
00317         }
00318 }
00319 
00320 void CloudViewer::loadSettings(QSettings & settings, const QString & group)
00321 {
00322         if(!group.isEmpty())
00323         {
00324                 settings.beginGroup(group);
00325         }
00326 
00327         float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
00328         this->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
00329         QVector3D pose(poseX, poseY, poseZ), focal(focalX, focalY, focalZ), up(upX, upY, upZ);
00330         pose = settings.value("camera_pose", pose).value<QVector3D>();
00331         focal = settings.value("camera_focal", focal).value<QVector3D>();
00332         up = settings.value("camera_up", up).value<QVector3D>();
00333         this->setCameraPosition(pose.x(),pose.y(),pose.z(), focal.x(),focal.y(),focal.z(), up.x(),up.y(),up.z());
00334 
00335         this->setGridShown(settings.value("grid", this->isGridShown()).toBool());
00336         this->setGridCellCount(settings.value("grid_cell_count", this->getGridCellCount()).toUInt());
00337         this->setGridCellSize(settings.value("grid_cell_size", this->getGridCellSize()).toFloat());
00338 
00339         this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
00340         this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
00341 
00342         this->setFrustumShown(settings.value("frustum_shown", this->isFrustumShown()).toBool());
00343         this->setFrustumScale(settings.value("frustum_scale", this->getFrustumScale()).toDouble());
00344         this->setFrustumColor(settings.value("frustum_color", this->getFrustumColor()).value<QColor>());
00345 
00346         this->setCameraTargetLocked(settings.value("camera_target_locked", this->isCameraTargetLocked()).toBool());
00347         this->setCameraTargetFollow(settings.value("camera_target_follow", this->isCameraTargetFollow()).toBool());
00348         if(settings.value("camera_free", this->isCameraFree()).toBool())
00349         {
00350                 this->setCameraFree();
00351         }
00352         this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());
00353 
00354         this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
00355         
00356         this->setRenderingRate(settings.value("rendering_rate", this->getRenderingRate()).toDouble());
00357 
00358         if(!group.isEmpty())
00359         {
00360                 settings.endGroup();
00361         }
00362 
00363         this->update();
00364 }
00365 
00366 bool CloudViewer::updateCloudPose(
00367                 const std::string & id,
00368                 const Transform & pose)
00369 {
00370         if(_addedClouds.contains(id))
00371         {
00372                 UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
00373                 if(_addedClouds.find(id).value() == pose ||
00374                    _visualizer->updatePointCloudPose(id, pose.toEigen3f()))
00375                 {
00376                         _addedClouds.find(id).value() = pose;
00377                         return true;
00378                 }
00379         }
00380         return false;
00381 }
00382 
00383 bool CloudViewer::addCloud(
00384                 const std::string & id,
00385                 const pcl::PCLPointCloud2Ptr & binaryCloud,
00386                 const Transform & pose,
00387                 bool rgb,
00388                 bool haveNormals,
00389                 const QColor & color)
00390 {
00391         int previousColorIndex = -1;
00392         if(_addedClouds.contains(id))
00393         {
00394                 previousColorIndex = _visualizer->getColorHandlerIndex(id);
00395                 this->removeCloud(id);
00396         }
00397 
00398         Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
00399         Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().rotation());
00400 
00401         // add random color channel
00402          pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
00403          colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
00404          if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
00405          {
00406                 QColor c = Qt::gray;
00407                 if(color.isValid())
00408                 {
00409                         c = color;
00410                 }
00411                 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, c.red(), c.green(), c.blue()));
00412                 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00413 
00414                 // x,y,z
00415                 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
00416                 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00417                 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
00418                 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00419                 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
00420                 _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00421 
00422                 if(rgb)
00423                 {
00424                         //rgb
00425                         colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
00426                         _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00427                 }
00428                 else if(previousColorIndex == 5)
00429                 {
00430                         previousColorIndex = -1;
00431                 }
00432 
00433                 if(haveNormals)
00434                 {
00435                         //normals
00436                         colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
00437                         _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00438                         colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
00439                         _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00440                         colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
00441                         _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
00442                 }
00443                 else if(previousColorIndex > 5)
00444                 {
00445                         previousColorIndex = -1;
00446                 }
00447 
00448                 if(previousColorIndex>=0)
00449                 {
00450                         _visualizer->updateColorHandlerIndex(id, previousColorIndex);
00451                 }
00452                 else if(rgb)
00453                 {
00454                         _visualizer->updateColorHandlerIndex(id, 5);
00455                 }
00456                 else if(color.isValid())
00457                 {
00458                         _visualizer->updateColorHandlerIndex(id, 1);
00459                 }
00460 
00461                 _addedClouds.insert(id, pose);
00462                 return true;
00463         }
00464         return false;
00465 }
00466 
00467 bool CloudViewer::addCloud(
00468                 const std::string & id,
00469                 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00470                 const Transform & pose,
00471                 const QColor & color)
00472 {
00473         pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00474         pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00475         return addCloud(id, binaryCloud, pose, true, true, color);
00476 }
00477 
00478 bool CloudViewer::addCloud(
00479                 const std::string & id,
00480                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00481                 const Transform & pose,
00482                 const QColor & color)
00483 {
00484         pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00485         pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00486         return addCloud(id, binaryCloud, pose, true, false, color);
00487 }
00488 
00489 bool CloudViewer::addCloud(
00490                 const std::string & id,
00491                 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
00492                 const Transform & pose,
00493                 const QColor & color)
00494 {
00495         pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00496         pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00497         return addCloud(id, binaryCloud, pose, false, true, color);
00498 }
00499 
00500 bool CloudViewer::addCloud(
00501                 const std::string & id,
00502                 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
00503                 const Transform & pose,
00504                 const QColor & color)
00505 {
00506         pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
00507         pcl::toPCLPointCloud2(*cloud, *binaryCloud);
00508         return addCloud(id, binaryCloud, pose, false, false, color);
00509 }
00510 
00511 bool CloudViewer::addCloudMesh(
00512         const std::string & id,
00513         const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
00514         const std::vector<pcl::Vertices> & polygons,
00515         const Transform & pose)
00516 {
00517         if(_addedClouds.contains(id))
00518         {
00519                 this->removeCloud(id);
00520         }
00521 
00522         UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
00523         if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id))
00524         {
00525                 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->LightingOff();
00526                 if(_backfaceCulling)
00527                 {
00528                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->BackfaceCullingOn();
00529                 }
00530                 if(_frontfaceCulling)
00531                 {
00532                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->FrontfaceCullingOn();
00533                 }
00534                 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00535                 _addedClouds.insert(id, pose);
00536                 return true;
00537         }
00538         return false;
00539 }
00540 
00541 bool CloudViewer::addCloudMesh(
00542         const std::string & id,
00543         const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00544         const std::vector<pcl::Vertices> & polygons,
00545         const Transform & pose)
00546 {
00547         if(_addedClouds.contains(id))
00548         {
00549                 this->removeCloud(id);
00550         }
00551 
00552         UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
00553         if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id))
00554         {
00555                 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->LightingOff();
00556                 if(_backfaceCulling)
00557                 {
00558                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->BackfaceCullingOn();
00559                 }
00560                 if(_frontfaceCulling)
00561                 {
00562                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->FrontfaceCullingOn();
00563                 }
00564                 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00565                 _addedClouds.insert(id, pose);
00566                 return true;
00567         }
00568         return false;
00569 }
00570 
00571 bool CloudViewer::addCloudMesh(
00572         const std::string & id,
00573         const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00574         const std::vector<pcl::Vertices> & polygons,
00575         const Transform & pose)
00576 {
00577         if(_addedClouds.contains(id))
00578         {
00579                 this->removeCloud(id);
00580         }
00581 
00582         UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
00583         if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id))
00584         {
00585                 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->LightingOff();
00586                 if(_backfaceCulling)
00587                 {
00588                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->BackfaceCullingOn();
00589                 }
00590                 if(_frontfaceCulling)
00591                 {
00592                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->FrontfaceCullingOn();
00593                 }
00594                 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00595                 _addedClouds.insert(id, pose);
00596                 return true;
00597         }
00598         return false;
00599 }
00600 
00601 bool CloudViewer::addCloudMesh(
00602         const std::string & id,
00603         const pcl::PolygonMesh::Ptr & mesh,
00604         const Transform & pose)
00605 {
00606         if(_addedClouds.contains(id))
00607         {
00608                 this->removeCloud(id);
00609         }
00610 
00611         UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
00612         if(_visualizer->addPolygonMesh(*mesh, id))
00613         {
00614                 _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->LightingOff();
00615                 if(_backfaceCulling)
00616                 {
00617                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->BackfaceCullingOn();
00618                 }
00619                 if(_frontfaceCulling)
00620                 {
00621                         _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->FrontfaceCullingOn();
00622                 }
00623                 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00624                 _addedClouds.insert(id, pose);
00625                 return true;
00626         }
00627 
00628         return false;
00629 }
00630 
00631 bool CloudViewer::addCloudTextureMesh(
00632         const std::string & id,
00633         const pcl::TextureMesh::Ptr & textureMesh,
00634         const Transform & pose)
00635 {
00636 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
00637         if(_addedClouds.contains(id))
00638         {
00639                 this->removeCloud(id);
00640         }
00641 
00642         UDEBUG("Adding %s", id.c_str());
00643         if(_visualizer->addTextureMesh(*textureMesh, id))
00644         {
00645                 _visualizer->updatePointCloudPose(id, pose.toEigen3f());
00646                 _addedClouds.insert(id, pose);
00647                 return true;
00648         }
00649 
00650 #endif
00651         // not implemented on lower version of PCL
00652         return false;
00653 }
00654 
00655 bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool showEdges, bool lightingOn)
00656 {
00657         UDEBUG("");
00658 #ifdef RTABMAP_OCTOMAP
00659         UASSERT(octomap!=0);
00660 
00661         pcl::IndicesPtr obstacles(new std::vector<int>);
00662 
00663         if(treeDepth > octomap->octree()->getTreeDepth())
00664         {
00665                 UWARN("Tree depth requested (%d) is deeper than the "
00666                           "actual maximum tree depth of %d. Using maximum depth.",
00667                           (int)treeDepth, (int)octomap->octree()->getTreeDepth());
00668         }
00669 
00670         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());
00671         if(obstacles->size())
00672         {
00673                 //get the renderer of the visualizer object
00674                 vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
00675 
00676                 if(_octomapActor)
00677                 {
00678                         renderer->RemoveActor(_octomapActor);
00679                         _octomapActor = 0;
00680                 }
00681 
00682                 //vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
00683                 //colors->SetName("colors");
00684                 //colors->SetNumberOfComponents(3);
00685                 vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
00686                 colors->SetName("colors");
00687                 colors->SetNumberOfValues(obstacles->size());
00688 
00689                 vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
00690                 lut->SetNumberOfTableValues(obstacles->size());
00691                 lut->Build();
00692 
00693                 // Create points
00694                 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
00695                 double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
00696                 for (unsigned int i = 0; i < obstacles->size(); i++)
00697                 {
00698                         points->InsertNextPoint(
00699                                         cloud->at(obstacles->at(i)).x,
00700                                         cloud->at(obstacles->at(i)).y,
00701                                         cloud->at(obstacles->at(i)).z);
00702                         colors->InsertValue(i,i);
00703 
00704                         lut->SetTableValue(i,
00705                                         double(cloud->at(obstacles->at(i)).r) / 255.0,
00706                                         double(cloud->at(obstacles->at(i)).g) / 255.0,
00707                                         double(cloud->at(obstacles->at(i)).b) / 255.0);
00708                 }
00709 
00710                 // Combine into a polydata
00711                 vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
00712                 polydata->SetPoints(points);
00713                 polydata->GetPointData()->SetScalars(colors);
00714 
00715                 // Create anything you want here, we will use a cube for the demo.
00716                 vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
00717                 cubeSource->SetBounds(-s, s, -s, s, -s, s);
00718 
00719                 vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
00720                 mapper->SetSourceConnection(cubeSource->GetOutputPort());
00721 #if VTK_MAJOR_VERSION <= 5
00722                 mapper->SetInputConnection(polydata->GetProducerPort());
00723 #else
00724                 mapper->SetInputData(polydata);
00725 #endif
00726                 mapper->SetScalarRange(0, obstacles->size() - 1);
00727                 mapper->SetLookupTable(lut);
00728                 mapper->ScalingOff();
00729                 mapper->Update();
00730 
00731                 vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
00732                 octomapActor->SetMapper(mapper);
00733 
00734                 octomapActor->GetProperty()->SetRepresentationToSurface();
00735                 octomapActor->GetProperty()->SetEdgeVisibility(showEdges);
00736                 octomapActor->GetProperty()->SetLighting(lightingOn);
00737 
00738                 renderer->AddActor(octomapActor);
00739                 _octomapActor = octomapActor.GetPointer();
00740 
00741                 return true;
00742         }
00743 #endif
00744         return false;
00745 }
00746 
00747 void CloudViewer::removeOctomap()
00748 {
00749         UDEBUG("");
00750 #ifdef RTABMAP_OCTOMAP
00751         if(_octomapActor)
00752         {
00753                 vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
00754                 renderer->RemoveActor(_octomapActor);
00755                 _octomapActor = 0;
00756         }
00757 #endif
00758 }
00759 
00760 bool CloudViewer::addOccupancyGridMap(
00761                 const cv::Mat & map8U,
00762                 float resolution, // cell size
00763                 float xMin,
00764                 float yMin,
00765                 float opacity)
00766 {
00767 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
00768         UASSERT(map8U.channels() == 1 && map8U.type() == CV_8U);
00769 
00770         float xSize = float(map8U.cols) * resolution;
00771         float ySize = float(map8U.rows) * resolution;
00772 
00773         UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
00774         if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
00775         {
00776                 _visualizer->removeShape("map");
00777         }
00778 
00779         if(xSize > 0.0f && ySize > 0.0f)
00780         {
00781                 pcl::TextureMeshPtr mesh(new pcl::TextureMesh());
00782                 pcl::PointCloud<pcl::PointXYZ> cloud;
00783                 cloud.push_back(pcl::PointXYZ(xMin,       yMin,       0));
00784                 cloud.push_back(pcl::PointXYZ(xSize+xMin, yMin,       0));
00785                 cloud.push_back(pcl::PointXYZ(xSize+xMin, ySize+yMin, 0));
00786                 cloud.push_back(pcl::PointXYZ(xMin,       ySize+yMin, 0));
00787                 pcl::toPCLPointCloud2(cloud, mesh->cloud);
00788 
00789                 std::vector<pcl::Vertices> polygons(1);
00790                 polygons[0].vertices.push_back(0);
00791                 polygons[0].vertices.push_back(1);
00792                 polygons[0].vertices.push_back(2);
00793                 polygons[0].vertices.push_back(3);
00794                 polygons[0].vertices.push_back(0);
00795                 mesh->tex_polygons.push_back(polygons);
00796 
00797                 // default texture materials parameters
00798                 pcl::TexMaterial material;
00799                 // hack, can we read from memory?
00800                 std::string tmpPath = (_workingDirectory+"/.tmp_map.png").toStdString();
00801                 cv::imwrite(tmpPath, map8U);
00802                 material.tex_file = tmpPath;
00803                 mesh->tex_materials.push_back(material);
00804 
00805 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
00806                 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > coordinates;
00807 #else
00808                 std::vector<Eigen::Vector2f> coordinates;
00809 #endif
00810                 coordinates.push_back(Eigen::Vector2f(0,1));
00811                 coordinates.push_back(Eigen::Vector2f(1,1));
00812                 coordinates.push_back(Eigen::Vector2f(1,0));
00813                 coordinates.push_back(Eigen::Vector2f(0,0));
00814                 mesh->tex_coordinates.push_back(coordinates);
00815 
00816                 _visualizer->addTextureMesh(*mesh, "map");
00817                 _visualizer->getCloudActorMap()->find("map")->second.actor->GetProperty()->LightingOff();
00818                 setCloudOpacity("map", 0.7);
00819 
00820                 //removed tmp texture file
00821                 QFile::remove(tmpPath.c_str());
00822         }
00823         return true;
00824 #else
00825         // not implemented on lower version of PCL
00826         return false;
00827 #endif
00828 }
00829 
00830 void CloudViewer::removeOccupancyGridMap()
00831 {
00832 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
00833         if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
00834         {
00835                 _visualizer->removeShape("map");
00836         }
00837 #endif
00838 }
00839 
00840 void CloudViewer::addOrUpdateCoordinate(
00841                         const std::string & id,
00842                         const Transform & transform,
00843                         double scale)
00844 {
00845         if(id.empty())
00846         {
00847                 UERROR("id should not be empty!");
00848                 return;
00849         }
00850 
00851         removeCoordinate(id);
00852 
00853         if(!transform.isNull())
00854         {
00855                 _coordinates.insert(id);
00856 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
00857                 _visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id);
00858 #else
00859                 // Well, on older versions, just update the main coordinate
00860                 _visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0);
00861 #endif
00862         }
00863 }
00864 
00865 bool CloudViewer::updateCoordinatePose(
00866                 const std::string & id,
00867                 const Transform & pose)
00868 {
00869 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
00870         if(_coordinates.find(id) != _coordinates.end() && !pose.isNull())
00871         {
00872                 UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
00873                 return _visualizer->updateCoordinateSystemPose(id, pose.toEigen3f());
00874         }
00875 #else
00876         UERROR("CloudViewer::updateCoordinatePose() is not available on PCL < 1.7.2");
00877 #endif
00878         return false;
00879 }
00880 
00881 void CloudViewer::removeCoordinate(const std::string & id)
00882 {
00883         if(id.empty())
00884         {
00885                 UERROR("id should not be empty!");
00886                 return;
00887         }
00888 
00889         if(_coordinates.find(id) != _coordinates.end())
00890         {
00891 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
00892                 _visualizer->removeCoordinateSystem(id);
00893 #else
00894                 // Well, on older versions, just update the main coordinate
00895                 _visualizer->removeCoordinateSystem(0);
00896 #endif
00897                 _coordinates.erase(id);
00898         }
00899 }
00900 
00901 void CloudViewer::removeAllCoordinates()
00902 {
00903         std::set<std::string> coordinates = _coordinates;
00904         for(std::set<std::string>::iterator iter = coordinates.begin(); iter!=coordinates.end(); ++iter)
00905         {
00906                 this->removeCoordinate(*iter);
00907         }
00908         UASSERT(_coordinates.empty());
00909 }
00910 
00911 void CloudViewer::addOrUpdateLine(
00912                         const std::string & id,
00913                         const Transform & from,
00914                         const Transform & to,
00915                         const QColor & color,
00916                         bool arrow)
00917 {
00918         if(id.empty())
00919         {
00920                 UERROR("id should not be empty!");
00921                 return;
00922         }
00923 
00924         removeLine(id);
00925 
00926         if(!from.isNull() && !to.isNull())
00927         {
00928                 _lines.insert(id);
00929 
00930                 QColor c = Qt::gray;
00931                 if(color.isValid())
00932                 {
00933                         c = color;
00934                 }
00935 
00936                 pcl::PointXYZ pt1(from.x(), from.y(), from.z());
00937                 pcl::PointXYZ pt2(to.x(), to.y(), to.z());
00938 
00939                 if(arrow)
00940                 {
00941                         _visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id);
00942                 }
00943                 else
00944                 {
00945                         _visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id);
00946                 }
00947         }
00948 }
00949 
00950 void CloudViewer::removeLine(const std::string & id)
00951 {
00952         if(id.empty())
00953         {
00954                 UERROR("id should not be empty!");
00955                 return;
00956         }
00957 
00958         if(_lines.find(id) != _lines.end())
00959         {
00960                 _visualizer->removeShape(id);
00961                 _lines.erase(id);
00962         }
00963 }
00964 
00965 void CloudViewer::removeAllLines()
00966 {
00967         std::set<std::string> arrows = _lines;
00968         for(std::set<std::string>::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter)
00969         {
00970                 this->removeLine(*iter);
00971         }
00972         UASSERT(_lines.empty());
00973 }
00974 
00975 static const float frustum_vertices[] = {
00976     0.0f,  0.0f, 0.0f,
00977         1.0f, 1.0f, 1.0f,
00978         1.0f, -1.0f, 1.0f,
00979         1.0f, -1.0f, -1.0f,
00980         1.0f, 1.0f, -1.0f};
00981 
00982 static const int frustum_indices[] = {
00983     1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4};
00984 
00985 void CloudViewer::addOrUpdateFrustum(
00986                         const std::string & id,
00987                         const Transform & transform,
00988                         double scale,
00989                         const QColor & color)
00990 {
00991         if(id.empty())
00992         {
00993                 UERROR("id should not be empty!");
00994                 return;
00995         }
00996 
00997         removeFrustum(id);
00998 
00999         if(!transform.isNull())
01000         {
01001                 _frustums.insert(id);
01002 
01003                 int frustumSize = sizeof(frustum_vertices)/sizeof(float);
01004                 UASSERT(frustumSize>0 && frustumSize % 3 == 0);
01005                 frustumSize/=3;
01006                 pcl::PointCloud<pcl::PointXYZ> frustumPoints;
01007                 frustumPoints.resize(frustumSize);
01008                 float scaleX = 0.5f * scale;
01009                 float scaleY = 0.4f * scale; //4x3 arbitrary ratio
01010                 float scaleZ = 0.3f * scale;
01011                 QColor c = Qt::gray;
01012                 if(color.isValid())
01013                 {
01014                         c = color;
01015                 }
01016                 Eigen::Affine3f t = transform.toEigen3f();
01017                 for(int i=0; i<frustumSize; ++i)
01018                 {
01019                         frustumPoints[i].x = frustum_vertices[i*3]*scaleX;
01020                         frustumPoints[i].y = frustum_vertices[i*3+1]*scaleY;
01021                         frustumPoints[i].z = frustum_vertices[i*3+2]*scaleZ;
01022                         frustumPoints[i] = pcl::transformPoint(frustumPoints[i], t);
01023                 }
01024 
01025                 pcl::PolygonMesh mesh;
01026                 pcl::Vertices vertices;
01027                 vertices.vertices.resize(sizeof(frustum_indices)/sizeof(int));
01028                 for(unsigned int i=0; i<vertices.vertices.size(); ++i)
01029                 {
01030                         vertices.vertices[i] = frustum_indices[i];
01031                 }
01032                 pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
01033                 mesh.polygons.push_back(vertices);
01034                 _visualizer->addPolylineFromPolygonMesh(mesh, id);
01035                 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
01036 
01037         }
01038 }
01039 
01040 bool CloudViewer::updateFrustumPose(
01041                 const std::string & id,
01042                 const Transform & pose)
01043 {
01044         if(_frustums.find(id) != _frustums.end() && !pose.isNull())
01045         {
01046                 UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
01047                 return _visualizer->updateShapePose(id, pose.toEigen3f());
01048         }
01049         return false;
01050 }
01051 
01052 void CloudViewer::removeFrustum(const std::string & id)
01053 {
01054         if(id.empty())
01055         {
01056                 UERROR("id should not be empty!");
01057                 return;
01058         }
01059 
01060         if(_frustums.find(id) != _frustums.end())
01061         {
01062                 _visualizer->removeShape(id);
01063                 _frustums.erase(id);
01064         }
01065 }
01066 
01067 void CloudViewer::removeAllFrustums()
01068 {
01069         std::set<std::string> frustums = _frustums;
01070         for(std::set<std::string>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
01071         {
01072                 this->removeFrustum(*iter);
01073         }
01074         UASSERT(_frustums.empty());
01075 }
01076 
01077 void CloudViewer::addOrUpdateGraph(
01078                 const std::string & id,
01079                 const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
01080                 const QColor & color)
01081 {
01082         if(id.empty())
01083         {
01084                 UERROR("id should not be empty!");
01085                 return;
01086         }
01087 
01088         removeGraph(id);
01089 
01090         if(graph->size())
01091         {
01092                 _graphes.insert(id);
01093 
01094                 pcl::PolygonMesh mesh;
01095                 pcl::Vertices vertices;
01096                 vertices.vertices.resize(graph->size());
01097                 for(unsigned int i=0; i<vertices.vertices.size(); ++i)
01098                 {
01099                         vertices.vertices[i] = i;
01100                 }
01101                 pcl::toPCLPointCloud2(*graph, mesh.cloud);
01102                 mesh.polygons.push_back(vertices);
01103                 _visualizer->addPolylineFromPolygonMesh(mesh, id);
01104                 _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
01105 
01106                 this->addCloud(id+"_nodes", graph, Transform::getIdentity(), color);
01107                 this->setCloudPointSize(id+"_nodes", 5);
01108         }
01109 }
01110 
01111 void CloudViewer::removeGraph(const std::string & id)
01112 {
01113         if(id.empty())
01114         {
01115                 UERROR("id should not be empty!");
01116                 return;
01117         }
01118 
01119         if(_graphes.find(id) != _graphes.end())
01120         {
01121                 _visualizer->removeShape(id);
01122                 _graphes.erase(id);
01123                 removeCloud(id+"_nodes");
01124         }
01125 }
01126 
01127 void CloudViewer::removeAllGraphs()
01128 {
01129         std::set<std::string> graphes = _graphes;
01130         for(std::set<std::string>::iterator iter = graphes.begin(); iter!=graphes.end(); ++iter)
01131         {
01132                 this->removeGraph(*iter);
01133         }
01134         UASSERT(_graphes.empty());
01135 }
01136 
01137 void CloudViewer::addOrUpdateText(
01138                         const std::string & id,
01139                         const std::string & text,
01140                         const Transform & position,
01141                         double scale,
01142                         const QColor & color)
01143 {
01144         if(id.empty())
01145         {
01146                 UERROR("id should not be empty!");
01147                 return;
01148         }
01149 
01150         removeCoordinate(id);
01151 
01152         if(!position.isNull())
01153         {
01154                 _texts.insert(id);
01155                 _visualizer->addText3D(
01156                                 text,
01157                                 pcl::PointXYZ(position.x(), position.y(), position.z()),
01158                                 scale,
01159                                 color.redF(),
01160                                 color.greenF(),
01161                                 color.blueF(),
01162                                 id);
01163         }
01164 }
01165 
01166 void CloudViewer::removeText(const std::string & id)
01167 {
01168         if(id.empty())
01169         {
01170                 UERROR("id should not be empty!");
01171                 return;
01172         }
01173 
01174         if(_texts.find(id) != _texts.end())
01175         {
01176                 _visualizer->removeText3D(id);
01177                 _texts.erase(id);
01178         }
01179 }
01180 
01181 void CloudViewer::removeAllTexts()
01182 {
01183         std::set<std::string> texts = _texts;
01184         for(std::set<std::string>::iterator iter = texts.begin(); iter!=texts.end(); ++iter)
01185         {
01186                 this->removeText(*iter);
01187         }
01188         UASSERT(_texts.empty());
01189 }
01190 
01191 bool CloudViewer::isTrajectoryShown() const
01192 {
01193         return _aShowTrajectory->isChecked();
01194 }
01195 
01196 unsigned int CloudViewer::getTrajectorySize() const
01197 {
01198         return _maxTrajectorySize;
01199 }
01200 
01201 void CloudViewer::setTrajectoryShown(bool shown)
01202 {
01203         _aShowTrajectory->setChecked(shown);
01204 }
01205 
01206 void CloudViewer::setTrajectorySize(unsigned int value)
01207 {
01208         _maxTrajectorySize = value;
01209 }
01210 
01211 void CloudViewer::clearTrajectory()
01212 {
01213         _trajectory->clear();
01214         _visualizer->removeShape("trajectory");
01215         this->update();
01216 }
01217 
01218 bool CloudViewer::isFrustumShown() const
01219 {
01220         return _aShowFrustum->isChecked();
01221 }
01222 
01223 float CloudViewer::getFrustumScale() const
01224 {
01225         return _frustumScale;
01226 }
01227 
01228 QColor CloudViewer::getFrustumColor() const
01229 {
01230         return _frustumColor;
01231 }
01232 
01233 void CloudViewer::setFrustumShown(bool shown)
01234 {
01235         if(!shown)
01236         {
01237                 std::set<std::string> frustumsCopy = _frustums;
01238                 for(std::set<std::string>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
01239                 {
01240                         if(uStrContains(*iter, "reference_frustum"))
01241                         {
01242                                 this->removeFrustum(*iter);
01243                         }
01244                 }
01245                 std::set<std::string> linesCopy = _lines;
01246                 for(std::set<std::string>::iterator iter=linesCopy.begin(); iter!=linesCopy.end(); ++iter)
01247                 {
01248                         if(uStrContains(*iter, "reference_frustum_line"))
01249                         {
01250                                 this->removeLine(*iter);
01251                         }
01252                 }
01253                 this->update();
01254         }
01255         _aShowFrustum->setChecked(shown);
01256 }
01257 
01258 void CloudViewer::setFrustumScale(float value)
01259 {
01260         _frustumScale = value;
01261 }
01262 
01263 void CloudViewer::setFrustumColor(QColor value)
01264 {
01265         if(!value.isValid())
01266         {
01267                 value = Qt::gray;
01268         }
01269         for(std::set<std::string>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
01270         {
01271                 if(uStrContains(*iter, "reference_frustum"))
01272                 {
01273                         _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), *iter);
01274                 }
01275         }
01276         this->update();
01277         _frustumColor = value;
01278 }
01279 
01280 void CloudViewer::resetCamera()
01281 {
01282         _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
01283         if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull())
01284         {
01285                 // reset relative to last current pose
01286                 if(_aLockViewZ->isChecked())
01287                 {
01288                         _visualizer->setCameraPosition(
01289                                         _lastPose.x()-1, _lastPose.y(), _lastPose.z(),
01290                                         _lastPose.x(), _lastPose.y(), _lastPose.z(),
01291                                         0, 0, 1);
01292                 }
01293                 else
01294                 {
01295                         _visualizer->setCameraPosition(
01296                                         _lastPose.x()-1, _lastPose.y(), _lastPose.z(),
01297                                         _lastPose.x(), _lastPose.y(), _lastPose.z(),
01298                                         _lastPose.r31(), _lastPose.r32(), _lastPose.r33());
01299                 }
01300         }
01301         else
01302         {
01303                 _visualizer->setCameraPosition(
01304                                 -1, 0, 0,
01305                                 0, 0, 0,
01306                                 0, 0, 1);
01307         }
01308         this->update();
01309 }
01310 
01311 void CloudViewer::removeAllClouds()
01312 {
01313         _addedClouds.clear();
01314         _visualizer->removeAllPointClouds();
01315 }
01316 
01317 
01318 bool CloudViewer::removeCloud(const std::string & id)
01319 {
01320         bool success = _visualizer->removePointCloud(id);
01321         _addedClouds.remove(id); // remove after visualizer
01322         return success;
01323 }
01324 
01325 bool CloudViewer::getPose(const std::string & id, Transform & pose)
01326 {
01327         if(_addedClouds.contains(id))
01328         {
01329                 pose = _addedClouds.value(id);
01330                 return true;
01331         }
01332         return false;
01333 }
01334 
01335 Transform CloudViewer::getTargetPose() const
01336 {
01337         if(_lastPose.isNull())
01338         {
01339                 return Transform::getIdentity();
01340         }
01341         return _lastPose;
01342 }
01343 
01344 void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
01345 {
01346         _backfaceCulling = enabled;
01347         _frontfaceCulling = frontfaceCulling;
01348 }
01349 
01350 void CloudViewer::setRenderingRate(double rate)
01351 {
01352         _renderingRate = rate;
01353         _visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(_renderingRate);
01354 }
01355 
01356 void CloudViewer::getCameraPosition(
01357                 float & x, float & y, float & z,
01358                 float & focalX, float & focalY, float & focalZ,
01359                 float & upX, float & upY, float & upZ) const
01360 {
01361         std::vector<pcl::visualization::Camera> cameras;
01362         _visualizer->getCameras(cameras);
01363         if(cameras.size())
01364         {
01365                 x = cameras.begin()->pos[0];
01366                 y = cameras.begin()->pos[1];
01367                 z = cameras.begin()->pos[2];
01368                 focalX = cameras.begin()->focal[0];
01369                 focalY = cameras.begin()->focal[1];
01370                 focalZ = cameras.begin()->focal[2];
01371                 upX = cameras.begin()->view[0];
01372                 upY = cameras.begin()->view[1];
01373                 upZ = cameras.begin()->view[2];
01374         }
01375         else
01376         {
01377                 UERROR("No camera set!?");
01378         }
01379 }
01380 
01381 void CloudViewer::setCameraPosition(
01382                 float x, float y, float z,
01383                 float focalX, float focalY, float focalZ,
01384                 float upX, float upY, float upZ)
01385 {
01386         _lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
01387         _visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
01388 }
01389 
01390 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
01391 {
01392         if(!pose.isNull())
01393         {
01394                 Eigen::Affine3f m = pose.toEigen3f();
01395                 Eigen::Vector3f pos = m.translation();
01396 
01397                 Eigen::Vector3f lastPos(0,0,0);
01398                 if(_trajectory->size())
01399                 {
01400                         lastPos[0]=_trajectory->back().x;
01401                         lastPos[1]=_trajectory->back().y;
01402                         lastPos[2]=_trajectory->back().z;
01403                 }
01404 
01405                 _trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2]));
01406                 if(_maxTrajectorySize>0)
01407                 {
01408                         while(_trajectory->size() > _maxTrajectorySize)
01409                         {
01410                                 _trajectory->erase(_trajectory->begin());
01411                         }
01412                 }
01413                 if(_aShowTrajectory->isChecked())
01414                 {
01415                         _visualizer->removeShape("trajectory");
01416                         pcl::PolygonMesh mesh;
01417                         pcl::Vertices vertices;
01418                         vertices.vertices.resize(_trajectory->size());
01419                         for(unsigned int i=0; i<vertices.vertices.size(); ++i)
01420                         {
01421                                 vertices.vertices[i] = i;
01422                         }
01423                         pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
01424                         mesh.polygons.push_back(vertices);
01425                         _visualizer->addPolylineFromPolygonMesh(mesh, "trajectory");
01426                 }
01427 
01428                 if(pose != _lastPose || _lastPose.isNull())
01429                 {
01430                         if(_lastPose.isNull())
01431                         {
01432                                 _lastPose.setIdentity();
01433                         }
01434 
01435                         std::vector<pcl::visualization::Camera> cameras;
01436                         _visualizer->getCameras(cameras);
01437 
01438                         if(_aLockCamera->isChecked())
01439                         {
01440                                 //update camera position
01441                                 Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
01442                                 cameras.front().pos[0] += diff[0];
01443                                 cameras.front().pos[1] += diff[1];
01444                                 cameras.front().pos[2] += diff[2];
01445                                 cameras.front().focal[0] += diff[0];
01446                                 cameras.front().focal[1] += diff[1];
01447                                 cameras.front().focal[2] += diff[2];
01448                         }
01449                         else if(_aFollowCamera->isChecked())
01450                         {
01451                                 Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
01452                                                                                                                           cameras.front().focal[1] - cameras.front().pos[1],
01453                                                                                                                           cameras.front().focal[2] - cameras.front().pos[2]).normalized();
01454                                 Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
01455                                 Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
01456                                 Eigen::Vector3f xAxis = yAxis.cross(zAxis);
01457                                 Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
01458                                                         yAxis[0], yAxis[1], yAxis[2],0,
01459                                                         zAxis[0], zAxis[1], zAxis[2],0);
01460 
01461                                 Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
01462                                                         PR[4], PR[5], PR[6], cameras.front().pos[1],
01463                                                         PR[8], PR[9], PR[10], cameras.front().pos[2]);
01464                                 Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
01465                                                         PR[4], PR[5], PR[6], cameras.front().focal[1],
01466                                                         PR[8], PR[9], PR[10], cameras.front().focal[2]);
01467                                 Transform N = pose;
01468                                 Transform O = _lastPose;
01469                                 Transform O2N = O.inverse()*N;
01470                                 Transform F2O = F.inverse()*O;
01471                                 Transform T = F2O * O2N * F2O.inverse();
01472                                 Transform Fp = F * T;
01473                                 Transform P2F = P.inverse()*F;
01474                                 Transform Pp = P * P2F * T * P2F.inverse();
01475 
01476                                 cameras.front().pos[0] = Pp.x();
01477                                 cameras.front().pos[1] = Pp.y();
01478                                 cameras.front().pos[2] = Pp.z();
01479                                 cameras.front().focal[0] = Fp.x();
01480                                 cameras.front().focal[1] = Fp.y();
01481                                 cameras.front().focal[2] = Fp.z();
01482                                 //FIXME: the view up is not set properly...
01483                                 cameras.front().view[0] = _aLockViewZ->isChecked()?0:Fp[8];
01484                                 cameras.front().view[1] = _aLockViewZ->isChecked()?0:Fp[9];
01485                                 cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10];
01486                         }
01487 
01488 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
01489                         if(_coordinates.find("reference") != _coordinates.end())
01490                         {
01491                                 this->updateCoordinatePose("reference", pose);
01492                         }
01493                         else
01494 #endif
01495                         {
01496                                 this->addOrUpdateCoordinate("reference", pose, 0.2);
01497                         }
01498 
01499                         vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer();
01500                         vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
01501                         cam->SetPosition (cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2]);
01502                         cam->SetFocalPoint (cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2]);
01503                         cam->SetViewUp (cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
01504                         renderer->ResetCameraClippingRange();
01505                 }
01506         }
01507 
01508         _lastPose = pose;
01509 }
01510 
01511 void CloudViewer::updateCameraFrustum(const Transform & pose, const StereoCameraModel & model)
01512 {
01513         std::vector<CameraModel> models;
01514         models.push_back(model.left());
01515         updateCameraFrustums(pose, models);
01516 }
01517 
01518 void CloudViewer::updateCameraFrustum(const Transform & pose, const CameraModel & model)
01519 {
01520         std::vector<CameraModel> models;
01521         models.push_back(model);
01522         updateCameraFrustums(pose, models);
01523 }
01524 
01525 void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector<CameraModel> & models)
01526 {
01527         if(!pose.isNull())
01528         {
01529                 // commented: update pose is crashing...
01530                 /*if(_frustums.find("reference_frustum") != _frustums.end())
01531                 {
01532                         this->updateFrustumPose("reference_frustum", pose);
01533                 }
01534                 else */ if(_aShowFrustum->isChecked())
01535                 {
01536                         Transform baseToCamera;
01537                         Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
01538 
01539                         for(unsigned int i=0; i<models.size(); ++i)
01540                         {
01541                                 baseToCamera = Transform::getIdentity();
01542                                 if(!models[i].localTransform().isNull() && !models[i].localTransform().isIdentity())
01543                                 {
01544                                         baseToCamera = models[i].localTransform()*opticalRot.inverse();
01545                                 }
01546                                 this->addOrUpdateFrustum(uFormat("reference_frustum_%d", i), pose * baseToCamera, _frustumScale, _frustumColor);
01547                                 if(!baseToCamera.isIdentity())
01548                                 {
01549                                         this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);
01550                                 }
01551                         }
01552                 }
01553         }
01554 }
01555 
01556 const QColor & CloudViewer::getDefaultBackgroundColor() const
01557 {
01558         return _defaultBgColor;
01559 }
01560 
01561 void CloudViewer::setDefaultBackgroundColor(const QColor & color)
01562 {
01563         if(_currentBgColor == _defaultBgColor)
01564         {
01565                 setBackgroundColor(color);
01566         }
01567         _defaultBgColor = color;
01568 }
01569 
01570 const QColor & CloudViewer::getBackgroundColor() const
01571 {
01572         return _currentBgColor;
01573 }
01574 
01575 void CloudViewer::setBackgroundColor(const QColor & color)
01576 {
01577         _currentBgColor = color;
01578         _visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
01579 }
01580 
01581 void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
01582 {
01583         pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
01584         pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
01585         if(iter != cloudActorMap->end())
01586         {
01587                 iter->second.actor->SetVisibility(isVisible?1:0);
01588         }
01589         else
01590         {
01591                 UERROR("Cannot find actor named \"%s\".", id.c_str());
01592         }
01593 }
01594 
01595 bool CloudViewer::getCloudVisibility(const std::string & id)
01596 {
01597         pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
01598         pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
01599         if(iter != cloudActorMap->end())
01600         {
01601                 return iter->second.actor->GetVisibility() != 0;
01602         }
01603         else
01604         {
01605                 UERROR("Cannot find actor named \"%s\".", id.c_str());
01606         }
01607         return false;
01608 }
01609 
01610 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
01611 {
01612         double lastOpacity;
01613         _visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id);
01614         if(lastOpacity != opacity)
01615         {
01616                 _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
01617         }
01618 }
01619 
01620 void CloudViewer::setCloudPointSize(const std::string & id, int size)
01621 {
01622         double lastSize;
01623         _visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id);
01624         if((int)lastSize != size)
01625         {
01626                 _visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
01627         }
01628 }
01629 
01630 void CloudViewer::setCameraTargetLocked(bool enabled)
01631 {
01632         _aLockCamera->setChecked(enabled);
01633 }
01634 
01635 void CloudViewer::setCameraTargetFollow(bool enabled)
01636 {
01637         _aFollowCamera->setChecked(enabled);
01638 }
01639 
01640 void CloudViewer::setCameraFree()
01641 {
01642         _aLockCamera->setChecked(false);
01643         _aFollowCamera->setChecked(false);
01644 }
01645 
01646 void CloudViewer::setCameraLockZ(bool enabled)
01647 {
01648         _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
01649         _aLockViewZ->setChecked(enabled);
01650 }
01651 
01652 void CloudViewer::setGridShown(bool shown)
01653 {
01654         _aShowGrid->setChecked(shown);
01655         if(shown)
01656         {
01657                 this->addGrid();
01658         }
01659         else
01660         {
01661                 this->removeGrid();
01662         }
01663 }
01664 
01665 bool CloudViewer::isCameraTargetLocked() const
01666 {
01667         return _aLockCamera->isChecked();
01668 }
01669 bool CloudViewer::isCameraTargetFollow() const
01670 {
01671         return _aFollowCamera->isChecked();
01672 }
01673 bool CloudViewer::isCameraFree() const
01674 {
01675         return !_aFollowCamera->isChecked() && !_aLockCamera->isChecked();
01676 }
01677 bool CloudViewer::isCameraLockZ() const
01678 {
01679         return _aLockViewZ->isChecked();
01680 }
01681 bool CloudViewer::isGridShown() const
01682 {
01683         return _aShowGrid->isChecked();
01684 }
01685 unsigned int CloudViewer::getGridCellCount() const
01686 {
01687         return _gridCellCount;
01688 }
01689 float CloudViewer::getGridCellSize() const
01690 {
01691         return _gridCellSize;
01692 }
01693 double CloudViewer::getRenderingRate() const
01694 {
01695         return _renderingRate;
01696 }
01697 
01698 void CloudViewer::setGridCellCount(unsigned int count)
01699 {
01700         if(count > 0)
01701         {
01702                 _gridCellCount = count;
01703                 if(_aShowGrid->isChecked())
01704                 {
01705                         this->removeGrid();
01706                         this->addGrid();
01707                 }
01708         }
01709         else
01710         {
01711                 UERROR("Cannot set grid cell count < 1, count=%d", count);
01712         }
01713 }
01714 void CloudViewer::setGridCellSize(float size)
01715 {
01716         if(size > 0)
01717         {
01718                 _gridCellSize = size;
01719                 if(_aShowGrid->isChecked())
01720                 {
01721                         this->removeGrid();
01722                         this->addGrid();
01723                 }
01724         }
01725         else
01726         {
01727                 UERROR("Cannot set grid cell size <= 0, value=%f", size);
01728         }
01729 }
01730 void CloudViewer::addGrid()
01731 {
01732         if(_gridLines.empty())
01733         {
01734                 float cellSize = _gridCellSize;
01735                 int cellCount = _gridCellCount;
01736                 double r=0.5;
01737                 double g=0.5;
01738                 double b=0.5;
01739                 int id = 0;
01740                 float min = -float(cellCount/2) * cellSize;
01741                 float max = float(cellCount/2) * cellSize;
01742                 std::string name;
01743                 for(float i=min; i<=max; i += cellSize)
01744                 {
01745                         //over x
01746                         name = uFormat("line%d", ++id);
01747                         _visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name);
01748                         _gridLines.push_back(name);
01749                         //over y or z
01750                         name = uFormat("line%d", ++id);
01751                         _visualizer->addLine(
01752                                         pcl::PointXYZ(min, i, 0),
01753                                         pcl::PointXYZ(max, i, 0),
01754                                         r, g, b, name);
01755                         _gridLines.push_back(name);
01756                 }
01757         }
01758 }
01759 
01760 void CloudViewer::removeGrid()
01761 {
01762         for(std::list<std::string>::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter)
01763         {
01764                 _visualizer->removeShape(*iter);
01765         }
01766         _gridLines.clear();
01767 }
01768 
01769 Eigen::Vector3f rotatePointAroundAxe(
01770                 const Eigen::Vector3f & point,
01771                 const Eigen::Vector3f & axis,
01772                 float angle)
01773 {
01774         Eigen::Vector3f direction = point;
01775         Eigen::Vector3f zAxis = axis;
01776         float dotProdZ = zAxis.dot(direction);
01777         Eigen::Vector3f ptOnZaxis = zAxis * dotProdZ;
01778         direction -= ptOnZaxis;
01779         Eigen::Vector3f xAxis = direction.normalized();
01780         Eigen::Vector3f yAxis = zAxis.cross(xAxis);
01781 
01782         Eigen::Matrix3f newFrame;
01783         newFrame << xAxis[0], yAxis[0], zAxis[0],
01784                                   xAxis[1], yAxis[1], zAxis[1],
01785                                   xAxis[2], yAxis[2], zAxis[2];
01786 
01787         // transform to axe frame
01788         // transpose=inverse for orthogonal matrices
01789         Eigen::Vector3f newDirection = newFrame.transpose() * direction;
01790 
01791         // rotate about z
01792         float cosTheta = cos(angle);
01793         float sinTheta = sin(angle);
01794         float magnitude = newDirection.norm();
01795         newDirection[0] = ( magnitude * cosTheta );
01796         newDirection[1] = ( magnitude * sinTheta );
01797 
01798         // transform back to global frame
01799         direction = newFrame * newDirection;
01800 
01801         return direction + ptOnZaxis;
01802 }
01803 
01804 void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
01805         if(event->key() == Qt::Key_Up ||
01806                 event->key() == Qt::Key_Down ||
01807                 event->key() == Qt::Key_Left ||
01808                 event->key() == Qt::Key_Right)
01809         {
01810                 _keysPressed -= (Qt::Key)event->key();
01811         }
01812         else
01813         {
01814                 QVTKWidget::keyPressEvent(event);
01815         }
01816 }
01817 
01818 void CloudViewer::keyPressEvent(QKeyEvent * event)
01819 {
01820         if(event->key() == Qt::Key_Up ||
01821                 event->key() == Qt::Key_Down ||
01822                 event->key() == Qt::Key_Left ||
01823                 event->key() == Qt::Key_Right)
01824         {
01825                 _keysPressed += (Qt::Key)event->key();
01826 
01827                 std::vector<pcl::visualization::Camera> cameras;
01828                 _visualizer->getCameras(cameras);
01829 
01830                 //update camera position
01831                 Eigen::Vector3f pos(cameras.front().pos[0], cameras.front().pos[1], _aLockViewZ->isChecked()?0:cameras.front().pos[2]);
01832                 Eigen::Vector3f focal(cameras.front().focal[0], cameras.front().focal[1], _aLockViewZ->isChecked()?0:cameras.front().focal[2]);
01833                 Eigen::Vector3f viewUp(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
01834                 Eigen::Vector3f cummulatedDir(0,0,0);
01835                 Eigen::Vector3f cummulatedFocalDir(0,0,0);
01836                 float step = 0.2f;
01837                 float stepRot = 0.02f; // radian
01838                 if(_keysPressed.contains(Qt::Key_Up))
01839                 {
01840                         Eigen::Vector3f dir;
01841                         if(event->modifiers() & Qt::ShiftModifier)
01842                         {
01843                                 dir = viewUp * step;// up
01844                         }
01845                         else
01846                         {
01847                                 dir = (focal-pos).normalized() * step; // forward
01848                         }
01849                         cummulatedDir += dir;
01850                 }
01851                 if(_keysPressed.contains(Qt::Key_Down))
01852                 {
01853                         Eigen::Vector3f dir;
01854                         if(event->modifiers() & Qt::ShiftModifier)
01855                         {
01856                                 dir = viewUp * -step;// down
01857                         }
01858                         else
01859                         {
01860                                 dir = (focal-pos).normalized() * -step; // backward
01861                         }
01862                         cummulatedDir += dir;
01863                 }
01864                 if(_keysPressed.contains(Qt::Key_Right))
01865                 {
01866                         if(event->modifiers() & Qt::ShiftModifier)
01867                         {
01868                                 // rotate right
01869                                 Eigen::Vector3f point = (focal-pos);
01870                                 Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, -stepRot);
01871                                 Eigen::Vector3f diff = newPoint - point;
01872                                 cummulatedFocalDir += diff;
01873                         }
01874                         else
01875                         {
01876                                 Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * step; // strafing right
01877                                 cummulatedDir += dir;
01878                         }
01879                 }
01880                 if(_keysPressed.contains(Qt::Key_Left))
01881                 {
01882                         if(event->modifiers() & Qt::ShiftModifier)
01883                         {
01884                                 // rotate left
01885                                 Eigen::Vector3f point = (focal-pos);
01886                                 Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, stepRot);
01887                                 Eigen::Vector3f diff = newPoint - point;
01888                                 cummulatedFocalDir += diff;
01889                         }
01890                         else
01891                         {
01892                                 Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * -step; // strafing left
01893                                 cummulatedDir += dir;
01894                         }
01895                 }
01896 
01897                 cameras.front().pos[0] += cummulatedDir[0];
01898                 cameras.front().pos[1] += cummulatedDir[1];
01899                 cameras.front().pos[2] += cummulatedDir[2];
01900                 cameras.front().focal[0] += cummulatedDir[0] + cummulatedFocalDir[0];
01901                 cameras.front().focal[1] += cummulatedDir[1] + cummulatedFocalDir[1];
01902                 cameras.front().focal[2] += cummulatedDir[2] + cummulatedFocalDir[2];
01903                 _visualizer->setCameraPosition(
01904                         cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
01905                         cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
01906                         cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
01907 
01908                 update();
01909 
01910                 emit configChanged();
01911         }
01912         else
01913         {
01914                 QVTKWidget::keyPressEvent(event);
01915         }
01916 }
01917 
01918 void CloudViewer::mousePressEvent(QMouseEvent * event)
01919 {
01920         if(event->button() == Qt::RightButton)
01921         {
01922                 event->accept();
01923         }
01924         else
01925         {
01926                 QVTKWidget::mousePressEvent(event);
01927         }
01928 }
01929 
01930 void CloudViewer::mouseMoveEvent(QMouseEvent * event)
01931 {
01932         QVTKWidget::mouseMoveEvent(event);
01933 
01934         // camera view up z locked?
01935         if(_aLockViewZ->isChecked())
01936         {
01937                 std::vector<pcl::visualization::Camera> cameras;
01938                 _visualizer->getCameras(cameras);
01939 
01940                 cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
01941 
01942                 if(     _lastCameraOrientation!=cv::Vec3d(0,0,0) &&
01943                         _lastCameraPose!=cv::Vec3d(0,0,0) &&
01944                         (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) &&
01945                          uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1])))
01946                 {
01947                         cameras.front().pos[0] = _lastCameraPose[0];
01948                         cameras.front().pos[1] = _lastCameraPose[1];
01949                         cameras.front().pos[2] = _lastCameraPose[2];
01950                 }
01951                 else if(newCameraOrientation != cv::Vec3d(0,0,0))
01952                 {
01953                         _lastCameraOrientation = newCameraOrientation;
01954                         _lastCameraPose = cv::Vec3d(cameras.front().pos);
01955                 }
01956                 cameras.front().view[0] = 0;
01957                 cameras.front().view[1] = 0;
01958                 cameras.front().view[2] = 1;
01959 
01960                 _visualizer->setCameraPosition(
01961                         cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
01962                         cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
01963                         cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
01964 
01965         }
01966         this->update();
01967 
01968         emit configChanged();
01969 }
01970 
01971 void CloudViewer::wheelEvent(QWheelEvent * event)
01972 {
01973         QVTKWidget::wheelEvent(event);
01974         if(_aLockViewZ->isChecked())
01975         {
01976                 std::vector<pcl::visualization::Camera> cameras;
01977                 _visualizer->getCameras(cameras);
01978                 _lastCameraPose = cv::Vec3d(cameras.front().pos);
01979         }
01980         emit configChanged();
01981 }
01982 
01983 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
01984 {
01985         QAction * a = _menu->exec(event->globalPos());
01986         if(a)
01987         {
01988                 handleAction(a);
01989                 emit configChanged();
01990         }
01991 }
01992 
01993 void CloudViewer::handleAction(QAction * a)
01994 {
01995         if(a == _aSetTrajectorySize)
01996         {
01997                 bool ok;
01998                 int value = QInputDialog::getInt(this, tr("Set trajectory size"), tr("Size (0=infinite)"), _maxTrajectorySize, 0, 10000, 10, &ok);
01999                 if(ok)
02000                 {
02001                         _maxTrajectorySize = value;
02002                 }
02003         }
02004         else if(a == _aClearTrajectory)
02005         {
02006                 this->clearTrajectory();
02007         }
02008         else if(a == _aShowFrustum)
02009         {
02010                 this->setFrustumShown(a->isChecked());
02011         }
02012         else if(a == _aSetFrustumScale)
02013         {
02014                 bool ok;
02015                 double value = QInputDialog::getDouble(this, tr("Set frustum scale"), tr("Scale"), _frustumScale, 0.0, 999.0, 1, &ok);
02016                 if(ok)
02017                 {
02018                         this->setFrustumScale(value);
02019                 }
02020         }
02021         else if(a == _aSetFrustumColor)
02022         {
02023                 QColor value = QColorDialog::getColor(_frustumColor, this);
02024                 if(value.isValid())
02025                 {
02026                         this->setFrustumColor(value);
02027                 }
02028         }
02029         else if(a == _aResetCamera)
02030         {
02031                 this->resetCamera();
02032         }
02033         else if(a == _aShowGrid)
02034         {
02035                 if(_aShowGrid->isChecked())
02036                 {
02037                         this->addGrid();
02038                 }
02039                 else
02040                 {
02041                         this->removeGrid();
02042                 }
02043 
02044                 this->update();
02045         }
02046         else if(a == _aSetGridCellCount)
02047         {
02048                 bool ok;
02049                 int value = QInputDialog::getInt(this, tr("Set grid cell count"), tr("Count"), _gridCellCount, 1, 10000, 10, &ok);
02050                 if(ok)
02051                 {
02052                         this->setGridCellCount(value);
02053                 }
02054         }
02055         else if(a == _aSetGridCellSize)
02056         {
02057                 bool ok;
02058                 double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
02059                 if(ok)
02060                 {
02061                         this->setGridCellSize(value);
02062                 }
02063         }
02064         else if(a == _aSetBackgroundColor)
02065         {
02066                 QColor color = this->getDefaultBackgroundColor();
02067                 color = QColorDialog::getColor(color, this);
02068                 if(color.isValid())
02069                 {
02070                         this->setDefaultBackgroundColor(color);
02071                         this->update();
02072                 }
02073         }
02074         else if(a == _aSetRenderingRate)
02075         {
02076                 bool ok;
02077                 double value = QInputDialog::getDouble(this, tr("Rendering rate"), tr("Rate (hz)"), _renderingRate, 0, 60, 0, &ok);
02078                 if(ok)
02079                 {
02080                         this->setRenderingRate(value);
02081                 }
02082         }
02083         else if(a == _aLockViewZ)
02084         {
02085                 if(_aLockViewZ->isChecked())
02086                 {
02087                         this->update();
02088                 }
02089         }
02090 }
02091 
02092 } /* namespace rtabmap */

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:15