diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index abc0ec1516ec54fa890541862771078b9e5164be..40a529a910c6622055880b2ada42d632ace774fb 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -528,22 +528,21 @@ namespace armarx
{
TIMING_START(Visu);
- std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
+ objpose::ObjectPoseSeq objectPoses;
ObjectFinder objectFinder;
- float minConfidence = -1;
+ visu.minConfidence = -1;
{
std::scoped_lock lock(dataMutex);
const IceUtil::Time now = TimeUtil::GetTime();
- data.updateObjectPoses(now);
- objectPoses = data.getStoredObjectPoses();
+ objectPoses = data.getObjectPoses(now);
objectFinder = data.objectFinder;
if (data.decay.enabled)
{
- minConfidence = data.decay.removeObjectsBelowConfidence;
+ visu.minConfidence = data.decay.removeObjectsBelowConfidence;
}
}
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, minConfidence, objectFinder);
+ const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
arviz.commit(layers);
TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
index e5745f0c48ec74561bc2d2dcd10252a3670d4a4b..96c5afad46d8fb8edf959bd5c7c1178f4ad956ed 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
@@ -41,21 +41,16 @@ namespace armarx::objpose::observer
ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now)
{
- bool synchronized = false;
- ObjectPoseSeq result;
+ bool agentSynchronized = false;
+ ObjectPoseSeq latestObjectPoses = getLatestObjectPoses();
- for (auto& [providerName, objectPoses] : objectPoses)
+ ObjectPoseSeq result;
+ for (ObjectPose& objectPose : latestObjectPoses)
{
- // Update data.
- updateObjectPoses(objectPoses, now, robot, synchronized);
-
- // Collect results.
- for (const ObjectPose& objectPose : objectPoses)
+ updateObjectPose(objectPose, now, robot, agentSynchronized);
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
{
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
+ result.push_back(objectPose);
}
}
return result;
@@ -138,15 +133,6 @@ namespace armarx::objpose::observer
}
- void Data::updateObjectPoses(IceUtil::Time now)
- {
- bool synchronized = false;
- for (auto& [providerName, objectPoses] : objectPoses)
- {
- updateObjectPoses(objectPoses, now, robot, synchronized);
- }
- }
-
void Data::updateObjectPoses(
ObjectPoseSeq& objectPoses,
@@ -204,6 +190,33 @@ namespace armarx::objpose::observer
objectPose.updateAttached(agent);
}
+ ObjectPoseSeq Data::getLatestObjectPoses() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ ObjectPoseSeq result;
+
+ for (const auto& [_, provSegment] : *coreSegment)
+ {
+ for (const auto& [_, entity] : provSegment)
+ {
+ if (!entity.empty())
+ {
+ for (const armem::EntityInstance& instance : entity.getLatestSnapshot())
+ {
+ arondto::ObjectPose dto;
+ dto.fromAron(instance.data());
+
+ ObjectPose& pose = result.emplace_back();
+ fromAron(dto, pose);
+ }
+ }
+ }
+ }
+
+ return result;
+ }
+
ObjectPose* Data::findObjectPose(const ObjectID& objectID, const std::string& providerName)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
index 938dcf601ca908e0dbe81059c15d7269c70b51e6..f538cc284f788ab76a86a03f99d4c725f53dc201 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
@@ -69,7 +69,6 @@ namespace armarx::objpose::observer
void setObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
- void updateObjectPoses(IceUtil::Time now);
void updateObjectPoses(
ObjectPoseSeq& objectPoses,
IceUtil::Time now,
@@ -96,6 +95,10 @@ namespace armarx::objpose::observer
void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
bool& synchronized) const;
+ private:
+
+ ObjectPoseSeq getLatestObjectPoses() const;
+
public:
@@ -115,7 +118,6 @@ namespace armarx::objpose::observer
/// Decay model.
Decay decay;
-
private:
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
index 467dba24f947b38c02b47044986cf3086775b6a1..11f8a3ebbda0911471d02e721f71f472e0b44c47 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
@@ -31,31 +31,49 @@ namespace armarx::objpose::observer
std::vector<viz::Layer> Visu::visualizeCommit(
const std::map<std::string, ObjectPoseSeq>& objectPoses,
- float minConfidence,
const ObjectFinder& objectFinder) const
{
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
- layers.push_back(visualizeProvider(name, poses, minConfidence, objectFinder));
+ layers.push_back(visualizeProvider(name, poses, objectFinder));
}
return layers;
}
+ std::vector<viz::Layer> Visu::visualizeCommit(
+ const ObjectPoseSeq& objectPoses,
+ const ObjectFinder& objectFinder) const
+ {
+ std::map<std::string, viz::Layer> layers;
+
+ auto getLayer = [this, &layers](const std::string & providerName) -> viz::Layer &
+ {
+ auto it = layers.find(providerName);
+ if (it == layers.end())
+ {
+ it = layers.emplace(providerName, arviz.layer(providerName)).first;
+ }
+ return it->second;
+ };
+
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ visualizeObjectPose(getLayer(objectPose.providerName), objectPose, objectFinder);
+ }
+
+ return simox::alg::get_values(layers);
+ }
+
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
const ObjectPoseSeq& objectPoses,
- float minConfidence,
const ObjectFinder& objectFinder) const
{
viz::Layer layer = arviz.layer(providerName);
for (const ObjectPose& objectPose : objectPoses)
{
- const bool show = not(objectPose.confidence < minConfidence);
- if (show)
- {
- visualizeObjectPose(layer, objectPose, objectFinder);
- }
+ visualizeObjectPose(layer, objectPose, objectFinder);
}
return layer;
}
@@ -65,6 +83,11 @@ namespace armarx::objpose::observer
const ObjectPose& objectPose,
const ObjectFinder& objectFinder) const
{
+ const bool show = not(objectPose.confidence < minConfidence);
+ if (!show)
+ {
+ return;
+ }
const armarx::ObjectID id = objectPose.objectID;
const std::string key = id.str();
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
index 9677f6198adcd9f52918061925876860640ba999..fcdfd4f240a903dc505bd7eb3732c6242838e0e4 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
@@ -29,17 +29,22 @@ namespace armarx::objpose::observer
std::vector<viz::Layer> visualizeCommit(
const std::map<std::string, ObjectPoseSeq>& objectPoses,
- float minConfidence,
+ const ObjectFinder& objectFinder
+ ) const;
+
+ /// Visualize the given object poses, with one layer per provider.
+ std::vector<viz::Layer> visualizeCommit(
+ const ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
viz::Layer visualizeProvider(
const std::string& providerName,
const ObjectPoseSeq& objectPoses,
- float minConfidence,
const ObjectFinder& objectFinder
) const;
+
void visualizeObjectPose(
viz::Layer& layer,
const ObjectPose& objectPose,
@@ -55,6 +60,7 @@ namespace armarx::objpose::observer
float frequencyHz = 25;
bool inGlobalFrame = true;
+ float minConfidence = -1;
float alpha = 1.0;
bool alphaByConfidence = false;
bool oobbs = false;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 36379f97d0d3d047058dbb9a9d57e9517a0caa71..d5c3d55167c4e1f70654b0a8e4a8990a40c4a35b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -20,7 +20,7 @@ namespace armarx::objpose
{
std::string frameName;
std::string agentName;
- Eigen::Matrix4f poseInFrame;
+ Eigen::Matrix4f poseInFrame = Eigen::Matrix4f::Identity();
};