New IO functionality in AbstractOcTree as general interface

octomap/include/octomap/AbstractOcTree.h

@@ -1,17 +1,71 @@
 #ifndef OCTOMAP_ABSTRACT_OCTREE
 #define OCTOMAP_ABSTRACT_OCTREE
 
+// $Id:  $
+
+/**
+* OctoMap:
+* A probabilistic, flexible, and compact 3D mapping library for robotic systems.
+* @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2011.
+* @see http://octomap.sourceforge.net/
+* License: New BSD License
+*/
+
+/*
+ * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the University of Freiburg nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <cstddef>
+#include <fstream>
+#include <string>
+#include <iostream>
+#include <map>
+
 namespace octomap {
-
   /*!
    * This abstract class is an interface to all octrees
    */
   class AbstractOcTree {
-
+    friend class StaticMapInit;
   public:
-    AbstractOcTree() {};
+    AbstractOcTree();
     virtual ~AbstractOcTree() {};
 
+    /// virtual constructor: creates a new object of same type
+    virtual AbstractOcTree* create() const = 0;
+
+    /// returns actual class name as string for identification
+    virtual std::string getTreeType() const = 0;
+
+
+
+    virtual double getResolution() const = 0;
+    virtual void setResolution(double res) = 0;
     virtual size_t size() const = 0;
     virtual size_t memoryUsage() const = 0;
     virtual void getMetricMin(double& x, double& y, double& z) = 0;
@@ -21,12 +75,47 @@ namespace octomap {
     virtual void prune() = 0;
     virtual void expand() = 0;
 
+    /// Write file header and complete tree to file (serialization)
+    bool write(const std::string& filename) const;
+    /// Write file header and complete tree to stream (serialization)
+    bool write(std::ostream& s) const;
+
+    /**
+     * Creates a certain OcTree (factory pattern)
+     *
+     * @param id unique ID of OcTree
+     * @param res resolution of OcTree
+     * @return pointer to newly created OcTree (empty). NULL if the ID is unknown!
+     */
+    static AbstractOcTree* createTree(const std::string id, double res);
+
+    /// Read the file header, create the appropriate class and deserialize.
+    /// This creates a new octree which you need to delete yourself.
+    static AbstractOcTree* read(const std::string& filename);
+    /// Read the file header, create the appropriate class and deserialize.
+    /// This creates a new octree which you need to delete yourself.
+    static AbstractOcTree* read(std::istream &s);
+    /// Read complete state of tree from stream
+    virtual std::istream& readData(std::istream &s) = 0;
+    /// Write complete state of tree to stream, prune tree first (lossless compression)
+    virtual std::ostream& writeData(std::ostream &s) = 0;
+    /// Write complete state of tree to stream, no pruning (const version)
+    virtual std::ostream& writeDataConst(std::ostream &s) const = 0;
+  private:
+    /// create private store, Construct on first use
+    static std::map<std::string, AbstractOcTree*>& classIDMapping();
 
   protected:
+    static bool readHeader(std::istream &s, std::string& id, unsigned& size, double& res);
+    static void registerTreeType(AbstractOcTree* tree);
 
+    static const std::string fileHeader;
+    static const std::string binaryFileHeader;
   };
 
 
+
+
 } // end namespace
 
 

octomap/include/octomap/ColorOcTree.h

