CovarianceSampling.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 "CovarianceSampling.h"
36 
37 #include <vector>
38 #include <list>
39 #include <utility>
40 #include <unordered_map>
41 
42 // Eigenvalues
43 #include "Eigen/QR"
44 #include "Eigen/Eigenvalues"
45 
46 // CovarianceSamplingDataPointsFilter
47 template <typename T>
49  PointMatcher<T>::DataPointsFilter("CovarianceSamplingDataPointsFilter",
50  CovarianceSamplingDataPointsFilter::availableParameters(), params),
51  nbSample{Parametrizable::get<std::size_t>("nbSample")}
52 {
53  try
54  {
55  const std::uint8_t tnm = this->template get<std::uint8_t>("torqueNorm");
56  normalizationMethod = TorqueNormMethod(tnm);
57  }
58  catch (const InvalidParameter&)
59  {
60  normalizationMethod = TorqueNormMethod::Lavg;
61  }
62 }
63 
64 template <typename T>
67 {
68  DataPoints output(input);
69  inPlaceFilter(output);
70  return output;
71 }
72 
73 template <typename T>
75 {
76  const std::size_t featDim(cloud.features.rows());
77  assert(featDim == 4); //3D pts only
78 
79  //Check number of points
80  const std::size_t nbPoints = cloud.getNbPoints();
81  if(nbSample >= nbPoints)
82  return;
83 
84  //Check if there is normals info
85  if (!cloud.descriptorExists("normals"))
86  throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find normals in descriptors.");
87 
88  const auto& normals = cloud.getDescriptorViewByName("normals");
89 
90  std::vector<std::size_t> keepIndexes;
91  keepIndexes.resize(nbSample);
92 
94  //A.1 and A.2 - Compute candidates
95  std::vector<std::size_t> candidates ;
96  candidates.resize(nbPoints);
97 
98  for (std::size_t i = 0; i < nbPoints; ++i) candidates[i] = i;
99 
100  const std::size_t nbCandidates = candidates.size();
101 
102  //Compute centroid
103  Vector3 center;
104  for(std::size_t i = 0; i < featDim - 1; ++i) center(i) = T(0.);
105 
106  for (std::size_t i = 0; i < nbCandidates; ++i)
107  for (std::size_t f = 0; f < 3; ++f)
108  center(f) += cloud.features(f,candidates[i]);
109 
110  for(std::size_t i = 0; i < 3; ++i) center(i) /= T(nbCandidates);
111 
112  //Compute torque normalization
113  T Lnorm = 1.0;
114 
115  if(normalizationMethod == TorqueNormMethod::L1)
116  {
117  Lnorm = 1.0;
118  }
119  else if(normalizationMethod == TorqueNormMethod::Lavg)
120  {
121  Lnorm = 0.0;
122  for (std::size_t i = 0; i < nbCandidates; ++i)
123  Lnorm += (cloud.features.col(candidates[i]).head(3) - center).norm();
124  Lnorm /= nbCandidates;
125  }
126  else if(normalizationMethod == TorqueNormMethod::Lmax)
127  {
128  const Vector minValues = cloud.features.rowwise().minCoeff();
129  const Vector maxValues = cloud.features.rowwise().maxCoeff();
130  const Vector3 radii = maxValues.head(3) - minValues.head(3);
131 
132  Lnorm = radii.maxCoeff() / 2.; //radii.mean() / 2.;
133  }
134 
135  //A.3 - Compute 6x6 covariance matrix + EigenVectors
136  auto computeCovariance = [Lnorm, nbCandidates, &cloud, &center, &normals, &candidates](Matrix66 & cov) -> void {
137  //Compute F matrix, see Eq. (4)
138  Eigen::Matrix<T, 6, Eigen::Dynamic> F(6, nbCandidates);
139 
140  for(std::size_t i = 0; i < nbCandidates; ++i)
141  {
142  const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c
143  const Vector3 ni = normals.col(candidates[i]).head(3);
144 
145  //compute (1 / L) * (pi - c) x ni
146  F.template block<3, 1>(0, i) = (1. / Lnorm) * p.cross(ni);
147  //set ni part
148  F.template block<3, 1>(3, i) = ni;
149  }
150 
151  // Compute the covariance matrix Cov = FF'
152  cov = F * F.transpose();
153  };
154 
155  Matrix66 covariance;
156  computeCovariance(covariance);
157 
158  Eigen::EigenSolver<Matrix66> solver(covariance);
159  const Matrix66 eigenVe = solver.eigenvectors().real();
160  const Vector6 eigenVa = solver.eigenvalues().real();
161 
163  //B.1 - Compute the v-6 for each candidate
164  std::vector<Vector6, Eigen::aligned_allocator<Vector6>> v; // v[i] = [(pi-c) x ni ; ni ]'
165  v.resize(nbCandidates);
166 
167  for(std::size_t i = 0; i < nbCandidates; ++i)
168  {
169  const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c
170  const Vector3 ni = normals.col(candidates[i]).head(3);
171 
172  v[i].template block<3, 1>(0, 0) = (1. / Lnorm) * p.cross(ni);
173  v[i].template block<3, 1>(3, 0) = ni;
174  }
175 
176  //B.2 - Compute the 6 sorted list based on dot product (vi . Xk) = magnitude, with Xk the kth-EigenVector
177  std::vector<std::list<std::pair<int, T>>> L; // contain list of pair (index, magnitude) contribution to the eigens vectors
178  L.resize(6);
179 
180  //sort by decreasing magnitude
181  auto comp = [](const std::pair<int, T>& p1, const std::pair<int, T>& p2) -> bool {
182  return p1.second > p2.second;
183  };
184 
185  for(std::size_t k = 0; k < 6; ++k)
186  {
187  for(std::size_t i = 0; i < nbCandidates; ++i )
188  {
189  L[k].push_back(std::make_pair(i, std::fabs( v[i].dot(eigenVe.template block<6,1>(0, k)) )));
190  }
191 
192  L[k].sort(comp);
193  }
194 
195  std::vector<T> t(6, T(0.)); //contains the sums of squared magnitudes
196  std::vector<bool> sampledPoints(nbCandidates, false); //maintain flag to avoid resampling the same point in an other list
197 
199  for(std::size_t i = 0; i < nbSample; ++i)
200  {
201  //B.3 - Equally constrained all eigen vectors
202  // magnitude contribute to t_i where i is the indice of th least contrained eigen vector
203 
204  //Find least constrained vector
205  std::size_t k = 0;
206  for (std::size_t i = 0; i < 6; ++i)
207  {
208  if (t[k] > t[i])
209  k = i;
210  }
211  // Add the point from the top of the list corresponding to the dimension to the set of samples
212  while(sampledPoints[L[k].front().first])
213  L[k].pop_front(); //remove already sampled point
214 
215  //Get index to keep
216  const std::size_t idToKeep = static_cast<std::size_t>(L[k].front().first);
217  L[k].pop_front();
218 
219  sampledPoints[idToKeep] = true; //set flag to avoid resampling
220 
221  //B.4 - Update the running total
222  for (std::size_t k = 0; k < 6; ++k)
223  {
224  const T magnitude = v[idToKeep].dot(eigenVe.template block<6, 1>(0, k));
225  t[k] += (magnitude * magnitude);
226  }
227 
228  keepIndexes[i] = candidates[idToKeep];
229  }
230 
231  //TODO: evaluate performances between this solution and sorting the indexes
232  // We build map of (old index to new index), in case we sample pts at the begining of the pointcloud
233  std::unordered_map<std::size_t, std::size_t> mapidx;
234  std::size_t idx = 0;
235 
237  for(std::size_t id : keepIndexes)
238  {
239  //retrieve index from lookup table if sampling in already switched element
240  if(id<idx)
241  id = mapidx[id];
242  //Switch columns id and idx
243  cloud.swapCols(idx, id);
244  //Maintain new index position
245  mapidx[idx] = id;
246  //Update index
247  ++idx;
248  }
249  cloud.conservativeResize(nbSample);
250 }
251 
252 // Compute c = Lambda_6 / Lambda_1, where Lambda_1 <= ... <= Lambda_6
253 // Stability is obtained where c is as close as possible of 1.0
254 template <typename T>
256 {
257  Vector eigenVa = Vector::Identity(6, 1);
258 
259  Eigen::EigenSolver<Matrix66> solver(cov);
260  eigenVa = solver.eigenvalues().real();
261 
262  return eigenVa.maxCoeff() / eigenVa.minCoeff();
263 }
264 
CovarianceSamplingDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: CovarianceSampling.cpp:66
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
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
CovarianceSamplingDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: CovarianceSampling.cpp:74
CovarianceSamplingDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: CovarianceSampling.h:49
CovarianceSamplingDataPointsFilter::CovarianceSamplingDataPointsFilter
CovarianceSamplingDataPointsFilter(const Parameters &params=Parameters())
Definition: CovarianceSampling.cpp:48
CovarianceSamplingDataPointsFilter::Vector
PointMatcher< T >::Vector Vector
Definition: CovarianceSampling.h:58
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
align_sequence.params
params
Definition: align_sequence.py:13
CovarianceSamplingDataPointsFilter::Vector3
Eigen::Matrix< T, 3, 1 > Vector3
Definition: CovarianceSampling.h:63
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:440
PointMatcherSupport::Parametrizable::InvalidParameter
An exception thrown when one tries to fetch the value of an unexisting parameter.
Definition: Parametrizable.h:144
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: pointmatcher/DataPoints.cpp:158
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
CovarianceSamplingDataPointsFilter
Definition: CovarianceSampling.h:40
CovarianceSamplingDataPointsFilter::computeConditionNumber
static T computeConditionNumber(const Matrix66 &cov)
Definition: CovarianceSampling.cpp:255
CovarianceSamplingDataPointsFilter::Matrix66
Eigen::Matrix< T, 6, 6 > Matrix66
Definition: CovarianceSampling.h:61
CovarianceSampling.h
CovarianceSamplingDataPointsFilter::Vector6
Eigen::Matrix< T, 6, 1 > Vector6
Definition: CovarianceSampling.h:62
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


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