PointToPointSimilarity.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 "ErrorMinimizersImpl.h"
37 #include "PointMatcherPrivate.h"
38 #include "Eigen/SVD"
39 
40 using namespace Eigen;
41 
42 template<typename T>
44 {
45 
46  // Copy error element to use as storage later
47  // TODO: check that, might worth it to only copy useful parts
48  ErrorElements mPts = mPts_const;
49 
50  // now minimize on kept points
51  const int dimCount(mPts.reading.features.rows());
52  //const int ptsCount(mPts.reading.features.cols()); //Both point clouds have now the same number of (matched) point
53 
54 
55  // Compute the (weighted) mean of each point cloud
56  //TODO: change the member weights to be a Vector
57  const Vector w = mPts.weights.row(0);
58  const T w_sum_inv = T(1.)/w.sum();
59  const Vector meanReading =
60  (mPts.reading.features.topRows(dimCount-1).array().rowwise() * w.array().transpose()).rowwise().sum() * w_sum_inv;
61  const Vector meanReference =
62  (mPts.reference.features.topRows(dimCount-1).array().rowwise() * w.array().transpose()).rowwise().sum() * w_sum_inv;
63 
64 
65  // Remove the mean from the point clouds
66  mPts.reading.features.topRows(dimCount-1).colwise() -= meanReading;
67  mPts.reference.features.topRows(dimCount-1).colwise() -= meanReference;
68 
69  const T sigma = mPts.reading.features.topRows(dimCount-1).colwise().squaredNorm().cwiseProduct(w.transpose()).sum();
70 
71  // Singular Value Decomposition
72  const Matrix m(mPts.reference.features.topRows(dimCount-1) * w.asDiagonal()
73  * mPts.reading.features.topRows(dimCount-1).transpose());
74  const JacobiSVD<Matrix> svd(m, ComputeThinU | ComputeThinV);
75  Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
76  typedef typename JacobiSVD<Matrix>::SingularValuesType SingularValuesType;
77  SingularValuesType singularValues = svd.singularValues();
78  // It is possible to get a reflection instead of a rotation. In this case, we
79  // take the second best solution, guaranteed to be a rotation. For more details,
80  // read the tech report: "Least-Squares Rigid Motion Using SVD", Olga Sorkine
81  // http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
82  if (rotMatrix.determinant() < 0.)
83  {
84  Matrix tmpV = svd.matrixV().transpose();
85  tmpV.row(dimCount-2) *= -1.;
86  rotMatrix = svd.matrixU() * tmpV;
87  singularValues(dimCount-2) *= -1.;
88  }
89  T scale = singularValues.sum() / sigma;
90  if (sigma < 0.0001) scale = T(1);
91  const Vector trVector(meanReference - scale * rotMatrix * meanReading);
92 
93  Matrix result(Matrix::Identity(dimCount, dimCount));
94  result.topLeftCorner(dimCount-1, dimCount-1) = scale * rotMatrix;
95  result.topRightCorner(dimCount-1, 1) = trVector;
96 
97  return result;
98 }
99 
100 template<typename T>
102  const DataPoints& filteredReading,
103  const DataPoints& filteredReference,
104  const OutlierWeights& outlierWeights,
105  const Matches& matches) const
106 {
107  assert(matches.ids.rows() > 0);
108 
109  // Fetch paired points
110  typename ErrorMinimizer::ErrorElements mPts(filteredReading, filteredReference, outlierWeights, matches);
111 
113 }
114 
115 template<typename T>
117 {
118  //NOTE: computing overlap of 2 point clouds can be complicated due to
119  // the sparse nature of the representation. Here is only an estimate
120  // of the true overlap.
121  const int nbPoints = this->lastErrorElements.reading.features.cols();
122  const int dim = this->lastErrorElements.reading.features.rows();
123  if(nbPoints == 0)
124  {
125  throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
126  }
127 
128  if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise"))
129  {
130  LOG_INFO_STREAM("PointToPointSimilarityErrorMinimizer - warning, no sensor noise found. Using best estimate given outlier rejection instead.");
131  return this->getWeightedPointUsedRatio();
132  }
133 
134  const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
135 
136  const Vector dists = (this->lastErrorElements.reading.features.topRows(dim-1) - this->lastErrorElements.reference.features.topRows(dim-1)).colwise().norm();
137  const T mean = dists.sum()/nbPoints;
138 
139  int count = 0;
140  for(int i=0; i < nbPoints; i++)
141  {
142  if(dists(i) < (mean + noises(0,i)))
143  {
144  count++;
145  }
146  }
147 
148  return (T)count/(T)nbPoints;
149 }
150 
PointToPointSimilarityErrorMinimizer
Definition: PointToPointSimilarity.h:42
PointToPointSimilarityErrorMinimizer::Vector
PointMatcher< T >::Vector Vector
Definition: PointToPointSimilarity.h:49
build_map.T
T
Definition: build_map.py:34
PointToPointSimilarityErrorMinimizer::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: PointToPointSimilarity.cpp:101
LOG_INFO_STREAM
#define LOG_INFO_STREAM(args)
Definition: PointMatcherPrivate.h:58
PointMatcherPrivate.h
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
ErrorMinimizersImpl.h
PointToPointErrorMinimizer::computeResidualError
static T computeResidualError(const ErrorElements &mPts)
Definition: PointToPoint.cpp:155
PointMatcher::ErrorMinimizer::ErrorElements
A structure holding data ready for minimization. The data are "normalized", for instance there are no...
Definition: PointMatcher.h:534
PointToPointSimilarityErrorMinimizer::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: PointToPointSimilarity.cpp:116
icp_advance_api.matches
matches
Definition: icp_advance_api.py:114
PointMatcher::ErrorMinimizer::ErrorElements::reading
DataPoints reading
reading point cloud
Definition: PointMatcher.h:536
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
PointMatcher::Matches
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:371
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
PointToPointSimilarityErrorMinimizer::compute
virtual TransformationParameters compute(const ErrorElements &mPts)
Find the transformation that minimizes the error given matched pair of points. This function most be ...
Definition: PointToPointSimilarity.cpp:43
PointToPointSimilarityErrorMinimizer::OutlierWeights
PointMatcher< T >::OutlierWeights OutlierWeights
Definition: PointToPointSimilarity.h:47
PointMatcher::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182
PointToPointSimilarityErrorMinimizer::Matrix
PointMatcher< T >::Matrix Matrix
Definition: PointToPointSimilarity.h:50


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