Add conversion test

source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp

@@ -27,11 +27,42 @@
 #include <RobotAPI/Test.h>
 #include <RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h>
 
+#include <RobotAPI/components/ObjectPoseObserver/ice_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
 #include <iostream>
 
-BOOST_AUTO_TEST_CASE(testExample)
+using namespace armarx;
+
+
+BOOST_AUTO_TEST_CASE(test_from_to_OOBB)
 {
-    armarx::ObjectPoseObserver instance;
+    Eigen::Vector3f pos(-100, -200, -300);
+    Eigen::Matrix3f ori = Eigen::AngleAxisf(1.0, Eigen::Vector3f(1, 2, 3).normalized()).toRotationMatrix();
+    Eigen::Vector3f extents(40, 50, 60);
+
+    armarx::objpose::Box box;
+    box.position = new Vector3(pos);
+    box.orientation = new Quaternion(ori);
+    box.extents = new Vector3(extents);
+
+
+    const float prec = 1e-3;
+
+    simox::OrientedBoxf oobb;
+    armarx::objpose::fromIce(box, oobb);
+    ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec);
+    ARMARX_CHECK(oobb.rotation().isApprox(ori, prec));
+    ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec);
+
+    armarx::objpose::Box boxOut;
+    armarx::objpose::toIce(boxOut, oobb);
+
+    Eigen::Vector3f posOut = Vector3Ptr::dynamicCast(boxOut.position)->toEigen();
+    Eigen::Matrix3f oriOut = QuaternionPtr::dynamicCast(boxOut.orientation)->toEigen();
+    Eigen::Vector3f extentsOut = Vector3Ptr::dynamicCast(boxOut.extents)->toEigen();
 
-    BOOST_CHECK_EQUAL(true, true);
+    ARMARX_CHECK_LESS_EQUAL((posOut - pos).norm(), prec);
+    ARMARX_CHECK(oriOut.isApprox(ori, prec));
+    ARMARX_CHECK_LESS_EQUAL((extentsOut - extents).norm(), prec);
 }