diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
index 194498128f23b4f06d3e8e6c23689fa7a1cc5527..3d2e803748affca99ddc6fc859296cbd6efdb4fd 100644
--- a/source/armarx/navigation/human/test/human_tracker_test.cpp
+++ b/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,39 +109,82 @@ 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::Vector2f(0, 0.1);
+ const Eigen::Vector2f rightFootPos = pos + Eigen::Vector2f(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++;
}
using Pose = Eigen::Isometry3f;
Pose leftFootPose = Pose::Identity();
- leftFootPose.translation() = leftFootPos;
+ leftFootPose.translation() = Eigen::Vector3f(leftFootPos.x(), leftFootPos.y(), 0);
// leftFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
Pose rightFootPose = Pose::Identity();
- rightFootPose.translation() = rightFootPos;
+ rightFootPose.translation() = Eigen::Vector3f(rightFootPos.x(), rightFootPos.y(), 0);
+ ;
// rightFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
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::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;
+
+ // CONVERSIONS
+ const float timespanMs = timestepMs * cameraSteps;
+ const Eigen::Vector3f 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);