Fix bugs

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;
     }
 

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