PointToPlane.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--2012,
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 
36 #include <iostream>
37 
38 #include "Eigen/SVD"
39 
40 #include "ErrorMinimizersImpl.h"
41 #include "PointMatcherPrivate.h"
42 #include "Functions.h"
43 
44 using namespace Eigen;
45 using namespace std;
46 
52 
53 template<typename T>
55  ErrorMinimizer(name(), availableParameters(), params),
56  force2D(Parametrizable::get<T>("force2D")),
57  force4DOF(Parametrizable::get<T>("force4DOF"))
58 {
59  if(force2D)
60  {
61  if (force4DOF)
62  {
63  throw PointMatcherSupport::ConfigurationError("Force 2D cannot be used together with force4DOF.");
64  }
65  else
66  {
67  LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 2D.");
68  }
69  }
70  else if(force4DOF)
71  {
72  LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 4-DOF (yaw,x,y,z).");
73  }
74  else
75  {
76  LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 3D.");
77  }
78 }
79 
80 template<typename T>
82  ErrorMinimizer(name(), paramsDoc, params),
83  force2D(Parametrizable::get<T>("force2D")),
84  force4DOF(Parametrizable::get<T>("force4DOF"))
85 {
86  if(force2D)
87  {
88  if (force4DOF)
89  {
90  throw PointMatcherSupport::ConfigurationError("Force 2D cannot be used together with force4DOF.");
91  }
92  else
93  {
94  LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 2D.");
95  }
96  }
97  else if(force4DOF)
98  {
99  LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 4-DOF (yaw,x,y,z).");
100  }
101  else
102  {
103  LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 3D.");
104  }
105 }
106 
107 
108 template<typename T, typename MatrixA, typename Vector>
109 void solvePossiblyUnderdeterminedLinearSystem(const MatrixA& A, const Vector & b, Vector & x) {
110  assert(A.cols() == A.rows());
111  assert(b.cols() == 1);
112  assert(b.rows() == A.rows());
113  assert(x.cols() == 1);
114  assert(x.rows() == A.cols());
115 
116  typedef typename PointMatcher<T>::Matrix Matrix;
117 
118  BOOST_AUTO(Aqr, A.fullPivHouseholderQr());
119  if (!Aqr.isInvertible())
120  {
121  // Solve reduced problem R1 x = Q1^T b instead of QR x = b, where Q = [Q1 Q2] and R = [ R1 ; R2 ] such that ||R2|| is small (or zero) and therefore A = QR ~= Q1 * R1
122  const int rank = Aqr.rank();
123  const int rows = A.rows();
124  const Matrix Q1t = Aqr.matrixQ().transpose().block(0, 0, rank, rows);
125  const Matrix R1 = (Q1t * A * Aqr.colsPermutation()).block(0, 0, rank, rows);
126 
127  const bool findMinimalNormSolution = true; // TODO is that what we want?
128 
129  // The under-determined system R1 x = Q1^T b is made unique ..
130  if(findMinimalNormSolution){
131  // by getting the solution of smallest norm (x = R1^T * (R1 * R1^T)^-1 Q1^T b.
132  x = R1.template triangularView<Eigen::Upper>().transpose() * (R1 * R1.transpose()).llt().solve(Q1t * b);
133  } else {
134  // by solving the simplest problem that yields fewest nonzero components in x
135  x.block(0, 0, rank, 1) = R1.block(0, 0, rank, rank).template triangularView<Eigen::Upper>().solve(Q1t * b);
136  x.block(rank, 0, rows - rank, 1).setZero();
137  }
138 
139  x = Aqr.colsPermutation() * x;
140 
141  BOOST_AUTO(ax , (A * x).eval());
142  if (!b.isApprox(ax, 1e-5)) {
143  LOG_INFO_STREAM("PointMatcher::icp - encountered almost singular matrix while minimizing point to plane distance. QR solution was too inaccurate. Trying more accurate approach using double precision SVD.");
144  x = A.template cast<double>().jacobiSvd(ComputeThinU | ComputeThinV).solve(b.template cast<double>()).template cast<T>();
145  ax = A * x;
146 
147  if((b - ax).norm() > 1e-5 * std::max(A.norm() * x.norm(), b.norm())){
148  LOG_WARNING_STREAM("PointMatcher::icp - encountered numerically singular matrix while minimizing point to plane distance and the current workaround remained inaccurate."
149  << " b=" << b.transpose()
150  << " !~ A * x=" << (ax).transpose().eval()
151  << ": ||b- ax||=" << (b - ax).norm()
152  << ", ||b||=" << b.norm()
153  << ", ||ax||=" << ax.norm());
154  }
155  }
156  }
157  else {
158  // Cholesky decomposition
159  x = A.llt().solve(b);
160  }
161 }
162 
163 template<typename T>
165 {
166  ErrorElements mPts = mPts_const;
167  return compute_in_place(mPts);
168 }
169 
170 
171 template<typename T>
173 {
174  const int dim = mPts.reading.features.rows();
175  const int nbPts = mPts.reading.features.cols();
176 
177  // Adjust if the user forces 2D minimization on XY-plane
178  int forcedDim = dim - 1;
179  if(force2D && dim == 4)
180  {
181  mPts.reading.features.conservativeResize(3, Eigen::NoChange);
182  mPts.reading.features.row(2) = Matrix::Ones(1, nbPts);
183  mPts.reference.features.conservativeResize(3, Eigen::NoChange);
184  mPts.reference.features.row(2) = Matrix::Ones(1, nbPts);
185  forcedDim = dim - 2;
186  }
187 
188  // Fetch normal vectors of the reference point cloud (with adjustment if needed)
189  const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim));
190 
191  // Note: Normal vector must be precalculated to use this error. Use appropriate input filter.
192  assert(normalRef.rows() > 0);
193 
194  // Compute cross product of cross = cross(reading X normalRef)
195  Matrix cross;
196  Matrix matrixGamma(3,3);
197  if(!force4DOF)
198  {
199  // Compute cross product of cross = cross(reading X normalRef)
200  cross = this->crossProduct(mPts.reading.features, normalRef);
201  }
202  else
203  {
204  //VK: Instead for "cross" as in 3D, we need only a dot product with the matrixGamma factor for 4DOF
205  //VK: This should be published in 2020 or 2021
206  matrixGamma << 0,-1, 0,
207  1, 0, 0,
208  0, 0, 0;
209  cross = ((matrixGamma*mPts.reading.features.block(0, 0, 3, nbPts)).transpose()*normalRef).diagonal().transpose();
210  }
211 
212 
213  // wF = [weights*cross, weights*normals]
214  // F = [cross, normals]
215  Matrix wF(normalRef.rows()+ cross.rows(), normalRef.cols());
216  Matrix F(normalRef.rows()+ cross.rows(), normalRef.cols());
217 
218  for(int i=0; i < cross.rows(); i++)
219  {
220  wF.row(i) = mPts.weights.array() * cross.row(i).array();
221  F.row(i) = cross.row(i);
222  }
223  for(int i=0; i < normalRef.rows(); i++)
224  {
225  wF.row(i + cross.rows()) = mPts.weights.array() * normalRef.row(i).array();
226  F.row(i + cross.rows()) = normalRef.row(i);
227  }
228 
229  // Unadjust covariance A = wF * F'
230  const Matrix A = wF * F.transpose();
231 
232  const Matrix deltas = mPts.reading.features - mPts.reference.features;
233 
234  // dot product of dot = dot(deltas, normals)
235  Matrix dotProd = Matrix::Zero(1, normalRef.cols());
236 
237  for(int i=0; i<normalRef.rows(); i++)
238  {
239  dotProd += (deltas.row(i).array() * normalRef.row(i).array()).matrix();
240  }
241 
242  // b = -(wF' * dot)
243  const Vector b = -(wF * dotProd.transpose());
244 
245  Vector x(A.rows());
246 
247  solvePossiblyUnderdeterminedLinearSystem<T>(A, b, x);
248 
249  // Transform parameters to matrix
250  Matrix mOut;
251  if(dim == 4 && !force2D)
252  {
253  Eigen::Transform<T, 3, Eigen::Affine> transform;
254  // PLEASE DONT USE EULAR ANGLES!!!!
255  // Rotation in Eular angles follow roll-pitch-yaw (1-2-3) rule
256  /*transform = Eigen::AngleAxis<T>(x(0), Eigen::Matrix<T,1,3>::UnitX())
257  * Eigen::AngleAxis<T>(x(1), Eigen::Matrix<T,1,3>::UnitY())
258  * Eigen::AngleAxis<T>(x(2), Eigen::Matrix<T,1,3>::UnitZ());*/
259 
260  // Normal 6DOF takes the whole rotation vector from the solution to construct the output quaternion
261  if (!force4DOF)
262  {
263  transform = Eigen::AngleAxis<T>(x.head(3).norm(), x.head(3).normalized()); //x=[alpha,beta,gamma,x,y,z]
264  } else // 4DOF needs only one number, the rotation around the Z axis
265  {
266  Vector unitZ(3,1);
267  unitZ << 0,0,1;
268  transform = Eigen::AngleAxis<T>(x(0), unitZ); //x=[gamma,x,y,z]
269  }
270 
271  // Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time!
272  /*const T pitch = -asin(transform(2,0));
273  const T roll = atan2(transform(2,1), transform(2,2));
274  const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch));
275  std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/
276  if (!force4DOF)
277  {
278  transform.translation() = x.segment(3, 3); //x=[alpha,beta,gamma,x,y,z]
279  } else
280  {
281  transform.translation() = x.segment(1, 3); //x=[gamma,x,y,z]
282  }
283 
284  mOut = transform.matrix();
285 
286  if (mOut != mOut)
287  {
288  // Degenerate situation. This can happen when the source and reading clouds
289  // are identical, and then b and x above are 0, and the rotation matrix cannot
290  // be determined, it comes out full of NaNs. The correct rotation is the identity.
291  mOut.block(0, 0, dim-1, dim-1) = Matrix::Identity(dim-1, dim-1);
292  }
293  }
294  else
295  {
296  Eigen::Transform<T, 2, Eigen::Affine> transform;
297  transform = Eigen::Rotation2D<T> (x(0));
298  transform.translation() = x.segment(1, 2);
299 
300  if(force2D)
301  {
302  mOut = Matrix::Identity(dim, dim);
303  mOut.topLeftCorner(2, 2) = transform.matrix().topLeftCorner(2, 2);
304  mOut.topRightCorner(2, 1) = transform.matrix().topRightCorner(2, 1);
305  }
306  else
307  {
308  mOut = transform.matrix();
309  }
310  }
311  return mOut;
312 }
313 
314 
315 template<typename T>
317 {
318  const int dim = mPts.reading.features.rows();
319  const int nbPts = mPts.reading.features.cols();
320 
321  // Adjust if the user forces 2D minimization on XY-plane
322  int forcedDim = dim - 1;
323  if(force2D && dim == 4)
324  {
325  mPts.reading.features.conservativeResize(3, Eigen::NoChange);
326  mPts.reading.features.row(2) = Matrix::Ones(1, nbPts);
327  mPts.reference.features.conservativeResize(3, Eigen::NoChange);
328  mPts.reference.features.row(2) = Matrix::Ones(1, nbPts);
329  forcedDim = dim - 2;
330  }
331 
332  // Fetch normal vectors of the reference point cloud (with adjustment if needed)
333  const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim));
334 
335  // Note: Normal vector must be precalculated to use this error. Use appropriate input filter.
336  assert(normalRef.rows() > 0);
337 
338  const Matrix deltas = mPts.reading.features - mPts.reference.features;
339 
340  // dotProd = dot(deltas, normals) = d.n
341  Matrix dotProd = Matrix::Zero(1, normalRef.cols());
342  for(int i = 0; i < normalRef.rows(); i++)
343  {
344  dotProd += (deltas.row(i).array() * normalRef.row(i).array()).matrix();
345  }
346  // residual = w*(d.n)┬▓
347  dotProd = (mPts.weights.row(0).array() * dotProd.array().square()).matrix();
348 
349  // return sum of the norm of each dot product
350  return dotProd.sum();
351 }
352 
353 
354 template<typename T>
356  const DataPoints& filteredReading,
357  const DataPoints& filteredReference,
358  const OutlierWeights& outlierWeights,
359  const Matches& matches) const
360 {
361  assert(matches.ids.rows() > 0);
362 
363  // Fetch paired points
364  typename ErrorMinimizer::ErrorElements mPts(filteredReading, filteredReference, outlierWeights, matches);
365 
367 }
368 
369 template<typename T>
371 {
372 
373  // Gather some information on what kind of point cloud we have
374  const bool hasReadingNoise = this->lastErrorElements.reading.descriptorExists("simpleSensorNoise");
375  const bool hasReferenceNoise = this->lastErrorElements.reference.descriptorExists("simpleSensorNoise");
376  const bool hasReferenceDensity = this->lastErrorElements.reference.descriptorExists("densities");
377 
378  const int nbPoints = this->lastErrorElements.reading.features.cols();
379  const int dim = this->lastErrorElements.reading.features.rows();
380 
381  // basix safety check
382  if(nbPoints == 0)
383  {
384  throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
385  }
386 
387  Eigen::Array<T, 1, Eigen::Dynamic> uncertainties(nbPoints);
388 
389  // optimal case
390  if (hasReadingNoise && hasReferenceNoise && hasReferenceDensity)
391  {
392  // find median density
393 
394  Matrix densities = this->lastErrorElements.reference.getDescriptorViewByName("densities");
395  vector<T> values(densities.data(), densities.data() + densities.size());
396 
397  // sort up to half the values
398  nth_element(values.begin(), values.begin() + (values.size() * 0.5), values.end());
399 
400  // extract median value
401  const T medianDensity = values[values.size() * 0.5];
402  const T medianRadius = 1.0/pow(medianDensity, 1/3.0);
403 
404  uncertainties = (medianRadius +
405  this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise").array() +
406  this->lastErrorElements.reference.getDescriptorViewByName("simpleSensorNoise").array());
407  }
408  else if(hasReadingNoise && hasReferenceNoise)
409  {
410  uncertainties = this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise") +
411  this->lastErrorElements.reference.getDescriptorViewByName("simpleSensorNoise");
412  }
413  else if(hasReadingNoise)
414  {
415  uncertainties = this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise");
416  }
417  else if(hasReferenceNoise)
418  {
419  uncertainties = this->lastErrorElements.reference.getDescriptorViewByName("simpleSensorNoise");
420  }
421  else
422  {
423  LOG_INFO_STREAM("PointToPlaneErrorMinimizer - warning, no sensor noise and density. Using best estimate given outlier rejection instead.");
424  return this->getWeightedPointUsedRatio();
425  }
426 
427 
428  const Vector dists = (this->lastErrorElements.reading.features.topRows(dim-1) - this->lastErrorElements.reference.features.topRows(dim-1)).colwise().norm();
429 
430 
431  // here we can only loop through a list of links, but we are interested in whether or not
432  // a point has at least one valid match.
433  int count = 0;
434  int nbUniquePoint = 1;
435  Vector lastValidPoint = this->lastErrorElements.reading.features.col(0) * 2.;
436  for(int i=0; i < nbPoints; i++)
437  {
438  const Vector point = this->lastErrorElements.reading.features.col(i);
439 
440  if(lastValidPoint != point)
441  {
442  // NOTE: we tried with the projected distance over the normal vector before:
443  // projectionDist = delta dotProduct n.normalized()
444  // but this doesn't make sense
445 
446 
447  if(PointMatcherSupport::anyabs(dists(i, 0)) < (uncertainties(0,i)))
448  {
449  lastValidPoint = point;
450  count++;
451  }
452  }
453 
454  // Count unique points
455  if(i > 0)
456  {
457  if(point != this->lastErrorElements.reading.features.col(i-1))
458  nbUniquePoint++;
459  }
460 
461  }
462  //cout << "count: " << count << ", nbUniquePoint: " << nbUniquePoint << ", this->lastErrorElements.nbRejectedPoints: " << this->lastErrorElements.nbRejectedPoints << endl;
463 
464  return (T)count/(T)(nbUniquePoint + this->lastErrorElements.nbRejectedPoints);
465 }
466 
467 template struct PointToPlaneErrorMinimizer<float>;
468 template struct PointToPlaneErrorMinimizer<double>;
Vector
PM::Vector Vector
Definition: pypoint_matcher_helper.h:55
PointToPlaneErrorMinimizer::OutlierWeights
PointMatcher< T >::OutlierWeights OutlierWeights
Definition: PointToPlane.h:52
PointToPlaneErrorMinimizer::getOverlap
virtual T getOverlap() const
If not redefined by child class, return the ratio of how many points were used (with weight) for erro...
Definition: PointToPlane.cpp:370
PointToPlaneErrorMinimizer::Vector
PointMatcher< T >::Vector Vector
Definition: PointToPlane.h:56
build_map.T
T
Definition: build_map.py:34
icp_customized.name
string name
Definition: icp_customized.py:45
LOG_INFO_STREAM
#define LOG_INFO_STREAM(args)
Definition: PointMatcherPrivate.h:58
P
PointMatcherSupport::Parametrizable P
Definition: PointToPlane.cpp:48
PointToPlaneErrorMinimizer::force4DOF
const bool force4DOF
Definition: PointToPlane.h:79
Matrix
PM::Matrix Matrix
Definition: pypoint_matcher_helper.h:59
PointMatcherSupport::ConfigurationError
An expception thrown when the yaml config file contains invalid configuration (e.g....
Definition: PointMatcher.h:96
PointMatcherPrivate.h
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
x
x
PointToPlaneErrorMinimizer::Matrix
PointMatcher< T >::Matrix Matrix
Definition: PointToPlane.h:57
ErrorMinimizersImpl.h
solvePossiblyUnderdeterminedLinearSystem
void solvePossiblyUnderdeterminedLinearSystem(const MatrixA &A, const Vector &b, Vector &x)
Definition: PointToPlane.cpp:109
PointMatcher::ErrorMinimizer::ErrorElements
A structure holding data ready for minimization. The data are "normalized", for instance there are no...
Definition: PointMatcher.h:534
PointToPlaneErrorMinimizer::computeResidualError
static T computeResidualError(ErrorElements mPts, const bool &force2D)
Definition: PointToPlane.cpp:316
PointMatcherSupport::Parametrizable::ParametersDoc
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
Definition: Parametrizable.h:187
PointToPlaneErrorMinimizer
Definition: PointToPlane.h:42
align_sequence.params
params
Definition: align_sequence.py:13
ParameterDoc
Parametrizable::ParameterDoc ParameterDoc
Definition: PointToPlane.cpp:50
icp_advance_api.matches
matches
Definition: icp_advance_api.py:114
PointMatcher::Matrix
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
PointMatcher::ErrorMinimizer::ErrorElements::reading
DataPoints reading
reading point cloud
Definition: PointMatcher.h:536
LOG_WARNING_STREAM
#define LOG_WARNING_STREAM(args)
Definition: PointMatcherPrivate.h:68
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
PointMatcher::ErrorMinimizer::ErrorElements::reference
DataPoints reference
reference point cloud
Definition: PointMatcher.h:537
PointToPlaneErrorMinimizer::PointToPlaneErrorMinimizer
PointToPlaneErrorMinimizer(const Parameters &params=Parameters())
Definition: PointToPlane.cpp:54
Parameters
Parametrizable::Parameters Parameters
Definition: PointToPlane.cpp:49
PointToPlaneErrorMinimizer::compute
virtual TransformationParameters compute(const ErrorElements &mPts)
Find the transformation that minimizes the error given matched pair of points. This function most be ...
Definition: PointToPlane.cpp:164
PointToPlaneErrorMinimizer::Parameters
Parametrizable::Parameters Parameters
Definition: PointToPlane.h:46
PointMatcher::Matches
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:371
PointToPlaneErrorMinimizer::compute_in_place
TransformationParameters compute_in_place(ErrorElements &mPts)
Definition: PointToPlane.cpp:172
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
PointToPlaneErrorMinimizer::ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
Definition: PointToPlane.h:48
std
PointMatcherSupport::get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
Definition: Bibliography.cpp:57
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
PointMatcher::ErrorMinimizer::ErrorElements::weights
OutlierWeights weights
weights for every association
Definition: PointMatcher.h:538
PointMatcherSupport::Parametrizable::ParameterDoc
The documentation of a parameter.
Definition: Parametrizable.h:160
PointToPlaneErrorMinimizer::getResidualError
virtual T getResidualError(const DataPoints &filteredReading, const DataPoints &filteredReference, const OutlierWeights &outlierWeights, const Matches &matches) const
If not redefined by child class, return max value for T.
Definition: PointToPlane.cpp:355
Parametrizable
PointMatcherSupport::Parametrizable Parametrizable
Definition: PointToPlane.cpp:47
PointToPlaneErrorMinimizer::force2D
const bool force2D
Definition: PointToPlane.h:78
PointMatcherSupport::anyabs
static T anyabs(const T &v)
Definition: Functions.h:44
PointMatcher::ErrorMinimizer
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
Definition: PointMatcher.h:531
Functions.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
PointMatcherSupport::pow
constexpr T pow(const T base, const std::size_t exponent)
Definition: utils.h:47
ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
Definition: PointToPlane.cpp:51
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
PointMatcher::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182


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