diff --git a/octomap/doxygen.h b/octomap/doxygen.h index c8d0949f..e5b515da 100644 --- a/octomap/doxygen.h +++ b/octomap/doxygen.h @@ -83,7 +83,7 @@ You can find an overview at http://octomap.github.com/ and the code repository a Jump right in and have a look at the main class \ref octomap::OcTree OcTree and the examples in src/octomap/simple.cpp. To integrate single measurements into the 3D map have a look at \ref octomap::OcTree::insertRay "OcTree::insertRay(...)", to insert full 3D scans (pointclouds) please have a look at - \ref octomap::OcTree::insertScan "OcTree::insertScan(...)". Queries can be performed e.g. with \ref octomap::OcTree::search "OcTree::search(...)" or + \ref octomap::OcTree::insertPointCloud "OcTree::insertPointCloud(...)". Queries can be performed e.g. with \ref octomap::OcTree::search "OcTree::search(...)" or \ref octomap::OcTree::castRay(...) "OcTree::castRay(...)". The preferred way to batch-access or process nodes in an Octree is with the iterators \ref leaf_iterator "leaf_iterator", \ref tree_iterator "tree_iterator", or \ref leaf_bbx_iterator "leaf_bbx_iterator".

diff --git a/octomap/include/octomap/OccupancyOcTreeBase.h b/octomap/include/octomap/OccupancyOcTreeBase.h index 67fc3376..2b0ebe60 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.h +++ b/octomap/include/octomap/OccupancyOcTreeBase.h @@ -75,107 +75,125 @@ namespace octomap { /// Copy constructor OccupancyOcTreeBase(const OccupancyOcTreeBase& rhs); - /** - * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP. - * Special care is taken that each voxel - * in the map is updated only once, and occupied nodes have a preference over free ones. - * This avoids holes in the floor from mutual deletion and is more efficient than the plain - * ray insertion in insertScanRays(). - * - * @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 pruning whether the tree is (losslessly) pruned after insertion (default: true) - * @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 insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, - double maxrange=-1., bool pruning=true, bool lazy_eval = false); - - /** - * Integrate a 3d scan (transform scan before tree update), parallelized with OpenMP. - * Special care is taken that each voxel - * in the map is updated only once, and occupied nodes have a preference over free ones. - * This avoids holes in the floor from mutual deletion and is more efficient than the plain - * ray insertion in insertScanRays(). - * - * @param scan Pointcloud (measurement endpoints) relative to frame origin - * @param sensor_origin origin of sensor relative to frame origin - * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin - * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) - * @param pruning whether the tree is (losslessly) pruned after insertion (default: true) - * @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 insertScan(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin, - double maxrange=-1., bool pruning = true, bool lazy_eval = false); - - /** - * Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP. - * - * @param scan ScanNode contains Pointcloud data and frame/sensor origin - * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) - * @param pruning whether the tree is (losslessly) pruned after insertion (default: true) - * @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. - */ - virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false); - - /// @note Deprecated, use insertScanRays instead. - OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& scan, const point3d& sensor_origin, double maxrange, bool pruning = true, 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. - * Discretization effects can lead to the deletion of occupied space, it is - * usually recommended to use insertScan() instead. + * Special care is taken that each voxel + * in the map is updated only once, and occupied nodes have a preference over free ones. + * This avoids holes in the floor from mutual deletion and is more efficient than the plain + * ray insertion in insertPointCloudRays(). * * @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 pruning whether the tree is (losslessly) pruned after insertion (default: true) * @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 insertScanRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool pruning = true, bool lazy_eval = false); + virtual void insertPointCloud(const Pointcloud& scan, const octomap::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 - * 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); + * Integrate a 3d scan (transform scan before tree update), parallelized with OpenMP. + * Special care is taken that each voxel + * in the map is updated only once, and occupied nodes have a preference over free ones. + * This avoids holes in the floor from mutual deletion and is more efficient than the plain + * ray insertion in insertPointCloudRays(). + * + * @param scan Pointcloud (measurement endpoints) relative to frame origin + * @param sensor_origin origin of sensor relative to frame origin + * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin + * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) + * @param pruning whether the tree is (losslessly) pruned after insertion (default: true) + * @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 insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin, + double maxrange=-1., 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. - * - * @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* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false); + * Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP. + * + * @param scan ScanNode contains Pointcloud data and frame/sensor origin + * @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. + */ + virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false); + + /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically. + OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, + double maxrange=-1., bool pruning=true, bool lazy_eval = false)) + { + this->insertPointCloud(scan, sensor_origin, maxrange, lazy_eval); + } + + /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically. + OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const point3d& sensor_origin, + const pose6d& frame_origin, double maxrange=-1., bool pruning = true, bool lazy_eval = false)) + { + this->insertPointCloud(scan, sensor_origin, frame_origin, maxrange, lazy_eval); + } + + /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically. + OCTOMAP_DEPRECATED(virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false)){ + this->insertPointCloud(scan, maxrange, lazy_eval); + } + + /// @note Deprecated, use insertPointCloudRays instead. pruning is now done automatically. + OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& scan, const point3d& sensor_origin, double maxrange, bool lazy_eval = false)){ + this->insertPointCloudRays(scan, sensor_origin, maxrange, lazy_eval); + } /** - * Manipulate log_odds value of voxel directly. - * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it. + * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP. + * This function simply inserts all rays of the point clouds as batch operation. + * Discretization effects can lead to the deletion of occupied space, it is + * usually recommended to use insertPointCloud() instead. * - * @param x - * @param y - * @param z - * @param log_odds_update value to be added (+) to log_odds value of node + * @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 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(double x, double y, double z, float log_odds_update, bool lazy_eval = false); + 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 + * 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. + * + * @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* 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. + * + * @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* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false); /** * Integrate occupancy measurement. @@ -225,8 +243,8 @@ namespace octomap { /** * Insert one ray between origin and end into the tree. * integrateMissOnRay() is called for the ray, the end point is updated as occupied. - * It is usually more efficient to insert complete pointcloudsm with insertScan() or - * insertScanRays(). + * It is usually more efficient to insert complete pointcloudsm with insertPointCloud() or + * insertPointCloudRays(). * * @param origin origin of sensor in global coordinates * @param end endpoint of measurement in global coordinates @@ -294,7 +312,7 @@ namespace octomap { /** - * Helper for insertScan. Computes all octree nodes affected by the point cloud + * Helper for insertPointCloud(). Computes all octree nodes affected by the point cloud * integration at once. Here, occupied nodes have a preference over free * ones. * diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index bdc7cdce..adb64c6c 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -69,19 +69,19 @@ namespace octomap { } - // performs transformation to data and sensor origin first template - void OccupancyOcTreeBase::insertScan(const ScanNode& scan, double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloud(const ScanNode& scan, double maxrange, bool lazy_eval) { + // performs transformation to data and sensor origin first Pointcloud& cloud = *(scan.scan); pose6d frame_origin = scan.pose; point3d sensor_origin = frame_origin.inv().transform(scan.pose.trans()); - insertScan(cloud, sensor_origin, frame_origin, maxrange, pruning, lazy_eval); + insertPointCloud(cloud, sensor_origin, frame_origin, maxrange, lazy_eval); } template - void OccupancyOcTreeBase::insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, - double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin, + double maxrange, bool lazy_eval) { KeySet free_cells, occupied_cells; computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange); @@ -95,23 +95,19 @@ namespace octomap { } } - // performs transformation to data and sensor origin first template - void OccupancyOcTreeBase::insertScan(const Pointcloud& pc, const point3d& sensor_origin, const pose6d& frame_origin, - double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloud(const Pointcloud& pc, const point3d& sensor_origin, const pose6d& frame_origin, + double maxrange, bool lazy_eval) { + // performs transformation to data and sensor origin first Pointcloud transformed_scan (pc); transformed_scan.transform(frame_origin); point3d transformed_sensor_origin = frame_origin.transform(sensor_origin); - insertScan(transformed_scan, transformed_sensor_origin, maxrange, pruning, lazy_eval); + insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval); } - template - void OccupancyOcTreeBase::insertScanNaive(const Pointcloud& pc, const point3d& origin, double maxrange, bool pruning, bool lazy_eval) { - this->insertScanRays(pc, origin, maxrange, pruning, lazy_eval); - } template - void OccupancyOcTreeBase::insertScanRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool lazy_eval) { if (pc.size() < 1) return; @@ -140,9 +136,6 @@ namespace octomap { } } - - if (pruning) - this->prune(); } diff --git a/octomap/src/eval_octree_accuracy.cpp b/octomap/src/eval_octree_accuracy.cpp index 484312cb..ff238174 100644 --- a/octomap/src/eval_octree_accuracy.cpp +++ b/octomap/src/eval_octree_accuracy.cpp @@ -108,7 +108,7 @@ int main(int argc, char** argv) { if (currentScan % skip_scan_eval != 0){ if (max_scan_no > 0) cout << "("<insertScan(**scan_it, maxrange, false); + tree->insertPointCloud(**scan_it, maxrange); } else cout << "(SKIP) " << flush; @@ -118,6 +118,8 @@ int main(int argc, char** argv) { currentScan++; } + tree->expand(); + cout << "\nEvaluating scans\n===========================\n"; currentScan = 1; diff --git a/octomap/src/graph2tree.cpp b/octomap/src/graph2tree.cpp index 2f837574..3e9bcb26 100644 --- a/octomap/src/graph2tree.cpp +++ b/octomap/src/graph2tree.cpp @@ -240,9 +240,9 @@ int main(int argc, char** argv) { else cout << "("<insertScanRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, (compression==1)); + tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange); else - tree->insertScan((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, (compression==1)); + tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange); if (compression == 2){ tree->toMaxLikelihood(); diff --git a/octomap/src/testing/test_changedkeys.cpp b/octomap/src/testing/test_changedkeys.cpp index 1e8dfcd9..20499879 100644 --- a/octomap/src/testing/test_changedkeys.cpp +++ b/octomap/src/testing/test_changedkeys.cpp @@ -81,7 +81,7 @@ int main(int argc, char** argv) { } // insert in global coordinates: - tree.insertScan(cloud, origin, -1, false); + tree.insertPointCloud(cloud, origin, -1); } printChanges(tree); diff --git a/octomap/src/testing/test_scans.cpp b/octomap/src/testing/test_scans.cpp index 2dfbd467..733c4093 100644 --- a/octomap/src/testing/test_scans.cpp +++ b/octomap/src/testing/test_scans.cpp @@ -31,7 +31,7 @@ int main(int argc, char** argv) { } // insert in global coordinates: - tree.insertScan(cloud, origin); + tree.insertPointCloud(cloud, origin); cout << "done." << endl; cout << "writing to spherical_scan.bt..." << endl; diff --git a/octomap/src/testing/unit_tests.cpp b/octomap/src/testing/unit_tests.cpp index 08198517..3e73a3df 100644 --- a/octomap/src/testing/unit_tests.cpp +++ b/octomap/src/testing/unit_tests.cpp @@ -249,7 +249,7 @@ int main(int argc, char** argv) { } OcTree tree (0.05); - tree.insertScan(*measurement, origin); + tree.insertPointCloud(*measurement, origin); EXPECT_EQ ((int) tree.size(), 54076); ScanGraph* graph = new ScanGraph(); diff --git a/octovis/src/ViewerGui.cpp b/octovis/src/ViewerGui.cpp index 45ff855d..8bb9ddf5 100644 --- a/octovis/src/ViewerGui.cpp +++ b/octovis/src/ViewerGui.cpp @@ -314,7 +314,7 @@ namespace octomap{ unsigned numScans = m_scanGraph->size(); unsigned currentScan = 1; for (it = m_scanGraph->begin(); it != m_nextScanToAdd; it++) { - tree->insertScan(**it, m_laserMaxRange); + tree->insertPointCloud(**it, m_laserMaxRange); fprintf(stderr, "generateOctree:: inserting scan node with %d points, origin: %.2f ,%.2f , %.2f.\n", (unsigned int) (*it)->scan->size(), (*it)->pose.x(), (*it)->pose.y(), (*it)->pose.z() ); @@ -368,7 +368,7 @@ namespace octomap{ return; } // not used with ColorOcTrees, omitting casts - ((OcTree*) r->octree)->insertScan(**m_nextScanToAdd, m_laserMaxRange); + ((OcTree*) r->octree)->insertPointCloud(**m_nextScanToAdd, m_laserMaxRange); m_nextScanToAdd++; }