diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp index 3c25d4ae423e64333a8813f8b22f5b308650e625..e45d942ad4a123c50fff874c394f30a92dad2bad 100644 --- a/source/armarx/navigation/human/HumanFilter.cpp +++ b/source/armarx/navigation/human/HumanFilter.cpp @@ -3,6 +3,12 @@ namespace armarx::navigation::human { + /** + * @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::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime) { //initialize new human for detected human @@ -12,7 +18,6 @@ namespace armarx::navigation::human //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; @@ -37,6 +42,12 @@ namespace armarx::navigation::human ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0); } + /** + * @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 + * @param detectionTime the point in time for which the pose should be predicted + */ void HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime) { @@ -49,11 +60,18 @@ namespace armarx::navigation::human human.pose = fromUkfState(ukf->getCurrentState()); } + /** + * @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 HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime) { double dt = (detectionTime - human.detectionTime).toSecondsDouble(); + //update ukf with new observation SystemModelT::ObsT observation = toUkfObs(pose); ukf->update(observation); @@ -62,14 +80,22 @@ namespace armarx::navigation::human // calculate velocity Eigen::Vector2f ds = newPose.translation() - oldPose.translation(); Eigen::Vector2f linVelocity = ds / dt; + // apply exponential smoothing to velocity + //TODO try other approaches? Eigen::Vector2f newVelocity = params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity; + //update stored information about the human human.detectionTime = detectionTime; human.pose = newPose; human.linearVelocity = newVelocity; } + /** + * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent + * state + * @return the human + */ const Human& HumanFilter::get() const {