double kpoAnalyzerThread::computeCloudResolution (const CloudConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector<int> indices (2);
std::vector<float> sqr_distances (2);
pcl::search::KdTree<PointType> tree;
tree.setInputCloud (cloud);
for (size_t i = 0; i < cloud->size (); ++i)
{
if (! pcl_isfinite ((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
return res;
}
void
cloud_cb (const CloudConstPtr& cloud)
{
PCDWriter w;
sprintf (buf, "frame_%06d.pcd", i);
w.writeBinaryCompressed (buf, *cloud);
PCL_INFO ("Wrote a cloud with %zu (%ux%u) points in %s.\n",
cloud->size (), cloud->width, cloud->height, buf);
++i;
}
/*
* callback for localized point clouds
*/
void GridConstructionNode::scanCallback (const gu::LocalizedCloud::Ptr& loc_cloud)
{
Lock lock(mutex_);
last_cloud_=loc_cloud;
if (last_cloud_)
{
if(last_cloud_->sensor_pose.position.z == -1.0)
{
ROS_INFO("Resetting map");
clouds_.clear();
}
else
clouds_.push_back(last_cloud_);
last_cloud_.reset();
}
}
void GridConstructionNode::buildGrid (const ros::WallTimerEvent& scan)
{
if (last_cloud_) {
{
Lock lock(mutex_);
clouds_.push_back(last_cloud_);
last_cloud_.reset();
}
ROS_DEBUG_NAMED ("build_grid", "Building grid with %zu scans", clouds_.size());
// Figure out current position
gm::PoseStamped identity, odom_pose;
identity.pose.orientation = tf::createQuaternionMsgFromYaw(0);
identity.header.frame_id = sensor_frame_;
identity.header.stamp = ros::Time();
tf_.transformPose(fixed_frame_, identity, odom_pose);
// Set up map dimensions
nm::MapMetaData info;
info.origin.position.x = odom_pose.pose.position.x-local_grid_size_/2;
info.origin.position.y = odom_pose.pose.position.y-local_grid_size_/2;
info.origin.orientation = tf::createQuaternionMsgFromYaw(0);
info.resolution = resolution_;
info.width = local_grid_size_/resolution_;
info.height = local_grid_size_/resolution_;
nm::OccupancyGrid fake_grid;
fake_grid.info = info;
gu::OverlayClouds overlay = gu::createCloudOverlay(fake_grid, fixed_frame_, 0.1, 10, 2);
vector<CloudConstPtr> clouds(clouds_.begin(), clouds_.end());
BOOST_FOREACH (CloudConstPtr cloud, clouds_)
gu::addCloud(&overlay, cloud);
nm::OccupancyGrid::ConstPtr grid = gu::getGrid(overlay);
ROS_DEBUG_NAMED ("build_grid", "Done building grid");
grid_pub_.publish(grid);
}
}
