pose.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
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 
28 #include <mrpt/poses/CPose2D.h>
29 #include <mrpt/poses/CPose3D.h>
30 #include <mrpt/poses/CPosePDFGaussian.h>
31 #include <mrpt/poses/CPose3DPDFGaussian.h>
32 #include <mrpt/poses/CPosePDFGaussianInf.h>
33 #include <mrpt/poses/CPose3DPDFGaussianInf.h>
34 #include <mrpt/math/CQuaternion.h>
35 #include <mrpt/math/lightweight_geom_data.h>
36 #include <geometry_msgs/PoseWithCovariance.h>
37 #include <geometry_msgs/Pose.h>
38 #include <geometry_msgs/Quaternion.h>
39 #include <tf/tf.h>
40 
41 #include <mrpt/version.h>
42 #if MRPT_VERSION < 0x199
43 #include <mrpt/utils/mrpt_macros.h>
44 #else
45 #include <mrpt/core/exceptions.h>
46 #endif
47 
48 #include "mrpt_bridge/pose.h"
49 
50 mrpt::math::CMatrixDouble33& mrpt_bridge::convert(
51  const tf::Matrix3x3& _src, mrpt::math::CMatrixDouble33& _des)
52 {
53  for (int r = 0; r < 3; r++)
54  for (int c = 0; c < 3; c++) _des(r, c) = _src[r][c];
55  return _des;
56 }
57 
58 tf::Matrix3x3& mrpt_bridge::convert(
59  const mrpt::math::CMatrixDouble33& _src, tf::Matrix3x3& _des)
60 {
61  for (int r = 0; r < 3; r++)
62  for (int c = 0; c < 3; c++) _des[r][c] = _src(r, c);
63  return _des;
64 }
65 
66 tf::Transform& mrpt_bridge::convert(
67  const mrpt::poses::CPose3DPDFGaussian& _src, tf::Transform& _des)
68 {
69  return convert(_src.mean, _des);
70 }
71 
72 geometry_msgs::PoseWithCovariance& mrpt_bridge::convert(
73  const mrpt::poses::CPose3DPDFGaussian& _src,
74  geometry_msgs::PoseWithCovariance& _des)
75 {
76  convert(_src.mean, _des.pose);
77 
78  // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation
79  // # Row-major representation of the 6x6 covariance matrix
80  // # The orientation parameters use a fixed-axis representation.
81  // # In order, the parameters are:
82  // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about
83  // Z axis)
84  // float64[36] covariance
85  // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a
86  // transform Jacobian here!"
87  // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations.
88 
89  const unsigned int indxs_map[6] = {0, 1, 2,
90  5, 4, 3}; // X,Y,Z,YAW,PITCH,ROLL
91 
92  for (int i = 0; i < 6; i++)
93  {
94  for (int j = 0; j < 6; j++)
95  {
96  _des.covariance[indxs_map[i] * 6 + indxs_map[j]] = _src.cov(i, j);
97  }
98  }
99  return _des;
100 }
101 
102 geometry_msgs::PoseWithCovariance& mrpt_bridge::convert(
103  const mrpt::poses::CPose3DPDFGaussianInf& _src,
104  geometry_msgs::PoseWithCovariance& _des)
105 {
106  mrpt::poses::CPose3DPDFGaussian mrpt_gaussian;
107  mrpt_gaussian.copyFrom(_src);
108  convert(mrpt_gaussian, _des);
109 
110  return _des;
111 }
112 
113 tf::Transform& mrpt_bridge::convert(
114  const mrpt::poses::CPose3D& _src, tf::Transform& _des)
115 {
116  tf::Vector3 origin(_src[0], _src[1], _src[2]);
117  mrpt::math::CMatrixDouble33 R;
118  _src.getRotationMatrix(R);
119  tf::Matrix3x3 basis;
120  _des.setBasis(convert(R, basis));
121  _des.setOrigin(origin);
122 
123  return _des;
124 }
125 mrpt::poses::CPose3D& mrpt_bridge::convert(
126  const tf::Transform& _src, mrpt::poses::CPose3D& _des)
127 {
128  const tf::Vector3& t = _src.getOrigin();
129  _des.x() = t[0], _des.y() = t[1], _des.z() = t[2];
130  const tf::Matrix3x3& basis = _src.getBasis();
131  mrpt::math::CMatrixDouble33 R;
132  convert(basis, R);
133  _des.setRotationMatrix(R);
134 
135  return _des;
136 }
137 
138 geometry_msgs::Pose& mrpt_bridge::convert(
139  const mrpt::poses::CPose3D& _src, geometry_msgs::Pose& _des)
140 {
141  _des.position.x = _src[0];
142  _des.position.y = _src[1];
143  _des.position.z = _src[2];
144 
145  mrpt::math::CQuaternionDouble q;
146  _src.getAsQuaternion(q);
147 
148  _des.orientation.x = q.x();
149  _des.orientation.y = q.y();
150  _des.orientation.z = q.z();
151  _des.orientation.w = q.r();
152 
153  return _des;
154 }
155 
157 geometry_msgs::Pose& mrpt_bridge::convert(
158  const mrpt::poses::CPose2D& _src, geometry_msgs::Pose& _des)
159 {
160  _des.position.x = _src.x();
161  _des.position.y = _src.y();
162  _des.position.z = 0;
163 
164  const double yaw = _src.phi();
165  if (std::abs(yaw) < 1e-10)
166  {
167  _des.orientation.x = 0.;
168  _des.orientation.y = 0.;
169  _des.orientation.z = .5 * yaw;
170  _des.orientation.w = 1.;
171  }
172  else
173  {
174  const double s = ::sin(yaw * .5);
175  const double c = ::cos(yaw * .5);
176  _des.orientation.x = 0.;
177  _des.orientation.y = 0.;
178  _des.orientation.z = s;
179  _des.orientation.w = c;
180  }
181 
182  return _des;
183 }
184 
186 geometry_msgs::Pose& mrpt_bridge::convert(
187  const mrpt::math::TPose2D& _src, geometry_msgs::Pose& _des)
188 {
189  _des.position.x = _src.x;
190  _des.position.y = _src.y;
191  _des.position.z = 0;
192 
193  const double yaw = _src.phi;
194  if (std::abs(yaw) < 1e-10)
195  {
196  _des.orientation.x = 0.;
197  _des.orientation.y = 0.;
198  _des.orientation.z = .5 * yaw;
199  _des.orientation.w = 1.;
200  }
201  else
202  {
203  const double s = ::sin(yaw * .5);
204  const double c = ::cos(yaw * .5);
205  _des.orientation.x = 0.;
206  _des.orientation.y = 0.;
207  _des.orientation.z = s;
208  _des.orientation.w = c;
209  }
210 
211  return _des;
212 }
213 
214 mrpt::poses::CPose2D& mrpt_bridge::convert(
215  const geometry_msgs::Pose& _src, mrpt::poses::CPose2D& _des)
216 {
217  _des.x(_src.position.x);
218  _des.y(_src.position.y);
219 
220  mrpt::math::CQuaternionDouble quat;
221  convert(_src.orientation, quat);
222 
223  double roll, pitch, yaw;
224  quat.rpy(roll, pitch, yaw);
225 
226  _des.phi(yaw);
227 
228  return _des;
229 }
230 
231 geometry_msgs::PoseWithCovariance& mrpt_bridge::convert(
232  const mrpt::poses::CPosePDFGaussian& _src,
233  geometry_msgs::PoseWithCovariance& _des)
234 {
235  convert(_src.mean, _des.pose);
236 
237  // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation
238  // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a
239  // transform Jacobian here!"
240  // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations.
241 
242  // geometry_msgs/PoseWithCovariance msg stores the covariance matrix in
243  // row-major representation
244  // Indexes are :
245  // [ 0 1 2 3 4 5 ]
246  // [ 6 7 8 9 10 11 ]
247  // [ 12 13 14 15 16 17 ]
248  // [ 18 19 20 21 22 23 ]
249  // [ 24 25 26 27 28 29 ]
250  // [ 30 31 32 33 34 35 ]
251 
252  _des.covariance[0] = _src.cov(0, 0);
253  _des.covariance[1] = _src.cov(0, 1);
254  _des.covariance[5] = _src.cov(0, 2);
255  _des.covariance[6] = _src.cov(1, 0);
256  _des.covariance[7] = _src.cov(1, 1);
257  _des.covariance[11] = _src.cov(1, 2);
258  _des.covariance[30] = _src.cov(2, 0);
259  _des.covariance[31] = _src.cov(2, 1);
260  _des.covariance[35] = _src.cov(2, 2);
261 
262  return _des;
263 }
264 
265 geometry_msgs::PoseWithCovariance& mrpt_bridge::convert(
266  const mrpt::poses::CPosePDFGaussianInf& _src,
267  geometry_msgs::PoseWithCovariance& _des)
268 {
269  mrpt::poses::CPosePDFGaussian mrpt_gaussian;
270  mrpt_gaussian.copyFrom(_src);
271 
272  convert(mrpt_gaussian, _des);
273  return _des;
274 }
275 
276 mrpt::poses::CPose3DPDFGaussian& mrpt_bridge::convert(
277  const geometry_msgs::PoseWithCovariance& _src,
278  mrpt::poses::CPose3DPDFGaussian& _des)
279 {
280  convert(_src.pose, _des.mean);
281 
282  const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};
283 
284  for (int i = 0; i < 6; i++)
285  {
286  for (int j = 0; j < 6; j++)
287  {
288  _des.cov(i, j) = _src.covariance[indxs_map[i] * 6 + indxs_map[j]];
289  }
290  }
291 
292  return _des;
293 }
294 
295 mrpt::poses::CPose3DPDFGaussianInf& mrpt_bridge::convert(
296  const geometry_msgs::PoseWithCovariance& _src,
297  mrpt::poses::CPose3DPDFGaussianInf& _des)
298 {
299  mrpt::poses::CPose3DPDFGaussian mrpt_gaussian;
300  convert(
301  _src, mrpt_gaussian); // Intermediate transform => CPose3DPDFGaussian
302  _des.copyFrom(mrpt_gaussian);
303 
304  return _des;
305 }
306 
307 mrpt::poses::CPosePDFGaussian& mrpt_bridge::convert(
308  const geometry_msgs::PoseWithCovariance& _src,
309  mrpt::poses::CPosePDFGaussian& _des)
310 {
311  convert(_src.pose, _des.mean);
312 
313  _des.cov(0, 0) = _src.covariance[0];
314  _des.cov(0, 1) = _src.covariance[1];
315  _des.cov(0, 2) = _src.covariance[5];
316  _des.cov(1, 0) = _src.covariance[0 + 6];
317  _des.cov(1, 1) = _src.covariance[1 + 6];
318  _des.cov(1, 2) = _src.covariance[5 + 6];
319  _des.cov(2, 0) = _src.covariance[0 + 30];
320  _des.cov(2, 1) = _src.covariance[1 + 30];
321  _des.cov(2, 2) = _src.covariance[5 + 30];
322 
323  return _des;
324 }
325 
326 mrpt::poses::CPosePDFGaussianInf& mrpt_bridge::convert(
327  const geometry_msgs::PoseWithCovariance& _src,
328  mrpt::poses::CPosePDFGaussianInf& _des)
329 {
330  mrpt::poses::CPosePDFGaussian mrpt_gaussian;
331  convert(_src, mrpt_gaussian); // intermediate transform: PoseWithCovariance
332  // => CPosePDFGaussian
333  _des.copyFrom(mrpt_gaussian);
334 
335  return _des;
336 }
337 
338 mrpt::poses::CQuaternionDouble& mrpt_bridge::convert(
339  const geometry_msgs::Quaternion& _src, mrpt::poses::CQuaternionDouble& _des)
340 {
341  _des.x(_src.x);
342  _des.y(_src.y);
343  _des.z(_src.z);
344  _des.r(_src.w);
345  // Ensure the real part of the quaternion is >=0.
346  // It seems ROS does not always ensure it.
347  if (_des.r() < 0)
348  {
349  _des.r(-_des.r());
350  _des.x(-_des.x());
351  _des.y(-_des.y());
352  _des.z(-_des.z());
353  }
354  return _des;
355 }
356 
357 geometry_msgs::Quaternion& mrpt_bridge::convert(
358  const mrpt::poses::CQuaternionDouble& _src, geometry_msgs::Quaternion& _des)
359 {
360  _des.x = _src.x();
361  _des.y = _src.y();
362  _des.z = _src.z();
363  _des.w = _src.r();
364 
365  return _des;
366 }
367 
368 mrpt::poses::CPose3D& mrpt_bridge::convert(
369  const geometry_msgs::Pose& _src, mrpt::poses::CPose3D& _des)
370 {
371  mrpt::math::CQuaternionDouble q(
372  _src.orientation.w, _src.orientation.x, _src.orientation.y,
373  _src.orientation.z);
374  // Ensure the real part of the quaternion is >=0.
375  // It seems ROS does not always ensure it.
376  if (q.r() < 0)
377  {
378  q.r(-q.r());
379  q.x(-q.x());
380  q.y(-q.y());
381  q.z(-q.z());
382  }
383 
384  _des = mrpt::poses::CPose3D(
385  q, _src.position.x, _src.position.y, _src.position.z);
386 
387  return _des;
388 }
389 
390 tf::Transform& mrpt_bridge::convert(
391  const mrpt::math::TPose3D& _src, tf::Transform& _des)
392 {
393  return mrpt_bridge::convert(mrpt::poses::CPose3D(_src), _des);
394 }
395 
396 tf::Transform& mrpt_bridge::convert(
397  const mrpt::poses::CPose2D& _src, tf::Transform& _des)
398 {
399  return mrpt_bridge::convert(mrpt::poses::CPose3D(_src), _des);
400 }
401 tf::Transform& mrpt_bridge::convert(
402  const mrpt::math::TPose2D& _src, tf::Transform& _des)
403 {
404  return mrpt_bridge::convert(
405  mrpt::poses::CPose3D(mrpt::math::TPose3D(_src)), _des);
406 }
tf::Matrix3x3
pose.h
s
XmlRpcServer s
tf::Transform::setBasis
TFSIMD_FORCE_INLINE void setBasis(const Matrix3x3 &basis)
mrpt::poses::CQuaternionDouble
math::CQuaternion< double > CQuaternionDouble
Definition: pose.h:62
tf::Transform::getBasis
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
geometry_msgs::Pose_
Definition: include/mrpt_bridge/beacon.h:24
tf::Transform
geometry_msgs::PoseWithCovariance_
Definition: pose.h:39
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
mrpt::math::CQuaternion
Definition: pose.h:51
tf.h
mrpt_bridge::convert
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
geometry_msgs::Quaternion_
Definition: pose.h:42
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10