Sphericality.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 "Sphericality.h"
36 
37 
38 /*
39  The SphericalityDataPointFilter estimates a heuristic value which indicates how much local geometry around
40  a given point resemble a plane or a sphere (uniform distribution). The value can lie in <-1,1> interval, -1 for
41  perfect plane, 1 for perfectly uniform distribution. The estimation is based on three eigenvalues coming from another
42  data points filter (this filter can operate only on 3D data). In the case the largest eigenvalue is zero, the filter
43  outputs NaNs. If the middle eigenvalue is zero and the largest one is non-zero, the filter outputs zeros.
44 
45  Implemented by Vladimir Kubelka (kubelvla@gmail.com), NORLAB, Universite Laval, 2020
46 */
47 
48 // Constructor
49 template<typename T>
51  PointMatcher<T>::DataPointsFilter("SphericalityDataPointsFilter",
52  SphericalityDataPointsFilter::availableParameters(), params),
53  keepUnstructureness(Parametrizable::get<int>("keepUnstructureness")),
54  keepStructureness(Parametrizable::get<T>("keepStructureness"))
55 {
56 }
57 
58 // Compute
59 template<typename T>
62  const DataPoints& input)
63 {
64  DataPoints output(input);
65  inPlaceFilter(output);
66  return output;
67 }
68 
69 // In-place filter
70 template<typename T>
72  DataPoints& cloud)
73 {
74 
75  typedef typename DataPoints::View View;
76  typedef typename DataPoints::Label Label;
77  typedef typename DataPoints::Labels Labels;
78 
79  const size_t pointsCount(cloud.features.cols());
80  const size_t descDim(cloud.descriptors.rows());
81  const size_t labelDim(cloud.descriptorLabels.size());
82 
83  // Check that the required eigenValue descriptor exists in the pointcloud
84  if (!cloud.descriptorExists("eigValues"))
85  {
86  throw InvalidField("SphericalityDataPointsFilter: Error, no eigValues found in descriptors.");
87  }
88 
89  // Validate descriptors and labels
90  size_t insertDim(0);
91  for(unsigned int i = 0; i < labelDim ; ++i)
92  insertDim += cloud.descriptorLabels[i].span;
93  if (insertDim != descDim)
94  throw InvalidField("SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
95 
96  // Reserve memory for new descriptors
97  const size_t unidimensionalDescriptorDimension(1);
98 
99 
100  boost::optional<View> sphericality; // these optionals may cause a compiler warning, but it is ok,
101  boost::optional<View> unstructureness; // it is intended to be uninitialized
102  boost::optional<View> structureness;
103 
104  Labels cloudLabels;
105  cloudLabels.push_back(Label("sphericality", unidimensionalDescriptorDimension));
106  if (keepUnstructureness)
107  cloudLabels.push_back(Label("unstructureness", unidimensionalDescriptorDimension));
108  if (keepStructureness)
109  cloudLabels.push_back(Label("structureness", unidimensionalDescriptorDimension));
110 
111  // Reserve memory
112  cloud.allocateDescriptors(cloudLabels);
113 
114  // Get the views
115  const View eigValues = cloud.getDescriptorViewByName("eigValues");
116  if (eigValues.rows() != 3) // And check the dimensions
117  {
118  throw InvalidField("SphericalityDataPointsFilter: Error, the number of eigValues is not 3.");
119  }
120 
121  sphericality = cloud.getDescriptorViewByName("sphericality");
122  if (keepUnstructureness)
123  unstructureness = cloud.getDescriptorViewByName("unstructureness");
124  if (keepStructureness)
125  structureness = cloud.getDescriptorViewByName("structureness");
126 
127  // Iterate through the point cloud and evaluate the Sphericality
128  for (size_t i = 0; i < pointsCount; ++i)
129  {
130  // extract the three eigenvalues relevant to the current point
131  Vector eig_vals_col = eigValues.col(i);
132  // might be already sorted but sort anyway
133  std::sort(eig_vals_col.data(),eig_vals_col.data()+eig_vals_col.size());
134 
135  // Finally, evaluate the Sphericality
136  T sphericalityVal;
137  T unstructurenessVal;
138  T structurenessVal;
139 
140  // First, avoid division by zero
141  //TODO: Is there a more suitable limit for considering the values almost-zero? (VK)
142  if (eig_vals_col(2) < std::numeric_limits<T>::min() ||
143  eig_vals_col(1) < 0.0 ||
144  eig_vals_col(0) < 0.0)
145  {
146  // If the largest eigenvalue is zero or even worse -- any of them is negative,
147  // these descriptors are not well defined (0/0 or nonsense input) and assigned NaN
148  sphericalityVal = std::numeric_limits<T>::quiet_NaN();
149  unstructurenessVal = std::numeric_limits<T>::quiet_NaN();
150  structurenessVal = std::numeric_limits<T>::quiet_NaN();
151 
152  } else if (eig_vals_col(1) < std::numeric_limits<T>::min()) {
153  // If there are two zero eigenvalues and one non-zero, we assign zeros to the heuristic
154  sphericalityVal = 0.0;
155  unstructurenessVal = 0.0;
156  structurenessVal = 0.0;
157 
158  } else {
159  // Otherwise, follow eq.(1) from Kubelka V. et al., "Radio propagation models for differential GNSS based
160  // on dense point clouds", JFR, hopefully published in 2020
161  unstructurenessVal = eig_vals_col(0) / eig_vals_col(2);
162  structurenessVal = (eig_vals_col(1) / eig_vals_col(2)) *
163  ((eig_vals_col(1) - eig_vals_col(0)) / sqrt(eig_vals_col(0)*eig_vals_col(0) + eig_vals_col(1)*eig_vals_col(1)));
164  sphericalityVal = unstructurenessVal - structurenessVal;
165  }
166 
167  // store it in the pointcloud
168  // sphericality always
169  (sphericality.get())(0,i) = sphericalityVal;
170  // these two only when requested by the user
171  if (unstructureness)
172  (unstructureness.get())(0,i) = unstructurenessVal;
173  if (structureness)
174  (structureness.get())(0,i) = structurenessVal;
175 
176  }
177 }
178 
Label
DataPoints::Label Label
Definition: pypoint_matcher_helper.h:17
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
DataPointsFilter
PM::DataPointsFilter DataPointsFilter
Definition: pypoint_matcher_helper.h:22
build_map.T
T
Definition: build_map.py:34
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
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
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
SphericalityDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: Sphericality.h:54
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
SphericalityDataPointsFilter
Definition: Sphericality.h:50
Sphericality.h
align_sequence.params
params
Definition: align_sequence.py:13
SphericalityDataPointsFilter::Vector
PointMatcher< T >::Vector Vector
Definition: Sphericality.h:59
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
Labels
DataPoints::Labels Labels
Definition: pypoint_matcher_helper.h:18
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
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
SphericalityDataPointsFilter::SphericalityDataPointsFilter
SphericalityDataPointsFilter(const Parameters &params=Parameters())
Definition: Sphericality.cpp:50
SphericalityDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: Sphericality.cpp:61
SphericalityDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: Sphericality.cpp:71
PointMatcher::DataPoints::View
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:210
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
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:221


libpointmatcher
Author(s):
autogenerated on Mon Jan 1 2024 03:24:43