CRobot2DPoseEstimator.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CRobot2DPoseEstimator.h>
#include <mrpt/system/datetime.h>
#include <iostream>
#include <mutex>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace std;

/* --------------------------------------------------------
                Ctor
   -------------------------------------------------------- */
CRobot2DPoseEstimator::CRobot2DPoseEstimator() { reset(); }
/* --------------------------------------------------------
                Dtor
   -------------------------------------------------------- */
CRobot2DPoseEstimator::~CRobot2DPoseEstimator() = default;
/* --------------------------------------------------------
                reset
   -------------------------------------------------------- */
void CRobot2DPoseEstimator::reset()
{
    std::lock_guard<std::mutex> lock(m_cs);

    m_last_loc_time.reset();
    m_last_odo_time.reset();

    m_last_loc = TPose2D(0, 0, 0);
    m_loc_odo_ref = TPose2D(0, 0, 0);
    m_last_odo = TPose2D(0, 0, 0);

    m_robot_vel_local = TTwist2D(.0, .0, .0);
}

/** Updates the filter so the pose is tracked to the current time */
void CRobot2DPoseEstimator::processUpdateNewPoseLocalization(
    const TPose2D& newPose, Clock::time_point cur_tim)
{
    std::lock_guard<std::mutex> lock(m_cs);

    // Overwrite old localization data:
    m_last_loc_time = cur_tim;
    m_last_loc = newPose;

    // And create interpolated odometry data to work as "reference pose":
    if (m_last_odo_time)
    {
        const double dT = timeDifference(m_last_odo_time.value(), cur_tim);
        extrapolateRobotPose(m_last_odo, m_robot_vel_local, dT, m_loc_odo_ref);
    }
}

/** Updates the filter so the pose is tracked to the current time */
void CRobot2DPoseEstimator::processUpdateNewOdometry(
    const TPose2D& newGlobalOdometry, Clock::time_point cur_tim,
    bool hasVelocities, const mrpt::math::TTwist2D& velLocal)
{
    MRPT_START

    std::lock_guard<std::mutex> lock(m_cs);

    if (m_last_odo_time)
    {
        const double dT = timeDifference(m_last_odo_time.value(), cur_tim);
        if (dT <= 0)
            std::cerr << "[CRobot2DPoseEstimator::processUpdateNewOdometry] "
                         "WARNING: Diff. in timestamps between odometry should "
                         "be >0, and it's "
                      << dT << "\n";
    }

    // First, update velocities:
    if (hasVelocities)
    {
        m_robot_vel_local = velLocal;
    }
    else
    {  // Note: JLBC 23/Nov/2016: I have removed an estimation of velocity from
        // increments of odometry
        // because it was really bad. Just don't make up things: if the user
        // doesn't provide us velocities,
        // we don't use velocities.
        m_robot_vel_local = TTwist2D(.0, .0, .0);
    }

    // And now times & odo:
    m_last_odo_time = cur_tim;
    m_last_odo = newGlobalOdometry;

    MRPT_END
}

bool CRobot2DPoseEstimator::getCurrentEstimate(
    mrpt::math::TPose2D& pose, mrpt::math::TTwist2D& velLocal,
    mrpt::math::TTwist2D& velGlobal, mrpt::Clock::time_point tim_query) const
{
    if (!m_last_odo_time || !m_last_loc_time) return false;

    const double dTimeLoc = timeDifference(m_last_loc_time.value(), tim_query);
    if (dTimeLoc > params.max_localiz_age) return false;

    //  Overall estimate:
    // last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
    const TPose2D p =
        (CPose2D(m_last_loc) + (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
            .asTPose();

    // Add the extrapolation:
    const double dTimeOdo = timeDifference(m_last_odo_time.value(), tim_query);
    if (dTimeOdo > params.max_odometry_age) return false;

    extrapolateRobotPose(p, m_robot_vel_local, dTimeOdo, pose);

    // Constant speed model:
    velLocal = m_robot_vel_local;
    velGlobal = velLocal;
    velGlobal.rotate(pose.phi);

    return true;
}

/** get the current estimate
 * \return true is the estimate can be trusted. False if the real observed data
 * is too old.
 */
bool CRobot2DPoseEstimator::getLatestRobotPose(TPose2D& pose) const
{
    if (!m_last_odo_time && !m_last_loc_time)
    {
        pose = TPose2D(0, 0, 0);
        return false;
    }

    bool ret_odo;
    if (m_last_odo_time && m_last_loc_time)
        ret_odo = (m_last_odo_time.value() > m_last_loc_time.value());
    else if (m_last_odo_time)
        ret_odo = true;
    else
        ret_odo = false;

    if (ret_odo)
        pose = (CPose2D(m_last_loc) +
                (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
                   .asTPose();
    else
        pose = m_last_loc;

    return true;
}

// An auxiliary method to extrapolate the pose of a robot located at "p"
//  with velocities (v,w) after a time delay "delta_time".
void CRobot2DPoseEstimator::extrapolateRobotPose(
    const TPose2D& p, const mrpt::math::TTwist2D& velLocal,
    const double delta_time, TPose2D& new_p)
{
    if (velLocal.vx == 0 && velLocal.vy == 0 && velLocal.omega == 0)
    {  // Still
        new_p = p;
    }
    else if (std::abs(velLocal.vy) > 1e-2)
    {  // non-Ackermann-like vehicle: extrapolate as a straight line:
        const TPoint2D dp =
            TPoint2D(delta_time * velLocal.vx, delta_time * velLocal.vy);
        TPoint2D pg;
        CPose2D(p).composePoint(dp, pg);
        new_p.x = pg.x;
        new_p.y = pg.y;
        new_p.phi = p.phi + delta_time * velLocal.omega;
    }
    else
    {
        // vy==0, assume we are in a Ackermann-like steering vehicle: compute an
        // arc:
        const double R = velLocal.vx / velLocal.omega;  // Radius
        const double theta = velLocal.omega * delta_time;  // traversed arc
        const double cc = cos(p.phi);
        const double ss = sin(p.phi);

        const double arc_x = R * sin(theta);
        const double arc_y = R * (1 - cos(theta));

        new_p.x = p.x + cc * arc_x - ss * arc_y;
        new_p.y = p.y + ss * arc_x + cc * arc_y;
        new_p.phi = p.phi + theta;
    }
}

bool CRobot2DPoseEstimator::getLatestRobotPose(CPose2D& pose) const
{
    mrpt::math::TPose2D p;
    bool v = getLatestRobotPose(p);
    if (v)
    {
        pose.x(p.x);
        pose.y(p.y);
        pose.phi(p.phi);
    }
    return v;
}