Octovis improvements

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;
+    };
   };
 
 

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,31 +141,132 @@ 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!
     GLfloat* octree_grid_vertex_array;
     unsigned int octree_grid_vertex_size;
 
     std::list<octomap::OcTreeVolume> 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;
   };
 }
 

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;

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