Merge branch 'devel'

README.md

@@ -5,13 +5,7 @@ OctoMap - A probabilistic, flexible, and compact 3D mapping library for robotic
 Authors: Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2013.
 http://octomap.github.com
 
-Further Contributors:
-* C. Sprunk, University of Freiburg
-* J. Mueller, University of Freiburg
-* S. Osswald, University of Freiburg
-* R. Schmitt, University of Freiburg
-* R. Bogdan Rusu, Willow Garage Inc.
-* C. Dornhege, University of Freiburg
+See the [list of contributors](octomap/AUTHORS.txt) for further authors.
 
 License: 
   * octomap: [New BSD License](octomap/LICENSE.txt)

dynamicEDT3D/dynamicEDT3DConfig.cmake.in

@@ -6,12 +6,10 @@
 #  OCTOMAP_LIBRARIES    - libraries to link against
 
 # Tell the user project where to find our headers and libraries
-set(DYNAMICEDT3D_INCLUDE_DIRS @DYNAMICEDT3D_INCLUDE_DIRS@)
-set(DYNAMICEDT3D_LIBRARY_DIRS @DYNAMICEDT3D_LIB_DIR@)
+set(DYNAMICEDT3D_INCLUDE_DIRS "@DYNAMICEDT3D_INCLUDE_DIRS@")
+set(DYNAMICEDT3D_LIBRARY_DIRS "@DYNAMICEDT3D_LIB_DIR@")
 
 # Our library dependencies (contains definitions for IMPORTED targets)
 # include("@FOOBAR_CMAKE_DIR@/FooBarLibraryDepends.cmake")
 
 set(DYNAMICEDT3D_LIBRARIES dynamicedt3d)
-
-#set(FOOBAR_EXECUTABLE bar)

octomap/AUTHORS.txt

@@ -0,0 +1,12 @@
+OctoMap was originally developed by Kai M. Wurm and Armin Hornung,
+University of Freiburg
+
+Further contributors:
+* C. Sprunk, University of Freiburg
+* J. Mueller, University of Freiburg
+* S. Osswald, University of Freiburg
+* R. Schmitt, University of Freiburg
+* R. Bogdan Rusu, Willow Garage Inc.
+* C. Dornhege, University of Freiburg
+* F-M. de Rainville, Universite Laval Quebec
+* B. Jensen, TU Munich

octomap/CHANGELOG.txt

@@ -1,3 +1,15 @@
+v1.6.2: 2013-11-26
+==================
+- Improved OSX 10.9 compatibility (thx to B. Jensen)
+- Improved  VC++ compatibility (thx to tmdiv)
+- New fct. getRayIntersection to get real intersection of raycasting on
+  voxel (thanks to F-M. de Rainville)
+- Fix #54: Quote paths in CMake configs to avoid problems with spaces
+- Fix #53: Auto-pruning after updateNode
+- Fix #44: CMAKE_SHARED_LIBRARY_SUFFIX in CMakeConfig (Win / Mac compatibility)
+- octovis now includes QGLViewer 2.4 (Qt5 support)
+- octovis now looks for the octomap version of the same number
+
 v1.6.1: 2013-06-19
 ==================
 - New function swapContent to exchange the contents of two octrees (issue #32)

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 1)
+set(OCTOMAP_PATCH_VERSION 2)
 set(OCTOMAP_VERSION ${OCTOMAP_MAJOR_VERSION}.${OCTOMAP_MINOR_VERSION}.${OCTOMAP_PATCH_VERSION})
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0003 NEW)

octomap/README.md

@@ -4,13 +4,7 @@ Octomap - A probabilistic, flexible, and compact 3D mapping library for robotic
 Authors: Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2013.
 http://octomap.github.com
 
-Further Contributors:
-* C. Sprunk, University of Freiburg
-* J. Mueller, University of Freiburg
-* S. Osswald, University of Freiburg
-* R. Schmitt, University of Freiburg
-* R. Bogdan Rusu, Willow Garage Inc.
-* C. Dornhege, University of Freiburg
+See the [list of contributors](AUTHORS.txt) for further authors.
 
 License for octomap: [New BSD License](LICENSE.txt)
 

