MRPT  2.0.1
CRobot2DPoseEstimator.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 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/wrap2pi.h>
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/system/datetime.h>
17 #include <iostream>
18 #include <mutex>
19 
20 using namespace mrpt;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
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.reset();
42  m_last_odo_time.reset();
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, Clock::time_point 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)
63  {
64  const double dT = timeDifference(m_last_odo_time.value(), 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, Clock::time_point cur_tim,
72  bool hasVelocities, const mrpt::math::TTwist2D& velLocal)
73 {
75 
76  std::lock_guard<std::mutex> lock(m_cs);
77 
78  if (m_last_odo_time)
79  {
80  const double dT = timeDifference(m_last_odo_time.value(), 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::Clock::time_point tim_query) const
112 {
113  if (!m_last_odo_time || !m_last_loc_time) return false;
114 
115  const double dTimeLoc = timeDifference(m_last_loc_time.value(), tim_query);
116  if (dTimeLoc > params.max_localiz_age) return false;
117 
118  // Overall estimate:
119  // last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
120  const TPose2D p =
121  (CPose2D(m_last_loc) + (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
122  .asTPose();
123 
124  // Add the extrapolation:
125  const double dTimeOdo = timeDifference(m_last_odo_time.value(), tim_query);
126  if (dTimeOdo > params.max_odometry_age) return false;
127 
128  extrapolateRobotPose(p, m_robot_vel_local, dTimeOdo, pose);
129 
130  // Constant speed model:
131  velLocal = m_robot_vel_local;
132  velGlobal = velLocal;
133  velGlobal.rotate(pose.phi);
134 
135  return true;
136 }
137 
138 /** get the current estimate
139  * \return true is the estimate can be trusted. False if the real observed data
140  * is too old.
141  */
143 {
144  if (!m_last_odo_time && !m_last_loc_time)
145  {
146  pose = TPose2D(0, 0, 0);
147  return false;
148  }
149 
150  bool ret_odo;
151  if (m_last_odo_time && m_last_loc_time)
152  ret_odo = (m_last_odo_time.value() > m_last_loc_time.value());
153  else if (m_last_odo_time)
154  ret_odo = true;
155  else
156  ret_odo = false;
157 
158  if (ret_odo)
159  pose = (CPose2D(m_last_loc) +
160  (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
161  .asTPose();
162  else
163  pose = m_last_loc;
164 
165  return true;
166 }
167 
168 // An auxiliary method to extrapolate the pose of a robot located at "p"
169 // with velocities (v,w) after a time delay "delta_time".
171  const TPose2D& p, const mrpt::math::TTwist2D& velLocal,
172  const double delta_time, TPose2D& new_p)
173 {
174  if (velLocal.vx == 0 && velLocal.vy == 0 && velLocal.omega == 0)
175  { // Still
176  new_p = p;
177  }
178  else if (std::abs(velLocal.vy) > 1e-2)
179  { // non-Ackermann-like vehicle: extrapolate as a straight line:
180  const TPoint2D dp =
181  TPoint2D(delta_time * velLocal.vx, delta_time * velLocal.vy);
182  TPoint2D pg;
183  CPose2D(p).composePoint(dp, pg);
184  new_p.x = pg.x;
185  new_p.y = pg.y;
186  new_p.phi = p.phi + delta_time * velLocal.omega;
187  }
188  else
189  {
190  // vy==0, assume we are in a Ackermann-like steering vehicle: compute an
191  // arc:
192  const double R = velLocal.vx / velLocal.omega; // Radius
193  const double theta = velLocal.omega * delta_time; // traversed arc
194  const double cc = cos(p.phi);
195  const double ss = sin(p.phi);
196 
197  const double arc_x = R * sin(theta);
198  const double arc_y = R * (1 - cos(theta));
199 
200  new_p.x = p.x + cc * arc_x - ss * arc_y;
201  new_p.y = p.y + ss * arc_x + cc * arc_y;
202  new_p.phi = p.phi + theta;
203  }
204 }
205 
207 {
209  bool v = getLatestRobotPose(p);
210  if (v)
211  {
212  pose.x(p.x);
213  pose.y(p.y);
214  pose.phi(p.phi);
215  }
216  return v;
217 }
#define MRPT_START
Definition: exceptions.h:241
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
double x
X,Y coordinates.
Definition: TPose2D.h:30
T x
X,Y coordinates.
Definition: TPoint2D.h:25
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
Definition: Twist2D.cpp:40
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
mrpt::vision::TStereoCalibParams params
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:199
void reset()
Resets all internal state.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
This base provides a set of functions for maths stuff.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::Clock::time_point cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::Clock::time_point tim_query=mrpt::Clock::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
virtual ~CRobot2DPoseEstimator()
Destructor.
#define MRPT_END
Definition: exceptions.h:245
Lightweight 2D pose.
Definition: TPose2D.h:22
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.h:123
static void extrapolateRobotPose(const mrpt::math::TPose2D &p, const mrpt::math::TTwist2D &robot_vel_local, const double delta_time, mrpt::math::TPose2D &new_p)
Auxiliary static method to extrapolate the pose of a robot located at "p" with velocities (v...
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::Clock::time_point tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source...
double phi
Orientation (rads)
Definition: TPose2D.h:32
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28



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