Add distance function and start impl (WIP!)

source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt

@@ -11,11 +11,18 @@ armarx_add_component(dynamic_scene_provider
         ArVizDrawer.cpp
         HumanTracker.cpp
         HumanGrouper.cpp
+        DistanceFunctionDecorator.cpp
+        OrientationDecorator.cpp
+        EuclideanDistance.cpp
     HEADERS
         Component.h
         ArVizDrawer.h
         HumanTracker.h
         HumanGrouper.h
+        DistanceFunction.h
+        DistanceFunctionDecorator.h
+        OrientationDecorator.h
+        EuclideanDistance.h
     DEPENDENCIES
         # ArmarXCore
         ArmarXCore

source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h

+#pragma once
+
+template <typename T>
+class DistanceFunction
+{
+public:
+    DistanceFunction();
+    virtual double computeDistance(T& t1, T& t2) = 0;
+};
+

source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp

+#include "DistanceFunctionDecorator.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+    template<typename T>
+    DistanceFunctionDecorator<T>::DistanceFunctionDecorator(DistanceFunction<T> &subject)
+    {
+        this->subject = subject;
+    }
+
+    template<typename T>
+    double DistanceFunctionDecorator<T>::computeDistance(T &t1, T &t2)
+    {
+        return subject.computeDistance(t1, t2);
+    }
+}

source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h

+#include "DistanceFunction.h"
+
+#pragma once
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+    template <typename T>
+    class DistanceFunctionDecorator : DistanceFunction<T>
+    {
+    public:
+        DistanceFunctionDecorator(DistanceFunction<T>& subject);
+        virtual double computeDistance(T& t1, T& t2);
+    private:
+        DistanceFunction<T> subject;
+    };
+}

source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp

+#include "DistanceFunction.h"
+#include "EuclideanDistance.h"
+
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+    using Human = armarx::navigation::human::Human;
+
+    double EuclideanDistance::computeDistance(Human &h1, Human &h2)
+    {
+        return (h1.pose.translation() - h2.pose.translation()).norm();
+    }
+}
+

source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h

+#include "DistanceFunction.h"
+
+#include "armarx/navigation/human/types.h"
+
+#pragma once
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+    using Human = armarx::navigation::human::Human;
+
+    class EuclideanDistance : DistanceFunction<Human>
+    {
+    public:
+        EuclideanDistance();
+        double computeDistance(Human &h1, Human &h2);
+    };
+}

source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp

@@ -9,7 +9,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
     void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
     {
-
+        currentHumans = newHumans;
     }
 
     std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()

source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h

@@ -27,10 +27,10 @@
 namespace armarx::navigation::components::dynamic_scene_provider
 {
     /**
+     * @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.
-     *
-     * @brief Identifies social interaction groups in a collection of detected humans.
      */
     class HumanGrouper
     {
@@ -41,21 +41,29 @@ namespace armarx::navigation::components::dynamic_scene_provider
         HumanGrouper();
 
         /**
+         * @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.
-         *
-         * @brief Sets the currently detected humans.
          * @param newHumans a vector containing the most recently detected humans
          */
         void updateHumans(std::vector<human::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.
-         *
-         * @brief Recognizes groups in the current humans.
          * @return a vector containing the recognized human groups
          */
         std::vector<human::HumanGroup> getCurrentGroups();
+
+        struct DistanceFunction
+        {
+
+        };
+
+    private:
+        std::vector<human::Human> currentHumans;
     };
 } // namespace armarx::navigation::components::dynamic_scene_provider

source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp

+#include "OrientationDecorator.h"
+
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+    OrientationDecorator::OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor)
+    : DistanceFunctionDecorator<Human>(subject)
+    {
+        this->influenceFactor = influenceFactor;
+    }
+
+    double OrientationDecorator::computeDistance(Human &t1, Human &t2)
+    {
+        double orientationFactor = 1; // TODO: implement
+        return 0;
+    }
+}

source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h

+#pragma once
+
+#include "DistanceFunctionDecorator.h"
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+    using Human = armarx::navigation::human::Human;
+
+    class OrientationDecorator : DistanceFunctionDecorator<Human>
+    {
+    public:
+        OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor);
+
+        virtual double computeDistance(Human &t1, Human &t2);
+
+    private:
+        double influenceFactor;
+    };
+}
+