MRPT  2.0.4
pose.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9
10 /*
11  * pose.cpp
12  *
13  * Created on: Mar 14, 2012
14  * Author: Pablo Iñigo Blasco
15  *
16  * To understand better how this is implemented see the references:
17  * - http://www.mrpt.org/2D_3D_Geometry
18  *
19  * Aug 17, 2019: Refactored into mrpt::ros1bridge library for MRPT 2.0 (JLBC)
20  *
21  */
22
23 #include <geometry_msgs/Pose.h>
24 #include <geometry_msgs/PoseWithCovariance.h>
25 #include <geometry_msgs/Quaternion.h>
26 #include <mrpt/core/exceptions.h>
27 #include <mrpt/math/CQuaternion.h>
28 #include <mrpt/math/TPose2D.h>
29 #include <mrpt/poses/CPose2D.h>
30 #include <mrpt/poses/CPose3D.h>
35 #include <mrpt/ros1bridge/pose.h>
36 #include <tf2/LinearMath/Matrix3x3.h>
37
38 // MRPT -> ROS functions:
39 namespace mrpt::ros1bridge
40 {
41 tf2::Matrix3x3 toROS(const mrpt::math::CMatrixDouble33& src)
42 {
43  tf2::Matrix3x3 des;
44  for (int r = 0; r < 3; r++)
45  for (int c = 0; c < 3; c++) des[r][c] = src(r, c);
46  return des;
47 }
48
49 tf2::Transform toROS_tfTransform(const mrpt::poses::CPose2D& src)
50 {
52 }
53
54 geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose2D& src)
55 {
57 }
58
59 tf2::Transform toROS_tfTransform(const mrpt::math::TPose2D& src)
60 {
62 }
63
64 geometry_msgs::Pose toROS_Pose(const mrpt::math::TPose2D& src)
65 {
66  geometry_msgs::Pose des;
67
68  des.position.x = src.x;
69  des.position.y = src.y;
70  des.position.z = 0;
71
72  const double yaw = src.phi;
73  if (std::abs(yaw) < 1e-10)
74  {
75  des.orientation.x = 0.;
76  des.orientation.y = 0.;
77  des.orientation.z = .5 * yaw;
78  des.orientation.w = 1.;
79  }
80  else
81  {
82  const double s = ::sin(yaw * .5);
83  const double c = ::cos(yaw * .5);
84  des.orientation.x = 0.;
85  des.orientation.y = 0.;
86  des.orientation.z = s;
87  des.orientation.w = c;
88  }
89
90  return des;
91 }
92
93 tf2::Transform toROS_tfTransform(const mrpt::poses::CPose3D& src)
94 {
95  tf2::Transform des;
96  des.setBasis(toROS(src.getRotationMatrix()));
97  des.setOrigin(tf2::Vector3(src.x(), src.y(), src.z()));
98  return des;
99 }
100
101 geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose3D& src)
102 {
103  geometry_msgs::Pose des;
104  des.position.x = src.x();
105  des.position.y = src.y();
106  des.position.z = src.z();
107
109  src.getAsQuaternion(q);
110
111  des.orientation.x = q.x();
112  des.orientation.y = q.y();
113  des.orientation.z = q.z();
114  des.orientation.w = q.r();
115
116  return des;
117 }
118
119 tf2::Transform toROS_tfTransform(const mrpt::math::TPose3D& src)
120 {
122 }
123
124 geometry_msgs::Pose toROS_Pose(const mrpt::math::TPose3D& src)
125 {
127 }
128
129 geometry_msgs::PoseWithCovariance toROS_Pose(
131 {
132  geometry_msgs::PoseWithCovariance des;
133  des.pose = toROS_Pose(src.mean);
134
136  // # Row-major representation of the 6x6 covariance matrix
137  // # The orientation parameters use a fixed-axis representation.
138  // # In order, the parameters are:
140  // Z axis)
141  // float64[36] covariance
142  // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a
143  // transform Jacobian here!"
144  // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations.
145
146  // X,Y,Z,YAW,PITCH,ROLL
147  const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};
148  for (int i = 0; i < 6; i++)
149  for (int j = 0; j < 6; j++)
150  des.covariance[indxs_map[i] * 6 + indxs_map[j]] = src.cov(i, j);
151  return des;
152 }
153
154 geometry_msgs::PoseWithCovariance toROS(
156 {
158  src2.copyFrom(src);
160 }
161
162 geometry_msgs::PoseWithCovariance toROS(
164 {
165  geometry_msgs::PoseWithCovariance des;
166
167  des.pose = toROS_Pose(src.mean);
168
170  // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a
171  // transform Jacobian here!"
172  // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations.
173
174  // geometry_msgs/PoseWithCovariance msg stores the covariance matrix in
175  // row-major representation
176  // Indexes are :
177  // [ 0 1 2 3 4 5 ]
178  // [ 6 7 8 9 10 11 ]
179  // [ 12 13 14 15 16 17 ]
180  // [ 18 19 20 21 22 23 ]
181  // [ 24 25 26 27 28 29 ]
182  // [ 30 31 32 33 34 35 ]
183
184  des.covariance[0] = src.cov(0, 0);
185  des.covariance[1] = src.cov(0, 1);
186  des.covariance[5] = src.cov(0, 2);
187  des.covariance[6] = src.cov(1, 0);
188  des.covariance[7] = src.cov(1, 1);
189  des.covariance[11] = src.cov(1, 2);
190  des.covariance[30] = src.cov(2, 0);
191  des.covariance[31] = src.cov(2, 1);
192  des.covariance[35] = src.cov(2, 2);
193
194  return des;
195 }
196
197 geometry_msgs::PoseWithCovariance toROS(
199 {
201  src2.copyFrom(src);
202
204 }
205
206 geometry_msgs::Quaternion toROS(const mrpt::math::CQuaternionDouble& src)
207 {
208  geometry_msgs::Quaternion des;
209  des.x = src.x();
210  des.y = src.y();
211  des.z = src.z();
212  des.w = src.r();
213  return des;
214 }
215
216 } // namespace mrpt::ros1bridge
217
218 // ROS -> MRPT functions:
219 namespace mrpt::ros1bridge
220 {
221 mrpt::poses::CPose3D fromROS(const tf2::Transform& src)
222 {
224  const tf2::Vector3& t = src.getOrigin();
225  des.x(t[0]);
226  des.y(t[1]);
227  des.z(t[2]);
228  des.setRotationMatrix(fromROS(src.getBasis()));
229  return des;
230 }
231 mrpt::math::CMatrixDouble33 fromROS(const tf2::Matrix3x3& src)
232 {
234  for (int r = 0; r < 3; r++)
235  for (int c = 0; c < 3; c++) des(r, c) = src[r][c];
236  return des;
237 }
238
239 mrpt::poses::CPose3D fromROS(const geometry_msgs::Pose& src)
240 {
242  src.orientation.w, src.orientation.x, src.orientation.y,
243  src.orientation.z);
244  return mrpt::poses::CPose3D(
245  q, src.position.x, src.position.y, src.position.z);
246 }
247
249  const geometry_msgs::PoseWithCovariance& src)
250 {
252
253  des.mean = fromROS(src.pose);
254
255  const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};
256
257  for (int i = 0; i < 6; i++)
258  for (int j = 0; j < 6; j++)
259  des.cov(i, j) = src.covariance[indxs_map[i] * 6 + indxs_map[j]];
260
261  return des;
262 }
263
264 mrpt::math::CQuaternionDouble fromROS(const geometry_msgs::Quaternion& src)
265 {
267  des.x(src.x);
268  des.y(src.y);
269  des.z(src.z);
270  des.r(src.w);
271  return des;
272 }
273
274 } // namespace mrpt::ros1bridge
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
CPose2D mean
The mean value.
CPose3D mean
The mean value.
double x
X,Y coordinates.
Definition: TPose2D.h:30
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:237
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:501
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Lightweight 2D pose.
Definition: TPose2D.h:22
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
tf2::Transform toROS_tfTransform(const mrpt::poses::CPose2D &src)
Definition: pose.cpp:49
geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose2D &src)
Definition: pose.cpp:54
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
double phi