First (hacky) steps for improving node subclassing (#98):

createChild(), getChild(), expandNode(), pruneNode moved into OcTreeBaseImpl.
OcTreeDataNode is now a friend of OcTreeBaseImpl, refactoring by removing expand and prune OcTreeDataNode.

octomap/include/octomap/OcTreeBaseImpl.h

@@ -108,6 +108,36 @@ namespace octomap {
     inline unsigned int getTreeDepth () const { return tree_depth; }
 
     inline double getNodeSize(unsigned depth) const {assert(depth <= tree_depth); return sizeLookupTable[depth];}
+
+
+    // -- Tree structure operations formerly contained in the nodes ---
+
+    // TODO: createChild(), getChild(), getChild() const, expandNode()  probelmatic => casts!
+
+    bool createNodeChild(NODE* node, unsigned int childIdx);
+
+    NODE* getNodeChild(NODE* node, unsigned int childIdx) const;
+
+    const NODE* getNodeChild(const NODE* node, unsigned int childIdx) const;
+
+    /**
+     * Expands a node (reverse of pruning): All children are created and
+     * their occupancy probability is set to the node's value.
+     *
+     * You need to verify that this is indeed a pruned node (i.e. not a
+     * leaf at the lowest level)
+     *
+     */
+    void expandNode(NODE* node);
+
+    /**
+     * Prunes a node when it is collapsible
+     * @return true if pruning was successful
+     */
+    bool pruneNode(NODE* node);
+
+
+    // --------
 
     /**
      * \return Pointer to the root node of the tree. This pointer
@@ -464,7 +494,8 @@ namespace octomap {
     /// (const-parameters can't be changed) -  use the copy constructor instead.
     OcTreeBaseImpl<NODE,INTERFACE>& operator=(const OcTreeBaseImpl<NODE,INTERFACE>&);
 
-  protected:
+  protected:  
+    void allocNodeChildren(NODE* node);
 
     NODE* root; ///< Pointer to the root NODE, NULL for empty tree
 

octomap/include/octomap/OcTreeBaseImpl.hxx

@@ -171,6 +171,67 @@ namespace octomap {
 
     size_changed = true;
   }
+
+  template <class NODE,class I>
+  bool OcTreeBaseImpl<NODE,I>::createNodeChild(NODE* node, unsigned int childIdx){
+    assert(childIdx < 8);
+    if (node->children == NULL) {
+      allocNodeChildren(node);
+    }
+    assert (node.children[childIdx] == NULL);
+    node->children[childIdx] = new NODE();
+    return true;
+  }
+
+  template <class NODE,class I>  
+  NODE* OcTreeBaseImpl<NODE,I>::getNodeChild(NODE* node, unsigned int childIdx) const{
+    assert((childIdx < 8) && (node->children != NULL));
+    assert(node->children[childIdx] != NULL);
+    return static_cast<NODE*>(node->children[childIdx]);
+  }
+
+  template <class NODE,class I>
+  const NODE* OcTreeBaseImpl<NODE,I>::getNodeChild(const NODE* node, unsigned int childIdx) const{
+    assert((childIdx < 8) && (node->children != NULL));
+    assert(node->children[childIdx] != NULL);
+    return static_cast<const NODE*>(node->children[childIdx]);
+  }
+
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::expandNode(NODE* node){
+    assert(!node->hasChildren());
+
+    for (unsigned int k=0; k<8; k++) {
+      createNodeChild(node, k);
+      static_cast<NODE*>(node->children[k])->setValue(node->getValue()); // TODO: copy ctor for data nodes instead?
+    }
+  }
+
+  template <class NODE,class I>
+  bool OcTreeBaseImpl<NODE,I>::pruneNode(NODE* node){
+
+    if (!node->collapsible())
+      return false;
+
+    // set value to children's values (all assumed equal)
+    node->setValue(node->getChild(0)->getValue());
+
+    // delete children
+    for (unsigned int i=0;i<8;i++) {
+      delete static_cast<NODE*>(node->children[i]);
+    }
+    delete[] node->children;
+    node->children = NULL;
+
+    return true;
+  }
+
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::allocNodeChildren(NODE* node){
+    node->allocChildren(); // TODO move here?
+  }
+
+
 
   template <class NODE,class I>
   inline unsigned short int OcTreeBaseImpl<NODE,I>::coordToKey(double coordinate, unsigned depth) const{
@@ -552,7 +613,7 @@ namespace octomap {
       if ((!node->hasChildren()) && (node != this->root)) {
         // current node does not have children AND it's not the root node
         // -> expand pruned node
-        node->expandNode();
+        expandNode(node);
         this->tree_size+=8;
         this->size_changed = true;
       } else { // no branch here, node does not exist
@@ -594,7 +655,7 @@ namespace octomap {
 
     else {
       // max level reached
-      if (node->pruneNode()) {
+      if (pruneNode(node)) {
         num_pruned++;
         tree_size -= 8;
         size_changed = true;
@@ -613,7 +674,7 @@ namespace octomap {
 
     // current node has no children => can be expanded
     if (!node->hasChildren()){
-      node->expandNode();
+      expandNode(node);
       tree_size +=8;
       size_changed = true;
     }

octomap/include/octomap/OcTreeDataNode.h

@@ -45,6 +45,9 @@ namespace octomap {
 
 
   };
+
+  // forward declaration for friend in OcTreeDataNode
+  template<typename NODE,typename I> class OcTreeBaseImpl;
 
   /**
    * Basic node in the OcTree that can hold arbitrary data of type T in value.
@@ -59,6 +62,8 @@ namespace octomap {
    * See ColorOcTreeNode in ColorOcTree.h for an example. 
    */
   template<typename T> class OcTreeDataNode: public AbstractOcTreeNode {
+    template<typename NODE, typename I>
+    friend class OcTreeBaseImpl;
 
   public:
 
@@ -100,25 +105,6 @@ namespace octomap {
     /// Deletes the i-th child of the node
     void deleteChild(unsigned int i);
 
-    // -- pruning of children  -----------------------
-
-
-    /**
-     * Prunes a node when it is collapsible
-     * @return true if pruning was successful
-     */
-    bool pruneNode();
-
-    /**
-     * Expands a node (reverse of pruning): All children are created and
-     * their occupancy probability is set to the node's value.
-     *
-     * You need to verify that this is indeed a pruned node (i.e. not a
-     * leaf at the lowest level)
-     *
-     */
-    void expandNode();
-
     /// @return value stored in the node
     T getValue() const{return value;};
     /// sets value to be stored in the node

octomap/include/octomap/OcTreeDataNode.hxx

@@ -158,35 +158,6 @@ namespace octomap {
     return true;
   }
 
-  template <typename T>
-  bool OcTreeDataNode<T>::pruneNode() {
-
-    if (!this->collapsible())
-      return false;
-
-    // set value to children's values (all assumed equal)
-    setValue(getChild(0)->getValue());
-
-    // delete children
-    for (unsigned int i=0;i<8;i++) {
-      delete static_cast<OcTreeDataNode<T>*>(children[i]);
-    }
-    delete[] children;
-    children = NULL;
-
-    return true;
-  }
-
-  template <typename T>
-  void OcTreeDataNode<T>::expandNode() {
-    assert(!hasChildren());
-
-    for (unsigned int k=0; k<8; k++) {
-      createChild(k);
-      static_cast<OcTreeDataNode<T>*>(children[k])->setValue(value);
-    }
-  }
-
   // ============================================================
   // =  File IO           =======================================
   // ============================================================

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -378,7 +378,7 @@ namespace octomap {
         if ((!node->hasChildren()) && !node_just_created ) {
           // current node does not have children AND it is not a new node 
           // -> expand pruned node
-          node->expandNode();
+          this->expandNode(node);
           this->tree_size+=8;
           this->size_changed = true;
         }
@@ -397,7 +397,7 @@ namespace octomap {
         NODE* retval = updateNodeRecurs(node->getChild(pos), created_node, key, depth+1, log_odds_update, lazy_eval);
         // prune node if possible, otherwise set own probability
         // note: combining both did not lead to a speedup!
-        if (node->pruneNode()){
+        if (this->pruneNode(node)){
           this->tree_size -= 8;
           // return pointer to current parent (pruned), the just updated node no longer exists
           retval = node;
@@ -447,7 +447,7 @@ namespace octomap {
         if ((!node->hasChildren()) && !node_just_created ) {
           // current node does not have children AND it is not a new node
           // -> expand pruned node
-          node->expandNode();
+          this->expandNode(node);
           this->tree_size+=8;
           this->size_changed = true;
         }
@@ -466,7 +466,7 @@ namespace octomap {
         NODE* retval = setNodeValueRecurs(node->getChild(pos), created_node, key, depth+1, log_odds_value, lazy_eval);
         // prune node if possible, otherwise set own probability
         // note: combining both did not lead to a speedup!
-        if (node->pruneNode()){
+        if (this->pruneNode(node)){
           this->tree_size -= 8;
           // return pointer to current parent (pruned), the just updated node no longer exists
           retval = node;

octomap/src/octree2pointcloud.cpp

@@ -78,7 +78,7 @@ int main(int argc, char** argv) {
     }
     for (vector<OcTreeNode*>::iterator it = collapsed_occ_nodes.begin(); it != collapsed_occ_nodes.end(); ++it)
     {
-      (*it)->expandNode();
+      tree->expandNode(*it);
     }
     cout << "expanded " << collapsed_occ_nodes.size() << " nodes" << endl;
   } while(collapsed_occ_nodes.size() > 0);