insertPointCloudRays improvement

octomap/include/octomap/OccupancyOcTreeBase.h

@@ -142,8 +142,9 @@ 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 endpoint_occupied specifies if the end point should be updated as occupied or free
      */
-     virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false);
+     virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false, bool endpoint_occupied = true);
 
      /**
       * Set log_odds value of voxel to log_odds_value. This only works if key is at the lowest

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -113,7 +113,7 @@ namespace octomap {
 
 
   template <class NODE>
-  void OccupancyOcTreeBase<NODE>::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool lazy_eval) {
+  void OccupancyOcTreeBase<NODE>::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool lazy_eval, bool endpoint_occupied) {
     if (pc.size() < 1)
       return;
 
@@ -137,7 +137,7 @@ namespace octomap {
           for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) {
             updateNode(*it, false, lazy_eval); // insert freespace measurement
           }
-          updateNode(p, true, lazy_eval); // update endpoint to be occupied
+          updateNode(p, endpoint_occupied, lazy_eval); // update endpoint according to the parameter
         }
       }