void srs_env_model::EMOcTree::insertColoredScan(const typePointCloud& coloredScan, const octomap::point3d& sensor_origin, double maxrange, bool pruning, bool lazy_eval) { // convert colored scan to octomap pcl octomap::Pointcloud scan; octomap::pointcloudPCLToOctomap(coloredScan, scan); // insert octomap pcl to count new probabilities octomap::KeySet free_cells, occupied_cells; computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange); // insert data into tree ----------------------- for (octomap::KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) { updateNode(*it, false, lazy_eval); } for (octomap::KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) { updateNode(*it, true, lazy_eval); } // TODO: does pruning make sense if we used "lazy_eval"? if (pruning) this->prune(); // update node colors BOOST_FOREACH (const pcl::PointXYZRGB& pt, coloredScan.points) { // averageNodeColor or integrateNodeColor can be used to count final color averageNodeColor(pt.x, pt.y, pt.z, (unsigned char)pt.r, (unsigned char)pt.g, (unsigned char)pt.b, (unsigned char)255); } }
void PatchGodunov::updateState(FArrayBox& a_U, FluxBox& a_F, Real& a_maxWaveSpeed, const FArrayBox& a_S, const Real& a_dt, const Box& a_box) { CH_assert(isDefined()); CH_assert(a_box == m_currentBox); int numPrim = m_gdnvPhysics->numPrimitives(); int numFlux = m_gdnvPhysics->numFluxes(); FluxBox whalf(a_box,numPrim); whalf.setVal(0.0); a_F.resize(a_box,numFlux); a_F.setVal(0.0); computeWHalf(whalf, a_U, a_S, a_dt, a_box); FArrayBox dU(a_U.box(),a_U.nComp()); computeUpdate(dU, a_F, a_U, whalf, a_dt, a_box); a_U += dU; // Get and return the maximum wave speed on this patch/grid a_maxWaveSpeed = m_gdnvPhysics->getMaxWaveSpeed(a_U, m_currentBox); }