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); }