Skip to content
Snippets Groups Projects

Draft: Implement human grouping

Open Timo Weberruß requested to merge implement-human-groups into dev
4 files
+ 5
8
Compare changes
  • Side-by-side
  • Inline
Files
4
/**
* 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);
}
Loading