HumanFilter.cpp

Go to the documentation of this file.

#include "HumanFilter.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)
    {
        double dt = (detectionTime - lastDetectedHuman.detectionTime).toSecondsDouble();
        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 (dt > 0) {
            Eigen::Vector2f ds = newPose.translation() - lastDetectedHuman.pose.translation();
            Eigen::Vector2f linVelocity = ds / dt;
            // apply exponential smoothing to velocity
            //TODO try other approaches?
            newVelocity = params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
        }
 
        // Temporary fix
        newVelocity.setZero();
 
        //update stored information about the human
        human.detectionTime = detectionTime;
        human.pose = newPose;
        human.linearVelocity = newVelocity;
 
        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));
    }
 
 
} // namespace armarx::navigation::human