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

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Thu May 12 2022 02:26:47