From c7bed5cbd01bf2436ed098845aea2bcfea8514d2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 7 Nov 2022 12:47:15 +0100
Subject: [PATCH] Fix bugs
---
.../armarx/navigation/human/HumanTracker.cpp | 17 +++++++------
.../human/test/human_tracker_test.cpp | 25 +++++++++++--------
2 files changed, 24 insertions(+), 18 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index ea22b062..4cde5d0d 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -7,6 +7,7 @@
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
+
namespace armarx::navigation::human
{
void
@@ -53,13 +54,13 @@ namespace armarx::navigation::human
// footprints
std::vector<AdvancedCluster> potentialFootprints;
std::vector<Cluster> unusedClusters;
- for (auto it = measurements.clusters.begin(); it != measurements.clusters.end();)
+ for (const auto& cluster : measurements.clusters)
{
- auto& cluster = *it;
if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
{
- Eigen::Vector2f clusterCenter = cluster.ellipsoid.pose.translation();
+ // TODO: Why is the ellipse translation() here 3D? Can we just use the first 2 entries?
+ Eigen::Vector2f clusterCenter = cluster.ellipsoid.pose.translation().segment(0, 2);
potentialFootprints.push_back(AdvancedCluster{
.center = clusterCenter, .cluster = cluster, .associated = false});
@@ -70,7 +71,6 @@ namespace armarx::navigation::human
}
}
-
// associate humans and clusters by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, potentialFootprints);
@@ -97,9 +97,11 @@ namespace armarx::navigation::human
auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
auto& footprint2 = posDistance.newCluster->cluster;
- Eigen::Vector2f centerPos = (footprint1.ellipsoid.pose.translation() +
- footprint2.ellipsoid.pose.translation()) /
- 2;
+ // TODO: Why is the ellipse translation() here 3D? Can we just use the first 2 entries?
+ Eigen::Vector2f centerPos =
+ (footprint1.ellipsoid.pose.translation().segment(0, 2) +
+ footprint2.ellipsoid.pose.translation().segment(0, 2)) /
+ 2;
//create pose with orientation from old human
core::Pose2D pose = core::Pose2D::Identity();
@@ -146,7 +148,6 @@ namespace armarx::navigation::human
unusedClusters.push_back(cluster.cluster);
}
}
-
return unusedClusters;
}
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
index 25e24fa3..19449812 100644
--- a/source/armarx/navigation/human/test/human_tracker_test.cpp
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -19,6 +19,8 @@
* GNU General Public License
*/
+#include <iostream>
+
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/interface/core/BasicVectorTypes.h>
@@ -68,7 +70,6 @@ namespace armarx::navigation::human
const Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
const int cameraSteps = 10;
-
// CAMERA MEASUREMENTS
const Eigen::Vector3f posDelta =
@@ -106,25 +107,27 @@ namespace armarx::navigation::human
Point(0.002, 0.001)};
const Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
- const Eigen::Vector3f leftFootPos = laserPos + Point(0, 0.1);
- const Eigen::Vector3f rightFootPos = laserPos + Point(0, -0.1);
+ const Eigen::Vector3f leftFootPos = laserPos + Eigen::Vector3f(0, 0.1, 0);
+ const Eigen::Vector3f rightFootPos = laserPos + Eigen::Vector3f(0, -0.1, 0);
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 + offset);
- rightFootPoints.emplace_back(rightFootPos + offset);
+ leftFootPoints.emplace_back(leftFootPos.segment(0, 2) + offset);
+ rightFootPoints.emplace_back(rightFootPos.segment(0, 2) + offset);
+ i++;
}
using Pose = Eigen::Isometry3f;
Pose leftFootPose = Pose::Identity();
leftFootPose.translation() = leftFootPos;
- leftFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
+ // leftFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
Pose rightFootPose = Pose::Identity();
rightFootPose.translation() = rightFootPos;
- rightFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
+ // 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)};
@@ -136,13 +139,15 @@ namespace armarx::navigation::human
std::vector<Cluster>{leftFootCluster, rightFootCluster};
const LaserMm laserMm = {.detectionTime = tLaser, .clusters = clusters};
+
tracker.update(laserMm);
const std::vector<Human> humans = tracker.getTrackedHumans();
BOOST_CHECK_EQUAL(humans.size(), 1); // should have detected the one human through laser
const 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);
+ BOOST_CHECK(h.pose.translation().x() - laserPos.x() < 0.05);
+ BOOST_CHECK(h.pose.translation().y() - laserPos.y() < 0.05);
+ BOOST_CHECK(h.linearVelocity.x() - movementSpeedMetPerSec.x() < 0.05);
+ BOOST_CHECK(h.linearVelocity.y() - movementSpeedMetPerSec.y() < 0.05);
}
} // namespace armarx::navigation::human
--
GitLab