Add human tracker test (this, for real)

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

+/**
+ * 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