octomap/include/octomap/AbstractOccupancyOcTree.h

@@ -186,9 +186,9 @@ namespace octomap {
 
     /// sets the threshold for occupancy (sensor model)
     void setOccupancyThres(double prob){occ_prob_thres_log = logodds(prob); }
-    /// sets the probablility for a "hit" (will be converted to logodds) - sensor model
+    /// sets the probability for a "hit" (will be converted to logodds) - sensor model
     void setProbHit(double prob){prob_hit_log = logodds(prob); assert(prob_hit_log >= 0.0);}
-    /// sets the probablility for a "miss" (will be converted to logodds) - sensor model
+    /// sets the probability for a "miss" (will be converted to logodds) - sensor model
     void setProbMiss(double prob){prob_miss_log = logodds(prob); assert(prob_miss_log <= 0.0);}
     /// sets the minimum threshold for occupancy clamping (sensor model)
     void setClampingThresMin(double thresProb){clamping_thres_min = logodds(thresProb); }
@@ -200,13 +200,13 @@ namespace octomap {
     /// @return threshold (logodds) for occupancy - sensor model
     float getOccupancyThresLog() const {return occ_prob_thres_log; }
 
-    /// @return probablility for a "hit" in the sensor model (probability)
+    /// @return probability for a "hit" in the sensor model (probability)
     double getProbHit() const {return probability(prob_hit_log); }
-    /// @return probablility for a "hit" in the sensor model (logodds)
+    /// @return probability for a "hit" in the sensor model (logodds)
     float getProbHitLog() const {return prob_hit_log; }
-    /// @return probablility for a "miss"  in the sensor model (probability)
+    /// @return probability for a "miss"  in the sensor model (probability)
     double getProbMiss() const {return probability(prob_miss_log); }
-    /// @return probablility for a "miss"  in the sensor model (logodds)
+    /// @return probability for a "miss"  in the sensor model (logodds)
     float getProbMissLog() const {return prob_miss_log; }
 
     /// @return minimum threshold for occupancy clamping in the sensor model (probability)

octomap/include/octomap/OcTreeKey.h

@@ -34,15 +34,29 @@
 #ifndef OCTOMAP_OCTREE_KEY_H
 #define OCTOMAP_OCTREE_KEY_H
 
+/* According to c++ standard including this header has no practical effect
+ * but it can be used to determine the c++ standard library implementation.
+ */
+#include <ciso646>
 
 #include <assert.h>
-#ifdef __GNUC__
+
+/* Libc++ does not implement the TR1 namespace, all c++11 related functionality
+ * is instead implemented in the std namespace.
+ */
+#if defined(__GNUC__) && ! defined(_LIBCPP_VERSION)
   #include <tr1/unordered_set>
-  #include <tr1/unordered_map>
-#else
+  #include <tr1/unordered_map>
+  namespace octomap {
+    namespace unordered_ns = std::tr1;
+  };
+#else
   #include <unordered_set>
-  #include <unordered_map>
-#endif
+  #include <unordered_map>
+  namespace octomap {
+    namespace unordered_ns = std;
+  }
+#endif
 
 namespace octomap {
 
@@ -95,14 +109,14 @@ namespace octomap {
    * @note you need to use boost::unordered_set instead if your compiler does not
    * yet support tr1!
    */
-  typedef std::tr1::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
+  typedef unordered_ns::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
 
   /**
    * Data structrure to efficiently track changed nodes as a combination of
    * OcTreeKeys and a bool flag (to denote newly created nodes)
    *
    */
-  typedef std::tr1::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
+  typedef unordered_ns::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
 
 
   class KeyRay {

octomap/include/octomap/OccupancyOcTreeBase.h

@@ -279,6 +279,20 @@ namespace octomap {
     virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
                  bool ignoreUnknownCells=false, double maxRange=-1.0) const;
 
+    /**
+     * Retrieves the entry point of a ray into a voxel. This is the closest intersection point of the ray
+     * originating from origin and a plane of the axis aligned cube.
+     * 
+     * @param[in] origin Starting point of ray
+     * @param[in] direction A vector pointing in the direction of the raycast. Does not need to be normalized.
+     * @param[in] center The center of the voxel where the ray terminated. This is the output of castRay.
+     * @param[out] intersection The entry point of the ray into the voxel, on the voxel surface.
+     * @param[in] delta A small increment to avoid ambiguity of beeing exactly on a voxel surface. A positive value will get the point out of the hit voxel, while a negative valuewill get it inside.
+     * @return Whether or not an intesection point has been found. Either, the ray never cross the voxel or the ray is exactly parallel to the only surface it intersect.
+     */
+    virtual bool getRayIntersection(const point3d& origin, const point3d& direction, const point3d& center,
+                 point3d& intersection, double delta=0.0) const;
+
 		/**
 		 * Performs a step of the marching cubes surface reconstruction algorithm
 		 * to retreive the normal of the triangles that fall in the cube
@@ -394,7 +408,7 @@ namespace octomap {
     virtual void integrateHit(NODE* occupancyNode) const;
     /// integrate a "miss" measurement according to the tree's sensor model
     virtual void integrateMiss(NODE* occupancyNode) const;
-    // update logodds value of node, given update is added to current value.
+    /// update logodds value of node by adding to the current value.
     virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;
 
     /// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -340,10 +340,13 @@ 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 (node->pruneNode()){
           this->tree_size -= 8;
-        else
+          // return pointer to current parent (pruned), the just updated node no longer exists
+          retval = node;
+        } else{
           node->updateOccupancyChildren();
+        }
 
         return retval;
       }
@@ -626,6 +629,94 @@ namespace octomap {
     return true;
   }
 
+  template <class NODE>
+  bool OccupancyOcTreeBase<NODE>::getRayIntersection (const point3d& origin, const point3d& direction, const point3d& center,
+                 point3d& intersection, double delta/*=0.0*/) const {
+    // We only need three normals for the six planes
+    octomap::point3d normalX(1, 0, 0);
+    octomap::point3d normalY(0, 1, 0);
+    octomap::point3d normalZ(0, 0, 1);
+
+    // One point on each plane, let them be the center for simplicity
+    octomap::point3d pointXNeg(center(0) - this->resolution / 2.0, center(1), center(2));
+    octomap::point3d pointXPos(center(0) + this->resolution / 2.0, center(1), center(2));
+    octomap::point3d pointYNeg(center(0), center(1) - this->resolution / 2.0, center(2));
+    octomap::point3d pointYPos(center(0), center(1) + this->resolution / 2.0, center(2));
+    octomap::point3d pointZNeg(center(0), center(1), center(2) - this->resolution / 2.0);
+    octomap::point3d pointZPos(center(0), center(1), center(2) + this->resolution / 2.0);
+
+    double lineDotNormal = 0.0;
+    double d = 0.0;
+    double outD = std::numeric_limits<double>::max();
+    octomap::point3d intersect;
+    bool found = false;
+
+    // Find the intersection (if any) with each place
+    // Line dot normal will be zero if they are parallel, in which case no intersection can be the entry one
+    // if there is an intersection does it occur in the bounded plane of the voxel
+    // if yes keep only the closest (smallest distance to sensor origin).
+    if((lineDotNormal = normalX.dot(direction))){
+      d = (pointXNeg - origin).dot(normalX) / lineDotNormal;
+      intersect = direction * d + origin;
+      if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
+         intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
+        outD = std::min(outD, d);
+        found = true;
+      }
+
+      d = (pointXPos - origin).dot(normalX) / lineDotNormal;
+      intersect = direction * d + origin;
+      if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
+         intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
+        outD = std::min(outD, d);
+        found = true;
+      }
+    }
+
+    if((lineDotNormal = normalY.dot(direction))){
+      d = (pointYNeg - origin).dot(normalY) / lineDotNormal;
+      intersect = direction * d + origin;
+      if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
+         intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
+        outD = std::min(outD, d);
+        found = true;
+      }
+
+      d = (pointYPos - origin).dot(normalY) / lineDotNormal;
+      intersect = direction * d + origin;
+      if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
+         intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
+        outD = std::min(outD, d);
+        found = true;
+      }
+    }
+
+    if((lineDotNormal = normalZ.dot(direction))){
+      d = (pointZNeg - origin).dot(normalZ) / lineDotNormal;
+      intersect = direction * d + origin;
+      if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
+         intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
+        outD = std::min(outD, d);
+        found = true;
+      }
+
+      d = (pointZPos - origin).dot(normalZ) / lineDotNormal;
+      intersect = direction * d + origin;
+      if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
+         intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
+        outD = std::min(outD, d);
+        found = true;
+      }
+    }
+
+    // Substract (add) a fraction to ensure no ambiguity on the starting voxel
+    // Don't start on a bondary.
+    if(found)
+      intersection = direction * (outD + delta) + origin;
+
+    return found;
+  }
+
 
   template <class NODE> inline bool 
   OccupancyOcTreeBase<NODE>::integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval) {

octomap/octomap-config.cmake.in

@@ -21,18 +21,12 @@
 # ===================================================================================
 
 
-set(OCTOMAP_INCLUDE_DIRS @OCTOMAP_INCLUDE_DIRS@)
-set(OCTOMAP_LIBRARY_DIRS @OCTOMAP_LIB_DIR@)
+set(OCTOMAP_INCLUDE_DIRS "@OCTOMAP_INCLUDE_DIRS@")
+set(OCTOMAP_LIBRARY_DIRS "@OCTOMAP_LIB_DIR@")
 
-# 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_EXT}
-  @OCTOMAP_LIB_DIR@/liboctomath${SO_EXT}
+  "@OCTOMAP_LIB_DIR@/liboctomap@CMAKE_SHARED_LIBRARY_SUFFIX@"
+  "@OCTOMAP_LIB_DIR@/liboctomath@CMAKE_SHARED_LIBRARY_SUFFIX@"
 )

