Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "MaxDist.h"
00036
00037 #include "pointmatcher/Functions.h"
00038
00039
00040
00041 template<typename T>
00042 MaxDistDataPointsFilter<T>::MaxDistDataPointsFilter(const Parameters& params) :
00043 PointMatcher<T>::DataPointsFilter("MaxDistDataPointsFilter",
00044 MaxDistDataPointsFilter::availableParameters(), params),
00045 dim(Parametrizable::get<unsigned>("dim")),
00046 maxDist(Parametrizable::get<T>("maxDist"))
00047 {
00048 }
00049
00050
00051 template<typename T>
00052 typename PointMatcher<T>::DataPoints MaxDistDataPointsFilter<T>::filter(
00053 const DataPoints& input)
00054 {
00055 DataPoints output(input);
00056 inPlaceFilter(output);
00057 return output;
00058 }
00059
00060
00061 template<typename T>
00062 void MaxDistDataPointsFilter<T>::inPlaceFilter(
00063 DataPoints& cloud)
00064 {
00065 using namespace PointMatcherSupport;
00066
00067 if (dim >= cloud.features.rows() - 1)
00068 {
00069 throw InvalidParameter(
00070 (boost::format("MaxDistDataPointsFilter: Error, filtering on dimension number %1%, larger than authorized axis id %2%") % dim % (cloud.features.rows() - 2)).str());
00071 }
00072
00073 const int nbPointsIn = cloud.features.cols();
00074 const int nbRows = cloud.features.rows();
00075
00076 int j = 0;
00077 if(dim == -1)
00078 {
00079 const T absMaxDist = anyabs(maxDist);
00080 for (int i = 0; i < nbPointsIn; ++i)
00081 {
00082 if (cloud.features.col(i).head(nbRows-1).norm() < absMaxDist)
00083 {
00084 cloud.setColFrom(j, cloud, i);
00085 ++j;
00086 }
00087 }
00088 }
00089 else
00090 {
00091 for (int i = 0; i < nbPointsIn; ++i)
00092 {
00093 if ((cloud.features(dim, i)) < maxDist)
00094 {
00095 cloud.setColFrom(j, cloud, i);
00096 ++j;
00097 }
00098 }
00099 }
00100
00101 cloud.conservativeResize(j);
00102 }
00103
00104 template struct MaxDistDataPointsFilter<float>;
00105 template struct MaxDistDataPointsFilter<double>;