SpectralDecomposition.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 "SpectralDecomposition.h"
36 
37 #include <random>
38 
39 // SpectralDecomposition
40 template <typename T>
42  PointMatcher<T>::DataPointsFilter("SpectralDecompositionDataPointsFilter",
43  SpectralDecompositionDataPointsFilter::availableParameters(), params),
44  k{Parametrizable::get<std::size_t>("k")},
45  sigma{Parametrizable::get<T>("sigma")},
46  radius{Parametrizable::get<T>("radius")},
47  itMax{Parametrizable::get<std::size_t>("itMax")},
48  keepNormals{Parametrizable::get<bool>("keepNormals")},
49  keepLabels{Parametrizable::get<bool>("keepLabels")},
50  keepLambdas{Parametrizable::get<bool>("keepLambdas")},
51  keepTensors{Parametrizable::get<bool>("keepTensors")}
52 {
53 }
54 
55 template <typename T>
58 {
59  DataPoints output(input);
60  inPlaceFilter(output);
61  return output;
62 }
63 
64 template <typename T>
66 {
67  const std::size_t nbPts = cloud.getNbPoints();
68 
69  if(k > nbPts) return;
70 
71  TensorVoting<T> tv{sigma, k};
72 
73 //--- 1. Vote to determine prefered orientation + density estimation -----------
75  tv.cfvote(cloud, true);
76  tv.decompose();
77  tv.toDescriptors();
78  addDescriptor(cloud, tv, false /*normals*/, false /*labels*/, true /*lambdas*/, false /*tensors*/);
79 
80 //--- 2. Filter iteratively on each measure (surfaceness, curveness, pointness) to uniformize density
81  std::size_t it = 0;
82  const std::size_t itMax_ = itMax;
83  const std::size_t k_ = k;
84  std::size_t oldnbPts = nbPts;
85 
86  auto checkConvergence = [&oldnbPts, &it, &itMax_, &k_](const DataPoints& pts, const std::size_t threshold) mutable ->bool{
87  const std::size_t nbPts = pts.getNbPoints();
88  bool ret = (oldnbPts - nbPts) < threshold;
89 
90  oldnbPts = nbPts;
91 
92  return ret or ++it >= itMax_ or k_ >= nbPts;
93  };
94 
95  const T xi3 = xi_expectation(3, sigma, radius);
96  const T xi2 = xi_expectation(2, sigma, radius);
97  const T xi1 = xi_expectation(1, sigma, radius);
98 
99  do
100  {
101  // 2.1 On pointness
102  filterPointness(cloud, xi3, tv.k);
103  // 2.2 On curveness
104  filterCurveness(cloud, xi1, tv.k);
105  // 2.3 On surfaceness
106  filterSurfaceness(cloud, xi2, tv.k);
107 
108  //Re-compute vote...
109  tv.encode(cloud, TensorVoting<T>::Encoding::BALL);
110  tv.cfvote(cloud, true);
111  tv.decompose();
112  tv.toDescriptors();
113 
114  addDescriptor(cloud, tv, false /*normals*/, false /*labels*/, true /*lambdas*/, false /*tensors*/);
115  }
116  while(not checkConvergence(cloud, 5 /*delta points*/));
117 
118 //--- 3. Re-encode as Aware tensors + Re-vote ----------------------------------
119  addDescriptor(cloud, tv, false /*normals*/, false /*labels*/, false /*lambdas*/, true /*tensors*/);
120  tv.encode(cloud, TensorVoting<T>::Encoding::AWARE_TENSOR);
121  tv.cfvote(cloud);
122  tv.decompose();
123  tv.toDescriptors();
124 
125 //--- 4. Add descriptors
126  addDescriptor(cloud, tv, keepNormals, true, keepLambdas, keepTensors);//TODO: add remove not kept descriptors
127 
128 //--- 5. Remove outliers
129  removeOutlier(cloud, tv);
130 }
131 
132 
133 template <typename T>
134 void SpectralDecompositionDataPointsFilter<T>::addDescriptor(DataPoints& pts, const TensorVoting<T> &tv, bool keepNormals_, bool keepLabels_, bool keepLambdas_, bool keepTensors_) const
135 {
136  const std::size_t nbPts = pts.getNbPoints();
137 
138  Matrix labels = Matrix::Zero(1, nbPts);
139  Matrix l1 = PM::Matrix::Zero(1, nbPts);
140  Matrix l2 = PM::Matrix::Zero(1, nbPts);
141  Matrix l3 = PM::Matrix::Zero(1, nbPts);
142 
143  if(keepLabels_ or keepLambdas_)
144  {
145  #pragma omp parallel for
146  for(std::size_t i = 0; i < nbPts; ++i)
147  {
148  const T lambda1 = tv.surfaceness(i) + tv.curveness(i) + tv.pointness(i);
149  const T lambda2 = tv.curveness(i) + tv.pointness(i);
150  const T lambda3 = tv.pointness(i);
151 
152  int index;
153  Vector coeff = (Vector(3) << lambda3, (lambda2 - lambda3), (lambda1 - lambda2)).finished();
154  coeff.maxCoeff(&index);
155 
156  labels(i) = index + 1 ;
157 
158  l1(i) = lambda1 * k;
159  l2(i) = lambda2 * k;
160  l3(i) = lambda3 * k;
161  }
162  }
163  try
164  {
165  pts.addDescriptor("surfaceness", tv.surfaceness);
166  pts.addDescriptor("curveness", tv.curveness);
167  pts.addDescriptor("pointness", tv.pointness);
168 
169  if(keepLambdas_)
170  {
171  pts.addDescriptor("lambda1", l1);
172  pts.addDescriptor("lambda2", l2);
173  pts.addDescriptor("lambda3", l3);
174  }
175 
176  if(keepNormals_)
177  {
178  pts.addDescriptor("normals", tv.normals);
179  pts.addDescriptor("tangents", tv.tangents);
180  }
181  if(keepLabels_)
182  {
183  pts.addDescriptor("labels", labels);
184  }
185  if(keepTensors_)
186  {
187  pts.addDescriptor("sticks", tv.sticks);
188  pts.addDescriptor("plates", tv.plates);
189  pts.addDescriptor("balls", tv.balls);
190  }
191  }
192  catch (...) {
193  std::cerr << "SpectralDecomposition<T>::inPlaceFilter::addDescriptor: Cannot add descriptors to pointcloud" << std::endl;
194  }
195 
196 }
197 
198 
199 //------------------------------------------------------------------------------
200 // Outlier filter
201 //------------------------------------------------------------------------------
202 template <typename T>
204 {
205  static constexpr int POINT = 0;
206  static constexpr int CURVE = 1;
207  static constexpr int SURFACE = 2;
208 
209  static constexpr T th = 0.1; //threshold at 10%
210 
211  const std::size_t nbPts = pts.getNbPoints();
212 
213  const T th_p = (tv.pointness.maxCoeff() - tv.pointness.minCoeff()) * th + tv.pointness.minCoeff();
214  const T th_c = (tv.curveness.maxCoeff() - tv.curveness.minCoeff()) * th + tv.curveness.minCoeff();
215  const T th_s = (tv.surfaceness.maxCoeff() - tv.surfaceness.minCoeff()) * th + tv.surfaceness.minCoeff();
216 
217 
218  std::size_t j = 0;
219  for (std::size_t i = 0; i < nbPts; ++i)
220  {
221  const T surfaceness = tv.surfaceness(i);
222  const T curveness = tv.curveness(i);
223  const T pointness = tv.pointness(i);
224 
225  int label;
226  (Vector(3) << pointness, curveness, surfaceness).finished().maxCoeff(&label);
227 
228  bool keepPt = ((label == POINT) and (pointness >= th_p))
229  or ((label == CURVE) and (curveness >= th_c))
230  or ((label == SURFACE) and (surfaceness >= th_s));
231 
232  if (keepPt)
233  {
234  pts.setColFrom(j, pts, i);
235  ++j;
236  }
237  }
238  pts.conservativeResize(j);
239 }
240 
241 //------------------------------------------------------------------------------
242 // Filter
243 //------------------------------------------------------------------------------
244 template <typename T>
246 {
247  constexpr std::size_t seed = 1;
248  std::mt19937 gen(seed); //Standard mersenne_twister_engine seeded with seed
249  std::uniform_real_distribution<> uni01(0., 1.);
250 
251  const std::size_t nbPts = pts.getNbPoints();
252 
253  // Check field exists
254  if (!pts.descriptorExists("lambda1") or !pts.descriptorExists("lambda2") or !pts.descriptorExists("lambda3"))
255  {
256  throw InvalidField("SpectralDecomposition<T>::filter: Error, lambdas field not found in descriptors.");
257  }
258 
259  const auto& lambda1 = pts.getDescriptorViewByName("lambda1");
260  const auto& lambda2 = pts.getDescriptorViewByName("lambda2");
261  const auto& lambda3 = pts.getDescriptorViewByName("lambda3");
262 
263  std::size_t j = 0;
264  for (std::size_t i = 0; i < nbPts; ++i)
265  {
266  const T randv = uni01(gen);
267 
268  const T nl1 = lambda1(0,i) / k;
269  const T nl2 = lambda2(0,i) / k;
270  const T nl3 = lambda3(0,i) / k;
271 
272  if (nl1 < xi or nl2 < 0.75 * xi or nl3 < 0.75 * xi or randv < 0.5)
273  {
274  pts.setColFrom(j, pts, i);
275  ++j;
276  }
277  }
278  pts.conservativeResize(j);
279 }
280 
281 template <typename T>
283 {
284  constexpr std::size_t seed = 1;
285  std::mt19937 gen(seed); //Standard mersenne_twister_engine seeded with seed
286  std::uniform_real_distribution<> uni01(0., 1.);
287 
288  const std::size_t nbPts = pts.getNbPoints();
289 
290  // Check field exists
291  if (!pts.descriptorExists("lambda1") or !pts.descriptorExists("lambda2") or !pts.descriptorExists("lambda3"))
292  {
293  throw InvalidField("SpectralDecomposition<T>::filter: Error, lambdas field not found in descriptors.");
294  }
295 
296  const auto& lambda1 = pts.getDescriptorViewByName("lambda1");
297  const auto& lambda2 = pts.getDescriptorViewByName("lambda2");
298  const auto& lambda3 = pts.getDescriptorViewByName("lambda3");
299 
300  std::size_t j = 0;
301  for (std::size_t i = 0; i < nbPts; ++i)
302  {
303  const T randv = uni01(gen);
304 
305  const T nl1 = lambda1(0,i) / k;
306  const T nl2 = lambda2(0,i) / k;
307  const T nl3 = lambda3(0,i) / k;
308 
309  if (nl1 < xi or nl2 < xi or nl3 < 0.5 * xi or randv < 0.5)
310  {
311  pts.setColFrom(j, pts, i);
312  ++j;
313  }
314  }
315  pts.conservativeResize(j);
316 }
317 
318 template <typename T>
320 {
321  constexpr std::size_t seed = 1;
322  std::mt19937 gen(seed); //Standard mersenne_twister_engine seeded with seed
323  std::uniform_real_distribution<> uni01(0., 1.);
324 
325  const std::size_t nbPts = pts.getNbPoints();
326 
327  // Check field exists
328  if (!pts.descriptorExists("lambda1") or !pts.descriptorExists("lambda2") or !pts.descriptorExists("lambda3"))
329  {
330  throw InvalidField("SpectralDecomposition<T>::filter: Error, lambdas field not found in descriptors.");
331  }
332 
333  const auto& lambda1 = pts.getDescriptorViewByName("lambda1");
334  const auto& lambda2 = pts.getDescriptorViewByName("lambda2");
335  const auto& lambda3 = pts.getDescriptorViewByName("lambda3");
336 
337  std::size_t j = 0;
338  for (std::size_t i = 0; i < nbPts; ++i)
339  {
340  const T randv = uni01(gen);
341 
342  const T nl1 = lambda1(0,i) / k;
343  const T nl2 = lambda2(0,i) / k;
344  const T nl3 = lambda3(0,i) / k;
345 
346  if (nl1 < (5./6.) * xi or nl2 < (5./6.) * xi or nl3 < (5./6.) * xi or randv < 0.2)
347  {
348  pts.setColFrom(j, pts, i);
349  ++j;
350  }
351  }
352  pts.conservativeResize(j);
353 }
354 
SpectralDecompositionDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: SpectralDecomposition.cpp:65
SpectralDecompositionDataPointsFilter::SpectralDecompositionDataPointsFilter
SpectralDecompositionDataPointsFilter(const Parameters &params=Parameters())
Definition: SpectralDecomposition.cpp:41
Vector
PM::Vector Vector
Definition: pypoint_matcher_helper.h:55
SpectralDecompositionDataPointsFilter::filterCurveness
void filterCurveness(DataPoints &pts, T xi, std::size_t k) const
Definition: SpectralDecomposition.cpp:282
TensorVoting::normals
Matrix normals
Definition: sparsetv.h:96
SpectralDecompositionDataPointsFilter::Vector
PM::Vector Vector
Definition: SpectralDecomposition.h:69
build_map.T
T
Definition: build_map.py:34
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: pointmatcher/DataPoints.cpp:583
PointMatcher::DataPoints::addDescriptor
void addDescriptor(const std::string &name, const Matrix &newDescriptor)
Add a descriptor by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:533
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
TensorVoting::curveness
Matrix curveness
Definition: sparsetv.h:93
TensorVoting
Definition: sparsetv.h:52
SpectralDecompositionDataPointsFilter::addDescriptor
void addDescriptor(DataPoints &pts, const TensorVoting< T > &tv, bool keepNormals_, bool keepLabels_, bool keepLambdas_, bool keepTensors_) const
Definition: SpectralDecomposition.cpp:134
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
TensorVoting::surfaceness
Matrix surfaceness
Definition: sparsetv.h:92
align_sequence.params
params
Definition: align_sequence.py:13
TensorVoting::balls
Matrix balls
Definition: sparsetv.h:101
PointMatcher::DataPoints::setColFrom
void setColFrom(Index thisCol, const DataPoints &that, Index thatCol)
Set column thisCol equal to column thatCol of that, copy features and descriptors if any....
Definition: pointmatcher/DataPoints.cpp:393
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:440
SpectralDecomposition.h
SpectralDecompositionDataPointsFilter
Definition: SpectralDecomposition.h:49
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: pointmatcher/DataPoints.cpp:158
TensorVoting::encode
void encode(const DP &pts, Encoding encoding=Encoding::UBALL)
Definition: sparsetv.hpp:55
SpectralDecompositionDataPointsFilter::filterPointness
void filterPointness(DataPoints &pts, T xi, std::size_t k) const
Definition: SpectralDecomposition.cpp:319
SpectralDecompositionDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: SpectralDecomposition.h:59
TensorVoting::plates
Matrix plates
Definition: sparsetv.h:100
PointMatcher::DataPoints::conservativeResize
void conservativeResize(Index pointCount)
Resize the cloud to pointCount points, conserving existing ones.
Definition: pointmatcher/DataPoints.cpp:328
TensorVoting::sticks
Matrix sticks
Definition: sparsetv.h:99
TensorVoting::pointness
Matrix pointness
Definition: sparsetv.h:94
SpectralDecompositionDataPointsFilter::removeOutlier
void removeOutlier(DataPoints &pts, const TensorVoting< T > &tv) const
Definition: SpectralDecomposition.cpp:203
TensorVoting::tangents
Matrix tangents
Definition: sparsetv.h:97
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
SpectralDecompositionDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: SpectralDecomposition.cpp:57
SpectralDecompositionDataPointsFilter::Matrix
PM::Matrix Matrix
Definition: SpectralDecomposition.h:68
SpectralDecompositionDataPointsFilter::filterSurfaceness
void filterSurfaceness(DataPoints &pts, T xi, std::size_t k) const
Definition: SpectralDecomposition.cpp:245


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