45 , max_batch_size_(max_batch_size)
46 , max_sensor_delta_(1e-3)
48 process_occupied_cells_set_(NULL)
49 , process_model_cells_set_(NULL)
73 ROS_DEBUG(
"Pushing %lu occupied cells and %lu model cells for lazy updating...",
74 (
long unsigned int)occupied_cells->size(), (
long unsigned int)model_cells->size());
98 ROS_WARN(
"Previous batch update did not complete. Ignoring set of cells to be freed.");
99 delete occupied_cells;
106 const float lg_0 =
tree_->getClampingThresMinLog() -
tree_->getClampingThresMaxLog();
107 const float lg_miss =
tree_->getProbMissLog();
124 ROS_DEBUG(
"Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells",
141 free_cells1[*jt] += it->second;
161 free_cells1.erase(it->first);
162 free_cells2.erase(it->first);
168 free_cells1.erase(*it);
169 free_cells2.erase(*it);
171 ROS_DEBUG(
"Marking %lu cells as free...", (
long unsigned int)(free_cells1.size() + free_cells2.size()));
180 tree_->updateNode(*it, lg_0);
183 for (OcTreeKeyCountMap::iterator it = free_cells1.begin(), end = free_cells1.end(); it != end; ++it)
184 tree_->updateNode(it->first, it->second * lg_miss);
185 for (OcTreeKeyCountMap::iterator it = free_cells2.begin(), end = free_cells2.end(); it != end; ++it)
186 tree_->updateNode(it->first, it->second * lg_miss);
190 ROS_ERROR(
"Internal error while updating octree");
192 tree_->unlockWrite();
193 tree_->triggerUpdateCallback();
209 unsigned int batch_size = 0;
225 for (octomap::KeySet::iterator it = s->begin(), end = s->end(); it != end; ++it)
226 (*occupied_cells_set)[*it]++;
239 ROS_DEBUG(
"Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size);
247 for (octomap::KeySet::iterator it = add_occ->begin(), end = add_occ->end(); it != end; ++it)
248 (*occupied_cells_set)[*it]++;
252 model_cells_set->insert(mod_occ->begin(), mod_occ->end());
260 ROS_DEBUG(
"Pushing %u sets of occupied/model cells to free cells update thread", batch_size);
262 occupied_cells_set = NULL;
boost::mutex update_cell_sets_lock_
std::tr1::unordered_map< octomap::OcTreeKey, unsigned int, octomap::OcTreeKey::KeyHash > OcTreeKeyCountMap
std::size_t max_batch_size_
std::shared_ptr< OccMapTree > OccMapTreePtr
octomap::KeySet * process_model_cells_set_
boost::thread process_thread_
octomap::point3d process_sensor_origin_
OcTreeKeyCountMap * process_occupied_cells_set_
std::deque< octomap::KeySet * > occupied_cells_sets_
boost::mutex cell_process_lock_
LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size=10)
std::vector< OcTreeKey >::iterator iterator
boost::condition_variable update_condition_
boost::thread update_thread_
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
void pushLazyUpdate(octomap::KeySet *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin)
std::deque< octomap::KeySet * > model_cells_sets_
std::deque< octomap::point3d > sensor_origins_
boost::condition_variable process_condition_
void pushBatchToProcess(OcTreeKeyCountMap *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin)