Make use of kalman filter optional in HumanTracker

source/armarx/navigation/human/HumanFilter.cpp

@@ -3,7 +3,7 @@
 namespace armarx::navigation::human
 {
 
-    HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
+    HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter) : useKalmanFilter(useKalmanFilter)
     {
         //initialize new human for detected human
         human.pose = pose0;
@@ -11,29 +11,31 @@ namespace armarx::navigation::human
         human.detectionTime = detectionTime;
         lastDetectedHuman = 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;
-        Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
+        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>::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>::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;
+            UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+                UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
 
-        //initial position and orientation according to detected human
-        SystemModelT::StateT state0 = toUkfState(pose0);
+            //initial position and orientation according to detected human
+            SystemModelT::StateT state0 = toUkfState(pose0);
 
-        ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+            ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+        }
     }
 
     void
@@ -41,32 +43,45 @@ namespace armarx::navigation::human
     {
         double dt = (detectionTime - human.detectionTime).toSecondsDouble();
 
-        SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
-        ukf->propagation(control, dt);
+        if (useKalmanFilter) {
+            SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
+            ukf->propagation(control, dt);
 
-        human.pose = fromUkfState(ukf->getCurrentState());
-        human.detectionTime = detectionTime;
+            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();
-
-        //update ukf with new observation
-        SystemModelT::ObsT observation = toUkfObs(pose);
-        ukf->update(observation);
-
-        core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
-        newPose.linear() = pose.linear();
+        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 ds = newPose.translation() - lastDetectedHuman.pose.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;
+        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;
+        }
 
         //update stored information about the human
         human.detectionTime = detectionTime;

source/armarx/navigation/human/HumanFilter.h

@@ -59,7 +59,7 @@ namespace armarx::navigation::human
          * @param pose0 the first known pose of the human
          * @param detectionTime the point of detection
          */
-        HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+        HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter);
 
         /**
          * @brief HumanFilter::propagation propagate the system model to the given point in time. This
@@ -105,6 +105,7 @@ namespace armarx::navigation::human
          */
         Human human{};
         std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf = nullptr;
+        bool useKalmanFilter;
     };
 
 } // namespace armarx::navigation::human

source/armarx/navigation/human/HumanTracker.cpp

@@ -39,7 +39,7 @@ namespace armarx::navigation::human
             {
                 //add new tracked human to list of tracked humans
                 trackedHumans.push_back(TrackedHuman{
-                    .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
+                    .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime, parameters.useKalmanFilter},
                     .trackingId = detectedHuman.trackingId,
                     .associatedCluster = nullptr,
                     .associated = true});

source/armarx/navigation/human/HumanTracker.h

@@ -113,6 +113,9 @@ namespace armarx::navigation::human
             // alpha value from interval [0,1] to determine how much the current (and respectively
             // the old) velocity should be weighted when calculating the new velocity
             float velocityAlpha = 0.7;
+
+            // whether to use the kalman filter inside the HumanFilter
+            bool useKalmanFilter = false;
         };
 
         /**