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
191  if(keepNormals || keepEigenValues || keepEigenVectors)
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 j=0; j<idxSize; ++j)
206  {
207  eigenVa(j,0) = tmp_eigenVa(idx[j], 0);
208  eigenVe.col(j) = tmp_eigenVe.col(idx[j]);
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 
SurfaceNormalDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: SurfaceNormal.cpp:72
Label
DataPoints::Label Label
Definition: pypoint_matcher_helper.h:17
SurfaceNormalDataPointsFilter::Matrix
PointMatcher< T >::Matrix Matrix
Definition: SurfaceNormal.h:53
SurfaceNormalDataPointsFilter::SurfaceNormalDataPointsFilter
SurfaceNormalDataPointsFilter(const Parameters &params=Parameters())
Definition: SurfaceNormal.cpp:52
PointMatcherSupport::toParam
std::string toParam(const S &value)
Return the string value using lexical_cast.
Definition: Parametrizable.h:123
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
DataPointsFilter
PM::DataPointsFilter DataPointsFilter
Definition: pypoint_matcher_helper.h:22
compute_overlap.knn
int knn
Definition: compute_overlap.py:128
Matches
PM::Matches Matches
Definition: pypoint_matcher_helper.h:19
build_map.T
T
Definition: build_map.py:34
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
SurfaceNormalDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: SurfaceNormal.cpp:82
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherPrivate.h
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
utils.h
SurfaceNormalDataPointsFilter
Surface normals estimation. Find the normal for every point using eigen-decomposition of neighbour po...
Definition: SurfaceNormal.h:43
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
SurfaceNormalDataPointsFilter::Vector
PointMatcher< T >::Vector Vector
Definition: SurfaceNormal.h:52
SurfaceNormalDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: SurfaceNormal.h:47
align_sequence.params
params
Definition: align_sequence.py:13
MatchersImpl.h
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
icp_advance_api.matches
matches
Definition: icp_advance_api.py:114
Labels
DataPoints::Labels Labels
Definition: pypoint_matcher_helper.h:18
LOG_WARNING_STREAM
#define LOG_WARNING_STREAM(args)
Definition: PointMatcherPrivate.h:68
icp_customized.matcher
matcher
Definition: icp_customized.py:121
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
MatchersImpl::KDTreeMatcher
Definition: MatchersImpl.h:74
SurfaceNormal.h
PointMatcher::Matches
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:371
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
PointMatcherSupport::get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
Definition: Bibliography.cpp:57
PointMatcher::DataPoints::View
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:210
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
IO.h
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:555
PointMatcher::DataPoints::allocateDescriptors
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
Definition: pointmatcher/DataPoints.cpp:526
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:221


libpointmatcher
Author(s):
autogenerated on Mon Jul 1 2024 02:22:43