10 #include <gtest/gtest.h> 14 TEST(CRobot2DPoseEstimator, defaultCtor)
26 TEST(CRobot2DPoseEstimator, extrapolateRobotPose)
30 const double dt = 2.0;
35 EXPECT_DOUBLE_EQ(new_p.
x, p.
x + dt * vl.
vx);
36 EXPECT_DOUBLE_EQ(new_p.
y, p.
y + dt * vl.
vy);
48 TEST(CRobot2DPoseEstimator, integrateOdometryAndLocalization)
53 const double dt = 1.0;
54 const auto odo2 = odo1 + vel1 * dt;
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
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.
void reset()
Resets all internal state.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
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)
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:
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) ...
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)
double omega
Angular velocity (rad/s)