discrete updates: only add one point per voxel

Only add and ray trace one point per voxel. When another point is in
a voxel that has already been added, skip it.

Author: c-andy-martin

octomap_server/src/octomap_server.cpp

@@ -497,6 +497,8 @@ void OctomapServer::insertScan(
 {
   const auto sensor_origin = octomap::pointTfToOctomap(sensor_origin_tf);
 
+  bool discrete = true;
+
   if (!octree_->coordToKeyChecked(sensor_origin, update_bbox_min_) ||
     !octree_->coordToKeyChecked(sensor_origin, update_bbox_max_))
   {
@@ -513,18 +515,25 @@ void OctomapServer::insertScan(
       point = sensor_origin + (point - sensor_origin).normalized() * max_range_;
     }
 
-    // only clear space (ground points)
-    if (octree_->computeRayKeys(sensor_origin, point, key_ray_)) {
-      free_cells.insert(key_ray_.begin(), key_ray_.end());
-    }
-
+    // free endpoint
     octomap::OcTreeKey end_key;
     if (octree_->coordToKeyChecked(point, end_key)) {
+      if (!free_cells.insert(end_key).second) {
+        if (discrete) {
+          // This ray has aleady been traced
+          continue;
+        }
+      }
       updateMinKey(end_key, update_bbox_min_);
       updateMaxKey(end_key, update_bbox_max_);
     } else {
       RCLCPP_ERROR_STREAM(get_logger(), "Could not generate Key for endpoint " << point);
     }
+
+    // only clear space (ground points)
+    if (octree_->computeRayKeys(sensor_origin, point, key_ray_)) {
+      free_cells.insert(key_ray_.begin(), key_ray_.end());
+    }
   }
 
   // all other points: free on ray, occupied on endpoint: