From f51aa67605568713340270984d5b11cf25c046cc Mon Sep 17 00:00:00 2001 From: Armin Hornung Date: Sun, 29 Jan 2017 22:35:12 +0100 Subject: [PATCH] Octovis improvements: Optimized rendering with amortized voxel construction and slider for occupancy threshold. Contributed by K. Stepanas. --- octovis/include/octovis/ColorOcTreeDrawer.h | 27 +- octovis/include/octovis/OcTreeDrawer.h | 140 +++++- octovis/include/octovis/ViewerGui.h | 6 +- octovis/include/octovis/ViewerSettingsPanel.h | 7 + .../include/octovis/ViewerSettingsPanel.ui | 304 +++++++----- octovis/src/ColorOcTreeDrawer.cpp | 166 +++---- octovis/src/OcTreeDrawer.cpp | 456 +++++++++++++----- octovis/src/ViewerGui.cpp | 86 ++++ octovis/src/ViewerSettingsPanel.cpp | 42 +- 9 files changed, 887 insertions(+), 347 deletions(-) diff --git a/octovis/include/octovis/ColorOcTreeDrawer.h b/octovis/include/octovis/ColorOcTreeDrawer.h index 67367305..ac871938 100644 --- a/octovis/include/octovis/ColorOcTreeDrawer.h +++ b/octovis/include/octovis/ColorOcTreeDrawer.h @@ -38,8 +38,31 @@ namespace octomap { virtual void setOcTree(const AbstractOcTree& tree_pnt, const pose6d& origin, int map_id_); protected: - - + /// Specialisation of @c AbstractOcTreeIterator for handling @c ColorOcTree instances. + class ColorOcTreeIterator : public AbstractOcTreeIterator { + public: + ColorOcTreeIterator(const ColorOcTree *tree); + + virtual void setOccupancyThres(double threshold); + virtual double getOccupancyThres() const; + virtual unsigned int getTreeSize() const; + virtual void getMetricMin(double& x, double& y, double& z) const; + virtual void getMetricMax(double& x, double& y, double& z) const; + virtual bool isValid() const; + virtual bool begin(unsigned int max_tree_depth); + virtual bool moveNext(); + virtual bool getColor(unsigned char& r, unsigned char& g, unsigned char& b) const; + virtual float getOccupancy() const; + virtual point3d getCoordinate() const; + virtual double getSize() const; + virtual bool isLeaf() const; + virtual bool isOccupied() const; + virtual bool isAtThreshold() const; + + private: + const ColorOcTree* m_tree; + ColorOcTree::tree_iterator m_it; + }; }; diff --git a/octovis/include/octovis/OcTreeDrawer.h b/octovis/include/octovis/OcTreeDrawer.h index 34d719a3..1a81afd2 100644 --- a/octovis/include/octovis/OcTreeDrawer.h +++ b/octovis/include/octovis/OcTreeDrawer.h @@ -31,10 +31,33 @@ namespace octomap { class OcTreeDrawer: public octomap::SceneObject { public: + /// Current construction phase for amortised initialisation. + enum BuildPhase { + NONE, ///< No phase. Nothing to do. + INIT, ///< Initialisation phase. + COUNT, ///< Note counting phase. + BUILD, ///< Building voxels. + COMPLETE ///< Construction complete in this update. + }; + OcTreeDrawer(); virtual ~OcTreeDrawer(); void clear(); + /// Updates the amortised construction of the render items. + /// + /// Processing continues until @p timeout is reached, or construction is completed. + /// The progress may be monitored by checking the return value and the @p progress + /// parameters. + /// + /// @param timeout Processing timeout in milliseconds. Use zero to force completion (no timeout). + /// @param[out] progress Optional pointer to write current progress value. This is generally the + /// number of tree nodes touched in the current phase. + /// @param[out] maxProgress Optional pointer to write maximum progress to. This is generally the + /// total number of nodes in the tree. + /// @return The current construction phase for the tree. See @c BuildPhase. + BuildPhase update(unsigned int timeout, unsigned int* progress = 0, unsigned int* maxProgress = 0); + void draw() const; // initialization of drawer ------------------------- @@ -57,6 +80,9 @@ namespace octomap { /// clear the visualization of the OcTree selection void clearOcTreeSelection(); + void setOccupancyThreshold(double threshold);// { m_occupancyThreshold = threshold; } + inline double getOccupancyThreshold() const { return m_occupancyThreshold; } + /// sets alpha level for occupied cells void setAlphaOccupied(double alpha); void setAlternativeDrawing(bool flag){m_alternativeDrawing = flag;} @@ -115,24 +141,124 @@ namespace octomap { GLfloat** glColorArray); - void initOctreeGridVis(); + void initOctreeGridVis(bool expand = false); + + void buildVoxels(unsigned int timeout = 1000/30); + + /// Abstract tree iteration adaptor used to process the current tree type. + /// + /// General usage is to call @c begin(), call various node methods while @c isValid() is true + /// and use @c moveNext() to skip to the next node. + class AbstractOcTreeIterator { + public: + /// Virtual destructor (empty). + virtual inline ~AbstractOcTreeIterator() {} + + /// Sets the occupancy threshold for the tree. Nodes are considered occupied above this threshold. + virtual void setOccupancyThres(double threshold) = 0; + /// Returns the current occupancy threshold. + virtual double getOccupancyThres() const = 0; + /// Returns the number of nodes in the tree. + virtual unsigned int getTreeSize() const = 0; + /// minimum value of the bounding box of all known space in x, y, z + virtual void getMetricMin(double& x, double& y, double& z) const = 0; + /// maximum value of the bounding box of all known space in x, y, z + virtual void getMetricMax(double& x, double& y, double& z) const = 0; + /// Returns true if the currently referencing a valid node. + virtual bool isValid() const = 0; + /// Begins iteration of the tree. + /// @param max_tree_depth Maximum traversal depth. + /// @return True if the tree is not empty. + virtual bool begin(unsigned int max_tree_depth) = 0; + /// Moves to the next node. + /// @return True if successfully referencing a new node, false if iteration is complete. + virtual bool moveNext() = 0; + /// Requests the color for the current node. Only for nodes supporting color. + /// RGB values are only valid when returning true. + /// + /// Do not call when @c isValid() is false. + /// @param[out] r Red color channel. + /// @param[out] g Green color channel. + /// @param[out] b Blue color channel. + /// @return true if color is supported and valid for the current node. + virtual bool getColor(unsigned char& r, unsigned char& g, unsigned char& b) const { return false; } + /// Returns the occupancy value for the current node. + /// Do not call when @c isValid() is false. + virtual float getOccupancy() const = 0; + /// Returns the coordinate for the current node. + /// Do not call when @c isValid() is false. + virtual point3d getCoordinate() const = 0; + /// Returns the size of the current node. + /// Do not call when @c isValid() is false. + virtual double getSize() const = 0; + /// Returns true if the current node is a leaf. + /// Do not call when @c isValid() is false. + virtual bool isLeaf() const = 0; + /// Returns true if the current node is occupied. + /// Do not call when @c isValid() is false. + virtual bool isOccupied() const = 0; + /// Returns true if the current node is at the occupancy threshold. + /// Do not call when @c isValid() is false. + virtual bool isAtThreshold() const = 0; + }; + + /// Specialisation of @c AbstractOcTreeIterator for handling @c OcTree instances. + class OcTreeIterator : public AbstractOcTreeIterator { + public: + OcTreeIterator(const OcTree *tree); + + virtual void setOccupancyThres(double threshold); + virtual double getOccupancyThres() const; + virtual unsigned int getTreeSize() const; + virtual void getMetricMin(double& x, double& y, double& z) const; + virtual void getMetricMax(double& x, double& y, double& z) const; + virtual bool isValid() const; + virtual bool begin(unsigned int max_tree_depth); + virtual bool moveNext(); + virtual float getOccupancy() const; + virtual point3d getCoordinate() const; + virtual double getSize() const; + virtual bool isLeaf() const; + virtual bool isOccupied() const; + virtual bool isAtThreshold() const; + + private: + const OcTree* m_tree; + OcTree::tree_iterator m_it; + }; + + //! Data structure tracking progressive regeneration of the octree for display. + //! This is used to amortise generation of render primitives. + struct Regeneration { + BuildPhase phase; ///< Current construction phase. + bool showAll; + bool uses_origin; + unsigned int progress, maxProgress; + unsigned int cnt_occupied, cnt_occupied_thres, cnt_free, cnt_free_thres; + AbstractOcTreeIterator* it; + + Regeneration(); + ~Regeneration(); + }; //! OpenGL representation of Octree cells (cubes) GLfloat** m_occupiedThresArray; - unsigned int m_occupiedThresSize; + unsigned int m_occupiedThresSize, m_occupiedThresCap; GLfloat** m_freeThresArray; - unsigned int m_freeThresSize; + unsigned int m_freeThresSize, m_freeThresCap; GLfloat** m_occupiedArray; - unsigned int m_occupiedSize; + unsigned int m_occupiedSize, m_occupiedCap; GLfloat** m_freeArray; - unsigned int m_freeSize; + unsigned int m_freeSize, m_freeCap; GLfloat** m_selectionArray; unsigned int m_selectionSize; //! Color array for occupied cells (height) GLfloat* m_occupiedThresColorArray; + unsigned int m_occupiedThresColorSize; GLfloat* m_occupiedColorArray; + unsigned int m_occupiedColorSize; //! OpenGL representation of Octree (grid structure) // TODO: put in its own drawer object! @@ -140,6 +266,7 @@ namespace octomap { unsigned int octree_grid_vertex_size; std::list m_grid_voxels; + bool m_gridVoxelsBuilt; bool m_drawOccupied; bool m_drawOcTreeGrid; @@ -152,11 +279,14 @@ namespace octomap { unsigned int m_max_tree_depth; double m_alphaOccupied; + double m_occupancyThreshold; octomap::pose6d origin; octomap::pose6d initial_origin; int map_id; + + Regeneration m_regeneration; }; } diff --git a/octovis/include/octovis/ViewerGui.h b/octovis/include/octovis/ViewerGui.h index 41a5dee7..6cef4c61 100644 --- a/octovis/include/octovis/ViewerGui.h +++ b/octovis/include/octovis/ViewerGui.h @@ -51,7 +51,7 @@ namespace octomap { Q_OBJECT public: - ViewerGui(const std::string& filename="", QWidget *parent = 0, unsigned int initTreeDepth = 16); + ViewerGui(const std::string& filename="", QWidget *parent = 0, unsigned int initDepth = 16); ~ViewerGui(); static const unsigned int LASERTYPE_URG = 0; @@ -65,11 +65,14 @@ namespace octomap { void changeTreeDepth(int depth); void addNextScans(unsigned scans); void gotoFirstScan(); + void recalculateOccupancy(double cutoff); bool isShown(); private slots: + void updateDrawers(); + // auto-connected Slots (by name)) void on_actionExit_triggered(); @@ -210,6 +213,7 @@ namespace octomap { bool m_cameraStored; QLabel* m_mapSizeStatus; QLabel* m_mapMemoryStatus; + QLabel* m_buildingStatus; /// Filename of last loaded file, in case it is necessary to reload it std::string m_filename; diff --git a/octovis/include/octovis/ViewerSettingsPanel.h b/octovis/include/octovis/ViewerSettingsPanel.h index 371ced2f..e0bd33c6 100644 --- a/octovis/include/octovis/ViewerSettingsPanel.h +++ b/octovis/include/octovis/ViewerSettingsPanel.h @@ -43,16 +43,21 @@ public slots: void setNumberOfScans(unsigned scans); void setCurrentScan(unsigned scan); void setResolution(double resolution); + void setOccupancyCutoff(double cutoff, bool dirty = true); void setTreeDepth(int depth); + void changeOccupancyCutoff(int cutoff); + void changeOccupancyCutoff(double cutoff); private slots: void on_firstScanButton_clicked(); void on_lastScanButton_clicked(); void on_nextScanButton_clicked(); void on_fastFwdScanButton_clicked(); + void on_recalculateOccupancyButton_clicked(); signals: void treeDepthChanged(int depth); + void recaculateCutoff(double cutoff); void addNextScans(unsigned scans); void gotoFirstScan(); @@ -65,6 +70,8 @@ private slots: unsigned m_numberScans; unsigned m_treeDepth; double m_resolution; + double m_occupancyCutoff; + bool m_notifyCutoff; }; #endif // VIEWERSETTINGSPANEL_H diff --git a/octovis/include/octovis/ViewerSettingsPanel.ui b/octovis/include/octovis/ViewerSettingsPanel.ui index d7d7a82b..9cebe2e5 100644 --- a/octovis/include/octovis/ViewerSettingsPanel.ui +++ b/octovis/include/octovis/ViewerSettingsPanel.ui @@ -18,147 +18,197 @@ ViewerSettingsPanel - + - - - - 0 - 0 - - - - Tree depth cutoff - - - Qt::AlignHCenter|Qt::AlignTop - - - false - - - false - - - - - - - - - 4 - 0 - - - - - 16777196 - 16777215 - - - - false - - - QAbstractSpinBox::NoButtons - - - 1 - - - 16 - - - 16 - - + + + + + + 0 + 0 + + + + Tree depth cutoff + + + Qt::AlignHCenter|Qt::AlignTop + + + false + + + false + + + + + + + + + 4 + 0 + + + + + 16777196 + 16777215 + + + + false + + + QAbstractSpinBox::NoButtons + + + 1 + + + 16 + + + 16 + + + + + + + + 0 + 75 + + + + + 16777215 + 500 + + + + 1 + + + 16 + + + 1 + + + 16 + + + Qt::Vertical + + + true + + + true + + + QSlider::TicksBothSides + + + 1 + + + + - - - - - 0 - 75 - - - - - 16777215 - 500 - - - - 1 - - - 16 - - - 1 - - - 16 - - - Qt::Vertical - - - true - - - true - - - QSlider::TicksBothSides - - - 1 - - + + + + QLayout::SetMinimumSize + + + + + Leaf size: + + + + + + + 0 m + + + Qt::PlainText + + + + - - - - - QLayout::SetMinimumSize - - - - - Leaf size: - - - - - - - 0 m - - - Qt::PlainText - - + + + + + + Occupancy cutoff + + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + + + + + &Recalculate + + + + + + + 100 + + + Qt::Vertical + + + QSlider::TicksBothSides + + + 10 + + + + - - - + + + - - + + Scan graph progress - + - + - - + + first scan - + first scan diff --git a/octovis/src/ColorOcTreeDrawer.cpp b/octovis/src/ColorOcTreeDrawer.cpp index 70266b7c..fadc34ae 100644 --- a/octovis/src/ColorOcTreeDrawer.cpp +++ b/octovis/src/ColorOcTreeDrawer.cpp @@ -26,6 +26,77 @@ namespace octomap { + ColorOcTreeDrawer::ColorOcTreeIterator::ColorOcTreeIterator(const ColorOcTree* tree) : m_tree(tree) + { + } + + void ColorOcTreeDrawer::ColorOcTreeIterator::setOccupancyThres(double threshold){ + // TODO: need a better way than using a const cast for this. + const_cast(m_tree)->setOccupancyThres(threshold); + } + + double ColorOcTreeDrawer::ColorOcTreeIterator::getOccupancyThres() const { + return m_tree->getOccupancyThres(); + } + + unsigned int ColorOcTreeDrawer::ColorOcTreeIterator::getTreeSize() const { + return m_tree->size(); + } + + void ColorOcTreeDrawer::ColorOcTreeIterator::getMetricMin(double& x, double& y, double& z) const { + return m_tree->getMetricMin(x, y, z); + } + + void ColorOcTreeDrawer::ColorOcTreeIterator::getMetricMax(double& x, double& y, double& z) const { + return m_tree->getMetricMax(x, y, z); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::isValid() const { + return m_it != m_tree->end_tree(); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::begin(unsigned int max_tree_depth){ + m_it = m_tree->begin_tree(max_tree_depth); + return m_it != m_tree->end_tree(); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::moveNext(){ + ++m_it; + return m_it != m_tree->end_tree(); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::getColor(unsigned char& r, unsigned char& g, unsigned char& b) const { + const ColorOcTreeNode::Color c = m_it->getColor(); + r = c.r; + g = c.g; + b = c.b; + return true; + } + + float ColorOcTreeDrawer::ColorOcTreeIterator::getOccupancy() const{ + return m_it->getOccupancy(); + } + + point3d ColorOcTreeDrawer::ColorOcTreeIterator::getCoordinate() const{ + return m_it.getCoordinate(); + } + + double ColorOcTreeDrawer::ColorOcTreeIterator::getSize() const{ + return m_it.getSize(); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::isLeaf() const{ + return m_it.isLeaf(); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::isOccupied() const{ + return m_tree->isNodeOccupied(*m_it); + } + + bool ColorOcTreeDrawer::ColorOcTreeIterator::isAtThreshold() const{ + return m_tree->isNodeAtThreshold(*m_it); + } + ColorOcTreeDrawer::ColorOcTreeDrawer() : OcTreeDrawer() { } @@ -47,92 +118,15 @@ namespace octomap { this->origin = origin_; // maximum size to prevent crashes on large maps: (should be checked in a better way than a constant) - bool showAll = (tree.size() < 5 * 1e6); - bool uses_origin = ( (origin_.rot().x() != 0.) && (origin_.rot().y() != 0.) - && (origin_.rot().z() != 0.) && (origin_.rot().u() != 1.) ); - - // walk the tree one to find the number of nodes in each category - // (this is used to set up the OpenGL arrays) - // TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead... - unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0); - for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth), - end=tree.end_tree(); it!= end; ++it) { - if (it.isLeaf()) { - if (tree.isNodeOccupied(*it)){ // occupied voxels - if (tree.isNodeAtThreshold(*it)) ++cnt_occupied_thres; - else ++cnt_occupied; - } - else if (showAll) { // freespace voxels - if (tree.isNodeAtThreshold(*it)) ++cnt_free_thres; - else ++cnt_free; - } - } - } - // setup GL arrays for cube quads and cube colors - initGLArrays(cnt_occupied , m_occupiedSize , &m_occupiedArray , &m_occupiedColorArray); - initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray); - initGLArrays(cnt_free , m_freeSize , &m_freeArray, NULL); - initGLArrays(cnt_free_thres , m_freeThresSize , &m_freeThresArray, NULL); - - std::vector cube_template; - initCubeTemplate(origin, cube_template); - - unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0); - unsigned int color_idx_occupied(0), color_idx_occupied_thres(0); - - m_grid_voxels.clear(); - OcTreeVolume voxel; // current voxel, possibly transformed - for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth), - end=tree.end_tree(); it!= end; ++it) { - - if (it.isLeaf()) { // voxels for leaf nodes - if (uses_origin) - voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize()); - else - voxel = OcTreeVolume(it.getCoordinate(), it.getSize()); - - if (tree.isNodeOccupied(*it)){ // occupied voxels - if (tree.isNodeAtThreshold(*it)) { - idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray); - color_idx_occupied_thres = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b, - (unsigned char) (it->getOccupancy() * 255.), - color_idx_occupied_thres, &m_occupiedThresColorArray); - } - else { - idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray); - color_idx_occupied = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b, - (unsigned char)(it->getOccupancy() * 255.), - color_idx_occupied, &m_occupiedColorArray); - } - } - else if (showAll) { // freespace voxels - if (tree.isNodeAtThreshold(*it)) { - idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray); - } - else { - idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray); - } - } - - // grid structure voxel - if (showAll) m_grid_voxels.push_back(voxel); - } - - else { // inner node voxels (for grid structure only) - if (showAll) { - if (uses_origin) - voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize()); - else - voxel = OcTreeVolume(it.getCoordinate(), it.getSize()); - m_grid_voxels.push_back(voxel); - } - } - } // end for all voxels - - m_octree_grid_vis_initialized = false; - - if(m_drawOcTreeGrid) - initOctreeGridVis(); + m_regeneration.showAll = (tree.size() < 5 * 1e6); + m_regeneration.uses_origin = ( (origin_.rot().x() != 0.) && (origin_.rot().y() != 0.) + && (origin_.rot().z() != 0.) && (origin_.rot().u() != 1.) ); + + // Start amortised construction of the voxels. + delete m_regeneration.it; + m_regeneration.it = new ColorOcTreeIterator(&tree); + m_regeneration.phase = OcTreeDrawer::INIT; + buildVoxels(); } } // end namespace diff --git a/octovis/src/OcTreeDrawer.cpp b/octovis/src/OcTreeDrawer.cpp index 6868a7dd..c8bbc230 100644 --- a/octovis/src/OcTreeDrawer.cpp +++ b/octovis/src/OcTreeDrawer.cpp @@ -24,17 +24,100 @@ #include #include +#include #define OTD_RAD2DEG 57.2957795 namespace octomap { + OcTreeDrawer::OcTreeIterator::OcTreeIterator(const OcTree* tree) : m_tree(tree) + { + } + + void OcTreeDrawer::OcTreeIterator::setOccupancyThres(double threshold){ + // TODO: need a better way than using a const cast for this. + const_cast(m_tree)->setOccupancyThres(threshold); + } + + double OcTreeDrawer::OcTreeIterator::getOccupancyThres() const { + return m_tree->getOccupancyThres(); + } + + unsigned int OcTreeDrawer::OcTreeIterator::getTreeSize() const { + return m_tree->size(); + } + + void OcTreeDrawer::OcTreeIterator::getMetricMin(double& x, double& y, double& z) const { + return m_tree->getMetricMin(x, y, z); + } + + void OcTreeDrawer::OcTreeIterator::getMetricMax(double& x, double& y, double& z) const { + return m_tree->getMetricMax(x, y, z); + } + + bool OcTreeDrawer::OcTreeIterator::isValid() const { + return m_it != m_tree->end_tree(); + } + + bool OcTreeDrawer::OcTreeIterator::begin(unsigned int max_tree_depth){ + m_it = m_tree->begin_tree(max_tree_depth); + return m_it != m_tree->end_tree(); + } + + bool OcTreeDrawer::OcTreeIterator::moveNext(){ + ++m_it; + return m_it != m_tree->end_tree(); + } + + float OcTreeDrawer::OcTreeIterator::getOccupancy() const{ + return m_it->getOccupancy(); + } + + point3d OcTreeDrawer::OcTreeIterator::getCoordinate() const{ + return m_it.getCoordinate(); + } + + double OcTreeDrawer::OcTreeIterator::getSize() const{ + return m_it.getSize(); + } + + bool OcTreeDrawer::OcTreeIterator::isLeaf() const{ + return m_it.isLeaf(); + } + + bool OcTreeDrawer::OcTreeIterator::isOccupied() const{ + return m_tree->isNodeOccupied(*m_it); + } + + bool OcTreeDrawer::OcTreeIterator::isAtThreshold() const{ + return m_tree->isNodeAtThreshold(*m_it); + } + + + OcTreeDrawer::Regeneration::Regeneration() : phase(OcTreeDrawer::NONE), + showAll(false), uses_origin(false), + progress(0), maxProgress(0), + cnt_occupied(0), cnt_occupied_thres(0), + cnt_free(0), cnt_free_thres(0), + it(NULL) + { + } + + OcTreeDrawer::Regeneration::~Regeneration(){ + delete it; + } OcTreeDrawer::OcTreeDrawer() : SceneObject(), m_occupiedThresSize(0), m_freeThresSize(0), m_occupiedSize(0), m_freeSize(0), m_selectionSize(0), - octree_grid_vertex_size(0), m_alphaOccupied(0.8), map_id(0) + octree_grid_vertex_size(0), + m_max_tree_depth(0), + m_alphaOccupied(0.8), + m_occupancyThreshold(0.5), + map_id(0) { + m_occupiedCap = m_occupiedThresCap = m_freeCap = m_freeThresCap = m_occupiedColorSize = m_occupiedThresColorSize = 0; m_octree_grid_vis_initialized = false; + m_gridVoxelsBuilt = false; m_drawOccupied = true; m_drawOcTreeGrid = false; m_drawFree = false; @@ -50,6 +133,8 @@ namespace octomap { m_occupiedColorArray = NULL; m_occupiedThresColorArray = NULL; m_selectionArray = NULL; + octree_grid_vertex_array = NULL; + octree_grid_vertex_size = 0; // origin and movement initial_origin = octomap::pose6d(0,0,0,0,0,0); @@ -60,6 +145,14 @@ namespace octomap { clear(); } + OcTreeDrawer::BuildPhase OcTreeDrawer::update(unsigned int timeout, unsigned int* progress, unsigned int* maxProgress) { + // Continue amortised construction. + buildVoxels(timeout); + if (progress) *progress = m_regeneration.progress; + if (maxProgress) *maxProgress = m_regeneration.maxProgress; + return m_regeneration.phase; + } + void OcTreeDrawer::draw() const { static int gl_list_index = -1; if(m_alternativeDrawing && gl_list_index < 0){ @@ -78,42 +171,42 @@ namespace octomap { glNewList(gl_list_index, GL_COMPILE_AND_EXECUTE); } - // push current status - glPushMatrix(); - // octomap::pose6d relative_transform = origin * initial_origin.inv(); + // push current status + glPushMatrix(); + // octomap::pose6d relative_transform = origin * initial_origin.inv(); - octomap::pose6d relative_transform = origin;// * initial_origin; + octomap::pose6d relative_transform = origin;// * initial_origin; - // apply relative transform - const octomath::Quaternion& q = relative_transform.rot(); - glTranslatef(relative_transform.x(), relative_transform.y(), relative_transform.z()); - - // convert quaternion to angle/axis notation - float scale = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z()); - if (scale) { - float axis_x = q.x() / scale; - float axis_y = q.y() / scale; - float axis_z = q.z() / scale; - float angle = acos(q.u()) * 2.0f * OTD_RAD2DEG; // opengl expects DEG - glRotatef(angle, axis_x, axis_y, axis_z); - } + // apply relative transform + const octomath::Quaternion& q = relative_transform.rot(); + glTranslatef(relative_transform.x(), relative_transform.y(), relative_transform.z()); + + // convert quaternion to angle/axis notation + float scale = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z()); + if (scale) { + float axis_x = q.x() / scale; + float axis_y = q.y() / scale; + float axis_z = q.z() / scale; + float angle = acos(q.u()) * 2.0f * OTD_RAD2DEG; // opengl expects DEG + glRotatef(angle, axis_x, axis_y, axis_z); + } - glEnableClientState(GL_VERTEX_ARRAY); + glEnableClientState(GL_VERTEX_ARRAY); - if (m_drawOccupied) - drawOccupiedVoxels(); - if (m_drawFree) - drawFreeVoxels(); - if (m_drawOcTreeGrid) - drawOctreeGrid(); - if (m_drawSelection) - drawSelection(); + if (m_drawOccupied) + drawOccupiedVoxels(); + if (m_drawFree) + drawFreeVoxels(); + if (m_drawOcTreeGrid) + drawOctreeGrid(); + if (m_drawSelection) + drawSelection(); - if (m_displayAxes) { - drawAxes(); - } + if (m_displayAxes) { + drawAxes(); + } - glDisableClientState(GL_VERTEX_ARRAY); + glDisableClientState(GL_VERTEX_ARRAY); // reset previous status glPopMatrix(); @@ -148,93 +241,15 @@ namespace octomap { this->origin = origin; // maximum size to prevent crashes on large maps: (should be checked in a better way than a constant) - bool showAll = (octree.size() < 5 * 1e6); - bool uses_origin = ( (origin.rot().x() != 0.) && (origin.rot().y() != 0.) + m_regeneration.showAll = (octree.size() < 5 * 1e6); + m_regeneration.uses_origin = ( (origin.rot().x() != 0.) && (origin.rot().y() != 0.) && (origin.rot().z() != 0.) && (origin.rot().u() != 1.) ); - // walk the tree one to find the number of nodes in each category - // (this is used to set up the OpenGL arrays) - // TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead... - unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0); - for(OcTree::tree_iterator it = octree.begin_tree(this->m_max_tree_depth), - end=octree.end_tree(); it!= end; ++it) { - if (it.isLeaf()) { - if (octree.isNodeOccupied(*it)){ // occupied voxels - if (octree.isNodeAtThreshold(*it)) ++cnt_occupied_thres; - else ++cnt_occupied; - } - else if (showAll) { // freespace voxels - if (octree.isNodeAtThreshold(*it)) ++cnt_free_thres; - else ++cnt_free; - } - } - } - // setup GL arrays for cube quads and cube colors - initGLArrays(cnt_occupied , m_occupiedSize , &m_occupiedArray , &m_occupiedColorArray); - initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray); - initGLArrays(cnt_free , m_freeSize , &m_freeArray, NULL); - initGLArrays(cnt_free_thres , m_freeThresSize , &m_freeThresArray, NULL); - - double minX, minY, minZ, maxX, maxY, maxZ; - octree.getMetricMin(minX, minY, minZ); - octree.getMetricMax(maxX, maxY, maxZ); - - // set min/max Z for color height map - m_zMin = minZ; - m_zMax = maxZ; - - std::vector cube_template; - initCubeTemplate(origin, cube_template); - - unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0); - unsigned int color_idx_occupied(0), color_idx_occupied_thres(0); - - m_grid_voxels.clear(); - OcTreeVolume voxel; // current voxel, possibly transformed - for(OcTree::tree_iterator it = octree.begin_tree(this->m_max_tree_depth), - end=octree.end_tree(); it!= end; ++it) { - - if (it.isLeaf()) { // voxels for leaf nodes - if (uses_origin) - voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize()); - else - voxel = OcTreeVolume(it.getCoordinate(), it.getSize()); - - if (octree.isNodeOccupied(*it)){ // occupied voxels - if (octree.isNodeAtThreshold(*it)) { - idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray); - color_idx_occupied_thres = setCubeColorHeightmap(voxel, color_idx_occupied_thres, &m_occupiedThresColorArray); - } - else { - idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray); - color_idx_occupied = setCubeColorHeightmap(voxel, color_idx_occupied, &m_occupiedColorArray); - } - } - else if (showAll) { // freespace voxels - if (octree.isNodeAtThreshold(*it)) { - idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray); - } - else { - idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray); - } - } - } - - else { // inner node voxels (for grid structure only) - if (showAll) { - if (uses_origin) - voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize()); - else - voxel = OcTreeVolume(it.getCoordinate(), it.getSize()); - m_grid_voxels.push_back(voxel); - } - } - } // end for all voxels - - m_octree_grid_vis_initialized = false; - - if(m_drawOcTreeGrid) - initOctreeGridVis(); + // Start amortised construction of the voxels. + delete m_regeneration.it; + m_regeneration.it = new OcTreeIterator(&octree); + m_regeneration.phase = OcTreeDrawer::INIT; + buildVoxels(); } void OcTreeDrawer::setOcTreeSelection(const std::list& selectedVoxels){ @@ -247,6 +262,16 @@ namespace octomap { clearCubes(&m_selectionArray, m_selectionSize); } + void OcTreeDrawer::setOccupancyThreshold(double threshold){ + if (threshold != m_occupancyThreshold){ + m_occupancyThreshold = threshold; + clear(); + // Need to rebuild the voxels. + m_regeneration.phase = OcTreeDrawer::INIT; + buildVoxels(); + } + } + void OcTreeDrawer::initGLArrays(const unsigned int& num_cubes, unsigned int& glArraySize, GLfloat*** glArray, GLfloat** glColorArray) { @@ -528,20 +553,38 @@ namespace octomap { } } - void OcTreeDrawer::initOctreeGridVis() { + void OcTreeDrawer::initOctreeGridVis(bool expand) { if (m_octree_grid_vis_initialized) return; - clearOcTreeStructure(); + unsigned int previousSize = octree_grid_vertex_size; + GLfloat* previousArray = octree_grid_vertex_array; + + if (!expand){ + clearOcTreeStructure(); + } // allocate arrays for octree grid visualization octree_grid_vertex_size = m_grid_voxels.size() * 12 * 2 * 3; octree_grid_vertex_array = new GLfloat[octree_grid_vertex_size]; + unsigned voxelOffset = 0; + if (expand && previousSize && previousSize <= octree_grid_vertex_size){ + memcpy(octree_grid_vertex_array, previousArray, sizeof(GLfloat) * previousSize); + delete [] previousArray; + // Determine which voxel we continue building at. + voxelOffset = previousSize / (12 * 2 * 3); + previousArray = NULL; + previousSize = 0u; + } + // generate the cubes, 12 lines each - std::list::iterator it_rec; - unsigned int i = 0; + std::list::iterator it_rec = m_grid_voxels.begin(); + unsigned int i = voxelOffset * 12 * 2 * 3; + while (voxelOffset-- > 0) { + ++it_rec; + } double x,y,z; - for (it_rec=m_grid_voxels.begin(); it_rec != m_grid_voxels.end(); it_rec++) { + for (; it_rec != m_grid_voxels.end(); it_rec++) { x = it_rec->first.x(); y = it_rec->first.y(); @@ -663,11 +706,13 @@ namespace octomap { void OcTreeDrawer::clear() { //clearOcTree(); - clearCubes(&m_occupiedArray, m_occupiedSize, &m_occupiedColorArray); - clearCubes(&m_occupiedThresArray, m_occupiedThresSize, &m_occupiedThresColorArray); - clearCubes(&m_freeArray, m_freeSize); - clearCubes(&m_freeThresArray, m_freeThresSize); + clearCubes(&m_occupiedArray, m_occupiedCap, &m_occupiedColorArray); + clearCubes(&m_occupiedThresArray, m_occupiedThresCap, &m_occupiedThresColorArray); + clearCubes(&m_freeArray, m_freeCap); + clearCubes(&m_freeThresArray, m_freeThresCap); clearCubes(&m_selectionArray, m_selectionSize); + m_occupiedSize = m_occupiedThresSize = m_freeSize = m_freeThresSize = m_selectionSize = 0u; + m_occupiedThresColorSize = m_occupiedColorSize = 0u; clearOcTreeStructure(); } @@ -921,6 +966,167 @@ namespace octomap { glPopMatrix(); } + void OcTreeDrawer::buildVoxels(unsigned int timeout){ + QElapsedTimer timer; + timer.start(); + + switch (m_regeneration.phase) + { + case OcTreeDrawer::NONE: + default: + break; + + case OcTreeDrawer::INIT: + m_regeneration.cnt_occupied = m_regeneration.cnt_occupied_thres = m_regeneration.cnt_free = m_regeneration.cnt_free_thres = 0; + m_regeneration.it->begin(m_max_tree_depth); + m_regeneration.phase = OcTreeDrawer::COUNT; + m_regeneration.progress = 0; + m_regeneration.maxProgress = m_regeneration.it->getTreeSize(); + if (!m_gridVoxelsBuilt){ + clearOcTreeStructure(); + m_grid_voxels.clear(); + } + // Don't break; + case OcTreeDrawer::COUNT: + { + // TODO: resolve a better way to temporarily change the occupancy threshold. + double occupancyThresCache = m_regeneration.it->getOccupancyThres(); + m_regeneration.it->setOccupancyThres(m_occupancyThreshold); + + // walk the tree one to find the number of nodes in each category + // (this is used to set up the OpenGL arrays) + // TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead... + while (m_regeneration.it->isValid() && (timeout == 0 || timer.elapsed() < timeout)){ + ++m_regeneration.progress; + if (m_regeneration.it->isLeaf()) { + if (m_regeneration.it->isOccupied()){ // occupied voxels + if (m_regeneration.it->isAtThreshold()) ++m_regeneration.cnt_occupied_thres; + else ++m_regeneration.cnt_occupied; + } + else if (m_regeneration.showAll) { // freespace voxels + if (m_regeneration.it->isAtThreshold()) ++m_regeneration.cnt_free_thres; + else ++m_regeneration.cnt_free; + } + } + m_regeneration.it->moveNext(); + } + // Restore occupancy threshold. + m_regeneration.it->setOccupancyThres(occupancyThresCache); + + if (!m_regeneration.it->isValid()){ + // Done counting. Setup GL arrays and move to the next phase. + m_regeneration.phase = OcTreeDrawer::BUILD; + m_regeneration.progress = 0; + // setup GL arrays for cube quads and cube colors + initGLArrays(m_regeneration.cnt_occupied , m_occupiedCap , &m_occupiedArray , &m_occupiedColorArray); + initGLArrays(m_regeneration.cnt_occupied_thres, m_occupiedThresCap, &m_occupiedThresArray, &m_occupiedThresColorArray); + initGLArrays(m_regeneration.cnt_free , m_freeCap , &m_freeArray, NULL); + initGLArrays(m_regeneration.cnt_free_thres , m_freeThresCap , &m_freeThresArray, NULL); + + m_occupiedThresSize = m_freeThresSize = m_occupiedSize = m_freeSize = m_selectionSize = 0u; + m_occupiedThresColorSize = m_occupiedColorSize = 0u; + + m_regeneration.it->begin(this->m_max_tree_depth); + } + } + break; + + case OcTreeDrawer::BUILD: + { + // TODO: resolve a better way to temporarily change the occupancy threshold. + double occupancyThresCache = m_regeneration.it->getOccupancyThres(); + m_regeneration.it->setOccupancyThres(m_occupancyThreshold); + + double minX, minY, minZ, maxX, maxY, maxZ; + m_regeneration.it->getMetricMin(minX, minY, minZ); + m_regeneration.it->getMetricMax(maxX, maxY, maxZ); + + // set min/max Z for color height map + m_zMin = minZ; + m_zMax = maxZ; + + std::vector cube_template; + initCubeTemplate(origin, cube_template); + + OcTreeVolume voxel; // current voxel, possibly transformed + while (m_regeneration.it->isValid() && (timeout == 0 || timer.elapsed() < timeout)) { + ++m_regeneration.progress; + if (m_regeneration.it->isLeaf()) { // voxels for leaf nodes + if (m_regeneration.uses_origin) + voxel = OcTreeVolume(origin.rot().rotate(m_regeneration.it->getCoordinate()), m_regeneration.it->getSize()); + else + voxel = OcTreeVolume(m_regeneration.it->getCoordinate(), m_regeneration.it->getSize()); + + if (m_regeneration.it->isOccupied()){ // occupied voxels + if (m_regeneration.it->isAtThreshold()) { + m_occupiedThresSize = generateCube(voxel, cube_template, m_occupiedThresSize, &m_occupiedThresArray); + unsigned char r, g, b; + if (!m_regeneration.it->getColor(r, g, b)){ + m_occupiedThresColorSize = setCubeColorHeightmap(voxel, m_occupiedThresColorSize, &m_occupiedThresColorArray); + } else { + m_occupiedThresColorSize = setCubeColorRGBA(r, g, b, + (unsigned char) (m_regeneration.it->getOccupancy() * 255.), + m_occupiedThresColorSize, &m_occupiedThresColorArray); + } + } + else { + m_occupiedSize = generateCube(voxel, cube_template, m_occupiedSize, &m_occupiedArray); + unsigned char r, g, b; + if (!m_regeneration.it->getColor(r, g, b)){ + m_occupiedColorSize = setCubeColorHeightmap(voxel, m_occupiedColorSize, &m_occupiedColorArray); + } else { + m_occupiedColorSize = setCubeColorRGBA(r, g, b, + (unsigned char)(m_regeneration.it->getOccupancy() * 255.), + m_occupiedColorSize, &m_occupiedColorArray); + } + } + } + else if (m_regeneration.showAll) { // freespace voxels + if (m_regeneration.it->isAtThreshold()) { + m_freeThresSize = generateCube(voxel, cube_template, m_freeThresSize, &m_freeThresArray); + } + else { + m_freeSize = generateCube(voxel, cube_template, m_freeSize, &m_freeArray); + } + } + } + + else { // inner node voxels (for grid structure only) + if (!m_gridVoxelsBuilt){ + if (m_regeneration.showAll) { + if (m_regeneration.uses_origin) + voxel = OcTreeVolume(origin.rot().rotate(m_regeneration.it->getCoordinate()), m_regeneration.it->getSize()); + else + voxel = OcTreeVolume(m_regeneration.it->getCoordinate(), m_regeneration.it->getSize()); + m_grid_voxels.push_back(voxel); + } + } + } + m_regeneration.it->moveNext(); + } // end for all voxels + // Restore occupancy threshold. + m_regeneration.it->setOccupancyThres(occupancyThresCache); + + // Expand octree grid. + m_octree_grid_vis_initialized = false; + + if(m_drawOcTreeGrid) + initOctreeGridVis(true); + + if (!m_regeneration.it->isValid()){ + m_gridVoxelsBuilt = true; + m_regeneration.phase = OcTreeDrawer::COMPLETE; + m_regeneration.progress = m_regeneration.maxProgress = 0; + } + } + break; + + case OcTreeDrawer::COMPLETE: + m_regeneration.phase = OcTreeDrawer::NONE; + break; + } + } + } // namespace diff --git a/octovis/src/ViewerGui.cpp b/octovis/src/ViewerGui.cpp index a748ff4f..bd97a3b0 100644 --- a/octovis/src/ViewerGui.cpp +++ b/octovis/src/ViewerGui.cpp @@ -30,6 +30,7 @@ #include #include +#include #define _MAXRANGE_URG 5.1 #define _MAXRANGE_SICK 50.0 @@ -71,8 +72,12 @@ ViewerGui::ViewerGui(const std::string& filename, QWidget *parent, unsigned int // status bar m_mapSizeStatus = new QLabel("Map size", this); m_mapMemoryStatus = new QLabel("Memory consumption", this); + m_buildingStatus = new QLabel("", this); m_mapSizeStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); m_mapMemoryStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); + m_buildingStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken); + m_buildingStatus->setVisible(false); + statusBar()->addPermanentWidget(m_buildingStatus); statusBar()->addPermanentWidget(m_mapSizeStatus); statusBar()->addPermanentWidget(m_mapMemoryStatus); @@ -83,6 +88,8 @@ ViewerGui::ViewerGui(const std::string& filename, QWidget *parent, unsigned int connect(settingsPanel, SIGNAL(treeDepthChanged(int)), this, SLOT(changeTreeDepth(int))); connect(settingsPanel, SIGNAL(addNextScans(unsigned)), this, SLOT(addNextScans(unsigned))); connect(settingsPanel, SIGNAL(gotoFirstScan()), this, SLOT(gotoFirstScan())); + connect(settingsPanel, SIGNAL(recaculateCutoff(double)), this, SLOT(recalculateOccupancy(double))); + settingsPanel->setOccupancyCutoff(m_occupancyThresh, false); connect(settingsCameraPanel, SIGNAL(jumpToFrame(unsigned)), m_cameraFollowMode, SLOT(jumpToFrame(unsigned))); connect(settingsCameraPanel, SIGNAL(play()), m_cameraFollowMode, SLOT(play())); @@ -122,6 +129,13 @@ ViewerGui::ViewerGui(const std::string& filename, QWidget *parent, unsigned int connect(ui.actionReset_view, SIGNAL(triggered()), m_glwidget, SLOT(resetView())); + QTimer *updateTimer = new QTimer(this); + connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateDrawers())); + updateTimer->start(1000/30); + + // Force registration of the ColorOcTree type. Required for statically linked Windows builds. + ColorOcTree t(1); + if (filename != ""){ m_filename = filename; openFile(); @@ -180,6 +194,11 @@ bool ViewerGui::getOctreeRecord(int id, OcTreeRecord*& otr) { } void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d origin) { + if (OcTree *octree = dynamic_cast(tree)) { + octree->setOccupancyThres(m_occupancyThresh); + } else if (ColorOcTree *octree = dynamic_cast(tree)) { + octree->setOccupancyThres(m_occupancyThresh); + } // is id in use? OcTreeRecord* r; bool foundRecord = getOctreeRecord(id, r); @@ -188,10 +207,12 @@ void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d delete r->octree_drawer; if (dynamic_cast(tree)) { r->octree_drawer = new OcTreeDrawer(); + r->octree_drawer->setOccupancyThreshold(m_occupancyThresh); // fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id); } else if (dynamic_cast(tree)) { r->octree_drawer = new ColorOcTreeDrawer(); + r->octree_drawer->setOccupancyThreshold(m_occupancyThresh); } else{ OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str()); } @@ -212,10 +233,12 @@ void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d otr.id = id; if (dynamic_cast(tree)) { otr.octree_drawer = new OcTreeDrawer(); + otr.octree_drawer->setOccupancyThreshold(m_occupancyThresh); // fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id); } else if (dynamic_cast(tree)) { otr.octree_drawer = new ColorOcTreeDrawer(); + otr.octree_drawer->setOccupancyThreshold(m_occupancyThresh); } else{ OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str()); } @@ -358,6 +381,30 @@ void ViewerGui::gotoFirstScan(){ } } +void ViewerGui::recalculateOccupancy(double cutoff){ + m_occupancyThresh = cutoff; + std::map::iterator iter; + + // Process viable trees, adjust thresholds and build a dirty list. + // We don't update the trees in this loop because it would invalidate the iterator. + for (iter = m_octrees.begin(); iter != m_octrees.end(); ++iter){ + iter->second.octree_drawer->setOccupancyThreshold(m_occupancyThresh); + //if (OcTree *octree = dynamic_cast(iter->second.octree)){ + // octree->setOccupancyThres(m_occupancyThresh); + // delete iter->second.octree_drawer; + // iter->second.octree_drawer = new OcTreeDrawer(); + // m_glwidget->addSceneObject(iter->second.octree_drawer); + //} else if (ColorOcTree *octree = dynamic_cast(iter->second.octree)){ + // octree->setOccupancyThres(m_occupancyThresh); + // delete iter->second.octree_drawer; + // iter->second.octree_drawer = new ColorOcTreeDrawer(); + // m_glwidget->addSceneObject(iter->second.octree_drawer); + //} + } + + //showOcTree(); +} + void ViewerGui::addNextScan(){ if (m_scanGraph){ @@ -621,6 +668,45 @@ void ViewerGui::changeTreeDepth(int depth){ showOcTree(); } +void ViewerGui::updateDrawers(){ + // Update tree drawers for amortised voxel construction. + std::map::iterator iter; + + //m_buildingStatus->setText(QString()); + //m_buildingStatus->setVisible(false); + + OcTreeDrawer::BuildPhase phase = OcTreeDrawer::NONE; + unsigned int progress = 0; + unsigned int maxProgress = 0; + for (iter = m_octrees.begin(); iter != m_octrees.end(); ++iter){ + unsigned int p = 0, mp = 0; + OcTreeDrawer::BuildPhase ph = OcTreeDrawer::NONE; + phase = iter->second.octree_drawer->update(1000 / 30, &progress, &maxProgress); + if (phase != OcTreeDrawer::NONE){ + // Set the status + switch (phase) + { + case OcTreeDrawer::COUNT: + m_buildingStatus->setText(tr("Counting voxels %1/%2").arg(progress).arg(maxProgress)); + m_buildingStatus->setVisible(true); + break; + case OcTreeDrawer::BUILD: + m_buildingStatus->setText(tr("Building voxels %1/%2").arg(progress).arg(maxProgress)); + m_buildingStatus->setVisible(true); + m_glwidget->updateGL(); + break; + default: + case OcTreeDrawer::COMPLETE: + m_buildingStatus->setText(QString()); + m_buildingStatus->setVisible(false); + m_glwidget->updateGL(); + break; + } + + return; + } + } +} void ViewerGui::on_actionExit_triggered(){ this->close(); diff --git a/octovis/src/ViewerSettingsPanel.cpp b/octovis/src/ViewerSettingsPanel.cpp index d99a7b60..9c3a82b8 100644 --- a/octovis/src/ViewerSettingsPanel.cpp +++ b/octovis/src/ViewerSettingsPanel.cpp @@ -25,10 +25,15 @@ #include ViewerSettingsPanel::ViewerSettingsPanel(QWidget *parent) - : QWidget(parent), m_currentScan(0), m_numberScans(0), m_treeDepth(_TREE_MAX_DEPTH), m_resolution(0.1) + : QWidget(parent), m_currentScan(0), m_numberScans(0), m_treeDepth(_TREE_MAX_DEPTH), m_resolution(0.1), + m_occupancyCutoff(0.5), m_notifyCutoff(true) { ui.setupUi(this); connect(ui.treeDepth, SIGNAL(valueChanged(int)), this, SLOT(setTreeDepth(int))); + setOccupancyCutoff(0.5); + connect(ui.occCutoffSlider, SIGNAL(valueChanged(int)), this, SLOT(changeOccupancyCutoff(int))); + connect(ui.occCutoffBox, SIGNAL(valueChanged(double)), this, SLOT(changeOccupancyCutoff(double))); + ui.recalculateOccupancyButton->setEnabled(false); scanProgressChanged(); leafSizeChanged(); @@ -68,6 +73,11 @@ void ViewerSettingsPanel::on_firstScanButton_clicked(){ emit gotoFirstScan(); } +void ViewerSettingsPanel::on_recalculateOccupancyButton_clicked() { + ui.recalculateOccupancyButton->setEnabled(false); + emit recaculateCutoff(m_occupancyCutoff); +} + void ViewerSettingsPanel::scanProgressChanged(){ if (int(m_numberScans) > 1) ui.scanProgressBar->setMaximum(int(m_numberScans)); @@ -111,6 +121,14 @@ void ViewerSettingsPanel::setResolution(double resolution){ leafSizeChanged(); } +void ViewerSettingsPanel::setOccupancyCutoff(double cutoff, bool dirty) { + m_occupancyCutoff = cutoff; + bool recalc = ui.recalculateOccupancyButton->isEnabled() || dirty; + ui.occCutoffBox->setValue(cutoff); + ui.occCutoffSlider->setValue(int(cutoff * 100.0)); + ui.recalculateOccupancyButton->setEnabled(recalc); +} + void ViewerSettingsPanel::setTreeDepth(int depth){ emit treeDepthChanged(depth); m_treeDepth = depth; @@ -119,6 +137,28 @@ void ViewerSettingsPanel::setTreeDepth(int depth){ leafSizeChanged(); } +void ViewerSettingsPanel::changeOccupancyCutoff(int cutoff) { + m_occupancyCutoff = double(cutoff) * 0.01; + if (m_notifyCutoff) { + // Temporarily prevent re-broadcasting change event. + m_notifyCutoff = false; + ui.occCutoffBox->setValue(m_occupancyCutoff); + m_notifyCutoff = true; + } + ui.recalculateOccupancyButton->setEnabled(true); +} + +void ViewerSettingsPanel::changeOccupancyCutoff(double cutoff) { + m_occupancyCutoff = cutoff; + if (m_notifyCutoff) { + // Temporarily prevent re-broadcasting change event. + m_notifyCutoff = false; + ui.occCutoffSlider->setValue(int(cutoff * 100.0)); + m_notifyCutoff = true; + } + ui.recalculateOccupancyButton->setEnabled(true); +} + void ViewerSettingsPanel::leafSizeChanged(){ double leafSize = m_resolution * pow(2.0, (int) (_TREE_MAX_DEPTH-m_treeDepth)); ui.leafSize->setText(QString::number(leafSize)+" m");