void OctreeChangePublisher::cloud_cb(const sensor_msgs::PointCloud2 &pc)
{
if(pc.fields.size() <= 0){
return;
}
boost::mutex::scoped_lock lock(mtx_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
std::vector<int> indices;
pcl::fromROSMsg(pc, *cloud);
octree_->setInputCloud (cloud);
octree_->addPointsFromInputCloud ();
if (counter_ != 0){
boost::shared_ptr<std::vector<int> > newPointIdxVector (new std::vector<int>);
octree_->getPointIndicesFromNewVoxels (*newPointIdxVector, noise_filter_);
filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGB>);
filtered_cloud->points.reserve(newPointIdxVector->size());
for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
filtered_cloud->points.push_back(cloud->points[*it]);
sensor_msgs::PointCloud2 octree_change_pointcloud2;
pcl::toROSMsg(*filtered_cloud, octree_change_pointcloud2);
octree_change_pointcloud2.header = pc.header;
octree_change_pointcloud2.is_dense = false;
diff_pub_.publish(octree_change_pointcloud2);
}
octree_->switchBuffers ();
counter_++;
}
void
cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
std::cerr << cloud->points.size() << " -- ";
// assign point cloud to octree
octree->setInputCloud (cloud);
// add points from cloud to octree
octree->addPointsFromInputCloud ();
std::cerr << octree->getLeafCount() << " -- ";
boost::shared_ptr<std::vector<int> > newPointIdxVector (new std::vector<int>);
// get a vector of new points, which did not exist in previous buffer
octree->getPointIndicesFromNewVoxels (*newPointIdxVector, noise_filter_);
std::cerr << newPointIdxVector->size() << std::endl;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr filtered_cloud;
switch (mode_)
{
case REDDIFF_MODE:
filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA> (*cloud));
filtered_cloud->points.reserve(newPointIdxVector->size());
for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
filtered_cloud->points[*it].rgba = 255<<16;
if (!viewer.wasStopped())
viewer.showCloud (filtered_cloud);
break;
case ONLYDIFF_MODE:
filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
filtered_cloud->points.reserve(newPointIdxVector->size());
for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
filtered_cloud->points.push_back(cloud->points[*it]);
if (!viewer.wasStopped())
viewer.showCloud (filtered_cloud);
break;
}
// switch buffers - reset tree
octree->switchBuffers ();
}
void OctreeChangePublisher::cloud_cb(const sensor_msgs::PointCloud2 &pc)
{
if(pc.fields.size() <= 0){
return;
}
if(counter_ > loop_rate_ && counter_ % loop_rate_ != 0 ){
sensor_msgs::PointCloud2 octree_change_pointcloud2;
pcl::toROSMsg(*filtered_cloud, octree_change_pointcloud2);
octree_change_pointcloud2.header = pc.header;
octree_change_pointcloud2.is_dense = false;
diff_pub_.publish(octree_change_pointcloud2);
counter_++;
return;
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
std::vector<int> indices;
pcl::fromROSMsg(pc, *cloud);
// assign point cloud to octree
octree_->setInputCloud (cloud);
// add points from cloud to octree
octree_->addPointsFromInputCloud ();
if (counter_ != 0){
boost::shared_ptr<std::vector<int> > newPointIdxVector (new std::vector<int>);
// get a vector of new points, which did not exist in previous buffer
octree_->getPointIndicesFromNewVoxels (*newPointIdxVector, noise_filter_);
filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
filtered_cloud->points.reserve(newPointIdxVector->size());
for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
filtered_cloud->points.push_back(cloud->points[*it]);
//publish
sensor_msgs::PointCloud2 octree_change_pointcloud2;
pcl::toROSMsg(*filtered_cloud, octree_change_pointcloud2);
octree_change_pointcloud2.header = pc.header;
octree_change_pointcloud2.is_dense = false;
diff_pub_.publish(octree_change_pointcloud2);
}
// switch buffers - reset tree
octree_->switchBuffers ();
counter_++;
}
