Add grouping test and fix bugs

source/armarx/navigation/human/HumanGrouper.cpp

 #include "HumanGrouper.h"
 
+#include <unordered_set>
+
 #include "CombinedDistance.h"
 #include "ConvexHullGenerator.h"
 
@@ -45,8 +47,10 @@ namespace armarx::navigation::human
             {
                 const Human child = currentHumans_.at(childIdx);
 
-                if (this->distanceFunction->computeDistance(parent, child) <
-                    settings.groupingThreshold)
+                if ((humanGroupMap.count(childIdx) == 0 ||
+                     humanGroupMap[childIdx] != humanGroupMap[parentIdx]) &&
+                    this->distanceFunction->computeDistance(parent, child) <
+                        settings.groupingThreshold)
                 {
                     humanGroupMap[childIdx] = humanGroupMap[parentIdx];
                     groups.at(humanGroupMap[childIdx])

source/armarx/navigation/human/HumanGrouper.h

@@ -71,14 +71,11 @@ namespace armarx::navigation::human
          * @brief Recognizes groups in the current humans.
          *
          * Identifies and returns social groups in the humans given to this instance by the last
-         * call to updateHumans.
+         * call to updateHumans. Will also return groups of size one for single people.
          * @return a vector containing the recognized human groups
          */
         std::vector<human::HumanGroup> getCurrentGroups();
 
-
-        void setCurrentHumans(const std::vector<Human>& newCurrentHumans);
-
     private:
         const std::unique_ptr<DistanceFunction> distanceFunction;
         const std::unique_ptr<GroupShapeGenerator> shapeGenerator;

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

@@ -43,6 +43,7 @@
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/human/CombinedDistance.h>
 #include <armarx/navigation/human/ConvexHullGenerator.h>
+#include <armarx/navigation/human/HumanGrouper.h>
 #include <armarx/navigation/human/MovementDistance.h>
 #include <armarx/navigation/human/types.h>
 
@@ -52,6 +53,7 @@ using armarx::navigation::human::ConvexHullGenerator;
 using armarx::navigation::human::EuclideanDistance;
 using armarx::navigation::human::Human;
 using armarx::navigation::human::HumanGroup;
+using armarx::navigation::human::HumanGrouper;
 using armarx::navigation::human::MovementDistance;
 using armarx::navigation::human::OrientationDistance;
 using armarx::navigation::human::shapes::Polygon;
@@ -220,3 +222,59 @@ BOOST_AUTO_TEST_CASE(testConvexHullGenerator1_triangle)
     BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose4.translation()) ==
                 hull.vertices.end());
 }
+
+BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
+{
+    /*          ^     ^
+     *          |     |
+     *         h1>   <h2
+     *
+     *            h3
+     *            V
+     */
+
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(2, 1);
+    pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0, 1),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(3, 0.9);
+    pose2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0, 1.1),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose3 = Pose2D::Identity();
+    pose3.translation() = Eigen::Vector2f(2.5, 0);
+    pose3.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h3 = {.pose = pose3,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    HumanGrouper::GroupingSettings settings = {
+        .groupingThreshold = 2, .maxOrientationInfluence = 3, .movementInfluence = 2};
+
+    HumanGrouper grouper = HumanGrouper(settings);
+
+    std::vector<Human> humans = {h1, h2, h3};
+    grouper.updateHumans(humans);
+
+    std::vector<HumanGroup> groups = grouper.getCurrentGroups();
+
+    BOOST_CHECK_EQUAL(groups.size(), 2);
+    HumanGroup g1 = groups.at(0);
+    HumanGroup g2 = groups.at(1);
+
+    BOOST_CHECK((g1.humans.size() == 2) != (g2.humans.size() == 2));
+    HumanGroup& groupOfTwo = g1.humans.size() == 2 ? g1 : g2;
+
+
+    // Check that the group of two contains h1 and h2
+    BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h1) !=
+                groupOfTwo.humans.end());
+    BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h2) !=
+                groupOfTwo.humans.end());
+}

source/armarx/navigation/human/types.cpp

@@ -14,6 +14,14 @@ namespace armarx::navigation::human
         return estimation;
     }
 
+    bool
+    Human::operator==(const Human& other) const
+    {
+        return other.pose.matrix() == this->pose.matrix() &&
+               other.linearVelocity == this->linearVelocity &&
+               other.detectionTime == this->detectionTime;
+    }
+
     aron::data::DictPtr
     Human::toAron() const
     {

source/armarx/navigation/human/types.h

@@ -38,6 +38,8 @@ namespace armarx::navigation::human
 
         core::Pose2D estimateAt(const DateTime& time) const;
 
+        bool operator==(const Human& other) const;
+
         aron::data::DictPtr toAron() const;
         static Human FromAron(const aron::data::DictPtr& dict);
     };