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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019