Draft: Implement human grouping

Closes #34

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

+/**
+ * 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);
+}