void UniformGrid::refine(KdTree * tree)
{
int level1;
float hh;
Vector3F sample, subs;
int u;
unsigned k;
BoundingBox box;
m_cellsToRefine->begin();
while (!m_cellsToRefine->end()) {
sdb::CellValue * parentCell = m_cellsToRefine->value();
if(parentCell->visited > 0) {
k = m_cellsToRefine->key();
level1 = parentCell->level + 1;
hh = cellSizeAtLevel(level1) * .5f;
sample = cellCenter(k);
removeCell(k);
for(u = 0; u < 8; u++) {
subs = sample + Vector3F(hh * Cell8ChildOffset[u][0],
hh * Cell8ChildOffset[u][1],
hh * Cell8ChildOffset[u][2]);
box.setMin(subs.x - hh, subs.y - hh, subs.z - hh);
box.setMax(subs.x + hh, subs.y + hh, subs.z + hh);
if(tree->intersectBox(box))
addCell(subs, level1);
}
}
m_cellsToRefine->next();
}
}
sm::LaserScan::Ptr
simulateRangeScan (const nm::OccupancyGrid& grid, const gm::Pose& sensor_pose,
const sm::LaserScan& scanner_info, const bool unknown_obstacles)
{
sm::LaserScan::Ptr result(new sm::LaserScan(scanner_info));
const double angle_range = scanner_info.angle_max - scanner_info.angle_min;
const unsigned n =
(unsigned) round(1+angle_range/scanner_info.angle_increment);
const gm::Point& p0 = sensor_pose.position;
const Cell c0 = pointCell(grid.info, p0);
const double theta0 = tf::getYaw(sensor_pose.orientation);
result->ranges.resize(n);
for (unsigned i=0; i<n; i++)
{
const double theta = scanner_info.angle_min+i*scanner_info.angle_increment;
const gm::Point scan_max =
rayEndPoint(p0, theta0 + theta, scanner_info.range_max+1);
result->ranges[i] = scanner_info.range_max+1; // Default if loop terminates
BOOST_FOREACH (const Cell& c, rayTrace(grid.info, p0, scan_max, true))
{
const gm::Point p = cellCenter(grid.info, c);
const double d = sqrt(pow(p.x-p0.x, 2) + pow(p.y-p0.y, 2));
char data = grid.data[cellIndex(grid.info, c)];
if (d > scanner_info.range_max)
break;
else if (data == OCCUPIED && !(c==c0))
{
result->ranges[i] = d;
break;
}
else if (data == UNKNOWN && !(c==c0))
{
result->ranges[i] = unknown_obstacles ? d : scanner_info.range_max+1;
break;
}
}
}
return result;
}
