transformation2.h
Go to the documentation of this file.
1 /**********************************************************************
2  *
3  * This source code is part of the Tree-based Network Optimizer (TORO)
4  *
5  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
6  * Slawomir Grzonka, and Wolfram Burgard
7  *
8  * TORO is licences under the Common Creative License,
9  * Attribution-NonCommercial-ShareAlike 3.0
10  *
11  * You are free:
12  * - to Share - to copy, distribute and transmit the work
13  * - to Remix - to adapt the work
14  *
15  * Under the following conditions:
16  *
17  * - Attribution. You must attribute the work in the manner specified
18  * by the author or licensor (but not in any way that suggests that
19  * they endorse you or your use of the work).
20  *
21  * - Noncommercial. You may not use this work for commercial purposes.
22  *
23  * - Share Alike. If you alter, transform, or build upon this work,
24  * you may distribute the resulting work only under the same or
25  * similar license to this one.
26  *
27  * Any of the above conditions can be waived if you get permission
28  * from the copyright holder. Nothing in this license impairs or
29  * restricts the author's moral rights.
30  *
31  * TORO is distributed in the hope that it will be useful,
32  * but WITHOUT ANY WARRANTY; without even the implied
33  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
34  * PURPOSE.
35  **********************************************************************/
36 
44 #ifndef _TRANSFORMATION2_HXX_
45 #define _TRANSFORMATION2_HXX_
46 
47 #include <cmath>
48 
49 namespace AISNavigation
50 {
51 
53 template <class T>
54 struct Vector2{
55  T values[2] ;
56 
58  Vector2(T x, T y) {values[0]=x; values[1]=y;}
60  Vector2() {values[0]=0; values[1]=0;}
61 
63  inline const T& x() const {return values[0];}
65  inline const T& y() const {return values[1];}
66 
68  inline T& x() {return values[0];}
70  inline T& y() {return values[1];}
71 
73  inline T norm2() const {
74  return values[0]*values[0]+values[1]*values[1];
75  }
76 
77 };
78 
80 template <class T>
81 inline Vector2<T> operator * (const T& d, const Vector2<T>& v) {
82  return Vector2<T>(v.values[0]*d, v.values[1]*d);
83 }
84 
86 template <class T>
87 inline Vector2<T> operator * (const Vector2<T>& v, const T& d) {
88  return Vector2<T>(v.values[0]*d, v.values[1]*d);
89 }
90 
92 template <class T>
93 inline T operator * (const Vector2<T>& v1, const Vector2<T>& v2){
94  return v1.values[0]*v2.values[0]
95  + v1.values[1]*v2.values[1];
96 }
97 
99 template <class T>
100 inline Vector2<T> operator + (const Vector2<T>& v1, const Vector2<T>& v2){
101  return Vector2<T>(v1.values[0]+v2.values[0],
102  v1.values[1]+v2.values[1]);
103 }
104 
106 template <class T>
108  return Vector2<T>(v1.values[0]-v2.values[0],
109  v1.values[1]-v2.values[1]);
110 }
111 
112 
119 template <class T>
120 struct Pose2{
121  T values[3];
122 
124  inline const T& x() const {return values[0];}
126  inline const T& y() const {return values[1];}
128  inline const T& theta() const {return values[2];}
129 
131  inline T& x() {return values[0];}
133  inline T& y() {return values[1];}
135  inline T& theta() {return values[2];}
136 
138  Pose2(){
139  values[0]=0.; values[1]=0.; values[2]=0.;
140  }
141 
143  Pose2(const T& x, const T& y, const T& theta){
144  values[0]=x, values[1]=y, values[2]=theta;
145  }
146 };
147 
149 template <class T>
150 Pose2<T> operator * (const Pose2<T>& v, const T& d){
151  Pose2<T> r;
152  for (int i=0; i<3; i++){
153  r.values[i]=v.values[i]*d;
154  }
155  return r;
156 }
157 
158 
160 template <class T>
164 
168  Transformation2(bool initAsIdentity = true){
169  if (initAsIdentity) {
170  rotationMatrix[0][0]=1.; rotationMatrix[0][1]=0.;
171  rotationMatrix[1][0]=0.; rotationMatrix[1][1]=1.;
172  translationVector[0]=0.;
173  translationVector[1]=0.;
174  }
175  }
176 
178  inline static Transformation2<T> identity(){
179  Transformation2<T> m(true);
180  return m;
181  }
182 
184  Transformation2 (const T& x, const T& y, const T& theta){
185  setRotation(theta);
186  setTranslation(x,y);
187  }
188 
190  Transformation2 (const T& _theta, const Vector2<T>& trans){
191  setRotation(_theta);
192  setTranslation(trans.x(), trans.y());
193  }
194 
195 
198  setRotation(v.theta());
199  setTranslation(v.x(),v.y());
200  }
201 
202 
204  inline Vector2<T> translation() const {
205  return Vector2<T>(translationVector[0],
206  translationVector[1]);
207  }
208 
210  inline T rotation() const {
211  return atan2(rotationMatrix[1][0],rotationMatrix[0][0]);
212  }
213 
215  inline Pose2<T> toPoseType() const {
217  T r=rotation();
218  Pose2<T> rv(t.x(), t.y(), r );
219  return rv;
220  }
221 
223  inline void setTranslation(const Vector2<T>& t){
224  setTranslation(t.x(),t.y());
225  }
226 
228  inline void setRotation(const T& theta){
229  T s=sin(theta), c=cos(theta);
230  rotationMatrix[0][0]=c, rotationMatrix[0][1]=-s;
231  rotationMatrix[1][0]=s, rotationMatrix[1][1]= c;
232  }
233 
235  inline void setTranslation(const T& x, const T& y){
236  translationVector[0]=x;
237  translationVector[1]=y;
238  }
239 
241  inline Transformation2<T> inv() const {
242  Transformation2<T> rv(*this);
243  for (int i=0; i<2; i++)
244  for (int j=0; j<2; j++){
246  }
247 
248  for (int i=0; i<2; i++){
249  rv.translationVector[i]=0;
250  for (int j=0; j<2; j++){
252  }
253  }
254  return rv;
255  }
256 
257 };
258 
260 template <class T>
262  return Vector2<T>(
263  m.rotationMatrix[0][0]*v.values[0]+
264  m.rotationMatrix[0][1]*v.values[1]+
265  m.translationVector[0],
266  m.rotationMatrix[1][0]*v.values[0]+
267  m.rotationMatrix[1][1]*v.values[1]+
268  m.translationVector[1]);
269 }
270 
272 template <class T>
275  for (int i=0; i<2; i++)
276  for (int j=0; j<2; j++){
277  rt.rotationMatrix[i][j]=0.;
278  for (int k=0; k<2; k++)
279  rt.rotationMatrix[i][j]+=m1.rotationMatrix[i][k]*m2.rotationMatrix[k][j];
280  }
281  for (int i=0; i<2; i++){
282  rt.translationVector[i]=m1.translationVector[i];
283  for (int j=0; j<2; j++)
284  rt.translationVector[i]+=m1.rotationMatrix[i][j]*m2.translationVector[j];
285  }
286  return rt;
287 }
288 
289 
291 template <class T>
292 struct SMatrix3{
293  T values[3][3];
294  T det() const;
295  SMatrix3<T> transpose() const;
296  SMatrix3<T> adj() const;
297  SMatrix3<T> inv() const;
298 };
299 
300 
302 template <class T>
304  Pose2<T> v;
305  for (int i=0; i<3; i++){
306  v.values[i]=0.;
307  for (int j=0; j<3; j++)
308  v.values[i]+=m.values[i][j]*p.values[j];
309  }
310  return v;
311 }
312 
314 template <class T>
316  SMatrix3<T> m;
317  for (int i=0; i<3; i++)
318  for (int j=0; j<3; j++)
319  m.values[i][j]=d*s.values[i][j];
320  return m;
321 }
322 
324 template <class T>
326  SMatrix3<T> m;
327  for (int i=0; i<3; i++)
328  for (int j=0; j<3; j++){
329  m.values[i][j]=0.;
330  for (int k=0; k<3; k++){
331  m.values[i][j]+=s1.values[i][k]*s2.values[k][j];
332  }
333  }
334  return m;
335 }
336 
338 template <class T>
340  SMatrix3<T> m;
341  for (int i=0; i<3; i++)
342  for (int j=0; j<3; j++){
343  m.values[i][j]=s1.values[i][j]+s2.values[i][j];
344  }
345  return m;
346 }
347 
348 
350 template <class T>
352  T dp= values[0][0]*values[1][1]*values[2][2]
353  +values[0][1]*values[1][2]*values[2][0]
354  +values[0][2]*values[1][0]*values[2][1];
355  T dm=values[2][0]*values[1][1]*values[0][2]
356  +values[2][1]*values[1][2]*values[0][0]
357  +values[2][2]*values[1][0]*values[0][1];
358  return dp-dm;
359 }
360 
362 template <class T>
364  SMatrix3<T> m;
365  for (int i=0; i<3; i++)
366  for (int j=0; j<3; j++)
367  m.values[j][i]=values[i][j];
368  return m;
369 }
370 
372 template <class T>
374  SMatrix3<T> m;
375  m.values[0][0]= values[1][1]*values[2][2]-values[2][1]*values[1][2];
376  m.values[0][1]=-values[1][0]*values[2][2]+values[1][2]*values[2][0];
377  m.values[0][2]= values[1][0]*values[2][1]-values[2][0]*values[1][1];
378  m.values[1][0]=-values[0][1]*values[2][2]+values[2][1]*values[0][2];
379  m.values[1][1]= values[0][0]*values[2][2]-values[2][0]*values[0][2];
380  m.values[1][2]=-values[0][0]*values[2][1]+values[2][0]*values[0][1];
381  m.values[2][0]= values[0][1]*values[1][2]-values[1][1]*values[0][2];
382  m.values[2][1]=-values[0][0]*values[1][2]+values[1][0]*values[0][2];
383  m.values[2][2]= values[0][0]*values[1][1]-values[1][0]*values[0][1];
384  return m;
385 }
386 
388 template <class T>
390  T id=1./det();
391  SMatrix3<T> i=adj().transpose();
392  return i*id;
393 }
394 
395 
396 
398 template <class T>
400  typedef T BaseType;
401  typedef Pose2<T> PoseType;
403  typedef T RotationType;
408 };
409 
410 } // namespace AISNavigation
411 
412 #endif
AISNavigation::SMatrix3::transpose
SMatrix3< T > transpose() const
Definition: transformation2.h:363
AISNavigation::Vector2::y
const T & y() const
Definition: transformation2.h:65
AISNavigation::Transformation2::rotation
T rotation() const
Definition: transformation2.h:210
AISNavigation::Vector2::Vector2
Vector2(T x, T y)
Definition: transformation2.h:58
AISNavigation::Pose2::y
T & y()
Definition: transformation2.h:133
AISNavigation::Pose2::theta
const T & theta() const
Definition: transformation2.h:128
v1
v1
s
RealScalar s
AISNavigation::Transformation2::setTranslation
void setTranslation(const Vector2< T > &t)
Definition: transformation2.h:223
c
Scalar Scalar * c
AISNavigation::operator*
Vector2< T > operator*(const T &d, const Vector2< T > &v)
Definition: transformation2.h:81
AISNavigation::Vector2::norm2
T norm2() const
Definition: transformation2.h:73
m1
Matrix3d m1
AISNavigation::Operations2D::PoseType
Pose2< T > PoseType
Definition: transformation2.h:401
AISNavigation::Pose2::y
const T & y() const
Definition: transformation2.h:126
AISNavigation::Transformation2::Transformation2
Transformation2(const T &x, const T &y, const T &theta)
Definition: transformation2.h:184
y
Matrix3f y
AISNavigation::Transformation2::Transformation2
Transformation2(const Pose2< T > &v)
Definition: transformation2.h:197
AISNavigation::Operations2D::TransformationType
Transformation2< T > TransformationType
Definition: transformation2.h:405
AISNavigation::Transformation2::Transformation2
Transformation2(const T &_theta, const Vector2< T > &trans)
Definition: transformation2.h:190
AISNavigation::Vector2::x
const T & x() const
Definition: transformation2.h:63
AISNavigation::Transformation2::identity
static Transformation2< T > identity()
Definition: transformation2.h:178
AISNavigation::Transformation2::setTranslation
void setTranslation(const T &x, const T &y)
Definition: transformation2.h:235
Vector2::y
float y
AISNavigation::Pose2::Pose2
Pose2(const T &x, const T &y, const T &theta)
Definition: transformation2.h:143
AISNavigation::Operations2D::BaseType
T BaseType
Definition: transformation2.h:400
AISNavigation::Vector2::x
T & x()
Definition: transformation2.h:68
m2
MatrixType m2(n_dims)
AISNavigation::Transformation2::toPoseType
Pose2< T > toPoseType() const
Definition: transformation2.h:215
AISNavigation::Pose2::values
T values[3]
container for x, y, and theta
Definition: transformation2.h:121
j
std::ptrdiff_t j
AISNavigation::Vector2::y
T & y()
Definition: transformation2.h:70
AISNavigation::Transformation2::translation
Vector2< T > translation() const
Definition: transformation2.h:204
AISNavigation::Operations2D
Tenmplate class to define the operations in 2D
Definition: transformation2.h:399
AISNavigation::Operations2D::ParametersType
Pose2< T > ParametersType
Definition: transformation2.h:402
AISNavigation::SMatrix3
A class to represent symmetric 3x3 matrices.
Definition: transformation2.h:292
AISNavigation::Vector2::Vector2
Vector2()
Definition: transformation2.h:60
AISNavigation::Vector2
Template class for representing a 2D point (x and y coordinate)
Definition: transformation2.h:54
AISNavigation::Pose2
2D Point (x,y) with orientation (theta)
Definition: transformation2.h:120
AISNavigation::Vector2::values
T values[2]
container for x and y
Definition: transformation2.h:55
d
d
x
x
m
Matrix3f m
p
Point3_ p(2)
Eigen::Triplet< double >
AISNavigation::Transformation2::rotationMatrix
T rotationMatrix[2][2]
the rotation matrix
Definition: transformation2.h:162
AISNavigation::Transformation2::Transformation2
Transformation2(bool initAsIdentity=true)
Definition: transformation2.h:168
AISNavigation::operator+
Vector2< T > operator+(const Vector2< T > &v1, const Vector2< T > &v2)
Definition: transformation2.h:100
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
AISNavigation::Pose2::Pose2
Pose2()
Definition: transformation2.h:138
AISNavigation::Transformation2::setRotation
void setRotation(const T &theta)
Definition: transformation2.h:228
AISNavigation::Transformation2::inv
Transformation2< T > inv() const
Definition: transformation2.h:241
trans
Matrix trans(const Matrix &A)
values
leaf::MyValues values
AISNavigation::Pose2::x
T & x()
Definition: transformation2.h:131
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
id
id
AISNavigation::SMatrix3::inv
SMatrix3< T > inv() const
Definition: transformation2.h:389
v
Array< int, Dynamic, 1 > v
AISNavigation::Transformation2::translationVector
T translationVector[2]
the translation vector
Definition: transformation2.h:163
AISNavigation::Operations2D::InformationType
SMatrix3< T > InformationType
Definition: transformation2.h:407
Vector2::x
float x
AISNavigation::SMatrix3::values
T values[3][3]
Definition: transformation2.h:293
t
Point2 t(10, 10)
AISNavigation::operator-
Vector2< T > operator-(const Vector2< T > &v1, const Vector2< T > &v2)
Definition: transformation2.h:107
AISNavigation::Pose2::x
const T & x() const
Definition: transformation2.h:124
AISNavigation::Transformation2
A class to represent 2D transformations (rotation and translation)
Definition: transformation2.h:161
AISNavigation::Operations2D::TranslationType
Vector2< T > TranslationType
Definition: transformation2.h:404
AISNavigation
Definition: posegraph.h:57
AISNavigation::Pose2::theta
T & theta()
Definition: transformation2.h:135
AISNavigation::SMatrix3::det
T det() const
Definition: transformation2.h:351
i
int i
AISNavigation::Operations2D::CovarianceType
SMatrix3< T > CovarianceType
Definition: transformation2.h:406
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)
AISNavigation::Operations2D::RotationType
T RotationType
Definition: transformation2.h:403
AISNavigation::SMatrix3::adj
SMatrix3< T > adj() const
Definition: transformation2.h:373
v2
v2


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:22