diff --git a/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml index c583f1eedb1e774dd189c7298d9c0ec921f4a58a..13a51e4e886dec94708e07301c0b679fe4e60a7a 100644 --- a/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml +++ b/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml @@ -51,6 +51,11 @@ Core segment type of Object/Instance. <PointCloud type="PointXYZRGB"/> </ObjectChild> --> + <!-- Orientation of the bounding box in the 'sens' frame --> + <ObjectChild key="boundingBoxOrientation"> + <Orientation /> + </ObjectChild> + <!-- bounding box in local sensFrame (see poseSensFrame) --> <ObjectChild key="bounding_box"> <simox::arondto::AxisAlignedBoundingBox /> diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp index 345703832fd49d3b07739fe6efb3715bf5c4a82d..b6e5e66d607e75751f467c325400c7e0b8828f10 100644 --- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp @@ -89,9 +89,12 @@ namespace armarx::armem::server::obj::familiar_object_instance if (visualizeBoundingBox) { - const Eigen::Isometry3f global_T_bb = - Eigen::Isometry3f{familiarObject.poseGlobal->pose} * - Eigen::Translation3f{familiarObject.bounding_box.center}; + const Eigen::Isometry3f global_T_obj{familiarObject.poseGlobal->pose}; + Eigen::Isometry3f obj_T_bb = Eigen::Isometry3f::Identity(); + obj_T_bb.translation() = familiarObject.bounding_box.center; + obj_T_bb.linear() = familiarObject.boundingBoxOrientation.toRotationMatrix(); + + const Eigen::Isometry3f global_T_bb = global_T_obj * obj_T_bb; auto box = viz::Box(objectId.str()); box.pose(global_T_bb.matrix());