From 482aee62149a70d1538cf23de64dc2c48502d2c4 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:17:31 +0200
Subject: [PATCH] Add human tracker test (this, for real)
---
.../human/test/human_tracker_test.cpp | 96 +++++++++++++++++++
1 file changed, 96 insertions(+)
create mode 100644 source/armarx/navigation/human/test/human_tracker_test.cpp
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
new file mode 100644
index 00000000..641c8f5f
--- /dev/null
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -0,0 +1,96 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/HumanTracker.h>
+#include <armarx/navigation/human/types.h>
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+using armarx::armem::human::HumanPose;
+using armarx::armem::human::PoseKeypoint;
+using armarx::navigation::core::Pose2D;
+using CamMm = armarx::navigation::human::HumanTracker::CameraMeasurement;
+using LaserMm = armarx::navigation::human::HumanTracker::LaserMeasurement;
+using Eigen::Vector2f;
+
+namespace armarx::navigation::human
+{
+ Eigen::Quaternionf
+ quatFromEuler(float roll, float pitch, float yaw)
+ {
+ return Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());
+ }
+
+ BOOST_AUTO_TEST_CASE(testLaserTracking)
+ {
+ HumanTracker tracker = HumanTracker();
+
+ Eigen::Vector3f initialPosition(0, 0, 2);
+ Eigen::Quaternionf orientation = quatFromEuler(0, 0, M_PI_2);
+
+ std::int64_t timestepMs = 100;
+ Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
+ int cameraSteps = 10;
+
+ Eigen::Vector3f posDelta = movementSpeedMetPerSec * (timestepMs / 1000);
+
+ std::vector<CamMm> camMeasurements = std::vector<CamMm>();
+ for (int i = 0; i < cameraSteps; i++)
+ {
+ DateTime t = DateTime(Duration::MilliSeconds(i * timestepMs));
+
+ Eigen::Vector3f newPos = initialPosition + i * posDelta;
+ FramedPosition headPosition = FramedPosition(newPos, "", "");
+ FramedOrientation headOrientation = FramedOrientation(orientation, "", "");
+ PoseKeypoint head = {.label = "HEAD",
+ .confidence = 0.95,
+ .positionGlobal = headPosition,
+ .orientationGlobal = headOrientation};
+ HumanPose pose = {"posemodelid", {{"HEAD", head}}};
+ std::vector<armem::human::HumanPose> humanPoses = {pose};
+ CamMm camMm = {t, humanPoses};
+
+ tracker.update(camMm);
+ }
+
+ // TODO: now add a lasersensor measurement roughly at the next human pose
+
+ DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
+
+ std::vector<Cluster> clusters = std::vector<Cluster>();
+ LaserMm laserMm;
+ tracker.update(laserMm);
+ }
+} // namespace armarx::navigation::human
--
GitLab