Merge branch 'devel'

octomap/CHANGELOG.txt

@@ -1,3 +1,10 @@
+v1.6.1: 2013-06-19
+==================
+- New function swapContent to exchange the contents of two octrees (issue #32)
+- New normal estimation based on marching cubes (thanks to F-M. de Rainville)
+- Fixed octovis edit box affecting small selection ranges
+- Fixed compilation with gcc 4.8 and OSX (issues #23 #28)
+
 v1.6.0: 2013-04-05
 ==================
 - Speedup: Pruning is now done only on affected nodes on each call to
@@ -16,6 +23,14 @@ v1.6.0: 2013-04-05
   to C. Dornhege)
 - Sample data is now in octomap/share
 
+v1.5.6: 2013-06-10
+==================
+- Fixed compilation with OSX 10.8 (issue #23)
+
+v1.5.5: 2013-05-05
+==================
+- Fixed compilation of external code against OctoMap with gcc4.8 (issue #28)
+
 v1.5.4: 2013-02-27
 ==================
 - Removed binvox binaries from sources for improved packaging compatibility

octomap/CMakeLists.txt

@@ -6,7 +6,7 @@ ENABLE_TESTING()
 # version (e.g. for packaging)
 set(OCTOMAP_MAJOR_VERSION 1)
 set(OCTOMAP_MINOR_VERSION 6)
-set(OCTOMAP_PATCH_VERSION 0)
+set(OCTOMAP_PATCH_VERSION 1)
 set(OCTOMAP_VERSION ${OCTOMAP_MAJOR_VERSION}.${OCTOMAP_MINOR_VERSION}.${OCTOMAP_PATCH_VERSION})
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0003 NEW)

octomap/README.md

@@ -76,7 +76,7 @@ GETTING STARTED
 ---------------
 
 Jump right in and have a look at the example
-src/octomap/simple.cpp
+src/octomap/simple_example.cpp
 
 Or start the 3D viewer with `bin/octovis`
 

octomap/doxygen.h

@@ -80,11 +80,11 @@ You can find an overview at http://octomap.github.com/ and the code repository a
  
 
 <p>
-  Jump right in and have a look at the main class \ref octomap::OcTree OcTree and the examples in src/octomap/simple.cpp.
+  Jump right in and have a look at the main class \ref octomap::OcTree OcTree and the examples in src/octomap/simple_example.cpp.
   To integrate single measurements into the 3D map have a look at
   \ref octomap::OcTree::insertRay "OcTree::insertRay(...)", to insert full 3D scans (pointclouds) please have a look at
   \ref octomap::OcTree::insertPointCloud "OcTree::insertPointCloud(...)". Queries can be performed e.g. with \ref octomap::OcTree::search "OcTree::search(...)" or
-  \ref octomap::OcTree::castRay(...) "OcTree::castRay(...)". The preferred way to batch-access or process nodes in an Octree is with the iterators
+  \ref octomap::OcTree::castRay "OcTree::castRay(...)". The preferred way to batch-access or process nodes in an Octree is with the iterators
   \ref leaf_iterator "leaf_iterator",  \ref tree_iterator "tree_iterator", or \ref leaf_bbx_iterator "leaf_bbx_iterator".</p>
 
   \image html uml_overview.png

octomap/include/octomap/OcTreeBaseImpl.h

@@ -82,8 +82,20 @@ namespace octomap {
     OcTreeBaseImpl(double resolution);
     virtual ~OcTreeBaseImpl();
 
+    /// Deep copy constructor
     OcTreeBaseImpl(const OcTreeBaseImpl<NODE,INTERFACE>& rhs);
 
+
+    /**
+     * Swap contents of two octrees, i.e., only the underlying
+     * pointer / tree structure. You have to ensure yourself that the
+     * metadata (resolution etc) matches. No memory is cleared
+     * in this function
+     */
+    void swapContent(OcTreeBaseImpl<NODE,INTERFACE>& rhs);
+
+    /// Comparison between two octrees, all meta data, all
+    /// nodes, and the structure must be identical
     bool operator== (const OcTreeBaseImpl<NODE,INTERFACE>& rhs) const;
 
     std::string getTreeType() const {return "OcTreeBaseImpl";}
@@ -105,19 +117,22 @@ namespace octomap {
     inline NODE* getRoot() const { return root; }
 
     /** 
-     *  Search node at specified depth given a 3d point (depth=0: search full tree depth)
+     *  Search node at specified depth given a 3d point (depth=0: search full tree depth).
+     *  You need to check if the returned node is NULL, since it can be in unknown space.
      *  @return pointer to node if found, NULL otherwise
      */
     NODE* search(double x, double y, double z, unsigned int depth = 0) const;
 
     /**
      *  Search node at specified depth given a 3d point (depth=0: search full tree depth)
+     *  You need to check if the returned node is NULL, since it can be in unknown space.
      *  @return pointer to node if found, NULL otherwise
      */
     NODE* search(const point3d& value, unsigned int depth = 0) const;
 
     /**
      *  Search a node at specified depth given an addressing key (depth=0: search full tree depth)
+     *  You need to check if the returned node is NULL, since it can be in unknown space.
      *  @return pointer to node if found, NULL otherwise
      */
     NODE* search(const OcTreeKey& key, unsigned int depth = 0) const;
@@ -146,11 +161,12 @@ namespace octomap {
     /// Deletes the complete tree structure
     void clear();
 
-    OcTreeBaseImpl deepCopy() const;
-
-
-    /// Lossless compression of OcTree: merge children to parent when there are
-    /// eight children with identical values
+    /**
+     * Lossless compression of the octree: A node will replace all of its eight
+     * children if they have identical values. You usually don't have to call
+     * prune() after a regular occupancy update, updateNode() incrementally
+     * prunes all affected nodes.
+     */
     virtual void prune();
 
     /// Expands all pruned nodes (reverse of prune())
@@ -165,7 +181,7 @@ namespace octomap {
     /// \return Memory usage of the complete octree in bytes (may vary between architectures)
     virtual size_t memoryUsage() const;
 
-    /// \return Memory usage of the a single octree node
+    /// \return Memory usage of a single octree node
     virtual inline size_t memoryUsageNode() const {return sizeof(NODE); };
 
     /// \return Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)

octomap/include/octomap/OcTreeBaseImpl.hxx

@@ -113,6 +113,17 @@ namespace octomap {
 
   }
 
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::swapContent(OcTreeBaseImpl<NODE,I>& other){
+    NODE* this_root = root;
+    root = other.root;
+    other.root = this_root;
+
+    size_t this_size = this->tree_size;
+    this->tree_size = other.tree_size;
+    other.tree_size = this_size;
+  }
+
   template <class NODE,class I>
   bool OcTreeBaseImpl<NODE,I>::operator== (const OcTreeBaseImpl<NODE,I>& other) const{
     if (tree_depth != other.tree_depth || tree_max_val != other.tree_max_val

octomap/include/octomap/OcTreeIterator.hxx

@@ -130,7 +130,7 @@
         return tree->keyToCoord(stack.top().key[2], stack.top().depth);
       }
 
-      /// @return the side if the volume occupied by the current node
+      /// @return the side of the volume occupied by the current node
       double getSize() const {return  tree->getNodeSize(stack.top().depth); }
 
       /// return depth of the current node

octomap/include/octomap/OccupancyOcTreeBase.h

@@ -37,6 +37,7 @@
 
 #include <list>
 #include <stdlib.h>
+#include <vector>
 
 #include "octomap_types.h"
 #include "octomap_utils.h"
@@ -82,6 +83,8 @@ namespace octomap {
     * This avoids holes in the floor from mutual deletion and is more efficient than the plain
     * ray insertion in insertPointCloudRays().
     *
+    * @note replaces insertScan()
+    *
     * @param scan Pointcloud (measurement endpoints), in global reference frame
     * @param sensor_origin measurement origin in global reference frame
     * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
@@ -98,6 +101,8 @@ namespace octomap {
     * This avoids holes in the floor from mutual deletion and is more efficient than the plain
     * ray insertion in insertPointCloudRays().
     *
+    * @note replaces insertScan()
+    *
     * @param scan Pointcloud (measurement endpoints) relative to frame origin
     * @param sensor_origin origin of sensor relative to frame origin
     * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin
@@ -111,6 +116,8 @@ namespace octomap {
     /**
     * Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP.
     *
+    * @note replaces insertScan
+    *
     * @param scan ScanNode contains Pointcloud data and frame/sensor origin
     * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
     * @param lazy_eval whether the tree is left 'dirty' after the update (default: false).
@@ -265,14 +272,25 @@ namespace octomap {
      * @param[in] origin starting coordinate of ray
      * @param[in] direction A vector pointing in the direction of the raycast. Does not need to be normalized.
      * @param[out] end returns the center of the cell that was hit by the ray, if successful
-     * @param[in] ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
+     * @param[in] ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unknown cell is hit.
      * @param[in] maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
      * @return whether or not an occupied cell was hit
      */
     virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
                  bool ignoreUnknownCells=false, double maxRange=-1.0) const;
 
-
+		/**
+		 * Performs a step of the marching cubes surface reconstruction algorithm
+		 * to retreive the normal of the triangles that fall in the cube
+		 * formed by the voxels located at the vertex of a given voxel.
+		 *
+		 * @param[in] voxel for which retreive the normals
+		 * @param[out] triangles normals
+		 * @param[in] unknownStatus consider unknown cells as free (false) or occupied (default, true).
+		 * @return True if the input voxel is known in the occupancy grid, and false if it is unknown.
+		 */
+		bool getNormals(const point3d& point, std::vector<point3d>& normals, bool unknownStatus=true) const;
+
     //-- set BBX limit (limits tree updates to this bounding box)
 
     ///  use or ignore BBX limit (default: ignore)

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -33,6 +33,8 @@
 
 #include <bitset>
 
+#include <octomap/MCTables.h>
+
 namespace octomap {
 
   template <class NODE>
@@ -426,6 +428,79 @@ namespace octomap {
     }
   }
 
+  template <class NODE>
+  bool OccupancyOcTreeBase<NODE>::getNormals(const point3d& point, std::vector<point3d>& normals,
+                                             bool unknownStatus) const {
+    normals.clear();
+
+    OcTreeKey init_key;
+    if ( !OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>::coordToKeyChecked(point, init_key) ) {
+      OCTOMAP_WARNING_STR("Voxel out of bounds");
+      return false;
+    }
+
+    int vertex_values[8];
+
+    OcTreeKey current_key;
+    NODE* current_node;
+
+    int x_index[4] = {-1, 1, 1, -1};
+    int y_index[4] = {1, 1, -1, -1};
+    int z_index[2] = {-1, 1};
+
+    int k = 0;
+    for(int j = 0; j < 2; ++j){
+      for(int i = 0; i < 4; ++i){
+        current_key[0] = init_key[0] + x_index[i];
+        current_key[1] = init_key[1] + y_index[i];
+        current_key[2] = init_key[2] + z_index[j];
+        current_node = this->search(current_key);
+
+        if(current_node){
+          vertex_values[k] = this->isNodeOccupied(current_node);
+
+          // point3d coord = this->keyToCoord(current_key);
+          // OCTOMAP_WARNING_STR("vertex " << k << " at " << coord << "; value " << vertex_values[k]);
+        }else{
+          // Occupancy of unknown cells
+          vertex_values[k] = unknownStatus;
+        }
+        ++k;
+      }
+    }
+
+    int cube_index = 0;
+    if (vertex_values[0]) cube_index |= 1;
+    if (vertex_values[1]) cube_index |= 2;
+    if (vertex_values[2]) cube_index |= 4;
+    if (vertex_values[3]) cube_index |= 8;
+    if (vertex_values[4]) cube_index |= 16;
+    if (vertex_values[5]) cube_index |= 32;
+    if (vertex_values[6]) cube_index |= 64;
+    if (vertex_values[7]) cube_index |= 128;
+
+    // OCTOMAP_WARNING_STR("cubde_index: " << cube_index);
+
+    // All vertices are occupied or free resulting in no normal
+    if (edgeTable[cube_index] == 0)
+      return true;
+
+    // No interpolation is done yet, we use vertexList in <MCTables.h>.
+    for(int i = 0; triTable[cube_index][i] != -1; i += 3){
+      point3d p1 = vertexList[triTable[cube_index][i  ]];
+      point3d p2 = vertexList[triTable[cube_index][i+1]];
+      point3d p3 = vertexList[triTable[cube_index][i+2]];
+      point3d v1 = p2 - p1;
+      point3d v2 = p3 - p1;
+
+      // Right hand side cross product to retrieve the normal in the good
+      // direction (pointing to the free nodes).
+      normals.push_back(v1.cross(v2).normalize());
+    }
+
+    return true;
+  }
+
   template <class NODE>
   bool OccupancyOcTreeBase<NODE>::castRay(const point3d& origin, const point3d& directionP, point3d& end, 
                                           bool ignoreUnknown, double maxRange) const {

octomap/octomap-config.cmake.in

@@ -24,11 +24,15 @@
 set(OCTOMAP_INCLUDE_DIRS @OCTOMAP_INCLUDE_DIRS@)
 set(OCTOMAP_LIBRARY_DIRS @OCTOMAP_LIB_DIR@)
 
-# Our library dependencies (contains definitions for IMPORTED targets)
-# include("@FOOBAR_CMAKE_DIR@/FooBarLibraryDepends.cmake")
+# library ext. hardcoded to dylib on Apple 
+if(APPLE)
+  set(SO_EXT ".dylib")
+else(APPLE)
+  set(SO_EXT ".so")
+endif(APPLE)
 
 # Set library names as absolute paths:
 set(OCTOMAP_LIBRARIES
-  @OCTOMAP_LIB_DIR@/liboctomap.so
-  @OCTOMAP_LIB_DIR@/liboctomath.so
+  @OCTOMAP_LIB_DIR@/liboctomap${SO_EXT}
+  @OCTOMAP_LIB_DIR@/liboctomath${SO_EXT}
 )

octomap/package.xml

@@ -1,15 +1,15 @@
 <package>
   <name>octomap</name>
-  <version>1.6.0</version>
+  <version>1.6.1</version>
   <description>The OctoMap library implements a 3D occupancy grid mapping approach, providing data structures and mapping algorithms in C++. The map implementation is based on an octree. See
-  http://octomap.github.com for details.</description>
+  http://octomap.github.io for details.</description>
 
   <author email="[email protected]">Kai M. Wurm</author>
   <author email="[email protected]">Armin Hornung</author>
   <maintainer email="[email protected]">Armin Hornung</maintainer>
   <license>BSD</license>
 
-  <url type="website">http://octomap.github.com</url>
+  <url type="website">http://octomap.github.io</url>
   <url type="bugtracker">https://github.com/OctoMap/octomap/issues</url>
 
   <run_depend>catkin</run_depend>

octomap/src/CMakeLists.txt

@@ -44,8 +44,11 @@ TARGET_LINK_LIBRARIES(eval_octree_accuracy octomap)
 ADD_EXECUTABLE(compare_octrees compare_octrees.cpp)
 TARGET_LINK_LIBRARIES(compare_octrees octomap)
 
-ADD_EXECUTABLE(simple simple.cpp)
-TARGET_LINK_LIBRARIES(simple octomap)
+ADD_EXECUTABLE(simple_example simple_example.cpp)
+TARGET_LINK_LIBRARIES(simple_example octomap)
+
+ADD_EXECUTABLE(normals_example normals_example.cpp)
+TARGET_LINK_LIBRARIES(normals_example octomap)
 
 
 # installation:

octomap/src/compare_octrees.cpp

@@ -37,6 +37,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include <list>
+#include <cmath>
 
 using std::cout;
 using std::endl;
@@ -126,7 +127,7 @@ int main(int argc, char** argv) {
       else
         kld +=log(p1/p2)*p1 + log((1-p1)/(1-p2))*(1-p1);
 
-      if (isnan(kld)){
+      if (std::isnan(kld)){
         OCTOMAP_ERROR("KLD is nan! KLD(%f,%f)=%f; sum = %f", p1, p2, kld, kld_sum);
         exit(-1);
       }