ViewerGui.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License (octovis): GNU GPL v2
00008  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
00009  *
00010  *
00011  * This program is free software; you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation; either version 2 of the License, or
00014  * (at your option) any later version.
00015  *
00016  * This program is distributed in the hope that it will be useful, but
00017  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
00018  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
00019  * for more details.
00020  *
00021  * You should have received a copy of the GNU General Public License along
00022  * with this program; if not, write to the Free Software Foundation, Inc.,
00023  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00024  */
00025 
00026 #include <iostream>
00027 #include <fstream>
00028 //#include <octomap/octomap_timing.h>
00029 
00030 #include <octovis/ViewerGui.h>
00031 #include <octovis/ColorOcTreeDrawer.h>
00032 #include <octomap/MapCollection.h>
00033 
00034 
00035 #define _MAXRANGE_URG 5.1
00036 #define _MAXRANGE_SICK 50.0
00037 
00038 namespace octomap{
00039 
00040 ViewerGui::ViewerGui(const std::string& filename, QWidget *parent)
00041 : QMainWindow(parent), m_scanGraph(NULL),
00042   m_trajectoryDrawer(NULL), m_pointcloudDrawer(NULL),
00043   m_cameraFollowMode(NULL),
00044   m_octreeResolution(0.1), m_laserMaxRange(-1.), m_occupancyThresh(0.5),
00045   m_max_tree_depth(16), m_laserType(LASERTYPE_SICK),
00046   m_cameraStored(false), m_filename("") {
00047 
00048   ui.setupUi(this);
00049   m_glwidget = new ViewerWidget(this);
00050   this->setCentralWidget(m_glwidget);
00051 
00052   // Settings panel at the right side
00053   ViewerSettingsPanel* settingsPanel = new ViewerSettingsPanel(this);
00054   QDockWidget* settingsDock = new QDockWidget("Octree / Scan graph settings", this);
00055   settingsDock->setWidget(settingsPanel);
00056   this->addDockWidget(Qt::RightDockWidgetArea, settingsDock);
00057   ui.menuShow->addAction(settingsDock->toggleViewAction());
00058 
00059   // Camera settings panel at the right side
00060   ViewerSettingsPanelCamera* settingsCameraPanel = new ViewerSettingsPanelCamera(this);
00061   QDockWidget *settingsCameraDock = new QDockWidget("Camera settings", this);
00062   settingsCameraDock->setWidget(settingsCameraPanel);
00063   this->addDockWidget(Qt::RightDockWidgetArea, settingsCameraDock);
00064   this->tabifyDockWidget(settingsDock, settingsCameraDock);
00065   settingsDock->raise();
00066   ui.menuShow->addAction(settingsCameraDock->toggleViewAction());
00067 
00068   // status bar
00069   m_mapSizeStatus = new QLabel("Map size", this);
00070   m_mapMemoryStatus = new QLabel("Memory consumption", this);
00071   m_mapSizeStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken);
00072   m_mapMemoryStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken);
00073   statusBar()->addPermanentWidget(m_mapSizeStatus);
00074   statusBar()->addPermanentWidget(m_mapMemoryStatus);
00075 
00076   m_cameraFollowMode = new CameraFollowMode();
00077 
00078   connect(this, SIGNAL(updateStatusBar(QString, int)), statusBar(), SLOT(showMessage(QString, int)));
00079 
00080   connect(settingsPanel, SIGNAL(treeDepthChanged(int)), this, SLOT(changeTreeDepth(int)));
00081   connect(settingsPanel, SIGNAL(addNextScans(unsigned)), this, SLOT(addNextScans(unsigned)));
00082   connect(settingsPanel, SIGNAL(gotoFirstScan()), this, SLOT(gotoFirstScan()));
00083 
00084   connect(settingsCameraPanel, SIGNAL(jumpToFrame(unsigned)), m_cameraFollowMode, SLOT(jumpToFrame(unsigned)));
00085   connect(settingsCameraPanel, SIGNAL(play()), m_cameraFollowMode, SLOT(play()));
00086   connect(settingsCameraPanel, SIGNAL(pause()), m_cameraFollowMode, SLOT(pause()));
00087   connect(settingsCameraPanel, SIGNAL(clearCameraPath()), m_cameraFollowMode, SLOT(clearCameraPath()));
00088   connect(settingsCameraPanel, SIGNAL(saveToCameraPath()), m_cameraFollowMode, SLOT(saveToCameraPath()));
00089   connect(settingsCameraPanel, SIGNAL(removeFromCameraPath()), m_cameraFollowMode, SLOT(removeFromCameraPath()));
00090   connect(settingsCameraPanel, SIGNAL(addToCameraPath()), m_cameraFollowMode, SLOT(addToCameraPath()));
00091   connect(settingsCameraPanel, SIGNAL(followCameraPath()), m_cameraFollowMode, SLOT(followCameraPath()));
00092   connect(settingsCameraPanel, SIGNAL(followRobotPath()), m_cameraFollowMode, SLOT(followRobotPath()));
00093 
00094   connect(m_cameraFollowMode, SIGNAL(changeNumberOfFrames(unsigned)), settingsCameraPanel, SLOT(setNumberOfFrames(unsigned)));
00095   connect(m_cameraFollowMode, SIGNAL(frameChanged(unsigned)), settingsCameraPanel, SLOT(setCurrentFrame(unsigned)));
00096   connect(m_cameraFollowMode, SIGNAL(stopped()), settingsCameraPanel, SLOT(setStopped()));
00097   connect(m_cameraFollowMode, SIGNAL(scanGraphAvailable(bool)), settingsCameraPanel, SLOT(setRobotTrajectoryAvailable(bool)));
00098 
00099   connect(m_cameraFollowMode, SIGNAL(deleteCameraPath(int)), m_glwidget, SLOT(deleteCameraPath(int)));
00100   connect(m_cameraFollowMode, SIGNAL(removeFromCameraPath(int,int)), m_glwidget, SLOT(removeFromCameraPath(int,int)));
00101   connect(m_cameraFollowMode, SIGNAL(appendToCameraPath(int, const octomath::Pose6D&)), m_glwidget, SLOT(appendToCameraPath(int, const octomath::Pose6D&)));
00102   connect(m_cameraFollowMode, SIGNAL(appendCurrentToCameraPath(int)), m_glwidget, SLOT(appendCurrentToCameraPath(int)));
00103   connect(m_cameraFollowMode, SIGNAL(addCurrentToCameraPath(int,int)), m_glwidget, SLOT(addCurrentToCameraPath(int,int)));
00104   connect(m_cameraFollowMode, SIGNAL(updateCameraPath(int,int)), m_glwidget, SLOT(updateCameraPath(int,int)));
00105   connect(m_cameraFollowMode, SIGNAL(playCameraPath(int,int)), m_glwidget, SLOT(playCameraPath(int,int)));
00106   connect(m_cameraFollowMode, SIGNAL(stopCameraPath(int)), m_glwidget, SLOT(stopCameraPath(int)));
00107   connect(m_cameraFollowMode, SIGNAL(jumpToCamFrame(int, int)), m_glwidget, SLOT(jumpToCamFrame(int, int)));
00108   connect(m_glwidget, SIGNAL(cameraPathStopped(int)), m_cameraFollowMode, SLOT(cameraPathStopped(int)));
00109   connect(m_glwidget, SIGNAL(cameraPathFrameChanged(int, int)), m_cameraFollowMode, SLOT(cameraPathFrameChanged(int, int)));
00110 
00111   connect(this, SIGNAL(changeNumberOfScans(unsigned)), settingsPanel, SLOT(setNumberOfScans(unsigned)));
00112   connect(this, SIGNAL(changeCurrentScan(unsigned)), settingsPanel, SLOT(setCurrentScan(unsigned)));
00113   connect(this, SIGNAL(changeResolution(double)), settingsPanel, SLOT(setResolution(double)));
00114 
00115   connect(settingsCameraPanel, SIGNAL(changeCamPosition(double, double, double, double, double, double)),
00116           m_glwidget, SLOT(setCamPosition(double, double, double, double, double, double)));
00117   connect(m_cameraFollowMode, SIGNAL(changeCamPose(const octomath::Pose6D&)),
00118           m_glwidget, SLOT(setCamPose(const octomath::Pose6D&)));
00119 
00120   connect(ui.actionReset_view, SIGNAL(triggered()), m_glwidget, SLOT(resetView()));
00121 
00122   if (filename != ""){
00123     m_filename = filename;
00124     openFile();
00125   }
00126 }
00127 
00128 ViewerGui::~ViewerGui() {
00129   if (m_trajectoryDrawer){
00130     m_glwidget->removeSceneObject(m_trajectoryDrawer);
00131     delete m_trajectoryDrawer;
00132     m_trajectoryDrawer = NULL;
00133   }
00134 
00135   if (m_pointcloudDrawer){
00136     m_glwidget->removeSceneObject(m_pointcloudDrawer);
00137     delete m_pointcloudDrawer;
00138     m_pointcloudDrawer = NULL;
00139   }
00140 
00141   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) {
00142     m_glwidget->removeSceneObject(it->second.octree_drawer);
00143     delete (it->second.octree_drawer);
00144     delete (it->second.octree);
00145   }
00146   m_octrees.clear();
00147 
00148   if(m_cameraFollowMode) {
00149     delete m_cameraFollowMode;
00150     m_cameraFollowMode = NULL;
00151   }
00152 }
00153 
00154 bool ViewerGui::isShown() {
00155   return  m_glwidget->isVisible();
00156 }
00157 
00158 void ViewerGui::showInfo(QString string, bool newline) {
00159   std::cerr << string.toLocal8Bit().data();
00160   if (newline) std::cerr << std::endl;
00161   else std::cerr << std::flush;
00162   int duration = 0;
00163   if (newline)
00164     duration = 3000;
00165   emit updateStatusBar(string, duration);
00166 }
00167 
00168 bool ViewerGui::getOctreeRecord(int id, OcTreeRecord*& otr) {
00169   std::map<int, OcTreeRecord>::iterator it = m_octrees.find(id);
00170   if( it != m_octrees.end() ) {
00171     otr = &(it->second);
00172     return true;
00173   }
00174   else {
00175     return false;
00176   }
00177 }
00178 
00179 void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d origin) {
00180   // is id in use?
00181       OcTreeRecord* r;
00182       bool foundRecord = getOctreeRecord(id, r);
00183       if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) !=0){
00184         // delete old drawer, create new
00185         delete r->octree_drawer;
00186         if (dynamic_cast<OcTree*>(tree)) {
00187           r->octree_drawer = new OcTreeDrawer();
00188           //        fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id);
00189         }
00190         else if (dynamic_cast<ColorOcTree*>(tree)) {
00191           r->octree_drawer = new ColorOcTreeDrawer();
00192         } else{
00193           OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str());
00194         }
00195 
00196         delete r->octree;
00197         r->octree = tree;
00198         r->origin = origin;
00199 
00200       } else if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) ==0) {
00201         // only swap out tree
00202 
00203         delete r->octree;
00204         r->octree = tree;
00205         r->origin = origin;
00206       } else {
00207         // add new record
00208         OcTreeRecord otr;
00209         otr.id = id;
00210         if (dynamic_cast<OcTree*>(tree)) {
00211           otr.octree_drawer = new OcTreeDrawer();
00212           //        fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id);
00213         }
00214         else if (dynamic_cast<ColorOcTree*>(tree)) {
00215           otr.octree_drawer = new ColorOcTreeDrawer();
00216         } else{
00217           OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str());
00218         }
00219         otr.octree = tree;
00220         otr.origin = origin;
00221         m_octrees[id] = otr;
00222         m_glwidget->addSceneObject(otr.octree_drawer);
00223       }
00224 }
00225 
00226 void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id) {
00227   octomap::pose6d o; // initialized to (0,0,0) , (0,0,0,1) by default
00228   addOctree(tree, id, o);
00229 }
00230 
00231 void ViewerGui::showOcTree() {
00232 
00233   // update viewer stat
00234   double minX, minY, minZ, maxX, maxY, maxZ;
00235   minX = minY = minZ = -10; // min bbx for drawing
00236   maxX = maxY = maxZ = 10;  // max bbx for drawing
00237   double sizeX, sizeY, sizeZ;
00238   sizeX = sizeY = sizeZ = 0.;
00239   size_t memoryUsage = 0;
00240   size_t num_nodes = 0;
00241   size_t memorySingleNode = 0;
00242 
00243 
00244   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) {
00245     // get map bbx
00246     double lminX, lminY, lminZ, lmaxX, lmaxY, lmaxZ;
00247     it->second.octree->getMetricMin(lminX, lminY, lminZ);
00248     it->second.octree->getMetricMax(lmaxX, lmaxY, lmaxZ);
00249     // transform to world coords using map origin
00250     octomap::point3d pmin(lminX, lminY, lminZ);
00251     octomap::point3d pmax(lmaxX, lmaxY, lmaxZ);
00252     pmin = it->second.origin.transform(pmin);
00253     pmax = it->second.origin.transform(pmax);
00254     lminX = pmin.x(); lminY = pmin.y(); lminZ = pmin.z();
00255     lmaxX = pmax.x(); lmaxY = pmax.y(); lmaxZ = pmax.z();
00256     // update global bbx
00257     if (lminX < minX) minX = lminX;
00258     if (lminY < minY) minY = lminY;
00259     if (lminZ < minZ) minZ = lminZ;
00260     if (lmaxX > maxX) maxX = lmaxX;
00261     if (lmaxY > maxY) maxY = lmaxY;
00262     if (lmaxZ > maxZ) maxZ = lmaxZ;
00263     double lsizeX, lsizeY, lsizeZ;
00264     // update map stats
00265     it->second.octree->getMetricSize(lsizeX, lsizeY, lsizeZ);
00266     if (lsizeX > sizeX) sizeX = lsizeX;
00267     if (lsizeY > sizeY) sizeY = lsizeY;
00268     if (lsizeZ > sizeZ) sizeZ = lsizeZ;
00269     memoryUsage += it->second.octree->memoryUsage();
00270     num_nodes += it->second.octree->size();
00271     memorySingleNode = std::max(memorySingleNode, it->second.octree->memoryUsageNode());
00272   }
00273 
00274   m_glwidget->setSceneBoundingBox(qglviewer::Vec(minX, minY, minZ), qglviewer::Vec(maxX, maxY, maxZ));
00275 
00276   //if (m_octrees.size()) {
00277   QString size = QString("%L1 x %L2 x %L3 m^3; %L4 nodes").arg(sizeX).arg(sizeY).arg(sizeZ).arg(unsigned(num_nodes));
00278   QString memory = QString("Single node: %L1 B; ").arg(memorySingleNode)
00279             + QString ("Octree: %L1 B (%L2 MB)").arg(memoryUsage).arg((double) memoryUsage/(1024.*1024.), 0, 'f', 3);
00280   m_mapMemoryStatus->setText(memory);
00281   m_mapSizeStatus->setText(size);
00282   //}
00283 
00284   m_glwidget->updateGL();
00285 
00286   // generate cubes -> display
00287   // timeval start;
00288   // timeval stop;
00289   // gettimeofday(&start, NULL);  // start timer
00290   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) {
00291     it->second.octree_drawer->setMax_tree_depth(m_max_tree_depth);
00292     it->second.octree_drawer->setOcTree(*it->second.octree, it->second.origin, it->second.id);
00293   }
00294   //    gettimeofday(&stop, NULL);  // stop timer
00295   //    double time_to_generate = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
00296   //    fprintf(stderr, "setOcTree took %f sec\n", time_to_generate);
00297   m_glwidget->updateGL();
00298 }
00299 
00300 
00301 void ViewerGui::generateOctree() {
00302 
00303   if (m_scanGraph) {
00304 
00305     QApplication::setOverrideCursor(Qt::WaitCursor);
00306 
00307     showInfo("Generating OcTree... ");
00308     std::cerr << std::endl;
00309 
00310     //if (m_ocTree) delete m_ocTree;
00311     OcTree* tree = new octomap::OcTree(m_octreeResolution);
00312 
00313     octomap::ScanGraph::iterator it;
00314     unsigned numScans = m_scanGraph->size();
00315     unsigned currentScan = 1;
00316     for (it = m_scanGraph->begin(); it != m_nextScanToAdd; it++) {
00317       tree->insertPointCloud(**it, m_laserMaxRange);
00318       fprintf(stderr, "generateOctree:: inserting scan node with %d points, origin: %.2f  ,%.2f , %.2f.\n",
00319               (unsigned int) (*it)->scan->size(), (*it)->pose.x(), (*it)->pose.y(), (*it)->pose.z()  );
00320 
00321       std::cout << " S ("<<currentScan<<"/"<<numScans<<") " << std::flush;
00322       currentScan++;
00323     }
00324 
00325     this->addOctree(tree, DEFAULT_OCTREE_ID);
00326     this->showOcTree();
00327 
00328     showInfo("Done.", true);
00329     QApplication::restoreOverrideCursor();
00330   }
00331   else {
00332     std::cerr << "generateOctree called but no ScanGraph present!\n";
00333   }
00334 
00335 }
00336 
00337 // ==  incremental graph generation   =======================
00338 
00339 void ViewerGui::gotoFirstScan(){
00340   if (m_scanGraph){
00341     showInfo("Inserting first scan node into tree... ", true);
00342     QApplication::setOverrideCursor(Qt::WaitCursor);
00343 
00344     m_nextScanToAdd = m_scanGraph->begin();
00345 
00346     // if (m_ocTree) delete m_ocTree;
00347     // m_ocTree = new octomap::OcTree(m_octreeResolution);
00348     OcTree* tree = new octomap::OcTree(m_octreeResolution);
00349     this->addOctree(tree, DEFAULT_OCTREE_ID);
00350 
00351     addNextScan();
00352 
00353     QApplication::restoreOverrideCursor();
00354     showOcTree();
00355   }
00356 }
00357 
00358 void ViewerGui::addNextScan(){
00359 
00360   if (m_scanGraph){
00361     showInfo("Inserting next scan node into tree... ", true);
00362 
00363     QApplication::setOverrideCursor(Qt::WaitCursor);
00364     if (m_nextScanToAdd != m_scanGraph->end()){
00365       OcTreeRecord* r;
00366       if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) {
00367         fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID);
00368         return;
00369       }
00370       // not used with ColorOcTrees, omitting casts
00371       ((OcTree*) r->octree)->insertPointCloud(**m_nextScanToAdd, m_laserMaxRange);
00372       m_nextScanToAdd++;
00373     }
00374 
00375     QApplication::restoreOverrideCursor();
00376     showOcTree();
00377 
00378   }
00379 }
00380 
00381 
00382 void ViewerGui::addNextScans(unsigned scans){
00383   for (unsigned i = 0; i < scans; ++i){
00384     addNextScan();
00385   }
00386 }
00387 
00388 
00389 // ==  file I/O   ===========================================
00390 
00391 void ViewerGui::openFile(){
00392   if (!m_filename.empty()){
00393     m_glwidget->clearAll();
00394 
00395     QString temp = QString(m_filename.c_str());
00396     QFileInfo fileinfo(temp);
00397     if (fileinfo.suffix() == "graph"){
00398       openGraph();
00399     }else if (fileinfo.suffix() == "bt"){
00400       openTree();
00401     }
00402     else if (fileinfo.suffix() == "ot")
00403     {
00404       openOcTree();
00405     }
00406     else if (fileinfo.suffix() == "hot"){
00407       openMapCollection();
00408     }
00409     else if (fileinfo.suffix() == "dat"){
00410       openPointcloud();
00411     }
00412     else {
00413       QMessageBox::warning(this, "Unknown file", "Cannot open file, unknown extension: "+fileinfo.suffix(), QMessageBox::Ok);
00414     }
00415   }
00416 }
00417 
00418 
00419 void ViewerGui::openGraph(bool completeGraph){
00420 
00421   QApplication::setOverrideCursor(Qt::WaitCursor);
00422   showInfo("Loading scan graph from file " + QString(m_filename.c_str()) );
00423 
00424   if (m_scanGraph) delete m_scanGraph;
00425   m_scanGraph = new octomap::ScanGraph();
00426   m_scanGraph->readBinary(m_filename);
00427 
00428   loadGraph(completeGraph);
00429 }
00430 
00431 
00432 void ViewerGui::openPointcloud(){
00433 
00434   QApplication::setOverrideCursor(Qt::WaitCursor);
00435   showInfo("Loading ASCII pointcloud from file "+QString(m_filename.c_str()) + "...");
00436 
00437   if (m_scanGraph) delete m_scanGraph;
00438   m_scanGraph = new octomap::ScanGraph();
00439 
00440 
00441   // read pointcloud from file
00442   std::ifstream s(m_filename.c_str());
00443   Pointcloud* pc = new Pointcloud();
00444 
00445   if (!s) {
00446     std::cout <<"ERROR: could not read " << m_filename << std::endl;
00447     return;
00448   }
00449 
00450   pc->read(s);
00451 
00452   pose6d laser_pose(0,0,0,0,0,0);
00453   m_scanGraph->addNode(pc, laser_pose);
00454 
00455   loadGraph(true);
00456   showInfo("Done.", true);
00457 }
00458 
00459 
00460 void ViewerGui::setOcTreeUISwitches() {
00461   ui.actionPointcloud->setChecked(false);
00462   ui.actionPointcloud->setEnabled(false);
00463   ui.actionOctree_cells->setChecked(true);
00464   ui.actionOctree_cells->setEnabled(true);
00465   ui.actionFree->setChecked(false);
00466   ui.actionFree->setEnabled(true);
00467   ui.actionOctree_structure->setEnabled(true);
00468   ui.actionOctree_structure->setChecked(false);
00469   ui.actionTrajectory->setEnabled(false);
00470   ui.actionConvert_ml_tree->setEnabled(true);
00471   ui.actionReload_Octree->setEnabled(true);
00472   ui.actionSettings->setEnabled(false);
00473 }
00474 
00475 void ViewerGui::openTree(){
00476   OcTree* tree = new octomap::OcTree(m_filename);
00477   this->addOctree(tree, DEFAULT_OCTREE_ID);
00478 
00479   m_octreeResolution = tree->getResolution();
00480   emit changeResolution(m_octreeResolution);
00481 
00482   setOcTreeUISwitches();
00483   showOcTree();
00484   m_glwidget->resetView();
00485 }
00486 
00487 void ViewerGui::openOcTree(){
00488   AbstractOcTree* tree = AbstractOcTree::read(m_filename);
00489 
00490   if (tree){
00491     this->addOctree(tree, DEFAULT_OCTREE_ID);
00492 
00493     m_octreeResolution = tree->getResolution();
00494     emit changeResolution(m_octreeResolution);
00495 
00496     setOcTreeUISwitches();
00497     showOcTree();
00498     m_glwidget->resetView();
00499 
00500     if (tree->getTreeType() == "ColorOcTree"){
00501       // map color and height map share the same color array and QAction
00502       ui.actionHeight_map->setText ("Map color");  // rename QAction in Menu
00503       this->on_actionHeight_map_toggled(true); // enable color view
00504       ui.actionHeight_map->setChecked(true);
00505     }
00506   }
00507   else {
00508     QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok);
00509   }
00510 }
00511 
00512 
00513 // EXPERIMENTAL
00514 void ViewerGui::openMapCollection() {
00515 
00516   OCTOMAP_DEBUG("Opening hierarchy from %s...\n", m_filename.c_str());
00517 
00518   std::ifstream infile(m_filename.c_str(), std::ios_base::in |std::ios_base::binary);
00519   if (!infile.is_open()) {
00520     QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok);
00521     return;
00522   }
00523   infile.close();
00524 
00525   MapCollection<MapNode<OcTree> > collection(m_filename);
00526   int i=0;
00527   for (MapCollection<MapNode<OcTree> >::iterator it = collection.begin();
00528       it != collection.end(); ++it) {
00529     OCTOMAP_DEBUG("Adding hierarchy node %s\n", (*it)->getId().c_str());
00530     OcTree* tree = (*it)->getMap();
00531     if (!tree)
00532       OCTOMAP_ERROR("Error while reading node %s\n", (*it)->getId().c_str());
00533     else {
00534       OCTOMAP_DEBUG("Read tree with %zu tree nodes\n", tree->size());
00535     }
00536     pose6d  origin = (*it)->getOrigin();
00537     this->addOctree(tree, i, origin);
00538     ++i;
00539   }
00540   setOcTreeUISwitches();
00541   showOcTree();
00542   m_glwidget->resetView();
00543   OCTOMAP_DEBUG("done\n");
00544 }
00545 
00546 void ViewerGui::loadGraph(bool completeGraph) {
00547 
00548   ui.actionSettings->setEnabled(true);
00549   ui.actionPointcloud->setEnabled(true);
00550   ui.actionPointcloud->setChecked(false);
00551   ui.actionTrajectory->setEnabled(true);
00552   ui.actionOctree_cells->setEnabled(true);
00553   ui.actionOctree_cells->setChecked(true);
00554   ui.actionOctree_structure->setEnabled(true);
00555   ui.actionOctree_structure->setChecked(false);
00556   ui.actionFree->setChecked(false);
00557   ui.actionFree->setEnabled(true);
00558   ui.actionReload_Octree->setEnabled(true);
00559   ui.actionConvert_ml_tree->setEnabled(true);
00560 
00561   unsigned graphSize = m_scanGraph->size();
00562   unsigned currentScan;
00563 
00564   if (completeGraph){
00565     fprintf(stderr, "loadGraph:: generating octree from complete graph.\n" );
00566     m_nextScanToAdd = m_scanGraph->end();
00567     generateOctree();
00568     currentScan = graphSize;
00569   }
00570   else{
00571     m_nextScanToAdd = m_scanGraph->begin();
00572 
00573     //if (m_ocTree) delete m_ocTree;
00574     //m_ocTree = new octomap::OcTree(m_octreeResolution);
00575     OcTree* tree = new octomap::OcTree(m_octreeResolution);
00576     this->addOctree(tree, DEFAULT_OCTREE_ID);
00577 
00578     addNextScan();
00579 
00580     currentScan = 1;
00581   }
00582 
00583   m_glwidget->resetView();
00584   QApplication::restoreOverrideCursor();
00585 
00586   emit changeNumberOfScans(graphSize);
00587   emit changeCurrentScan(currentScan);
00588   showInfo("Done (" +QString::number(currentScan)+ " of "+ QString::number(graphSize)+" nodes)", true);
00589 
00590   if (!m_trajectoryDrawer){
00591     m_trajectoryDrawer = new TrajectoryDrawer();
00592   }
00593   m_trajectoryDrawer->setScanGraph(*m_scanGraph);
00594 
00595   if (!m_pointcloudDrawer){
00596     m_pointcloudDrawer = new PointcloudDrawer();
00597   }
00598   m_pointcloudDrawer->setScanGraph(*m_scanGraph);
00599 
00600   m_cameraFollowMode->setScanGraph(m_scanGraph);
00601 
00602   if (ui.actionTrajectory->isChecked())
00603     m_glwidget->addSceneObject(m_trajectoryDrawer);
00604 
00605   if (ui.actionPointcloud->isChecked())
00606     m_glwidget->addSceneObject(m_pointcloudDrawer);
00607 }
00608 
00609 void ViewerGui::changeTreeDepth(int depth){
00610   // range check:
00611   if (depth < 1 || depth > 16)
00612     return;
00613 
00614   m_max_tree_depth = unsigned(depth);
00615 
00616   if (m_octrees.size() > 0)
00617     showOcTree();
00618 }
00619 
00620 
00621 void ViewerGui::on_actionExit_triggered(){
00622   this->close();
00623 }
00624 
00625 void ViewerGui::on_actionHelp_triggered(){
00626   m_glwidget->help();
00627 }
00628 
00629 void ViewerGui::on_actionSettings_triggered(){
00630 
00631   ViewerSettings dialog(this);
00632   dialog.setResolution(m_octreeResolution);
00633   dialog.setLaserType(m_laserType);
00634   dialog.setMaxRange(m_laserMaxRange);
00635 
00636 
00637   if (dialog.exec()){
00638 
00639     double oldResolution = m_octreeResolution;
00640     double oldLaserMaxRange = m_laserMaxRange;
00641     double oldType = m_laserType;
00642 
00643     m_octreeResolution = dialog.getResolution();
00644     m_laserType = dialog.getLaserType();
00645     m_laserMaxRange = dialog.getMaxRange();
00646 
00647     // apply new settings
00648     bool resolutionChanged = (std::abs(oldResolution - m_octreeResolution) > 1e-5);
00649     bool maxRangeChanged = (std::abs(oldLaserMaxRange - m_laserMaxRange) > 1e-5);
00650 
00651     if (resolutionChanged)
00652       emit changeResolution(m_octreeResolution);
00653 
00654     if (oldType != m_laserType){ // parameters changed, reload file:
00655       openFile();
00656     } else if (resolutionChanged || maxRangeChanged){
00657       generateOctree();
00658     }
00659 
00660   }
00661 }
00662 
00663 void ViewerGui::on_actionOpen_file_triggered(){
00664   QString filename = QFileDialog::getOpenFileName(this,
00665                                                   tr("Open data file"), "",
00666                                                   "All supported files (*.graph *.bt *.ot *.dat);;OcTree file (*.ot);;Bonsai tree file (*.bt);;Binary scan graph (*.graph);;Pointcloud (*.dat);;All files (*)");
00667   if (!filename.isEmpty()){
00668 #ifdef _WIN32      
00669     m_filename = std::string(filename.toLocal8Bit().data());
00670 #else       
00671     m_filename = filename.toStdString();
00672 #endif
00673     openFile();
00674   }
00675 }
00676 
00677 
00678 void ViewerGui::on_actionOpen_graph_incremental_triggered(){
00679   QString filename = QFileDialog::getOpenFileName(this,
00680                                                   tr("Open graph file incrementally (at start)"), "",
00681                                                   "Binary scan graph (*.graph)");
00682   if (!filename.isEmpty()){
00683     m_glwidget->clearAll();
00684 
00685 #ifdef _WIN32      
00686     m_filename = std::string(filename.toLocal8Bit().data());
00687 #else       
00688     m_filename = filename.toStdString();
00689 #endif
00690 
00691     openGraph(false);
00692   }
00693 }
00694 
00695 void ViewerGui::on_actionSave_file_triggered(){
00696 
00697   OcTreeRecord* r;
00698   if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) {
00699     fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID);
00700     QMessageBox::warning(this, tr("3D Mapping Viewer"),
00701                          "Error: No OcTree present.",
00702                          QMessageBox::Ok);
00703     return;
00704   }
00705 
00706   QString filename = QFileDialog::getSaveFileName(this, tr("Save octree file"),
00707                                                   "", tr("Full OcTree (*.ot);;Bonsai Tree file (*.bt);;"));
00708 
00709   if (filename != ""){
00710     QApplication::setOverrideCursor(Qt::WaitCursor);
00711     showInfo("Writing file... ", false);
00712 
00713     QFileInfo fileinfo(filename);
00714     std::string std_filename;
00715 #ifdef _WIN32      
00716     std_filename = filename.toLocal8Bit().data();
00717 #else       
00718     std_filename = filename.toStdString();
00719 #endif
00720 
00721     AbstractOcTree* t = r->octree;
00722 
00723     if (fileinfo.suffix() == "bt") {
00724       AbstractOccupancyOcTree* ot = dynamic_cast<AbstractOccupancyOcTree*> (t);
00725       if (ot)
00726         ot->writeBinaryConst(std_filename);
00727       else{
00728         QMessageBox::warning(this, "Unknown tree type",
00729                              "Could not convert to occupancy tree for writing .bt file",
00730                              QMessageBox::Ok);
00731       }
00732     }
00733     else if (fileinfo.suffix() == "ot"){
00734       r->octree->write(std_filename);
00735     }
00736     else {
00737       QMessageBox::warning(this, "Unknown file",
00738                            "Cannot write file, unknown extension: "+fileinfo.suffix(),
00739                            QMessageBox::Ok);
00740     }
00741 
00742     QApplication::restoreOverrideCursor();
00743     showInfo("Done.", true);
00744   }
00745 }
00746 
00747 void ViewerGui::on_actionClear_nodes_in_selection_triggered(){
00748   point3d min, max;
00749   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00750   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00751 
00752   updateNodesInBBX(min, max, false);
00753 }
00754 
00755 void ViewerGui::on_actionClear_selection_triggered(){
00756   point3d min, max;
00757   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00758   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00759 
00760   setNodesInBBX(min, max, false);
00761 }
00762 
00763 void ViewerGui::on_actionClear_unknown_in_selection_triggered()
00764 {
00765   point3d min, max;
00766   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00767   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00768 
00769   setNonNodesInBBX(min, max, false);
00770 }
00771 
00772 void ViewerGui::on_actionFill_unknown_in_selection_triggered()
00773 {
00774   point3d min, max;
00775   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00776   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00777 
00778   setNonNodesInBBX(min, max, true);
00779 }
00780 
00781 void ViewerGui::on_actionFill_selection_triggered(){
00782   point3d min, max;
00783   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00784   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00785 
00786   setNodesInBBX(min, max, true);
00787 }
00788 
00789 void ViewerGui::on_actionFill_nodes_in_selection_triggered(){
00790   point3d min, max;
00791   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00792   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00793 
00794   updateNodesInBBX(min, max, true);
00795 }
00796 
00797 void ViewerGui::on_actionDelete_nodes_in_selection_triggered(){
00798   point3d min, max;
00799   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00800   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00801 
00802   for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
00803     OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);
00804 
00805     if (octree){
00806       for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max),
00807           end=octree->end_leafs_bbx(); it!= end; ++it){
00808         octree->deleteNode(it.getKey(), it.getDepth());
00809       }
00810     } else{
00811       QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
00812                            QMessageBox::Ok);
00813 
00814     }
00815   }
00816 
00817   showOcTree();
00818 }
00819 
00820 void ViewerGui::on_actionDelete_nodes_outside_of_selection_triggered(){
00821   point3d min, max;
00822   m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
00823   m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());
00824 
00825   for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
00826     OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);
00827 
00828     if (octree){
00829       octomap::OcTreeKey minKey, maxKey;
00830 
00831       if (!octree->coordToKeyChecked(min, minKey) || !octree->coordToKeyChecked(max, maxKey)){
00832         return;
00833       }
00834 
00835       for(OcTree::leaf_iterator it = octree->begin_leafs(),
00836           end=octree->end_leafs(); it!= end; ++it){
00837         // check if outside of bbx:
00838         OcTreeKey k = it.getKey();
00839         if  (k[0] < minKey[0] || k[1] < minKey[1] || k[2] < minKey[2]
00840                                                                    || k[0] > maxKey[0] || k[1] > maxKey[1] || k[2] > maxKey[2])
00841         {
00842           octree->deleteNode(k, it.getDepth());
00843         }
00844       }
00845     } else
00846       QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
00847                            QMessageBox::Ok);
00848   }
00849 
00850   showOcTree();
00851 }
00852 
00853 void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, bool occupied){
00854   for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
00855     OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);
00856 
00857     if (octree){
00858       float logodds;
00859       if (occupied)
00860         logodds = octree->getClampingThresMaxLog();
00861       else
00862         logodds = octree->getClampingThresMinLog();
00863 
00864       for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max),
00865           end=octree->end_leafs_bbx(); it!= end; ++it)
00866       {
00867         // directly set values of leafs:
00868         it->setLogOdds(logodds);
00869       }
00870 
00871       // update inner nodes to make tree consistent:
00872       octree->updateInnerOccupancy();
00873 
00874     } else
00875       QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
00876                            QMessageBox::Ok);
00877 
00878   }
00879 
00880   showOcTree();
00881 }
00882 
00883 
00884 void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, bool occupied){
00885   for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
00886     OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);
00887 
00888     if (octree){
00889       float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog();
00890       if (!occupied)
00891         logodds *= -1;
00892 
00893       OcTreeKey minKey(0,0,0);
00894       OcTreeKey maxKey(0,0,0);
00895       octree->coordToKeyChecked(min, minKey);
00896       octree->coordToKeyChecked(max, maxKey);
00897       OcTreeKey k;
00898       for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){
00899         for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){
00900           for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){
00901             octree->updateNode(k, logodds);
00902           }
00903         }
00904       }
00905     }
00906 
00907   }
00908 
00909   showOcTree();
00910 }
00911 
00912 void ViewerGui::setNonNodesInBBX(const point3d& min, const point3d& max, bool occupied) {
00913   for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
00914     OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);
00915 
00916     if (octree){
00917       float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog();
00918       if (!occupied)
00919         logodds *= -1;
00920 
00921       OcTreeKey minKey(0,0,0);
00922       OcTreeKey maxKey(0,0,0);
00923       octree->coordToKeyChecked(min, minKey);
00924       octree->coordToKeyChecked(max, maxKey);
00925       OcTreeKey k;
00926       for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){
00927         for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){
00928           for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){
00929             OcTreeNode* n = octree->search(k);
00930             if(!n)
00931               octree->updateNode(k, logodds);
00932           }
00933         }
00934       }
00935     }
00936 
00937   }
00938 
00939   showOcTree();
00940 }
00941 
00942 void ViewerGui::on_actionExport_view_triggered(){
00943   m_glwidget->openSnapshotFormatDialog();
00944   m_glwidget->saveSnapshot(false);
00945 }
00946 
00947 void ViewerGui::on_actionExport_sequence_triggered(bool checked){
00948   if(checked) {
00949     if(m_glwidget->openSnapshotFormatDialog()) {
00950       m_glwidget->saveSnapshot(false);
00951       m_glwidget->setSnapshotCounter(0);
00952       connect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool)));
00953     } else {
00954       ui.actionExport_sequence->setChecked(false);
00955     }
00956   } else {
00957     disconnect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool)));
00958   }
00959 }
00960 
00961 void ViewerGui::on_actionPrintout_mode_toggled(bool checked){
00962   if (checked) {
00963     ui.actionHeight_map->setChecked(false);
00964     ui.actionSemanticColoring->setChecked(false);
00965   }
00966 
00967   m_glwidget->enablePrintoutMode(checked);
00968 }
00969 
00970 void ViewerGui::on_actionSelection_box_toggled(bool checked){
00971 
00972   ui.menuDelete_nodes->setEnabled(checked);
00973   ui.menuFill_selection->setEnabled(checked);
00974   ui.menuChange_nodes_in_selection->setEnabled(checked);
00975 
00976 
00977   m_glwidget->enableSelectionBox(checked);
00978 
00979 
00980   m_glwidget->updateGL();
00981 }
00982 
00983 void ViewerGui::on_actionHeight_map_toggled(bool checked){
00984   if (checked) {
00985     ui.actionPrintout_mode->setChecked(false);
00986     ui.actionSemanticColoring->setChecked(false);
00987   }
00988   m_glwidget->enableHeightColorMode(checked);
00989 }
00990 
00991 void ViewerGui::on_actionSemanticColoring_toggled(bool checked) {
00992   if (checked) {
00993     ui.actionHeight_map->setChecked(false);
00994     ui.actionPrintout_mode->setChecked(false);
00995   }
00996 
00997   m_glwidget->enableSemanticColoring(checked);
00998 }
00999 
01000 
01001 void ViewerGui::on_actionStore_camera_triggered(){
01002   m_glwidget->camera()->deletePath(0);
01003   m_glwidget->camera()->addKeyFrameToPath(0);
01004   m_cameraStored = true;
01005   ui.actionRestore_camera->setEnabled(true);
01006 }
01007 
01008 void ViewerGui::on_actionRestore_camera_triggered(){
01009   if (m_cameraStored){
01010     m_glwidget->camera()->playPath(0);
01011   }
01012 }
01013 
01014 void ViewerGui::on_actionPointcloud_toggled(bool checked){
01015   if (m_pointcloudDrawer){
01016     if (checked)
01017       m_glwidget->addSceneObject(m_pointcloudDrawer);
01018     else
01019       m_glwidget->removeSceneObject(m_pointcloudDrawer);
01020   }
01021 }
01022 
01023 void ViewerGui::on_actionTrajectory_toggled(bool checked){
01024   if (m_trajectoryDrawer){
01025     if (checked)
01026       m_glwidget->addSceneObject(m_trajectoryDrawer);
01027     else
01028       m_glwidget->removeSceneObject(m_trajectoryDrawer);
01029   }
01030 }
01031 
01032 void ViewerGui::on_actionAxes_toggled(bool checked){
01033   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01034       it != m_octrees.end(); ++it) {
01035     it->second.octree_drawer->enableAxes(checked);
01036   }
01037   m_glwidget->updateGL();
01038 }
01039 
01040 void ViewerGui::on_actionHideBackground_toggled(bool checked) {
01041   OcTreeRecord* r;
01042   if (getOctreeRecord(DEFAULT_OCTREE_ID, r)) {
01043     if (checked) m_glwidget->removeSceneObject(r->octree_drawer);
01044     else         m_glwidget->addSceneObject(r->octree_drawer);
01045     m_glwidget->updateGL();
01046   }
01047 }
01048 
01049 void ViewerGui::on_actionClear_triggered() {
01050   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01051       it != m_octrees.end(); ++it) {
01052     m_glwidget->removeSceneObject(it->second.octree_drawer);
01053     delete (it->second.octree_drawer);
01054     delete (it->second.octree);
01055   }
01056   m_octrees.clear();
01057   showOcTree();
01058 }
01059 
01060 void ViewerGui::on_actionTest_triggered(){
01061 
01062 }
01063 
01064 void ViewerGui::on_actionReload_Octree_triggered(){
01065   if (m_scanGraph) {
01066     generateOctree();
01067   } else {
01068     openFile();
01069   }
01070 }
01071 
01072 void ViewerGui::on_actionConvert_ml_tree_triggered(){
01073   QApplication::setOverrideCursor(Qt::WaitCursor);
01074   if (m_octrees.size()) {
01075     showInfo("Converting OcTree to maximum Likelihood map... ");
01076     for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01077         it != m_octrees.end(); ++it) {
01078       AbstractOcTree* t = it->second.octree;
01079       if (dynamic_cast<OcTree*>(t)) {
01080         ((OcTree*) t)->toMaxLikelihood();
01081       }
01082       else if (dynamic_cast<OcTree*>(t)) {
01083         ((ColorOcTree*) t)->toMaxLikelihood();
01084       }
01085     }
01086     showInfo("Done.", true);
01087     showOcTree();
01088     QApplication::restoreOverrideCursor();
01089   }
01090 }
01091 
01092 
01093 void ViewerGui::on_actionPrune_tree_triggered(){
01094   QApplication::setOverrideCursor(Qt::WaitCursor);
01095   if (m_octrees.size()) {
01096     showInfo("Pruning OcTree... ");
01097     for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01098         it != m_octrees.end(); ++it) {
01099       it->second.octree->prune();
01100     }
01101     showOcTree();
01102     showInfo("Done.", true);
01103   }
01104   QApplication::restoreOverrideCursor();
01105 }
01106 
01107 
01108 void ViewerGui::on_actionExpand_tree_triggered(){
01109 
01110   QApplication::setOverrideCursor(Qt::WaitCursor);
01111 
01112   // if (m_ocTree) {
01113   if (m_octrees.size()) {
01114     showInfo("Expanding OcTree... ");
01115     for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01116         it != m_octrees.end(); ++it) {
01117       it->second.octree->expand();
01118     }
01119     showOcTree();
01120 
01121     showInfo("Done.", true);
01122   }
01123   QApplication::restoreOverrideCursor();
01124 }
01125 
01126 
01127 void ViewerGui::on_actionOctree_cells_toggled(bool enabled) {
01128   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01129       it != m_octrees.end(); ++it) {
01130     it->second.octree_drawer->enableOcTreeCells(enabled);
01131   }
01132   m_glwidget->updateGL();
01133 }
01134 
01135 void ViewerGui::on_actionOctree_structure_toggled(bool enabled) {
01136   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01137       it != m_octrees.end(); ++it) {
01138     it->second.octree_drawer->enableOcTree(enabled);
01139   }
01140   m_glwidget->updateGL();
01141 }
01142 
01143 void ViewerGui::on_actionFree_toggled(bool enabled) {
01144   for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin();
01145       it != m_octrees.end(); ++it) {
01146     it->second.octree_drawer->enableFreespace(enabled);
01147   }
01148   m_glwidget->updateGL();
01149 
01150 }
01151 
01152 void ViewerGui::on_actionSelected_toggled(bool enabled) {
01153   // if(m_octreeDrawer) {
01154     //   m_octreeDrawer->enableSelection(enabled);
01155 
01156   //   // just for testing, you should set the selection somewhere else and only enable it here:
01157   //   if (enabled){
01158   //     std::list<OcTreeVolume> selection;
01159   //     std::pair<octomath::Vector3, double> volume(octomath::Vector3(0.0, 0.0, 0.0), 0.2);
01160   //     selection.push_back(volume);
01161   //     m_octreeDrawer->setOcTreeSelection(selection);
01162 
01163   //   } else{
01164   //     m_octreeDrawer->clearOcTreeSelection();
01165   //   }
01166   //   m_glwidget->updateGL();
01167   // }
01168 }
01169 
01170 
01171 void ViewerGui::on_action_bg_black_triggered() {
01172   m_glwidget->setBackgroundColor( QColor(0,0,0) );
01173   m_glwidget->qglClearColor( m_glwidget->backgroundColor() );
01174 }
01175 
01176 void ViewerGui::on_action_bg_white_triggered() {
01177   m_glwidget->setBackgroundColor( QColor(255,255,255) );
01178   m_glwidget->qglClearColor( m_glwidget->backgroundColor() );
01179 }
01180 
01181 void ViewerGui::on_action_bg_gray_triggered() {
01182   m_glwidget->setBackgroundColor( QColor(117,117,117) );
01183   m_glwidget->qglClearColor( m_glwidget->backgroundColor() );
01184 }
01185 
01186 void ViewerGui::on_savecampose_triggered() {
01187   QString filename = QFileDialog::getSaveFileName(this, "Save Viewer State", "camera.xml", "Camera/State file (*.xml)");
01188   if (!filename.isEmpty()) {
01189     saveCameraPosition(filename.toAscii().constData());
01190   }
01191 }
01192 
01193 void ViewerGui::on_loadcampose_triggered() {
01194   QString filename = QFileDialog::getOpenFileName(this, "Load Viewer State", "camera.xml", "Camera/State file (*.xml)");
01195   if (!filename.isEmpty()) {
01196     loadCameraPosition(filename.toAscii().constData());
01197   }
01198 }
01199 
01200 
01201 
01202 void ViewerGui::saveCameraPosition(const char* filename) const {
01203   // HACK get non-const pointer to myself
01204   ViewerWidget* aux = const_cast<ViewerWidget*>( m_glwidget);
01205   aux->setStateFileName(QString(filename));
01206   aux->saveStateToFile();
01207   aux->setStateFileName(QString::null);
01208 }
01209 
01210 void ViewerGui::loadCameraPosition(const char* filename) {
01211   m_glwidget->setStateFileName(QString(filename));
01212   m_glwidget->restoreStateFromFile();
01213   m_glwidget->setStateFileName(QString::null);
01214 }
01215 
01216 
01217 }
01218 
01219 


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:26