std::vector<T> filter( const std::vector<T> & v, const VectorXb & x ) {
std::vector<T> r( x.cast<int>().array().sum() );
for( int i=0, j=0; i<x.size(); i++ )
if( x[i] )
r[j++] = v[i];
return r;
}
static VectorXi find( const VectorXb & mask ) {
VectorXi r( mask.cast<int>().sum() );
for( int i=0, k=0; i<mask.size(); i++ )
if( mask[i] )
r[k++] = i;
return r;
}
VectorXi IOUSet::computeTree(const VectorXb & s) const {
VectorXi r = VectorXi::Zero(parent_.size());
std::copy( s.data(), s.data()+s.size(), r.data() );
for( int i=0; i<r.size(); i++ )
if( parent_[i] >= 0 )
r[ parent_[i] ] += r[i];
return r;
}
typename pcl::PointCloud<T>::Ptr maskFilterDisorganized(typename pcl::PointCloud<T>::ConstPtr in, const VectorXb& mask) {
int n = mask.sum();
typename pcl::PointCloud<T>::Ptr out(new typename pcl::PointCloud<T>());
out->points.reserve(n);
for (int i=0; i < mask.size(); ++i) {
if (mask[i]) out->points.push_back(in->points[i]);
}
setWidthToSize(out);
return out;
}
typename pcl::PointCloud<T>::Ptr maskFilterOrganized(typename pcl::PointCloud<T>::ConstPtr in, const VectorXb& mask) {
typename pcl::PointCloud<T>::Ptr out(new typename pcl::PointCloud<T>(*in));
for (int i=0; i < mask.size(); ++i) {
if (!mask[i]) {
T& pt = out->points[i];
pt.x = NAN;
pt.y = NAN;
pt.z = NAN;
}
}
return out;
}