template <typename VoxelT, typename WeightT> void
pcl::TSDFVolume<VoxelT, WeightT>::convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const
{
int sx = header_.resolution(0);
int sy = header_.resolution(1);
int sz = header_.resolution(2);
const int step = 2;
const int cloud_size = header_.getVolumeSize() / (step*step*step);
cloud->clear();
cloud->reserve (std::min (cloud_size/10, 500000));
int volume_idx = 0, cloud_idx = 0;
//#pragma omp parallel for // if used, increment over idx not possible! use index calculation
for (int z = 0; z < sz; z+=step)
for (int y = 0; y < sy; y+=step)
for (int x = 0; x < sx; x+=step, ++cloud_idx)
{
volume_idx = sx*sy*z + sx*y + x;
// pcl::PointXYZI &point = cloud->points[cloud_idx];
if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
continue;
pcl::PointXYZI point;
point.x = x; point.y = y; point.z = z;//*64;
point.intensity = volume_->at(volume_idx);
cloud->push_back (point);
}
// cloud->width = cloud_size;
// cloud->height = 1;
}
void DownSampler::downSamplePointsStochastic(
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& points,
const int target_num_points,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& down_sampled_points)
{
const size_t num_points = points->size();
// Check if the points are already sufficiently down-sampled.
if (target_num_points >= num_points * 0.8){
*down_sampled_points = *points;
return;
}
// Allocate space for the new points.
down_sampled_points->reserve(target_num_points);
//Just to ensure that we don't end up with 0 points, add 1 point to this
down_sampled_points->push_back((*points)[0]);
// Randomly select points with (targetNumPoints / num_points) probability.
for (size_t i = 1; i < num_points; ++i){
const pcl::PointXYZRGB& pt = (*points)[i];
if (rand() % num_points < target_num_points){
down_sampled_points->push_back(pt);
}
}
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::getCentroidCloud (pcl::PointCloud<PointSuperVoxel>::Ptr &cloud )
{
bool use_existing_points = true;
if ((cloud == 0) || (cloud->size () != this->OctreeBaseT::getLeafCount ()))
{
cloud.reset ();
cloud = boost::make_shared<pcl::PointCloud<PointSuperVoxel> > ();
cloud->reserve (this->OctreeBaseT::getLeafCount ());
use_existing_points = false;
}
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
leaf_itr = this->leaf_begin ();
LeafContainerT* leafContainer;
PointSuperVoxel point;
for ( int index = 0; leaf_itr != this->leaf_end (); ++leaf_itr,++index)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
if (!use_existing_points)
{
leafContainer->getCentroid (point);
cloud->push_back (point);
}
else
{
leafContainer->getCentroid (cloud->points[index]);
}
}
}
void DownSampler::downSamplePointsDeterministic(
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& points,
const int target_num_points,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& down_sampled_points,
const bool use_ceil)
{
const size_t num_points = points->size();
// Check if the points are already sufficiently down-sampled.
if (target_num_points >= num_points * 0.8){
*down_sampled_points = *points;
return;
}
// Select every N points to reach the target number of points.
int everyN = 0;
if (use_ceil) {
everyN = ceil(static_cast<double>(num_points) /
static_cast<double>(target_num_points));
} else {
everyN = static_cast<double>(num_points) /
static_cast<double>(target_num_points);
}
// Allocate space for the new points.
down_sampled_points->reserve(target_num_points);
//Just to ensure that we don't end up with 0 points, add 1 point to this
down_sampled_points->push_back((*points)[0]);
// Select every N points to reach the target number of points.
for (size_t i = 1; i < num_points; ++i) {
if (i % everyN == 0){
const pcl::PointXYZRGB& pt = (*points)[i];
down_sampled_points->push_back(pt);
}
}
}