Merge branch 'devel'

increase_version.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+
+# Increases the version number of package.xml and CMakeLists.txt files in
+# subfolders. The first argument specifies the version increase:
+# major, minor, or patch (default, e.g. 1.6.2 --> 1.6.3)
+#
+# Borrows heaviliy from ROS / catkin release tools
+
+
+import re
+import sys
+import copy
+
+manifest_match = "<version>(\d+)\.(\d+)\.(\d+)</version>"
+
+
+if __name__ == '__main__':
+  bump = "patch"
+  if len(sys.argv) > 1:
+    bump = sys.argv[1]
+    if bump not in {"major","minor","patch"}:
+      print sys.argv[0]+" [major|minor|patch] (default: patch)"
+      exit(-1)
+
+
+
+  manifests=["octomap/package.xml","octovis/package.xml"]
+  cmakelists=["octomap/CMakeLists.txt","octovis/CMakeLists.txt"]
+  versions = []
+
+  # find versions in package.xml
+  for manifest in manifests:
+    with open(manifest, 'r') as f:
+      package_str = f.read()
+    match = re.search(manifest_match, package_str)
+    if match is None:
+      print "Error: no version tag found in %s" % manifest
+      exit(-1)
+    else:
+      v= match.groups()
+      v = [int(x) for x in v]
+      versions.append(v)
+
+  # find version in CMakeLists:
+  for cmake in cmakelists:
+    with open(cmake, 'r') as f:
+      cmake_str = f.read()
+    v = []
+    for m in ["MAJOR","MINOR","PATCH"]:
+      searchstr = "_%s_VERSION (\d+)\)" % m
+      match = re.search(searchstr, cmake_str)
+      if match is None:
+        print "Error: no version tag %s found in %s" % (searchstr,cmake)
+        exit(-1)
+
+      v.append(int(match.group(1)))
+
+    versions.append(v)
+
+  new_version = copy.deepcopy(versions[0])
+  for v in versions:
+    if v != versions[0]:
+      print "Error: check current versions, mismatch: %d.%d.%d vs. %d.%d.%d" %(tuple(v)+tuple(versions[0]))
+      exit(-1)
+
+  print "OctoMap component versions found: %d.%d.%d" % tuple(versions[0])
+  # "bump version" from catkin:
+  # find the desired index
+  idx = dict(major=0, minor=1, patch=2)[bump]
+  # increment the desired part
+  new_version[idx] += 1
+  # reset all parts behind the bumped part
+  new_version = new_version[:idx + 1] + [0 for x in new_version[idx + 1:]]
+  new_version_str = "%d.%d.%d" % tuple(new_version)
+  print 'Updating to new version: %s\n' % new_version_str
+
+  # adjust CMakeLists
+  for cmake in cmakelists:
+    with open(cmake, 'r') as f:
+      cmake_str = f.read()
+    idx = dict(MAJOR=0, MINOR=1, PATCH=2)
+    for m in ["MAJOR","MINOR","PATCH"]:      
+      old_str = "_%s_VERSION %d)" % (m,versions[0][idx[m]])
+      new_str = "_%s_VERSION %d)" % (m,new_version[idx[m]])
+      cmake_str = cmake_str.replace(old_str, new_str)
+
+    with open(cmake, 'w') as f:
+      f.write(cmake_str)
+
+
+
+  # adjust package.xml
+  for manifest in manifests:
+    with open(manifest, 'r') as f:
+      package_str = f.read()
+    old_str = "<version>%d.%d.%d</version>" % tuple(versions[0])
+    new_str = "<version>%s</version>" % new_version_str
+    new_package_str = package_str.replace(old_str, new_str)
+
+    with open(manifest, 'w') as f:
+      f.write(new_package_str)
+
+  print "Finished writing package.xml and CMakeLists.txt files."
+  print "Now check the output, adjust CHANGELOG, \"git commit\", and run \"git tag v%s\"." % new_version_str
+
+
+
+
+
+

octomap/CHANGELOG.txt

@@ -1,3 +1,9 @@
+v1.6.3: 2014:01-13
+==================
+- New function setNodeValue(...) to directly set a node's value (#31)
+- New approximate point cloud insertion as fast variant (#39, see flag for
+  insertPointCloud(...) / new function computeDiscreteUpdate(...)
+
 v1.6.2: 2013-11-26
 ==================
 - Improved OSX 10.9 compatibility (thx to B. Jensen)

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

octomap/include/octomap/OccupancyOcTreeBase.h

@@ -90,9 +90,11 @@ namespace octomap {
     * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
     * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
     *   This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
+    * @param discretize whether the scan is discretized first into octree key cells (default: false).
+    *   This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a potential speedup.*
     */
     virtual void insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin,
-                   double maxrange=-1., bool lazy_eval = false);
+                   double maxrange=-1., bool lazy_eval = false, bool discretize = false);
 
     /**
     * Integrate a 3d scan (transform scan before tree update), parallelized with OpenMP.
@@ -109,9 +111,11 @@ namespace octomap {
     * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
     * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
     *   This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
+    * @param discretize whether the scan is discretized first into octree key cells (default: false).
+    *   This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a potential speedup.*
     */
     virtual void insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin,
