Main MRPT website > C++ reference for MRPT 1.9.9
CRobot2DPoseEstimator.cpp
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 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPose2D.h>
13 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/math/wrap2pi.h>
16 #include <mutex>
17 #include <iostream>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 
23 using namespace mrpt::system;
24 using namespace std;
25 
26 /* --------------------------------------------------------
27  Ctor
28  -------------------------------------------------------- */
30 /* --------------------------------------------------------
31  Dtor
32  -------------------------------------------------------- */
34 /* --------------------------------------------------------
35  reset
36  -------------------------------------------------------- */
38 {
39  std::lock_guard<std::mutex> lock(m_cs);
40 
41  m_last_loc_time = INVALID_TIMESTAMP;
42  m_last_odo_time = INVALID_TIMESTAMP;
43 
44  m_last_loc = TPose2D(0, 0, 0);
45  m_loc_odo_ref = TPose2D(0, 0, 0);
46  m_last_odo = TPose2D(0, 0, 0);
47 
48  m_robot_vel_local = TTwist2D(.0, .0, .0);
49 }
50 
51 /** Updates the filter so the pose is tracked to the current time */
53  const TPose2D& newPose, TTimeStamp cur_tim)
54 {
55  std::lock_guard<std::mutex> lock(m_cs);
56 
57  // Overwrite old localization data:
58  m_last_loc_time = cur_tim;
59  m_last_loc = newPose;
60 
61  // And create interpolated odometry data to work as "reference pose":
62  if (m_last_odo_time != INVALID_TIMESTAMP)
63  {
64  const double dT = timeDifference(m_last_odo_time, cur_tim);
65  extrapolateRobotPose(m_last_odo, m_robot_vel_local, dT, m_loc_odo_ref);
66  }
67 }
68 
69 /** Updates the filter so the pose is tracked to the current time */
71  const TPose2D& newGlobalOdometry, TTimeStamp cur_tim, bool hasVelocities,
72  const mrpt::math::TTwist2D& velLocal)
73 {
75 
76  std::lock_guard<std::mutex> lock(m_cs);
77 
78  if (m_last_odo_time != INVALID_TIMESTAMP)
79  {
80  const double dT = timeDifference(m_last_odo_time, cur_tim);
81  if (dT <= 0)
82  std::cerr << "[CRobot2DPoseEstimator::processUpdateNewOdometry] "
83  "WARNING: Diff. in timestamps between odometry should "
84  "be >0, and it's "
85  << dT << "\n";
86  }
87 
88  // First, update velocities:
89  if (hasVelocities)
90  {
91  m_robot_vel_local = velLocal;
92  }
93  else
94  { // Note: JLBC 23/Nov/2016: I have removed an estimation of velocity from
95  // increments of odometry
96  // because it was really bad. Just don't make up things: if the user
97  // doesn't provide us velocities,
98  // we don't use velocities.
99  m_robot_vel_local = TTwist2D(.0, .0, .0);
100  }
101 
102  // And now times & odo:
103  m_last_odo_time = cur_tim;
104  m_last_odo = newGlobalOdometry;
105 
106  MRPT_END
107 }
108 
110  mrpt::math::TPose2D& pose, mrpt::math::TTwist2D& velLocal,
111  mrpt::math::TTwist2D& velGlobal, mrpt::system::TTimeStamp tim_query) const
112 {
113  if (m_last_odo_time == INVALID_TIMESTAMP ||
114  m_last_loc_time == INVALID_TIMESTAMP)
115  return false;
116 
117  const double dTimeLoc = timeDifference(m_last_loc_time, tim_query);
118  if (dTimeLoc > params.max_localiz_age) return false;
119 
120  // Overall estimate:
121  // last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
122  const TPose2D p =
123  (CPose2D(m_last_loc) + (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
124  .asTPose();
125 
126  // Add the extrapolation:
127  const double dTimeOdo = timeDifference(m_last_odo_time, tim_query);
128  if (dTimeOdo > params.max_odometry_age) return false;
129 
130  extrapolateRobotPose(p, m_robot_vel_local, dTimeOdo, pose);
131 
132  // Constant speed model:
133  velLocal = m_robot_vel_local;
134  velGlobal = velLocal;
135  velGlobal.rotate(pose.phi);
136 
137  return true;
138 }
139 
140 /** get the current estimate
141 * \return true is the estimate can be trusted. False if the real observed data
142 * is too old.
143 */
145 {
146  if (m_last_odo_time == INVALID_TIMESTAMP &&
147  m_last_loc_time == INVALID_TIMESTAMP)
148  {
149  pose = TPose2D(0, 0, 0);
150  return false;
151  }
152 
153  bool ret_odo;
154  if (m_last_odo_time != INVALID_TIMESTAMP &&
155  m_last_loc_time != INVALID_TIMESTAMP)
156  ret_odo = (m_last_odo_time > m_last_loc_time);
157  else if (m_last_odo_time != INVALID_TIMESTAMP)
158  ret_odo = true;
159  else
160  ret_odo = false;
161 
162  if (ret_odo)
163  pose = (CPose2D(m_last_loc) +
164  (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
165  .asTPose();
166  else
167  pose = m_last_loc;
168 
169  return true;
170 }
171 
172 // An auxiliary method to extrapolate the pose of a robot located at "p"
173 // with velocities (v,w) after a time delay "delta_time".
175  const TPose2D& p, const mrpt::math::TTwist2D& velLocal,
176  const double delta_time, TPose2D& new_p)
177 {
178  if (velLocal.vx == 0 && velLocal.vy == 0 && velLocal.omega == 0)
179  { // Still
180  new_p = p;
181  }
182  else if (std::abs(velLocal.vy) > 1e-2)
183  { // non-Ackermann-like vehicle: extrapolate as a straight line:
184  const TPoint2D dp =
185  TPoint2D(delta_time * velLocal.vx, delta_time * velLocal.vy);
186  TPoint2D pg;
187  CPose2D(p).composePoint(dp, pg);
188  new_p.x = pg.x;
189  new_p.y = pg.y;
190  new_p.phi = p.phi + delta_time * velLocal.omega;
191  }
192  else
193  {
194  // vy==0, assume we are in a Ackermann-like steering vehicle: compute an
195  // arc:
196  const double R = velLocal.vx / velLocal.omega; // Radius
197  const double theta = velLocal.omega * delta_time; // traversed arc
198  const double cc = cos(p.phi);
199  const double ss = sin(p.phi);
200 
201  const double arc_x = R * sin(theta);
202  const double arc_y = R * (1 - cos(theta));
203 
204  new_p.x = p.x + cc * arc_x - ss * arc_y;
205  new_p.y = p.y + ss * arc_x + cc * arc_y;
206  new_p.phi = p.phi + theta;
207  }
208 }
209 
211 {
213  bool v = getLatestRobotPose(p);
214  if (v)
215  {
216  pose.x(p.x);
217  pose.y(p.y);
218  pose.phi(p.phi);
219  }
220  return v;
221 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
#define MRPT_START
Definition: exceptions.h:262
double x
X,Y coordinates.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
double x
X,Y coordinates.
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
STL namespace.
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:175
void reset()
Resets all internal state.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
This base provides a set of functions for maths stuff.
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::system::TTimeStamp tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source...
double vx
Velocity components: X,Y (m/s)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
const float R
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define MRPT_END
Definition: exceptions.h:266
GLboolean reset
Definition: glext.h:3582
Lightweight 2D pose.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:209
static void extrapolateRobotPose(const mrpt::math::TPose2D &p, const mrpt::math::TTwist2D &robot_vel_local, const double delta_time, mrpt::math::TPose2D &new_p)
An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v...
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
double phi
Orientation (rads)
double omega
Angular velocity (rad/s)



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