Restructure human tracker test

source/armarx/navigation/human/test/human_tracker_test.cpp

@@ -45,6 +45,7 @@ using CamMm = armarx::navigation::human::HumanTracker::CameraMeasurement;
 using LaserMm = armarx::navigation::human::HumanTracker::LaserMeasurement;
 using Eigen::Vector2f;
 using Ellipsoid = armarx::armem::vision::Ellipsoid;
+using Circle = armarx::armem::vision::Circle;
 
 namespace armarx::navigation::human
 {
@@ -56,48 +57,50 @@ namespace armarx::navigation::human
                Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());
     }
 
-    BOOST_AUTO_TEST_CASE(testLaserTrackingTwoFeetCluster)
+    /**
+     * @brief generateCamMeasurements generates a series of linear camera measurements of a moving
+     * human's head. It will start at startPosition and linearly move to endPosition over the course of
+     * the given timespan, giving a vector with the given amount of steps inbetween.
+     *
+     * @param startPosition the 3D position of the human head at the first measurement
+     * @param endPosition the 3D position of the human head at the last measurement
+     * @param timespanMs the timespan in milliseconds over which the simulated human will move from start to end
+     * @param steps how many measurements will be made (incl. start and end measurement)
+     * @return a vector of <steps> measurements spread equally with regard to position and timespan
+     */
+    std::vector<CamMm>
+    generateCamMeasurements(const Eigen::Vector3f& startPosition,
+                            const Eigen::Vector3f& endPosition,
+                            const Eigen::Quaternionf& orientation,
+                            const int timespanMs,
+                            const int steps)
     {
-        HumanTracker tracker = HumanTracker();
-
-        // PARAMETERS
-
-        const Eigen::Vector3f initialPosition(0, 0, 2);
-        const float orientation = M_PI_2;
-        const Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
-
-        const std::int64_t timestepMs = 100;
-        const Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
-        const int cameraSteps = 10;
+        const double timestepMs = static_cast<double>(timespanMs) / steps;
+        const Eigen::Vector3f posDelta = (endPosition - startPosition) / steps;
 
-        // CAMERA MEASUREMENTS
-
-        const Eigen::Vector3f posDelta =
-            movementSpeedMetPerSec * (static_cast<float>(timestepMs) / 1000.0);
-
-        const std::vector<CamMm> camMeasurements = std::vector<CamMm>();
-        for (int i = 0; i < cameraSteps; i++)
+        std::vector<CamMm> measurements = std::vector<CamMm>();
+        for (int i = 0; i <= steps; i++)
         {
             const DateTime t = DateTime(Duration::MilliSeconds(i * timestepMs));
 
-            const Eigen::Vector3f newPos = initialPosition + i * posDelta;
+            const Eigen::Vector3f newPos = startPosition + i * posDelta;
             const FramedPosition headPosition = FramedPosition(newPos, "", "");
-            const FramedOrientation headOrientation = FramedOrientation(orientationQuat, "", "");
+            const FramedOrientation headOrientation = FramedOrientation(orientation, "", "");
             const PoseKeypoint head = {.label = "HEAD",
                                        .confidence = 0.95,
                                        .positionGlobal = headPosition,
                                        .orientationGlobal = headOrientation};
-            const HumanPose pose = {"posemodelid", {{"HEAD", head}}};
+            const HumanPose pose = {"posemodelid", {{"DONT_USE_ME", {}}, {"HEAD", head}}};
             const std::vector<armem::human::HumanPose> humanPoses = {pose};
             const CamMm camMm = {t, humanPoses};
-
-            tracker.update(camMm);
+            measurements.emplace_back(camMm);
         }
+        return measurements;
+    }
 
