TransformationCheckersImpl.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 
37 #include "Functions.h"
38 #include <boost/math/special_functions/fpclassify.hpp>
39 
40 using namespace std;
41 using namespace PointMatcherSupport;
42 
43 //--------------------------------------
44 // max iteration counter
45 template<typename T>
47  TransformationChecker("CounterTransformationChecker", CounterTransformationChecker::availableParameters(), params),
48  maxIterationCount(Parametrizable::get<unsigned>("maxIterationCount"))
49 {
50  this->limits.setZero(1);
51  this->limits(0) = maxIterationCount;
52 
53  this->conditionVariableNames.push_back("Iteration");
54  this->limitNames.push_back("Max iteration");
55 }
56 
57 template<typename T>
59 {
60  this->conditionVariables.setZero(1);
61 }
62 
63 template<typename T>
65 {
66  this->conditionVariables(0)++;
67 
68  //std::cout << "Iter: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
69  //cerr << parameters << endl;
70 
71  if (this->conditionVariables(0) >= this->limits(0))
72  {
73  iterate = false;
75  }
76 }
77 
80 
81 
82 //--------------------------------------
83 // error
84 template<typename T>
86  TransformationChecker("DifferentialTransformationChecker", DifferentialTransformationChecker::availableParameters(), params),
87  minDiffRotErr(Parametrizable::get<T>("minDiffRotErr")),
88  minDiffTransErr(Parametrizable::get<T>("minDiffTransErr")),
89  smoothLength(Parametrizable::get<unsigned>("smoothLength"))
90 {
91  this->limits.setZero(2);
92  this->limits(0) = minDiffRotErr;
93  this->limits(1) = minDiffTransErr;
94 
95  this->conditionVariableNames.push_back("Mean abs differential rot err");
96  this->conditionVariableNames.push_back("Mean abs differential trans err");
97  this->limitNames.push_back("Min differential rotation err");
98  this->limitNames.push_back("Min differential translation err");
99 
100 }
101 
102 template<typename T>
104 {
105  this->conditionVariables.setZero(2);
106 
107  rotations.clear();
108  translations.clear();
109 
110  if (parameters.rows() == 4)
111  {
112  rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
113  }
114  else
115  {
116  // Handle the 2D case
117  Eigen::Matrix<T,3,3> m(Matrix::Identity(3,3));
118  m.topLeftCorner(2,2) = parameters.topLeftCorner(2,2);
119  rotations.push_back(Quaternion(m));
120  }
121 
122  const unsigned int nbRows = parameters.rows()-1;
123  translations.push_back(parameters.topRightCorner(nbRows,1));
124 }
125 
126 template<typename T>
128 {
129  typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
130 
131  rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
132  const unsigned int nbRows = parameters.rows()-1;
133  translations.push_back(parameters.topRightCorner(nbRows,1));
134 
135  this->conditionVariables.setZero(2);
136  if(rotations.size() > smoothLength)
137  {
138  for(size_t i = rotations.size()-1; i >= rotations.size()-smoothLength; i--)
139  {
140  //Compute the mean derivative
141  this->conditionVariables(0) += anyabs(rotations[i].angularDistance(rotations[i-1]));
142  this->conditionVariables(1) += anyabs((translations[i] - translations[i-1]).norm());
143  }
144 
145  this->conditionVariables /= smoothLength;
146 
147  if(this->conditionVariables(0) < this->limits(0) && this->conditionVariables(1) < this->limits(1))
148  iterate = false;
149  }
150 
151  //std::cout << "Abs Rotation: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
152  //std::cout << "Abs Translation: " << this->conditionVariables(1) << " / " << this->limits(1) << std::endl;
153 
154  if (boost::math::isnan(this->conditionVariables(0)))
155  throw ConvergenceError("abs rotation norm not a number");
156  if (boost::math::isnan(this->conditionVariables(1)))
157  throw ConvergenceError("abs translation norm not a number");
158 }
159 
162 
163 //--------------------------------------
164 // bound
165 
166 template<typename T>
168  TransformationChecker("BoundTransformationChecker", BoundTransformationChecker::availableParameters(), params),
169  maxRotationNorm(Parametrizable::get<T>("maxRotationNorm")),
170  maxTranslationNorm(Parametrizable::get<T>("maxTranslationNorm"))
171 {
172  this->limits.setZero(2);
173  this->limits(0) = maxRotationNorm;
174  this->limits(1) = maxTranslationNorm;
175 
176  this->limitNames.push_back("Max rotation angle");
177  this->limitNames.push_back("Max translation norm");
178  this->conditionVariableNames.push_back("Rotation angle");
179  this->conditionVariableNames.push_back("Translation norm");
180 }
181 
182 template<typename T>
184 {
185  this->conditionVariables.setZero(2);
186  if (parameters.rows() == 4)
187  initialRotation3D = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
188  else if (parameters.rows() == 3)
189  initialRotation2D = acos(parameters(0,0));
190  else
191  throw runtime_error("BoundTransformationChecker only works in 2D or 3D");
192 
193  const unsigned int nbRows = parameters.rows()-1;
194  initialTranslation = parameters.topRightCorner(nbRows,1);
195 }
196 
197 template<typename T>
199 {
200  typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
201 
202  if (parameters.rows() == 4)
203  {
204  const Quaternion currentRotation = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
205  this->conditionVariables(0) = currentRotation.angularDistance(initialRotation3D);
206  }
207  else if (parameters.rows() == 3)
208  {
209  const T currentRotation(acos(parameters(0,0)));
210  this->conditionVariables(0) = normalizeAngle(currentRotation - initialRotation2D);
211  }
212  else
213  assert(false);
214  const unsigned int nbRows = parameters.rows()-1;
215  const Vector currentTranslation = parameters.topRightCorner(nbRows,1);
216  this->conditionVariables(1) = (currentTranslation - initialTranslation).norm();
217  if (this->conditionVariables(0) > this->limits(0) || this->conditionVariables(1) > this->limits(1))
218  {
219  ostringstream oss;
220  oss << "limit out of bounds: ";
221  oss << "rot: " << this->conditionVariables(0) << "/" << this->limits(0) << " ";
222  oss << "tr: " << this->conditionVariables(1) << "/" << this->limits(1);
223  throw ConvergenceError(oss.str());
224  }
225 }
226 
TransformationCheckersImpl::CounterTransformationChecker::MaxNumIterationsReached
Struct used to inform through an exeption that ICP reached max number of iterations.
Definition: TransformationCheckersImpl.h:61
build_map.T
T
Definition: build_map.py:34
TransformationCheckersImpl::CounterTransformationChecker::check
virtual void check(const TransformationParameters &parameters, bool &iterate)
Set iterate to false if iteration should stop.
Definition: TransformationCheckersImpl.cpp:64
TransformationCheckersImpl::Vector
PointMatcher< T >::Vector Vector
Definition: TransformationCheckersImpl.h:52
TransformationCheckersImpl::BoundTransformationChecker::check
virtual void check(const TransformationParameters &parameters, bool &iterate)
Set iterate to false if iteration should stop.
Definition: TransformationCheckersImpl.cpp:198
TransformationCheckersImpl::CounterTransformationChecker::CounterTransformationChecker
CounterTransformationChecker(const Parameters &params=Parameters())
Definition: TransformationCheckersImpl.cpp:46
PointMatcher::ConvergenceError
Point matcher did not converge.
Definition: PointMatcher.h:148
TransformationCheckersImpl::DifferentialTransformationChecker::init
virtual void init(const TransformationParameters &parameters, bool &iterate)
Init, set iterate to false if iteration should stop.
Definition: TransformationCheckersImpl.cpp:103
PointMatcher::TransformationChecker::limits
Vector limits
values of limits involved in conditions to stop ICP loop
Definition: PointMatcher.h:588
PointMatcherSupport::normalizeAngle
static T normalizeAngle(T v)
Definition: Functions.h:53
TransformationCheckersImpl::Quaternion
PointMatcher< T >::Quaternion Quaternion
Definition: TransformationCheckersImpl.h:54
TransformationCheckersImpl::BoundTransformationChecker::maxTranslationNorm
const T maxTranslationNorm
Definition: TransformationCheckersImpl.h:126
TransformationCheckersImpl::DifferentialTransformationChecker
Definition: TransformationCheckersImpl.h:81
TransformationCheckersImpl::DifferentialTransformationChecker::minDiffTransErr
const T minDiffTransErr
Definition: TransformationCheckersImpl.h:97
align_sequence.params
params
Definition: align_sequence.py:13
PointMatcher::TransformationChecker
A transformation checker can stop the iteration depending on some conditions.
Definition: PointMatcher.h:584
TransformationCheckersImpl::TransformationParameters
PointMatcher< T >::TransformationParameters TransformationParameters
Definition: TransformationCheckersImpl.h:51
TransformationCheckersImpl::BoundTransformationChecker::BoundTransformationChecker
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoundTransformationChecker(const Parameters &params=Parameters())
Definition: TransformationCheckersImpl.cpp:167
PointMatcher::TransformationChecker::conditionVariableNames
StringVector conditionVariableNames
names of variables involved in conditions to stop ICP loop
Definition: PointMatcher.h:591
TransformationCheckersImpl::CounterTransformationChecker
Definition: TransformationCheckersImpl.h:58
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
std
PointMatcherSupport::get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
Definition: Bibliography.cpp:57
TransformationCheckersImpl::BoundTransformationChecker
Definition: TransformationCheckersImpl.h:111
TransformationCheckersImpl.h
TransformationCheckersImpl::CounterTransformationChecker::init
virtual void init(const TransformationParameters &parameters, bool &iterate)
Init, set iterate to false if iteration should stop.
Definition: TransformationCheckersImpl.cpp:58
PointMatcherSupport::anyabs
static T anyabs(const T &v)
Definition: Functions.h:44
Functions.h
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
TransformationCheckersImpl::CounterTransformationChecker::maxIterationCount
const unsigned maxIterationCount
Definition: TransformationCheckersImpl.h:74
PointMatcher::TransformationChecker::limitNames
StringVector limitNames
names of limits involved in conditions to stop ICP loop
Definition: PointMatcher.h:590
TransformationCheckersImpl::DifferentialTransformationChecker::DifferentialTransformationChecker
DifferentialTransformationChecker(const Parameters &params=Parameters())
Definition: TransformationCheckersImpl.cpp:85
TransformationCheckersImpl::DifferentialTransformationChecker::minDiffRotErr
const T minDiffRotErr
Definition: TransformationCheckersImpl.h:96
TransformationCheckersImpl::DifferentialTransformationChecker::check
virtual void check(const TransformationParameters &parameters, bool &iterate)
Set iterate to false if iteration should stop.
Definition: TransformationCheckersImpl.cpp:127
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
TransformationCheckersImpl::BoundTransformationChecker::init
virtual void init(const TransformationParameters &parameters, bool &iterate)
Init, set iterate to false if iteration should stop.
Definition: TransformationCheckersImpl.cpp:183
TransformationCheckersImpl::BoundTransformationChecker::maxRotationNorm
const T maxRotationNorm
Definition: TransformationCheckersImpl.h:125


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