HumanFilter.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
 * @author     Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstdint>
#include <list>
#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/HumanSystemModel.h>
#include <armarx/navigation/human/types.h>
 
namespace armarx::navigation::human
{
 
    /**
     * @brief The HumanFilter class can be used to track and filter the state of a single human. It
     * hides implementation detail on how the filtering is done. New information about the human can
     * be fed by using the update method. The human itself can be obtained using the get method.
     */
    class HumanFilter
    {
 
    public:
        struct Parameters
        {
            //TODO which values are appropriate?
            double control_pos_cov = 0.01;
            double control_rot_cov = 0.01;
            double obs_pos_cov = 0.01;
            double obs_rot_cov = 0.01;
            double initial_state_pos_cov = 0.01;
            double initial_state_rot_cov = 0.01;
            double alpha = 0.5;
 
            float velocityAlpha = 0.15;
 
            unsigned int velocityAverageBinSize = 10;
            unsigned int velocityAverageBinDistance = 5;
            unsigned int positionAverageBinSize = 8;
        };
 
        /**
         * @brief HumanFilter::HumanFilter creates a new filter that filters 2D poses (position and
         * rotation in a two dimensional plane)
         * @param pose0 the first known pose of the human
         * @param detectionTime the point of detection
         */
        HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter);
 
        /**
         * @brief HumanFilter::propagation propagate the system model to the given point in time. This
         * means that the human pose is updated according to the filters prediction for the given point
         * in time. Should be called at most once between every two calls of HumanFilter::update.
         * @param detectionTime the point in time for which the pose should be predicted
         */
        void propagation(const DateTime& detectionTime);
        /**
         * @brief HumanFilter::update update the filter a new detected pose of the tracked human and
         * the detection time
         * @param pose the new detected pose of the human
         * @param detectionTime the detection time
         */
        void update(const core::Pose2D& pose, const DateTime& detectionTime);
        /**
         * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent
         * state
         * @return the human
         */
        const Human& get() const;
 
        Duration detectionAge(const DateTime& currentTime) const;
 
    private:
        using SystemModelT = kalman_filter::SystemModelSE2<double>;
 
        SystemModelT::StateT toUkfState(const core::Pose2D& pose);
        core::Pose2D fromUkfState(const SystemModelT::StateT& pose);
        SystemModelT::ControlT toUkfControl(const Eigen::Vector2f& vel);
        SystemModelT::ObsT toUkfObs(const core::Pose2D& pose);
 
        struct TimedVector2f
        {
            std::int64_t time;
            Eigen::Vector2f pose;
        };
 
        TimedVector2f getAveragedTimedPose(unsigned int start_idx, unsigned int end_idx);
 
    private:
        Parameters params{};
        /**
         * @brief oldHuman stores information about the human at the point in time of the last
         * HumanFilter::update call
         */
        Human lastDetectedHuman{};
        /**
         * @brief human stores information about the human at the point in time of the last
         * HumanFilter::propagation call
         */
        Human human{};
        std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf = nullptr;
        bool useKalmanFilter;
 
        std::list<TimedVector2f> pastPoses;
    };
 
} // namespace armarx::navigation::human