VoxelGrid.cpp
Go to the documentation of this file.
00001 // kate: replace-tabs off; indent-width 4; indent-mode normal
00002 // vim: ts=4:sw=4:noexpandtab
00003 /*
00004 
00005 Copyright (c) 2010--2018,
00006 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
00007 You can contact the authors at <f dot pomerleau at gmail dot com> and
00008 <stephane at magnenat dot net>
00009 
00010 All rights reserved.
00011 
00012 Redistribution and use in source and binary forms, with or without
00013 modification, are permitted provided that the following conditions are met:
00014     * Redistributions of source code must retain the above copyright
00015       notice, this list of conditions and the following disclaimer.
00016     * Redistributions in binary form must reproduce the above copyright
00017       notice, this list of conditions and the following disclaimer in the
00018       documentation and/or other materials provided with the distribution.
00019     * Neither the name of the <organization> nor the
00020       names of its contributors may be used to endorse or promote products
00021       derived from this software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 
00034 */
00035 #include "VoxelGrid.h"
00036 
00037 
00038 // VoxelGridDataPointsFilter
00039 template <typename T>
00040 VoxelGridDataPointsFilter<T>::VoxelGridDataPointsFilter() :
00041         vSizeX(1),
00042         vSizeY(1),
00043         vSizeZ(1),
00044         useCentroid(true),
00045         averageExistingDescriptors(true) 
00046 {
00047 }
00048 
00049 template <typename T>
00050 VoxelGridDataPointsFilter<T>::VoxelGridDataPointsFilter(const Parameters& params) :
00051         PointMatcher<T>::DataPointsFilter("VoxelGridDataPointsFilter", 
00052                 VoxelGridDataPointsFilter::availableParameters(), params),
00053         vSizeX(Parametrizable::get<T>("vSizeX")),
00054         vSizeY(Parametrizable::get<T>("vSizeY")),
00055         vSizeZ(Parametrizable::get<T>("vSizeZ")),
00056         useCentroid(Parametrizable::get<bool>("useCentroid")),
00057         averageExistingDescriptors(Parametrizable::get<bool>("averageExistingDescriptors"))
00058 {
00059 }
00060 
00061 template <typename T>
00062 typename PointMatcher<T>::DataPoints
00063 VoxelGridDataPointsFilter<T>::filter(const DataPoints& input)
00064 {
00065         DataPoints output(input);
00066         inPlaceFilter(output);
00067         return output;
00068 }
00069 
00070 template <typename T>
00071 void VoxelGridDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
00072 {
00073         const unsigned int numPoints(cloud.features.cols());
00074         const int featDim(cloud.features.rows());
00075         const int descDim(cloud.descriptors.rows());
00076         const int timeDim(cloud.times.rows());
00077         const unsigned int labelDim(cloud.descriptorLabels.size());
00078 
00079         assert (featDim == 3 || featDim == 4);
00080 
00081         int insertDim(0);
00082         if (averageExistingDescriptors)
00083         {
00084                 // TODO: this should be in the form of an assert
00085                 // Validate descriptors and labels
00086                 for(unsigned int i = 0; i < labelDim ; ++i)
00087                         insertDim += cloud.descriptorLabels[i].span;
00088                 if (insertDim != descDim)
00089                         throw InvalidField("VoxelGridDataPointsFilter: Error, descriptor labels do not match descriptor data");
00090                 //TODO: timeDim too?
00091         }
00092 
00093         // TODO: Check that the voxel size is not too small, given the size of the data
00094         // TODO: Check if sizes are positive
00095 
00096         // Calculate number of divisions along each axis
00097         Vector minValues = cloud.features.rowwise().minCoeff();
00098         Vector maxValues = cloud.features.rowwise().maxCoeff();
00099 
00100         const T minBoundX = minValues.x() / vSizeX;
00101         const T maxBoundX = maxValues.x() / vSizeX;
00102         const T minBoundY = minValues.y() / vSizeY;
00103         const T maxBoundY = maxValues.y() / vSizeY;
00104         T minBoundZ = 0;
00105         T maxBoundZ = 0;
00106 
00107         if (featDim == 4)
00108         {
00109                 minBoundZ = minValues.z() / vSizeZ;
00110                 maxBoundZ = maxValues.z() / vSizeZ;
00111         }
00112 
00113         // number of divisions is total size / voxel size voxels of equal length + 1
00114         // with remaining space
00115         const unsigned int numDivX = 1 + maxBoundX - minBoundX;
00116         const unsigned int numDivY = 1 + maxBoundY - minBoundY;;
00117         unsigned int numDivZ = 0;
00118 
00119         // If a 3D point cloud
00120         if (featDim == 4 )
00121           numDivZ = 1 + maxBoundZ - minBoundZ;
00122 
00123         unsigned int numVox = numDivX * numDivY;
00124         if (featDim == 4)
00125                 numVox *= numDivZ;
00126         
00127         if(numVox == 0)
00128         {
00129                 throw InvalidParameter("VoxelGridDataPointsFilter: The number of voxel couldn't be computed. There might be NaNs in the feature matrix. Use the fileter RemoveNaNDataPointsFilter before this one if it's the case.");
00130         }
00131 
00132         // Assume point cloud is randomly ordered
00133         // compute a linear index of the following type
00134         // i, j, k are the component indices
00135         // nx, ny number of divisions in x and y components
00136         // idx = i + j * nx + k * nx * ny
00137         std::vector<unsigned int> indices(numPoints);
00138 
00139         // vector to hold the first point in a voxel
00140         // this point will be ovewritten in the input cloud with
00141         // the output value
00142 
00143         std::vector<Voxel> voxels;
00144 
00145         // try allocating vector. If too big return error
00146         try 
00147         {
00148                 voxels = std::vector<Voxel>(numVox);
00149         } 
00150         catch (std::bad_alloc&) 
00151         {
00152                 throw InvalidParameter((boost::format("VoxelGridDataPointsFilter: Memory allocation error with %1% voxels.  Try increasing the voxel dimensions.") % numVox).str());
00153         }
00154 
00155         for (unsigned int p = 0; p < numPoints; ++p)
00156         {       
00157                 const unsigned int i = floor(cloud.features(0,p)/vSizeX - minBoundX);
00158                 const unsigned int j = floor(cloud.features(1,p)/vSizeY- minBoundY);
00159                 unsigned int k = 0;
00160                 unsigned int idx = 0;
00161                 if ( featDim == 4 )
00162                 {
00163                         k = floor(cloud.features(2,p)/vSizeZ - minBoundZ);
00164                         idx = i + j * numDivX + k * numDivX * numDivY;
00165                 }
00166                 else
00167                 {
00168                         idx = i + j * numDivX;
00169                 }
00170 
00171                 const unsigned int pointsInVox = voxels[idx].numPoints + 1;
00172 
00173                 if (pointsInVox == 1)
00174                 {
00175                         voxels[idx].firstPoint = p;
00176                 }
00177 
00178                 voxels[idx].numPoints = pointsInVox;
00179 
00180                 indices[p] = idx;
00181 
00182         }
00183 
00184 
00185         // store which points contain voxel position
00186         std::vector<unsigned int> pointsToKeep;
00187 
00188         // Store voxel centroid in output
00189         if (useCentroid)
00190         {
00191                 // Iterate through the indices and sum values to compute centroid
00192                 for (unsigned int p = 0; p < numPoints ; ++p)
00193                 {
00194                         const unsigned int idx = indices[p];
00195                         const unsigned int firstPoint = voxels[idx].firstPoint;
00196 
00197                         // If this is the first point in the voxel, leave as is
00198                         // if not sum up this point for centroid calculation
00199                         if (firstPoint != p)
00200                         {
00201                                 // Sum up features and descriptors (if we are also averaging descriptors)
00202 
00203                                 for (int f = 0; f < (featDim - 1); ++f)
00204                                 {
00205                                         cloud.features(f,firstPoint) += cloud.features(f,p);
00206                                 }
00207 
00208                                 if (averageExistingDescriptors) 
00209                                 {
00210                                         for (int d = 0; d < descDim; ++d)
00211                                         {
00212                                                 cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
00213                                         }
00214                                         for (int d = 0; d < timeDim; ++d)
00215                                         {
00216                                                 cloud.times(d,firstPoint) += cloud.times(d,p);
00217                                         }
00218                                 }
00219                         }
00220                 }
00221 
00222                 // Now iterating through the voxels
00223                 // Normalize sums to get centroid (average)
00224                 // Some voxels may be empty and are discarded
00225                 for(unsigned int idx = 0; idx < numVox; ++idx)
00226                 {
00227                         const unsigned int numPoints = voxels[idx].numPoints;
00228                         const unsigned int firstPoint = voxels[idx].firstPoint;
00229                         if(numPoints > 0)
00230                         {
00231                                 for (int f = 0; f < (featDim - 1); ++f)
00232                                         cloud.features(f,firstPoint) /= numPoints;
00233 
00234                                 if (averageExistingDescriptors) 
00235                                 {
00236                                         for ( int d = 0; d < descDim; ++d )
00237                                                 cloud.descriptors(d,firstPoint) /= numPoints;
00238                                         for ( int d = 0; d < timeDim; ++d )
00239                                                 cloud.times(d,firstPoint) /= numPoints;
00240                                 }
00241                                 pointsToKeep.push_back(firstPoint);
00242                         }
00243                 }
00244         }
00245         else
00246         {
00247                 // Although we don't sum over the features, we may still need to sum the descriptors
00248                 if (averageExistingDescriptors)
00249                 {
00250                         // Iterate through the indices and sum values to compute centroid
00251                         for (unsigned int p = 0; p < numPoints ; ++p)
00252                         {
00253                                 const unsigned int idx = indices[p];
00254                                 const unsigned int firstPoint = voxels[idx].firstPoint;
00255 
00256                                 // If this is the first point in the voxel, leave as is
00257                                 // if not sum up this point for centroid calculation
00258                                 if (firstPoint != p)
00259                                 {
00260                                         for (int d = 0; d < descDim; ++d)
00261                                         {
00262                                                 cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
00263                                         }
00264                                         for (int d = 0; d < timeDim; ++d)
00265                                         {
00266                                                 cloud.times(d,firstPoint) += cloud.times(d,p);
00267                                         }
00268                                 }
00269                         }
00270                 }
00271 
00272                 for (unsigned int idx = 0; idx < numVox; ++idx)
00273                 {
00274                         const unsigned int numPoints = voxels[idx].numPoints;
00275                         const unsigned int firstPoint = voxels[idx].firstPoint;
00276 
00277                         if (numPoints > 0)
00278                         {
00279                                 // get back voxel indices in grid format
00280                                 // If we are in the last division, the voxel is smaller in size
00281                                 // We adjust the center as from the end of the last voxel to the bounding area
00282                                 unsigned int i = 0;
00283                                 unsigned int j = 0;
00284                                 unsigned int k = 0;
00285                                 if (featDim == 4)
00286                                 {
00287                                         k = idx / (numDivX * numDivY);
00288                                         if (k == numDivZ)
00289                                                 cloud.features(3,firstPoint) = maxValues.z() - (k-1) * vSizeZ/2;
00290                                         else
00291                                                 cloud.features(3,firstPoint) = k * vSizeZ + vSizeZ/2;
00292                                 }
00293 
00294                                 j = (idx - k * numDivX * numDivY) / numDivX;
00295                                 if (j == numDivY)
00296                                         cloud.features(2,firstPoint) = maxValues.y() - (j-1) * vSizeY/2;
00297                                 else
00298                                         cloud.features(2,firstPoint) = j * vSizeY + vSizeY / 2;
00299 
00300                                 i = idx - k * numDivX * numDivY - j * numDivX;
00301                                 if (i == numDivX)
00302                                         cloud.features(1,firstPoint) = maxValues.x() - (i-1) * vSizeX/2;
00303                                 else
00304                                         cloud.features(1,firstPoint) = i * vSizeX + vSizeX / 2;
00305 
00306                                 // Descriptors : normalize if we are averaging or keep as is
00307                                 if (averageExistingDescriptors) 
00308                                 {
00309                                         for ( int d = 0; d < descDim; ++d)
00310                                                 cloud.descriptors(d,firstPoint) /= numPoints;
00311                                         for ( int d = 0; d < timeDim; ++d)
00312                                                 cloud.times(d,firstPoint) /= numPoints;
00313                                 }
00314                                 pointsToKeep.push_back(firstPoint);
00315                         }
00316                 }
00317         }
00318 
00319         // Move the points to be kept to the start
00320         // Bring the data we keep to the front of the arrays then
00321         // wipe the leftover unused space.
00322         std::sort(pointsToKeep.begin(), pointsToKeep.end());
00323         int numPtsOut = pointsToKeep.size();
00324         for (int i = 0; i < numPtsOut; ++i)
00325         {
00326                 const int k = pointsToKeep[i];
00327                 assert(i <= k);
00328                 cloud.features.col(i) = cloud.features.col(k);
00329                 if (cloud.descriptors.rows() != 0)
00330                         cloud.descriptors.col(i) = cloud.descriptors.col(k);
00331                 if (cloud.times.rows() != 0)
00332                         cloud.times.col(i) = cloud.times.col(k);
00333 
00334         }
00335         
00336         cloud.conservativeResize(numPtsOut);
00337         
00338         //cloud.features.conservativeResize(Eigen::NoChange, numPtsOut);
00339         //
00340         //if (cloud.descriptors.rows() != 0)
00341         //      cloud.descriptors.conservativeResize(Eigen::NoChange, numPtsOut);
00342         //if (cloud.times.rows() != 0)
00343         //      cloud.times.conservativeResize(Eigen::NoChange, numPtsOut)
00344 }
00345 
00346 template struct VoxelGridDataPointsFilter<float>;
00347 template struct VoxelGridDataPointsFilter<double>;
00348 
00349 


libpointmatcher
Author(s):
autogenerated on Thu Jun 20 2019 19:51:32