From dfb8abaa689e315cbee6a316722aadc74d8cee87 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 20 May 2020 17:59:32 +0200
Subject: [PATCH] Add getter for oobb()
---
.../ObjectPoseObserver/ObjectFinder.cpp | 18 ++++++++++++++++++
.../ObjectPoseObserver/ObjectFinder.h | 2 ++
2 files changed, 20 insertions(+)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
index 950ec0e00..eef7faa22 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
@@ -3,6 +3,7 @@
#include <SimoxUtility/filesystem/list_directory.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -251,6 +252,23 @@ namespace armarx
return simox::AxisAlignedBoundingBox(min, max);
}
+ simox::OrientedBox<float> ObjectInfo::oobb() const
+ {
+ nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+ nlohmann::json joobb = j.at("oobb");
+ 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;
+
+ simox::OrientedBox<float> oobb(corner,
+ ori.col(0) * extents(0),
+ ori.col(1) * extents(1),
+ ori.col(2) * extents(2));
+ return oobb;
+ }
+
bool ObjectInfo::checkPaths() const
{
namespace fs = std::filesystem;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
index 31cb3af89..7bbaa7c22 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
@@ -7,6 +7,7 @@
namespace simox
{
struct AxisAlignedBoundingBox;
+ template<class FloatT> class OrientedBox;
}
namespace armarx
@@ -100,6 +101,7 @@ namespace armarx
PackageFileLocation boundingBoxJson() const;
simox::AxisAlignedBoundingBox aabb() const;
+ simox::OrientedBox<float> oobb() const;
/**
--
GitLab