Transform.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/Transform.h>
29 
30 #include <pcl/common/eigen.h>
31 #include <pcl/common/common.h>
32 #include <rtabmap/core/util3d.h>
34 #include <rtabmap/utilite/UMath.h>
36 #include <rtabmap/utilite/UStl.h>
37 #include <iomanip>
38 
39 namespace rtabmap {
40 
41 Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1))
42 {
43 }
44 
45 // rotation matrix r## and origin o##
47  float r11, float r12, float r13, float o14,
48  float r21, float r22, float r23, float o24,
49  float r31, float r32, float r33, float o34)
50 {
51  data_ = (cv::Mat_<float>(3,4) <<
52  r11, r12, r13, o14,
53  r21, r22, r23, o24,
54  r31, r32, r33, o34);
55 }
56 
57 Transform::Transform(const cv::Mat & transformationMatrix)
58 {
59  UASSERT(transformationMatrix.cols == 4 &&
60  transformationMatrix.rows == 3 &&
61  (transformationMatrix.type() == CV_32FC1 || transformationMatrix.type() == CV_64FC1));
62  if(transformationMatrix.type() == CV_32FC1)
63  {
64  data_ = transformationMatrix;
65  }
66  else
67  {
68  transformationMatrix.convertTo(data_, CV_32F);
69  }
70 }
71 
72 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
73 {
74  Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
75  *this = fromEigen3f(t);
76 }
77 
78 Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw) :
79  data_(cv::Mat::zeros(3,4,CV_32FC1))
80 {
81  Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).normalized().toRotationMatrix();
82  data()[0] = rotation(0,0);
83  data()[1] = rotation(0,1);
84  data()[2] = rotation(0,2);
85  data()[3] = x;
86  data()[4] = rotation(1,0);
87  data()[5] = rotation(1,1);
88  data()[6] = rotation(1,2);
89  data()[7] = y;
90  data()[8] = rotation(2,0);
91  data()[9] = rotation(2,1);
92  data()[10] = rotation(2,2);
93  data()[11] = z;
94 }
95 
96 Transform::Transform(float x, float y, float theta)
97 {
98  Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta);
99  *this = fromEigen3f(t);
100 }
101 
103 {
104  return Transform(data_.clone());
105 }
106 
107 bool Transform::isNull() const
108 {
109  return (data_.empty() ||
110  (data()[0] == 0.0f &&
111  data()[1] == 0.0f &&
112  data()[2] == 0.0f &&
113  data()[3] == 0.0f &&
114  data()[4] == 0.0f &&
115  data()[5] == 0.0f &&
116  data()[6] == 0.0f &&
117  data()[7] == 0.0f &&
118  data()[8] == 0.0f &&
119  data()[9] == 0.0f &&
120  data()[10] == 0.0f &&
121  data()[11] == 0.0f) ||
122  uIsNan(data()[0]) ||
123  uIsNan(data()[1]) ||
124  uIsNan(data()[2]) ||
125  uIsNan(data()[3]) ||
126  uIsNan(data()[4]) ||
127  uIsNan(data()[5]) ||
128  uIsNan(data()[6]) ||
129  uIsNan(data()[7]) ||
130  uIsNan(data()[8]) ||
131  uIsNan(data()[9]) ||
132  uIsNan(data()[10]) ||
133  uIsNan(data()[11]));
134 }
135 
137 {
138  return data()[0] == 1.0f &&
139  data()[1] == 0.0f &&
140  data()[2] == 0.0f &&
141  data()[3] == 0.0f &&
142  data()[4] == 0.0f &&
143  data()[5] == 1.0f &&
144  data()[6] == 0.0f &&
145  data()[7] == 0.0f &&
146  data()[8] == 0.0f &&
147  data()[9] == 0.0f &&
148  data()[10] == 1.0f &&
149  data()[11] == 0.0f;
150 }
151 
153 {
154  *this = Transform();
155 }
156 
158 {
159  *this = getIdentity();
160 }
161 
162 float Transform::theta() const
163 {
164  float roll, pitch, yaw;
165  this->getEulerAngles(roll, pitch, yaw);
166  return yaw;
167 }
168 
170 {
171  bool invertible = false;
172  Eigen::Matrix4f inverse;
173  Eigen::Matrix4f::RealScalar det;
174  toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
175  return invertible;
176 }
177 
179 {
180  bool invertible = false;
181  Eigen::Matrix4f inverse;
182  Eigen::Matrix4f::RealScalar det;
183  toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
184  UASSERT_MSG(invertible, uFormat("This transform is not invertible! %s \n"
185  "[%f %f %f %f;\n"
186  " %f %f %f %f;\n"
187  " %f %f %f %f;\n"
188  " 0 0 0 1]", prettyPrint().c_str(),
189  r11(), r12(), r13(), o14(),
190  r21(), r22(), r23(), o24(),
191  r31(), r32(), r33(), o34()).c_str());
192  return fromEigen4f(inverse);
193 }
194 
196 {
197  return Transform(
198  data()[0], data()[1], data()[2], 0,
199  data()[4], data()[5], data()[6], 0,
200  data()[8], data()[9], data()[10], 0);
201 }
202 
204 {
205  return Transform(1,0,0, data()[3],
206  0,1,0, data()[7],
207  0,0,1, data()[11]);
208 }
209 
211 {
212  float x,y,z,roll,pitch,yaw;
214  float A = std::cos(yaw);
215  float B = std::sin(yaw);
216  return Transform(
217  A,-B, 0, x,
218  B, A, 0, y,
219  0, 0, 1, 0);
220 }
221 
223 {
224  float x,y,z,roll,pitch,yaw;
226  float A = std::cos(yaw);
227  float B = std::sin(yaw);
228  return Transform(
229  A,-B, 0, x,
230  B, A, 0, y,
231  0, 0, 1, z);
232 }
233 
234 bool Transform::is3DoF() const
235 {
236  return is4DoF() && z() == 0.0;
237 }
238 
239 bool Transform::is4DoF() const
240 {
241  return r13() == 0.0 &&
242  r23() == 0.0 &&
243  r31() == 0.0 &&
244  r32() == 0.0 &&
245  r33() == 1.0;
246 }
247 
249 {
250  return data_.colRange(0, 3).clone();
251 }
252 
254 {
255  return data_.col(3).clone();
256 }
257 
258 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
259 {
260  pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
261 }
262 
263 void Transform::getEulerAngles(float & roll, float & pitch, float & yaw) const
264 {
265  float x,y,z;
266  pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
267 }
268 
269 void Transform::getTranslation(float & x, float & y, float & z) const
270 {
271  x = this->x();
272  y = this->y();
273  z = this->z();
274 }
275 
276 float Transform::getAngle(float x, float y, float z) const
277 {
278  Eigen::Vector3f vA(x,y,z);
279  Eigen::Vector3f vB = this->toEigen3f().linear()*Eigen::Vector3f(1,0,0);
280  return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
281 }
282 
283 float Transform::getNorm() const
284 {
285  return uNorm(this->x(), this->y(), this->z());
286 }
287 
289 {
290  return uNormSquared(this->x(), this->y(), this->z());
291 }
292 
293 float Transform::getDistance(const Transform & t) const
294 {
295  return uNorm(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
296 }
297 
299 {
300  return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
301 }
302 
303 Transform Transform::interpolate(float t, const Transform & other) const
304 {
306  Eigen::Quaternionf qb=other.getQuaternionf();
307  Eigen::Quaternionf qres = qa.slerp(t, qb);
308 
309  float x = this->x() + t*(other.x() - this->x());
310  float y = this->y() + t*(other.y() - this->y());
311  float z = this->z() + t*(other.z() - this->z());
312 
313  return Transform(x,y,z, qres.x(), qres.y(), qres.z(), qres.w());
314 }
315 
317 {
318  if(!this->isNull())
319  {
321  m.linear() = Eigen::Quaternionf(m.linear()).normalized().toRotationMatrix();
322  *this = fromEigen3f(m);
323  }
324 }
325 
326 std::string Transform::prettyPrint() const
327 {
328  if(this->isNull())
329  {
330  return uFormat("xyz=[null] rpy=[null]");
331  }
332  else
333  {
334  float x,y,z,roll,pitch,yaw;
336  return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
337  }
338 }
339 
341 {
342  Eigen::Affine3f m = Eigen::Affine3f(toEigen4f()*t.toEigen4f());
343  // make sure rotation is always normalized!
344  m.linear() = Eigen::Quaternionf(m.linear()).normalized().toRotationMatrix();
345  return fromEigen3f(m);
346 }
347 
349 {
350  *this = *this * t;
351  return *this;
352 }
353 
354 bool Transform::operator==(const Transform & t) const
355 {
356  return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0;
357 }
358 
359 bool Transform::operator!=(const Transform & t) const
360 {
361  return !(*this == t);
362 }
363 
364 std::ostream& operator<<(std::ostream& os, const Transform& s)
365 {
366  os << "[" << s.data()[0] << ", " << s.data()[1] << ", " << s.data()[2] << ", " << s.data()[3] << ";" << std::endl
367  << " " << s.data()[4] << ", " << s.data()[5] << ", " << s.data()[6] << ", " << s.data()[7] << ";" << std::endl
368  << " " << s.data()[8] << ", " << s.data()[9] << ", " << s.data()[10]<< ", " << s.data()[11] << "]";
369  return os;
370 }
371 
372 Eigen::Matrix4f Transform::toEigen4f() const
373 {
374  Eigen::Matrix4f m;
375  m << data()[0], data()[1], data()[2], data()[3],
376  data()[4], data()[5], data()[6], data()[7],
377  data()[8], data()[9], data()[10], data()[11],
378  0,0,0,1;
379  return m;
380 }
381 Eigen::Matrix4d Transform::toEigen4d() const
382 {
383  Eigen::Matrix4d m;
384  m << data()[0], data()[1], data()[2], data()[3],
385  data()[4], data()[5], data()[6], data()[7],
386  data()[8], data()[9], data()[10], data()[11],
387  0,0,0,1;
388  return m;
389 }
390 
392 {
393  return Eigen::Affine3f(toEigen4f());
394 }
395 
397 {
398  return Eigen::Affine3d(toEigen4d());
399 }
400 
402 {
403  return Eigen::Quaternionf(this->toEigen3f().linear()).normalized();
404 }
405 
407 {
408  return Eigen::Quaterniond(this->toEigen3d().linear()).normalized();
409 }
410 
412 {
413  return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
414 }
415 
416 Transform Transform::fromEigen4f(const Eigen::Matrix4f & matrix)
417 {
418  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
419  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
420  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
421 }
422 Transform Transform::fromEigen4d(const Eigen::Matrix4d & matrix)
423 {
424  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
425  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
426  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
427 }
428 
430 {
431  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
432  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
433  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
434 }
436 {
437  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
438  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
439  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
440 }
441 
443 {
444  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
445  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
446  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
447 }
449 {
450  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
451  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
452  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
453 }
454 
456 {
457  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
458  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
459  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
460 }
462 {
463  return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
464  matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
465  matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
466 }
467 
475 Transform Transform::fromString(const std::string & string)
476 {
477  std::list<std::string> list = uSplit(string, ' ');
478  UASSERT_MSG(list.empty() || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12,
479  uFormat("Cannot parse \"%s\"", string.c_str()).c_str());
480 
481  std::vector<float> numbers(list.size());
482  int i = 0;
483  for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
484  {
485  numbers[i++] = uStr2Float(*iter);
486  }
487 
488  Transform t;
489  if(numbers.size() == 3)
490  {
491  t = Transform(numbers[0], numbers[1], numbers[2]);
492  }
493  else if(numbers.size() == 6)
494  {
495  t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
496  }
497  else if(numbers.size() == 7)
498  {
499 
500  t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5], numbers[6]);
501  }
502  else if(numbers.size() == 9)
503  {
504  t = Transform(numbers[0], numbers[1], numbers[2], 0,
505  numbers[3], numbers[4], numbers[5], 0,
506  numbers[6], numbers[7], numbers[8], 0);
507  }
508  else if(numbers.size() == 12)
509  {
510  t = Transform(numbers[0], numbers[1], numbers[2], numbers[3],
511  numbers[4], numbers[5], numbers[6], numbers[7],
512  numbers[8], numbers[9], numbers[10], numbers[11]);
513  }
514  return t;
515 }
516 
524 bool Transform::canParseString(const std::string & string)
525 {
526  std::list<std::string> list = uSplit(string, ' ');
527  return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
528 }
529 
531  const std::map<double, Transform> & tfBuffer,
532  const double & stamp)
533 {
534  UASSERT(!tfBuffer.empty());
535  std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
536  std::map<double, Transform>::const_iterator imuIterA = imuIterB;
537  if(imuIterA != tfBuffer.begin())
538  {
539  imuIterA = --imuIterA;
540  }
541  if(imuIterB == tfBuffer.end())
542  {
543  imuIterB = --imuIterB;
544  }
545  Transform imuT;
546  if(imuIterB->first == stamp)
547  {
548  imuT = imuIterB->second;
549  }
550  else if(imuIterA != imuIterB)
551  {
552  //interpolate:
553  imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
554  }
555  else if(stamp > imuIterB->first)
556  {
557  UWARN("No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
558  }
559  else
560  {
561  UWARN("No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
562  }
563  return imuT;
564 }
565 
567  const std::map<double, Transform> & tfBuffer,
568  const double & stamp,
569  double * stampDiff)
570 {
571  UASSERT(!tfBuffer.empty());
572  std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
573  std::map<double, Transform>::const_iterator imuIterA = imuIterB;
574  if(imuIterA != tfBuffer.begin())
575  {
576  imuIterA = --imuIterA;
577  }
578  if(imuIterB == tfBuffer.end())
579  {
580  imuIterB = --imuIterB;
581  }
582  Transform imuT;
583  if(imuIterB->first == stamp || imuIterA == imuIterB)
584  {
585  imuT = imuIterB->second;
586  if(stampDiff)
587  {
588  *stampDiff = fabs(imuIterB->first - stamp);
589  }
590  }
591  else if(imuIterA != imuIterB)
592  {
593  //interpolate:
594  imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
595  if(stampDiff)
596  {
597  *stampDiff = 0.0;
598  }
599  }
600  return imuT;
601 }
602 
603 
604 }
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
Definition: Transform.cpp:372
rtabmap::Transform::translationMatrix
cv::Mat translationMatrix() const
Definition: Transform.cpp:253
list::end
detail::list_iterator end() const
rtabmap::Transform::is3DoF
bool is3DoF() const
Definition: Transform.cpp:234
uNorm
T uNorm(const std::vector< T > &v)
Definition: UMath.h:573
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
Eigen::Transform
s
RealScalar s
list
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
rtabmap::Transform::fromEigen4f
static Transform fromEigen4f(const Eigen::Matrix4f &matrix)
Definition: Transform.cpp:416
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:283
this
this
B
Transform.h
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::Transform::operator*
Transform operator*(const Transform &t) const
Definition: Transform.cpp:340
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:475
rtabmap::Transform::isInvertible
bool isInvertible() const
Definition: Transform.cpp:169
os
ofstream os("timeSchurFactors.csv")
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
util3d.h
Vector2::y
float y
rtabmap::Transform::to4DoF
Transform to4DoF() const
Definition: Transform.cpp:222
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Transform::o14
float o14() const
Definition: Transform.h:72
vB
Vector vB(size_t i)
UMath.h
Basic mathematics functions.
rtabmap::Transform::Transform
Transform()
Definition: Transform.cpp:41
Eigen::Affine3f
Transform< float, 3, Affine > Affine3f
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:258
fabs
Real fabs(const Real &a)
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:391
uIsNan
bool uIsNan(const T &value)
Definition: UMath.h:40
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::Transform::getNormSquared
float getNormSquared() const
Definition: Transform.cpp:288
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
vA
Vector vA(size_t i)
list::size
size_t size() const
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:263
A
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::Transform::getAngle
float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const
Definition: Transform.cpp:276
rtabmap::Transform::getTransform
static Transform getTransform(const std::map< double, Transform > &tfBuffer, const double &stamp)
Definition: Transform.cpp:530
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
UConversion.h
Some conversion functions.
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
Definition: Transform.cpp:303
rtabmap::Transform::operator!=
bool operator!=(const Transform &t) const
Definition: Transform.cpp:359
rtabmap::Transform::o34
float o34() const
Definition: Transform.h:74
rtabmap::Transform::o24
float o24() const
Definition: Transform.h:73
Eigen::Quaternionf
Quaternion< float > Quaternionf
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
rtabmap::Transform::rotationMatrix
cv::Mat rotationMatrix() const
Definition: Transform.cpp:248
Eigen::Quaterniond
Quaternion< double > Quaterniond
UASSERT
#define UASSERT(condition)
m
Matrix3f m
rtabmap::Transform::operator==
bool operator==(const Transform &t) const
Definition: Transform.cpp:354
rtabmap::Transform::getDistanceSquared
float getDistanceSquared(const Transform &t) const
Definition: Transform.cpp:298
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:293
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:381
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:435
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::Transform::getClosestTransform
static RTABMAP_DEPRECATED Transform getClosestTransform(const std::map< double, Transform > &tfBuffer, const double &stamp, double *stampDiff)
Definition: Transform.cpp:566
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::Transform::fromEigen4d
static Transform fromEigen4d(const Eigen::Matrix4d &matrix)
Definition: Transform.cpp:422
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
qz
RealQZ< MatrixXf > qz(4)
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Transform
Definition: Transform.h:41
rtabmap::Transform::fromEigen3f
static Transform fromEigen3f(const Eigen::Affine3f &matrix)
Definition: Transform.cpp:429
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
Eigen::Transform::linear
EIGEN_DEVICE_FUNC LinearPart linear()
iter
iterator iter(handle obj)
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Transform::data_
cv::Mat data_
Definition: Transform.h:178
rtabmap::Transform::normalizeRotation
void normalizeRotation()
Definition: Transform.cpp:316
rtabmap::Transform::getTranslation
void getTranslation(float &x, float &y, float &z) const
Definition: Transform.cpp:269
Matrix< Scalar, Dynamic, Dynamic >
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:396
cv
Definition: Features2d.h:41
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:558
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
rtabmap::Transform::operator*=
Transform & operator*=(const Transform &t)
Definition: Transform.cpp:348
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
matrix
Map< const Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(const T *data, int rows, int cols, int stride)
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
t
Point2 t(10, 10)
rtabmap::Transform::translation
Transform translation() const
Definition: Transform.cpp:203
rtabmap::Transform::is4DoF
bool is4DoF() const
Definition: Transform.cpp:239
rtabmap
Definition: CameraARCore.cpp:35
list::empty
bool empty() const
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
rtabmap::Transform::getQuaterniond
Eigen::Quaterniond getQuaterniond() const
Definition: Transform.cpp:406
Eigen::Affine3d
Transform< double, 3, Affine > Affine3d
rtabmap::operator<<
RTABMAP_CORE_EXPORT std::ostream & operator<<(std::ostream &os, const CameraModel &model)
Definition: CameraModel.cpp:801
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
i
int i
other
other
rtabmap::Transform::getQuaternionf
Eigen::Quaternionf getQuaternionf() const
Definition: Transform.cpp:401
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)
rtabmap::Transform::canParseString
static bool canParseString(const std::string &string)
Definition: Transform.cpp:524
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62
list::begin
detail::list_iterator begin() const


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