@@ -114,6 +114,12 @@ namespace octomap {
 
   public:
     ColorOcTree(double _resolution);
+
+    /// virtual constructor: creates a new object of same type
+    /// (Covariant return type requires an up-to-date compiler)
+    ColorOcTree* create() const {return new ColorOcTree(resolution); }
+
+    std::string getTreeType() const {return "ColorOcTree";}
 
     // set node color at given key or coordinate. Replaces previous color.
     ColorOcTreeNode* setNodeColor(const OcTreeKey& key, const unsigned char& r, 
@@ -159,6 +165,21 @@ namespace octomap {
 
   protected:
     void updateInnerOccupancyRecurs(ColorOcTreeNode* node, unsigned int depth);
+
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once
+     */
+    class StaticMemberInitializer{
+       public:
+         StaticMemberInitializer() {
+           ColorOcTree* tree = new ColorOcTree(0.1);
+           AbstractOcTree::registerTreeType(tree);
+         }
+    };
+    /// static member to ensure static initialization (only once)
+    static StaticMemberInitializer colorOcTreeMemberInit;
+
   };
 
 } // end namespace

octomap/include/octomap/CountingOcTree.h

@@ -108,6 +108,20 @@ namespace octomap {
                                   unsigned int max_depth,
                                   CountingOcTreeNode* node, unsigned int depth,
                                   const OcTreeKey& parent_key) const;
+
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once
+     */
+    class StaticMemberInitializer{
+       public:
+         StaticMemberInitializer() {
+           CountingOcTree* tree = new CountingOcTree(0.1);
+           AbstractOcTree::registerTreeType(tree);
+         }
+    };
+    /// static member to ensure static initialization (only once)
+    static StaticMemberInitializer countingOcTreeMemberInit;
   };
 
 

octomap/include/octomap/OcTree.h

@@ -53,9 +53,6 @@ namespace octomap {
    */
   class OcTree : public OccupancyOcTreeBase <OcTreeNode> {
 
-  public:
-    static const int TREETYPE=3;
-
   public:
 
     /**
@@ -72,6 +69,28 @@ namespace octomap {
     OcTree(std::string _filename);
 
     virtual ~OcTree(){};
+
+    /// virtual constructor: creates a new object of same type
+    /// (Covariant return type requires an up-to-date compiler)
+    OcTree* create() const {return new OcTree(resolution); }
+
+    std::string getTreeType() const {return "OcTree";}
+
+
+  protected:
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once
+     */
+    class StaticMemberInitializer{
+    public:
+      StaticMemberInitializer() {
+        OcTree* tree = new OcTree(0.1);
+        AbstractOcTree::registerTreeType(tree);
+      }
+    };
+    /// to ensure static initialization (only once)
+    static StaticMemberInitializer ocTreeMemberInit;
   };
 
 } // end namespace

octomap/include/octomap/OcTreeBase.h

