HumanFilter.cpp

Go to the documentation of this file.

#include "HumanFilter.h"
 
#include <cstdint>
#include <memory>
 
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
 
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/types.h>
 
namespace armarx::navigation::human
{
 
    HumanFilter::HumanFilter(const core::Pose2D& pose0,
                             const DateTime& detectionTime,
                             bool useKalmanFilter) :
        useKalmanFilter(useKalmanFilter)
    {
        //initialize new human for detected human
        human.pose = pose0;
        human.linearVelocity = Eigen::Vector2f::Zero();
        human.detectionTime = detectionTime;
        lastDetectedHuman = human;
 
        if (useKalmanFilter)
        {
            //initialize new kalman filter for new detected human
            UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
                UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
            Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
            Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
 
            UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
                UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
            R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
            R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
 
            UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
                UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
            P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
            P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
 
            UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
                UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
 
            //initial position and orientation according to detected human
            SystemModelT::StateT state0 = toUkfState(pose0);
 
            ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
        }
    }
 
    void
    HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime)
    {
        double dt = (detectionTime - human.detectionTime).toSecondsDouble();
 
        if (useKalmanFilter)
        {
            SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
            ukf->propagation(control, dt);
 
            human.pose = fromUkfState(ukf->getCurrentState());
            human.detectionTime = detectionTime;
        }
        else
        {
            human.pose = human.estimateAt(detectionTime);
            human.detectionTime = detectionTime;
        }
    }
 
    void
    HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
    {
        core::Pose2D newPose;
 
        if (useKalmanFilter)
        {
            //update ukf with new observation
            SystemModelT::ObsT observation = toUkfObs(pose);
            ukf->update(observation);
 
            newPose = fromUkfState(ukf->getCurrentState());
            newPose.linear() = pose.linear();
        }
        else
        {
            newPose = pose;
        }
 
        // calculate velocity
        Eigen::Vector2f newVelocity = human.linearVelocity;
        if (pastPoses.size() >= (params.velocityAverageBinDistance + params.velocityAverageBinSize))
        {
            TimedVector2f p1 = getAveragedTimedPose(0, params.velocityAverageBinSize);
            TimedVector2f p2 = getAveragedTimedPose(params.velocityAverageBinDistance,
                                                    params.velocityAverageBinDistance +
                                                        params.velocityAverageBinSize);
            Eigen::Vector2f ds = p2.pose - p1.pose;
            double dt = (p2.time * 1000.0) / (p1.time * 1000.0);
            Eigen::Vector2f linVelocity = ds / dt;
            if (linVelocity.norm() > 1000)
            {
                linVelocity = linVelocity.normalized();
                linVelocity *= 1000;
            }
            newVelocity = linVelocity;
        }
        else
        {
            newVelocity = Eigen::Vector2f::Zero();
        }
 
        // update newPose with average over last Poses
        if (pastPoses.size() >= params.positionAverageBinSize)
        {
            TimedVector2f p = getAveragedTimedPose(pastPoses.size() - params.positionAverageBinSize,
                                                   params.positionAverageBinSize);
            newPose = core::Pose2D{Eigen::Translation2f{p.pose}};
        }
 
        // fix of Temporary fix
        // newVelocity.setZero();
 
        //update stored information about the human
        human.detectionTime = detectionTime;
        human.pose = newPose;
        human.linearVelocity = newVelocity;
 
        pastPoses.emplace_back(TimedVector2f{detectionTime.toMilliSecondsSinceEpoch(),
                                             Eigen::Vector2f{pose.translation()}});
        if (pastPoses.size() > (params.velocityAverageBinDistance + params.velocityAverageBinSize))
        {
            pastPoses.pop_front();
        }
 
        lastDetectedHuman = human;
    }
 
    const Human&
    HumanFilter::get() const
    {
        return human;
    }
 
    Duration
    HumanFilter::detectionAge(const DateTime& currentTime) const
    {
        return (currentTime - lastDetectedHuman.detectionTime);
    }
 
    HumanFilter::SystemModelT::StateT
    HumanFilter::toUkfState(const core::Pose2D& pose)
    {
        // [mm] to [m]
        return {{pose.translation().x() / 1000,
                 pose.translation().y() / 1000,
                 0 /*Eigen::Rotation2Df(pose.linear()).angle()*/}};
    }
 
    core::Pose2D
    HumanFilter::fromUkfState(const SystemModelT::StateT& pose)
    {
        core::Pose2D pose2d = core::Pose2D::Identity();
        // [m] to [mm]
        pose2d.translation().x() = pose.pose.x() * 1000;
        pose2d.translation().y() = pose.pose.y() * 1000;
        pose2d.linear() = Eigen::Rotation2Df(0 /*pose.pose.angle()*/).toRotationMatrix();
 
        return pose2d;
    }
 
    HumanFilter::SystemModelT::ControlT
    HumanFilter::toUkfControl(const Eigen::Vector2f& vel)
    {
        // convert velocity from global frame to human frame (required by kalman filter)
        const auto& global_R_human = human.pose.linear();
        const auto human_V_vel = global_R_human.inverse() * vel;
 
        // [mm] to [m]
        return {{human_V_vel.x() / 1000, human_V_vel.y() / 1000, 0}};
    }
 
    HumanFilter::SystemModelT::ObsT
    HumanFilter::toUkfObs(const core::Pose2D& pose)
    {
        return SystemModelT::observationFunction(toUkfState(pose));
    }
 
    HumanFilter::TimedVector2f
    HumanFilter::getAveragedTimedPose(unsigned int start_idx, unsigned int end_idx)
    {
        std::int64_t avgTime = 0;
        Eigen::Vector2f avgPose = Eigen::Vector2f::Zero();
        auto it = pastPoses.begin();
        for (std::int64_t i = 0; i < start_idx; ++i)
        {
            ++it; // advance iterater to correct window
        }
        const auto numItems = end_idx - start_idx;
        for (std::int64_t i = start_idx; i < end_idx; i++)
        {
            const auto& [time, pose] = *it;
 
            avgTime += time / numItems;
            avgPose += pose;
 
            ++it;
        }
        avgPose /= numItems;
        return {avgTime, avgPose};
    }
 
 
} // namespace armarx::navigation::human