diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 1110c342da3693c7868daa14a741bca2e0f67711..8d776c194c6d6ecafc9522a77211d0dd54330f24 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -189,6 +189,7 @@ namespace armarx::navigation::components::example_client
             client::NavigationStackConfig()
                 .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
                 .globalPlanner(global_planning::SPFAParams())
+                .localPlanner(local_planning::TimedElasticBandsParams())
                 .trajectoryController(
                     traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
 
@@ -289,6 +290,7 @@ namespace armarx::navigation::components::example_client
             client::NavigationStackConfig()
                 .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
                 .globalPlanner(global_planning::SPFAParams())
+                .localPlanner(local_planning::TimedElasticBandsParams())
                 .trajectoryController(
                     traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
 
@@ -366,6 +368,7 @@ namespace armarx::navigation::components::example_client
             client::NavigationStackConfig()
                 .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
                 .globalPlanner(global_planning::Point2PointParams())
+                .localPlanner(local_planning::TimedElasticBandsParams())
                 .trajectoryController(
                     traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
 
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9ddc696aba96611d4b09cbc79757c80da3f62c00..90868b31893384bc28c571868d86d7d8522229c0 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -37,6 +37,7 @@
 
 #include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
 #include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/types.h>
 #include <armarx/navigation/memory/client/costmap/Reader.h>
 #include <armarx/navigation/util/util.h>
 
@@ -47,7 +48,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
     const std::string Component::defaultName = "dynamic_scene_provider";
 
 
-    Component::Component()
+    Component::Component() : humanGrouper(HumanGrouper::GroupingSettings())
     {
         addPlugin(humanPoseReaderPlugin);
         addPlugin(laserScannerFeaturesReaderPlugin);
@@ -55,6 +56,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         addPlugin(costmapReaderPlugin);
         addPlugin(occupancyGridReaderPlugin);
         addPlugin(humanWriterPlugin);
+        addPlugin(humanGroupWriterPlugin);
     }
 
     armarx::PropertyDefinitionsPtr
@@ -242,6 +244,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
         // Laser scanner features
         //
 
+        ARMARX_INFO << "Not querying laser scanner features for testing purposes (no "
+                       "robot available)";
+
         /*
         TIMING_START(READ_LASERSCANNER_FROM_MEMORY);
 
@@ -251,13 +256,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
             .name = properties.laserScannerFeatures.name,
             .timestamp = timestamp};
 
-        const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
-            laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+                const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
+                    laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
 
-        ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
-                           armem::vision::laser_scanner_features::client::Reader::Result::Success);
+                ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+                                   armem::vision::laser_scanner_features::client::Reader::Result::Success);
 
-        ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+                ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
 
         TIMING_END_COMMENT_STREAM(
             READ_LASERSCANNER_FROM_MEMORY, "Timer: reading laserscanner from memory", ARMARX_VERBOSE);
@@ -394,6 +399,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
                                   ARMARX_VERBOSE);
 
 
+        ARMARX_INFO << "Not running tracker with lasersensors because they are not available for "
+                       "the current tests.";
+
         /*
         ARMARX_INFO << "Running human tracker with lasersensor measurements";
 
@@ -425,18 +433,60 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
         //TODO use clusters for obstacle creation
 
-        std::vector<human::Human> humans = humanTracker.getTrackedHumans();
+        humanTracker.update(human::HumanTracker::CameraMeasurement{
+            .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
+        using Human = armarx::navigation::human::Human;
+        std::vector<Human> humans = humanTracker.getTrackedHumans();
+
+
+        ARMARX_INFO << "For testing purposes, we will add another 3 humans!";
+        core::Pose2D p1 = core::Pose2D::Identity();
+        p1.linear() = Eigen::Rotation2Df(1.25 * M_PI).toRotationMatrix();
+        p1.translation() = Eigen::Vector2f(1000, 500);
+        Human h1 = {
+            .pose = p1, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+        core::Pose2D p2 = core::Pose2D::Identity();
+        p2.linear() = Eigen::Rotation2Df(-0.25 * M_PI).toRotationMatrix();
+        p2.translation() = Eigen::Vector2f(1000, -500);
+        Human h2 = {
+            .pose = p2, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+        core::Pose2D p3 = core::Pose2D::Identity();
+        p3.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+        p3.translation() = Eigen::Vector2f(1500, 0);
+        Human h3 = {
+            .pose = p3, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+
+
+        humans.emplace_back(h1);
+        humans.emplace_back(h2);
+        // periodically toggle third human for showing group of 2 vs group of 3
+        if ((timestamp.toSecondsSinceEpoch() / 5) % 2 == 0)
+        {
+            humans.emplace_back(h3);
+        }
 
+        humanWriterPlugin->get().store(humans, getName(), timestamp);
 
-        if(not humans.empty())
+        if (not humans.empty())
         {
             ARMARX_INFO << "Detected " << humans.size() << " humans";
             humanWriterPlugin->get().store(humans, getName(), timestamp);
         }
 
+        ARMARX_INFO << "Providing grouper with new humans";
+
+        humanGrouper.updateHumans(humans);
+
+        ARMARX_INFO << "Running human grouper";
+        using HumanGroup = armarx::navigation::human::HumanGroup;
+        std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
+
+        ARMARX_INFO << "Detected " << groups.size() << " human groups";
+
+        humanGroupWriterPlugin->get().store(groups, getName(), timestamp);
 
         TIMING_END_COMMENT_STREAM(
-            WRITE_BACK_HUMANS, "Timer: write humans to memory", ARMARX_VERBOSE);
+            WRITE_BACK_HUMANS, "Timer: write humans and human groups to memory", ARMARX_VERBOSE);
 
 
         TIMING_END_COMMENT_STREAM(
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index aec652dd6438b528ed939e80379715d34e795f60..63723a7a824056d6fcd9afa9478cab6b0f97423c 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -51,6 +51,8 @@
 #include "armarx/navigation/memory/client/costmap/Reader.h"
 #include "armarx/navigation/memory/client/human/Writer.h"
 #include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
+#include <armarx/navigation/human/HumanGrouper.h>
+#include <armarx/navigation/memory/client/human_group/Writer.h>
 
 
 namespace armarx::navigation::components::dynamic_scene_provider
@@ -204,8 +206,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
         ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
 
+        ReaderWriterPlugin<memory::client::human_group::Writer>* humanGroupWriterPlugin = nullptr;
+
 
         human::HumanTracker humanTracker;
+        using HumanGrouper = armarx::navigation::human::HumanGrouper;
+        HumanGrouper humanGrouper;
     };
 
 } // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index dcf4b7ad14653de4fe1dbdc83e56fd69fa591452..e2cc5737e3991c3b9fbd4898ad144e22e93abc02 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -51,8 +51,8 @@
 #include <armarx/navigation/core/aron/Location.aron.generated.h>
 #include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
 #include <armarx/navigation/core/aron/Twist.aron.generated.h>
-#include <armarx/navigation/human/aron/Human.aron.generated.h>
 #include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
 #include <armarx/navigation/location/constants.h>
 #include <armarx/navigation/memory/constants.h>
 
@@ -133,8 +133,8 @@ namespace armarx::navigation::components::navigation_memory
         workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
                                        navigation::human::arondto::Human::ToAronType());
 
-        // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
-        //                                navigation::human::arondto::Human::ToAronType());
+        workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+                                       navigation::human::arondto::HumanGroup::ToAronType());
 
         if (not properties.snapshotToLoad.empty())
         {
@@ -421,11 +421,13 @@ namespace armarx::navigation::components::navigation_memory
     void
     Component::visuRun()
     {
-        memory::Visu visu{arviz,
-                          workingMemory().getCoreSegment(navigation::location::coreSegmentID),
-                          workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
-                          workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
-                          workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
+        memory::Visu visu{
+            arviz,
+            workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+            workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+            workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+            workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
+            workingMemory().getCoreSegment(memory::constants::HumanGroupCoreSegmentName)};
 
 
         Properties::LocationGraph p;
@@ -458,6 +460,9 @@ namespace armarx::navigation::components::navigation_memory
             // Humans
             visu.drawHumans(layers, p.visuHumans, p.visuTransparent);
 
+            // Groups
+            visu.drawHumanGroups(layers, p.visuHumanGroups);
+
             arviz.commit(layers);
 
             metronome.waitForNextTick();
@@ -580,7 +585,7 @@ namespace armarx::navigation::components::navigation_memory
 
     bool
     Component::storeLocationGraph(const armarx::data::PackagePath& packagePath,
-                                         const Ice::Current& current)
+                                  const Ice::Current& current)
     {
         armem::server::wm::CoreSegment& locationCoreSegment =
             workingMemory().getCoreSegment(navigation::location::coreSegmentID);
@@ -651,4 +656,4 @@ namespace armarx::navigation::components::navigation_memory
     }
 
 
-}  // namespace armarx::navigation::components::navigation_memory
+} // namespace armarx::navigation::components::navigation_memory
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index b1b1a1b682e645c33ab1daef65c0cb10587a0796..619be519aef6dd3be5ed205ce3042d10aa975a8c 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -105,6 +105,7 @@ namespace armarx::navigation::components::navigation_memory
                 bool visuGraphEdges = true;
                 bool visuCostmaps = true;
                 bool visuHumans = true;
+                bool visuHumanGroups = true;
 
                 bool visuTransparent = false;
 
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 882deb59ae697593a36325bd0b3b8fff5625f9eb..d506f16e0f000acb4083a3f729428070df9f485e 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -53,12 +53,14 @@ namespace armarx::navigation::memory
                const armem::server::wm::CoreSegment& locSegment,
                const armem::server::wm::CoreSegment& graphSegment,
                const armem::server::wm::CoreSegment& costmapSegment,
-               const armem::server::wm::CoreSegment& humanSegment) :
+               const armem::server::wm::CoreSegment& humanSegment,
+               const armem::server::wm::CoreSegment& humanGroupSegment) :
         arviz(arviz),
         locSegment(locSegment),
         graphSegment(graphSegment),
         costmapSegment(costmapSegment),
         humanSegment(humanSegment),
+        humanGroupSegment(humanGroupSegment),
         visu(std::make_unique<graph::GraphVisu>())
     {
     }
@@ -203,7 +205,6 @@ namespace armarx::navigation::memory
                 mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
                 layer.add(mmm);
 
-
                 core::Pose pose3d = conv::to3D(human.pose);
                 pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
                 auto arrow = viz::Arrow(std::to_string(layer.size()))
@@ -224,6 +225,26 @@ namespace armarx::navigation::memory
             }
         }
 
+        void
+        visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
+        {
+            for (const auto& humanGroup : humanGroups)
+            {
+                viz::Polygon polygon(std::to_string(layer.size()));
+
+                const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
+                std::vector<Eigen::Vector3f> verts3D;
+                for (const auto& v2d : verts2D)
+                {
+                    verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 50));
+                }
+                polygon.points(verts3D);
+                polygon.color(simox::Color::kit_yellow());
+                polygon.lineWidth(10);
+                layer.add(polygon);
+            }
+        }
+
     } // namespace
 
     void
@@ -311,5 +332,47 @@ namespace armarx::navigation::memory
         }
     }
 
+    void
+    Visu::drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled)
+    {
+        if (not enabled)
+        {
+            return;
+        }
+
+        std::map<std::string, navigation::human::HumanGroups> namedProviderHumanGroups;
+
+        humanGroupSegment.doLocked(
+            [&]()
+            {
+                using namespace armem::server;
+
+                humanGroupSegment.forEachEntity(
+                    [&](const wm::Entity& entity)
+                    {
+                        namedProviderHumanGroups[entity.id().providerSegmentName];
+                        entity.getLatestSnapshot().forEachInstance(
+                            [&namedProviderHumanGroups](
+                                const armarx::armem::wm::EntityInstance& instance)
+                            {
+                                const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+                                    instance.data());
+
+                                navigation::human::HumanGroup humanGroup;
+                                fromAron(dto, humanGroup);
+
+                                namedProviderHumanGroups[instance.id().providerSegmentName]
+                                    .emplace_back(std::move(humanGroup));
+                            });
+                    });
+            });
+
+        for (const auto& [providerName, humanGroups] : namedProviderHumanGroups)
+        {
+            viz::Layer& layer = layers.emplace_back(arviz.layer("humanGroups_" + providerName));
+            visualize(humanGroups, layer);
+        }
+    }
+
 
 } // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index d965feccd52b78b895ded9ebaf1683ff64c95319..55b0e35f146ed59c1df99de5a30c6af0d630288e 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -48,7 +48,8 @@ namespace armarx::navigation::memory
              const armem::server::wm::CoreSegment& locSegment,
              const armem::server::wm::CoreSegment& graphSegment,
              const armem::server::wm::CoreSegment& costmapSegment,
-             const armem::server::wm::CoreSegment& humanSegment);
+             const armem::server::wm::CoreSegment& humanSegment,
+             const armem::server::wm::CoreSegment& humanGroupSegment);
         ~Visu();
 
 
@@ -56,6 +57,7 @@ namespace armarx::navigation::memory
         void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
         void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
         void drawHumans(std::vector<viz::Layer>& layers, bool enabled, bool visuTransparent);
+        void drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled);
 
 
         viz::ScopedClient arviz;
@@ -64,6 +66,7 @@ namespace armarx::navigation::memory
         const armem::server::wm::CoreSegment& graphSegment;
         const armem::server::wm::CoreSegment& costmapSegment;
         const armem::server::wm::CoreSegment& humanSegment;
+        const armem::server::wm::CoreSegment& humanGroupSegment;
 
         std::unique_ptr<navigation::graph::GraphVisu> visu;
     };
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 836826fb77312440fdc726cc105d91e2201f0fba..360b78b6c31bc8942cc4ce5ba597e798edfc8bf6 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -18,6 +18,12 @@ armarx_add_library(teb_human
         types.cpp
         aron_conversions.cpp
         ProxemicZoneCreator.cpp
+        HumanGrouper.cpp
+        EuclideanDistance.cpp
+        OrientationDistance.cpp
+        CombinedDistance.cpp
+        ConvexHullGenerator.cpp
+        MovementDistance.cpp
         HumanSystemModel.cpp
         UnscentedKalmanFilter_impl.cpp
         HumanTracker.cpp
@@ -27,6 +33,14 @@ armarx_add_library(teb_human
         aron_conversions.h
         shapes.h
         ProxemicZoneCreator.h
+        HumanGrouper.h
+        DistanceFunction.h
+        EuclideanDistance.h
+        OrientationDistance.h
+        CombinedDistance.h
+        GroupShapeGenerator.h
+        ConvexHullGenerator.h
+        MovementDistance.h
         HumanSystemModel.h
         HumanTracker.h
         HumanFilter.h
@@ -100,8 +114,24 @@ armarx_add_test(human_tracker_test
     TEST_FILES
         test/human_tracker_test.cpp
     DEPENDENCIES
-        ArmarXCore
-        armarx_navigation::core
-        armarx_navigation::teb_human
-        range-v3::range-v3
+            PUBLIC
+                ArmarXCore
+                armarx_navigation::core
+                armarx_navigation::teb_human
+
+            PRIVATE
+                range-v3::range-v3
+)
+
+armarx_add_test(human_test
+    TEST_FILES
+        test/human_test.cpp
+    DEPENDENCIES
+        PUBLIC
+            ArmarXCore
+            armarx_navigation::core
+            armarx_navigation::teb_human
+
+        PRIVATE
+            range-v3::range-v3
 )
diff --git a/source/armarx/navigation/human/CombinedDistance.cpp b/source/armarx/navigation/human/CombinedDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..18330577f961ed5130511092d8704e6f3581a0c6
--- /dev/null
+++ b/source/armarx/navigation/human/CombinedDistance.cpp
@@ -0,0 +1,28 @@
+#include "CombinedDistance.h"
+
+namespace armarx::navigation::human
+{
+    CombinedDistance::CombinedDistance(const double maxOrientationInfluence,
+                                       const double movementInfluence) :
+        euclidean(EuclideanDistance()),
+        orientation(OrientationDistance()),
+        movement(MovementDistance()),
+        maxOrientationInfluence(maxOrientationInfluence),
+        movementInfluence(movementInfluence)
+    {
+    }
+
+    double
+    CombinedDistance::computeDistance(const Human& h1, const Human& h2) const
+    {
+        double d = euclidean.computeDistance(h1, h2);
+
+        // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+        d *= (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2));
+
+        // scales the euclidean distance by a factor in [1, inf) depending on influence
+        d *= (1 + movementInfluence * movement.computeDistance(h1, h2));
+
+        return d;
+    }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..4cacc38d0971ce6499b0bcae046c12de84ad787e
--- /dev/null
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -0,0 +1,67 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+#include "EuclideanDistance.h"
+#include "MovementDistance.h"
+#include "OrientationDistance.h"
+
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief The CombinedDistance class combines the standard euclidean distance multiplicatively
+     * with an orientation distance in such a way that the distance is always at least the euclidean
+     * distance but will be scaled by up to the maximum influence factor if the people face away
+     * from each other. The distance will then again be increased by the scaled difference in movement
+     * between the two people. The distance will always be at least the euclidean distance.
+     */
+    class CombinedDistance : public DistanceFunction
+    {
+    public:
+        /**
+         * @brief CombinedDistance Creates a new combined distance with the specified orientation scale factor
+         * @param maxOrientationInfluence the euclidean distance between the humans will be scaled
+         * by up to this factor if they face away from each other
+         * @param movementInfluence scales the difference in velocity of the humans before applying
+         * it to the total distance
+         */
+        CombinedDistance(double maxOrientationInfluence, double movementInfluence);
+
+        /**
+         * @brief computeDistance computes the distance value for the two given people. Should
+         * always be commutative, i.e. computeDistance(h1, h2) = computeDistance(h2, h1)
+         * @param h1 the first human
+         * @param h2 the second human
+         * @return a value specifying a distance between the two people
+         */
+        double computeDistance(const Human& h1, const Human& h2) const override;
+
+    private:
+        const EuclideanDistance euclidean;
+        const OrientationDistance orientation;
+        const MovementDistance movement;
+        const double maxOrientationInfluence;
+        const double movementInfluence;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2aff0df1f43d4aba9a1a1b76a32049c50ad371e7
--- /dev/null
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -0,0 +1,49 @@
+#include "ConvexHullGenerator.h"
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/multi/geometries/register/multi_point.hpp>
+
+using Point = std::pair<double, double>;
+BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian, first, second)
+
+namespace armarx::navigation::human
+{
+
+    shapes::Polygon
+    ConvexHullGenerator::createShape(const HumanGroup& group) const
+    {
+        if (group.humans.empty())
+        {
+            throw InvalidArgumentException("Human group size has to be at least 1");
+        }
+
+        using BoostPoly = boost::geometry::model::polygon<Point>;
+
+        std::vector<Point> coordinates;
+        for (const Human& human : group.humans)
+        {
+            coordinates.emplace_back(human.pose.translation().x(), human.pose.translation().y());
+        }
+
+        BoostPoly poly;
+        BoostPoly hull;
+
+        poly.outer().assign(coordinates.begin(), coordinates.end());
+        boost::geometry::convex_hull(poly, hull);
+
+        std::vector<Eigen::Vector2f> vertices;
+
+        for (auto it = boost::begin(boost::geometry::exterior_ring(hull));
+             it != boost::end(boost::geometry::exterior_ring(hull));
+             ++it)
+        {
+            const double x = boost::geometry::get<0>(*it);
+            const double y = boost::geometry::get<1>(*it);
+
+            vertices.emplace_back(x, y);
+        }
+
+        return {.vertices = vertices};
+    }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..d99d845dbafc308bc7acc86b5b23282374d8dfc6
--- /dev/null
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -0,0 +1,44 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include "GroupShapeGenerator.h"
+#include <armarx/navigation/human/types.h>
+
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
+     * convex hull of their positions in 2D space.
+     */
+    class ConvexHullGenerator : public GroupShapeGenerator
+    {
+    public:
+        ConvexHullGenerator() = default;
+        virtual ~ConvexHullGenerator() = default;
+
+        shapes::Polygon createShape(const HumanGroup& group) const override;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/DistanceFunction.h b/source/armarx/navigation/human/DistanceFunction.h
new file mode 100644
index 0000000000000000000000000000000000000000..070d897472f6efa4d209960fdde84ae9d09eb36f
--- /dev/null
+++ b/source/armarx/navigation/human/DistanceFunction.h
@@ -0,0 +1,49 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief A distance function is able to compute a distance between two humans.
+     * The exact semantics of this distance depends on the implementation. For example,
+     * a HumanGrouper uses distances two group humans together (the smaller the distance between
+     * two humans, the more likely it is for them to belong to the same social group).
+     */
+    class DistanceFunction
+    {
+    public:
+        DistanceFunction() = default;
+        virtual ~DistanceFunction() = default;
+
+        /**
+         * @brief computeDistance computes the (semantic or literal, depending on the implementation) distance between the given humans
+         * @param h1 the first human
+         * @param h2 the second human
+         * @return the distance between the humans (greater values correspond to humans that are further away)
+         */
+        virtual double computeDistance(const Human& h1, const Human& h2) const = 0;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.cpp b/source/armarx/navigation/human/EuclideanDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..911d59a914634c00594bce7cd160af86b39b617e
--- /dev/null
+++ b/source/armarx/navigation/human/EuclideanDistance.cpp
@@ -0,0 +1,10 @@
+#include "EuclideanDistance.h"
+
+namespace armarx::navigation::human
+{
+    double
+    EuclideanDistance::computeDistance(const Human& h1, const Human& h2) const
+    {
+        return (h1.pose.translation() - h2.pose.translation()).norm();
+    }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1d39ff10bfffdc3636d1a882b3ba3c59a50adbf
--- /dev/null
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -0,0 +1,39 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief The EuclideanDistance class is an implementation of a distance function that computes
+     * the literal, euclidean distance between two human. (i.e. sqrt(dx²+dy²)).
+     */
+    class EuclideanDistance : public DistanceFunction
+    {
+    public:
+        EuclideanDistance() = default;
+        double computeDistance(const Human& h1, const Human& h2) const override;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/GroupShapeGenerator.h b/source/armarx/navigation/human/GroupShapeGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..34f777d9fb2bbdc17a42559b37125d32a7f0889f
--- /dev/null
+++ b/source/armarx/navigation/human/GroupShapeGenerator.h
@@ -0,0 +1,47 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief A strategy encapsuling a way to create a shape describing the special area occupied
+     * by a social human group
+     */
+    class GroupShapeGenerator
+    {
+    public:
+        GroupShapeGenerator() = default;
+        virtual ~GroupShapeGenerator() = default;
+
+        /**
+         * @brief createShape creates and returns the shape describing the given group
+         * @param group a social grouping of at least one human
+         * @return a polygon describing the proxemic zone occupied by the given group (not including
+         * the zones of the individual members of the group)
+         */
+        virtual shapes::Polygon createShape(const HumanGroup& group) const = 0;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..281a77d448668fc9bdb4422dbf2b1f09a46d256e
--- /dev/null
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -0,0 +1,70 @@
+#include "HumanGrouper.h"
+
+#include <unordered_set>
+
+#include "CombinedDistance.h"
+#include "ConvexHullGenerator.h"
+
+namespace armarx::navigation::human
+{
+    HumanGrouper::HumanGrouper(const GroupingSettings& settings) :
+        distanceFunction(std::make_unique<CombinedDistance>(settings.maxOrientationInfluence,
+                                                            settings.movementInfluence)),
+        shapeGenerator(std::make_unique<ConvexHullGenerator>()),
+        settings(settings)
+    {
+    }
+
+    void
+    HumanGrouper::updateHumans(const std::vector<human::Human>& newHumans)
+    {
+        currentHumans_ = newHumans;
+    }
+
+    std::vector<human::HumanGroup>
+    HumanGrouper::getCurrentGroups()
+    {
+        const DateTime now = DateTime::Now();
+
+        std::vector<human::HumanGroup> groups = {};
+
+        // maps index of human in currentHumans to index of group in groups
+        std::unordered_map<uint64_t, uint64_t> humanGroupMap;
+
+        for (uint64_t parentIdx = 0; parentIdx < currentHumans_.size(); parentIdx++)
+        {
+            const Human parent = currentHumans_.at(parentIdx);
+
+            // if no group exists yet for this human, create one just for them
+            if (humanGroupMap.count(parentIdx) == 0)
+            {
+                const human::HumanGroup group = {{{}}, {parent}, now};
+                groups.push_back(group);
+                humanGroupMap[parentIdx] = groups.size() - 1;
+            }
+
+            for (uint64_t childIdx = 0; childIdx < currentHumans_.size(); childIdx++)
+            {
+                const Human child = currentHumans_.at(childIdx);
+
+                if ((humanGroupMap.count(childIdx) == 0 ||
+                     humanGroupMap[childIdx] != humanGroupMap[parentIdx]) &&
+                    this->distanceFunction->computeDistance(parent, child) <
+                        settings.groupingThreshold)
+                {
+                    humanGroupMap[childIdx] = humanGroupMap[parentIdx];
+                    groups.at(humanGroupMap[childIdx])
+                        .humans.push_back(currentHumans_.at(childIdx));
+                }
+            }
+        }
+
+        for (human::HumanGroup& group : groups)
+        {
+            group.shape = shapeGenerator->createShape(group);
+        }
+
+        return groups;
+    }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
new file mode 100644
index 0000000000000000000000000000000000000000..fc3fe60dc8aab190d9f3cbe6d736db7915570560
--- /dev/null
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -0,0 +1,91 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+#include "GroupShapeGenerator.h"
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief Identifies social interaction groups in a collection of detected humans.
+     *
+     * Can identify social interaction groups in a collection of humans. The currently detected
+     * humans can be set by calling updateHumans. The grouper will also return groups of size 1,
+     * and every detected human will be in exactly one group.
+     */
+    class HumanGrouper
+    {
+        using Human = armarx::navigation::human::Human;
+
+    public:
+        /**
+         * @brief The GroupingSettings struct contains settings for group generation.
+         * This includes the influence of aligned orientation and how aggressively groups
+         * are generated.
+         */
+        struct GroupingSettings
+        {
+            // people strictly closer than this distance will be grouped
+            const double groupingThreshold = 2000;
+
+            // distances between humans will be scaled by up to this factor based on their orientation
+            const double maxOrientationInfluence = 1.5;
+
+            // distances between humans will be scaled by their movement difference depending on this factor
+            const double movementInfluence = 1.5;
+        };
+
+        /**
+         * @brief Creates a new HumanGrouper
+         * @param settings different parameters that will affect group generation
+         */
+        HumanGrouper(const GroupingSettings& settings);
+
+        /**
+         * @brief Sets the currently detected humans.
+         *
+         * Sets the currently detected humans. Only these humans will be assumed to currently exist
+         * when getCurrentGroups() is called the next time.
+         * @param newHumans a vector containing the most recently detected humans
+         */
+        void updateHumans(const std::vector<Human>& newHumans);
+
+        /**
+         * @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. Will also return groups of size one for single people.
+         * @return a vector containing the recognized human groups
+         */
+        std::vector<human::HumanGroup> getCurrentGroups();
+
+    private:
+        const std::unique_ptr<DistanceFunction> distanceFunction;
+        const std::unique_ptr<GroupShapeGenerator> shapeGenerator;
+        const GroupingSettings settings;
+
+        std::vector<Human> currentHumans_;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/MovementDistance.cpp b/source/armarx/navigation/human/MovementDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a71bda4df7a3766904251abf9d4a1b5ff6667289
--- /dev/null
+++ b/source/armarx/navigation/human/MovementDistance.cpp
@@ -0,0 +1,12 @@
+#include "MovementDistance.h"
+
+namespace armarx::navigation::human
+{
+    MovementDistance::MovementDistance() = default;
+
+    double
+    MovementDistance::computeDistance(const Human& h1, const Human& h2) const
+    {
+        return (h1.linearVelocity - h2.linearVelocity).norm();
+    }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/MovementDistance.h b/source/armarx/navigation/human/MovementDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..57fb065fb949a8f5fc571514e51ccb00144ed6ed
--- /dev/null
+++ b/source/armarx/navigation/human/MovementDistance.h
@@ -0,0 +1,40 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief A DistanceFunction that defines the distance between two humans as the difference
+     * in velocity (that is, the norm of the linear velocity difference vector).
+     */
+    class MovementDistance : public DistanceFunction
+    {
+    public:
+        MovementDistance();
+
+        double computeDistance(const Human& h1, const Human& h2) const override;
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..07f98cf2df9aeda6b63213d95b17b86f41c0d753
--- /dev/null
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -0,0 +1,58 @@
+#include "OrientationDistance.h"
+
+#include <boost/math/constants/constants.hpp>
+#include <math.h>
+
+namespace armarx::navigation::human
+{
+    OrientationDistance::OrientationDistance(const double max, const double min) :
+        max(max), min(min)
+    {
+    }
+
+    double
+    OrientationDistance::computeDistance(const Human& h1, const Human& h2) const
+    {
+        // ranges from 0 to 1
+        const double factor = getOrientationFactor(h1, h2);
+
+        return min + factor * (max - min);
+    }
+
+
+    double
+    OrientationDistance::getOrientationFactor(const Human& h1, const Human& h2)
+    {
+        const double lineOrientation = getLineOrientation(h2, h1);
+        const double angleH1 = normOrientation(Eigen::Rotation2Dd(h1.pose.linear()).angle() / M_PI);
+        const double angleH2 = normOrientation(Eigen::Rotation2Dd(h2.pose.linear()).angle() / M_PI);
+
+        // assuming the angles to be in the interval [0, 2pi]
+        const double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
+        const double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
+
+        // ranges from 0 (looking directly at each other) to 1 (back to back)
+        return (deviationFirst + deviationSecond) / 2;
+    }
+
+    double
+    OrientationDistance::getLineOrientation(const Human& h1, const Human& h2)
+    {
+        const double dx = (h1.pose.translation() - h2.pose.translation()).x();
+        const double dy = (h1.pose.translation() - h2.pose.translation()).y();
+        const double lineOrientation = atan2(dy, dx) / M_PI;
+
+        return lineOrientation;
+    }
+
+    // brings orientation to (-1, 1]
+    double
+    OrientationDistance::normOrientation(const double orientation)
+    {
+        // brings orientation to [0, 2)
+        double convertedOrientation = std::fmod(((std::fmod(orientation, 2)) + 2), 2);
+        // brings orientation to [-1, 1)
+        convertedOrientation -= static_cast<int>(convertedOrientation > 1) * 2;
+        return convertedOrientation;
+    }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/OrientationDistance.h b/source/armarx/navigation/human/OrientationDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b8175e6c94e8b79cff9ec5d42c43342188a601e
--- /dev/null
+++ b/source/armarx/navigation/human/OrientationDistance.h
@@ -0,0 +1,60 @@
+/**
+ * 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
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::human
+{
+    /**
+     * @brief The OrientationDistance class is a DistanceFunction that computes the distance between
+     * the two humans based upon their orientation towards or away from each other.
+     *
+     * This distance function will return
+     * - the specified minimum distance (0 by default) if the humans are exactly facing each other
+     * - the specified maximum distance (1 by default) if the humans are oriented back to back
+     * - a linear interpolation between the two for anything inbetween (for instance, two people
+     *   looking in the same direction will have a distance equal to the arithmetic mean of the
+     *   minimum and maximum distance, 0.5 by default)
+     */
+    class OrientationDistance : public DistanceFunction
+    {
+    public:
+        /**
+         * @brief OrientationDistance Creates a new OrientationDistance with the specified range
+         * @param max the distance two people will have when faced back to back
+         * @param min the distance two people will have when oriented face to face
+         */
+        OrientationDistance(double max = 1, double min = 0);
+        double computeDistance(const Human& h1, const Human& h2) const override;
+
+    private:
+        const double max;
+        const double min;
+
+        // helper methods
+        static double getOrientationFactor(const Human& h1, const Human& h2);
+        static double getLineOrientation(const Human& h2, const Human& h1);
+        static double normOrientation(double orientation);
+    };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..862c7e3b28abe6bd7e5fd1a10a8c0354aede4a27
--- /dev/null
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -0,0 +1,308 @@
+/**
+ * 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       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <math.h>
+
+#include <algorithm>
+#include <vector>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/human/EuclideanDistance.h>
+#include <armarx/navigation/human/OrientationDistance.h>
+#include <range/v3/view/zip.hpp>
+
+// test includes and other stuff
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+#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>
+
+using armarx::navigation::core::Pose2D;
+using armarx::navigation::human::CombinedDistance;
+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;
+
+BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
+{
+    const EuclideanDistance distance = EuclideanDistance();
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(3, 4);
+    pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(6, 8);
+    pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    BOOST_CHECK_CLOSE(d, 5, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
+{
+    const OrientationDistance distance = OrientationDistance(1, 0);
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(0, 0);
+    pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(1, 0);
+    pose2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    BOOST_CHECK(d < 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testOrientationDistance2_backToBack)
+{
+    const OrientationDistance distance = OrientationDistance(7, 0);
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(2, 2);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 3);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    BOOST_CHECK_CLOSE(d, 7, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
+{
+    const MovementDistance distance = MovementDistance();
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(-2, 3);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 5);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    BOOST_CHECK_CLOSE(d, 0.5, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
+{
+    const CombinedDistance combined = CombinedDistance(2, 3);
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(0, 0);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 0);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = combined.computeDistance(h1, h2);
+    BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator1_triangle)
+{
+    ConvexHullGenerator generator = ConvexHullGenerator();
+
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(-2, 0);
+    pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 0);
+    pose2.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose3 = Pose2D::Identity();
+    pose3.translation() = Eigen::Vector2f(0, 2);
+    pose3.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h3 = {.pose = pose3,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose4 = Pose2D::Identity();
+    pose4.translation() = Eigen::Vector2f(0, 1);
+    pose4.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h4 = {.pose = pose4,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+
+    std::vector<Human> humans = {h1, h2, h3, h4};
+
+    HumanGroup group = {.shape = {{}}, .humans = humans, .detectionTime = armarx::DateTime::Now()};
+
+    Polygon hull = generator.createShape(group);
+
+    // Check that the vertices of the polygon contain all 3 positions from the outer 3 people
+    BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose1.translation()) !=
+                hull.vertices.end());
+
+    BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose2.translation()) !=
+                hull.vertices.end());
+
+    BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose3.translation()) !=
+                hull.vertices.end());
+
+
+    // Check that the hull doesn't contain the inner person
+    BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose4.translation()) ==
+                hull.vertices.end());
+}
+
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator2_line)
+{
+    ConvexHullGenerator generator = ConvexHullGenerator();
+
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(-2, 0);
+    pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 0);
+    pose2.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0, 0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    std::vector<Human> humans = {h1, h2};
+
+    HumanGroup group = {.shape = {{}}, .humans = humans, .detectionTime = armarx::DateTime::Now()};
+
+    Polygon hull = generator.createShape(group);
+
+    // Check that the vertices of the polygon contains both positions
+    BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose1.translation()) !=
+                hull.vertices.end());
+
+    BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose2.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());
+
+    // Check that a shape was set (the detailed convex hull generation is tested seperatly)
+    BOOST_CHECK(groupOfTwo.shape.vertices.size() > 0);
+}
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
index 322021491589c466cccc1981fe946819c4caf812..0e45d1a8da21fb717e8177d3c6c0fe1ff869fa72 100644
--- a/source/armarx/navigation/human/test/human_tracker_test.cpp
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -239,7 +239,7 @@ namespace armarx::navigation::human
 
         BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 1);
 
-        CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000))};
+        CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000)), .humanPoses = {}};
         tracker.update(late);
 
         BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 0);
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
index 854b0d15569474955bde4550b77c6a168aa8238e..b2ec8b130fb1fa0ccc0090463906b6807c271721 100644
--- a/source/armarx/navigation/human/types.cpp
+++ b/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
     {
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 0c3e08043b9fcea45aa603a0ef01d0eeba87f63d..cbb1905dbc3fa41c1973d153c511606891c7f891 100644
--- a/source/armarx/navigation/human/types.h
+++ b/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);
     };
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 267142f9ea3e658d9c9b1770476061f988453565..9059612ce21b6425a880412b2e526a519038ac2c 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -11,6 +11,8 @@ armarx_add_library(memory
         client/costmap/Reader.cpp
         client/human/Reader.cpp
         client/human/Writer.cpp
+        client/human_group/Reader.cpp
+        client/human_group/Writer.cpp
         # ./client/events/Reader.cpp
     HEADERS
         memory.h
@@ -24,6 +26,8 @@ armarx_add_library(memory
         client/costmap/Reader.h
         client/human/Reader.h
         client/human/Writer.h
+        client/human_group/Reader.h
+        client/human_group/Writer.h
         # ./client/events/Reader.h
     DEPENDENCIES
         ArmarXCoreInterfaces
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
index f1ad2e2b96594dcf79c9b7ce5932de2fcccd320f..905eb69e53dff5ee0ea06ac0efa2bbb09e2cc980 100644
--- a/source/armarx/navigation/memory/client/human/Reader.cpp
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -41,23 +41,6 @@ namespace armarx::navigation::memory::client::human
         return qb;
     }
 
-    armarx::armem::client::query::Builder
-    Reader::buildHumanGroupsQuery(const Query& query) const
-    {
-        armarx::armem::client::query::Builder qb;
-
-        // clang-format off
-        qb
-        .coreSegments().withName(properties().coreSegmentName)
-        .providerSegments().withName(query.providerName)
-        .entities().withName("groups")
-        .snapshots().beforeOrAtTime(query.timestamp);
-        // clang-format on
-
-        return qb;
-    }
-
-
     std::string
     Reader::propertyPrefix() const
     {
@@ -107,95 +90,6 @@ namespace armarx::navigation::memory::client::human
         return humans;
     }
 
-    navigation::human::HumanGroups
-    asGroups(const armem::wm::ProviderSegment& providerSegment,
-             const DateTime& timestamp,
-             const Duration& maxAge)
-    {
-        navigation::human::HumanGroups humans;
-
-        ARMARX_CHECK(not providerSegment.empty()) << "No entities";
-        ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
-
-        providerSegment.forEachEntity(
-            [&humans, &timestamp, &maxAge](const armem::wm::Entity& entity)
-            {
-                const auto& entitySnapshot = entity.getLatestSnapshot();
-                ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
-
-                entitySnapshot.forEachInstance(
-                    [&](const armem::wm::EntityInstance& entityInstance)
-                    {
-                        const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
-
-                        if (dtToNow < maxAge and dtToNow.isPositive())
-                        {
-                            const auto dto = navigation::human::arondto::HumanGroup::FromAron(
-                                entityInstance.data());
-
-                            navigation::human::HumanGroup human;
-                            fromAron(dto, human);
-                            humans.push_back(human);
-                        }
-                    });
-            });
-
-        return humans;
-    }
-
-    Reader::HumanGroupResult
-    Reader::queryHumanGroups(const Query& query) const
-    {
-        const auto qb = buildHumansQuery(query);
-
-        ARMARX_DEBUG << "[MappingDataReader] query ... ";
-
-        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
-
-        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
-
-        if (not qResult.success)
-        {
-            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
-            return {.groups = {},
-                    .status = HumanGroupResult::Status::Error,
-                    .errorMessage = qResult.errorMessage};
-        }
-
-        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
-
-        if (not coreSegment.hasProviderSegment(query.providerName))
-        {
-            ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
-            return {.groups = {}, .status = HumanGroupResult::Status::NoData};
-        }
-
-        const armem::wm::ProviderSegment& providerSegment =
-            coreSegment.getProviderSegment(query.providerName);
-
-        if (providerSegment.empty())
-        {
-            ARMARX_DEBUG << "No entities.";
-            return {.groups = {},
-                    .status = HumanGroupResult::Status::NoData,
-                    .errorMessage = "No entities"};
-        }
-
-        try
-        {
-            return HumanGroupResult{.groups =
-                                        asGroups(providerSegment, query.timestamp, query.maxAge),
-                                    .status = HumanGroupResult::Status::Success};
-        }
-        catch (...)
-        {
-            return HumanGroupResult{.groups = {},
-                                    .status = HumanGroupResult::Status::Error,
-                                    .errorMessage = GetHandledExceptionString()};
-        }
-    }
-
-
     Reader::HumanResult
     Reader::queryHumans(const Query& query) const
     {
diff --git a/source/armarx/navigation/memory/client/human/Reader.h b/source/armarx/navigation/memory/client/human/Reader.h
index 91a17afaad4cff3b4a15ad7bc11b9be0b1cb1fd2..3a47556a072f3528cc6567d5dec1ce91ee3a8ba6 100644
--- a/source/armarx/navigation/memory/client/human/Reader.h
+++ b/source/armarx/navigation/memory/client/human/Reader.h
@@ -65,31 +65,10 @@ namespace armarx::navigation::memory::client::human
             }
         };
 
-        struct HumanGroupResult
-        {
-            armarx::navigation::human::HumanGroups groups;
-
-            enum Status
-            {
-                Success,
-                NoData,
-                Error
-            } status;
-
-            std::string errorMessage = "";
-
-            operator bool() const noexcept
-            {
-                return status == Status::Success;
-            }
-        };
-
         HumanResult queryHumans(const Query& query) const;
-        HumanGroupResult queryHumanGroups(const Query& query) const;
 
     protected:
         ::armarx::armem::client::query::Builder buildHumansQuery(const Query& query) const;
-        ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
 
         std::string propertyPrefix() const override;
         Properties defaultProperties() const override;
diff --git a/source/armarx/navigation/memory/client/human/Writer.cpp b/source/armarx/navigation/memory/client/human/Writer.cpp
index c38c2ac775e064890eecbf12d95c55ce017fb4a8..97767d14c5901e39f1f3cc8d309e324c0437d6e7 100644
--- a/source/armarx/navigation/memory/client/human/Writer.cpp
+++ b/source/armarx/navigation/memory/client/human/Writer.cpp
@@ -6,8 +6,8 @@
 #include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
 #include <armarx/navigation/algorithms/aron_conversions.h>
 #include <armarx/navigation/human/aron/Human.aron.generated.h>
-#include <armarx/navigation/memory/constants.h>
 #include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
 
 
 namespace armarx::navigation::memory::client::human
@@ -42,62 +42,12 @@ namespace armarx::navigation::memory::client::human
         std::transform(humans.begin(),
                        humans.end(),
                        std::back_inserter(update.instancesData),
-                       [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr {
-                            navigation::human::arondto::Human dto;
-                            toAron(dto, human);
-
-                            return dto.toAron();
-                       });
-
-
-        update.timeCreated = timestamp;
-
-        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
-        armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
-
-        ARMARX_DEBUG << updateResult;
-
-        if (not updateResult.success)
-        {
-            ARMARX_ERROR << updateResult.errorMessage;
-        }
-
-        return updateResult.success;
-    }
-
-    bool
-    Writer::store(const armarx::navigation::human::HumanGroups& groups,
-                  //   const std::string& name,
-                  const std::string& providerName,
-                  const armem::Time& timestamp)
-    {
-        std::lock_guard g{memoryWriterMutex()};
-
-        const auto result =
-            memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
-
-        if (not result.success)
-        {
-            ARMARX_ERROR << result.errorMessage;
-
-            // TODO(fabian.reister): message
-            return false;
-        }
-
-        const auto providerId = armem::MemoryID(result.segmentID);
-        const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
-
-        armem::EntityUpdate update;
-        update.entityID = entityID;
-
-        std::transform(groups.begin(),
-                       groups.end(),
-                       std::back_inserter(update.instancesData),
-                       [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr {
-                            navigation::human::arondto::HumanGroup dto;
-                            toAron(dto, group);
+                       [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr
+                       {
+                           navigation::human::arondto::Human dto;
+                           toAron(dto, human);
 
-                            return dto.toAron();
+                           return dto.toAron();
                        });
 
 
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
index b224149c4aeb3c4858530f19ed051700060fc179..9afb179ae78b87bc808b6f87cf1919625278957b 100644
--- a/source/armarx/navigation/memory/client/human/Writer.h
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -51,12 +51,7 @@ namespace armarx::navigation::memory::client::human
         ~Writer() override;
 
         bool store(const armarx::navigation::human::Humans& humans,
-                //    const std::string& name,
-                   const std::string& providerName,
-                   const armem::Time& timestamp);
-
-        bool store(const armarx::navigation::human::HumanGroups& groups,
-                //    const std::string& name,
+                   //    const std::string& name,
                    const std::string& providerName,
                    const armem::Time& timestamp);
 
diff --git a/source/armarx/navigation/memory/client/human_group/Reader.cpp b/source/armarx/navigation/memory/client/human_group/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f6c8ad53f7ca17c1d7139384e8a805a06527f9a
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+    Reader::~Reader() = default;
+
+    armarx::armem::client::query::Builder
+    Reader::buildHumanGroupsQuery(const Query& query) const
+    {
+        armarx::armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties().coreSegmentName)
+        .providerSegments().withName(query.providerName)
+        .entities().withName("groups")
+        .snapshots().beforeOrAtTime(query.timestamp);
+        // clang-format on
+
+        return qb;
+    }
+
+
+    std::string
+    Reader::propertyPrefix() const
+    {
+        return "mem.nav.human_group.";
+    }
+
+    armarx::armem::client::util::SimpleReaderBase::Properties
+    Reader::defaultProperties() const
+    {
+        return {.memoryName = memory::constants::NavigationMemoryName,
+                .coreSegmentName = memory::constants::HumanGroupCoreSegmentName};
+    }
+
+    navigation::human::HumanGroups
+    asGroups(const armem::wm::ProviderSegment& providerSegment,
+             const DateTime& timestamp,
+             const Duration& maxAge)
+    {
+        navigation::human::HumanGroups humanGroups;
+
+        ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+        ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+        providerSegment.forEachEntity(
+            [&humanGroups, &timestamp, &maxAge](const armem::wm::Entity& entity)
+            {
+                const auto& entitySnapshot = entity.getLatestSnapshot();
+                ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+                entitySnapshot.forEachInstance(
+                    [&](const armem::wm::EntityInstance& entityInstance)
+                    {
+                        const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
+
+                        if (dtToNow < maxAge and dtToNow.isPositive())
+                        {
+                            const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+                                entityInstance.data());
+
+                            navigation::human::HumanGroup humanGroup;
+                            fromAron(dto, humanGroup);
+                            humanGroups.push_back(humanGroup);
+                        }
+                    });
+            });
+
+        return humanGroups;
+    }
+
+    Reader::HumanGroupResult
+    Reader::queryHumanGroups(const Query& query) const
+    {
+        const auto qb = buildHumanGroupsQuery(query);
+
+        ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+        if (not qResult.success)
+        {
+            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+            return {.groups = {},
+                    .status = HumanGroupResult::Status::Error,
+                    .errorMessage = qResult.errorMessage};
+        }
+
+        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+        if (not coreSegment.hasProviderSegment(query.providerName))
+        {
+            ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
+            return {.groups = {}, .status = HumanGroupResult::Status::NoData};
+        }
+
+        const armem::wm::ProviderSegment& providerSegment =
+            coreSegment.getProviderSegment(query.providerName);
+
+        if (providerSegment.empty())
+        {
+            ARMARX_DEBUG << "No entities.";
+            return {.groups = {},
+                    .status = HumanGroupResult::Status::NoData,
+                    .errorMessage = "No entities"};
+        }
+
+        try
+        {
+            return HumanGroupResult{.groups =
+                                        asGroups(providerSegment, query.timestamp, query.maxAge),
+                                    .status = HumanGroupResult::Status::Success};
+        }
+        catch (...)
+        {
+            return HumanGroupResult{.groups = {},
+                                    .status = HumanGroupResult::Status::Error,
+                                    .errorMessage = GetHandledExceptionString()};
+        }
+    }
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Reader.h b/source/armarx/navigation/memory/client/human_group/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..537ac74d20f17ecf4253cef01444929b46a74348
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Reader.h
@@ -0,0 +1,77 @@
+/*
+ * 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     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+
+    class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+    {
+    public:
+        using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+        ~Reader() override;
+
+        struct Query
+        {
+            std::string providerName;
+            armem::Time timestamp;
+            Duration maxAge;
+        };
+
+        struct HumanGroupResult
+        {
+            armarx::navigation::human::HumanGroups groups;
+
+            enum Status
+            {
+                Success,
+                NoData,
+                Error
+            } status;
+
+            std::string errorMessage = "";
+
+            operator bool() const noexcept
+            {
+                return status == Status::Success;
+            }
+        };
+
+        HumanGroupResult queryHumanGroups(const Query& query) const;
+
+    protected:
+        ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
+
+        std::string propertyPrefix() const override;
+        Properties defaultProperties() const override;
+    };
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Writer.cpp b/source/armarx/navigation/memory/client/human_group/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d38c45d4608672a5913f5a7cdc94329f2e34626
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Writer.cpp
@@ -0,0 +1,83 @@
+#include "Writer.h"
+
+#include <iterator>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+
+namespace armarx::navigation::memory::client::human_group
+{
+    Writer::~Writer() = default;
+
+    bool
+    Writer::store(const armarx::navigation::human::HumanGroups& groups,
+                  //   const std::string& name,
+                  const std::string& providerName,
+                  const armem::Time& timestamp)
+    {
+        std::lock_guard g{memoryWriterMutex()};
+
+        const auto result =
+            memoryWriter().addSegment(memory::constants::HumanGroupCoreSegmentName, providerName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+
+            // TODO(fabian.reister): message
+            return false;
+        }
+
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
+
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+
+        std::transform(groups.begin(),
+                       groups.end(),
+                       std::back_inserter(update.instancesData),
+                       [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr
+                       {
+                           navigation::human::arondto::HumanGroup dto;
+                           toAron(dto, group);
+
+                           return dto.toAron();
+                       });
+
+
+        update.timeCreated = timestamp;
+
+        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+        armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+        ARMARX_DEBUG << updateResult;
+
+        if (not updateResult.success)
+        {
+            ARMARX_ERROR << updateResult.errorMessage;
+        }
+
+        return updateResult.success;
+    }
+
+    std::string
+    Writer::propertyPrefix() const
+    {
+        return "mem.nav.human_group.";
+    }
+
+    armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+    Writer::defaultProperties() const
+    {
+        return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+                                            .coreSegmentName =
+                                                memory::constants::HumanGroupCoreSegmentName};
+    }
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Writer.h b/source/armarx/navigation/memory/client/human_group/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3de9d343a4c83c95ada602d146c339e361fa86b
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Writer.h
@@ -0,0 +1,64 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+
+    /**
+    * @defgroup Component-ExampleClient ExampleClient
+    * @ingroup RobotAPI-Components
+    * A description of the component ExampleClient.
+    *
+    * @class ExampleClient
+    * @ingroup Component-ExampleClient
+    * @brief Brief description of class ExampleClient.
+    *
+    * Detailed description of class ExampleClient.
+    */
+    class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+    {
+    public:
+        using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+        ~Writer() override;
+
+        bool store(const armarx::navigation::human::HumanGroups& groups,
+                   //    const std::string& name,
+                   const std::string& providerName,
+                   const armem::Time& timestamp);
+
+    protected:
+        std::string propertyPrefix() const override;
+        Properties defaultProperties() const override;
+    };
+
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index f0932218de3bba512f5bbabaafba864e2e1afc37..23c01a1c112d74e1345f7e8f58bfe96ec1af190d 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,6 +33,7 @@ namespace armarx::navigation::memory::constants
     inline const std::string LocationCoreSegmentName = "Location";
     inline const std::string CostmapCoreSegmentName = "Costmap";
     inline const std::string HumanCoreSegmentName = "Human";
+    inline const std::string HumanGroupCoreSegmentName = "HumanGroup";
 
     inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
     inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";