@@ -79,6 +79,11 @@ namespace octomap {
     OcTreeBase(double _resolution);
     virtual ~OcTreeBase();
 
+    /// virtual constructor: creates a new object of same type
+    /// (Covariant return type requires an up-to-date compiler)
+    OcTreeBase<NODE>* create() const {return new OcTreeBase<NODE>(resolution); }
+
+    std::string getTreeType() const {return "OcTreeBase";}
 
     void setResolution(double r);
     inline double getResolution() const { return resolution; }
@@ -221,15 +226,15 @@ namespace octomap {
     // file IO
 
     /// Read complete state of tree from stream
-    std::istream& read(std::istream &s);
+    std::istream& readData(std::istream &s);
 
     /// Write complete state of tree to stream, prune tree first (lossless compression)
-    std::ostream& write(std::ostream &s);
+    std::ostream& writeData(std::ostream &s);
 
     /// Write complete state of tree to stream, no pruning (const version)
-    std::ostream& writeConst(std::ostream &s) const;
+    std::ostream& writeDataConst(std::ostream &s) const;
+
 
-    // -- experimental section  -----------------------
     /**
      * Base class for OcTree iterators. So far, all iterator's are
      * const with respect to the tree

octomap/include/octomap/OcTreeBase.hxx

@@ -528,19 +528,19 @@ namespace octomap {
 
 
   template <class NODE>
-  std::ostream& OcTreeBase<NODE>::write(std::ostream &s){
+  std::ostream& OcTreeBase<NODE>::writeData(std::ostream &s){
     this->prune();
-    return this->writeConst(s);
+    return this->writeDataConst(s);
   }
 
   template <class NODE>
-  std::ostream& OcTreeBase<NODE>::writeConst(std::ostream &s) const{
+  std::ostream& OcTreeBase<NODE>::writeDataConst(std::ostream &s) const{
     itsRoot->writeValue(s);
     return s;
   }
 
   template <class NODE>
-  std::istream& OcTreeBase<NODE>::read(std::istream &s) {
+  std::istream& OcTreeBase<NODE>::readData(std::istream &s) {
 
     if (!s.good()){
       OCTOMAP_WARNING_STR(__FILE__ << ":" << __LINE__ << "Warning: Input filestream not \"good\"");

octomap/include/octomap/OcTreeFileIO.h

@@ -41,68 +41,42 @@
  */
 
 
-#include <octomap/OcTree.h>
-#include <octomap/OcTreeBase.h>
-#include <octomap/OcTreeDataNode.h>
-#include <fstream>
-#include <string>
-#include <iostream>
-#include <typeinfo>
+#include <octomap/AbstractOcTree.h>
+#include <octomap/octomap_types.h>
+
+
 
 namespace octomap {
 
   /**
-   * Static class for reading and writing OcTrees to files.
-   * Implements a simple Factory design pattern.
+   * Class for reading and writing OcTrees to files.
+   * @note This class is deprecated and will be removed in the future.
+   * Use read and write directly in AbstractOcTree instead.
+   *
    *
    */
 
   class OcTreeFileIO {
   public:
-    template <class NODE>
-    static bool write(const OcTreeBase<NODE>* tree, const std::string& filename);
-
-
-    template <class NODE>
-    static std::ostream& write(const OcTreeBase<NODE>* tree, std::ostream& s);
-
-
-    // TODO: non-const version: prune tree first before writing?
-//    template <class NODE>
-//    static bool write(OcTreeBase<NODE>* tree, const std::string& filename);
-//
-//    template <class NODE>
-//    static std::ostream& write(OcTreeBase<NODE>* tree, std::ostream& s);
+    OcTreeFileIO() {};
+    ~OcTreeFileIO() {};
 
-    template <class NODE>
-    static OcTreeBase<NODE>* read(const std::string& filename);
+    /// Deprecated, use tree->write() instead
+    DEPRECATED( bool write(const AbstractOcTree* tree, const std::string& filename) );
+    /// Deprecated, use tree->write() instead
+    DEPRECATED( std::ostream& write(const AbstractOcTree* tree, std::ostream& s) );
+    /// Deprecated, AbstractOcTree::read() instead
+    DEPRECATED( AbstractOcTree* read(const std::string& filename) );
+    /// Deprecated, AbstractOcTree::read() instead
+    DEPRECATED( std::istream& read(std::istream& s, AbstractOcTree*& tree) );
 
-    template <class NODE>
-    static std::istream& read(std::istream& s, OcTreeBase<NODE>*& tree);
 
-    /**
-     * Map OcTree classes to IDs
-     *
-     * @param tree
-     * @return unique (unsigned) ID of OcTree class
-     */
-    template <class NODE>
-    static unsigned getTreeID(const OcTreeBase<NODE>* tree);
 
-    /**
-     * Creates a certain OcTree (factory pattern)
-     *
-     * @param id unique ID of OcTree
-     * @param res resolution of OcTree
-     * @return pointer to newly created OcTree (empty). NULL if the ID is unknown!
-     */
-    template <class NODE>
-    static OcTreeBase<NODE>* createTree(unsigned id, double res);
+  protected:
 
 
   };
 }
 
-#include "octomap/OcTreeFileIO.hxx"
 
 #endif

octomap/include/octomap/OcTreeKey.h

@@ -86,7 +86,7 @@ namespace octomap {
     struct KeyHash{
       size_t operator()(const OcTreeKey& key) const{
         // a hashing function 
-        return key.k[0] + 1327*key.k[1] + 345637*key.k[2];
+        return key.k[0] + 1337*key.k[1] + 345637*key.k[2];
       }
     };