Transform.h
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 #ifndef TRANSFORM_H_
29 #define TRANSFORM_H_
30 
31 #include <rtabmap/core/rtabmap_core_export.h>
32 #include <vector>
33 #include <string>
34 #include <map>
35 #include <Eigen/Core>
36 #include <Eigen/Geometry>
37 #include <opencv2/core/core.hpp>
38 
39 namespace rtabmap {
40 
41 class RTABMAP_CORE_EXPORT Transform
42 {
43 public:
44 
45  // Zero by default
46  Transform();
47  // rotation matrix r## and origin o##
48  Transform(float r11, float r12, float r13, float o14,
49  float r21, float r22, float r23, float o24,
50  float r31, float r32, float r33, float o34);
51  // should have 3 rows, 4 cols and type CV_32FC1
52  Transform(const cv::Mat & transformationMatrix);
53  // x,y,z, roll,pitch,yaw
54  Transform(float x, float y, float z, float roll, float pitch, float yaw);
55  // x,y,z, qx,qy,qz,qw
56  Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
57  // x,y, theta
58  Transform(float x, float y, float theta);
59 
60  Transform clone() const;
61 
62  float r11() const {return data()[0];}
63  float r12() const {return data()[1];}
64  float r13() const {return data()[2];}
65  float r21() const {return data()[4];}
66  float r22() const {return data()[5];}
67  float r23() const {return data()[6];}
68  float r31() const {return data()[8];}
69  float r32() const {return data()[9];}
70  float r33() const {return data()[10];}
71 
72  float o14() const {return data()[3];}
73  float o24() const {return data()[7];}
74  float o34() const {return data()[11];}
75 
76  float & operator[](int index) {return data()[index];}
77  const float & operator[](int index) const {return data()[index];}
78  float & operator()(int row, int col) {return data()[row*4 + col];}
79  const float & operator()(int row, int col) const {return data()[row*4 + col];}
80 
81  bool isNull() const;
82  bool isIdentity() const;
83 
84  void setNull();
85  void setIdentity();
86 
87  const cv::Mat & dataMatrix() const {return data_;}
88  const float * data() const {return (const float *)data_.data;}
89  float * data() {return (float *)data_.data;}
90  int size() const {return 12;}
91 
92  float & x() {return data()[3];}
93  float & y() {return data()[7];}
94  float & z() {return data()[11];}
95  const float & x() const {return data()[3];}
96  const float & y() const {return data()[7];}
97  const float & z() const {return data()[11];}
98 
99  float theta() const;
100 
101  bool isInvertible() const;
102  Transform inverse() const;
103  Transform rotation() const;
104  Transform translation() const;
105  Transform to3DoF() const;
106  Transform to4DoF() const;
107  bool is3DoF() const;
108  bool is4DoF() const;
109 
110  cv::Mat rotationMatrix() const;
111  cv::Mat translationMatrix() const;
112 
113  void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
114  void getEulerAngles(float & roll, float & pitch, float & yaw) const;
115  void getTranslation(float & x, float & y, float & z) const;
116  float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
117  float getNorm() const;
118  float getNormSquared() const;
119  float getDistance(const Transform & t) const;
120  float getDistanceSquared(const Transform & t) const;
121  Transform interpolate(float t, const Transform & other) const;
122  void normalizeRotation();
123  std::string prettyPrint() const;
124 
125  Transform operator*(const Transform & t) const;
126  Transform & operator*=(const Transform & t);
127  bool operator==(const Transform & t) const;
128  bool operator!=(const Transform & t) const;
129 
130  Eigen::Matrix4f toEigen4f() const;
131  Eigen::Matrix4d toEigen4d() const;
132  Eigen::Affine3f toEigen3f() const;
133  Eigen::Affine3d toEigen3d() const;
134 
135  Eigen::Quaternionf getQuaternionf() const;
136  Eigen::Quaterniond getQuaterniond() const;
137 
138 public:
139  static Transform getIdentity();
140  static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
141  static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
142  static Transform fromEigen3f(const Eigen::Affine3f & matrix);
143  static Transform fromEigen3d(const Eigen::Affine3d & matrix);
144  static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
145  static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
146  static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
147  static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);
148 
150  0.0f, -1.0f, 0.0f, 0.0f,
151  0.0f, 0.0f, 1.0f, 0.0f,
152  -1.0f, 0.0f, 0.0f, 0.0f);}
154  0.0f, 0.0f,-1.0f, 0.0f,
155  -1.0f, 0.0f, 0.0f, 0.0f,
156  0.0f, 1.0f, 0.0f, 0.0f);}
157 
165  static Transform fromString(const std::string & string);
166  static bool canParseString(const std::string & string);
167 
168  static Transform getTransform(
169  const std::map<double, Transform> & tfBuffer,
170  const double & stamp);
171  // Use Transform::getTransform() instead to get always accurate transforms.
172  RTABMAP_DEPRECATED static Transform getClosestTransform(
173  const std::map<double, Transform> & tfBuffer,
174  const double & stamp,
175  double * stampDiff);
176 
177 private:
178  cv::Mat data_;
179 };
180 
181 RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const Transform& s);
182 
184 {
185 public:
186  TransformStamped(const Transform & transform, const double & stamp) :
188  stamp_(stamp)
189  {}
190  const Transform & transform() const {return transform_;}
191  const double & stamp() const {return stamp_;}
192 
193 private:
195  double stamp_;
196 };
197 
198 }
199 
200 #endif /* TRANSFORM_H_ */
rtabmap::TransformStamped
Definition: Transform.h:183
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
Eigen::Transform
rtabmap::Transform::operator[]
const float & operator[](int index) const
Definition: Transform.h:77
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
rtabmap::Transform::operator()
float & operator()(int row, int col)
Definition: Transform.h:78
rtabmap::Transform::opengl_T_rtabmap
static Transform opengl_T_rtabmap()
Definition: Transform.h:149
rtabmap::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.")
col
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ColXpr col(Index i) const
y
Matrix3f y
operator*=
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 & operator*=(bfloat16 &a, const bfloat16 &b)
setIdentity
m setIdentity(3, 3)
rtabmap::Transform::operator[]
float & operator[](int index)
Definition: Transform.h:76
rtabmap::Transform::o14
float o14() const
Definition: Transform.h:72
rtabmap::TransformStamped::transform
const Transform & transform() const
Definition: Transform.h:190
glm::detail::operator==
GLM_FUNC_DECL bool operator==(tvec1< T, P > const &v1, tvec1< T, P > const &v2)
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
translation
translation
data
int data[]
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
rtabmap::Transform::rtabmap_T_opengl
static Transform rtabmap_T_opengl()
Definition: Transform.h:153
glm::detail::operator*
GLM_FUNC_DECL tmat2x2< T, P > operator*(tmat2x2< T, P > const &m, T const &s)
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
rtabmap::Transform::operator()
const float & operator()(int row, int col) const
Definition: Transform.h:79
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::Transform::o34
float o34() const
Definition: Transform.h:74
rtabmap::Transform::o24
float o24() const
Definition: Transform.h:73
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
rtabmap::Transform::y
const float & y() const
Definition: Transform.h:96
z
z
x
x
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Transform::data
float * data()
Definition: Transform.h:89
rtabmap::Transform::dataMatrix
const cv::Mat & dataMatrix() const
Definition: Transform.h:87
rtabmap::TransformStamped::stamp_
double stamp_
Definition: Transform.h:195
rtabmap::TransformStamped::stamp
const double & stamp() const
Definition: Transform.h:191
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Transform::x
const float & x() const
Definition: Transform.h:95
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
qz
RealQZ< MatrixXf > qz(4)
Eigen::Quaternion
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Transform
Definition: Transform.h:41
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::Transform::data_
cv::Mat data_
Definition: Transform.h:178
glm::detail::operator!=
GLM_FUNC_DECL bool operator!=(tvec1< T, P > const &v1, tvec1< T, P > const &v2)
rtabmap::Transform::z
const float & z() const
Definition: Transform.h:97
Eigen::Matrix
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
rtabmap::TransformStamped::TransformStamped
TransformStamped(const Transform &transform, const double &stamp)
Definition: Transform.h:186
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::TransformStamped::transform_
Transform transform_
Definition: Transform.h:194
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
rtabmap::operator<<
RTABMAP_CORE_EXPORT std::ostream & operator<<(std::ostream &os, const CameraModel &model)
Definition: CameraModel.cpp:801
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62


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