-                   double maxrange=-1., bool lazy_eval = false);
+                   double maxrange=-1., bool lazy_eval = false, bool discretize = false);
 
     /**
     * Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP.
@@ -122,8 +126,10 @@ namespace octomap {
     * @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).
     *   This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
+    * @param discretize whether the scan is discretized first into octree key cells (default: false).
+    *   This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a potential speedup.
     */
-    virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false);
+    virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false);
 
     /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically.
     OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
@@ -164,7 +170,7 @@ namespace octomap {
      virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false);
 
      /**
-      * Manipulate log_odds value of a voxel directly. This only works if key is at the lowest
+      * Set log_odds value of voxel to log_odds_value. This only works if key is at the lowest
       * octree level
       *
       * @param key OcTreeKey of the NODE that is to be updated
@@ -173,11 +179,49 @@ namespace octomap {
       *   This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
       * @return pointer to the updated NODE
       */
+     virtual NODE* setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval = false);
+
+     /**
+      * Set log_odds value of voxel to log_odds_value.
+      * Looks up the OcTreeKey corresponding to the coordinate and then calls setNodeValue() with it.
+      *
+      * @param value 3d coordinate of the NODE that is to be updated
+      * @param log_odds_update value to be added (+) to log_odds value of node
+      * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
+      *   This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
+      * @return pointer to the updated NODE
+      */
+     virtual NODE* setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval = false);
+
+     /**
+      * Set log_odds value of voxel to log_odds_value.
+      * Looks up the OcTreeKey corresponding to the coordinate and then calls setNodeValue() with it.
+      *
+      * @param x
+      * @param y
+      * @param z
+      * @param log_odds_update value to be added (+) to log_odds value of node
+      * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
+      *   This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
+      * @return pointer to the updated NODE
+      */
+     virtual NODE* setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval = false);
+
+     /**
+      * Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
+      * This only works if key is at the lowest octree level
+      *
+      * @param key OcTreeKey of the NODE that is to be updated
+      * @param log_odds_update value to be added (+) to log_odds value of node
+      * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
+      *   This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
+      * @return pointer to the updated NODE
+      */
      virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false);
 
      /**
-      * Manipulate log_odds value of voxel directly.
-      * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
+      * Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
+      * Looks up the OcTreeKey corresponding to the coordinate and then calls updateNode() with it.
       *
       * @param value 3d coordinate of the NODE that is to be updated
       * @param log_odds_update value to be added (+) to log_odds value of node
@@ -188,8 +232,8 @@ namespace octomap {
      virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false);
 
      /**
-      * Manipulate log_odds value of voxel directly.
-      * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
+      * Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
+      * Looks up the OcTreeKey corresponding to the coordinate and then calls updateNode() with it.
       *
       * @param x
       * @param y
@@ -358,6 +402,25 @@ namespace octomap {
                        KeySet& occupied_cells,
                        double maxrange);
 
+
+    /**
+     * Helper for insertPointCloud(). Computes all octree nodes affected by the point cloud
+     * integration at once. Here, occupied nodes have a preference over free
+     * ones. This function first discretizes the scan with the octree grid, which results
+     * in fewer raycasts (=speedup) but a slightly different result than computeUpdate().
+     *
+     * @param scan point cloud measurement to be integrated
+     * @param origin origin of the sensor for ray casting
+     * @param free_cells keys of nodes to be cleared
+     * @param occupied_cells keys of nodes to be marked occupied
+     * @param maxrange maximum range for raycasting (-1: unlimited)
+     */
+    void computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin,
+                       KeySet& free_cells,
+                       KeySet& occupied_cells,
+                       double maxrange);
+
+
     // -- I/O  -----------------------------------------
 
     /**
@@ -433,6 +496,9 @@ namespace octomap {
     NODE* updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
                            unsigned int depth, const float& log_odds_update, bool lazy_eval = false);
 
+    NODE* setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
+                           unsigned int depth, const float& log_odds_value, bool lazy_eval = false);
+
     void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);
 
     void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);