void VoxelGridModel::updateWorld(const vector<geometry_msgs::Point>& footprint,
const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
//remove points in the laser scan boundry
for(unsigned int i = 0; i < laser_scans.size(); ++i)
removePointsInScanBoundry(laser_scans[i], 10.0);
//iterate through all observations and update the grid
for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
const Observation& obs = *it;
const pcl::PointCloud<pcl::PointXYZ>& cloud = obs.cloud_;
for(unsigned int i = 0; i < cloud.points.size(); ++i){
//filter out points that are too high
if(cloud.points[i].z > max_z_)
continue;
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
+ (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
+ (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
if(sq_dist >= sq_obstacle_range_)
continue;
//insert the point
insert(cloud.points[i]);
}
}
//remove the points that are in the footprint of the robot
//removePointsInPolygon(footprint);
}
void PointGrid::updateWorld(const vector<geometry_msgs::Point>& footprint,
const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
//for our 2D point grid we only remove freespace based on the first laser scan
if(laser_scans.empty())
return;
removePointsInScanBoundry(laser_scans[0]);
//iterate through all observations and update the grid
for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
const Observation& obs = *it;
const sensor_msgs::PointCloud& cloud = (obs.cloud_);
for(unsigned int i = 0; i < cloud.points.size(); ++i){
//filter out points that are too high
if(cloud.points[i].z > max_z_)
continue;
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
+ (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
+ (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
if(sq_dist >= sq_obstacle_range_)
continue;
//insert the point
insert(cloud.points[i]);
}
}
//remove the points that are in the footprint of the robot
removePointsInPolygon(footprint);
}
