OutlierFiltersImpl.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 "OutlierFiltersImpl.h"
00037 #include "PointMatcherPrivate.h"
00038 #include "Functions.h"
00039 
00040 #include <algorithm>
00041 #include <vector>
00042 #include <iostream>
00043 #include <limits>
00044 
00045 using namespace std;
00046 using namespace PointMatcherSupport;
00047 
00048 // NullOutlierFilter
00049 template<typename T>
00050 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::NullOutlierFilter::compute(
00051         const DataPoints& filteredReading,
00052         const DataPoints& filteredReference,
00053         const Matches& input)
00054 {
00055         return OutlierWeights::Constant(input.ids.rows(), input.ids.cols(), 1);
00056 }
00057 
00058 template struct OutlierFiltersImpl<float>::NullOutlierFilter;
00059 template struct OutlierFiltersImpl<double>::NullOutlierFilter;
00060 
00061 
00062 // MaxDistOutlierFilter
00063 template<typename T>
00064 OutlierFiltersImpl<T>::MaxDistOutlierFilter::MaxDistOutlierFilter(const Parameters& params):
00065         OutlierFilter("MaxDistOutlierFilter", MaxDistOutlierFilter::availableParameters(), params),
00066         maxDist(Parametrizable::get<T>("maxDist"))
00067 {
00068 }
00069 
00070 
00071 template<typename T>
00072 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::MaxDistOutlierFilter::compute(
00073         const DataPoints& filteredReading,
00074         const DataPoints& filteredReference,
00075         const Matches& input)
00076 {
00077         return OutlierWeights((input.dists.array() <= maxDist).template cast<T>());
00078 }
00079 
00080 template struct OutlierFiltersImpl<float>::MaxDistOutlierFilter;
00081 template struct OutlierFiltersImpl<double>::MaxDistOutlierFilter;
00082 
00083 // MinDistOutlierFilter
00084 template<typename T>
00085 OutlierFiltersImpl<T>::MinDistOutlierFilter::MinDistOutlierFilter(const Parameters& params):
00086         OutlierFilter("MinDistOutlierFilter", MinDistOutlierFilter::availableParameters(), params),
00087         minDist(Parametrizable::get<T>("minDist"))
00088 {
00089 }
00090 
00091 template<typename T>
00092 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::MinDistOutlierFilter::compute(
00093         const DataPoints& filteredReading,
00094         const DataPoints& filteredReference,
00095         const Matches& input)
00096 {
00097         return OutlierWeights((input.dists.array() >= minDist).template cast<T>());
00098 }
00099 
00100 template struct OutlierFiltersImpl<float>::MinDistOutlierFilter;
00101 template struct OutlierFiltersImpl<double>::MinDistOutlierFilter;
00102 
00103 
00104 
00105 // MedianDistOutlierFilter
00106 template<typename T>
00107 OutlierFiltersImpl<T>::MedianDistOutlierFilter::MedianDistOutlierFilter(const Parameters& params):
00108         OutlierFilter("MedianDistOutlierFilter", MedianDistOutlierFilter::availableParameters(), params),
00109         factor(Parametrizable::get<T>("factor"))
00110 {
00111 }
00112 
00113 template<typename T>
00114 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::MedianDistOutlierFilter::compute(
00115         const DataPoints& filteredReading,
00116         const DataPoints& filteredReference,
00117         const Matches& input)
00118 {
00119         const T median = input.getDistsQuantile(0.5);
00120         const T limit = factor * median;
00121         return OutlierWeights((input.dists.array() <= limit).template cast<T>());
00122 }
00123 
00124 template struct OutlierFiltersImpl<float>::MedianDistOutlierFilter;
00125 template struct OutlierFiltersImpl<double>::MedianDistOutlierFilter;
00126 
00127 
00128 // TrimmedDistOutlierFilter
00129 template<typename T>
00130 OutlierFiltersImpl<T>::TrimmedDistOutlierFilter::TrimmedDistOutlierFilter(const Parameters& params):
00131         OutlierFilter("TrimmedDistOutlierFilter", TrimmedDistOutlierFilter::availableParameters(), params),
00132         ratio(Parametrizable::get<T>("ratio"))
00133 {
00134 }
00135 
00136 template<typename T>
00137 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::TrimmedDistOutlierFilter::compute(
00138         const DataPoints& filteredReading,
00139         const DataPoints& filteredReference,
00140         const Matches& input)
00141 {
00142         const T limit = input.getDistsQuantile(ratio);
00143         return OutlierWeights((input.dists.array() <= limit).template cast<T>());
00144 }
00145 
00146 template struct OutlierFiltersImpl<float>::TrimmedDistOutlierFilter;
00147 template struct OutlierFiltersImpl<double>::TrimmedDistOutlierFilter;
00148 
00149 // VarTrimmedDistOutlierFilter
00150 template<typename T>
00151 OutlierFiltersImpl<T>::VarTrimmedDistOutlierFilter::VarTrimmedDistOutlierFilter(const Parameters& params):
00152         OutlierFilter("VarTrimmedDistOutlierFilter", VarTrimmedDistOutlierFilter::availableParameters(), params),
00153         minRatio(Parametrizable::get<T>("minRatio")),
00154         maxRatio(Parametrizable::get<T>("maxRatio")),
00155         lambda(Parametrizable::get<T>("lambda"))
00156 {
00157         if (this->minRatio >= this->maxRatio)
00158         {
00159                 throw InvalidParameter((boost::format("VarTrimmedDistOutlierFilter: minRatio (%1%) should be smaller than maxRatio (%2%)") % minRatio % maxRatio).str());
00160         }
00161 }
00162 
00163 template<typename T>
00164 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::VarTrimmedDistOutlierFilter::compute(
00165         const DataPoints& filteredReading,
00166         const DataPoints& filteredReference,
00167         const Matches& input)
00168 {
00169         const T tunedRatio = optimizeInlierRatio(input);
00170         LOG_INFO_STREAM("Optimized ratio: " << tunedRatio);
00171 
00172         const T limit = input.getDistsQuantile(tunedRatio);
00173         return OutlierWeights((input.dists.array() <= limit).template cast<T>());
00174 }
00175 
00176 template<typename T>
00177 T OutlierFiltersImpl<T>::VarTrimmedDistOutlierFilter::optimizeInlierRatio(const Matches& matches)
00178 {
00179         typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00180         typedef typename Eigen::Array<T, Eigen::Dynamic, 1> LineArray;
00181         
00182         const int points_nbr = matches.dists.rows() * matches.dists.cols();
00183         
00184         // vector containing the squared distances of the matches
00185         std::vector<T> tmpSortedDist;
00186         tmpSortedDist.reserve(points_nbr);
00187         for (int x = 0; x < matches.dists.cols(); ++x)
00188                 for (int y = 0; y < matches.dists.rows(); ++y)
00189                         if ((matches.dists(y, x) != numeric_limits<T>::infinity()) && (matches.dists(y, x) > 0))
00190                                 tmpSortedDist.push_back(matches.dists(y, x));
00191         if (tmpSortedDist.size() == 0)
00192                 throw ConvergenceError("no outlier to filter");
00193                         
00194         std::sort(tmpSortedDist.begin(), tmpSortedDist.end());
00195 
00196         const int minEl = floor(this->minRatio*points_nbr);
00197         const int maxEl = floor(this->maxRatio*points_nbr);
00198 
00199         // Return std::vector to an eigen::vector
00200         Eigen::Map<LineArray> sortedDist(&tmpSortedDist[0], points_nbr);
00201 
00202         const LineArray trunkSortedDist = sortedDist.segment(minEl, maxEl-minEl);
00203         const T lowerSum = sortedDist.head(minEl).sum();
00204         const LineArray ids = LineArray::LinSpaced(trunkSortedDist.rows(), minEl+1, maxEl);
00205         const LineArray ratio = ids / points_nbr;
00206         const LineArray deno = ratio.pow(this->lambda);
00207         const LineArray FRMS = deno.inverse().square() * ids.inverse() * (lowerSum + trunkSortedDist);
00208         int minIndex(0);// = FRMS.minCoeff();
00209         FRMS.minCoeff(&minIndex);
00210         const T optRatio = (float)(minIndex + minEl)/ (float)points_nbr;
00211         
00212         //cout << "Optimized ratio: " << optRatio << endl;
00213         
00214         return optRatio;
00215 
00216 }
00217 
00218 template struct OutlierFiltersImpl<float>::VarTrimmedDistOutlierFilter;
00219 template struct OutlierFiltersImpl<double>::VarTrimmedDistOutlierFilter;
00220 
00221 // SurfaceNormalOutlierFilter
00222 template<typename T>
00223 OutlierFiltersImpl<T>::SurfaceNormalOutlierFilter::SurfaceNormalOutlierFilter(const Parameters& params):
00224         OutlierFilter("SurfaceNormalOutlierFilter", SurfaceNormalOutlierFilter::availableParameters(), params),
00225         eps(cos(Parametrizable::get<T>("maxAngle"))),
00226         warningPrinted(false)
00227 {
00228         //waring: eps is change to cos(maxAngle)!
00229 }
00230 
00231 template<typename T>
00232 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::SurfaceNormalOutlierFilter::compute(
00233         const DataPoints& filteredReading,
00234         const DataPoints& filteredReference,
00235         const Matches& input)
00236 {
00237         const BOOST_AUTO(normalsReading, filteredReading.getDescriptorViewByName("normals"));
00238         const BOOST_AUTO(normalsReference, filteredReference.getDescriptorViewByName("normals"));
00239         
00240         // select weight from median
00241         OutlierWeights w(input.dists.rows(), input.dists.cols());
00242 
00243         if(normalsReading.cols() != 0 && normalsReference.cols() != 0)
00244         {
00245                 for (int x = 0; x < w.cols(); ++x) // pts in reading
00246                 {
00247                         const Vector normalRead = normalsReading.col(x).normalized();
00248 
00249                         for (int y = 0; y < w.rows(); ++y) // knn 
00250                         {
00251                                 const int idRef = input.ids(y, x);
00252 
00253                                 const Vector normalRef = normalsReference.col(idRef).normalized();
00254 
00255                                 const T value = anyabs(normalRead.dot(normalRef));
00256 
00257                                 if(value < eps) // test to keep the points
00258                                         w(y, x) = 0;
00259                                 else
00260                                         w(y, x) = 1;
00261                         }
00262                 }
00263         }
00264         else
00265         {
00266                 if(warningPrinted == false)
00267                 {
00268                         LOG_INFO_STREAM("SurfaceNormalOutlierFilter: surface normals not available. Skipping filtering");
00269                         warningPrinted = true;
00270                 }
00271 
00272                 w = Matrix::Ones(input.dists.rows(), input.dists.cols());
00273         }
00274         //abort();
00275         return w;
00276 }
00277 
00278 template struct OutlierFiltersImpl<float>::SurfaceNormalOutlierFilter;
00279 template struct OutlierFiltersImpl<double>::SurfaceNormalOutlierFilter;
00280 
00281 // GenericDescriptorOutlierFilter
00282 template<typename T>
00283 OutlierFiltersImpl<T>::GenericDescriptorOutlierFilter::GenericDescriptorOutlierFilter(const Parameters& params):
00284         OutlierFilter("GenericDescriptorOutlierFilter", GenericDescriptorOutlierFilter::availableParameters(), params),
00285         source(Parametrizable::getParamValueString("source")),
00286         descName(Parametrizable::getParamValueString("descName")),
00287         useSoftThreshold(Parametrizable::get<bool>("useSoftThreshold")),
00288         useLargerThan(Parametrizable::get<bool>("useLargerThan")),
00289         threshold(Parametrizable::get<T>("threshold"))
00290 {
00291         if(source != "reference" && source != "reading")
00292         {
00293                 throw InvalidParameter(
00294                 (boost::format("GenericDescriptorOutlierFilter: Error, the parameter named 'source' can only be set to 'reference' or 'reading' but was set to %1%") % source).str());
00295         }
00296 }
00297 
00298 template<typename T>
00299 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::GenericDescriptorOutlierFilter::compute(
00300         const DataPoints& filteredReading,
00301         const DataPoints& filteredReference,
00302         const Matches& input)
00303 {
00304         typedef typename DataPoints::ConstView ConstView;
00305 
00306         const int knn = input.dists.rows();
00307         const int readPtsCount = input.dists.cols();
00308         
00309         OutlierWeights w(knn, readPtsCount);
00310 
00311         const DataPoints *cloud;
00312 
00313         if(source == "reference")
00314                 cloud = &filteredReference;
00315         else
00316                 cloud = &filteredReference;
00317 
00318         ConstView desc(cloud->getDescriptorViewByName(descName));
00319 
00320         if(desc.rows() != 1)
00321         {
00322                 throw InvalidParameter(
00323                 (boost::format("GenericDescriptorOutlierFilter: Error, the parameter named 'descName' must be a 1D descriptor but the field %1% is %2%D") % descName % desc.rows()).str());
00324         }
00325 
00326         for(int k=0; k < knn; k++)
00327         {
00328                 for(int i=0; i < readPtsCount; i++)
00329                 {
00330                         if(useSoftThreshold == false)
00331                         {
00332                                 if(useLargerThan == true)
00333                                 {
00334                                         if(desc(0, input.ids(k,i)) > threshold)
00335                                                 w(k,i) = 1;
00336                                         else
00337                                                 w(k,i) = 0;
00338                                 }
00339                                 else
00340                                 {
00341                                         if(desc(0, input.ids(k,i)) < threshold)
00342                                                 w(k,i) = 1;
00343                                         else
00344                                                 w(k,i) = 0;
00345                                 }
00346                         }
00347                         else
00348                         {
00349                                 // use soft threshold by assigning the weight using the descriptor
00350                                 w(k,i) = desc(0, input.ids(k,i));
00351                         }
00352                 }
00353         }
00354 
00355         //Normalize
00356         if(useSoftThreshold)
00357                 w = w/w.maxCoeff();
00358 
00359         return w;
00360 }
00361 
00362 template struct OutlierFiltersImpl<float>::GenericDescriptorOutlierFilter;
00363 template struct OutlierFiltersImpl<double>::GenericDescriptorOutlierFilter;
00364 
00365 


upstream_src
Author(s):
autogenerated on Mon Oct 6 2014 10:27:42