-        // LASER MEASUREMENT
-
-        const DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
-
+    LaserMm
+    generateLaserMeasurement(const Eigen::Vector2f& pos, const DateTime& tLaser)
+    {
         using Point = Eigen::Vector2f;
 
         const std::vector<Point> relOffsets = {Point(0.01, 0.003),
@@ -106,17 +109,16 @@ namespace armarx::navigation::human
                                                Point(0.015, 0.009),
                                                Point(0.002, 0.001)};
 
-        const Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
-        const Eigen::Vector3f leftFootPos = laserPos + Eigen::Vector3f(0, 0.1, 0);
-        const Eigen::Vector3f rightFootPos = laserPos + Eigen::Vector3f(0, -0.1, 0);
+        const Eigen::Vector2f leftFootPos = pos + Eigen::Vector3f(0, 0.1);
+        const Eigen::Vector2f rightFootPos = pos + Eigen::Vector3f(0, -0.1);
 
         std::vector<Point> leftFootPoints = std::vector<Point>();
         std::vector<Point> rightFootPoints = std::vector<Point>();
         int i = 0;
         for (const Point& offset : relOffsets)
         {
-            leftFootPoints.emplace_back(leftFootPos.segment(0, 2) + offset);
-            rightFootPoints.emplace_back(rightFootPos.segment(0, 2) + offset);
+            leftFootPoints.emplace_back(leftFootPos + offset);
+            rightFootPoints.emplace_back(rightFootPos + offset);
             i++;
         }
 
@@ -132,13 +134,56 @@ namespace armarx::navigation::human
         const Ellipsoid leftFootEllipse = {.pose = leftFootPose, .radii = Point(0.05, 0.04)};
         const Ellipsoid rightFootEllipse = {.pose = rightFootPose, .radii = Point(0.05, 0.04)};
 
-        const Cluster leftFootCluster = {.ellipsoid = leftFootEllipse, .points = leftFootPoints};
-        const Cluster rightFootCluster = {.ellipsoid = rightFootEllipse, .points = rightFootPoints};
+        const Circle leftFootCircle = {.center = leftFootPos, .radius = 0.1};
+        const Circle rightFootCircle = {.center = rightFootPos, .radius = 0.1};
+
+        const Cluster leftFootCluster = {
+            .circle = leftFootCircle, .ellipsoid = leftFootEllipse, .points = leftFootPoints};
+        const Cluster rightFootCluster = {
+            .circle = rightFootCircle, .ellipsoid = rightFootEllipse, .points = rightFootPoints};
 
         const std::vector<Cluster> clusters =
             std::vector<Cluster>{leftFootCluster, rightFootCluster};
 
         const LaserMm laserMm = {.detectionTime = tLaser, .clusters = clusters};
+        return laserMm;
+    }
+
+    BOOST_AUTO_TEST_CASE(testLaserTrackingTwoFeetCluster)
+    {
+        HumanTracker tracker = HumanTracker();
+
+        // PARAMETERS
+
+        const Eigen::Vector2f initialPosition(0, 0, 2);
+        const float orientation = M_PI_2;
+        const Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
+
+        const std::int64_t timestepMs = 100;
+        const Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
+        const int cameraSteps = 10;
+
+        // CONVERSIONS
+        const float timespanMs = timestepMs * cameraSteps;
+        const Eigen::Vector2f endPosition =
+            initialPosition + movementSpeedMetPerSec * timespanMs / 1000;
+        const Eigen::Vector3f posDelta = (endPosition - initialPosition) / cameraSteps;
+
+        // CAMERA MEASUREMENTS
+
+        const std::vector<CamMm> camMeasurements = generateCamMeasurements(
+            initialPosition, endPosition, orientationQuat, timespanMs, cameraSteps);
+
+        for (const CamMm& measurement : camMeasurements)
+        {
+            tracker.update(measurement);
+        }
+
+        // LASER MEASUREMENT
+
+        const DateTime tLaser = DateTime(Duration::MilliSeconds(timespanMs + timestepMs));
+        Eigen::Vector2f laserPos = (endPosition + posDelta).segment(0, 2);
+        const LaserMm laserMm = generateLaserMeasurement(laserPos, tLaser);
 
         tracker.update(laserMm);