SurfaceNormal.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2018,
6 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 #include "SurfaceNormal.h"
36 
37 // Eigenvalues
38 #include "Eigen/QR"
39 #include "Eigen/Eigenvalues"
40 
41 #include "PointMatcherPrivate.h"
42 #include "IO.h"
43 #include "MatchersImpl.h"
44 
45 #include <boost/format.hpp>
46 
48 
49 // SurfaceNormalDataPointsFilter
50 // Constructor
51 template<typename T>
53  PointMatcher<T>::DataPointsFilter("SurfaceNormalDataPointsFilter",
54  SurfaceNormalDataPointsFilter::availableParameters(), params),
55  knn(Parametrizable::get<int>("knn")),
56  maxDist(Parametrizable::get<T>("maxDist")),
57  epsilon(Parametrizable::get<T>("epsilon")),
58  keepNormals(Parametrizable::get<bool>("keepNormals")),
59  keepDensities(Parametrizable::get<bool>("keepDensities")),
60  keepEigenValues(Parametrizable::get<bool>("keepEigenValues")),
61  keepEigenVectors(Parametrizable::get<bool>("keepEigenVectors")),
62  keepMatchedIds(Parametrizable::get<bool>("keepMatchedIds")),
63  keepMeanDist(Parametrizable::get<bool>("keepMeanDist")),
64  sortEigen(Parametrizable::get<bool>("sortEigen")),
65  smoothNormals(Parametrizable::get<bool>("smoothNormals"))
66 {
67 }
68 
69 // Compute
70 template<typename T>
73  const DataPoints& input)
74 {
75  DataPoints output(input);
76  inPlaceFilter(output);
77  return output;
78 }
79 
80 // In-place filter
81 template<typename T>
83  DataPoints& cloud)
84 {
85  typedef typename DataPoints::View View;
86  typedef typename DataPoints::Label Label;
87  typedef typename DataPoints::Labels Labels;
88  typedef typename MatchersImpl<T>::KDTreeMatcher KDTreeMatcher;
89  typedef typename PointMatcher<T>::Matches Matches;
90 
91  using namespace PointMatcherSupport;
92 
93  const int pointsCount(cloud.features.cols());
94  const int featDim(cloud.features.rows());
95  const int descDim(cloud.descriptors.rows());
96  const unsigned int labelDim(cloud.descriptorLabels.size());
97 
98  // Validate descriptors and labels
99  int insertDim(0);
100  for(unsigned int i = 0; i < labelDim ; ++i)
101  insertDim += cloud.descriptorLabels[i].span;
102  if (insertDim != descDim)
103  throw InvalidField("SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
104 
105  // Reserve memory for new descriptors
106  const int dimNormals(featDim-1);
107  const int dimDensities(1);
108  const int dimEigValues(featDim-1);
109  const int dimEigVectors((featDim-1)*(featDim-1));
110  //const int dimMatchedIds(knn);
111  const int dimMeanDist(1);
112 
113  boost::optional<View> normals;
114  boost::optional<View> densities;
115  boost::optional<View> eigenValues;
116  boost::optional<View> eigenVectors;
117  boost::optional<View> matchedValues;
118  boost::optional<View> matchIds;
119  boost::optional<View> meanDists;
120 
121  Labels cloudLabels;
122  if (keepNormals)
123  cloudLabels.push_back(Label("normals", dimNormals));
124  if (keepDensities)
125  cloudLabels.push_back(Label("densities", dimDensities));
126  if (keepEigenValues)
127  cloudLabels.push_back(Label("eigValues", dimEigValues));
128  if (keepEigenVectors)
129  cloudLabels.push_back(Label("eigVectors", dimEigVectors));
130  if (keepMatchedIds)
131  cloudLabels.push_back(Label("matchedIds", knn));
132  if (keepMeanDist)
133  cloudLabels.push_back(Label("meanDists", dimMeanDist));
134 
135  // Reserve memory
136  cloud.allocateDescriptors(cloudLabels);
137 
138  if (keepNormals)
139  normals = cloud.getDescriptorViewByName("normals");
140  if (keepDensities)
141  densities = cloud.getDescriptorViewByName("densities");
142  if (keepEigenValues)
143  eigenValues = cloud.getDescriptorViewByName("eigValues");
144  if (keepEigenVectors)
145  eigenVectors = cloud.getDescriptorViewByName("eigVectors");
146  if (keepMatchedIds)
147  matchIds = cloud.getDescriptorViewByName("matchedIds");
148  if (keepMeanDist)
149  meanDists = cloud.getDescriptorViewByName("meanDists");
150 
151  using namespace PointMatcherSupport;
152  // Build kd-tree
154  boost::assign::insert(param) ( "knn", toParam(knn) );
155  boost::assign::insert(param) ( "epsilon", toParam(epsilon) );
156  boost::assign::insert(param) ( "maxDist", toParam(maxDist) );
157 
158  KDTreeMatcher matcher(param);
159  matcher.init(cloud);
160 
161  Matches matches(typename Matches::Dists(knn, pointsCount), typename Matches::Ids(knn, pointsCount));
162  matches = matcher.findClosests(cloud);
163 
164  // Search for surrounding points and compute descriptors
165  int degenerateCount(0);
166  for (int i = 0; i < pointsCount; ++i)
167  {
168  bool isDegenerate = false;
169  // Mean of nearest neighbors (NN)
170  Matrix d(featDim-1, knn);
171  int realKnn = 0;
172 
173  for(int j = 0; j < int(knn); ++j)
174  {
175  if (matches.dists(j,i) != Matches::InvalidDist)
176  {
177  const int refIndex(matches.ids(j,i));
178  d.col(realKnn) = cloud.features.block(0, refIndex, featDim-1, 1);
179  ++realKnn;
180  }
181  }
182  d.conservativeResize(Eigen::NoChange, realKnn);
183 
184  const Vector mean = d.rowwise().sum() / T(realKnn);
185  const Matrix NN = d.colwise() - mean;
186 
187  const Matrix C((NN * NN.transpose()) / T(realKnn));
188  Vector eigenVa = Vector::Zero(featDim-1, 1);
189  Matrix eigenVe = Matrix::Zero(featDim-1, featDim-1);
190  // Ensure that the matrix is suited for eigenvalues calculation
192  {
193  if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
194  {
195  const Eigen::EigenSolver<Matrix> solver(C);
196  eigenVa = solver.eigenvalues().real();
197  eigenVe = solver.eigenvectors().real();
198 
199  if(sortEigen)
200  {
201  const std::vector<size_t> idx = sortIndexes<T>(eigenVa);
202  const size_t idxSize = idx.size();
203  Vector tmp_eigenVa = eigenVa;
204  Matrix tmp_eigenVe = eigenVe;
205  for(size_t i=0; i<idxSize; ++i)
206  {
207  eigenVa(i,0) = tmp_eigenVa(idx[i], 0);
208  eigenVe.col(i) = tmp_eigenVe.col(idx[i]);
209  }
210  }
211  }
212  else
213  {
214  //std::cout << "WARNING: Matrix C needed for eigen decomposition is degenerated. Expected cause: no noise in data" << std::endl;
215  ++degenerateCount;
216  isDegenerate = true;
217  }
218  }
219 
220  if(keepNormals)
221  {
222  if(sortEigen)
223  normals->col(i) = eigenVe.col(0);
224  else
225  normals->col(i) = computeNormal<T>(eigenVa, eigenVe);
226 
227  // clamp normals to [-1,1] to handle approximation errors
228  normals->col(i) = normals->col(i).cwiseMax(-1.0).cwiseMin(1.0);
229  }
230  if(keepDensities)
231  {
232  if(isDegenerate)
233  (*densities)(0, i) = 0.;
234  else
235  (*densities)(0, i) = computeDensity<T>(NN);
236  }
237  if(keepEigenValues)
238  eigenValues->col(i) = eigenVa;
239  if(keepEigenVectors)
240  eigenVectors->col(i) = serializeEigVec<T>(eigenVe);
241  if(keepMeanDist)
242  {
243  if(isDegenerate)
244  (*meanDists)(0, i) = std::numeric_limits<std::size_t>::max();
245  else
246  {
247  const Vector point = cloud.features.block(0, i, featDim-1, 1);
248  (*meanDists)(0, i) = (point - mean).norm();
249  }
250  }
251 
252  }
253 
254  if(keepMatchedIds)
255  {
256  matchIds.get() = matches.ids.template cast<T>();
257  }
258 
259  if(smoothNormals)
260  {
261  for (int i = 0; i < pointsCount; ++i)
262  {
263  const Vector currentNormal = normals->col(i);
264  Vector mean = Vector::Zero(featDim-1);
265  int n=0;
266  for(int j = 0; j < int(knn); ++j)
267  {
268  if (matches.dists(j,i) != Matches::InvalidDist)
269  {
270  const int refIndex(matches.ids(j,i));
271  const Vector normal = normals->col(refIndex);
272  if(currentNormal.dot(normal) > 0.)
273  mean += normal;
274  else // flip normal vector
275  mean -= normal;
276 
277  ++n;
278  }
279  }
280 
281  normals->col(i) = mean / T(n);
282  }
283  }
284 
285  if (degenerateCount)
286  {
287  LOG_WARNING_STREAM("WARNING: Matrix C needed for eigen decomposition was degenerated in " << degenerateCount << " points over " << pointsCount << " (" << float(degenerateCount)*100.f/float(pointsCount) << " %)");
288  }
289 
290 }
291 
294 
DataPoints::Label Label
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
std::string toParam(const S &value)
Return the a string value using lexical_cast.
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
PointMatcher< T >::Matrix Matrix
Definition: SurfaceNormal.h:53
The name for a certain number of dim.
Definition: PointMatcher.h:221
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
SurfaceNormalDataPointsFilter(const Parameters &params=Parameters())
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Functions and classes that are not dependant on scalar type are defined in this namespace.
DataPoints::Labels Labels
Functions and classes that are dependant on scalar type are defined in this templatized class...
Definition: PointMatcher.h:130
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy...
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:371
const M::mapped_type & get(const M &m, const typename M::key_type &k)
#define LOG_WARNING_STREAM(args)
The superclass of classes that are constructed using generic parameters. This class provides the para...
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:210
Surface normals estimation. Find the normal for every point using eigen-decomposition of neighbour po...
Definition: SurfaceNormal.h:43
PointMatcher< T >::Vector Vector
Definition: SurfaceNormal.h:52
Parametrizable::Parameters Parameters
Definition: SurfaceNormal.h:47
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
PM::Matches Matches
PM::DataPointsFilter DataPointsFilter
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
PointMatcher< T >::DataPoints::InvalidField InvalidField
Definition: SurfaceNormal.h:55
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334


libpointmatcher
Author(s):
autogenerated on Sat May 27 2023 02:38:03