MRPT  2.0.1
pose_unittest.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 |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*
11  * test_pose.cpp
12  *
13  * Created on: Mar 15, 2012
14  * Author: Pablo IƱigo Blasco
15  */
16 
17 #include <geometry_msgs/Pose.h>
18 #include <geometry_msgs/PoseWithCovariance.h>
19 #include <geometry_msgs/Quaternion.h>
20 #include <gtest/gtest.h>
21 #include <mrpt/math/CQuaternion.h>
24 #include <mrpt/ros1bridge/pose.h>
25 
26 #include <Eigen/Dense>
27 
28 using namespace std;
29 
30 TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
31 {
32  tf2::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8);
34 
35  for (int r = 0; r < 3; r++)
36  for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]);
37 }
38 TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3)
39 {
41  for (int r = 0, i = 0; r < 3; r++)
42  for (int c = 0; c < 3; c++, i++) src(r, c) = i;
43 
44  const tf2::Matrix3x3 des = mrpt::ros1bridge::toROS(src);
45 
46  for (int r = 0; r < 3; r++)
47  for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c));
48 }
49 
50 // Declare a test
51 TEST(PoseConversions, reference_frame_change_with_rotations)
52 {
53  geometry_msgs::PoseWithCovariance ros_msg_original_pose;
54 
55  ros_msg_original_pose.pose.position.x = 1;
56  ros_msg_original_pose.pose.position.y = 0;
57  ros_msg_original_pose.pose.position.z = 0;
58  ros_msg_original_pose.pose.orientation.x = 0;
59  ros_msg_original_pose.pose.orientation.y = 0;
60  ros_msg_original_pose.pose.orientation.z = 0;
61  ros_msg_original_pose.pose.orientation.w = 1;
62 
63  // to mrpt
64  mrpt::poses::CPose3DPDFGaussian mrpt_original_pose =
65  mrpt::ros1bridge::fromROS(ros_msg_original_pose);
66 
67  EXPECT_EQ(
68  ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]);
69 
70 #if 0
71  // to tf
72  tf::Pose tf_original_pose;
73  tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose);
74 
75  // rotate yaw pi in MRPT
76  mrpt::poses::CPose3D rotation_mrpt;
77  double yaw = M_PI / 2.0;
78  rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0);
79  mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean;
80  EXPECT_NEAR(mrpt_result[1], 1.0, 0.01);
81 
82  // rotate yaw pi in TF
83  tf::Quaternion rotation_tf;
84  rotation_tf.setRPY(0, 0, yaw);
85  tf::Pose rotation_pose_tf;
86  rotation_pose_tf.setIdentity();
87  rotation_pose_tf.setRotation(rotation_tf);
88  tf::Pose tf_result = rotation_pose_tf * tf_original_pose;
89  EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01);
90 
91  geometry_msgs::Pose mrpt_ros_result;
92  mrpt_bridge::convert(mrpt_result, mrpt_ros_result);
93 
94  EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01);
95  EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01);
96  EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01);
97 #endif
98 }
99 
101  double x, double y, double z, double yaw, double pitch, double roll)
102 {
103  const mrpt::poses::CPose3D p3D(x, y, z, yaw, pitch, roll);
104 
105  // Convert MRPT->ROS
106  geometry_msgs::Pose ros_p3D = mrpt::ros1bridge::toROS_Pose(p3D);
107 
108  // Compare ROS quat vs. MRPT quat:
110  p3D.getAsQuaternion(q);
111 
112  EXPECT_NEAR(ros_p3D.position.x, p3D.x(), 1e-4) << "p: " << p3D << endl;
113  EXPECT_NEAR(ros_p3D.position.y, p3D.y(), 1e-4) << "p: " << p3D << endl;
114  EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) << "p: " << p3D << endl;
115 
116  EXPECT_NEAR(ros_p3D.orientation.x, q.x(), 1e-4) << "p: " << p3D << endl;
117  EXPECT_NEAR(ros_p3D.orientation.y, q.y(), 1e-4) << "p: " << p3D << endl;
118  EXPECT_NEAR(ros_p3D.orientation.z, q.z(), 1e-4) << "p: " << p3D << endl;
119  EXPECT_NEAR(ros_p3D.orientation.w, q.r(), 1e-4) << "p: " << p3D << endl;
120 
121  // Test the other path: ROS->MRPT
123 
124  // p_bis==p3D?
125  EXPECT_NEAR(
126  (p_bis.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0,
127  1e-4)
128  << "p_bis: " << p_bis << endl
129  << "p3D: " << p3D << endl;
130 }
131 
132 // Declare a test
134 {
135  using mrpt::DEG2RAD;
136  using mrpt::RAD2DEG;
137  using namespace mrpt; // for 0.0_deg
138 
139  check_CPose3D_tofrom_ROS(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
140  check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg);
141 
142  check_CPose3D_tofrom_ROS(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg);
143  check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg);
144  check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg);
145 
146  check_CPose3D_tofrom_ROS(1, 2, 3, -5.0_deg, 15.0_deg, -30.0_deg);
147 }
148 
149 // Declare a test
150 TEST(PoseConversions, check_CPose2D_to_ROS)
151 {
152  const mrpt::poses::CPose2D p2D(1, 2, 0.56);
153 
154  // Convert MRPT->ROS
155  const geometry_msgs::Pose ros_p2D = mrpt::ros1bridge::toROS_Pose(p2D);
156 
157  // Compare vs. 3D pose:
159  const mrpt::poses::CPose3D p3D_ros = mrpt::ros1bridge::fromROS(ros_p2D);
160 
161  // p3D_ros should equal p3D
162  EXPECT_NEAR(
163  (p3D_ros.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0,
164  1e-4)
165  << "p3D_ros: " << p3D_ros << endl
166  << "p3D: " << p3D << endl;
167 }
CPose3D mean
The mean value.
void check_CPose3D_tofrom_ROS(double x, double y, double z, double yaw, double pitch, double roll)
TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
STL namespace.
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
constexpr double DEG2RAD(const double x)
Degrees to radians.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
bool convert(const sensor_msgs::LaserScan &msg, const mrpt::poses::CPose3D &pose, mrpt::obs::CObservation2DRangeScan &obj)
Definition: laser_scan.cpp:20
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
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
constexpr double RAD2DEG(const double x)
Radians to degrees.
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose2D &src)
Definition: pose.cpp:54
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020