diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp index feaf2f203aca7445081f2f91e3d511a7b5bbdbd5..421f3b5d0181639d5634ccb5becae3d5deafbe69 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp +++ b/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; }