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

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sat Apr 28 2018 02:49:17