objpose.cpp
Go to the documentation of this file.
1#include "objpose.h"
2
3#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
4
5#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
6#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
7#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
8#include <RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.aron.generated.h>
9#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
10#include <RobotAPI/libraries/aron/common/aron_conversions.h>
11
12void
13armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
14{
15 aron::fromAron(dto.frameName, bo.frameName);
16 aron::fromAron(dto.agentName, bo.agentName);
17 aron::fromAron(dto.poseInFrame, bo.poseInFrame);
18}
19
20void
21armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
22{
23 aron::toAron(dto.frameName, bo.frameName);
24 aron::toAron(dto.agentName, bo.agentName);
25 aron::toAron(dto.poseInFrame, bo.poseInFrame);
26}
27
28void
29armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
30{
31 aron::fromAron(dto.mean, bo.mean);
32 aron::fromAron(dto.covariance, bo.covariance);
33}
34
35void
36armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
37{
38 aron::toAron(dto.mean, bo.mean);
39 aron::toAron(dto.covariance, bo.covariance);
40}
41
42void
43armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
44{
45 switch (dto.value)
46 {
47 case arondto::ObjectType::AnyObject:
48 bo = ObjectType::AnyObject;
49 return;
50 case arondto::ObjectType::KnownObject:
51 bo = ObjectType::KnownObject;
52 return;
53 case arondto::ObjectType::UnknownObject:
54 bo = ObjectType::UnknownObject;
55 return;
56 }
57 ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
58}
59
60void
61armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
62{
63 switch (bo)
64 {
65 case ObjectType::AnyObject:
66 dto.value = arondto::ObjectType::AnyObject;
67 return;
68 case ObjectType::KnownObject:
69 dto.value = arondto::ObjectType::KnownObject;
70 return;
71 case ObjectType::UnknownObject:
72 dto.value = arondto::ObjectType::UnknownObject;
73 return;
74 }
75 ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
76}
77
78void
79armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
80{
81 aron::fromAron(dto.providerName, bo.providerName);
82 fromAron(dto.objectType, bo.objectType);
83 aron::fromAron(dto.isStatic, bo.isStatic);
84 fromAron(dto.objectID, bo.objectID);
85
86 aron::fromAron(dto.objectPoseRobot, bo.objectPoseRobot);
87 aron::fromAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
88 aron::fromAron(dto.objectPoseGlobal, bo.objectPoseGlobal);
89 aron::fromAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
90 aron::fromAron(dto.objectPoseOriginal, bo.objectPoseOriginal);
91 aron::fromAron(dto.objectPoseOriginalFrame, bo.objectPoseOriginalFrame);
92 aron::fromAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
93 aron::fromAron(dto.objectJointValues, bo.objectJointValues);
94
95 aron::fromAron(dto.robotConfig, bo.robotConfig);
96 aron::fromAron(dto.robotPose, bo.robotPose);
97 aron::fromAron(dto.robotName, bo.robotName);
98
99 if (dto.attachmentValid)
100 {
101 bo.attachment = ObjectAttachmentInfo();
102 fromAron(dto.attachment, *bo.attachment);
103 }
104 else
105 {
106 bo.attachment = std::nullopt;
107 }
108 aron::fromAron(dto.confidence, bo.confidence);
109 aron::fromAron(dto.timestamp, bo.timestamp);
110
111
112 if (dto.localOOBBValid)
113 {
114 bo.localOOBB = simox::OrientedBoxf();
115 fromAron(dto.localOOBB, *bo.localOOBB);
116 }
117 else
118 {
119 bo.localOOBB = std::nullopt;
120 }
121}
122
123void
124armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
125{
126 aron::toAron(dto.providerName, bo.providerName);
127 toAron(dto.objectType, bo.objectType);
128 aron::toAron(dto.isStatic, bo.isStatic);
129 toAron(dto.objectID, bo.objectID);
130
131 aron::toAron(dto.objectPoseRobot, bo.objectPoseRobot);
132 aron::toAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
133 aron::toAron(dto.objectPoseGlobal, bo.objectPoseGlobal);
134 aron::toAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
135 aron::toAron(dto.objectPoseOriginal, bo.objectPoseOriginal);
136 aron::toAron(dto.objectPoseOriginalFrame, bo.objectPoseOriginalFrame);
137 aron::toAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
138
139 aron::toAron(dto.objectJointValues, bo.objectJointValues);
140
141 aron::toAron(dto.robotConfig, bo.robotConfig);
142 aron::toAron(dto.robotPose, bo.robotPose);
143 aron::toAron(dto.robotName, bo.robotName);
144
145 if (bo.attachment)
146 {
147 dto.attachmentValid = true;
148 toAron(dto.attachment, *bo.attachment);
149 }
150 else
151 {
152 dto.attachmentValid = false;
153 toAron(dto.attachment, ObjectAttachmentInfo());
154 }
155
156 aron::toAron(dto.confidence, bo.confidence);
157 aron::toAron(dto.timestamp, bo.timestamp);
158
159 if (bo.localOOBB)
160 {
161 dto.localOOBBValid = true;
162 toAron(dto.localOOBB, *bo.localOOBB);
163 }
164 else
165 {
166 dto.localOOBBValid = false;
167 toAron(dto.localOOBB, simox::OrientedBoxf());
168 }
169}
aron_conversions.h
aron_conversions.h
ARMARX_UNEXPECTED_ENUM_VALUE
#define ARMARX_UNEXPECTED_ENUM_VALUE(EnumType, value)
Throw an UnexpectedEnumValueException.
Definition UnexpectedEnumValueException.h:58
armarx::aron::fromAron
void fromAron(const T &dto, T &bo)
Definition aron_conversions.h:84
armarx::aron::toAron
void toAron(T &dto, const T &bo)
Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs (Business Objects) and back.
Definition aron_conversions.h:77
armarx::objpose::toAron
void toAron(arondto::ObjectAttachmentInfo &dto, const ObjectAttachmentInfo &bo)
Definition objpose.cpp:21
armarx::objpose::fromAron
void fromAron(const arondto::ObjectAttachmentInfo &dto, ObjectAttachmentInfo &bo)
Definition objpose.cpp:13
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition ObjectPose.h:34
armarx::objpose::ObjectPose::confidence
float confidence
Confidence in [0, 1] (1 = full, 0 = none).
Definition ObjectPose.h:96
armarx::objpose::ObjectPose::objectID
armarx::ObjectID objectID
The object ID, i.e. dataset, class name and instance name.
Definition ObjectPose.h:56
armarx::objpose::ObjectPose::attachment
std::optional< ObjectAttachmentInfo > attachment
Attachment information.
Definition ObjectPose.h:93
armarx::objpose::ObjectPose::robotConfig
std::map< std::string, float > robotConfig
The robot config when the object was observed.
Definition ObjectPose.h:86
armarx::objpose::ObjectPose::providerName
std::string providerName
Name of the providing component.
Definition ObjectPose.h:59
armarx::objpose::ObjectPose::isStatic
bool isStatic
Whether object is static. Static objects don't decay.
Definition ObjectPose.h:63
armarx::objpose::ObjectPose::timestamp
DateTime timestamp
Source timestamp.
Definition ObjectPose.h:98
armarx::objpose::ObjectPose::objectPoseRobot
Eigen::Matrix4f objectPoseRobot
The object pose in the robot root frame.
Definition ObjectPose.h:66
armarx::objpose::ObjectPose::robotPose
Eigen::Matrix4f robotPose
The robot pose when the object was observed.
Definition ObjectPose.h:88
armarx::objpose::ObjectPose::objectPoseOriginal
Eigen::Matrix4f objectPoseOriginal
The object pose in the frame it was originally localized in.
Definition ObjectPose.h:76
armarx::objpose::ObjectPose::objectType
ObjectType objectType
Known or unknown object.
Definition ObjectPose.h:61
armarx::objpose::ObjectPose::objectPoseGlobalGaussian
std::optional< PoseManifoldGaussian > objectPoseGlobalGaussian
... and with uncertainty.
Definition ObjectPose.h:73
armarx::objpose::ObjectPose::objectJointValues
std::map< std::string, float > objectJointValues
The object's joint values if it is articulated.
Definition ObjectPose.h:83
armarx::objpose::ObjectPose::localOOBB
std::optional< simox::OrientedBoxf > localOOBB
Object bounding box in object's local coordinate frame.
Definition ObjectPose.h:102
armarx::objpose::ObjectPose::objectPoseOriginalGaussian
std::optional< PoseManifoldGaussian > objectPoseOriginalGaussian
... and with uncertainty.
Definition ObjectPose.h:80
armarx::objpose::ObjectPose::objectPoseGlobal
Eigen::Matrix4f objectPoseGlobal
The object pose in the global frame.
Definition ObjectPose.h:71
armarx::objpose::ObjectPose::robotName
std::string robotName
The name of the robot.
Definition ObjectPose.h:90
armarx::objpose::ObjectPose::objectPoseRobotGaussian
std::optional< PoseManifoldGaussian > objectPoseRobotGaussian
... and with uncertainty.
Definition ObjectPose.h:68
armarx::objpose::ObjectPose::objectPoseOriginalFrame
std::string objectPoseOriginalFrame
The frame the object was originally localized in.
Definition ObjectPose.h:78
armarx::objpose::PoseManifoldGaussian
A "gaussian" distribution in pose space (i.e.
Definition PoseManifoldGaussian.h:25
armarx::objpose::PoseManifoldGaussian::mean
Eigen::Matrix4f mean
The mean (i.e. a pose).
Definition PoseManifoldGaussian.h:28
armarx::objpose::PoseManifoldGaussian::covariance
Eigen::Matrix< float, 6, 6 > covariance
The covariance matrix.
Definition PoseManifoldGaussian.h:30