MRPT  1.9.9
pose_utils.h
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 details in https://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+
9  */
10 
11 #include <mrpt/core/exceptions.h>
14 #include <mrpt/poses/CPose2D.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/poses/CPose3DQuat.h>
17 #include <mrpt/system/datetime.h>
18 #include <mrpt/system/filesystem.h>
19 #include <mrpt/system/os.h>
21 
22 #include <string>
23 #include <vector>
24 
26 {
27 inline void getPoseFromString(const std::string& s, mrpt::poses::CPose2D& p)
28 {
29  using namespace std;
30  using namespace mrpt::system;
31  using namespace mrpt::poses;
32 
33  p.fromStringRaw(s);
34 
35 } // end of getPoseFromString
36 
37 /** Set the current object value from a string (e.g.: "x y z qx qy qz qw")
38  *
39  * Specialization for files in the TUM format
40  * \exception std::exception On invalid format
41  */
42 template <bool QUAT_REPR = true, bool TUM_FORMAT = true>
43 void getPoseFromString(const std::string& s, mrpt::poses::CPose3D& p)
44 {
45  std::vector<std::string> curr_tokens;
46  mrpt::system::tokenize(s, " ", curr_tokens);
47 
48  ASSERTMSG_(
49  curr_tokens.size() == 7,
51  "Invalid number of tokens in given string\n"
52  "\tExpected: 7\n"
53  "\tTokens size: %" USIZE_STR "\n",
54  curr_tokens.size()));
55 
57  quat.r(atof(curr_tokens[6].c_str()));
58  quat.x(atof(curr_tokens[3].c_str()));
59  quat.y(atof(curr_tokens[4].c_str()));
60  quat.z(atof(curr_tokens[5].c_str()));
61  double roll, pitch, yaw;
62  quat.rpy(roll, pitch, yaw);
63 
64  p.setFromValues(
65  atof(curr_tokens[0].c_str()), // x
66  atof(curr_tokens[1].c_str()), // y
67  atof(curr_tokens[2].c_str()), // z
68  roll, pitch, yaw);
69 } // end of fromStringQuat
70 
71 template <>
72 inline void getPoseFromString</*QUAT_REPR=*/false, /*TUM_FORMAT=*/false>(
73  const std::string& s, mrpt::poses::CPose3D& p)
74 {
75  p.fromStringRaw(s);
76 } // end of getPoseFromString
77 
78 /**Invalid form. TUM ground truth files are always in Quaternion form. */
79 template <>
81  const std::string& s, mrpt::poses::CPose3D& p)
82 {
83  THROW_EXCEPTION("Invalid combination: QUAT_REPR=false, TUM_FORMAT=true");
84 }
85 
86 /**\brief Specialization for strings in Quaternion form */
87 template <>
88 inline void getPoseFromString</*QUAT_REPR=*/true, /*TUM_FORMAT=*/false>(
89  const std::string& s, mrpt::poses::CPose3D& p)
90 {
92  p_quat.fromStringRaw(s);
93  p = mrpt::poses::CPose3D(p_quat);
94 } // end of getPoseFromString
95 
96 } // namespace mrpt::poses::internal
97 
98 /**\name Parsing of textfiles with poses */
99 /**\{*/
100 /**\brief Parse the textfile and fill in the corresponding \a poses vector.
101  *
102  * The file to be parsed is to contain 2D or 3D poses along with their
103  * corresponding timestamps, one line for each.
104  *
105  * The expected format is the following:
106  *
107  * For 2D Poses: timestamp x y theta (in rad)
108  * For 3D Poses in RPY form : x y z yaw pitch roll
109  * For 3D Poses in Quaternion form : x y z qw qx qy qz
110  * For 3D Poses in Quaternion form [TUM Datasets] : x y z qx qy qz qw
111  *
112  * The 2D format abides to the groundtruth file format used by the
113  * <em>GridMapNavSimul</em> application
114  *
115  * The TUM format is compatible with the groundtruth format for the
116  * TUM RGBD datasets as generated by the * <em>rgbd_dataset2rawlog</em> MRPT
117  * tool.
118  *
119  * \param[in] fname Filename from which the timestamps and poses are read
120  * \param[out] poses_vec std::vector which is to contain the 2D poses.
121  * \param[out] timestamps std::vector which is to contain the timestamps
122  * for the corresponding ground truth poses. Ignore this argument if
123  * timestamps are not needed.
124  * \param[in] substract_init_offset If true, the filled poses are to start from
125  * 0, that means, that if the first found pose is non-zero, it's going to be
126  * considered and offset and substracted from all poses in the file.
127  *
128  * \sa
129  * https://www.mrpt.org/Collection_of_Kinect_RGBD_datasets_with_ground_truth_CVPR_TUM_2011
130  *
131  * \ingroup mrpt_base_grp
132  */
133 template <class POSE_T>
135  const std::string& fname, std::vector<POSE_T>* poses_vec,
136  std::vector<mrpt::system::TTimeStamp>* timestamps = NULL,
137  bool substract_init_offset = false)
138 {
139  MRPT_START
140 
141  using namespace std;
142  using namespace mrpt::system;
143  using namespace mrpt::poses;
144  using namespace internal;
145 
146  // make sure file exists
147  ASSERTMSG_(
148  fileExists(fname), format(
149  "\nFile %s was not found.\n"
150  "Either specify a valid filename or set set the "
151  "m_visualize_GT flag to false\n",
152  fname.c_str()));
153 
154  CFileInputStream file_GT(fname);
155  ASSERTMSG_(
156  file_GT.fileOpenCorrectly(),
157  "\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
158  ASSERTMSG_(poses_vec, "std::vector<POSE_T>* is not valid.");
159 
160  string curr_line;
161 
162  // move to the first non-commented line
163  for (size_t i = 0; file_GT.readLine(curr_line); i++)
164  {
165  if (curr_line.at(0) != '#')
166  {
167  break;
168  }
169  }
170 
171  // handle the first pose as an offset
172  POSE_T pose_offset_opposite;
173  if (substract_init_offset)
174  {
175  POSE_T pose_offset;
177  std::string(
178  curr_line.begin() + curr_line.find_first_of(" \t") + 1,
179  curr_line.end()),
180  pose_offset);
181  pose_offset_opposite = pose_offset.getOppositeScalar();
182  }
183 
184  // parse the file - get timestamp and pose and fill in the vector
185  for (; file_GT.readLine(curr_line);)
186  {
187  // timestamp
188  if (timestamps)
189  {
190  std::string timestamp_str = std::string(
191  curr_line.begin(),
192  curr_line.begin() + curr_line.find_first_of(" \t"));
193  timestamps->push_back(atof(timestamp_str.c_str()));
194  }
195 
196  POSE_T curr_pose;
198  std::string(
199  curr_line.begin() + curr_line.find_first_of(" \t") + 1,
200  curr_line.end()),
201  curr_pose);
202 
203  // scalar substraction of initial offset
204  if (substract_init_offset)
205  {
206  curr_pose.addComponents(pose_offset_opposite);
207  }
208 
209  // push the newly created pose
210  poses_vec->push_back(curr_pose);
211  } // end for loop
212 
213  file_GT.close();
214 
215  MRPT_END
216 }
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
STL namespace.
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose2D.cpp:420
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
void readFileWithPoses(const std::string &fname, std::vector< POSE_T > *poses_vec, std::vector< mrpt::system::TTimeStamp > *timestamps=NULL, bool substract_init_offset=false)
Parse the textfile and fill in the corresponding poses vector.
Definition: pose_utils.h:134
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::io::CFileInputStream CFileInputStream
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void getPoseFromString< false, true >(const std::string &s, mrpt::poses::CPose3D &p)
Invalid form.
Definition: pose_utils.h:80
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
#define MRPT_END
Definition: exceptions.h:245
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
void getPoseFromString(const std::string &s, mrpt::poses::CPose2D &p)
Definition: pose_utils.h:27
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
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:433



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020