void DataPointsFiltersImpl<T>::MaxDensityDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
typedef typename DataPoints::View View;
typedef typename DataPoints::ConstView ConstView;
// Force densities to be computed
if (!cloud.descriptorExists("densities"))
{
throw InvalidField("MaxDensityDataPointsFilter: Error, no densities found in descriptors.");
}
const int nbPointsIn = cloud.features.cols();
View densities = cloud.getDescriptorViewByName("densities");
const T lastDensity = densities.maxCoeff();
const int nbSaturatedPts = (densities.cwise() == lastDensity).count();
// fill cloud values
int j = 0;
for (int i = 0; i < nbPointsIn; i++)
{
const T density(densities(0,i));
if (density > maxDensity)
{
const float r = (float)std::rand()/(float)RAND_MAX;
float acceptRatio = maxDensity/density;
// Handle saturation value of density
if (density == lastDensity)
{
acceptRatio = acceptRatio * (1-nbSaturatedPts/nbPointsIn);
}
if (r < acceptRatio)
{
cloud.setColFrom(j, cloud, i);
j++;
}
}
else
{
cloud.setColFrom(j, cloud, i);
j++;
}
}
cloud.conservativeResize(j);
}
void DataPointsFiltersImpl<T>::MaxQuantileOnAxisDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
if (int(dim) >= cloud.features.rows())
throw InvalidParameter((boost::format("MaxQuantileOnAxisDataPointsFilter: Error, filtering on dimension number %1%, larger than feature dimensionality %2%") % dim % cloud.features.rows()).str());
const int nbPointsIn = cloud.features.cols();
const int nbPointsOut = nbPointsIn * ratio;
// build array
vector<T> values;
values.reserve(cloud.features.cols());
for (int x = 0; x < cloud.features.cols(); ++x)
values.push_back(cloud.features(dim, x));
// get quartiles value
nth_element(values.begin(), values.begin() + (values.size() * ratio), values.end());
const T limit = values[nbPointsOut];
// copy towards beginning the elements we keep
int j = 0;
for (int i = 0; i < nbPointsIn; i++)
{
if (cloud.features(dim, i) < limit)
{
assert(j <= i);
cloud.setColFrom(j, cloud, i);
j++;
}
}
assert(j <= nbPointsOut);
cloud.conservativeResize(j);
}
void DataPointsFiltersImpl<T>::BoundingBoxDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
const int nbPointsIn = cloud.features.cols();
const int nbRows = cloud.features.rows();
int j = 0;
for (int i = 0; i < nbPointsIn; i++)
{
bool keepPt = false;
const Vector point = cloud.features.col(i);
// FIXME: improve performance by using Eigen array operations
const bool x_in = (point(0) > xMin && point(0) < xMax);
const bool y_in = (point(1) > yMin && point(1) < yMax);
const bool z_in = (point(2) > zMin && point(2) < zMax) || nbRows == 3;
const bool in_box = x_in && y_in && z_in;
if(removeInside)
keepPt = !in_box;
else
keepPt = in_box;
if(keepPt)
{
cloud.setColFrom(j, cloud, i);
j++;
}
}
cloud.conservativeResize(j);
}
void DataPointsFiltersImpl<T>::MaxDistDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
if (dim >= cloud.features.rows() - 1)
{
throw InvalidParameter(
(boost::format("MaxDistDataPointsFilter: Error, filtering on dimension number %1%, larger than authorized axis id %2%") % dim % (cloud.features.rows() - 2)).str());
}
const int nbPointsIn = cloud.features.cols();
const int nbRows = cloud.features.rows();
int j = 0;
if(dim == -1) // Euclidean distance
{
for (int i = 0; i < nbPointsIn; i++)
{
const T absMaxDist = anyabs(maxDist);
if (cloud.features.col(i).head(nbRows-1).norm() < absMaxDist)
{
cloud.setColFrom(j, cloud, i);
j++;
}
}
}
else // Single-axis distance
{
for (int i = 0; i < nbPointsIn; i++)
{
if ((cloud.features(dim, i)) < maxDist)
{
cloud.setColFrom(j, cloud, i);
j++;
}
}
}
cloud.conservativeResize(j);
}
void DataPointsFiltersImpl<T>::RandomSamplingDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
const int nbPointsIn = cloud.features.cols();
int j = 0;
for (int i = 0; i < nbPointsIn; i++)
{
const float r = (float)std::rand()/(float)RAND_MAX;
if (r < prob)
{
cloud.setColFrom(j, cloud, i);
j++;
}
}
cloud.conservativeResize(j);
}
void RemoveNaNDataPointsFilter<T>::inPlaceFilter(
DataPoints& cloud)
{
const int nbPointsIn = cloud.features.cols();
int j = 0;
for (int i = 0; i < nbPointsIn; ++i)
{
const BOOST_AUTO(colArray, cloud.features.col(i).array());
const BOOST_AUTO(hasNaN, !(colArray == colArray).all());
if (!hasNaN)
{
cloud.setColFrom(j, cloud, i);
++j;
}
}
cloud.conservativeResize(j);
}
void DataPointsFiltersImpl<T>::FixStepSamplingDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
const int iStep(step);
const int nbPointsIn = cloud.features.cols();
const int phase(rand() % iStep);
int j = 0;
for (int i = phase; i < nbPointsIn; i += iStep)
{
cloud.setColFrom(j, cloud, i);
j++;
}
cloud.conservativeResize(j);
const double deltaStep(startStep * stepMult - startStep);
step *= stepMult;
if (deltaStep < 0 && step < endStep)
step = endStep;
if (deltaStep > 0 && step > endStep)
step = endStep;
}
