Merge branch 'devel' for 1.7.0 release

dynamicEDT3D/CMakeLists.txt

@@ -5,8 +5,8 @@ ENABLE_TESTING()
 
 # version (e.g. for packaging)
 set(DYNAMICEDT3D_MAJOR_VERSION 1)
-set(DYNAMICEDT3D_MINOR_VERSION 6)
-set(DYNAMICEDT3D_PATCH_VERSION 8)
+set(DYNAMICEDT3D_MINOR_VERSION 7)
+set(DYNAMICEDT3D_PATCH_VERSION 0)
 set(DYNAMICEDT3D_VERSION ${DYNAMICEDT3D_MAJOR_VERSION}.${DYNAMICEDT3D_MINOR_VERSION}.${DYNAMICEDT3D_PATCH_VERSION})
 set(DYNAMICEDT3D_SOVERSION ${DYNAMICEDT3D_MAJOR_VERSION}.${DYNAMICEDT3D_MINOR_VERSION})
 

dynamicEDT3D/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>dynamic_edt_3d</name>
-  <version>1.6.8</version>
+  <version>1.7.0</version>
   <description> The dynamicEDT3D library implements an inrementally updatable Euclidean distance transform (EDT) in 3D. It comes with a wrapper to use the OctoMap 3D representation and hooks into the change detection of the OctoMap library to propagate changes to the EDT.</description>
 
   <author email="[email protected]">Christoph Sprunk</author>

octomap/CHANGELOG.txt

@@ -1,3 +1,13 @@
+v1.7.0: 2015-11-27
+==================
+- BBX iterators fixed for empty trees (thx to F. Boniardi)
+- graph2tree tool option for nodes in global frame
+- New octree2pointcloud PCL conversion tool (thx to F. Ferri)
+- Improved change detection / diff calculation (thx to C. Brew)
+- getUnknownLeafCenters now allows queries at a specified depth (thx to 
+  A. Ecins)
+- Fixed hashing overflow with clang (thx to L. Riano)
+
 v1.6.7: 2014-08-31
 ==================
 - FSF address in octovis license header for OctoMap package in Fedora

octomap/CMakeLists.txt

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

octomap/include/octomap/OcTreeBaseImpl.h

@@ -213,7 +213,7 @@ namespace octomap {
     // -- access tree nodes  ------------------
 
     /// return centers of leafs that do NOT exist (but could) in a given bounding box
-    void getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax) const;
+    void getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax, unsigned int depth = 0) const;
 
 
     // -- raytracing  -----------------------
