Merge branch 'ObjectPoseObserver' into ObjectPoseObserver-gui

source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp

@@ -86,10 +86,11 @@ namespace armarx
 
         simox::AxisAlignedBoundingBox aabb(min, max);
 
-        ARMARX_CHECK(aabb.center().isApprox(center)) << aabb.center().transpose() << "\n" << center.transpose();
-        ARMARX_CHECK(aabb.extents().isApprox(extents)) << aabb.extents().transpose() << "\n" << extents.transpose();
-        ARMARX_CHECK(aabb.min().isApprox(min)) << aabb.min().transpose() << "\n" << min.transpose();
-        ARMARX_CHECK(aabb.max().isApprox(max)) << aabb.max().transpose() << "\n" << max.transpose();
+        static const float prec = 1e-4f;
+        ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose();
+        ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose();
+        ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose();
+        ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose();
 
         return aabb;
     }
@@ -109,9 +110,10 @@ namespace armarx
                                        ori.col(1) * extents(1),
                                        ori.col(2) * extents(2));
 
-        ARMARX_CHECK(oobb.center().isApprox(pos)) << oobb.center().transpose() << "\n" << pos.transpose();
-        ARMARX_CHECK(oobb.rotation().isApprox(ori)) << oobb.rotation() << "\n" << ori;
-        ARMARX_CHECK(oobb.dimensions().isApprox(extents)) << oobb.dimensions().transpose() << "\n" << extents.transpose();
+        static const float prec = 1e-4f;
+        ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec) << oobb.center().transpose() << "\n" << pos.transpose();
+        ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori;
+        ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec) << oobb.dimensions().transpose() << "\n" << extents.transpose();
         return oobb;
     }