diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index fd9a7d126abb44851473a41bd61bbfb7b020c7d0..13c76d0e2700e73bdf788b39eb10696098801836 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -24,8 +24,10 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/json.h>
+#include <SimoxUtility/math/pose/pose.h>
+
#include <Eigen/Geometry>
@@ -245,21 +247,21 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
+ objpose::ObjectPoseMap Segment::getObjectPoses(IceUtil::Time now)
{
- ObjectPoseSeq objectPoses = getLatestObjectPoses();
+ ObjectPoseMap objectPoses = getLatestObjectPoses();
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
- objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
+ objpose::ObjectPoseMap Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
- ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
+ ObjectPoseMap objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
@@ -296,11 +298,11 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, IceUtil::Time now)
{
bool agentSynchronized = false;
- for (ObjectPose& objectPose : objectPoses)
+ for (auto& [id, objectPose] : objectPoses)
{
updateObjectPose(objectPose, now, robot, agentSynchronized);
}
@@ -308,14 +310,14 @@ namespace armarx::armem::server::obj::instance
void Segment::updateObjectPoses(
- ObjectPoseSeq& objectPoses,
+ ObjectPoseMap& objectPoses,
IceUtil::Time now,
VirtualRobot::RobotPtr agent,
bool& agentSynchronized) const
{
- for (ObjectPose& pose : objectPoses)
+ for (auto& [id, objectPose] : objectPoses)
{
- updateObjectPose(pose, now, agent, agentSynchronized);
+ updateObjectPose(objectPose, now, agent, agentSynchronized);
}
}
@@ -335,14 +337,14 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPoseSeq Segment::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
+ Segment::ObjectPoseMap Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
{
- ObjectPoseSeq result;
- for (const ObjectPose& objectPose : objectPoses)
+ ObjectPoseMap result;
+ for (const auto& [id, objectPose] : objectPoses)
{
if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
{
- result.push_back(objectPose);
+ result[id] = objectPose;
}
}
return result;
@@ -368,24 +370,24 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
+ objpose::ObjectPoseMap Segment::getLatestObjectPoses() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return getLatestObjectPoses(*coreSegment);
}
- objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
+ objpose::ObjectPoseMap Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
{
- ObjectPoseSeq result;
+ ObjectPoseMap result;
getLatestObjectPoses(coreSeg, result);
return result;
}
- objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
+ objpose::ObjectPoseMap Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
{
- ObjectPoseSeq result;
+ ObjectPoseMap result;
getLatestObjectPoses(provSeg, result);
return result;
}
@@ -399,7 +401,7 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseSeq& out)
+ void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseMap& out)
{
for (const auto& [_, provSegment] : coreSeg)
{
@@ -408,14 +410,23 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
+ void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseMap& out)
{
for (const auto& [_, entity] : provSegment)
{
if (!entity.empty())
{
- ObjectPose& pose = out.emplace_back();
- getLatestObjectPose(entity, pose);
+ ObjectPose pose = getLatestObjectPose(entity);
+ // Try to insert. Fails and returns false if an entry already exists.
+ const auto [it, success] = out.insert({pose.objectID, pose});
+ if (!success)
+ {
+ // An entry with that ID already exists. We keep the newest.
+ if (it->second.timestamp < pose.timestamp)
+ {
+ it->second = pose;
+ }
+ }
}
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 440de8135f2d498d254d2c5e0454582574947526..3d7af768b9ec04457050342d6b83480386d9d579 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -42,6 +42,7 @@ namespace armarx::armem::server::obj::instance
};
using ObjectPose = objpose::ObjectPose;
using ObjectPoseSeq = objpose::ObjectPoseSeq;
+ using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
public:
@@ -66,8 +67,8 @@ namespace armarx::armem::server::obj::instance
const wm::CoreSegment& getCoreSegment() const;
- objpose::ObjectPoseSeq getObjectPoses(IceUtil::Time now);
- objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+ objpose::ObjectPoseMap getObjectPoses(IceUtil::Time now);
+ objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
@@ -93,12 +94,12 @@ namespace armarx::armem::server::obj::instance
bool& synchronized) const;
- static ObjectPoseSeq getLatestObjectPoses(const wm::CoreSegment& coreSeg);
- static ObjectPoseSeq getLatestObjectPoses(const wm::ProviderSegment& provSeg);
+ static ObjectPoseMap getLatestObjectPoses(const wm::CoreSegment& coreSeg);
+ static ObjectPoseMap getLatestObjectPoses(const wm::ProviderSegment& provSeg);
static ObjectPose getLatestObjectPose(const wm::Entity& entity);
- static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseSeq& out);
- static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out);
+ static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseMap& out);
static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
@@ -106,13 +107,13 @@ namespace armarx::armem::server::obj::instance
private:
- ObjectPoseSeq getLatestObjectPoses() const;
+ ObjectPoseMap getLatestObjectPoses() const;
void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
+ ObjectPoseMap& objectPoses,
IceUtil::Time now);
void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
+ ObjectPoseMap& objectPoses,
IceUtil::Time now,
VirtualRobot::RobotPtr agent,
bool& agentSynchronized
@@ -125,7 +126,7 @@ namespace armarx::armem::server::obj::instance
) const;
- ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
+ ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
void storeDetachedSnapshot(
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 02f93f7e2f73fdd48847a3e19822619a05d3c4a9..54b298b6c1f98e160d5803659ca078de1158c9cd 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -217,7 +217,7 @@ namespace armarx::armem::server::obj::instance
TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPoses(now)));
TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
@@ -242,7 +242,7 @@ namespace armarx::armem::server::obj::instance
TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now)));
TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
@@ -418,7 +418,7 @@ namespace armarx::armem::server::obj::instance
{
TIMING_START(tVisu);
- objpose::ObjectPoseSeq objectPoses;
+ objpose::ObjectPoseMap objectPoses;
visu.minConfidence = -1;
{
std::scoped_lock lock(memoryMutex);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 5cc251ca984e22a0129411b6b05a59db79d8a543..6f337c8240be05af068c69b50c6703cbd63da576 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -43,30 +43,46 @@ namespace armarx::armem::server::obj::instance
return layers;
}
+
std::vector<viz::Layer> Visu::visualizeCommit(
const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder) const
{
- std::map<std::string, viz::Layer> layers;
-
- auto getLayer = [this, &layers](const std::string & providerName) -> viz::Layer &
+ std::map<std::string, viz::Layer> stage;
+ for (const objpose::ObjectPose& objectPose : objectPoses)
{
- auto it = layers.find(providerName);
- if (it == layers.end())
- {
- it = layers.emplace(providerName, arviz.layer(providerName)).first;
- }
- return it->second;
- };
+ visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
+ }
+ return simox::alg::get_values(stage);
+ }
- for (const objpose::ObjectPose& objectPose : objectPoses)
+
+ std::vector<viz::Layer> Visu::visualizeCommit(
+ const objpose::ObjectPoseMap& objectPoses,
+ const ObjectFinder& objectFinder) const
+ {
+ std::map<std::string, viz::Layer> stage;
+ for (const auto& [id, objectPose] : objectPoses)
{
- visualizeObjectPose(getLayer(objectPose.providerName), objectPose, objectFinder);
+ visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
}
+ return simox::alg::get_values(stage);
+ }
- return simox::alg::get_values(layers);
+
+ viz::Layer& Visu::getLayer(
+ const std::string& providerName,
+ std::map<std::string, viz::Layer>& stage) const
+ {
+ auto it = stage.find(providerName);
+ if (it == stage.end())
+ {
+ it = stage.emplace(providerName, arviz.layer(providerName)).first;
+ }
+ return it->second;
}
+
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
const objpose::ObjectPoseSeq& objectPoses,
@@ -85,8 +101,8 @@ namespace armarx::armem::server::obj::instance
const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder) const
{
- const bool show = not(objectPose.confidence < minConfidence);
- if (!show)
+ const bool show = objectPose.confidence >= minConfidence;
+ if (not show)
{
return;
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 1cd1d41c835e56e820ce3db8b3397b1e117d8202..19dbeccd5addc942e4c417aa6505210ac6d82e92 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -37,6 +37,10 @@ namespace armarx::armem::server::obj::instance
const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
+ std::vector<viz::Layer> visualizeCommit(
+ const objpose::ObjectPoseMap& objectPoses,
+ const ObjectFinder& objectFinder
+ ) const;
viz::Layer visualizeProvider(
const std::string& providerName,
@@ -52,6 +56,11 @@ namespace armarx::armem::server::obj::instance
) const;
+ private:
+
+ viz::Layer& getLayer(const std::string& providerName, std::map<std::string, viz::Layer>& stage) const;
+
+
public:
viz::Client arviz;