DataPointsFiltersImpl.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--2012,
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 
00036 #include "DataPointsFiltersImpl.h"
00037 #include "PointMatcherPrivate.h"
00038 #include "MatchersImpl.h"
00039 #include "Functions.h"
00040 
00041 #include <algorithm>
00042 #include <boost/format.hpp>
00043 
00044 // Eigenvalues
00045 #include "Eigen/QR"
00046 #include "Eigen/Eigenvalues"
00047 
00048 using namespace std;
00049 using namespace PointMatcherSupport;
00050 
00051 // IdentityDataPointsFilter
00052 template<typename T>
00053 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::IdentityDataPointsFilter::filter(
00054         const DataPoints& input)
00055 {
00056         DataPoints output(input);
00057         inPlaceFilter(output);
00058         return output;
00059 }
00060 
00061 // In-place filter
00062 template<typename T>
00063 void DataPointsFiltersImpl<T>::IdentityDataPointsFilter::inPlaceFilter(
00064         DataPoints& cloud)
00065 {
00066 }
00067 
00068 template struct DataPointsFiltersImpl<float>::IdentityDataPointsFilter;
00069 template struct DataPointsFiltersImpl<double>::IdentityDataPointsFilter;
00070 
00071 
00072 // RemoveNaNDataPointsFilter
00073 template<typename T>
00074 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::RemoveNaNDataPointsFilter::filter(
00075         const DataPoints& input)
00076 {
00077         DataPoints output(input);
00078         inPlaceFilter(output);
00079         return output;
00080 }
00081 
00082 // In-place filter
00083 template<typename T>
00084 void DataPointsFiltersImpl<T>::RemoveNaNDataPointsFilter::inPlaceFilter(
00085         DataPoints& cloud)
00086 {
00087         const int nbPointsIn = cloud.features.cols();
00088 
00089         int j = 0;
00090         for (int i = 0; i < nbPointsIn; ++i)
00091         {
00092                 const BOOST_AUTO(colArray, cloud.features.col(i).array());
00093                 const BOOST_AUTO(hasNaN, !(colArray == colArray).all());
00094                 if (!hasNaN)
00095                 {
00096                         cloud.setColFrom(j, cloud, i);
00097                         j++;
00098                 }
00099         }
00100 
00101         cloud.conservativeResize(j);
00102 }
00103 
00104 template struct DataPointsFiltersImpl<float>::RemoveNaNDataPointsFilter;
00105 template struct DataPointsFiltersImpl<double>::RemoveNaNDataPointsFilter;
00106 
00107 
00108 // MaxDistDataPointsFilter
00109 // Constructor
00110 template<typename T>
00111 DataPointsFiltersImpl<T>::MaxDistDataPointsFilter::MaxDistDataPointsFilter(const Parameters& params):
00112         DataPointsFilter("MaxDistDataPointsFilter", MaxDistDataPointsFilter::availableParameters(), params),
00113         dim(Parametrizable::get<unsigned>("dim")),
00114         maxDist(Parametrizable::get<T>("maxDist"))
00115 {
00116 }
00117 
00118 // Compute
00119 template<typename T>
00120 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::MaxDistDataPointsFilter::filter(
00121         const DataPoints& input)
00122 {
00123         DataPoints output(input);
00124         inPlaceFilter(output);
00125         return output;
00126 }
00127 
00128 // In-place filter
00129 template<typename T>
00130 void DataPointsFiltersImpl<T>::MaxDistDataPointsFilter::inPlaceFilter(
00131         DataPoints& cloud)
00132 {
00133         if (dim >= cloud.features.rows() - 1)
00134         {
00135                 throw InvalidParameter(
00136                         (boost::format("MaxDistDataPointsFilter: Error, filtering on dimension number %1%, larger than authorized axis id %2%") % dim % (cloud.features.rows() - 2)).str());
00137         }
00138 
00139         const int nbPointsIn = cloud.features.cols();
00140         const int nbRows = cloud.features.rows();
00141 
00142         int j = 0;
00143         if(dim == -1) // Euclidean distance
00144         {
00145                 for (int i = 0; i < nbPointsIn; i++)
00146                 {
00147                         const T absMaxDist = anyabs(maxDist);
00148                         if (cloud.features.col(i).head(nbRows-1).norm() < absMaxDist)
00149                         {
00150                                 cloud.setColFrom(j, cloud, i);
00151                                 j++;
00152                         }
00153                 }
00154         }
00155         else // Single-axis distance
00156         {
00157                 for (int i = 0; i < nbPointsIn; i++)
00158                 {
00159                         if ((cloud.features(dim, i)) < maxDist)
00160                         {
00161                                 cloud.setColFrom(j, cloud, i);
00162                                 j++;
00163                         }
00164                 }
00165         }
00166 
00167         cloud.conservativeResize(j);
00168 }
00169 
00170 template struct DataPointsFiltersImpl<float>::MaxDistDataPointsFilter;
00171 template struct DataPointsFiltersImpl<double>::MaxDistDataPointsFilter;
00172 
00173 
00174 // MinDistDataPointsFilter
00175 // Constructor
00176 template<typename T>
00177 DataPointsFiltersImpl<T>::MinDistDataPointsFilter::MinDistDataPointsFilter(const Parameters& params):
00178         DataPointsFilter("MinDistDataPointsFilter", MinDistDataPointsFilter::availableParameters(), params),
00179         dim(Parametrizable::get<unsigned>("dim")),
00180         minDist(Parametrizable::get<T>("minDist"))
00181 {
00182 }
00183 
00184 // Compute
00185 template<typename T>
00186 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::MinDistDataPointsFilter::filter(
00187         const DataPoints& input)
00188 {
00189         DataPoints output(input);
00190         inPlaceFilter(output);
00191         return output;
00192 }
00193 
00194 // In-place filter
00195 template<typename T>
00196 void DataPointsFiltersImpl<T>::MinDistDataPointsFilter::inPlaceFilter(
00197         DataPoints& cloud)
00198 {
00199         if (dim >= cloud.features.rows() - 1)
00200                 throw InvalidParameter((boost::format("MinDistDataPointsFilter: Error, filtering on dimension number %1%, larger than feature dimensionality %2%") % dim % (cloud.features.rows() - 2)).str());
00201 
00202         const int nbPointsIn = cloud.features.cols();
00203         const int nbRows = cloud.features.rows();
00204 
00205         int j = 0;
00206         if(dim == -1) // Euclidean distance
00207         {
00208                 const T absMinDist = anyabs(minDist);
00209                 for (int i = 0; i < nbPointsIn; i++)
00210                 {
00211                         if (cloud.features.col(i).head(nbRows-1).norm() > absMinDist)
00212                         {
00213                                 cloud.setColFrom(j, cloud, i);
00214                                 j++;
00215                         }
00216                 }
00217         }
00218         else // Single axis distance
00219         {
00220                 for (int i = 0; i < nbPointsIn; i++)
00221                 {
00222                         if ((cloud.features(dim, i)) > minDist)
00223                         {
00224                                 cloud.setColFrom(j, cloud, i);
00225                                 j++;
00226                         }
00227                 }
00228         }
00229 
00230         cloud.conservativeResize(j);
00231 
00232 }
00233 
00234 template struct DataPointsFiltersImpl<float>::MinDistDataPointsFilter;
00235 template struct DataPointsFiltersImpl<double>::MinDistDataPointsFilter;
00236 
00237 
00238 // BoundingBoxDataPointsFilter
00239 // Constructor
00240 template<typename T>
00241 DataPointsFiltersImpl<T>::BoundingBoxDataPointsFilter::BoundingBoxDataPointsFilter(const Parameters& params):
00242         DataPointsFilter("BoundingBoxDataPointsFilter", BoundingBoxDataPointsFilter::availableParameters(), params),
00243         xMin(Parametrizable::get<T>("xMin")),
00244         xMax(Parametrizable::get<T>("xMax")),
00245         yMin(Parametrizable::get<T>("yMin")),
00246         yMax(Parametrizable::get<T>("yMax")),
00247         zMin(Parametrizable::get<T>("zMin")),
00248         zMax(Parametrizable::get<T>("zMax")),
00249         removeInside(Parametrizable::get<bool>("removeInside"))
00250 {
00251 }
00252 
00253 // Compute
00254 template<typename T>
00255 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::BoundingBoxDataPointsFilter::filter(
00256         const DataPoints& input)
00257 {
00258         DataPoints output(input);
00259         inPlaceFilter(output);
00260         return output;
00261 }
00262 
00263 // In-place filter
00264 template<typename T>
00265 void DataPointsFiltersImpl<T>::BoundingBoxDataPointsFilter::inPlaceFilter(
00266         DataPoints& cloud)
00267 {
00268         const int nbPointsIn = cloud.features.cols();
00269         const int nbRows = cloud.features.rows();
00270 
00271         int j = 0;
00272         for (int i = 0; i < nbPointsIn; i++)
00273         {
00274                 bool keepPt = false;
00275                 const Vector point = cloud.features.col(i);
00276 
00277                 // FIXME: improve performance by using Eigen array operations
00278                 const bool x_in = (point(0) > xMin && point(0) < xMax);
00279                 const bool y_in = (point(1) > yMin && point(1) < yMax);
00280                 const bool z_in = (point(2) > zMin && point(2) < zMax) || nbRows == 3;
00281                 const bool in_box = x_in && y_in && z_in;
00282 
00283                 if(removeInside)
00284                         keepPt = !in_box;
00285                 else
00286                         keepPt = in_box;
00287 
00288                 if(keepPt)
00289                 {
00290                         cloud.setColFrom(j, cloud, i);
00291                         j++;
00292                 }
00293         }
00294 
00295         cloud.conservativeResize(j);
00296 }
00297 
00298 template struct DataPointsFiltersImpl<float>::BoundingBoxDataPointsFilter;
00299 template struct DataPointsFiltersImpl<double>::BoundingBoxDataPointsFilter;
00300 
00301 
00302 // MaxQuantileOnAxisDataPointsFilter
00303 // Constructor
00304 template<typename T>
00305 DataPointsFiltersImpl<T>::MaxQuantileOnAxisDataPointsFilter::MaxQuantileOnAxisDataPointsFilter(const Parameters& params):
00306         DataPointsFilter("MaxQuantileOnAxisDataPointsFilter", MaxQuantileOnAxisDataPointsFilter::availableParameters(), params),
00307         dim(Parametrizable::get<unsigned>("dim")),
00308         ratio(Parametrizable::get<T>("ratio"))
00309 {
00310 }
00311 
00312 // Compute
00313 template<typename T>
00314 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::MaxQuantileOnAxisDataPointsFilter::filter(
00315         const DataPoints& input)
00316 {
00317         DataPoints output(input);
00318         inPlaceFilter(output);
00319         return output;
00320 }
00321 
00322 // In-place filter
00323 template<typename T>
00324 void DataPointsFiltersImpl<T>::MaxQuantileOnAxisDataPointsFilter::inPlaceFilter(
00325         DataPoints& cloud)
00326 {
00327         if (int(dim) >= cloud.features.rows())
00328                 throw InvalidParameter((boost::format("MaxQuantileOnAxisDataPointsFilter: Error, filtering on dimension number %1%, larger than feature dimensionality %2%") % dim % cloud.features.rows()).str());
00329 
00330         const int nbPointsIn = cloud.features.cols();
00331         const int nbPointsOut = nbPointsIn * ratio;
00332 
00333         // build array
00334         vector<T> values;
00335         values.reserve(cloud.features.cols());
00336         for (int x = 0; x < cloud.features.cols(); ++x)
00337                 values.push_back(cloud.features(dim, x));
00338 
00339         // get quartiles value
00340         nth_element(values.begin(), values.begin() + (values.size() * ratio), values.end());
00341         const T limit = values[nbPointsOut];
00342 
00343         // copy towards beginning the elements we keep
00344         int j = 0;
00345         for (int i = 0; i < nbPointsIn; i++)
00346         {
00347                 if (cloud.features(dim, i) < limit)
00348                 {
00349                         assert(j <= i);
00350                         cloud.setColFrom(j, cloud, i);
00351                         j++;
00352                 }
00353         }
00354         assert(j <= nbPointsOut);
00355 
00356         cloud.conservativeResize(j);
00357 
00358 }
00359 
00360 template struct DataPointsFiltersImpl<float>::MaxQuantileOnAxisDataPointsFilter;
00361 template struct DataPointsFiltersImpl<double>::MaxQuantileOnAxisDataPointsFilter;
00362 
00363 
00364 // MaxDensityDataPointsFilter
00365 // Constructor
00366 template<typename T>
00367 DataPointsFiltersImpl<T>::MaxDensityDataPointsFilter::MaxDensityDataPointsFilter(const Parameters& params):
00368         DataPointsFilter("MaxDensityDataPointsFilter", MaxDensityDataPointsFilter::availableParameters(), params),
00369         maxDensity(Parametrizable::get<T>("maxDensity"))
00370 {
00371 }
00372 
00373 // Compute
00374 template<typename T>
00375 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::MaxDensityDataPointsFilter::filter(
00376         const DataPoints& input)
00377 {
00378         DataPoints output(input);
00379         inPlaceFilter(output);
00380         return output;
00381 }
00382 
00383 // In-place filter
00384 template<typename T>
00385 void DataPointsFiltersImpl<T>::MaxDensityDataPointsFilter::inPlaceFilter(
00386         DataPoints& cloud)
00387 {
00388         typedef typename DataPoints::View View;
00389 
00390         // Force densities to be computed
00391         if (!cloud.descriptorExists("densities"))
00392         {
00393                 throw InvalidField("MaxDensityDataPointsFilter: Error, no densities found in descriptors.");
00394         }
00395 
00396         const int nbPointsIn = cloud.features.cols();
00397         View densities = cloud.getDescriptorViewByName("densities");
00398         const T lastDensity = densities.maxCoeff();
00399         const int nbSaturatedPts = (densities.array() == lastDensity).count();
00400 
00401         // fill cloud values
00402         int j = 0;
00403         for (int i = 0; i < nbPointsIn; i++)
00404         {
00405                 const T density(densities(0,i));
00406                 if (density > maxDensity)
00407                 {
00408                         const float r = (float)std::rand()/(float)RAND_MAX;
00409                         float acceptRatio = maxDensity/density;
00410 
00411                         // Handle saturation value of density
00412                         if (density == lastDensity)
00413                         {
00414                                 acceptRatio = acceptRatio * (1-nbSaturatedPts/nbPointsIn);
00415                         }
00416 
00417                         if (r < acceptRatio)
00418                         {
00419                                 cloud.setColFrom(j, cloud, i);
00420                                 j++;
00421                         }
00422                 }
00423                 else
00424                 {
00425                         cloud.setColFrom(j, cloud, i);
00426                         j++;
00427                 }
00428         }
00429 
00430         cloud.conservativeResize(j);
00431 }
00432 
00433 template struct DataPointsFiltersImpl<float>::MaxDensityDataPointsFilter;
00434 template struct DataPointsFiltersImpl<double>::MaxDensityDataPointsFilter;
00435 
00436 
00437 // SurfaceNormalDataPointsFilter
00438 // Constructor
00439 template<typename T>
00440 DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::SurfaceNormalDataPointsFilter(const Parameters& params):
00441         DataPointsFilter("SurfaceNormalDataPointsFilter", SurfaceNormalDataPointsFilter::availableParameters(), params),
00442         knn(Parametrizable::get<int>("knn")),
00443         epsilon(Parametrizable::get<T>("epsilon")),
00444         keepNormals(Parametrizable::get<bool>("keepNormals")),
00445         keepDensities(Parametrizable::get<bool>("keepDensities")),
00446         keepEigenValues(Parametrizable::get<bool>("keepEigenValues")),
00447         keepEigenVectors(Parametrizable::get<bool>("keepEigenVectors")),
00448         keepMatchedIds(Parametrizable::get<bool>("keepMatchedIds"))
00449 {
00450 }
00451 
00452 // Compute
00453 template<typename T>
00454 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::filter(
00455         const DataPoints& input)
00456 {
00457         DataPoints output(input);
00458         inPlaceFilter(output);
00459         return output;
00460 }
00461 
00462 // In-place filter
00463 template<typename T>
00464 void DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::inPlaceFilter(
00465         DataPoints& cloud)
00466 {
00467         typedef typename DataPoints::View View;
00468         typedef typename DataPoints::Label Label;
00469         typedef typename DataPoints::Labels Labels;
00470         typedef typename MatchersImpl<T>::KDTreeMatcher KDTreeMatcher;
00471         typedef typename PointMatcher<T>::Matches Matches;
00472 
00473         const int pointsCount(cloud.features.cols());
00474         const int featDim(cloud.features.rows());
00475         const int descDim(cloud.descriptors.rows());
00476 
00477         // Validate descriptors and labels
00478         int insertDim(0);
00479         for(unsigned int i = 0; i < cloud.descriptorLabels.size(); i++)
00480                 insertDim += cloud.descriptorLabels[i].span;
00481         if (insertDim != descDim)
00482                 throw InvalidField("SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
00483 
00484         // Reserve memory for new descriptors
00485         const int dimNormals(featDim-1);
00486         const int dimDensities(1);
00487         const int dimEigValues(featDim-1);
00488         const int dimEigVectors((featDim-1)*(featDim-1));
00489         //const int dimMatchedIds(knn);
00490 
00491         boost::optional<View> normals;
00492         boost::optional<View> densities;
00493         boost::optional<View> eigenValues;
00494         boost::optional<View> eigenVectors;
00495         boost::optional<View> matchedValues;
00496 
00497         Labels cloudLabels;
00498         if (keepNormals)
00499                 cloudLabels.push_back(Label("normals", dimNormals));
00500         if (keepDensities)
00501                 cloudLabels.push_back(Label("densities", dimDensities));
00502         if (keepEigenValues)
00503                 cloudLabels.push_back(Label("eigValues", dimEigValues));
00504         if (keepEigenVectors)
00505                 cloudLabels.push_back(Label("eigVectors", dimEigVectors));
00506         cloud.allocateDescriptors(cloudLabels);
00507 
00508         if (keepNormals)
00509                 normals = cloud.getDescriptorViewByName("normals");
00510         if (keepDensities)
00511                 densities = cloud.getDescriptorViewByName("densities");
00512         if (keepEigenValues)
00513                 eigenValues = cloud.getDescriptorViewByName("eigValues");
00514         if (keepEigenVectors)
00515                 eigenVectors = cloud.getDescriptorViewByName("eigVectors");
00516         // TODO: implement keepMatchedIds
00517 //      if (keepMatchedIds)
00518 //      {
00519 //              cloud.allocateDescriptor("normals", dimMatchedIds);
00520 //              matchedValues = cloud.getDescriptorViewByName("normals");
00521 //      }
00522 
00523         // Build kd-tree
00524         Parametrizable::Parameters param;
00525         boost::assign::insert(param) ( "knn", toParam(knn) );
00526         boost::assign::insert(param) ( "epsilon", toParam(epsilon) );
00527         KDTreeMatcher matcher(param);
00528         matcher.init(cloud);
00529 
00530         Matches matches(typename Matches::Dists(knn, pointsCount), typename Matches::Ids(knn, pointsCount));
00531         matches = matcher.findClosests(cloud);
00532 
00533         // Search for surrounding points and compute descriptors
00534         int degenerateCount(0);
00535         for (int i = 0; i < pointsCount; ++i)
00536         {
00537                 // Mean of nearest neighbors (NN)
00538                 Matrix d(featDim-1, knn);
00539                 for(int j = 0; j < int(knn); j++)
00540                 {
00541                         const int refIndex(matches.ids(j,i));
00542                         d.col(j) = cloud.features.block(0, refIndex, featDim-1, 1);
00543                 }
00544 
00545                 const Vector mean = d.rowwise().sum() / T(knn);
00546                 const Matrix NN = d.colwise() - mean;
00547 
00548                 const Matrix C(NN * NN.transpose());
00549                 Vector eigenVa = Vector::Identity(featDim-1, 1);
00550                 Matrix eigenVe = Matrix::Identity(featDim-1, featDim-1);
00551                 // Ensure that the matrix is suited for eigenvalues calculation
00552                 if(keepNormals || keepEigenValues || keepEigenVectors)
00553                 {
00554                         if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
00555                         {
00556                                 const Eigen::EigenSolver<Matrix> solver(C);
00557                                 eigenVa = solver.eigenvalues().real();
00558                                 eigenVe = solver.eigenvectors().real();
00559                         }
00560                         else
00561                         {
00562                                 //std::cout << "WARNING: Matrix C needed for eigen decomposition is degenerated. Expected cause: no noise in data" << std::endl;
00563                                 ++degenerateCount;
00564                         }
00565                 }
00566 
00567                 if(keepNormals)
00568                         normals->col(i) = computeNormal(eigenVa, eigenVe);
00569                 if(keepDensities)
00570                         (*densities)(0, i) = computeDensity(NN);
00571                 if(keepEigenValues)
00572                         eigenValues->col(i) = eigenVa;
00573                 if(keepEigenVectors)
00574                         eigenVectors->col(i) = serializeEigVec(eigenVe);
00575         }
00576         if (degenerateCount)
00577         {
00578                 LOG_WARNING_STREAM("WARNING: Matrix C needed for eigen decomposition was degenerated in " << degenerateCount << " points over " << pointsCount << " (" << float(degenerateCount)*100.f/float(pointsCount) << " %)");
00579         }
00580 
00581 }
00582 
00583 template<typename T>
00584 typename PointMatcher<T>::Vector DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::computeNormal(const Vector eigenVa, const Matrix eigenVe)
00585 {
00586         // Keep the smallest eigenvector as surface normal
00587         int smallestId(0);
00588         T smallestValue(numeric_limits<T>::max());
00589         for(int j = 0; j < eigenVe.cols(); j++)
00590         {
00591                 if (eigenVa(j) < smallestValue)
00592                 {
00593                         smallestId = j;
00594                         smallestValue = eigenVa(j);
00595                 }
00596         }
00597 
00598         return eigenVe.col(smallestId);
00599 }
00600 
00601 template<typename T>
00602 typename PointMatcher<T>::Vector DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::serializeEigVec(const Matrix eigenVe)
00603 {
00604         // serialize row major
00605         const int eigenVeDim = eigenVe.cols();
00606         Vector output(eigenVeDim*eigenVeDim);
00607         for(int k=0; k < eigenVe.cols(); k++)
00608         {
00609                 output.segment(k*eigenVeDim, eigenVeDim) = 
00610                         eigenVe.row(k).transpose();
00611         }
00612 
00613         return output;
00614 }
00615 
00616 template<typename T>
00617 T DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::computeDensity(const Matrix NN)
00618 {
00619         //volume in meter
00620         T volume = (4./3.)*M_PI*std::pow(NN.colwise().norm().maxCoeff(), 3);
00621 
00622         //volume in decimeter
00623         //T volume = (4./3.)*M_PI*std::pow(NN.colwise().norm().maxCoeff()*10.0, 3);
00624         //const T minVolume = 4.18e-9; // minimum of volume of one millimeter radius
00625         //const T minVolume = 0.42; // minimum of volume of one centimeter radius (in dm^3)
00626 
00627         //if(volume < minVolume)
00628         //      volume = minVolume;
00629 
00630         return T(NN.cols())/(volume);
00631 }
00632 
00633 template struct DataPointsFiltersImpl<float>::SurfaceNormalDataPointsFilter;
00634 template struct DataPointsFiltersImpl<double>::SurfaceNormalDataPointsFilter;
00635 
00636 
00637 // SamplingSurfaceNormalDataPointsFilter
00638 
00639 // Constructor
00640 template<typename T>
00641 DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter::SamplingSurfaceNormalDataPointsFilter(const Parameters& params):
00642         DataPointsFilter("SamplingSurfaceNormalDataPointsFilter", SamplingSurfaceNormalDataPointsFilter::availableParameters(), params),
00643         ratio(Parametrizable::get<T>("ratio")),
00644         knn(Parametrizable::get<int>("knn")),
00645         samplingMethod(Parametrizable::get<int>("samplingMethod")),
00646         maxBoxDim(Parametrizable::get<T>("maxBoxDim")),
00647         averageExistingDescriptors(Parametrizable::get<bool>("averageExistingDescriptors")),
00648         keepNormals(Parametrizable::get<bool>("keepNormals")),
00649         keepDensities(Parametrizable::get<bool>("keepDensities")),
00650         keepEigenValues(Parametrizable::get<bool>("keepEigenValues")),
00651         keepEigenVectors(Parametrizable::get<bool>("keepEigenVectors"))
00652 {
00653 }
00654 
00655 // Compute
00656 template<typename T>
00657 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter::filter(
00658         const DataPoints& input)
00659 {
00660         DataPoints output(input);
00661         inPlaceFilter(output);
00662         return output;
00663 }
00664 
00665 // In-place filter
00666 template<typename T>
00667 void DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter::inPlaceFilter(
00668         DataPoints& cloud)
00669 {
00670         typedef typename DataPoints::View View;
00671         typedef typename DataPoints::Label Label;
00672         typedef typename DataPoints::Labels Labels;
00673 
00674         const int pointsCount(cloud.features.cols());
00675         const int featDim(cloud.features.rows());
00676         const int descDim(cloud.descriptors.rows());
00677 
00678         int insertDim(0);
00679         if (averageExistingDescriptors)
00680         {
00681                 // TODO: this should be in the form of an assert
00682                 // Validate descriptors and labels
00683                 for(unsigned int i = 0; i < cloud.descriptorLabels.size(); i++)
00684                         insertDim += cloud.descriptorLabels[i].span;
00685                 if (insertDim != descDim)
00686                         throw InvalidField("SamplingSurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
00687         }
00688 
00689         // Compute space requirement for new descriptors
00690         const int dimNormals(featDim-1);
00691         const int dimDensities(1);
00692         const int dimEigValues(featDim-1);
00693         const int dimEigVectors((featDim-1)*(featDim-1));
00694 
00695         // Allocate space for new descriptors
00696         Labels cloudLabels;
00697         if (keepNormals)
00698                 cloudLabels.push_back(Label("normals", dimNormals));
00699         if (keepDensities)
00700                 cloudLabels.push_back(Label("densities", dimDensities));
00701         if (keepEigenValues)
00702                 cloudLabels.push_back(Label("eigValues", dimEigValues));
00703         if (keepEigenVectors)
00704                 cloudLabels.push_back(Label("eigVectors", dimEigVectors));
00705         cloud.allocateDescriptors(cloudLabels);
00706 
00707         // we keep build data on stack for reentrant behaviour
00708         View cloudExistingDescriptors(cloud.descriptors.block(0,0,cloud.descriptors.rows(),cloud.descriptors.cols()));
00709         BuildData buildData(cloud.features, cloud.descriptors);
00710 
00711         // get views
00712         if (keepNormals)
00713                 buildData.normals = cloud.getDescriptorViewByName("normals");
00714         if (keepDensities)
00715                 buildData.densities = cloud.getDescriptorViewByName("densities");
00716         if (keepEigenValues)
00717                 buildData.eigenValues = cloud.getDescriptorViewByName("eigValues");
00718         if (keepEigenVectors)
00719                 buildData.eigenVectors = cloud.getDescriptorViewByName("eigVectors");
00720         // build the new point cloud
00721         buildNew(
00722                 buildData,
00723                 0,
00724                 pointsCount,
00725                 cloud.features.rowwise().minCoeff(),
00726                 cloud.features.rowwise().maxCoeff()
00727         );
00728 
00729         // Bring the data we keep to the front of the arrays then
00730         // wipe the leftover unused space.
00731         std::sort(buildData.indicesToKeep.begin(), buildData.indicesToKeep.end());
00732         int ptsOut = buildData.indicesToKeep.size();
00733         for (int i = 0; i < ptsOut; i++){
00734                 int k = buildData.indicesToKeep[i];
00735                 assert(i <= k);
00736                 cloud.features.col(i) = cloud.features.col(k);
00737                 if (cloud.descriptors.rows() != 0)
00738                         cloud.descriptors.col(i) = cloud.descriptors.col(k);
00739                 if(keepNormals)
00740                         buildData.normals->col(i) = buildData.normals->col(k);
00741                 if(keepDensities)
00742                         (*buildData.densities)(0,i) = (*buildData.densities)(0,k);
00743                 if(keepEigenValues)
00744                         buildData.eigenValues->col(i) = buildData.eigenValues->col(k);
00745                 if(keepEigenVectors)
00746                         buildData.eigenVectors->col(i) = buildData.eigenVectors->col(k);
00747         }
00748         cloud.features.conservativeResize(Eigen::NoChange, ptsOut);
00749         cloud.descriptors.conservativeResize(Eigen::NoChange, ptsOut);
00750 
00751         // warning if some points were dropped
00752         if(buildData.unfitPointsCount != 0)
00753                 LOG_INFO_STREAM("  SamplingSurfaceNormalDataPointsFilter - Could not compute normal for " << buildData.unfitPointsCount << " pts.");
00754 }
00755 
00756 template<typename T>
00757 size_t argMax(const typename PointMatcher<T>::Vector& v)
00758 {
00759         T maxVal(0);
00760         size_t maxIdx(0);
00761         for (int i = 0; i < v.size(); ++i)
00762         {
00763                 if (v[i] > maxVal)
00764                 {
00765                         maxVal = v[i];
00766                         maxIdx = i;
00767                 }
00768         }
00769         return maxIdx;
00770 }
00771 
00772 template<typename T>
00773 void DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter::buildNew(BuildData& data, const int first, const int last, const Vector minValues, const Vector maxValues) const
00774 {
00775         const int count(last - first);
00776         if (count <= int(knn))
00777         {
00778                 // compute for this range
00779                 fuseRange(data, first, last);
00780                 // TODO: make another filter that creates constant-density clouds,
00781                 // typically by stopping recursion after the median of the bounding cuboid
00782                 // is below a threshold, or that the number of points falls under a threshold
00783                 return;
00784         }
00785 
00786         // find the largest dimension of the box
00787         const int cutDim = argMax<T>(maxValues - minValues);
00788 
00789         // compute number of elements
00790         const int rightCount(count/2);
00791         const int leftCount(count - rightCount);
00792         assert(last - rightCount == first + leftCount);
00793 
00794         // sort, hack std::nth_element
00795         std::nth_element(
00796                 data.indices.begin() + first,
00797                 data.indices.begin() + first + leftCount,
00798                 data.indices.begin() + last,
00799                 CompareDim(cutDim, data)
00800         );
00801 
00802         // get value
00803         const int cutIndex(data.indices[first+leftCount]);
00804         const T cutVal(data.features(cutDim, cutIndex));
00805 
00806         // update bounds for left
00807         Vector leftMaxValues(maxValues);
00808         leftMaxValues[cutDim] = cutVal;
00809         // update bounds for right
00810         Vector rightMinValues(minValues);
00811         rightMinValues[cutDim] = cutVal;
00812 
00813         // recurse
00814         buildNew(data, first, first + leftCount, minValues, leftMaxValues);
00815         buildNew(data, first + leftCount, last, rightMinValues, maxValues);
00816 }
00817 
00818 template<typename T>
00819 void DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter::fuseRange(BuildData& data, const int first, const int last) const
00820 {
00821         const int colCount(last-first);
00822         const int featDim(data.features.rows());
00823 
00824         // build nearest neighbors list
00825         Matrix d(featDim-1, colCount);
00826         for (int i = 0; i < colCount; ++i)
00827                 d.col(i) = data.features.block(0,data.indices[first+i],featDim-1, 1);
00828         const Vector box = d.rowwise().maxCoeff() - d.rowwise().minCoeff();
00829         const T boxDim(box.maxCoeff());
00830         // drop box if it is too large
00831         if (boxDim > maxBoxDim)
00832         {
00833                 data.unfitPointsCount += colCount;
00834                 return;
00835         }
00836         const Vector mean = d.rowwise().sum() / T(colCount);
00837         const Matrix NN = (d.colwise() - mean);
00838 
00839         // compute covariance
00840         const Matrix C(NN * NN.transpose());
00841         Vector eigenVa = Vector::Identity(featDim-1, 1);
00842         Matrix eigenVe = Matrix::Identity(featDim-1, featDim-1);
00843         // Ensure that the matrix is suited for eigenvalues calculation
00844         if(keepNormals || keepEigenValues || keepEigenVectors)
00845         {
00846                 if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
00847                 {
00848                         const Eigen::EigenSolver<Matrix> solver(C);
00849                         eigenVa = solver.eigenvalues().real();
00850                         eigenVe = solver.eigenvectors().real();
00851                 }
00852                 else
00853                 {
00854                         data.unfitPointsCount += colCount;
00855                         return;
00856                 }
00857         }
00858 
00859         Vector normal;
00860         if(keepNormals)
00861                 normal = SurfaceNormalDataPointsFilter::computeNormal(eigenVa, eigenVe);
00862 
00863         T densitie = 0;
00864         if(keepDensities)
00865                 densitie = SurfaceNormalDataPointsFilter::computeDensity(NN);
00866 
00867         //if(keepEigenValues) nothing to do
00868 
00869         Vector serialEigVector;
00870         if(keepEigenVectors)
00871                 serialEigVector = SurfaceNormalDataPointsFilter::serializeEigVec(eigenVe);
00872 
00873         // some safety check
00874         if(data.descriptors.rows() != 0)
00875                 assert(data.descriptors.cols() != 0);
00876 
00877         // Filter points randomly
00878         if(samplingMethod == 0)
00879         {
00880                 for(int i=0; i<colCount; i++)
00881                 {
00882                         const float r = (float)std::rand()/(float)RAND_MAX;
00883                         if(r < ratio)
00884                         {
00885                                 // Keep points with their descriptors
00886                                 int k = data.indices[first+i];
00887                                 // Mark the indices which will be part of the final data
00888                                 data.indicesToKeep.push_back(k);
00889 
00890                                 // Build new descriptors
00891                                 if(keepNormals)
00892                                         data.normals->col(k) = normal;
00893                                 if(keepDensities)
00894                                         (*data.densities)(0,k) = densitie;
00895                                 if(keepEigenValues)
00896                                         data.eigenValues->col(k) = eigenVa;
00897                                 if(keepEigenVectors)
00898                                         data.eigenVectors->col(k) = serialEigVector;
00899 
00900                         }
00901                 }
00902         }
00903         else
00904         {
00905 
00906                 int k = data.indices[first];
00907                 // Mark the indices which will be part of the final data
00908                 data.indicesToKeep.push_back(k);
00909                 data.features.col(k).topRows(featDim-1) = mean;
00910                 data.features(featDim-1, k) = 1;
00911 
00912                 if(data.descriptors.rows() != 0)
00913                 {
00914                         // average the existing descriptors
00915                         if (averageExistingDescriptors)
00916                         {
00917                                 Vector mergedDesc(Vector::Zero(data.descriptors.rows()));
00918                                 for (int i = 0; i < colCount; ++i)
00919                                         mergedDesc += data.descriptors.col(data.indices[first+i]);
00920                                 mergedDesc /= T(colCount);
00921                                 data.descriptors.col(k) = mergedDesc;
00922                         }
00923                         // else just keep the first one
00924                 }
00925 
00926                 // Build new descriptors
00927                 if(keepNormals)
00928                         data.normals->col(k) = normal;
00929                 if(keepDensities)
00930                         (*data.densities)(0,k) = densitie;
00931                 if(keepEigenValues)
00932                         data.eigenValues->col(k) = eigenVa;
00933                 if(keepEigenVectors)
00934                         data.eigenVectors->col(k) = serialEigVector;
00935 
00936         }
00937 
00938 }
00939 
00940 template struct DataPointsFiltersImpl<float>::SamplingSurfaceNormalDataPointsFilter;
00941 template struct DataPointsFiltersImpl<double>::SamplingSurfaceNormalDataPointsFilter;
00942 
00943 // OrientNormalsDataPointsFilter
00944 // Constructor
00945 template<typename T>
00946 DataPointsFiltersImpl<T>::OrientNormalsDataPointsFilter::OrientNormalsDataPointsFilter(const Parameters& params):
00947         DataPointsFilter("OrientNormalsDataPointsFilter", OrientNormalsDataPointsFilter::availableParameters(), params),
00948         towardCenter(Parametrizable::get<bool>("towardCenter"))
00949 {
00950 }
00951 
00952 // OrientNormalsDataPointsFilter
00953 // Compute
00954 template<typename T>
00955 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::OrientNormalsDataPointsFilter::filter(
00956         const DataPoints& input)
00957 {
00958         DataPoints output(input);
00959         inPlaceFilter(output);
00960         return output;
00961 }
00962 
00963 // In-place filter
00964 template<typename T>
00965 void DataPointsFiltersImpl<T>::OrientNormalsDataPointsFilter::inPlaceFilter(
00966         DataPoints& cloud)
00967 {
00968         if (!cloud.descriptorExists("normals"))
00969                 throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find normals in descriptors.");
00970         if (!cloud.descriptorExists("observationDirections"))
00971                 throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find observation directions in descriptors.");
00972 
00973         BOOST_AUTO(normals, cloud.getDescriptorViewByName("normals"));
00974         const BOOST_AUTO(observationDirections, cloud.getDescriptorViewByName("observationDirections"));
00975         assert(normals.rows() == observationDirections.rows());
00976         for (int i = 0; i < cloud.features.cols(); i++)
00977         {
00978                 // Check normal orientation
00979                 const Vector vecP = observationDirections.col(i);
00980                 const Vector vecN = normals.col(i);
00981                 const double scalar = vecP.dot(vecN);
00982 
00983                 // Swap normal
00984                 if(towardCenter)
00985                 {
00986                         if (scalar < 0)
00987                                 normals.col(i) = -vecN;
00988                 }
00989                 else
00990                 {
00991                         if (scalar > 0)
00992                                 normals.col(i) = -vecN;
00993                 }
00994         }
00995 
00996 }
00997 
00998 template struct DataPointsFiltersImpl<float>::OrientNormalsDataPointsFilter;
00999 template struct DataPointsFiltersImpl<double>::OrientNormalsDataPointsFilter;
01000 
01001 
01002 // RandomSamplingDataPointsFilter
01003 // Constructor
01004 template<typename T>
01005 DataPointsFiltersImpl<T>::RandomSamplingDataPointsFilter::RandomSamplingDataPointsFilter(const Parameters& params):
01006         DataPointsFilter("RandomSamplingDataPointsFilter", RandomSamplingDataPointsFilter::availableParameters(), params),
01007         prob(Parametrizable::get<double>("prob"))
01008 {
01009 }
01010 
01011 // Constructor
01012 template<typename T>
01013 DataPointsFiltersImpl<T>::RandomSamplingDataPointsFilter::RandomSamplingDataPointsFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params):
01014         DataPointsFilter(className, paramsDoc, params),
01015         prob(Parametrizable::get<double>("prob"))
01016 {
01017 }
01018 
01019 // Compute
01020 template<typename T>
01021 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::RandomSamplingDataPointsFilter::filter(
01022         const DataPoints& input)
01023 {
01024         DataPoints output(input);
01025         inPlaceFilter(output);
01026         return output;
01027 }
01028 
01029 // In-place filter
01030 template<typename T>
01031 void DataPointsFiltersImpl<T>::RandomSamplingDataPointsFilter::inPlaceFilter(
01032         DataPoints& cloud)
01033 {
01034         const int nbPointsIn = cloud.features.cols();
01035 
01036         int j = 0;
01037         for (int i = 0; i < nbPointsIn; i++)
01038         {
01039                 const float r = (float)std::rand()/(float)RAND_MAX;
01040                 if (r < prob)
01041                 {
01042                         cloud.setColFrom(j, cloud, i);
01043                         j++;
01044                 }
01045         }
01046 
01047         cloud.conservativeResize(j);
01048 }
01049 
01050 template struct DataPointsFiltersImpl<float>::RandomSamplingDataPointsFilter;
01051 template struct DataPointsFiltersImpl<double>::RandomSamplingDataPointsFilter;
01052 
01053 
01054 // MaxPointCountDataPointsFilter
01055 // Constructor
01056 template<typename T>
01057 DataPointsFiltersImpl<T>::MaxPointCountDataPointsFilter::MaxPointCountDataPointsFilter(const Parameters& params):
01058         DataPointsFilter("MaxPointCountDataPointsFilter", MaxPointCountDataPointsFilter::availableParameters(), params),
01059         maxCount(Parametrizable::get<unsigned>("maxCount"))
01060 {
01061         try {
01062                 seed = Parametrizable::get<unsigned>("seed");
01063         } catch (const InvalidParameter& e) {
01064                 seed = static_cast<unsigned int> (1); // rand default seed number
01065         }
01066 }
01067 
01068 // Compute
01069 template<typename T>
01070 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::MaxPointCountDataPointsFilter::filter(
01071         const DataPoints& input)
01072 {
01073         DataPoints output(input);
01074         inPlaceFilter(output);
01075         return output;
01076 }
01077 
01078 // In-place filter
01079 template<typename T>
01080 void DataPointsFiltersImpl<T>::MaxPointCountDataPointsFilter::inPlaceFilter(
01081         DataPoints& cloud)
01082 {
01083         unsigned N = static_cast<unsigned> (cloud.features.cols());
01084         if (maxCount < N) {
01085                 DataPoints cloud_filtered = cloud.createSimilarEmpty(maxCount);
01086                 std::srand(seed);
01087 
01088                 unsigned top = N - maxCount;
01089                 unsigned i = 0;
01090                 unsigned index = 0;
01091                 for (size_t n = maxCount; n >= 2; n--)
01092                 {
01093                         float V = static_cast<float>(rand () / double (RAND_MAX));
01094                         unsigned S = 0;
01095                         float quot = static_cast<float> (top) / static_cast<float> (N);
01096                         while (quot > V)
01097                         {
01098                                 S++;
01099                                 top--;
01100                                 N--;
01101                                 quot = quot * static_cast<float> (top) / static_cast<float> (N);
01102                         }
01103                         index += S;
01104                         cloud_filtered.setColFrom(i++, cloud, index++);
01105                         N--;
01106                 }
01107 
01108                 index += N * static_cast<unsigned> (static_cast<float>(rand () / double (RAND_MAX)));
01109                 cloud_filtered.setColFrom(i++, cloud, index++);
01110                 PointMatcher<T>::swapDataPoints(cloud, cloud_filtered);
01111                 cloud.conservativeResize(i);
01112         }
01113 }
01114 
01115 template struct DataPointsFiltersImpl<float>::MaxPointCountDataPointsFilter;
01116 template struct DataPointsFiltersImpl<double>::MaxPointCountDataPointsFilter;
01117 
01118 
01119 // FixStepSamplingDataPointsFilter
01120 // Constructor
01121 template<typename T>
01122 DataPointsFiltersImpl<T>::FixStepSamplingDataPointsFilter::FixStepSamplingDataPointsFilter(const Parameters& params):
01123         DataPointsFilter("FixStepSamplingDataPointsFilter", FixStepSamplingDataPointsFilter::availableParameters(), params),
01124         startStep(Parametrizable::get<unsigned>("startStep")),
01125         endStep(Parametrizable::get<unsigned>("endStep")),
01126         stepMult(Parametrizable::get<double>("stepMult")),
01127         step(startStep)
01128 {
01129         LOG_INFO_STREAM("Using FixStepSamplingDataPointsFilter with startStep=" << startStep << ", endStep=" << endStep << ", stepMult=" << stepMult);
01130 }
01131 
01132 
01133 template<typename T>
01134 void DataPointsFiltersImpl<T>::FixStepSamplingDataPointsFilter::init()
01135 {
01136         step = startStep;
01137 }
01138 
01139 // Compute
01140 template<typename T>
01141 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::FixStepSamplingDataPointsFilter::filter(
01142         const DataPoints& input)
01143 {
01144         DataPoints output(input);
01145         inPlaceFilter(output);
01146         return output;
01147 }
01148 
01149 // In-place filter
01150 template<typename T>
01151 void DataPointsFiltersImpl<T>::FixStepSamplingDataPointsFilter::inPlaceFilter(
01152         DataPoints& cloud)
01153 {
01154         const int iStep(step);
01155         const int nbPointsIn = cloud.features.cols();
01156         const int phase(rand() % iStep);
01157 
01158         int j = 0;
01159         for (int i = phase; i < nbPointsIn; i += iStep)
01160         {
01161                 cloud.setColFrom(j, cloud, i);
01162                 j++;
01163         }
01164 
01165         cloud.conservativeResize(j);
01166 
01167         const double deltaStep(startStep * stepMult - startStep);
01168         step *= stepMult;
01169         if (deltaStep < 0 && step < endStep)
01170                 step = endStep;
01171         if (deltaStep > 0 && step > endStep)
01172                 step = endStep;
01173 
01174 }
01175 
01176 template struct DataPointsFiltersImpl<float>::FixStepSamplingDataPointsFilter;
01177 template struct DataPointsFiltersImpl<double>::FixStepSamplingDataPointsFilter;
01178 
01179 
01180 // ShadowDataPointsFilter
01181 // Constructor
01182 template<typename T>
01183 DataPointsFiltersImpl<T>::ShadowDataPointsFilter::ShadowDataPointsFilter(const Parameters& params):
01184         DataPointsFilter("ShadowDataPointsFilter", ShadowDataPointsFilter::availableParameters(), params),
01185         eps(sin(Parametrizable::get<T>("eps")))
01186 {
01187         //waring: maxAngle is change to sin(maxAngle)!
01188 }
01189 
01190 
01191 // Compute
01192 template<typename T>
01193 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::ShadowDataPointsFilter::filter(
01194         const DataPoints& input)
01195 {
01196         DataPoints output(input);
01197         inPlaceFilter(output);
01198         return output;
01199 }
01200 
01201 // In-place filter
01202 template<typename T>
01203 void DataPointsFiltersImpl<T>::ShadowDataPointsFilter::inPlaceFilter(
01204         DataPoints& cloud)
01205 {
01206         // Check if normals are present
01207         if (!cloud.descriptorExists("normals"))
01208         {
01209                 throw InvalidField("ShadowDataPointsFilter, Error: cannot find normals in descriptors");
01210         }
01211 
01212         const int dim = cloud.features.rows();
01213 
01214         const BOOST_AUTO(normals, cloud.getDescriptorViewByName("normals"));
01215         int j = 0;
01216 
01217         for(int i=0; i < cloud.features.cols(); i++)
01218         {
01219                 const Vector normal = normals.col(i).normalized();
01220                 const Vector point = cloud.features.block(0, i, dim-1, 1).normalized();
01221 
01222                 const T value = anyabs(normal.dot(point));
01223 
01224                 if(value > eps) // test to keep the points
01225                 {
01226                         cloud.features.col(j) = cloud.features.col(i);
01227                         cloud.descriptors.col(j) = cloud.descriptors.col(i);
01228                         j++;
01229                 }
01230         }
01231 
01232         cloud.conservativeResize(j);
01233 }
01234 
01235 template struct DataPointsFiltersImpl<float>::ShadowDataPointsFilter;
01236 template struct DataPointsFiltersImpl<double>::ShadowDataPointsFilter;
01237 
01238 
01239 // SimpleSensorNoiseDataPointsFilter
01240 // Constructor
01241 template<typename T>
01242 DataPointsFiltersImpl<T>::SimpleSensorNoiseDataPointsFilter::SimpleSensorNoiseDataPointsFilter(const Parameters& params):
01243         DataPointsFilter("SimpleSensorNoiseDataPointsFilter", SimpleSensorNoiseDataPointsFilter::availableParameters(), params),
01244         sensorType(Parametrizable::get<unsigned>("sensorType")),
01245         gain(Parametrizable::get<T>("gain"))
01246 {
01247   std::vector<string> sensorNames = boost::assign::list_of ("Sick LMS-1xx")("Hokuyo URG-04LX")("Hokuyo UTM-30LX")("Kinect / Xtion")("Sick Tim3xx");
01248         if (sensorType >= sensorNames.size())
01249         {
01250                 throw InvalidParameter(
01251                         (boost::format("SimpleSensorNoiseDataPointsFilter: Error, sensorType id %1% does not exist.") % sensorType).str());
01252         }
01253 
01254         LOG_INFO_STREAM("SimpleSensorNoiseDataPointsFilter - using sensor noise model: " << sensorNames[sensorType]);
01255 }
01256 
01257 
01258 // SimpleSensorNoiseDataPointsFilter
01259 // Compute
01260 template<typename T>
01261 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::SimpleSensorNoiseDataPointsFilter::filter(
01262         const DataPoints& input)
01263 {
01264         DataPoints output(input);
01265         inPlaceFilter(output);
01266         return output;
01267 }
01268 
01269 // In-place filter
01270 template<typename T>
01271 void DataPointsFiltersImpl<T>::SimpleSensorNoiseDataPointsFilter::inPlaceFilter(
01272         DataPoints& cloud)
01273 {
01274         cloud.allocateDescriptor("simpleSensorNoise", 1);
01275         BOOST_AUTO(noise, cloud.getDescriptorViewByName("simpleSensorNoise"));
01276 
01277         switch(sensorType)
01278         {
01279         case 0: // Sick LMS-1xx
01280         {
01281                 noise = computeLaserNoise(0.012, 0.0068, 0.0008, cloud.features);
01282                 break;
01283         }
01284         case 1: // Hokuyo URG-04LX
01285         {
01286                 noise = computeLaserNoise(0.028, 0.0013, 0.0001, cloud.features);
01287                 break;
01288         }
01289         case 2: // Hokuyo UTM-30LX
01290         {
01291                 noise = computeLaserNoise(0.018, 0.0006, 0.0015, cloud.features);
01292                 break;
01293         }
01294         case 3: // Kinect / Xtion
01295         {
01296                 const int dim = cloud.features.rows();
01297                 const Matrix squaredValues(cloud.features.topRows(dim-1).colwise().norm().array().square());
01298                 noise = squaredValues*(0.5*0.00285);
01299                 break;
01300         }
01301   case 4: // Sick Tim3xx
01302   {
01303     noise = computeLaserNoise(0.004, 0.0053, -0.0092, cloud.features);
01304     break;
01305   }
01306         default:
01307                 throw InvalidParameter(
01308                         (boost::format("SimpleSensorNoiseDataPointsFilter: Error, cannot compute noise for sensorType id %1% .") % sensorType).str());
01309         }
01310 
01311 }
01312 
01313 template<typename T>
01314 typename PointMatcher<T>::Matrix DataPointsFiltersImpl<T>::SimpleSensorNoiseDataPointsFilter::computeLaserNoise(const T minRadius, const T beamAngle, const T beamConst, const Matrix features)
01315 {
01316         typedef typename Eigen::Array<T, 2, Eigen::Dynamic> Array2rows;
01317 
01318         const int nbPoints = features.cols();
01319         const int dim = features.rows();
01320 
01321         Array2rows evalNoise = Array2rows::Constant(2, nbPoints, minRadius);
01322         evalNoise.row(0) =  beamAngle * features.topRows(dim-1).colwise().norm();
01323         evalNoise.row(0) += beamConst;
01324 
01325         return evalNoise.colwise().maxCoeff();
01326 
01327 }
01328 
01329 
01330 template struct DataPointsFiltersImpl<float>::SimpleSensorNoiseDataPointsFilter;
01331 template struct DataPointsFiltersImpl<double>::SimpleSensorNoiseDataPointsFilter;
01332 
01333 
01334 // ObservationDirectionDataPointsFilter
01335 // Constructor
01336 template<typename T>
01337 DataPointsFiltersImpl<T>::ObservationDirectionDataPointsFilter::ObservationDirectionDataPointsFilter(const Parameters& params):
01338         DataPointsFilter("ObservationDirectionDataPointsFilter", ObservationDirectionDataPointsFilter::availableParameters(), params),
01339         centerX(Parametrizable::get<T>("x")),
01340         centerY(Parametrizable::get<T>("y")),
01341         centerZ(Parametrizable::get<T>("z"))
01342 {
01343 }
01344 
01345 // Compute
01346 template<typename T>
01347 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::ObservationDirectionDataPointsFilter::filter(
01348         const DataPoints& input)
01349 {
01350         DataPoints output(input);
01351         inPlaceFilter(output);
01352         return output;
01353 }
01354 
01355 // In-place filter
01356 template<typename T>
01357 void DataPointsFiltersImpl<T>::ObservationDirectionDataPointsFilter::inPlaceFilter(
01358         DataPoints& cloud)
01359 {
01360         const int dim(cloud.features.rows() - 1);
01361         if (dim != 2 && dim != 3)
01362         {
01363                 throw InvalidField(
01364                         (boost::format("ObservationDirectionDataPointsFilter: Error, works only in 2 or 3 dimensions, cloud has %1% dimensions.") % dim).str()
01365                 );
01366         }
01367 
01368         Vector center(dim);
01369         center[0] = centerX;
01370         center[1] = centerY;
01371         if (dim == 3)
01372                 center[2] = centerZ;
01373 
01374         cloud.allocateDescriptor("observationDirections", dim);
01375         BOOST_AUTO(observationDirections, cloud.getDescriptorViewByName("observationDirections"));
01376 
01377         for (int i = 0; i < cloud.features.cols(); i++)
01378         {
01379                 // Check normal orientation
01380                 const Vector p(cloud.features.block(0, i, dim, 1));
01381                 observationDirections.col(i) = center - p;
01382         }
01383 
01384 }
01385 
01386 template struct DataPointsFiltersImpl<float>::ObservationDirectionDataPointsFilter;
01387 template struct DataPointsFiltersImpl<double>::ObservationDirectionDataPointsFilter;
01388 
01389 // VoxelGridDataPointsFilter
01390 template <typename T>
01391 DataPointsFiltersImpl<T>::VoxelGridDataPointsFilter::VoxelGridDataPointsFilter() :
01392         vSizeX(1),
01393         vSizeY(1),
01394         vSizeZ(1),
01395         useCentroid(true),
01396         averageExistingDescriptors(true) {}
01397 
01398 template <typename T>
01399 DataPointsFiltersImpl<T>::VoxelGridDataPointsFilter::VoxelGridDataPointsFilter(const Parameters& params) :
01400 DataPointsFilter("VoxelGridDataPointsFilter", VoxelGridDataPointsFilter::availableParameters(), params),
01401                 vSizeX(Parametrizable::get<T>("vSizeX")),
01402                 vSizeY(Parametrizable::get<T>("vSizeY")),
01403                 vSizeZ(Parametrizable::get<T>("vSizeZ")),
01404                 useCentroid(Parametrizable::get<bool>("useCentroid")),
01405                 averageExistingDescriptors(Parametrizable::get<bool>("averageExistingDescriptors"))
01406 {
01407 
01408 }
01409 
01410 template <typename T>
01411 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::VoxelGridDataPointsFilter::filter(const DataPoints& input)
01412 {
01413     DataPoints output(input);
01414         inPlaceFilter(output);
01415         return output;
01416 }
01417 
01418 template <typename T>
01419 void DataPointsFiltersImpl<T>::VoxelGridDataPointsFilter::inPlaceFilter(DataPoints& cloud)
01420 {
01421     const unsigned int numPoints(cloud.features.cols());
01422         const int featDim(cloud.features.rows());
01423         const int descDim(cloud.descriptors.rows());
01424 
01425         assert (featDim == 3 || featDim == 4);
01426 
01427         int insertDim(0);
01428         if (averageExistingDescriptors)
01429         {
01430                 // TODO: this should be in the form of an assert
01431                 // Validate descriptors and labels
01432                 for(unsigned int i = 0; i < cloud.descriptorLabels.size(); i++)
01433                         insertDim += cloud.descriptorLabels[i].span;
01434                 if (insertDim != descDim)
01435                         throw InvalidField("VoxelGridDataPointsFilter: Error, descriptor labels do not match descriptor data");
01436         }
01437 
01438         // TODO: Check that the voxel size is not too small, given the size of the data
01439 
01440         // Calculate number of divisions along each axis
01441         Vector minValues = cloud.features.rowwise().minCoeff();
01442         Vector maxValues = cloud.features.rowwise().maxCoeff();
01443 
01444     T minBoundX = minValues.x() / vSizeX;
01445     T maxBoundX = maxValues.x() / vSizeX;
01446     T minBoundY = minValues.y() / vSizeY;
01447     T maxBoundY = maxValues.y() / vSizeY;
01448     T minBoundZ = 0;
01449     T maxBoundZ = 0;
01450 
01451     if (featDim == 4)
01452     {
01453         minBoundZ = minValues.z() / vSizeZ;
01454         maxBoundZ = maxValues.z() / vSizeZ;
01455     }
01456 
01457     // number of divisions is total size / voxel size voxels of equal length + 1
01458     // with remaining space
01459     unsigned int numDivX = 1 + maxBoundX - minBoundX;
01460     unsigned int numDivY = 1 + maxBoundY - minBoundY;;
01461     unsigned int numDivZ = 0;
01462 
01463     // If a 3D point cloud
01464     if (featDim == 4 )
01465         numDivZ = 1 + maxBoundZ - minBoundZ;
01466 
01467     unsigned int numVox = numDivX * numDivY;
01468     if ( featDim == 4)
01469         numVox *= numDivZ;
01470 
01471     // Assume point cloud is randomly ordered
01472     // compute a linear index of the following type
01473     // i, j, k are the component indices
01474     // nx, ny number of divisions in x and y components
01475     // idx = i + j * nx + k * nx * ny
01476     std::vector<unsigned int> indices(numPoints);
01477 
01478     // vector to hold the first point in a voxel
01479     // this point will be ovewritten in the input cloud with
01480     // the output value
01481 
01482     std::vector<Voxel>* voxels;
01483 
01484     // try allocating vector. If too big return error
01485     try {
01486         voxels = new std::vector<Voxel>(numVox);
01487     } catch (std::bad_alloc&) {
01488         throw InvalidParameter((boost::format("VoxelGridDataPointsFilter: Memory allocation error with %1% voxels.  Try increasing the voxel dimensions.") % numVox).str());
01489     }
01490 
01491 
01492     for (unsigned int p = 0; p < numPoints; p++ )
01493     {
01494         unsigned int i = floor(cloud.features(0,p)/vSizeX - minBoundX);
01495         unsigned int j = floor(cloud.features(1,p)/vSizeY- minBoundY);
01496         unsigned int k = 0;
01497         unsigned int idx;
01498         if ( featDim == 4 )
01499         {
01500             k = floor(cloud.features(2,p)/vSizeZ - minBoundZ);
01501             idx = i + j * numDivX + k * numDivX * numDivY;
01502         }
01503         else
01504         {
01505             idx = i + j * numDivX;
01506         }
01507 
01508         unsigned int pointsInVox = (*voxels)[idx].numPoints + 1;
01509 
01510         if (pointsInVox == 1)
01511         {
01512             (*voxels)[idx].firstPoint = p;
01513         }
01514 
01515         (*voxels)[idx].numPoints = pointsInVox;
01516 
01517         indices[p] = idx;
01518 
01519     }
01520 
01521     // store which points contain voxel position
01522     std::vector<unsigned int> pointsToKeep;
01523 
01524     // Store voxel centroid in output
01525     if (useCentroid)
01526     {
01527         // Iterate through the indices and sum values to compute centroid
01528         for (unsigned int p = 0; p < numPoints ; p++)
01529         {
01530             unsigned int idx = indices[p];
01531             unsigned int firstPoint = (*voxels)[idx].firstPoint;
01532 
01533             // If this is the first point in the voxel, leave as is
01534             // if not sum up this point for centroid calculation
01535             if (firstPoint != p)
01536             {
01537                 // Sum up features and descriptors (if we are also averaging descriptors)
01538 
01539                 for (int f = 0; f < (featDim - 1); f++ )
01540                 {
01541                         cloud.features(f,firstPoint) += cloud.features(f,p);
01542                 }
01543 
01544                 if (averageExistingDescriptors) {
01545                         for (int d = 0; d < descDim; d++)
01546                         {
01547                                 cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
01548                         }
01549                 }
01550             }
01551         }
01552 
01553         // Now iterating through the voxels
01554         // Normalize sums to get centroid (average)
01555         // Some voxels may be empty and are discarded
01556         for(unsigned int idx = 0; idx < numVox; idx++)
01557         {
01558             unsigned int numPoints = (*voxels)[idx].numPoints;
01559             unsigned int firstPoint = (*voxels)[idx].firstPoint;
01560             if(numPoints > 0)
01561             {
01562                 for ( int f = 0; f < (featDim - 1); f++ )
01563                     cloud.features(f,firstPoint) /= numPoints;
01564 
01565                 if (averageExistingDescriptors) {
01566                         for ( int d = 0; d < descDim; d++ )
01567                                 cloud.descriptors(d,firstPoint) /= numPoints;
01568                 }
01569 
01570                 pointsToKeep.push_back(firstPoint);
01571             }
01572         }
01573     }
01574     else
01575     {
01576         // Although we don't sum over the features, we may still need to sum the descriptors
01577         if (averageExistingDescriptors)
01578         {
01579                 // Iterate through the indices and sum values to compute centroid
01580                 for (unsigned int p = 0; p < numPoints ; p++)
01581                 {
01582                         unsigned int idx = indices[p];
01583                         unsigned int firstPoint = (*voxels)[idx].firstPoint;
01584 
01585                         // If this is the first point in the voxel, leave as is
01586                         // if not sum up this point for centroid calculation
01587                         if (firstPoint != p)
01588                         {
01589                                 for (int d = 0; d < descDim; d++ )
01590                                 {
01591                                         cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
01592                                 }
01593                         }
01594                 }
01595         }
01596 
01597         for (unsigned int idx = 0; idx < numVox; idx++)
01598         {
01599             unsigned int numPoints = (*voxels)[idx].numPoints;
01600             unsigned int firstPoint = (*voxels)[idx].firstPoint;
01601 
01602             if (numPoints > 0)
01603             {
01604                 // get back voxel indices in grid format
01605                 // If we are in the last division, the voxel is smaller in size
01606                 // We adjust the center as from the end of the last voxel to the bounding area
01607                 unsigned int i = 0;
01608                 unsigned int j = 0;
01609                 unsigned int k = 0;
01610                 if (featDim == 4)
01611                 {
01612                     k = idx / (numDivX * numDivY);
01613                     if (k == numDivZ)
01614                         cloud.features(3,firstPoint) = maxValues.z() - (k-1) * vSizeZ/2;
01615                     else
01616                         cloud.features(3,firstPoint) = k * vSizeZ + vSizeZ/2;
01617                 }
01618 
01619                 j = (idx - k * numDivX * numDivY) / numDivX;
01620                 if (j == numDivY)
01621                     cloud.features(2,firstPoint) = maxValues.y() - (j-1) * vSizeY/2;
01622                 else
01623                     cloud.features(2,firstPoint) = j * vSizeY + vSizeY / 2;
01624 
01625                 i = idx - k * numDivX * numDivY - j * numDivX;
01626                 if (i == numDivX)
01627                     cloud.features(1,firstPoint) = maxValues.x() - (i-1) * vSizeX/2;
01628                 else
01629                     cloud.features(1,firstPoint) = i * vSizeX + vSizeX / 2;
01630 
01631                 // Descriptors : normalize if we are averaging or keep as is
01632                 if (averageExistingDescriptors) {
01633                         for ( int d = 0; d < descDim; d++ )
01634                                 cloud.descriptors(d,firstPoint) /= numPoints;
01635                 }
01636 
01637                 pointsToKeep.push_back(firstPoint);
01638             }
01639         }
01640 
01641     }
01642 
01643     // deallocate memory for voxels information
01644     delete voxels;
01645 
01646     // Move the points to be kept to the start
01647     // Bring the data we keep to the front of the arrays then
01648         // wipe the leftover unused space.
01649         std::sort(pointsToKeep.begin(), pointsToKeep.end());
01650         int numPtsOut = pointsToKeep.size();
01651         for (int i = 0; i < numPtsOut; i++){
01652                 int k = pointsToKeep[i];
01653                 assert(i <= k);
01654                 cloud.features.col(i) = cloud.features.col(k);
01655                 if (cloud.descriptors.rows() != 0)
01656                         cloud.descriptors.col(i) = cloud.descriptors.col(k);
01657         }
01658         cloud.features.conservativeResize(Eigen::NoChange, numPtsOut);
01659         
01660         if (cloud.descriptors.rows() != 0)
01661                 cloud.descriptors.conservativeResize(Eigen::NoChange, numPtsOut);
01662 }
01663 
01664 template struct DataPointsFiltersImpl<float>::VoxelGridDataPointsFilter;
01665 template struct DataPointsFiltersImpl<double>::VoxelGridDataPointsFilter;
01666 
01667 
01668 // CutAtDescriptorThresholdDataPointsFilter
01669 // Constructor
01670 template<typename T>
01671 DataPointsFiltersImpl<T>::CutAtDescriptorThresholdDataPointsFilter::CutAtDescriptorThresholdDataPointsFilter(const Parameters& params):
01672         DataPointsFilter("CutAtDescriptorThresholdDataPointsFilter", CutAtDescriptorThresholdDataPointsFilter::availableParameters(), params),
01673         descName(Parametrizable::get<std::string>("descName")),
01674         useLargerThan(Parametrizable::get<bool>("useLargerThan")),
01675         threshold(Parametrizable::get<T>("threshold"))
01676 {
01677 }
01678 
01679 // Compute
01680 template<typename T>
01681 typename PointMatcher<T>::DataPoints DataPointsFiltersImpl<T>::CutAtDescriptorThresholdDataPointsFilter::filter(
01682         const DataPoints& input)
01683 {
01684         DataPoints output(input);
01685         inPlaceFilter(output);
01686         return output;
01687 }
01688 
01689 // In-place filter
01690 template<typename T>
01691 void DataPointsFiltersImpl<T>::CutAtDescriptorThresholdDataPointsFilter::inPlaceFilter(
01692         DataPoints& cloud)
01693 {
01694         // Check field exists
01695         if (!cloud.descriptorExists(descName))
01696         {
01697                 throw InvalidField("CutAtDescriptorThresholdDataPointsFilter: Error, field not found in descriptors.");
01698         }
01699 
01700         const int nbPointsIn = cloud.features.cols();
01701         typename DataPoints::View values = cloud.getDescriptorViewByName(descName);
01702 
01703         // fill cloud values
01704         int j = 0;
01705         if (useLargerThan)
01706         {
01707                 for (int i = 0; i < nbPointsIn; i++)
01708                 {
01709                         const T value(values(0,i));
01710                         if (value <= threshold)
01711                         {
01712                                 cloud.setColFrom(j, cloud, i);
01713                                 j++;
01714                         }
01715                 }
01716         }
01717         else
01718         {
01719                 for (int i = 0; i < nbPointsIn; i++)
01720                 {
01721                         const T value(values(0,i));
01722                         if (value >= threshold)
01723                         {
01724                                 cloud.setColFrom(j, cloud, i);
01725                                 j++;
01726                         }
01727                 }
01728         }
01729         cloud.conservativeResize(j);
01730 }
01731 
01732 template struct DataPointsFiltersImpl<float>::CutAtDescriptorThresholdDataPointsFilter;
01733 template struct DataPointsFiltersImpl<double>::CutAtDescriptorThresholdDataPointsFilter;
01734 


libpointmatcher
Author(s):
autogenerated on Mon Sep 14 2015 02:59:04