Fix bugs in OrientationDistance

source/armarx/navigation/human/OrientationDistance.cpp

@@ -24,8 +24,8 @@ namespace armarx::navigation::human
     OrientationDistance::getOrientationFactor(const Human& h1, const Human& h2)
     {
         const double lineOrientation = getLineOrientation(h2, h1);
-        const double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
-        const double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
+        const double angleH1 = normOrientation(Eigen::Rotation2Dd(h1.pose.linear()).angle() / M_PI);
+        const double angleH2 = normOrientation(Eigen::Rotation2Dd(h2.pose.linear()).angle() / M_PI);
 
         printf("line orientation = %.2f\n", lineOrientation);
         printf("angle h1 = %.2f, angle h2 = %.2f\n", angleH1, angleH2);
@@ -41,22 +41,21 @@ namespace armarx::navigation::human
     }
 
     double
-    OrientationDistance::getLineOrientation(const Human& h2, const Human& h1)
+    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);
+        const double lineOrientation = atan2(dy, dx) / M_PI;
 
         return lineOrientation;
     }
 
-    // scales orientation to (-1, 1]
+    // brings orientation to (-1, 1]
     double
     OrientationDistance::normOrientation(const double orientation)
     {
-        double convertedOrientation = orientation / M_PI;
         // brings orientation to [0, 2)
-        convertedOrientation = std::fmod(((std::fmod(convertedOrientation, 2)) + 2), 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;

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

@@ -30,7 +30,9 @@
 #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>
+#include <math.h>
 
 // test includes and other stuff
 #define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
@@ -38,11 +40,11 @@
 
 #include <armarx/navigation/Test.h>
 #include <armarx/navigation/core/basic_types.h>
-#include <armarx/navigation/human/OrientationDistance.h>
 #include <armarx/navigation/human/types.h>
 
 using armarx::navigation::core::Pose2D;
 using armarx::navigation::human::EuclideanDistance;
+using armarx::navigation::human::OrientationDistance;
 using armarx::navigation::human::Human;
 
 BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -63,28 +65,31 @@ BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
                       .detectionTime = armarx::DateTime::Now()};
 
     double d = distance.computeDistance(h1, h2);
-    ARMARX_INFO << "Distance is" + std::to_string(d);
+    printf("Distance is %.2f\n", d);
     BOOST_CHECK_CLOSE(d, 5, 0.001);
 }
 
-//BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
-//{
-//    const OrientationDistance distance = OrientationDistance();
-//    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(2.3).toRotationMatrix();
-//    const Human h2 = {.pose = pose2,
-//                      .linearVelocity = Eigen::Vector2f(0.5, 0.8),
-//                      .detectionTime = armarx::DateTime::Now()};
+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);
+    printf("Distance is %.2f\n", d);
+    BOOST_CHECK(d < 0.001);
+}
 
 //BOOST_AUTO_TEST_CASE(testPathLength)
 //{