diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
index 436a9cebd18cdb70d3100edcdc80625de747a6c4..3c25d4ae423e64333a8813f8b22f5b308650e625 100644
--- a/source/armarx/navigation/human/HumanFilter.cpp
+++ b/source/armarx/navigation/human/HumanFilter.cpp
@@ -3,8 +3,117 @@
namespace armarx::navigation::human
{
- HumanFilter::HumanFilter()
+ HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
{
+ //initialize new human for detected human
+ human.pose = pose0;
+ human.linearVelocity = Eigen::Vector2f::Zero();
+ human.detectionTime = detectionTime;
+
+
+ //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();
+ oldPose = human.pose;
+
+ SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
+ ukf->propagation(control, dt);
+
+ human.pose = fromUkfState(ukf->getCurrentState());
+ }
+
+ void
+ HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
+ {
+ double dt = (detectionTime - human.detectionTime).toSecondsDouble();
+
+ SystemModelT::ObsT observation = toUkfObs(pose);
+ ukf->update(observation);
+
+ core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
+
+ // calculate velocity
+ Eigen::Vector2f ds = newPose.translation() - oldPose.translation();
+ Eigen::Vector2f linVelocity = ds / dt;
+ Eigen::Vector2f newVelocity =
+ params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+
+ human.detectionTime = detectionTime;
+ human.pose = newPose;
+ human.linearVelocity = newVelocity;
}
+ const Human&
+ HumanFilter::get() const
+ {
+ return human;
+ }
+
+
+ HumanFilter::SystemModelT::StateT
+ HumanFilter::toUkfState(const core::Pose2D& pose)
+ {
+ // [mm] to [m]
+ return {{pose.translation().x() / 1000,
+ pose.translation().y() / 1000,
+ Eigen::Rotation2Df(pose.linear()).angle()}};
+ }
+
+ core::Pose2D
+ HumanFilter::fromUkfState(const SystemModelT::StateT& pose)
+ {
+ core::Pose2D pose2d;
+ // [m] to [mm]
+ pose2d.translation().x() = pose.pose.x() * 1000;
+ pose2d.translation().y() = pose.pose.y() * 1000;
+ pose2d.linear() = Eigen::Rotation2Df(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
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index 9f4cf3ec4d6c06abb07a80c59d901cfe2a4c4272..14be9eb1298ec57377c1b8d0d0b3878e53bd1bce 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -22,13 +22,52 @@
#pragma once
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <armarx/navigation/human/types.h>
+
namespace armarx::navigation::human
{
class HumanFilter
{
+
public:
- HumanFilter();
+ 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.7;
+ };
+
+ HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+
+ void propagation(const DateTime& detectionTime);
+ void update(const core::Pose2D& pose, const DateTime& detectionTime);
+
+ const Human& get() 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);
+
+ private:
+ Parameters params;
+ core::Pose2D oldPose;
+ Human human;
+ std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf;
};
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 6db5b4b3611e099ae8276ce530e306a87deb0643..aecbfedaf9138ad67d8a577c14c46d4dadc8ee26 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -10,13 +10,6 @@
namespace armarx::navigation::human
{
- //TODO which values are appropriate?
- constexpr T pos_noise_std = 0.01;
- constexpr T rot_noise_std = 0.01;
- constexpr T obs_noise_std = 0.01;
- constexpr T initial_pos_state_cov = 0.01;
- constexpr T initial_rot_state_cov = 0.01;
-
HumanTracker::DetectedHuman
convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
{
@@ -49,7 +42,7 @@ namespace armarx::navigation::human
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- if ((measurements.detectionTime - human.human.detectionTime) >=
+ if ((measurements.detectionTime - human.humanFilter.get().detectionTime) >=
parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
@@ -57,7 +50,7 @@ namespace armarx::navigation::human
else
{
human.associated = false;
- human.human.pose = human.human.estimateAt(measurements.detectionTime);
+ human.humanFilter.propagation(measurements.detectionTime);
it++;
}
}
@@ -76,47 +69,11 @@ namespace armarx::navigation::human
{
if (!detectedHuman.associated)
{
- //initialize new human for detected human
- human::Human human{.pose = detectedHuman.pose,
- .linearVelocity = Eigen::Vector2f::Zero(),
- .detectionTime = detectedHuman.detectionTime};
-
- //initialize new kalman filter for new detected human
-
- UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
- UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
- //rotation noise at (0,0)
- Q.block<1, 1>(0, 0) *= rot_noise_std * rot_noise_std;
- //position noise at (1,1) and (2,2)
- Q.block<2, 2>(1, 1) *= pos_noise_std * pos_noise_std;
-
- //observation noise at (0,0), (1,1) and (2,2)
- UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
- UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std *
- obs_noise_std;
-
- UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
- UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
- P0.block<1, 1>(0, 0) *= initial_rot_state_cov * initial_rot_state_cov;
- P0.block<2, 2>(1, 1) *= initial_pos_state_cov * initial_pos_state_cov;
-
- UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
- UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
-
- //initial position and orientation according to detected human
- SystemModelT::StateT state0;
- //initialize with detected human pose position
- state0.position = human.pose.translation().cast<T>();
- //initialize with SO2 matrix according to detected human pose rotation
- state0.orientation = manif::SO2<T>{Eigen::Rotation2Df(human.pose.linear()).angle()};
-
- UnscentedKalmanFilter<SystemModelT> ukf{Q, R, alpha, state0, P0};
-
//add new tracked human to list of tracked humans
- trackedHumans.push_back(TrackedHuman{.human = human,
- .ukf = ukf,
- .trackingId = detectedHuman.trackingId,
- .associated = true});
+ trackedHumans.push_back(TrackedHuman{
+ .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
+ .trackingId = detectedHuman.trackingId,
+ .associated = true});
}
}
}
@@ -149,7 +106,8 @@ namespace armarx::navigation::human
posDistances.push_back(
{&oldHuman,
&newHuman,
- (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
+ (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
+ .norm()});
}
}
@@ -211,50 +169,7 @@ namespace armarx::navigation::human
trackedHuman->associated = true;
detectedHuman->associated = true;
- //estimate velocity via exponential smoothing
-
- float dt =
- (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
- Eigen::Vector2f ds =
- (detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
- Eigen::Vector2f linVelocity = ds / dt;
-
- Eigen::Vector2f velocity =
- parameters.velocityAlpha * linVelocity +
- (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
-
- float angle_det = Eigen::Rotation2Df(detectedHuman->pose.linear()).angle();
- float angle_track = Eigen::Rotation2Df(trackedHuman->human.pose.linear()).angle();
- //TODO Check angular velocity! Calculation at turning point may be wrong (jump from +pi to -pi)
- float angular_velocity = (angle_det - angle_track) / dt;
-
-
- //estimate pose via ukf
-
- //initialize with estimated velocity (control variables) from tracked human
- UnscentedKalmanFilter<SystemModelT>::ControlT control;
- control.angular_velocity.coeffs() << static_cast<T>(angular_velocity);
- control.euclidean_velocity << velocity.cast<T>();
-
- //propagate control input for time diff
- trackedHuman->ukf.propagation(control, dt);
-
- //initialize observation with detected human data
- UnscentedKalmanFilter<SystemModelT>::ObsT observation;
- observation.segment(0, 1) =
- Eigen::Matrix<T, 1, 1>{Eigen::Rotation2Df(detectedHuman->pose.linear()).angle()};
- observation.segment(1, 2) = detectedHuman->pose.translation().cast<T>();
-
- //update with new observation
- trackedHuman->ukf.update(observation);
-
- //use current state as new pose of tracked human
- UnscentedKalmanFilter<SystemModelT>::StateT state = trackedHuman->ukf.getCurrentState();
- core::Pose2D filteredPose = core::Pose2D::Identity();
- filteredPose.translation() = state.position.cast<float>();
- filteredPose.linear() = Eigen::Rotation2Df(state.orientation.angle()).toRotationMatrix();
-
- trackedHuman->human = {filteredPose, velocity, detectedHuman->detectionTime};
+ trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
trackedHuman->trackingId = detectedHuman->trackingId;
}
@@ -263,7 +178,7 @@ namespace armarx::navigation::human
{
return trackedHumans |
ranges::views::transform([](const TrackedHuman& h) -> human::Human
- { return h.human; }) |
+ { return h.humanFilter.get(); }) |
ranges::to_vector;
}
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 099aca7b508d62794a063bb2b8acc713a6a3caec..3c4cc28f4b45353fc9d1e4fcbc6616c6e9e4bafc 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -29,6 +29,7 @@
#include <VisionX/libraries/armem_human/types.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
#include <armarx/navigation/human/HumanSystemModel.h>
#include <armarx/navigation/human/types.h>
@@ -59,8 +60,7 @@ namespace armarx::navigation::human
struct TrackedHuman
{
- human::Human human;
- UnscentedKalmanFilter<SystemModelT> ukf;
+ HumanFilter humanFilter;
std::optional<std::string> trackingId = std::nullopt;
bool associated;
};