test_pose.cpp
Go to the documentation of this file.
1 /*
2  * test_pose.cpp
3  *
4  * Created on: Mar 15, 2012
5  * Author: Pablo Iñigo Blasco
6  */
7 
8 #include <mrpt/poses/CPosePDFGaussian.h>
9 #include <mrpt/poses/CPose3DPDFGaussian.h>
10 #include <mrpt/math/CQuaternion.h>
11 #include <geometry_msgs/PoseWithCovariance.h>
12 #include <geometry_msgs/Pose.h>
13 #include <geometry_msgs/Quaternion.h>
14 #include <tf/tf.h>
15 #include <mrpt_bridge/pose.h>
16 #include <gtest/gtest.h>
17 #include <Eigen/Dense>
18 
19 using namespace std;
20 
21 #if MRPT_VERSION >= 0x199
22 #define getAsVectorVal asVectorVal
23 using mrpt::DEG2RAD;
24 #else
25 using mrpt::utils::DEG2RAD;
26 #endif
27 
28 void checkPoseMatrixFromRotationParameters(
29  const double roll, const double pitch, const double yaw)
30 {
31  // TF-BULLET ROTATION
32  tf::Pose original_pose;
33  tf::Quaternion rotation;
34  rotation.setRPY(roll, pitch, yaw);
35  original_pose.setRotation(rotation);
36  tf::Matrix3x3 basis = original_pose.getBasis();
37 
38  // MRPT-ROTATION
39  mrpt::poses::CPose3D mrpt_original_pose;
40  mrpt_original_pose.setYawPitchRoll(yaw, pitch, roll);
41  mrpt::math::CMatrixDouble33 mrpt_basis =
42  mrpt_original_pose.getRotationMatrix();
43 
44  for (int i = 0; i < 3; i++)
45  for (int j = 0; j < 3; j++)
46  EXPECT_NEAR(basis[i][j], mrpt_basis(i, j), 0.01);
47 }
48 
49 TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
50 {
51  tf::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8);
52  mrpt::math::CMatrixDouble33 des;
53  mrpt_bridge::convert(src, des);
54  for (int r = 0; r < 3; r++)
55  for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]);
56 }
57 TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3)
58 {
59  mrpt::math::CMatrixDouble33 src;
60  for (int r = 0; r < 3; r++)
61  for (int c = 0; c < 3; c++) src(r, c) = 12.0 + r * 4 - c * 2 + r * c;
62 
63  tf::Matrix3x3 des;
64  mrpt_bridge::convert(src, des);
65  for (int r = 0; r < 3; r++)
66  for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c));
67 }
68 
69 TEST(PoseConversions, checkPoseMatrixFromRotationParameters)
70 {
71  checkPoseMatrixFromRotationParameters(0, 0, 0);
72  checkPoseMatrixFromRotationParameters(0.2, 0, 0);
73  checkPoseMatrixFromRotationParameters(0, 0.2, 0);
74  checkPoseMatrixFromRotationParameters(0, 0, 0.2);
75  checkPoseMatrixFromRotationParameters(0.4, 0.3, 0.2);
76  checkPoseMatrixFromRotationParameters(M_PI, -M_PI / 2.0, 0);
77 }
78 
79 // Declare a test
80 TEST(PoseConversions, reference_frame_change_with_rotations)
81 {
82  geometry_msgs::PoseWithCovariance ros_msg_original_pose;
83 
84  ros_msg_original_pose.pose.position.x = 1;
85  ros_msg_original_pose.pose.position.y = 0;
86  ros_msg_original_pose.pose.position.z = 0;
87  ros_msg_original_pose.pose.orientation.x = 0;
88  ros_msg_original_pose.pose.orientation.y = 0;
89  ros_msg_original_pose.pose.orientation.z = 0;
90  ros_msg_original_pose.pose.orientation.w = 1;
91 
92  // to mrpt
93  mrpt::poses::CPose3DPDFGaussian mrpt_original_pose;
94  mrpt_bridge::convert(ros_msg_original_pose, mrpt_original_pose);
95  EXPECT_EQ(
96  ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]);
97 
98  // to tf
99  tf::Pose tf_original_pose;
100  tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose);
101 
102  // rotate yaw pi in MRPT
103  mrpt::poses::CPose3D rotation_mrpt;
104  double yaw = M_PI / 2.0;
105  rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0);
106  mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean;
107  EXPECT_NEAR(mrpt_result[1], 1.0, 0.01);
108 
109  // rotate yaw pi in TF
110  tf::Quaternion rotation_tf;
111  rotation_tf.setRPY(0, 0, yaw);
112  tf::Pose rotation_pose_tf;
113  rotation_pose_tf.setIdentity();
114  rotation_pose_tf.setRotation(rotation_tf);
115  tf::Pose tf_result = rotation_pose_tf * tf_original_pose;
116  EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01);
117 
118  geometry_msgs::Pose mrpt_ros_result;
119  mrpt_bridge::convert(mrpt_result, mrpt_ros_result);
120 
121  EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01);
122  EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01);
123  EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01);
124 }
125 
126 void check_CPose3D_tofrom_ROS(
127  double x, double y, double z, double yaw, double pitch, double roll)
128 {
129  const mrpt::poses::CPose3D p3D(x, y, z, yaw, pitch, roll);
130 
131  // Convert MRPT->ROS
132  geometry_msgs::Pose ros_p3D;
133  mrpt_bridge::convert(p3D, ros_p3D);
134 
135  // Compare ROS quat vs. MRPT quat:
136  mrpt::math::CQuaternionDouble q;
137  p3D.getAsQuaternion(q);
138 
139  EXPECT_NEAR(ros_p3D.position.x, p3D.x(), 1e-4) << "p: " << p3D << endl;
140  EXPECT_NEAR(ros_p3D.position.y, p3D.y(), 1e-4) << "p: " << p3D << endl;
141  EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) << "p: " << p3D << endl;
142 
143  EXPECT_NEAR(ros_p3D.orientation.x, q.x(), 1e-4) << "p: " << p3D << endl;
144  EXPECT_NEAR(ros_p3D.orientation.y, q.y(), 1e-4) << "p: " << p3D << endl;
145  EXPECT_NEAR(ros_p3D.orientation.z, q.z(), 1e-4) << "p: " << p3D << endl;
146  EXPECT_NEAR(ros_p3D.orientation.w, q.r(), 1e-4) << "p: " << p3D << endl;
147 
148  // Test the other path: ROS->MRPT
149  mrpt::poses::CPose3D p_bis;
150  mrpt_bridge::convert(ros_p3D, p_bis);
151 
152  // p_bis==p3D?
153  EXPECT_NEAR(
154  (p_bis.getAsVectorVal() - p3D.getAsVectorVal())
155  .array()
156  .abs()
157  .maxCoeff(),
158  0, 1e-4)
159  << "p_bis: " << p_bis << endl
160  << "p3D: " << p3D << endl;
161 }
162 
163 // Declare a test
164 TEST(PoseConversions, check_CPose3D_tofrom_ROS)
165 {
166  check_CPose3D_tofrom_ROS(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
167  check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
168 
169  check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0));
170  check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0));
171  check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30));
172 
173  check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(-5), DEG2RAD(15), DEG2RAD(-30));
174 
175  check_CPose3D_tofrom_ROS(0, 0, 0, DEG2RAD(0), DEG2RAD(90), DEG2RAD(0));
176  check_CPose3D_tofrom_ROS(0, 0, 0, DEG2RAD(0), DEG2RAD(-90), DEG2RAD(0));
177 }
178 
179 // Declare a test
180 TEST(PoseConversions, check_CPose2D_to_ROS)
181 {
182  const mrpt::poses::CPose2D p2D(1, 2, 0.56);
183 
184  // Convert MRPT->ROS
185  geometry_msgs::Pose ros_p2D;
186  mrpt_bridge::convert(p2D, ros_p2D);
187 
188  // Compare vs. 3D pose:
189  const mrpt::poses::CPose3D p3D = mrpt::poses::CPose3D(p2D);
190  mrpt::poses::CPose3D p3D_ros;
191  mrpt_bridge::convert(ros_p2D, p3D_ros);
192 
193  // p3D_ros should equal p3D
194  EXPECT_NEAR(
195  (p3D_ros.getAsVectorVal() - p3D.getAsVectorVal())
196  .array()
197  .abs()
198  .maxCoeff(),
199  0, 1e-4)
200  << "p3D_ros: " << p3D_ros << endl
201  << "p3D: " << p3D << endl;
202 }
y
GLint GLint GLint GLint GLint GLint y
mrpt::poses::CPose3D::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf::poseMsgToTF
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
c
const GLfloat * c
tf::Transform::getBasis
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
geometry_msgs::Pose_
Definition: include/mrpt_bridge/beacon.h:24
M_PI
#define M_PI
checkPoseMatrixFromRotationParameters
void checkPoseMatrixFromRotationParameters(const double roll, const double pitch, const double yaw)
Definition: test_pose.cpp:28
src
GLuint src
mrpt::poses::CPose3D::setFromValues
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
tf::Transform::setIdentity
void setIdentity()
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
x
GLint GLint GLint GLint GLint x
tf::Quaternion::setRPY
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
mrpt::utils::DEG2RAD
double DEG2RAD(const double x)
mrpt_bridge::convert
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
Definition: beacon.cpp:24
geometry_msgs::PoseWithCovariance_
Definition: pose.h:39
TEST
TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
Definition: test_pose.cpp:49
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
z
GLdouble GLdouble z
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
EXPECT_EQ
#define EXPECT_EQ(val1, val2)
CPoseOrPoint< CPose3D >::getAsVectorVal
mrpt::math::CVectorDouble getAsVectorVal() const
r
GLdouble GLdouble GLdouble r
EXPECT_FLOAT_EQ
#define EXPECT_FLOAT_EQ(val1, val2)
q
GLdouble GLdouble GLdouble GLdouble q
mrpt::poses::CPose3D::setYawPitchRoll
void setYawPitchRoll(const double yaw_, const double pitch_, const double roll_)
check_CPose3D_tofrom_ROS
void check_CPose3D_tofrom_ROS(double x, double y, double z, double yaw, double pitch, double roll)
Definition: test_pose.cpp:126

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Feb 28 2020 03:22:14