void DataPointsFiltersImpl<T>::VoxelGridDataPointsFilter::inPlaceFilter(DataPoints& cloud)
{
const int numPoints(cloud.features.cols());
const int featDim(cloud.features.rows());
const int descDim(cloud.descriptors.rows());
assert (featDim == 3 || featDim == 4);
int insertDim(0);
if (averageExistingDescriptors)
{
// TODO: this should be in the form of an assert
// Validate descriptors and labels
for(unsigned int i = 0; i < cloud.descriptorLabels.size(); i++)
insertDim += cloud.descriptorLabels[i].span;
if (insertDim != descDim)
throw InvalidField("VoxelGridDataPointsFilter: Error, descriptor labels do not match descriptor data");
}
// TODO: Check that the voxel size is not too small, given the size of the data
// Calculate number of divisions along each axis
Vector minValues = cloud.features.rowwise().minCoeff();
Vector maxValues = cloud.features.rowwise().maxCoeff();
T minBoundX = minValues.x() / vSizeX;
T maxBoundX = maxValues.x() / vSizeX;
T minBoundY = minValues.y() / vSizeY;
T maxBoundY = maxValues.y() / vSizeY;
T minBoundZ = 0;
T maxBoundZ = 0;
if (featDim == 4)
{
minBoundZ = minValues.z() / vSizeZ;
maxBoundZ = maxValues.z() / vSizeZ;
}
// number of divisions is total size / voxel size voxels of equal length + 1
// with remaining space
unsigned int numDivX = 1 + maxBoundX - minBoundX;
unsigned int numDivY = 1 + maxBoundY - minBoundY;;
unsigned int numDivZ = 0;
// If a 3D point cloud
if (featDim == 4 )
numDivZ = 1 + maxBoundZ - minBoundZ;
unsigned int numVox = numDivX * numDivY;
if ( featDim == 4)
numVox *= numDivZ;
// Assume point cloud is randomly ordered
// compute a linear index of the following type
// i, j, k are the component indices
// nx, ny number of divisions in x and y components
// idx = i + j * nx + k * nx * ny
std::vector<unsigned int> indices(numPoints);
// vector to hold the first point in a voxel
// this point will be ovewritten in the input cloud with
// the output value
std::vector<Voxel>* voxels;
// try allocating vector. If too big return error
try {
voxels = new std::vector<Voxel>(numVox);
} catch (std::bad_alloc&) {
throw InvalidParameter((boost::format("VoxelGridDataPointsFilter: Memory allocation error with %1% voxels. Try increasing the voxel dimensions.") % numVox).str());
}
for (int p = 0; p < numPoints; p++ )
{
unsigned int i = floor(cloud.features(0,p)/vSizeX - minBoundX);
unsigned int j = floor(cloud.features(1,p)/vSizeY- minBoundY);
unsigned int k = 0;
unsigned int idx;
if ( featDim == 4 )
{
k = floor(cloud.features(2,p)/vSizeZ - minBoundZ);
idx = i + j * numDivX + k * numDivX * numDivY;
}
else
{
idx = i + j * numDivX;
}
unsigned int pointsInVox = (*voxels)[idx].numPoints + 1;
if (pointsInVox == 1)
{
(*voxels)[idx].firstPoint = p;
}
(*voxels)[idx].numPoints = pointsInVox;
indices[p] = idx;
}
// store which points contain voxel position
std::vector<unsigned int> pointsToKeep;
// Store voxel centroid in output
if (useCentroid)
{
// Iterate through the indices and sum values to compute centroid
for (int p = 0; p < numPoints ; p++)
{
unsigned int idx = indices[p];
unsigned int firstPoint = (*voxels)[idx].firstPoint;
// If this is the first point in the voxel, leave as is
// if not sum up this point for centroid calculation
if (firstPoint != p)
{
// Sum up features and descriptors (if we are also averaging descriptors)
for (int f = 0; f < (featDim - 1); f++ )
{
cloud.features(f,firstPoint) += cloud.features(f,p);
}
if (averageExistingDescriptors) {
for (int d = 0; d < descDim; d++)
{
cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
}
}
}
}
// Now iterating through the voxels
// Normalize sums to get centroid (average)
// Some voxels may be empty and are discarded
for( int idx = 0; idx < numVox; idx++)
{
unsigned int numPoints = (*voxels)[idx].numPoints;
unsigned int firstPoint = (*voxels)[idx].firstPoint;
if(numPoints > 0)
{
for ( int f = 0; f < (featDim - 1); f++ )
cloud.features(f,firstPoint) /= numPoints;
if (averageExistingDescriptors) {
for ( int d = 0; d < descDim; d++ )
cloud.descriptors(d,firstPoint) /= numPoints;
}
pointsToKeep.push_back(firstPoint);
}
}
}
else
{
// Although we don't sum over the features, we may still need to sum the descriptors
if (averageExistingDescriptors)
{
// Iterate through the indices and sum values to compute centroid
for (int p = 0; p < numPoints ; p++)
{
unsigned int idx = indices[p];
unsigned int firstPoint = (*voxels)[idx].firstPoint;
// If this is the first point in the voxel, leave as is
// if not sum up this point for centroid calculation
if (firstPoint != p)
{
for (int d = 0; d < descDim; d++ )
{
cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
}
}
}
}
for (int idx = 0; idx < numVox; idx++)
{
unsigned int numPoints = (*voxels)[idx].numPoints;
unsigned int firstPoint = (*voxels)[idx].firstPoint;
if (numPoints > 0)
{
// get back voxel indices in grid format
// If we are in the last division, the voxel is smaller in size
// We adjust the center as from the end of the last voxel to the bounding area
unsigned int i = 0;
unsigned int j = 0;
unsigned int k = 0;
if (featDim == 4)
{
k = idx / (numDivX * numDivY);
if (k == numDivZ)
cloud.features(3,firstPoint) = maxValues.z() - (k-1) * vSizeZ/2;
else
cloud.features(3,firstPoint) = k * vSizeZ + vSizeZ/2;
}
j = (idx - k * numDivX * numDivY) / numDivX;
if (j == numDivY)
cloud.features(2,firstPoint) = maxValues.y() - (j-1) * vSizeY/2;
else
cloud.features(2,firstPoint) = j * vSizeY + vSizeY / 2;
i = idx - k * numDivX * numDivY - j * numDivX;
if (i == numDivX)
cloud.features(1,firstPoint) = maxValues.x() - (i-1) * vSizeX/2;
else
cloud.features(1,firstPoint) = i * vSizeX + vSizeX / 2;
// Descriptors : normalize if we are averaging or keep as is
if (averageExistingDescriptors) {
for ( int d = 0; d < descDim; d++ )
cloud.descriptors(d,firstPoint) /= numPoints;
}
pointsToKeep.push_back(firstPoint);
}
}
}
// deallocate memory for voxels information
delete voxels;
// Move the points to be kept to the start
// Bring the data we keep to the front of the arrays then
// wipe the leftover unused space.
std::sort(pointsToKeep.begin(), pointsToKeep.end());
int numPtsOut = pointsToKeep.size();
for (int i = 0; i < numPtsOut; i++){
int k = pointsToKeep[i];
assert(i <= k);
cloud.features.col(i) = cloud.features.col(k);
if (cloud.descriptors.rows() != 0)
cloud.descriptors.col(i) = cloud.descriptors.col(k);
}
cloud.features.conservativeResize(Eigen::NoChange, numPtsOut);
if (cloud.descriptors.rows() != 0)
cloud.descriptors.conservativeResize(Eigen::NoChange, numPtsOut);
}
