From 68e8aedfd98cbec887b5d5eed37fd8b288c9f918 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 25 May 2020 14:21:24 +0200
Subject: [PATCH] Update OOBB loading
---
.../ObjectPoseObserver/ObjectFinder.cpp | 20 ++++++++++++++++---
1 file changed, 17 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
index eef7faa22..d24b27e90 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
@@ -246,26 +246,40 @@ namespace armarx
{
nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
nlohmann::json jaabb = j.at("aabb");
+
+ auto center = jaabb.at("center").get<Eigen::Vector3f>();
+ auto extents = jaabb.at("extents").get<Eigen::Vector3f>();
auto min = jaabb.at("min").get<Eigen::Vector3f>();
auto max = jaabb.at("max").get<Eigen::Vector3f>();
- return simox::AxisAlignedBoundingBox(min, max);
+ 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();
+
+ return aabb;
}
simox::OrientedBox<float> ObjectInfo::oobb() const
{
nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
nlohmann::json joobb = j.at("oobb");
+ auto pos = joobb.at("pos").get<Eigen::Vector3f>();
auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
- auto min = joobb.at("min").get<Eigen::Vector3f>();
auto extents = joobb.at("extents").get<Eigen::Vector3f>();
- Eigen::Vector3f corner = ori * min;
+ Eigen::Vector3f corner = pos - ori * extents / 2;
simox::OrientedBox<float> oobb(corner,
ori.col(0) * extents(0),
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();
return oobb;
}
--
GitLab