TransformationsImpl.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 "TransformationsImpl.h"
37 #include "Functions.h"
38 
39 #include <iostream>
40 
41 using namespace PointMatcherSupport;
42 
45  runtime_error(reason)
46 {}
47 
49 template<typename T>
51  const DataPoints& input,
52  const TransformationParameters& parameters) const
53 {
54  DataPoints transformedCloud = input;
55  inPlaceCompute(parameters, transformedCloud);
56  return transformedCloud;
57 }
58 
60 template<typename T>
62  const TransformationParameters& parameters,
63  DataPoints& cloud) const
64 {
65  assert(cloud.features.rows() == parameters.rows());
66  assert(parameters.rows() == parameters.cols());
67 
68  if(this->checkParameters(parameters) == false)
69  throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal.");
70 
71  // Apply the transformation to features
72  cloud.features.applyOnTheLeft(parameters);
73 
74  // Apply the rotation to descriptors
75  const unsigned int nbRows = parameters.rows()-1;
76  const unsigned int nbCols = parameters.cols()-1;
77  const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
78 
79  int descStartingRow(0);
80  const int descCols(cloud.descriptors.cols());
81 
82  for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i)
83  {
84  const int descSpan(cloud.descriptorLabels[i].span);
85  const std::string& descName(cloud.descriptorLabels[i].text);
86 
87  if (descName == "normals" || descName == "observationDirections")
88  {
89  cloud.descriptors.block(descStartingRow, 0, descSpan, descCols).applyOnTheLeft(R);
90  }
91  else if (descName == "eigVectors")
92  {
93  int vectorSpan = std::sqrt(descSpan);
94  int vectorStartingRow = descStartingRow;
95 
96  cloud.descriptors.block(vectorStartingRow, 0, vectorSpan, descCols).applyOnTheLeft(R);
97  vectorStartingRow += vectorSpan;
98  cloud.descriptors.block(vectorStartingRow, 0, vectorSpan, descCols).applyOnTheLeft(R);
99 
100  if (vectorSpan == 3)
101  {
102  vectorStartingRow += vectorSpan;
103  cloud.descriptors.block(vectorStartingRow, 0, vectorSpan, descCols).applyOnTheLeft(R);
104  }
105  }
106 
107  descStartingRow += descSpan;
108  }
109 }
110 
112 template<typename T>
114 {
115  //FIXME: FP - should we put that as function argument?
116  const T epsilon = 0.001;
117  const unsigned int nbRows = parameters.rows()-1;
118  const unsigned int nbCols = parameters.cols()-1;
119 
120  const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
121 
122  if(anyabs(1 - R.determinant()) > epsilon)
123  return false;
124  else
125  return true;
126 
127 }
128 
130 template<typename T>
132 {
133  TransformationParameters ortho = parameters;
134  if(ortho.cols() == 4)
135  {
136  //const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized();
137  const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
138  const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();
139 
140  const Eigen::Matrix<T, 3, 1> newCol0 = col1.cross(col2);
141  const Eigen::Matrix<T, 3, 1> newCol1 = col2.cross(newCol0);
142  const Eigen::Matrix<T, 3, 1> newCol2 = col2;
143 
144  ortho.block(0, 0, 3, 1) = newCol0;
145  ortho.block(0, 1, 3, 1) = newCol1;
146  ortho.block(0, 2, 3, 1) = newCol2;
147  }
148  else if(ortho.cols() == 3)
149  {
150  const T epsilon = 0.001;
151 
152  // R = [ a b]
153  // [-b a]
154  if(anyabs(parameters(0,0) - parameters(1,1)) > epsilon || anyabs(parameters(1,0) + parameters(0,1)) > epsilon)
155  {
156  throw TransformationError("RigidTransformation: Error, only proper rigid transformations are supported.");
157  }
158 
159  // mean of a and b
160  T a = (parameters(0,0) + parameters(1,1))/2;
161  T b = (-parameters(1,0) + parameters(0,1))/2;
162  T sum = sqrt(pow(a,2) + pow(b,2));
163 
164  a = a/sum;
165  b = b/sum;
166 
167  ortho(0,0) = a; ortho(0,1) = b;
168  ortho(1,0) = -b; ortho(1,1) = a;
169  }
170 
171 
172  return ortho;
173 }
174 
177 
179 template<typename T>
181  const DataPoints& input,
182  const TransformationParameters& parameters) const
183 {
184  DataPoints transformedCloud = input;
185  inPlaceCompute(parameters, transformedCloud);
186  return transformedCloud;
187 }
188 
190 template<typename T>
192  const TransformationParameters& parameters,
193  DataPoints& cloud) const
194 {
195  assert(cloud.features.rows() == parameters.rows());
196  assert(parameters.rows() == parameters.cols());
197 
198  if(this->checkParameters(parameters) == false)
199  throw TransformationError("SimilarityTransformation: Error, invalid similarity transform.");
200 
201  // Apply the transformation to features
202  cloud.features.applyOnTheLeft(parameters);
203 
204  // Apply the rotation to descriptors
205  const unsigned int nbRows = parameters.rows() - 1;
206  const unsigned int nbCols = parameters.cols() - 1;
207  const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
208 
209  int descStartingRow(0);
210  const int descCols(cloud.descriptors.cols());
211 
212  for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i)
213  {
214  const int descSpan(cloud.descriptorLabels[i].span);
215  const std::string& descName(cloud.descriptorLabels[i].text);
216 
217  if (descName == "normals" || descName == "observationDirections")
218  {
219  cloud.descriptors.block(descStartingRow, 0, descSpan, descCols).applyOnTheLeft(R);
220  }
221  else if (descName == "eigVectors")
222  {
223  int vectorSpan = std::sqrt(descSpan);
224  int vectorStartingRow = descStartingRow;
225 
226  cloud.descriptors.block(vectorStartingRow, 0, vectorSpan, descCols).applyOnTheLeft(R);
227  vectorStartingRow += vectorSpan;
228  cloud.descriptors.block(vectorStartingRow, 0, vectorSpan, descCols).applyOnTheLeft(R);
229 
230  if (vectorSpan == 3)
231  {
232  vectorStartingRow += vectorSpan;
233  cloud.descriptors.block(vectorStartingRow, 0, vectorSpan, descCols).applyOnTheLeft(R);
234  }
235  }
236 
237  descStartingRow += descSpan;
238  }
239 }
240 
242 template<typename T>
244 {
245  //FIXME: FP - should we put that as function argument?
246  return true;
247 }
248 
250 template<typename T>
252 {
253  return parameters;
254 }
255 
258 
259 template<typename T>
261  const TransformationParameters& parameters) const {
262  DataPoints transformedCloud = input;
263  inPlaceCompute(parameters, transformedCloud);
264  return transformedCloud;
265 }
266 
267 template<typename T>
269  const TransformationParameters& parameters,
270  DataPoints& cloud) const {
271  assert(cloud.features.rows() == parameters.rows());
272  assert(parameters.rows() == parameters.cols());
273 
274  if(this->checkParameters(parameters) == false)
275  throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity.");
276 
277  // Apply the transformation to features
278  cloud.features.applyOnTheLeft(parameters);
279 }
280 
281 template<typename T>
283  const TransformationParameters& parameters) const {
284  const int rows = parameters.rows();
285  const int cols = parameters.cols();
286 
287  // make a copy of the parameters to perform corrections on
288  TransformationParameters correctedParameters(parameters);
289 
290  // set the top left block to the identity matrix
291  correctedParameters.block(0,0,rows-1,cols-1).setIdentity();
292 
293  // fix the bottom row
294  correctedParameters.block(rows-1,0,1,cols-1).setZero();
295  correctedParameters(rows-1,cols-1) = 1;
296 
297  return correctedParameters;
298 }
299 
300 template<typename T>
302  const TransformationParameters& parameters) const {
303  const int rows = parameters.rows();
304  const int cols = parameters.cols();
305 
306  // make a copy of parameters to perform the check
307  TransformationParameters parameters_(parameters);
308 
309  // set the translation components of the transformation matrix to 0
310  parameters_.block(0,cols-1,rows-1,1).setZero();
311 
312  // If we have the identity matrix, than this is indeed a pure translation
313  if (parameters_.isApprox(TransformationParameters::Identity(rows,cols)))
314  return true;
315  else
316  return false;
317 }
318 
TransformationsImpl::PureTranslation::correctParameters
virtual TransformationParameters correctParameters(const TransformationParameters &parameters) const
Definition: TransformationsImpl.cpp:282
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
TransformationsImpl::RigidTransformation::compute
virtual DataPoints compute(const DataPoints &input, const TransformationParameters &parameters) const
RigidTransformation.
Definition: TransformationsImpl.cpp:50
TransformationsImpl::PureTranslation::compute
virtual DataPoints compute(const DataPoints &input, const TransformationParameters &parameters) const
Definition: TransformationsImpl.cpp:260
build_map.T
T
Definition: build_map.py:34
TransformationsImpl::RigidTransformation::inPlaceCompute
virtual void inPlaceCompute(const TransformationParameters &parameters, DataPoints &cloud) const
RigidTransformation.
Definition: TransformationsImpl.cpp:61
TransformationsImpl::SimilarityTransformation::compute
virtual DataPoints compute(const DataPoints &input, const TransformationParameters &parameters) const
SimilarityTransformation.
Definition: TransformationsImpl.cpp:180
PointMatcherSupport::TransformationError::TransformationError
TransformationError(const std::string &reason)
return an exception when a transformation has invalid parameters
Definition: TransformationsImpl.cpp:44
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
testing::internal::string
::std::string string
Definition: gtest.h:1979
TransformationsImpl::PureTranslation::inPlaceCompute
virtual void inPlaceCompute(const TransformationParameters &parameters, DataPoints &cloud) const
Definition: TransformationsImpl.cpp:268
TransformationsImpl::PureTranslation
Definition: TransformationsImpl.h:82
TransformationsImpl::SimilarityTransformation
Definition: TransformationsImpl.h:68
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
TransformationsImpl::SimilarityTransformation::correctParameters
virtual TransformationParameters correctParameters(const TransformationParameters &parameters) const
Nothing to correct for a similarity transform.
Definition: TransformationsImpl.cpp:251
TransformationsImpl::RigidTransformation::checkParameters
virtual bool checkParameters(const TransformationParameters &parameters) const
Ensure orthogonality of the rotation matrix.
Definition: TransformationsImpl.cpp:113
TransformationsImpl::TransformationParameters
PointMatcher< T >::TransformationParameters TransformationParameters
Definition: TransformationsImpl.h:51
TransformationsImpl::RigidTransformation
Definition: TransformationsImpl.h:54
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
TransformationsImpl.h
TransformationsImpl::RigidTransformation::correctParameters
virtual TransformationParameters correctParameters(const TransformationParameters &parameters) const
Force orthogonality of the rotation matrix.
Definition: TransformationsImpl.cpp:131
TransformationsImpl::SimilarityTransformation::inPlaceCompute
virtual void inPlaceCompute(const TransformationParameters &parameters, DataPoints &cloud) const
SimilarityTransformation.
Definition: TransformationsImpl.cpp:191
TransformationsImpl::SimilarityTransformation::checkParameters
virtual bool checkParameters(const TransformationParameters &parameters) const
Nothing to check for a similarity transform.
Definition: TransformationsImpl.cpp:243
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
PointMatcherSupport::TransformationError
An expection thrown when a transformation has invalid parameters.
Definition: PointMatcher.h:89
PointMatcherSupport::pow
constexpr T pow(const T base, const std::size_t exponent)
Definition: utils.h:47
PointMatcher::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182
TransformationsImpl::PureTranslation::checkParameters
virtual bool checkParameters(const TransformationParameters &parameters) const
Definition: TransformationsImpl.cpp:301


libpointmatcher
Author(s):
autogenerated on Mon Jan 1 2024 03:24:43