PointToPoint.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 PointMatcher<T>::ErrorMinimizer("PointToPointErrorMinimizer",
45  ParametersDoc(),
46  Parameters()) {}
47 
48 template<typename T>
50  ErrorMinimizer(className, paramsDoc, params)
51 {
52 }
53 
54 template<typename T>
56 {
57  ErrorElements mPts = mPts_const;
58  return compute_in_place(mPts);
59 }
60 
61 template<typename T>
63  const int dimCount(mPts.reading.features.rows());
64  //const int ptsCount(mPts.reading.features.cols()); //Both point clouds have now the same number of (matched) point
65 
66 
67  const Vector w = mPts.weights.row(0);
68  const T w_sum_inv = T(1.)/w.sum();
69  const Vector meanReading =
70  (mPts.reading.features.topRows(dimCount-1).array().rowwise() * w.array().transpose()).rowwise().sum() * w_sum_inv;
71  const Vector meanReference =
72  (mPts.reference.features.topRows(dimCount-1).array().rowwise() * w.array().transpose()).rowwise().sum() * w_sum_inv;
73 
74 
75  // Remove the mean from the point clouds
76  mPts.reading.features.topRows(dimCount-1).colwise() -= meanReading;
77  mPts.reference.features.topRows(dimCount-1).colwise() -= meanReference;
78 
79  // Singular Value Decomposition
80  const Matrix m(mPts.reference.features.topRows(dimCount-1) * w.asDiagonal()
81  * mPts.reading.features.topRows(dimCount-1).transpose());
82  const JacobiSVD<Matrix> svd(m, ComputeThinU | ComputeThinV);
83  Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
84  // It is possible to get a reflection instead of a rotation. In this case, we
85  // take the second best solution, guaranteed to be a rotation. For more details,
86  // read the tech report: "Least-Squares Rigid Motion Using SVD", Olga Sorkine
87  // http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
88  if (rotMatrix.determinant() < 0.)
89  {
90  Matrix tmpV = svd.matrixV().transpose();
91  tmpV.row(dimCount-2) *= -1.;
92  rotMatrix = svd.matrixU() * tmpV;
93  }
94  const Vector trVector(meanReference - rotMatrix * meanReading);
95 
96  Matrix result(Matrix::Identity(dimCount, dimCount));
97  result.topLeftCorner(dimCount-1, dimCount-1) = rotMatrix;
98  result.topRightCorner(dimCount-1, 1) = trVector;
99 
100  return result;
101 }
102 
103 template<typename T>
105  const DataPoints& filteredReading,
106  const DataPoints& filteredReference,
107  const OutlierWeights& outlierWeights,
108  const Matches& matches) const
109 {
110  assert(matches.ids.rows() > 0);
111 
112  // Fetch paired points
113  typename ErrorMinimizer::ErrorElements mPts(filteredReading, filteredReference, outlierWeights, matches);
114 
116 }
117 
118 template<typename T>
120 {
121  //NOTE: computing overlap of 2 point clouds can be complicated due to
122  // the sparse nature of the representation. Here is only an estimate
123  // of the true overlap.
124  const int nbPoints = this->lastErrorElements.reading.features.cols();
125  const int dim = this->lastErrorElements.reading.features.rows();
126  if(nbPoints == 0)
127  {
128  throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
129  }
130 
131  if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise"))
132  {
133  LOG_INFO_STREAM("PointToPointErrorMinimizer - warning, no sensor noise found. Using best estimate given outlier rejection instead.");
134  return this->getWeightedPointUsedRatio();
135  }
136 
137  const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
138 
139  const Vector dists = (this->lastErrorElements.reading.features.topRows(dim-1) - this->lastErrorElements.reference.features.topRows(dim-1)).colwise().norm();
140  const T mean = dists.sum()/nbPoints;
141 
142  int count = 0;
143  for(int i=0; i < nbPoints; i++)
144  {
145  if(dists(i) < (mean + noises(0,i)))
146  {
147  count++;
148  }
149  }
150 
151  return (T)count/(T)nbPoints;
152 }
153 
154 template<typename T>
156 {
157  //typedef typename PointMatcher<T>::Matrix Matrix;
158 
159  const Matrix deltas = mPts.reading.features - mPts.reference.features;
160 
161  // return sum of the norm of each delta
162  Matrix deltaNorms = deltas.colwise().norm();
163  return deltaNorms.sum();
164 }
165 
166 template struct PointToPointErrorMinimizer<float>;
167 template struct PointToPointErrorMinimizer<double>;
PointToPointErrorMinimizer::OutlierWeights
PointMatcher< T >::OutlierWeights OutlierWeights
Definition: PointToPoint.h:51
PointToPointErrorMinimizer::ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
Definition: PointToPoint.h:46
build_map.T
T
Definition: build_map.py:34
LOG_INFO_STREAM
#define LOG_INFO_STREAM(args)
Definition: PointMatcherPrivate.h:58
PointToPointErrorMinimizer::compute
virtual TransformationParameters compute(const ErrorElements &mPts)
Find the transformation that minimizes the error given matched pair of points. This function most be ...
Definition: PointToPoint.cpp:55
PointToPointErrorMinimizer::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: PointToPoint.cpp:119
PointToPointErrorMinimizer::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: PointToPoint.cpp:104
PointToPointErrorMinimizer::Matrix
PointMatcher< T >::Matrix Matrix
Definition: PointToPoint.h:54
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherPrivate.h
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
ErrorMinimizersImpl.h
testing::internal::string
::std::string string
Definition: gtest.h:1979
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
align_sequence.params
params
Definition: align_sequence.py:13
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
PointToPointErrorMinimizer::Vector
PointMatcher< T >::Vector Vector
Definition: PointToPoint.h:53
PointToPointErrorMinimizer::PointToPointErrorMinimizer
PointToPointErrorMinimizer()
Definition: PointToPoint.cpp:43
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
PointMatcher::ErrorMinimizer
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
Definition: PointMatcher.h:531
PointToPointErrorMinimizer::compute_in_place
TransformationParameters compute_in_place(ErrorElements &mPts)
Definition: PointToPoint.cpp:62
PointToPointErrorMinimizer
Definition: PointToPoint.h:42
PointToPointErrorMinimizer::Parameters
Parametrizable::Parameters Parameters
Definition: PointToPoint.h:45
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