octomap/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>octomap</name>
-  <version>1.6.1</version>
+  <version>1.6.2</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

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

octomap/src/Pointcloud.cpp

@@ -31,11 +31,16 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifdef _MSC_VER
+/* According to c++ standard including this header has no practical effect
+ * but it can be used to determine the c++ standard library implementation.
+ */ 
+#include <ciso646>
+
+#if defined(_MSC_VER) || defined(_LIBCPP_VERSION)
   #include <algorithm>
-#else
-  #include <ext/algorithm>
-#endif
+#else
+  #include <ext/algorithm>
+#endif
 #include <fstream>
 #include <math.h>
 
@@ -199,13 +204,13 @@ namespace octomap {
 
   void Pointcloud::subSampleRandom(unsigned int num_samples, Pointcloud& sample_cloud) {
     point3d_collection samples;
-    // visual studio does not support random_sample_n
-  #ifdef _MSC_VER
-    samples.reserve(this->size());
-    samples.insert(samples.end(), this->begin(), this->end());
-    std::random_shuffle(samples.begin(), samples.end());
-    samples.resize(num_samples);
-  #else
+    // visual studio does not support random_sample_n and neither does libc++
+  #if defined(_MSC_VER) || defined(_LIBCPP_VERSION)
+    samples.reserve(this->size());
+    samples.insert(samples.end(), this->begin(), this->end());
+    std::random_shuffle(samples.begin(), samples.end());
+    samples.resize(num_samples);
+  #else
     random_sample_n(begin(), end(), std::back_insert_iterator<point3d_collection>(samples), num_samples);
     for (unsigned int i=0; i<samples.size(); i++) {
       sample_cloud.push_back(samples[i]);