From 6242f7ab995eeb2606de7923d8c54ac4f4144f5e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 27 Oct 2022 15:38:12 +0200
Subject: [PATCH] Save WIP state of human tracker test
---
.../human/test/human_tracker_test.cpp | 26 ++++++++++++++++---
1 file changed, 22 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
index 641c8f5f..29d64823 100644
--- a/source/armarx/navigation/human/test/human_tracker_test.cpp
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -58,13 +58,15 @@ namespace armarx::navigation::human
HumanTracker tracker = HumanTracker();
Eigen::Vector3f initialPosition(0, 0, 2);
- Eigen::Quaternionf orientation = quatFromEuler(0, 0, M_PI_2);
+ float orientation = M_PI_2;
+ Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
std::int64_t timestepMs = 100;
Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
int cameraSteps = 10;
- Eigen::Vector3f posDelta = movementSpeedMetPerSec * (timestepMs / 1000);
+ Eigen::Vector3f posDelta =
+ movementSpeedMetPerSec * (static_cast<float>(timestepMs) / 1000.0);
std::vector<CamMm> camMeasurements = std::vector<CamMm>();
for (int i = 0; i < cameraSteps; i++)
@@ -73,7 +75,7 @@ namespace armarx::navigation::human
Eigen::Vector3f newPos = initialPosition + i * posDelta;
FramedPosition headPosition = FramedPosition(newPos, "", "");
- FramedOrientation headOrientation = FramedOrientation(orientation, "", "");
+ FramedOrientation headOrientation = FramedOrientation(orientationQuat, "", "");
PoseKeypoint head = {.label = "HEAD",
.confidence = 0.95,
.positionGlobal = headPosition,
@@ -85,10 +87,26 @@ namespace armarx::navigation::human
tracker.update(camMm);
}
- // TODO: now add a lasersensor measurement roughly at the next human pose
DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
+ using Point = Eigen::Vector2f;
+
+ std::vector<Point> relOffsets = {Point(0.01, 0.003),
+ Point(-0.02, 0.007),
+ Point(-0.01, -0.01),
+ Point(0.015, 0.009),
+ Point(0.002, 0.001)};
+
+ Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
+ //Eigen::Vector3f leftFootPos = laserPos
+
+ std::vector<Point> leftFootPoints = std::vector<Point>();
+ for (Point& offset : relOffsets)
+ {
+ //leftFootPoints.emplace_back();
+ }
+
std::vector<Cluster> clusters = std::vector<Cluster>();
LaserMm laserMm;
tracker.update(laserMm);
--
GitLab