@@ -259,9 +259,6 @@ namespace octomap {
     /// Pruning the tree first produces smaller files (lossless compression)
     std::ostream& writeData(std::ostream &s) const;
 
-    class leaf_iterator;
-    class tree_iterator;
-    class leaf_bbx_iterator;
     typedef leaf_iterator iterator;
 
     /// @return beginning of the tree as leaf iterator

octomap/include/octomap/OcTreeBaseImpl.hxx

@@ -327,10 +327,10 @@ namespace octomap {
 
     NODE* curNode (root);
 
-    unsigned int diff = tree_depth - depth;
+    int diff = tree_depth - depth;
 
     // follow nodes down to requested level (for diff = 0 it's the last level)
-    for (unsigned i=(tree_depth-1); i>=diff; --i) {
+    for (int i=(tree_depth-1); i>=diff; --i) {
       unsigned int pos = computeChildIdx(key_at_depth, i);
       if (curNode->childExists(pos)) {
         // cast needed: (nodes need to ensure it's the right pointer)
@@ -856,26 +856,31 @@ namespace octomap {
   }
 
   template <class NODE,class I>
-  void OcTreeBaseImpl<NODE,I>::getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax) const {
+  void OcTreeBaseImpl<NODE,I>::getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax, unsigned int depth) const {
 
+    assert(depth <= tree_depth);
+    if (depth == 0)
+      depth = tree_depth;
+
     float diff[3];
     unsigned int steps[3];
+    float step_size = this->resolution * pow(2, tree_depth-depth);
     for (int i=0;i<3;++i) {
       diff[i] = pmax(i) - pmin(i);
-      steps[i] = floor(diff[i] / this->resolution);
+      steps[i] = floor(diff[i] / step_size);
       //      std::cout << "bbx " << i << " size: " << diff[i] << " " << steps[i] << " steps\n";
     }
 
     point3d p = pmin;
     NODE* res;
     for (unsigned int x=0; x<steps[0]; ++x) {
-      p.x() += this->resolution;
+      p.x() += step_size;
       for (unsigned int y=0; y<steps[1]; ++y) {
-        p.y() += this->resolution;
+        p.y() += step_size;
         for (unsigned int z=0; z<steps[2]; ++z) {
           //          std::cout << "querying p=" << p << std::endl;
-          p.z() += this->resolution;
-          res = this->search(p);
+          p.z() += step_size;
+          res = this->search(p,depth);
           if (res == NULL) {
             node_centers.push_back(p);
           }

octomap/include/octomap/OcTreeIterator.hxx

@@ -375,9 +375,12 @@
       leaf_bbx_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* tree, const OcTreeKey& min, const OcTreeKey& max, unsigned char depth=0)
         : iterator_base(tree, depth), minKey(min), maxKey(max)
       {
+        // tree could be empty (= no stack)
+        if (this->stack.size() > 0){
           // advance from root to next valid leaf in bbx:
           this->stack.push(this->stack.top());
           this->operator ++();
+        }
       }
 
       leaf_bbx_iterator(const leaf_bbx_iterator& other) : iterator_base(other) {

octomap/include/octomap/OcTreeKey.h

@@ -96,8 +96,10 @@ namespace octomap {
     /// Provides a hash function on Keys
     struct KeyHash{
       size_t operator()(const OcTreeKey& key) const{
-        // a hashing function 
-        return key.k[0] + 1337*key.k[1] + 345637*key.k[2];
+        // a simple hashing function 
+	// explicit casts to size_t to operate on the complete range
+	// constanst will be promoted according to C++ standard
+        return size_t(key.k[0]) + 1447*size_t(key.k[1]) + 345637*size_t(key.k[2]);
       }
     };
 

octomap/include/octomap/OccupancyOcTreeBase.h

@@ -375,6 +375,7 @@ namespace octomap {
     //-- change detection on occupancy:
     /// track or ignore changes while inserting scans (default: ignore)
     void enableChangeDetection(bool enable) { use_change_detection = enable; }
+    bool isChangeDetectionEnabled() const { return use_change_detection; }
     /// Reset the set of changed keys. Call this after you obtained all changed nodes.
     void resetChangeDetection() { changed_keys.clear(); }
 
@@ -383,10 +384,13 @@ namespace octomap {
      * you need to enableChangeDetection() first. Here, an OcTreeKey always
      * refers to a node at the lowest tree level (its size is the minimum tree resolution)
      */
-    KeyBoolMap::const_iterator changedKeysBegin() {return changed_keys.begin();}
+    KeyBoolMap::const_iterator changedKeysBegin() const {return changed_keys.begin();}
 
     /// Iterator to traverse all keys of changed nodes.
-    KeyBoolMap::const_iterator changedKeysEnd() {return changed_keys.end();}
+    KeyBoolMap::const_iterator changedKeysEnd() const {return changed_keys.end();}
+
+    /// Number of changes since last reset.
+    size_t numChangesDetected() const { return changed_keys.size(); }
 
 
     /**

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -32,6 +32,7 @@
  */
 
 #include <bitset>
+#include <algorithm>
 
 #include <octomap/MCTables.h>
 
@@ -365,13 +366,13 @@ namespace octomap {
   template <class NODE>
   NODE* OccupancyOcTreeBase<NODE>::updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
                                                     unsigned int depth, const float& log_odds_update, bool lazy_eval) {
-    unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
     bool created_node = false;
 
     assert(node);
 
     // follow down to last level
     if (depth < this->tree_depth) {
+      unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
       if (!node->childExists(pos)) {
         // child does not exist, but maybe it's a pruned node?
         if ((!node->hasChildren()) && !node_just_created ) {
@@ -434,13 +435,13 @@ namespace octomap {
   template <class NODE>
   NODE* OccupancyOcTreeBase<NODE>::setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
                                                     unsigned int depth, const float& log_odds_value, bool lazy_eval) {
-    unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
     bool created_node = false;
 
     assert(node);
 
     // follow down to last level
     if (depth < this->tree_depth) {
+      unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
       if (!node->childExists(pos)) {
         // child does not exist, but maybe it's a pruned node?
         if ((!node->hasChildren()) && !node_just_created ) {

octomap/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>octomap</name>
-  <version>1.6.8</version>
+  <version>1.7.0</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.io for details.</description>
 

octomap/src/CMakeLists.txt

@@ -57,6 +57,9 @@ TARGET_LINK_LIBRARIES(normals_example octomap)
 ADD_EXECUTABLE(intersection_example intersection_example.cpp)
 TARGET_LINK_LIBRARIES(intersection_example octomap)
 
+ADD_EXECUTABLE(octree2pointcloud octree2pointcloud.cpp)
+TARGET_LINK_LIBRARIES(octree2pointcloud octomap)
+
 
 # installation:
 # store all header files to install:

octomap/src/graph2tree.cpp

@@ -57,6 +57,7 @@ void printUsage(char* self){
             "  -m <maxrange> (optional) \n"
             "  -n <max scan no.> (optional) \n"
             "  -log (enable a detailed log file with statistics) \n"
+            "  -g (nodes are already in global coordinates and no transformation is required) \n"
             "  -compressML (enable maximum-likelihood compression (lossy) after every scan)\n"
             "  -simple (simple scan insertion ray by ray instead of optimized) \n"
             "  -discretize (approximate raycasting on discretized coordinates, speeds up insertion) \n"
@@ -112,6 +113,7 @@ int main(int argc, char** argv) {
   bool detailedLog = false;
   bool simpleUpdate = false;
   bool discretize = false;
+  bool dontTransformNodes = false;
   unsigned char compression = 1;
 
   // get default sensor model values:
@@ -137,6 +139,8 @@ int main(int argc, char** argv) {
       res = atof(argv[++arg]);
     else if (! strcmp(argv[arg], "-log"))
       detailedLog = true;
+    else if (! strcmp(argv[arg], "-g"))
+      dontTransformNodes = true;
     else if (! strcmp(argv[arg], "-simple"))
       simpleUpdate = true;
     else if (! strcmp(argv[arg], "-discretize"))
@@ -205,15 +209,17 @@ int main(int argc, char** argv) {
   }
 
   // transform pointclouds first, so we can directly operate on them later
-  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
+  if (!dontTransformNodes) {
+    for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
 
-    pose6d frame_origin = (*scan_it)->pose;
-    point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
+      pose6d frame_origin = (*scan_it)->pose;
+      point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
 
-    (*scan_it)->scan->transform(frame_origin);
-    point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
-    (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion());
+      (*scan_it)->scan->transform(frame_origin);
+      point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
+      (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion());
 
+    }
   }
 
 

octomap/src/math/Quaternion.cpp

@@ -37,6 +37,7 @@
 
 #include <cassert>
 #include <math.h>
+#include <algorithm>
 
 
 // used from Vector: norm2, unit, *

octomap/src/octree2pointcloud.cpp

@@ -0,0 +1,114 @@
+/*
+ * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
+ * http://octomap.github.com/
+ *
+ * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
+ * All rights reserved.
+ * License: New BSD
+ *
+ * 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 <string.h>
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+
+#include <octomap/octomap.h>
+#include <octomap/octomap_timing.h>
+
+using namespace std;
+using namespace octomap;
+
+void printUsage(char* self){
+  cerr << "USAGE: " << self << " <InputFile.bt> <OutputFile.pcd>\n";
+  cerr << "This tool creates a point cloud of the occupied cells\n";
+  exit(0);
+}
+
+
+int main(int argc, char** argv) {
+  if (argc != 3)
+    printUsage(argv[0]);
+
+  string inputFilename = argv[1];
+  string outputFilename = argv[2];
+
+  OcTree* tree = new OcTree(0.1);
+  if (!tree->readBinary(inputFilename)){
+    OCTOMAP_ERROR("Could not open file, exiting.\n");
+    exit(1);
+  }
+
+  unsigned int maxDepth = tree->getTreeDepth();
+  cout << "tree depth is " << maxDepth << endl;
+
+  // expand collapsed occupied nodes until all occupied leaves are at maximum depth
+  vector<OcTreeNode*> collapsed_occ_nodes;
+  do {
+    collapsed_occ_nodes.clear();
+    for (OcTree::iterator it = tree->begin(); it != tree->end(); ++it)
+    {
+      if(tree->isNodeOccupied(*it) && it.getDepth() < maxDepth)
+      {
+        collapsed_occ_nodes.push_back(&(*it));
+      }
+    }
+    for (vector<OcTreeNode*>::iterator it = collapsed_occ_nodes.begin(); it != collapsed_occ_nodes.end(); ++it)
+    {
+      (*it)->expandNode();
+    }
+    cout << "expanded " << collapsed_occ_nodes.size() << " nodes" << endl;
+  } while(collapsed_occ_nodes.size() > 0);
+
+  vector<point3d> pcl;
+  for (OcTree::iterator it = tree->begin(); it != tree->end(); ++it)
+  {
+    if(tree->isNodeOccupied(*it))
+    {
+      pcl.push_back(it.getCoordinate());
+    }
+  }
+
+  delete tree;
+
+  ofstream f(outputFilename.c_str(), ofstream::out);
+  f << "# .PCD v0.7" << endl
+    << "VERSION 0.7" << endl
+    << "FIELDS x y z" << endl
+    << "SIZE 4 4 4" << endl
+    << "TYPE F F F" << endl
+    << "COUNT 1 1 1" << endl
+    << "WIDTH " << pcl.size() << endl
+    << "HEIGHT 1" << endl
+    << "VIEWPOINT 0 0 0 0 0 0 1" << endl
+    << "POINTS " << pcl.size() << endl
+    << "DATA ascii" << endl;
+  for (size_t i = 0; i < pcl.size(); i++)
+      f << pcl[i].x() << " " << pcl[i].y() << " " << pcl[i].z() << endl;
+  f.close();
+
+  return 0;
+}