Complete test case basic code

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

@@ -42,6 +42,7 @@ using armarx::navigation::core::Pose2D;
 using CamMm = armarx::navigation::human::HumanTracker::CameraMeasurement;
 using LaserMm = armarx::navigation::human::HumanTracker::LaserMeasurement;
 using Eigen::Vector2f;
+using Ellipsoid = armarx::armem::vision::Ellipsoid;
 
 namespace armarx::navigation::human
 {
@@ -53,10 +54,12 @@ namespace armarx::navigation::human
                Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());
     }
 
-    BOOST_AUTO_TEST_CASE(testLaserTracking)
+    BOOST_AUTO_TEST_CASE(testLaserTrackingTwoFeetCluster)
     {
         HumanTracker tracker = HumanTracker();
 
+        // PARAMETERS
+
         Eigen::Vector3f initialPosition(0, 0, 2);
         float orientation = M_PI_2;
         Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
@@ -65,6 +68,9 @@ namespace armarx::navigation::human
         Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
         int cameraSteps = 10;
 
+
+        // CAMERA MEASUREMENTS
+
         Eigen::Vector3f posDelta =
             movementSpeedMetPerSec * (static_cast<float>(timestepMs) / 1000.0);
 
@@ -87,6 +93,7 @@ namespace armarx::navigation::human
             tracker.update(camMm);
         }
 
+        // LASER MEASUREMENT
 
         DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
 
@@ -99,16 +106,42 @@ namespace armarx::navigation::human
                                          Point(0.002, 0.001)};
 
         Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
-        //Eigen::Vector3f leftFootPos = laserPos
+        Eigen::Vector3f leftFootPos = laserPos + Point(0, 0.1);
+        Eigen::Vector3f rightFootPos = laserPos + Point(0, -0.1);
 
         std::vector<Point> leftFootPoints = std::vector<Point>();
+        std::vector<Point> rightFootPoints = std::vector<Point>();
         for (Point& offset : relOffsets)
         {
-            //leftFootPoints.emplace_back();
+            leftFootPoints.emplace_back(leftFootPos + offset);
+            rightFootPoints.emplace_back(rightFootPos + offset);
         }
 
-        std::vector<Cluster> clusters = std::vector<Cluster>();
-        LaserMm laserMm;
+        using Pose = Eigen::Isometry3f;
+        Pose leftFootPose = Pose::Identity();
+        leftFootPose.translation() = leftFootPos;
+        leftFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
+
+        Pose rightFootPose = Pose::Identity();
+        rightFootPose.translation() = rightFootPos;
+        rightFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
+
+        Ellipsoid leftFootEllipse = {.pose = leftFootPose, .radii = Point(0.05, 0.04)};
+        Ellipsoid rightFootEllipse = {.pose = rightFootPose, .radii = Point(0.05, 0.04)};
+
+        Cluster leftFootCluster = {.ellipsoid = leftFootEllipse, .points = leftFootPoints};
+        Cluster rightFootCluster = {.ellipsoid = rightFootEllipse, .points = rightFootPoints};
+
+        std::vector<Cluster> clusters = std::vector<Cluster>{leftFootCluster, rightFootCluster};
+
+        LaserMm laserMm = {.detectionTime = tLaser, .clusters = clusters};
         tracker.update(laserMm);
+
+        std::vector<Human> humans = tracker.getTrackedHumans();
+        BOOST_CHECK_EQUAL(humans.size(), 1); // should have detected the one human through laser
+        Human h = humans.at(0);
+        BOOST_CHECK_CLOSE(h.pose.translation().x(), 1.0, 0.05);
+        BOOST_CHECK(h.pose.translation().y() < 0.05);
+        BOOST_CHECK_CLOSE(h.linearVelocity.x(), 1.0, 0.2);
     }
 } // namespace armarx::navigation::human