MRPT  1.9.9
CRobot2DPoseEstimator_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 #include <gtest/gtest.h>
12 #include <mrpt/system/datetime.h>
13 
14 TEST(CRobot2DPoseEstimator, defaultCtor)
15 {
17 
19  mrpt::math::TTwist2D vl, vg;
20 
21  bool ok = rpe.getCurrentEstimate(p, vl, vg);
22 
23  EXPECT_FALSE(ok);
24 }
25 
26 TEST(CRobot2DPoseEstimator, extrapolateRobotPose)
27 {
28  mrpt::math::TPose2D p = {0, 0, 0};
29  const mrpt::math::TTwist2D vl = {2.0, 1.0, -0.5};
30  const double dt = 2.0;
31 
32  mrpt::math::TPose2D new_p;
34 
35  EXPECT_DOUBLE_EQ(new_p.x, p.x + dt * vl.vx);
36  EXPECT_DOUBLE_EQ(new_p.y, p.y + dt * vl.vy);
37  EXPECT_DOUBLE_EQ(new_p.phi, p.phi + dt * vl.omega);
38 
39  // now, rotated 90deg:
40  p = mrpt::math::TPose2D{10.0, 20.0, mrpt::DEG2RAD(90.0)};
42 
43  EXPECT_NEAR(new_p.x, p.x - dt * vl.vy, 1e-5);
44  EXPECT_NEAR(new_p.y, p.y + dt * vl.vx, 1e-5);
45  EXPECT_NEAR(new_p.phi, p.phi + dt * vl.omega, 1e-5);
46 }
47 
48 TEST(CRobot2DPoseEstimator, integrateOdometryAndLocalization)
49 {
50  const mrpt::math::TPose2D odo1 = {0, 0, 0};
51  const mrpt::math::TPose2D loc1 = {10.0, 20.0, mrpt::DEG2RAD(90.0)};
52  const mrpt::math::TTwist2D vel1 = {1.0, 0.0, mrpt::DEG2RAD(10.0)};
53  const double dt = 1.0;
54  const auto odo2 = odo1 + vel1 * dt;
55 
57 
58  rpe.params.max_localiz_age = 2 * dt;
59  rpe.params.max_odometry_age = 2 * dt;
60 
61  const auto t0 = mrpt::Clock::now();
62  const auto t1 = mrpt::system::timestampAdd(t0, dt);
63 
64  // t0:
65  rpe.processUpdateNewOdometry(odo1, t0, true, vel1);
67 
69  mrpt::math::TTwist2D vl, vg;
70  bool ok = rpe.getCurrentEstimate(p, vl, vg, t0);
71  EXPECT_TRUE(ok);
72  EXPECT_NEAR(p.x, loc1.x, 1e-5);
73  EXPECT_NEAR(p.y, loc1.y, 1e-5);
74  EXPECT_NEAR(p.phi, loc1.phi, 1e-5);
75 
76  EXPECT_NEAR(vl.vx, vel1.vx, 1e-5);
77  EXPECT_NEAR(vl.vy, vel1.vy, 1e-5);
78  EXPECT_NEAR(vl.omega, vel1.omega, 1e-5);
79 
80  EXPECT_NEAR(vg.vx, -vel1.vy, 1e-5); // 90deg rot
81  EXPECT_NEAR(vg.vy, vel1.vx, 1e-5);
82  EXPECT_NEAR(vg.omega, vel1.omega, 1e-5);
83 
84  // t1:
85  rpe.processUpdateNewOdometry(odo2, t1, true, vel1);
86  ok = rpe.getCurrentEstimate(p, vl, vg, t1);
87  EXPECT_TRUE(ok);
88  EXPECT_NEAR(p.x, loc1.x - dt * vel1.vy, 1e-5);
89  EXPECT_NEAR(p.y, loc1.y + dt * vel1.vx, 1e-5);
90  EXPECT_NEAR(p.phi, loc1.phi + dt * vel1.omega, 1e-5);
91 
92  // reset:
93  rpe.reset();
94  ok = rpe.getCurrentEstimate(p, vl, vg, t1);
95  EXPECT_FALSE(ok);
96 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
double x
X,Y coordinates.
Definition: TPose2D.h:30
double max_odometry_age
To consider data old, in seconds.
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
Definition: Clock.cpp:94
void reset()
Resets all internal state.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
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.
constexpr double DEG2RAD(const double x)
Degrees to radians.
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
TEST(CRobot2DPoseEstimator, defaultCtor)
double max_localiz_age
To consider data old, in seconds.
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:
Lightweight 2D pose.
Definition: TPose2D.h:22
TOptions params
parameters of the filter.
mrpt::system::TTimeStamp timestampAdd(const mrpt::system::TTimeStamp tim, const double num_seconds)
Shifts a timestamp the given amount of seconds (>0: forwards in time, <0: backwards) ...
Definition: datetime.h:146
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
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 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020