Skip to content
Snippets Groups Projects

Social layers

Open Social layers
feature/store-proxemics-in-memory into master
Open Fabian Reister requested to merge feature/store-proxemics-in-memory into master
Viewing commit 2979fef6
Prev Next
Show latest version
1 file
+ 137
− 113
Compare changes
  • Side-by-side
  • Inline
source/armarx/navigation/human/test/human_test.cpp
+ 137
− 113
@@ -28,9 +28,8 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <SemanticObjectRelations/Shapes/Shape.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/human/EuclideanDistance.h>
#include <range/v3/view/zip.hpp>
// test includes and other stuff
@@ -38,119 +37,144 @@
#define ARMARX_BOOST_TEST
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/types.h>
BOOST_AUTO_TEST_CASE(testPathLength)
{
armarx::navigation::core::Path path{
armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
}
using armarx::navigation::core::Pose2D;
using armarx::navigation::human::EuclideanDistance;
using armarx::navigation::human::Human;
BOOST_AUTO_TEST_CASE(testResampleAlongLine)
BOOST_AUTO_TEST_CASE(testEuclideanDistance)
{
armarx::navigation::core::Path path{
armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
BOOST_CHECK_EQUAL(traj.points().size(), 2);
const auto resampledTraj = traj.resample(500);
for (const auto& pt : resampledTraj.positions())
{
ARMARX_DEBUG << VAROUT(pt);
}
BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
EuclideanDistance distance = EuclideanDistance();
Pose2D pose1 = Pose2D::Identity();
// pose1.translation() = Eigen::Vector2f(3, 4);
// pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
// 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();
// Human h2 = {.pose = pose2,
// .linearVelocity = Eigen::Vector2f(0.5, 0.8),
// .detectionTime = armarx::DateTime::Now()};
}
BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
{
armarx::navigation::core::Path path{
armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
BOOST_CHECK_EQUAL(traj.points().size(), 3);
const auto resampledTraj = traj.resample(500);
for (const auto& pt : resampledTraj.positions())
{
ARMARX_DEBUG << VAROUT(pt);
}
BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
}
BOOST_AUTO_TEST_CASE(testGraphRoutes)
{
using namespace armarx::navigation;
/*
*
* (0) -> (2) -> (3)
* ^
* |
* (1)
*/
core::Graph graph;
graph.addVertex(semrel::ShapeID(0));
graph.addVertex(semrel::ShapeID(1));
graph.addVertex(semrel::ShapeID(2));
graph.addVertex(semrel::ShapeID(3));
graph.addEdge(semrel::ShapeID(0), semrel::ShapeID(2));
graph.addEdge(semrel::ShapeID(1), semrel::ShapeID(2));
graph.addEdge(semrel::ShapeID(2), semrel::ShapeID(3));
const auto paths =
armarx::navigation::core::findPathsTo(graph.vertex(semrel::ShapeID(3)), graph);
// GT: paths
// - {3}
// - {2,3}
// - {0,2,3}
// - {1,2,3}
BOOST_CHECK_EQUAL(paths.size(), 4);
std::vector<std::vector<int>> gtPaths{{3}, {2, 3}, {0, 2, 3}, {1, 2, 3}};
const auto isMatchingPath = [](const auto& path, const auto& gtPath)
{
if (path.size() != gtPath.size())
{
return false;
}
// check that sequences match
for (const auto& [v, gtId] : ranges::views::zip(path, gtPath))
{
if (v.objectID().t != gtId)
{
return false;
}
}
ARMARX_INFO << "Found matching path: " << VAROUT(path) << ", " << VAROUT(gtPath);
return true;
};
// check that all required paths are available
for (const auto& gtPath : gtPaths)
{
BOOST_CHECK(std::any_of(paths.begin(),
paths.end(),
[&gtPath, &isMatchingPath](const auto& path)
{ return isMatchingPath(path, gtPath); }));
}
}
//BOOST_AUTO_TEST_CASE(testPathLength)
//{
// armarx::navigation::core::Path path{
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
// BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
//}
//BOOST_AUTO_TEST_CASE(testResampleAlongLine)
//{
// armarx::navigation::core::Path path{
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
// BOOST_CHECK_EQUAL(traj.points().size(), 2);
// const auto resampledTraj = traj.resample(500);
// for (const auto& pt : resampledTraj.positions())
// {
// ARMARX_DEBUG << VAROUT(pt);
// }
// BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
//}
//BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
//{
// armarx::navigation::core::Path path{
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
// BOOST_CHECK_EQUAL(traj.points().size(), 3);
// const auto resampledTraj = traj.resample(500);
// for (const auto& pt : resampledTraj.positions())
// {
// ARMARX_DEBUG << VAROUT(pt);
// }
// BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
//}
//BOOST_AUTO_TEST_CASE(testGraphRoutes)
//{
// using namespace armarx::navigation;
// /*
// *
// * (0) -> (2) -> (3)
// * ^
// * |
// * (1)
// */
// core::Graph graph;
// graph.addVertex(semrel::ShapeID(0));
// graph.addVertex(semrel::ShapeID(1));
// graph.addVertex(semrel::ShapeID(2));
// graph.addVertex(semrel::ShapeID(3));
// graph.addEdge(semrel::ShapeID(0), semrel::ShapeID(2));
// graph.addEdge(semrel::ShapeID(1), semrel::ShapeID(2));
// graph.addEdge(semrel::ShapeID(2), semrel::ShapeID(3));
// const auto paths =
// armarx::navigation::core::findPathsTo(graph.vertex(semrel::ShapeID(3)), graph);
// // GT: paths
// // - {3}
// // - {2,3}
// // - {0,2,3}
// // - {1,2,3}
// BOOST_CHECK_EQUAL(paths.size(), 4);
// std::vector<std::vector<int>> gtPaths{{3}, {2, 3}, {0, 2, 3}, {1, 2, 3}};
// const auto isMatchingPath = [](const auto& path, const auto& gtPath)
// {
// if (path.size() != gtPath.size())
// {
// return false;
// }
// // check that sequences match
// for (const auto& [v, gtId] : ranges::views::zip(path, gtPath))
// {
// if (v.objectID().t != gtId)
// {
// return false;
// }
// }
// ARMARX_INFO << "Found matching path: " << VAROUT(path) << ", " << VAROUT(gtPath);
// return true;
// };
// // check that all required paths are available
// for (const auto& gtPath : gtPaths)
// {
// BOOST_CHECK(std::any_of(paths.begin(),
// paths.end(),
// [&gtPath, &isMatchingPath](const auto& path)
// { return isMatchingPath(path, gtPath); }));
// }
//}