From fbbf2e28c118ad039542a10ad85b4fba35e067dc Mon Sep 17 00:00:00 2001 From: OndrejMasopust Date: Mon, 26 Apr 2021 15:58:48 +0200 Subject: [PATCH 1/6] improved insertPointCloudRays An option to specify if the endpoint is occupied was added. This can be used when an obstacle is farher than the range sensor measuring limit. The ray can be used to update free nodes while not updating any node as occupied. The function is now overloaded and still accpets the old version. --- octomap/include/octomap/OccupancyOcTreeBase.hxx | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index 8e3b312c..e51d82d9 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -111,9 +111,12 @@ namespace octomap { insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval, discretize); } + void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool lazy_eval) { + insertPointCloudRays(pc, origin, maxrange, true, lazy_eval); + } template - void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool endpoint_occupied, bool lazy_eval) { if (pc.size() < 1) return; @@ -137,7 +140,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 to be occupied } } From f226f8fb21a88c973a290ec09235600d3f009009 Mon Sep 17 00:00:00 2001 From: OndrejMasopust Date: Mon, 26 Apr 2021 15:59:38 +0200 Subject: [PATCH 2/6] just a comment edit --- octomap/include/octomap/OccupancyOcTreeBase.hxx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index e51d82d9..7cf968a3 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -140,7 +140,7 @@ namespace octomap { for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) { updateNode(*it, false, lazy_eval); // insert freespace measurement } - updateNode(p, endpoint_occupied, lazy_eval); // update endpoint to be occupied + updateNode(p, endpoint_occupied, lazy_eval); // update endpoint } } From 7004edd16088b13af045275c8e0f90ed171c690c Mon Sep 17 00:00:00 2001 From: OndrejMasopust Date: Mon, 26 Apr 2021 16:03:17 +0200 Subject: [PATCH 3/6] improved insertPointCloudRays .h definition and doc comment edit --- octomap/include/octomap/OccupancyOcTreeBase.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.h b/octomap/include/octomap/OccupancyOcTreeBase.h index a8532143..550916df 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.h +++ b/octomap/include/octomap/OccupancyOcTreeBase.h @@ -131,6 +131,11 @@ namespace octomap { */ virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false); + /** + * Works just like a wrapper with endpoint_occupied set to true. + */ + virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false); + /** * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP. * This function simply inserts all rays of the point clouds as batch operation. @@ -140,10 +145,11 @@ namespace octomap { * @param scan Pointcloud (measurement endpoints), in global reference frame * @param sensor_origin measurement origin in global reference frame * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) + * @param endpoint_occupied specifies it the endpoints should be updated as occupied or not * @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. */ - 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 endpoint_occupied = true, bool lazy_eval = false); /** * Set log_odds value of voxel to log_odds_value. This only works if key is at the lowest From b7de9ceef04e1e850e463d6702b8554a28785a91 Mon Sep 17 00:00:00 2001 From: OndrejMasopust Date: Tue, 27 Apr 2021 08:45:13 +0200 Subject: [PATCH 4/6] added missing template keyword --- octomap/include/octomap/OccupancyOcTreeBase.hxx | 1 + 1 file changed, 1 insertion(+) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index 7cf968a3..2c696cb3 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -111,6 +111,7 @@ namespace octomap { insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval, discretize); } + template void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool lazy_eval) { insertPointCloudRays(pc, origin, maxrange, true, lazy_eval); } From c75290b89f45aa37841aef8d24f463dacd67b0a6 Mon Sep 17 00:00:00 2001 From: OndrejMasopust Date: Wed, 16 Jun 2021 15:30:37 +0200 Subject: [PATCH 5/6] Update OccupancyOcTreeBase.h --- octomap/include/octomap/OccupancyOcTreeBase.h | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.h b/octomap/include/octomap/OccupancyOcTreeBase.h index 550916df..f9e79884 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.h +++ b/octomap/include/octomap/OccupancyOcTreeBase.h @@ -131,11 +131,6 @@ namespace octomap { */ virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false); - /** - * Works just like a wrapper with endpoint_occupied set to true. - */ - virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false); - /** * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP. * This function simply inserts all rays of the point clouds as batch operation. @@ -145,11 +140,11 @@ namespace octomap { * @param scan Pointcloud (measurement endpoints), in global reference frame * @param sensor_origin measurement origin in global reference frame * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) - * @param endpoint_occupied specifies it the endpoints should be updated as occupied or not * @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 endpoint_occupied = true, 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 From fed6e85794dbd338708fefe2c73f0a7209dce15a Mon Sep 17 00:00:00 2001 From: OndrejMasopust Date: Wed, 16 Jun 2021 15:31:26 +0200 Subject: [PATCH 6/6] Update OccupancyOcTreeBase.hxx --- octomap/include/octomap/OccupancyOcTreeBase.hxx | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index 2c696cb3..7f22d95a 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -111,13 +111,9 @@ namespace octomap { insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval, discretize); } - template - void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool lazy_eval) { - insertPointCloudRays(pc, origin, maxrange, true, lazy_eval); - } template - void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool endpoint_occupied, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool lazy_eval, bool endpoint_occupied) { if (pc.size() < 1) return; @@ -141,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, endpoint_occupied, lazy_eval); // update endpoint + updateNode(p, endpoint_occupied, lazy_eval); // update endpoint according to the parameter } }