diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index cce838327d84b2191f603bf22d654d7c5bc55382..e150f1e044f225c73aeda5f330a50ffef1c51a16 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -6,9 +6,11 @@
#include <SimoxUtility/algorithm/advanced.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "ArmarXCore/core/logging/Logging.h"
#include "error/ArMemError.h"
+
namespace armarx::armem
{
@@ -21,6 +23,8 @@ namespace armarx::armem
MemoryID::MemoryID(const std::string& string)
{
+ //ARMARX_INFO << "Trying to create MemoryID from string";
+ //ARMARX_INFO << VAROUT(string);
std::forward_list<std::string> items;
boost::split(items, string, boost::is_any_of(delimiter));
@@ -76,6 +80,7 @@ namespace armarx::armem
return;
}
this->instanceIndex = instanceIndexFromStr(*it);
+ //ARMARX_INFO << this->str();
}
MemoryID::MemoryID(const std::string& memoryName,
@@ -124,6 +129,31 @@ namespace armarx::armem
return "";
}
+ MemoryID MemoryID::cleanID() const
+ {
+ std::vector<std::string> allItems = this->getAllItems(true);
+ std::string newID = "";
+ for(int i = 0; i < allItems.size(); i++){
+ if(!allItems.at(i).empty()){
+ std::string toAppend = allItems.at(i);
+ if(allItems.at(i).find(delimiter) != std::string::npos){
+ size_t pos = 0;
+ std::string token;
+ while ((pos = toAppend.find(delimiter)) != std::string::npos) {
+ token = toAppend.substr(0, pos);
+ toAppend.erase(0, pos + delimiter.length());
+ }
+ }
+ newID.append(toAppend);
+ if(i < allItems.size() - 1 && !allItems.at(i+1).empty()){
+ newID.append(delimiter);
+ }
+ }
+ }
+ return MemoryID(newID);
+ }
+
+
bool
MemoryID::hasGap() const
{
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index d9dca2a2541bae3462a18ad1603a048f804fc551..e54617f4cd7d740b0b7cb95aea78c5b55a82b681 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -218,6 +218,8 @@ namespace armarx::armem
/// Get the lowest defined level (or empty string if there is none).
std::string getLeafItem() const;
+ MemoryID cleanID() const;
+
// Operators
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index 2fb8bd3cc6ffcb217f7882e66fa2ed1e1b52d429..c1eeea7275cecc1f4dd2c76d7723720681fcead1 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -269,16 +269,7 @@ namespace armarx::armem::base
EntityInstanceT&
addInstance(EntityInstanceT&& instance)
{
- if (instance.index() > 0 &&
- static_cast<size_t>(instance.index()) < this->_container.size())
- {
- throw error::InvalidArgument(
- std::to_string(instance.index()),
- "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index already exists.");
- }
- if (instance.index() > 0 &&
- static_cast<size_t>(instance.index()) > this->_container.size())
+ if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
{
throw error::InvalidArgument(
std::to_string(instance.index()),
@@ -286,8 +277,9 @@ namespace armarx::armem::base
"Cannot add an EntityInstance because its index is too big.");
}
- int index = static_cast<int>(this->_container.size());
+ int index = instance.index();
EntityInstanceT& added = this->_container.emplace_back(std::move(instance));
+
added.id() = this->id().withInstanceIndex(index);
return added;
}
@@ -295,7 +287,8 @@ namespace armarx::armem::base
EntityInstanceT&
addInstance()
{
- int index = static_cast<int>(this->size());
+ //ARMARX_INFO << "Trying to add an instance without instance yet generated";
+ int index = static_cast<int>(this->size()); //this is the problem, because instances have names 0 and 11 (I do not know why)
EntityInstanceT& added = this->_container.emplace_back(EntityInstanceT());
added.id() = this->id().withInstanceIndex(index);
return added;
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
index 9f5190364d66b999fa9d15d64bf9c12454d05715..07930b46e931513cd42f4a6dcdaacd5a3f80a56d 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
@@ -2,6 +2,7 @@
#include <functional>
#include <type_traits>
+#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
index 5856407816fe423e2f93bd0375a631863c304a89..e72b21abffa5e571ffab8437d35ce6026dd1ab62 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
@@ -52,6 +52,7 @@ namespace armarx::armem::wm
toMemory(Memory& m, const std::vector<EntitySnapshot>& e)
{
m.clear();
+
for (const auto& s : e)
{
if (!m.hasCoreSegment(s.id().coreSegmentName))
@@ -108,4 +109,42 @@ namespace armarx::armem::wm
sn->addInstance(s);
}
}
+
+ void toMemory(Memory &m, const armarx::armem::server::wm::Memory &structure, const std::vector<EntitySnapshot> &e)
+ {
+ // create an empty working memory:
+ m.clear();
+
+ for (const auto& s : e)
+ {
+ //get name of the core segment:
+ auto coreSegmentName = s.id().coreSegmentName;
+ auto* coreStructure = structure.findCoreSegment(coreSegmentName);
+
+ //if m does not have this core segment yet add the segment but copy the structure from the server wm:
+ if (!m.hasCoreSegment(coreSegmentName))
+ {
+ m.addCoreSegment(coreSegmentName, coreStructure->aronType());
+ }
+ auto* c = m.findCoreSegment(coreSegmentName);
+
+ //get name of provider segment:
+ auto providerSegmentName = s.id().providerSegmentName;
+ auto* providerStructure = coreStructure->findProviderSegment(providerSegmentName);
+
+ if (!c->hasProviderSegment(providerSegmentName))
+ {
+ c->addProviderSegment(providerSegmentName, providerStructure->aronType());
+ }
+ auto* p = c->findProviderSegment(providerSegmentName);
+
+ if (!p->hasEntity(s.id().entityName))
+ {
+ p->addEntity(s.id().entityName);
+ }
+ auto* en = p->findEntity(s.id().entityName);
+ en->addSnapshot(s);
+ }
+ }
+
} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
index 53ac02bfc5536150b2d0f01b9457537bb236d95a..7ea40b5f54eb2a994eb98cd5f278ee29582ce11b 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
@@ -3,6 +3,7 @@
#include <vector>
#include "memory_definitions.h"
+#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
namespace armarx::armem::wm
{
@@ -10,5 +11,15 @@ namespace armarx::armem::wm
void toMemory(Memory& m, const std::vector<ProviderSegment>& e);
void toMemory(Memory& m, const std::vector<Entity>& e);
void toMemory(Memory& m, const std::vector<EntitySnapshot>& e);
+
+ /**
+ * @brief toMemory converts a vector of entity snapshots into a working memory and considers
+ * the structure of the memory before
+ * @param m the (empty) new memory
+ * @param structure the server working memory for structure information
+ * @param e the vector of entity snapshots
+ */
+ void toMemory(Memory& m, const armarx::armem::server::wm::Memory& structure, const std::vector<EntitySnapshot>& e);
+
void toMemory(Memory& m, const std::vector<EntityInstance>& e);
} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index de2a911560d8d13c2516db3f22348c59cfa38365..0e2544ab302ba69802c699a238a79e71952ccacc 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -22,6 +22,7 @@ set(LIBS
RemoteGui
RobotAPI::aron
RobotAPI::armem
+ RobotAPI::aron::similarity
# LTM
RobotAPI::aron::converter::json
@@ -60,6 +61,8 @@ set(LIB_FILES
ltm/processors/filter/Filter.cpp
ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
ltm/processors/filter/equalityFilter/EqualityFilter.cpp
+ #ltm/processors/filter/similarityFilter/SimilarityFilter.cpp
+ ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
ltm/processors/extractor/Extractor.cpp
ltm/processors/extractor/imageExtractor/ImageExtractor.cpp
@@ -116,6 +119,8 @@ set(LIB_FILES
query_proc/wm/detail/CoreSegmentQueryProcessorBase.cpp
query_proc/wm/detail/MemoryQueryProcessorBase.cpp
query_proc/wm/wm.cpp
+
+ test/ForgettingExperiments.cpp
)
set(LIB_HEADERS
@@ -149,6 +154,8 @@ set(LIB_HEADERS
ltm/processors/filter/Filter.h
ltm/processors/filter/frequencyFilter/FrequencyFilter.h
ltm/processors/filter/equalityFilter/EqualityFilter.h
+ #ltm/processors/filter/similarityFilter/SimilarityFilter.h
+ ltm/processors/filter/importanceFilter/ImportanceFilter.h
ltm/processors/extractor/Extractor.h
ltm/processors/extractor/imageExtractor/ImageExtractor.h
@@ -208,6 +215,8 @@ set(LIB_HEADERS
query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
query_proc/wm/detail/MemoryQueryProcessorBase.h
query_proc/wm/wm.h
+
+ test/ForgettingExperiments.h
)
# Clear variable set by CMakeLists.txt in parent directory.
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 194785757e80ecbb9b77b06ec9224f500c85ec55..a033f89b0a490ca8f041f9fb3cfcd1bbdf4f035b 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,5 +1,7 @@
#include "MemoryToIceAdapter.h"
+#include <thread>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -233,15 +235,14 @@ namespace armarx::armem::server
ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
}
- // Consollidate to ltm
+
+ // Consollidate to ltm(s)
if (longtermMemory->isRecording())
{
- //ARMARX_IMPORTANT << longtermMemory->id().str();
- //ARMARX_IMPORTANT << longtermMemory->getPath();
-
// convert removedSnapshots to Memory
armem::wm::Memory m(longtermMemory->name());
- armem::wm::toMemory(m, updateResult.removedSnapshots);
+ armem::wm::toMemory(m, *workingMemory, updateResult.removedSnapshots);
+ //this removes information about segments, because new memory (wm) is used
// store memory
longtermMemory->store(m);
@@ -268,6 +269,10 @@ namespace armarx::armem::server
result.success = false;
result.errorMessage = e.what();
}
+ catch (...)
+ {
+ ARMARX_INFO << "Error during LTM consollidation";
+ }
}
if (publishUpdates)
@@ -399,7 +404,7 @@ namespace armarx::armem::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(longtermMemory);
ARMARX_IMPORTANT << "Enabling the recording of memory " << longtermMemory->id().str();
- longtermMemory->startRecording(); // TODO: config and execution time!
+ longtermMemory->startRecording();
dto::StartRecordResult ret;
ret.success = true;
@@ -413,7 +418,21 @@ namespace armarx::armem::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(longtermMemory);
ARMARX_IMPORTANT << "Disabling the recording of memory " << longtermMemory->id().str();
- longtermMemory->stopRecording();
+
+ //put calling stopRecording into a seperate thread and detach to make sure GUI does not freeze
+ auto ltm = longtermMemory;
+ std::thread stopRecordingThread([<m](){
+ ltm->stopRecording();
+ });
+ std::thread savingDataUpdateThread([<m](){
+ ltm->bufferFinished();
+ });
+ stopRecordingThread.detach();
+ savingDataUpdateThread.detach();
+
+ ARMARX_IMPORTANT
+ << "Stopped all LTM recordings, please wait with stopping the component until "
+ "all files are written";
dto::StopRecordResult ret;
ret.success = true;
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index f6fdfe9eaf1a985fe3ceba8a17c2665111333b0e..0ae95a4207d86842d1b11e82cd97af6030147401 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -25,7 +25,6 @@ namespace armarx::armem::server
void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
-
// WRITING
data::AddSegmentResult addSegment(const data::AddSegmentInput& input,
bool addCoreSegments = false);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
index 5e0f1908bc411d1270503a273d62d8508707f4be..87575f5d3005a1cf63ec906c41c404d0fb0a2837 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -25,19 +25,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string id_str = doc[FOREIGN_KEY];
- armem::MemoryID segment_id(id_str);
- ProviderSegment c(
- getMemoryBasePath(), getSettings(), getExportName(), segment_id, processors);
- func(c);
- }
- } else */
-
- // legacy
if (fullPathExists())
{
for (const auto& subdirName : getAllDirectories())
@@ -59,17 +46,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- auto c = ProviderSegment(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withProviderSegmentName(name),
- processors);
-
- return (bool)c.collectionExists();
- }*/
-
if (fullPathExists())
{
auto c = ProviderSegment(getMemoryBasePath(),
@@ -106,26 +82,7 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
-
- auto& conv = processors->defaultTypeConverter;
- auto setType = [&conv, &e](const std::vector<unsigned char>& aronJson)
- {
- auto typeAron = conv.convert(aronJson, "");
- e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
- };
-
- /*if (connected() && collectionExists())
- {
- // TODO:
- } else */
-
- if (std::string filename = TYPE_FILENAME + conv.suffix;
- fullPathExists() && fileExists(filename))
- {
- auto typeFileContent = readDataFromFile(filename);
- setType(typeFileContent);
- }
+ e.id() = id().getCoreSegmentID().cleanID();
forEachProviderSegment(
[&e](auto& x)
@@ -195,6 +152,8 @@ namespace armarx::armem::server::ltm
}
else
{
+ ARMARX_INFO << "CoreSegment does not have aron type, so aron type information "
+ "cannot be exported";
/*writeForeignKeyToPreviousDocument();*/
}
@@ -208,7 +167,6 @@ namespace armarx::armem::server::ltm
processors);
c.store(prov);
- statistics.recordedProviderSegments++;
});
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
index b34d9d5167b9f505d70e3c20caf8e317b7680be3..ac9433dfb78be5a973925feb9c917a67699f1d7e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
@@ -39,20 +39,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string id_str = doc[ID];
-
- armem::MemoryID segment_id = id();
- segment_id.timestampFromStr(id_str);
-
- EntitySnapshot c(
- getMemoryBasePath(), getSettings(), getExportName(), segment_id, processors);
- func(c);
- }
- } else */
if (fullPathExists())
{
@@ -201,15 +187,6 @@ namespace armarx::armem::server::ltm
Entity::hasSnapshot(const Time& n) const
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- auto c = EntitySnapshot(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withTimestamp(n),
- processors);
- return (bool)c.documentExists();
- }*/
if (fullPathExists())
{
@@ -379,7 +356,7 @@ namespace armarx::armem::server::ltm
Entity::_loadAllReferences(armem::wm::Entity& e)
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
+ e.id() = id().getEntityID().cleanID();
forEachSnapshot(
[&e](auto& x)
@@ -450,8 +427,7 @@ namespace armarx::armem::server::ltm
if (hasSnapshot(snap.id().timestamp))
{
- ARMARX_INFO << deactivateSpam()
- << "Ignoring to put an EntitiySnapshot into the LTM because "
+ ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because "
"the timestamp already existed (we assume snapshots are "
"const and do not change outside the ltm).";
return;
@@ -459,12 +435,14 @@ namespace armarx::armem::server::ltm
for (auto& f : processors->snapFilters)
{
- if (!f->accept(snap))
+
+ bool accepted = f->accept(snap);
+ if (!accepted)
{
- ARMARX_INFO << deactivateSpam()
- << "Ignoring to put an EntitiySnapshot into the LTM because it "
- "got filtered.";
+ //ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
return;
+ } else {
+ //ARMARX_INFO << "Storing EntitySnapshot";
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index b5800ec11fd3cfaa5f8c3fd3fcff6e4a4b5bf599..0be6975beb6785dbb2426b74c0d70000edf0ce27 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -31,16 +31,24 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
// assure that we put the right index to the snapshot
- ARMARX_CHECK(e.size() == (size_t)id().instanceIndex);
+ //ARMARX_CHECK(e.size() == (size_t)id().instanceIndex);
+ //int index = e.getInstanceIndices().size();
+
+ //ARMARX_INFO << VAROUT(getFullPath());
+ //ARMARX_INFO << VAROUT(getMemoryID().getEntityInstanceID().getLeafItem());
+ int index = std::stoi(getMemoryID().getEntityInstanceID().getLeafItem());
+
+ armem::wm::EntityInstance i = armem::wm::EntityInstance(index, e.id());
// add instance. Do not set data, since we only return references
- e.addInstance();
+ e.addInstance(i);
}
void
EntityInstance::_resolve(armem::wm::EntityInstance& e) const
{
std::lock_guard l(ltm_mutex);
+ //ARMARX_INFO << "resolve for entity instance " << e.id().str();
auto& dictConverter = processors->defaultObjectConverter;
aron::data::DictPtr datadict = nullptr;
@@ -89,6 +97,8 @@ namespace armarx::armem::server::ltm
}
} else */
+ //ARMARX_INFO << VAROUT(getFullPath());
+
if (fullPathExists())
{
@@ -183,13 +193,30 @@ namespace armarx::armem::server::ltm
auto metadataAron = std::make_shared<aron::data::Dict>();
to_aron(metadataAron, dataAron, e);
+ std::shared_ptr<aron::data::Dict> source;
+
+ bool saveAndExtract = false; //if true the data is saved in extracted form and in data.aron.json
+ // this is needed if several LTMs are recorded at once because otherwise the data from the data.aron.json
+ // is not there anymore to extract from
+
+ if (saveAndExtract)
+ {
+ source = dataAron->clone();
+ }
+ else
+ {
+ source = dataAron;
+ }
+
+ //ARMARX_INFO << "save and extract is " << saveAndExtract;
+
+
// check special members for special converters
for (auto& c : processors->converters)
{
ARMARX_CHECK_NOT_NULL(c);
ARMARX_CHECK_NOT_NULL(c->extractor);
-
- auto dataExt = c->extractor->extract(dataAron);
+ auto dataExt = c->extractor->extract(source);
for (const auto& [memberName, var] : dataExt.extraction)
{
@@ -202,8 +229,6 @@ namespace armarx::armem::server::ltm
ensureFileExists(filename, true);
writeDataToFile(filename, memberDataVec);
}
-
- dataAron = dataExt.dataWithoutExtraction;
}
// convert dict and metadata
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
index 693f015a20130142699eb5900eac0824a1728768..2f974697368c5a375dd9a27715f06cd1cc49dfaa 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
@@ -53,18 +53,10 @@ namespace armarx::armem::server::ltm
{
for (const auto& i : getAllDirectories())
{
- if (!util::fs::detail::isNumberString(i.filename()))
- {
- ARMARX_WARNING << "Found a non-index folder inside an entity '" << id().str()
- << "' with name '" << i.filename() << "'. "
- << "Ignoring this folder, however this is a bad situation.";
- continue;
- }
-
EntityInstance c(getMemoryBasePath(),
getSettings(),
getExportName(),
- id().withInstanceIndex(std::stoi(i.filename())),
+ id().withInstanceIndex(std::stoi(i.filename())).cleanID(),
processors);
func(c);
}
@@ -121,8 +113,11 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
- forEachInstance([&e](auto& x) { x.loadAllReferences(e); });
+ e.id() = id().getEntitySnapshotID().cleanID();
+
+ forEachInstance([&e](armem::server::ltm::EntityInstance& x) {
+ x.loadAllReferences(e);
+ });
}
void
@@ -148,6 +143,7 @@ namespace armarx::armem::server::ltm
void
EntitySnapshot::_store(const armem::wm::EntitySnapshot& p)
{
+
std::lock_guard l(ltm_mutex);
if (id().timestamp.isInvalid())
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
index 06f81e22c947328998187c84d79a7e8a1c5058c8..3f3000dd7b28717547e5721d52722cbf56a676f2 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
@@ -18,7 +18,8 @@ namespace armarx::armem::server::ltm
MongoDBStorageMixin::configureMixin(json);
}
- Memory::Memory() : Memory(std::filesystem::path("/tmp"), {}, "MemoryExport", "Test")
+ Memory::Memory() :
+ Memory(std::filesystem::path("/tmp/ARMARX/LTM_Exports"), {}, "MemoryExport", "Test")
{
}
@@ -41,6 +42,7 @@ namespace armarx::armem::server::ltm
DiskMemoryItemMixin(p, exportName, MemoryID(memoryName, "")),
MongoDBStorageMixin(s, exportName, MemoryID(memoryName, ""))
{
+ ARMARX_INFO << "Creating a new memory at " << p.string() << " and " << exportName;
}
void
@@ -62,6 +64,7 @@ namespace armarx::armem::server::ltm
{
BufferedBase::stop();
MongoDBStorageMixin::stop();
+ ARMARX_IMPORTANT << "Storing of data finished";
}
void
@@ -80,27 +83,14 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- // for each
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string segmentName = doc[FOREIGN_KEY];
- CoreSegment c(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withCoreSegmentName(segmentName),
- processors);
- func(c);
- }
- } else */
-
// legacy: check fs
if (fullPathExists())
{
for (const auto& subdirName : getAllDirectories())
{
+
std::string segmentName = util::fs::detail::unescapeName(subdirName);
+ ARMARX_INFO << VAROUT(id().withCoreSegmentName(segmentName));
CoreSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -110,6 +100,8 @@ namespace armarx::armem::server::ltm
}
}
+ ARMARX_INFO << "All CoreSegments handeled";
+
return true;
}
@@ -167,12 +159,21 @@ namespace armarx::armem::server::ltm
processors);
}
+ void Memory::createPropertyDefinitions(PropertyDefinitionsPtr &defs, const std::string &prefix)
+ {
+ MemoryBase::createPropertyDefinitions(defs, prefix);
+ DiskMemoryBase::createPropertyDefinitions(defs, prefix);
+ BufferedBase::createPropertyDefinitions(defs, prefix);
+ }
+
void
Memory::_loadAllReferences(armem::wm::Memory& m)
{
std::lock_guard l(ltm_mutex);
- m.id() = id();
+ m.id() = id().getMemoryID();
+
+ ARMARX_INFO << VAROUT(id());
forEachCoreSegment(
[&m](auto& x)
@@ -193,6 +194,7 @@ namespace armarx::armem::server::ltm
m.forEachCoreSegment(
[&](auto& e)
{
+ //ARMARX_INFO << "resolve for CoreSegment " << e.id().str();
CoreSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -246,6 +248,6 @@ namespace armarx::armem::server::ltm
});
// 4. update cache
- CachedBase::addToCache(memory);
+ //CachedBase::addToCache(memory);
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
index 30e1929dd3156dfb0f9d4412ac78e44bb62ce664..67b16811b8b066661501c782d3d73902532c6cd8 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
@@ -47,10 +47,15 @@ namespace armarx::armem::server::ltm
std::unique_ptr<CoreSegment>
findCoreSegment(const std::string& coreSegmentName) const final;
+ void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix) override;
+
protected:
void _loadAllReferences(armem::wm::Memory&) final;
void _resolve(armem::wm::Memory&) final;
void _store(const armem::wm::Memory&) final;
void _directlyStore(const armem::wm::Memory&) final;
+
+ private:
+ std::time_t current_date;
};
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
index 0cb941e1fa61aaef583b8d54d3005794039b47ed..cfb613221d44b6353a63e3ea1cebc148cc964a5a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
@@ -11,7 +11,6 @@ namespace armarx::armem::server::ltm
{
ProviderSegment::ProviderSegment(const detail::mixin::Path& p,
const detail::mixin::MongoDBSettings& s,
-
const std::string& exportName,
const armem::MemoryID& id /* UNESCAPED */,
const std::shared_ptr<Processors>& filters) :
@@ -44,6 +43,7 @@ namespace armarx::armem::server::ltm
for (const auto& subdirName : getAllDirectories())
{
+
std::string segmentName = util::fs::detail::unescapeName(subdirName);
Entity c(getMemoryBasePath(),
getSettings(),
@@ -73,6 +73,7 @@ namespace armarx::armem::server::ltm
if (fullPathExists())
{
+ ARMARX_INFO << VAROUT(id().getEntityID());
auto c = Entity(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -105,7 +106,9 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
+ e.id() = id().getProviderSegmentID().cleanID();
+
+ /*
auto& conv = processors->defaultTypeConverter;
auto setType = [&conv, &e](const std::vector<unsigned char>& aronJson)
@@ -114,11 +117,15 @@ namespace armarx::armem::server::ltm
e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
};
+ */
+
/*if (connected() && collectionExists())
{
// TODO:
} else */
+ /*
+
if (std::string filename = TYPE_FILENAME + conv.suffix;
fullPathExists() && fileExists(filename))
{
@@ -126,6 +133,8 @@ namespace armarx::armem::server::ltm
setType(typeFileContent);
}
+ */
+
forEachEntity(
[&e](auto& x)
{
@@ -194,6 +203,7 @@ namespace armarx::armem::server::ltm
}
else
{
+ ARMARX_INFO << "ProviderSegment does not seem to have an aron type, so aron type information connot be exported";
//writeForeignKeyToPreviousDocument();
}
@@ -209,6 +219,6 @@ namespace armarx::armem::server::ltm
c.store(e);
statistics.recordedEntities++;
});
- }
+ }
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 4ca25b8cb1944e345bf6c1f8c285b84c65ad648c..d79c7b3eefa22d8d6748564a839ec5b463d97778 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
@@ -2,11 +2,11 @@
#include <functional>
+
// BaseClass
#include "MemoryItem.h"
// ChildType
-#include "CoreSegmentBase.h"
// ArmarX
#include <ArmarXCore/core/Component.h>
@@ -18,6 +18,7 @@
#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/test/ForgettingExperiments.h>
namespace armarx::armem::server::ltm::detail
{
@@ -30,6 +31,10 @@ namespace armarx::armem::server::ltm::detail
{
armarx::core::time::DateTime lastEnabled = armarx::core::time::DateTime::Invalid();
long recordedCoreSegments = 0;
+ bool firstStart = true;
+ bool firstStop = true;
+ armarx::core::time::DateTime firstStarted = armarx::core::time::DateTime::Invalid();
+ armarx::core::time::DateTime firstStopped = armarx::core::time::DateTime::Invalid();
};
public:
@@ -46,6 +51,8 @@ namespace armarx::armem::server::ltm::detail
{
bool en = p.enabled_on_startup;
ARMARX_INFO << VAROUT(p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(p.export_name);
+ this->setExportName(p.export_name);
try
{
@@ -72,8 +79,14 @@ namespace armarx::armem::server::ltm::detail
void
enable()
{
- ARMARX_INFO << "Enabling LTM " << id().str();
+ auto now = armarx::core::time::DateTime::Now();
+ ARMARX_INFO << "Enabling LTM " << id().str() << " at " << now.toDateTimeString();
+ ARMARX_INFO << "Storing information at " << this->getExportName();
enabled = true;
+ if(statistics.firstStart){
+ statistics.firstStarted = armarx::core::time::DateTime::Now();
+ statistics.firstStart = false;
+ }
this->_enable();
}
@@ -81,9 +94,15 @@ namespace armarx::armem::server::ltm::detail
void
disable()
{
- ARMARX_INFO << "Disabling LTM " << id().str();
+ auto now = armarx::core::time::DateTime::Now();
+ ARMARX_INFO << "Disabling LTM " << id().str() << " at " << now.toDateTimeString();
enabled = false;
+ if(statistics.firstStop){
+ statistics.firstStopped = armarx::core::time::DateTime::Now();
+ statistics.firstStop = false;
+ }
this->_disable();
+ ARMARX_INFO << "Disabling of LTM finished";
}
/// return the full ltm as a wm::Memory with only references
@@ -137,6 +156,7 @@ namespace armarx::armem::server::ltm::detail
store(const armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_Append);
+
for (auto& f : processors->memFilters)
{
if (!f->accept(memory))
@@ -173,7 +193,8 @@ namespace armarx::armem::server::ltm::detail
createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
{
defs->optional(p.enabled_on_startup, prefix + "enabled");
- defs->optional(p.configuration_on_startup, prefix + "configuration");
+ defs->optional(p.configuration_on_startup, prefix + "configuration");
+ defs->optional(p.export_name, prefix + "exportName");
}
/// enable/disable
@@ -181,12 +202,14 @@ namespace armarx::armem::server::ltm::detail
startRecording()
{
statistics.lastEnabled = armarx::core::time::DateTime::Now();
+
enable();
}
void
stopRecording()
{
+
disable();
}
@@ -211,7 +234,20 @@ namespace armarx::armem::server::ltm::detail
return statistics;
}
- /// get level name
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics>
+ getFilterStatistics(){
+ try{
+ ARMARX_INFO << "Trying to save statistics";
+ auto stats = processors->getSnapshotFilterStatistics();
+ return stats;
+ }catch(...){
+ ARMARX_WARNING << "Saving statistics did not work";
+ }
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> emptyStatistics;
+ return emptyStatistics;
+ }
+
+ /// get level name1
static std::string
getLevelName()
{
@@ -250,7 +286,9 @@ namespace armarx::armem::server::ltm::detail
{
bool enabled_on_startup = false;
std::string configuration_on_startup =
- "{\"SnapshotFrequencyFilter\": { \"WaitingTimeInMs\": 1000}, \"PngConverter\": {}}";
+ "{ \"SnapshotFrequencyFilter\": {\"WaitingTimeInMsForFilter\" : 1000}, \"PngConverter\": {}}";
+ std::string export_name ="MemoryExport";
+ std::string export_path = "/tmp";
} p;
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
index bf212bb37f252afb3e848c8fe322d74a4b550b22..821f131bb10b628380359b601aac4d02ff5dbcb7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
@@ -23,12 +23,12 @@ namespace armarx::armem::server::ltm::detail
void setMemoryID(const MemoryID&);
void setMemoryName(const std::string& memoryName);
- std::string
- getExportName() const
+ virtual std::string getExportName() const
{
return exportName;
}
+
MemoryID
getMemoryID() const
{
@@ -54,7 +54,7 @@ namespace armarx::armem::server::ltm::detail
std::shared_ptr<Processors> processors;
private:
- std::string exportName = "MemoryExport";
+ std::string exportName = "MemoryExportMI";
MemoryID _id;
};
} // namespace armarx::armem::server::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
index 65021cdbb9a7487dc20f2ea930cb7a37d109e8fc..86f6306c700b9888636c6d7087cc849558a59417 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
@@ -70,8 +70,8 @@ namespace armarx::armem::server::ltm::detail
/// find entity segment
virtual std::shared_ptr<EntityT> findEntity(const std::string&) const = 0;
- aron::type::ObjectPtr
- aronType() const
+ ///get aron type
+ aron::type::ObjectPtr aronType() const
{
return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
index 0bbf62f24ab3dd9427a9854bfadf8fe048c98bb1..bc94ec838a5afda6a2d26740f068592bbf49a607 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -1,5 +1,9 @@
#pragma once
+#include <atomic>
+#include <chrono>
+#include <thread>
+
#include <SimoxUtility/json.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -31,6 +35,12 @@ namespace armarx::armem::server::ltm::detail::mixin
TIMING_END_STREAM(LTM_Memory_DirectlyStore, ARMARX_DEBUG);
}
+ void
+ bufferFinished()
+ {
+ //use this to count how much work is still left
+ }
+
protected:
void
setMixinMemoryID(const MemoryID& id)
@@ -51,6 +61,8 @@ namespace armarx::armem::server::ltm::detail::mixin
task = new armarx::PeriodicTask<BufferedMemoryMixin>(
this, &BufferedMemoryMixin::storeBuffer, waitingTimeMs);
task->start();
+ task->setDelayWarningTolerance(
+ waitingTimeMs); //a warning will be issued if the task takes longer than the waitingTime
}
}
@@ -62,6 +74,7 @@ namespace armarx::armem::server::ltm::detail::mixin
task->stop();
task = nullptr;
}
+
}
armem::wm::Memory
@@ -87,7 +100,12 @@ namespace armarx::armem::server::ltm::detail::mixin
return;
}
+ while (storeFlag.test_and_set(std::memory_order_acquire))
+ {
+ std::this_thread::yield();
+ }
this->directlyStore(*to_store);
+ storeFlag.clear(std::memory_order_release);
}
/// configuration
@@ -97,9 +115,17 @@ namespace armarx::armem::server::ltm::detail::mixin
if (json.find("BufferedMemory.storeFrequency") != json.end())
{
storeFrequency = json.at("BufferedMemory.storeFrequency");
+ ARMARX_INFO << "Setting store frequency from configuration json to "
+ << storeFrequency;
}
}
+ void
+ createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
+ {
+ defs->optional(storeFrequency, prefix + "storeFrequency");
+ }
+
virtual void _directlyStore(const armem::wm::Memory& memory) = 0;
void
@@ -109,6 +135,7 @@ namespace armarx::armem::server::ltm::detail::mixin
buffer->append(memory);
}
+
protected:
/// Internal memory for data consolidated from wm to ltm (double-buffer)
/// The to-put-to-ltm buffer (contains data in plain text)
@@ -116,6 +143,7 @@ namespace armarx::armem::server::ltm::detail::mixin
/// This means that it is not guaranteed that all data in the buffer will be stored in the ltm
std::unique_ptr<armem::wm::Memory> buffer;
std::unique_ptr<armem::wm::Memory> to_store;
+ std::atomic_flag storeFlag = ATOMIC_FLAG_INIT;
/// The frequency (Hz) to store data to the ltm
float storeFrequency = 10;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
index 8ceaf84eb19deaa0db3efdd5595179ed8d4bcbd4..19bfcd18bdb28870df2a9e69b1d0c74e387e07ba 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
@@ -23,15 +23,36 @@ namespace armarx::armem::server::ltm::detail::mixin
{
}
- void
- DiskMemoryItemMixin::setMemoryBasePath(const Path& n)
- {
- memoryBasePath = n;
+ Path
+ DiskMemoryItemMixin::addDateToMemoryBasePath(const Path& n) const
+ {
+ //set path to include date of creation as prefix:
+ auto current_date = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+ std::tm* localTime = std::localtime(¤t_date);
+
+ // convert current time into string:
+ std::stringstream ss;
+ ss << std::put_time(localTime, "%Y_%m_%d");
+ std::string dateString = ss.str();
+
+ // change memory base path to include current date (date of memory creation)
+ std::filesystem::path current_base_path = n;
+ current_base_path.append(dateString);
+
+ //inform user about change:
+ ARMARX_DEBUG << "Changed memory base path to include current date of "
+ << dateString
+ << " to "
+ << this->getMemoryBasePath()
+ ;
+
+ return current_base_path;
}
void
DiskMemoryItemMixin::setMixinExportName(const std::string& n)
{
+ ARMARX_INFO << "Currently setting export name to: " << n;
exportName = n;
}
@@ -48,21 +69,31 @@ namespace armarx::armem::server::ltm::detail::mixin
{
if (json.find("DiskMemory.memoryParentPath") != json.end())
{
- memoryBasePath = Path(json.at("DiskMemory.memoryParentPath"));
+ //memoryBasePath = Path(json.at("DiskMemory.memoryParentPath"));
}
}
Path
DiskMemoryItemMixin::getMemoryBasePath() const
{
- return memoryBasePath;
+ if(memoryBasePathString.empty()){
+ return memoryBasePath;
+ } else {
+ std::filesystem::path newPath;
+ newPath.assign(memoryBasePathString);
+ return this->addDateToMemoryBasePath(newPath);
+ }
}
Path
DiskMemoryItemMixin::getFullPath() const
{
auto p = getMemoryBasePath() / exportName;
- return util::fs::toPath(p, _id);
+ //ARMARX_INFO << VAROUT(_id);
+ //ARMARX_INFO << VAROUT(_id.cleanID());
+ auto cleanID = _id.cleanID(); //somehow, the iDs are jumbled when loading the LTM from disk, this solves it for now
+
+ return util::fs::toPath(p, cleanID);
}
bool
@@ -134,4 +165,9 @@ namespace armarx::armem::server::ltm::detail::mixin
auto p = getFullPath();
return util::fs::getAllFiles(p);
}
+
+ void DiskMemoryItemMixin::createPropertyDefinitions(PropertyDefinitionsPtr &defs, const std::string &prefix)
+ {
+ defs->optional(memoryBasePathString, prefix + "exportPath");
+ }
} // namespace armarx::armem::server::ltm::detail::mixin
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h
index b9550d4117a0c06675b3788e3c0065c040677fd3..bf02aa5dfdae044416a0d15e91cbdf18f6b1dd83 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h
@@ -44,16 +44,20 @@ namespace armarx::armem::server::ltm::detail::mixin
std::vector<Path> getAllDirectories() const;
std::vector<Path> getAllFiles() const;
+ void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix);
+
protected:
// setter
void setMixinMemoryID(const MemoryID& n);
- void setMemoryBasePath(const std::filesystem::path& n);
void setMixinExportName(const std::string& n);
- /// configuration
+ // configuration
void configureMixin(const nlohmann::json& json);
+ //construct correct memory base path
+ Path addDateToMemoryBasePath(const std::filesystem::path& n) const;
+
public:
static const int DEPTH_TO_DATA_FILES =
7; // from memory folder = 1 (cseg) + 1 (pseg) + 1 (ent) + 3 (snap) + 1 (inst)
@@ -67,6 +71,7 @@ namespace armarx::armem::server::ltm::detail::mixin
private:
std::filesystem::path memoryBasePath;
+ std::string memoryBasePathString;
std::string exportName;
armem::MemoryID _id;
};
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
index 9908e6c5b7f379be2ab7207a28610183c87395f5..5cb419cc05ccc2475bc1ae414b9303dbef25647c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
@@ -8,15 +8,16 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <bsoncxx/builder/stream/array.hpp>
-#include <bsoncxx/builder/stream/document.hpp>
-#include <bsoncxx/builder/stream/helpers.hpp>
-#include <bsoncxx/json.hpp>
+
#include <mongocxx/client.hpp>
#include <mongocxx/instance.hpp>
#include <mongocxx/pool.hpp>
#include <mongocxx/stdx.hpp>
#include <mongocxx/uri.hpp>
+#include <bsoncxx/builder/stream/array.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
+#include <bsoncxx/json.hpp>
namespace armarx::armem::server::ltm::util::mongodb
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
index 2b4f5afa549ab239b91d05ceafe3e49e13c2b36d..a1e16d2e35d2b6a77956799e6eb3545f8324e046 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
@@ -8,6 +8,7 @@
#include "extractor/imageExtractor/ImageExtractor.h"
#include "filter/equalityFilter/EqualityFilter.h"
#include "filter/frequencyFilter/FrequencyFilter.h"
+#include "filter/importanceFilter/ImportanceFilter.h"
namespace armarx::armem::server::ltm
{
@@ -15,21 +16,51 @@ namespace armarx::armem::server::ltm
Processors::configure(const nlohmann::json& config)
{
// Filters:
+ snapFilters.clear();
if (config.contains(processor::filter::SnapshotFrequencyFilter::NAME))
{
- ARMARX_IMPORTANT << "ADDING SNAPSHOT FILTER";
+ ARMARX_IMPORTANT << "ADDING SNAPSHOT FREQUENCY FILTER";
auto f = std::make_unique<processor::filter::SnapshotFrequencyFilter>();
f->configure(config[processor::filter::SnapshotFrequencyFilter::NAME]);
snapFilters.push_back(std::move(f));
}
+ if(config.contains(processor::filter::SnapshotSimilarityFilter::NAME)){
+ ARMARX_IMPORTANT << "ADDING SNAPSHOT SIMILARITY FILTER";
+ auto f = std::make_unique<processor::filter::SnapshotSimilarityFilter>();
+ f->configure(config[processor::filter::SnapshotSimilarityFilter::NAME]);
+ snapFilters.push_back(std::move(f));
+ }
+ if(config.contains(processor::filter::SnapshotImportanceFilter::NAME)){
+ ARMARX_IMPORTANT << "ADDING SNAPSHOT IMPORTANCE FILTER";
+ auto f = std::make_unique<processor::filter::SnapshotImportanceFilter>();
+ f->configure(config[processor::filter::SnapshotImportanceFilter::NAME]);
+ snapFilters.push_back(std::move(f));
+ }
// Converters
if (config.contains(processor::converter::data::image::PngConverter::NAME))
{
- ARMARX_IMPORTANT << "ADDING IMG CONVERTER";
+ ARMARX_IMPORTANT << "ADDING IMG CONVERTER PNG";
auto f = std::make_unique<processor::converter::data::image::PngConverter>();
f->configure(config[processor::converter::data::image::PngConverter::NAME]);
converters.push_back(std::move(f));
}
+ if (config.contains(processor::converter::data::image::ExrConverter::NAME))
+ {
+ ARMARX_IMPORTANT << "ADDING IMG CONVERTER EXR";
+ auto f = std::make_unique<processor::converter::data::image::ExrConverter>();
+ f->configure(config[processor::converter::data::image::ExrConverter::NAME]);
+ converters.push_back(std::move(f));
+ }
+ }
+
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> Processors::getSnapshotFilterStatistics()
+ {
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> stats;
+ ARMARX_INFO << "Number of active filters: " << snapFilters.size();
+ for(int i = 0; i < snapFilters.size(); i++){
+ stats[snapFilters.at(i)->getName()] = snapFilters.at(i)->getFilterStatistics();
+ }
+ return stats;
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
index 0f4186a2c872f30d6ef1bb9b3ce7652232e0c669..b5dc5c618bb8eac2258fdac4f626c113959552ad 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
@@ -23,6 +23,8 @@ namespace armarx::armem::server::ltm
void configure(const nlohmann::json& config);
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> getSnapshotFilterStatistics();
+
public:
// Unique Memory Filters
std::vector<std::unique_ptr<processor::MemoryFilter>> memFilters;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
index ce9cc7ab220fd794442b69c4b826b647122d022e..b65d01925c32dd8d45345c6dba78195713a83be5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
@@ -9,6 +9,9 @@ namespace armarx::armem::server::ltm::processor::converter::data::image
class ExrConverter : public ImageConverter
{
public:
+
+ static const constexpr char* NAME = "ExrConverter";
+
ExrConverter() :
ImageConverter(ConverterType::Binary,
"depthimage",
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h
index 8fb622d281147068c2c01d4902258867ce3545e3..34904df5c275158b25f9aebd64e60a3ef4f96499 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h
@@ -21,6 +21,7 @@ namespace armarx::armem::server::ltm::processor::converter::data::image
void configure(const nlohmann::json& json) override;
+
protected:
ConversionResult _convert(const aron::data::NDArrayPtr& data) final;
aron::data::NDArrayPtr _convert(const ConversionResult& data,
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
index c4c53c68888b5fe576572ef8166f9a20807d27ba..c947d7896c1ff08d04389857eeab3b18e5e79fed 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
@@ -11,4 +11,16 @@ namespace armarx::armem::server::ltm::processor
SnapshotFilter::configure(const nlohmann::json& json)
{
}
+
+ SnapshotFilter::FilterStatistics SnapshotFilter::getFilterStatistics()
+ {
+ return stats;
+ }
+
+ std::string SnapshotFilter::getName()
+ {
+ return "Base_Filter";
+ }
+
+
} // namespace armarx::armem::server::ltm::processor
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
index 9d84c0a0ffdd976b54deb4585279d28161223da6..9b32dc78511a9a9b1ae7bd819558ba43c963ee3c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
@@ -7,6 +7,7 @@
#include <SimoxUtility/json.h>
// ArmarX
+#include "RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
@@ -30,5 +31,20 @@ namespace armarx::armem::server::ltm::processor
virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
virtual void configure(const nlohmann::json& json);
+
+ struct FilterStatistics {
+ double accepted = 0;
+ double rejected = 0;
+ std::chrono::duration<double> additional_time = std::chrono::duration<double>::zero();
+ std::string additional_info = "";
+ aron::similarity::NDArraySimilarity::Type similarity_type;
+ std::chrono::high_resolution_clock::time_point start_time;
+ std::chrono::high_resolution_clock::time_point end_time;
+ int number_of_compared_objects = 2;
+ std::string importance_type = "";
+ } stats;
+
+ virtual FilterStatistics getFilterStatistics();
+ virtual std::string getName();
};
} // namespace armarx::armem::server::ltm::processor
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
index 92c8bb26ecf818492401e938b1c95cecb17519e1..715520f7b51d362cfab9df471ef677e5d926e464 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
@@ -1,51 +1,183 @@
#include "EqualityFilter.h"
+#include <list>
#include <IceUtil/Time.h>
+#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
+
namespace armarx::armem::server::ltm::processor::filter
{
bool
SnapshotSimilarityFilter::accept(const armem::wm::EntitySnapshot& e)
{
- auto entityID = e.id().getEntityID();
- auto genMs = e.time().toMilliSecondsSinceEpoch();
+ auto start = std::chrono::high_resolution_clock::now();
- long lastMs = 0;
- std::vector<aron::data::DictPtr> lastData;
- if (timestampLastCommitInMs.count(entityID) > 0)
- {
- lastData = dataLastCommit.at(entityID);
- lastMs = timestampLastCommitInMs.at(entityID);
+ int num_instances = 0;
+ std::vector<armarx::aron::data::NDArrayPtr> images_snapshot;
+ std::vector<armarx::aron::data::FloatPtr> floats_snapshot;
+ std::vector<float> distances;
+
+ e.forEachInstance([&num_instances, &images_snapshot, &floats_snapshot](armem::wm::EntityInstance& i){
+ auto data = aron::data::Dict::DynamicCastAndCheck(i.data());
+ for(auto key: data->getAllKeys()){
+ aron::data::Descriptor img_desc;
+ try{
+ auto d = data->at(key);
+; img_desc = data->at(key)->getDescriptor();
+ } catch (...){
+ ARMARX_INFO << "Problem with accessing image description";
+ img_desc = aron::data::Descriptor::NDARRAY;
+ }
+ if (img_desc == aron::data::Descriptor::NDARRAY){
+ auto img_nd = aron::data::NDArray::DynamicCastAndCheck(data->at(key));
+ images_snapshot.insert(images_snapshot.end(), img_nd);
+ num_instances++;
+ } else if (img_desc == aron::data::Descriptor::FLOAT){
+ auto fl = aron::data::Float::DynamicCastAndCheck(data->at(key));
+ floats_snapshot.push_back(fl);
+ num_instances++;
+ } else {
+ ARMARX_INFO << "data-type not yet supported. \n"
+ << "Only ndarray and float data types are supported for equality filters yet.";
+ }
+ }
+ });
+
+ if(images.size() < 2){
+ ARMARX_INFO << "Adding first images, because nothing to compare";
+ images.push_back(images_snapshot);
+ this->stats.accepted += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ return true;
+ } else if(images.size() < max_images){
+ ARMARX_INFO << "Not enough elements yet to do full comparison of last " << max_images
+ << " elements";
+ images.push_back(images_snapshot);
+ this->stats.accepted += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ return true;
}
- bool accept = false;
- std::vector<aron::data::DictPtr> genData;
- for (unsigned int i = 0; i != e.size(); ++i)
- {
- const auto& d = e.getInstance(i).data();
- genData.push_back(d);
-
- if (lastMs == 0 ||
- e.size() != lastData.size()) // nothing stored yet or we cannot compare
- {
- accept = true;
- break;
+ std::vector<armarx::aron::data::NDArrayPtr> lastCommittedImages;
+ int sizeOfCommited = 0;
+ for(int i = 0; i < max_images; i++){
+ std::vector<armarx::aron::data::NDArrayPtr> lastCommitImages = images.at(images.size() - i - 1);
+ sizeOfCommited = lastCommitImages.size();
+ for(int j = 0; j < lastCommitImages.size(); j++){
+ lastCommittedImages.push_back(lastCommitImages.at(j));
}
+ // we just concatenate all images (instances)
+ }
- const auto& el = lastData.at(i);
- if ((!d and el) || (d and !el) || (d && el && !(*d == *el))) // data unequal?
- {
- accept = true;
- break;
+ ARMARX_CHECK(sizeOfCommited == images_snapshot.size()); //make sure we have enough instances
+
+ for(int i= 0; i < images_snapshot.size(); i++){
+ armarx::aron::data::NDArrayPtr new_image = images_snapshot.at(i);
+ std::vector<armarx::aron::data::NDArrayPtr> commited_images;
+ for(int j = 0; j < max_images; j++){
+ int index = i + 2*j;
+ auto image = lastCommittedImages.at(index);
+ commited_images.emplace_back(image);
}
+
+ float distance = aron::similarity::NDArraySimilarity::calculate_similarity_multi(commited_images, new_image, this->similarity_type);
+
+ distances.insert(distances.end(), distance);
+ }
+
+ //check for criterion:
+ float sum_distances = 0;
+ float max_distance = 0;
+ for(auto d: distances){
+ sum_distances += d;
+ if(d > max_distance){
+ max_distance = d;
+ }
+ }
+
+ bool max = true; //set true if only maximum distance value is important and false if sum of distances is important
+ bool accept = false;
+
+ if(max){
+ accept = (max_distance > this->threshold);
+ } else {
+ accept = (sum_distances > this->threshold);
}
- if (!accept)
+ if(accept){
+ images.pop_front(); //delete first element
+ images.push_back(images_snapshot);
+ this->stats.accepted += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.additional_time += (end - start);
+ stats.end_time = end;
+ return true;
+ } else {
+ this->stats.rejected += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.additional_time += (end - start);
+ stats.end_time = end;
return false;
+ }
+ }
+
+ void SnapshotSimilarityFilter::configure(const nlohmann::json &json)
+ {
+ if (json.find(PARAM_THRESHOLD) != json.end())
+ {
+ threshold = json.at(PARAM_THRESHOLD);
+ ARMARX_INFO << VAROUT(threshold);
+ stats.additional_info += "Threshold-Parameter: ";
+ stats.additional_info += std::to_string(threshold);
+ }
+ if(json.find(PARAM_SIM_MEASURE) != json.end()){
+ std::string type_string = json.at(PARAM_SIM_MEASURE);
+ if(type_string == "MSE"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MSE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
+ } else if (type_string == "MAE"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MAE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
+ } else if (type_string == "Chernoff"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
+ } else if (type_string == "Cosine"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
+ } else {
+ ARMARX_WARNING << "Undefined similarity measure detected in JSON file";
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
+ }
+ ARMARX_INFO << VAROUT(this->similarity_type);
+ }
+ if(json.find(PARAM_MAX_OBJECTS) != json.end()){
+ max_images = json.at(PARAM_MAX_OBJECTS);
+ ARMARX_INFO << VAROUT(max_images);
+ stats.number_of_compared_objects = max_images;
+ }
+
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ }
+
+ SnapshotFilter::FilterStatistics SnapshotSimilarityFilter::getFilterStatistics()
+ {
+ return stats;
+ }
- dataLastCommit[entityID] = genData;
- timestampLastCommitInMs[entityID] = genMs;
- return true;
+ std::string SnapshotSimilarityFilter::getName()
+ {
+ return this->NAME;
}
+
} // namespace armarx::armem::server::ltm::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
index 203b1fe3fa87108b144c604a59ebf46e58ab350f..67448db2dba914f0d7de8f392433e55f03725026 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
@@ -8,6 +8,10 @@
// Aron
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include "RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h"
+#include "RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h"
+
+#include <chrono>
namespace armarx::armem::server::ltm::processor::filter
{
@@ -15,16 +19,28 @@ namespace armarx::armem::server::ltm::processor::filter
{
public:
static const constexpr char* NAME = "SnapshotSimilarityFilter";
+ static const constexpr char* PARAM_THRESHOLD = "Threshold";
+ static const constexpr char* PARAM_SIM_MEASURE = "SimilarityMeasure";
+ static const constexpr char* PARAM_MAX_OBJECTS = "NumberOfObjectsToCompare";
SnapshotSimilarityFilter() = default;
virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ void configure(const nlohmann::json& json) override;
- public:
- // TODO link aron/similarity
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
private:
- std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+ //std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+ std::deque<std::vector<armarx::aron::data::NDArrayPtr>> images;
+ std::deque<std::vector<armarx::aron::data::FloatPtr>> floats;
std::map<MemoryID, long> timestampLastCommitInMs;
+ std::double_t threshold;
+ FilterStatistics stats;
+ int max_images = 2;
+ aron::similarity::NDArraySimilarity::Type similarity_type;
+ aron::similarity::FloatSimilarity::Type float_similarity_type;
+
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
index 56c7250c37988efd1b5fc824aec8aabd2e040a17..aa773fc1faf83bb36a43801620def4508cf604cd 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
@@ -1,51 +1,49 @@
#include "FrequencyFilter.h"
-#include <IceUtil/Time.h>
+#include <chrono>
namespace armarx::armem::server::ltm::processor::filter
{
+
bool
- MemoryFrequencyFilter::accept(const armem::wm::Memory& e)
+ SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot &e)
{
- auto now = armem::Time::Now().toMilliSecondsSinceEpoch();
- if (waitingTimeInMs < 0 || (now - timestampLastCommitInMs) > waitingTimeInMs)
- {
- timestampLastCommitInMs = now;
- return true;
- }
- return false;
- }
- void
- MemoryFrequencyFilter::configure(const nlohmann::json& json)
- {
- if (json.find(PARAM_WAITING_TIME) != json.end())
- {
- waitingTimeInMs = json.at(PARAM_WAITING_TIME);
+ //accepting to many elements makes the filter slow and brings problems with the buffer with itself.
+ auto start = std::chrono::high_resolution_clock::now();
+ bool instances_accepted = false;
+ std::int64_t current;
+
+ if(this->lastTime < 1){ //this is the first instance to be saved
+ lastTime = e.time().toMilliSecondsSinceEpoch();
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ stats.accepted += 1;
+ return true; //the first one is always accepted
}
- }
- bool
- SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot& e)
- {
- auto entityID = e.id().getEntityID();
- auto genMs = e.time().toMilliSecondsSinceEpoch();
+ e.forEachInstance([this, &instances_accepted, ¤t](armem::wm::EntityInstance& i){
+ int difference = std::abs(i.metadata().sentTime.toMilliSecondsSinceEpoch() - lastTime);
+ if(difference > this->maxDifference){ //at least one instance is older than the last saved instance
+ instances_accepted = true;
+ current = i.metadata().sentTime.toMilliSecondsSinceEpoch();
+ }
+ });
- long lastMs = 0;
- if (timestampLastCommitInMs.count(entityID) > 0)
- {
- lastMs = timestampLastCommitInMs.at(entityID);
- }
+ //set stats:
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
- if (waitingTimeInMs < 0 || (genMs - lastMs) > waitingTimeInMs)
- {
- /*std::cout << "diff: " << (dataGeneratedInMs - timestampLastCommitInMs) << std::endl;
- std::cout << "gen: " << (dataGeneratedInMs) << std::endl;
- std::cout << "last: " << (timestampLastCommitInMs) << std::endl;*/
- timestampLastCommitInMs[entityID] = genMs;
- return true;
+ if(instances_accepted){
+ lastTime = current;
+ this->stats.accepted += 1;
+ } else {
+ this->stats.rejected += 1;
}
- return false;
+
+ return instances_accepted;
}
void
@@ -53,7 +51,23 @@ namespace armarx::armem::server::ltm::processor::filter
{
if (json.find(PARAM_WAITING_TIME) != json.end())
{
- waitingTimeInMs = json.at(PARAM_WAITING_TIME);
+ this->maxDifference = json.at(PARAM_WAITING_TIME);
+ ARMARX_INFO << VAROUT(maxDifference);
+ stats.additional_info = "Max Difference in ms: " + std::to_string(this->maxDifference);
}
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ stats.number_of_compared_objects = 1;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
+ }
+
+ SnapshotFilter::FilterStatistics SnapshotFrequencyFilter::getFilterStatistics()
+ {
+ stats.end_time = std::chrono::high_resolution_clock::now();
+ return this->stats;
+ }
+
+ std::string SnapshotFrequencyFilter::getName()
+ {
+ return this->NAME;
}
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
index 00ba1e9db0162d4d866a359c920008838ad5e279..1b778528ebd0812c91dada5f1e275f9bfcfa44ca 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
@@ -1,45 +1,28 @@
#pragma once
-#include <map>
-
// Base Class
#include "../Filter.h"
namespace armarx::armem::server::ltm::processor::filter
{
- class MemoryFrequencyFilter : public MemoryFilter
- {
- public:
- static const constexpr char* NAME = "MemoryFrequencyFilter";
- static const constexpr char* PARAM_WAITING_TIME = "WaitingTimeInMs";
-
- MemoryFrequencyFilter() = default;
-
- virtual bool accept(const armem::wm::Memory& e) override;
- void configure(const nlohmann::json& json) override;
-
- public:
- int waitingTimeInMs = -1;
-
- private:
- long timestampLastCommitInMs = 0;
- };
class SnapshotFrequencyFilter : public SnapshotFilter
{
public:
static const constexpr char* NAME = "SnapshotFrequencyFilter";
- static const constexpr char* PARAM_WAITING_TIME = "WaitingTimeInMs";
+ static const constexpr char* PARAM_WAITING_TIME = "WaitingTimeInMsForFilter";
SnapshotFrequencyFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ virtual bool accept(const armem::wm::EntitySnapshot &e) override;
void configure(const nlohmann::json& json) override;
- public:
- int waitingTimeInMs = -1;
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
private:
- std::map<MemoryID, long> timestampLastCommitInMs;
+ int maxDifference = 0;
+ std::int64_t lastTime = 0;
+
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ae255a4d4488048c7786aa1fafa3d67f2538f77
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
@@ -0,0 +1,84 @@
+#include "ImportanceFilter.h"
+
+
+namespace armarx::armem::server::ltm::processor::filter
+{
+
+bool SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot &e)
+{
+ auto start = std::chrono::high_resolution_clock::now();
+ bool instances_accepted = false;
+
+ e.forEachInstance([this, &instances_accepted](armem::wm::EntityInstance& i){
+ if(!instances_accepted){ //if no instance was accepted yet
+ instances_accepted = important(i); //see if another instance triggers acceptance
+ }
+ });
+
+ //set stats:
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ if(instances_accepted){
+ stats.accepted += 1;
+ } else {
+ stats.rejected += 1;
+ }
+
+ return instances_accepted;
+}
+
+void SnapshotImportanceFilter::configure(const nlohmann::json &json)
+{
+ if(json.find(PARAM_THRESHOLD) != json.end()){
+ this->threshold = json.at(PARAM_THRESHOLD);
+ ARMARX_INFO << VAROUT(this->threshold);
+ stats.additional_info += "Threshold for importance: ";
+ stats.additional_info += std::to_string(this->threshold);
+ }
+ if(json.find(PARAM_TYPE) != json.end()){
+ auto t = json.at(PARAM_TYPE);
+ if(t == "Confidence"){
+ this->type = ImportanceType::CONFIDENCE;
+ }
+ if(t == "Accesses"){
+ this->type = ImportanceType::ACCESSES;
+ }
+ stats.importance_type = t;
+ ARMARX_INFO << VAROUT(stats.importance_type);
+ }
+
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ stats.number_of_compared_objects = 1;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
+}
+
+SnapshotFilter::FilterStatistics SnapshotImportanceFilter::getFilterStatistics()
+{
+ return this->stats;
+}
+
+std::string SnapshotImportanceFilter::getName()
+{
+ return this->NAME;
+}
+
+bool SnapshotImportanceFilter::important(armem::wm::EntityInstance &i)
+{
+ if(this->type == ImportanceType::CONFIDENCE){
+ auto c = i.metadata().confidence;
+ if ( c > this->threshold){
+ //for now the whole entity snapshot is accepted if at least one instance has a confidence higher than the threshold
+ return true;
+ }
+ } else if (this->type == ImportanceType::ACCESSES){
+ auto a = i.metadata().numAccessed;
+ if (a > this->threshold){
+ return true;
+ }
+ }
+ return false;
+}
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..929bb2e2ac6b03b31f9225cbda74b4a4b34f8e8d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
@@ -0,0 +1,41 @@
+#pragma once
+
+// Base Class
+#include "../Filter.h"
+
+#include <chrono>
+
+namespace armarx::armem::server::ltm::processor::filter
+{
+
+enum ImportanceType{
+ CONFIDENCE,
+ ACCESSES
+};
+
+class SnapshotImportanceFilter : public SnapshotFilter
+{
+public:
+
+ static const constexpr char* NAME = "SnapshotImportanceFilter";
+ static const constexpr char* PARAM_THRESHOLD = "Threshold";
+ static const constexpr char* PARAM_TYPE = "Type";
+
+ SnapshotImportanceFilter() = default;
+
+ virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ void configure(const nlohmann::json& json) override;
+
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
+
+private:
+ double threshold = 1.0;
+ FilterStatistics stats;
+ ImportanceType type;
+
+ bool important(armem::wm::EntityInstance& i);
+
+};
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index d379cf033d76392d07417b0345aa09a3cca900bf..665d49ca87dbb6596e70f97530ff589da9b34338 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -8,6 +8,8 @@
#include <RobotAPI/libraries/armem/client/util/MemoryListener.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <chrono>
+
namespace armarx::armem::server::plugins
{
@@ -41,7 +43,6 @@ namespace armarx::armem::server::plugins
workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
}
- // stuff for ltm
longtermMemory.createPropertyDefinitions(properties, prefix + "ltm.");
}
@@ -93,10 +94,35 @@ namespace armarx::armem::server::plugins
void
Plugin::preOnDisconnectComponent()
{
+ ARMARX_INFO << "Preparing to save statistics for " << this->workingMemory.name();
+ try{
+ auto first_stats = longtermMemory.getFilterStatistics();
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> information;
+ std::map<std::string, armarx::core::time::DateTime> times;
+
+ try{
+ times["Started LTM1"] = longtermMemory.getStatistics().firstStarted;
+ times["Stopped LTM1"] = longtermMemory.getStatistics().firstStopped;
+ information["LTM"] = first_stats;
+ } catch(...){
+ ARMARX_INFO << "Something went wrong after getting the statistics";
+ }
+ auto exportPath = longtermMemory.getMemoryBasePath();
+ auto exportName = longtermMemory.getExportName();
+ auto recording_started = longtermMemory.getStatistics().firstStarted;
+ auto recording_stopped = longtermMemory.getStatistics().firstStopped;
+ test::save_statistics(information, times, recording_started, recording_stopped,
+ exportPath, exportName, longtermMemory.name());
+ }catch (...){
+ ARMARX_INFO << "Something went wrong with the statistics saving process";
+ }
+
+ statistics_saved = true;
+
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
removeServer();
- }
+ }
}
void
@@ -114,6 +140,7 @@ namespace armarx::armem::server::plugins
mns::dto::RegisterServerResult
Plugin::registerServer(armarx::Component& parent)
{
+ ARMARX_INFO << "Registering server for " << workingMemory.name();
ARMARX_TRACE;
MemoryID id = MemoryID().withMemoryName(workingMemory.name());
@@ -137,6 +164,8 @@ namespace armarx::armem::server::plugins
result.errorMessage = e.what();
ARMARX_WARNING << e.what();
}
+
+
return result;
}
@@ -152,6 +181,7 @@ namespace armarx::armem::server::plugins
result.success = true;
ARMARX_DEBUG << "Removed memory server for " << id
<< " from the Memory Name System (MNS).";
+
}
catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
{
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
index e37c5e093232af4aae66114703a7debb8f122ba4..0712e4d9f2281adf2c435a7585a019b97907c9d7 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -85,6 +85,7 @@ namespace armarx::armem::server::plugins
std::atomic_bool initialized = false;
std::atomic_bool connected = false;
+ std::atomic_bool statistics_saved = false;
};
} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0025f3e20582dea2ab6891cf11ec9b8b5584a989
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
@@ -0,0 +1,280 @@
+#include "ForgettingExperiments.h"
+
+#include <iostream>
+#include <fstream>
+#include <chrono>
+#include <filesystem>
+
+namespace armarx::armem::server::test
+{
+
+void get_information_single_ltm(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>& filter
+ )
+{
+ /*
+ nlohmann::json info;
+ for(const auto& pair: filter.second){
+ if(pair.second.accepted + pair.second.rejected < 1){
+ ended_without_recording = true;
+ ARMARX_INFO << "Trying to not add JSON file because accepted + rejected < 1";
+ }
+ std::map<std::string, std::string> information;
+ information["Memory Name: "] = memoryName;
+ information["additional time needed: "] = (std::to_string(pair.second.additional_time.count()) + " sec");
+ information["number of accepted elements: "] = std::to_string(pair.second.accepted);
+ information["number of rejected elements: "] = std::to_string(pair.second.rejected);
+ information["Additional information: "] = pair.second.additional_info;
+ information["Similarity-Type: "] = std::to_string(pair.second.similarity_type);
+ information["Number of objects compared each time: "] = std::to_string((pair.second.number_of_compared_objects));
+ information["Importance type: "] = pair.second.importance_type;
+ auto time = std::chrono::high_resolution_clock::to_time_t(pair.second.start_time);
+ auto t = localtime(&time);
+ std::stringstream ss;
+ ss << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ std::string timeString = ss.str();
+ information["Filter instance created at: "] = timeString;
+ time = std::chrono::high_resolution_clock::to_time_t(pair.second.end_time);
+ t = localtime(&time);
+ std::stringstream s;
+ s << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ timeString = s.str();
+ information["Filter instance last used at: "] = timeString;
+ info[pair.first] = information;
+
+ armarx::core::time::DateTime start_time = time_stats["Started " + filter.first];
+ auto started = start_time.toDateTimeString();
+ info["Started"] = started;
+ ARMARX_INFO << started;
+
+ armarx::core::time::DateTime stop_time = time_stats["Stopped " + filter.first];
+ auto stopped = stop_time.toDateTimeString();
+ info["Stopped"] = stopped;
+ ARMARX_INFO << stopped;
+ }
+ jsonData[filter.first] = info;
+ info.clear();
+ */
+}
+
+void save_statistics(
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> stats,
+ std::map<std::string, armarx::core::time::DateTime> time_stats,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ std::filesystem::path exportPath,
+ std::string exportName,
+ std::string memoryName,
+ std::string sim_json_data,
+ std::string object_memory_json_data)
+{
+ ARMARX_INFO << "Saving statistics";
+
+ std::filesystem::path d_p;
+ try{
+ d_p = test::getStatisticsDirectory(exportPath, exportName, memoryName);
+ std::filesystem::create_directories(d_p);
+ ARMARX_INFO << "Experiments will be saved at: " << d_p.string();
+ } catch(...){
+ ARMARX_WARNING << "Creating needed directories for saving statistics did not work";
+ }
+
+ //json object for statistics:
+ nlohmann::json jsonData;
+
+ //get current date:
+ auto now = std::chrono::high_resolution_clock::now();
+ auto now_time = std::chrono::high_resolution_clock::to_time_t(now);
+ auto time = localtime(&now_time);
+
+ std::stringstream ss;
+ ss << std::put_time(time, "%Y-%m-%d %H:%M:%S");
+ std::string timeString = ss.str();
+ std::stringstream s;
+ s << std::put_time(time, "%Y%m%d%H%M%S");
+ std::string timeID = s.str();
+ jsonData["Statistics saved at"] = timeString;
+
+ bool ended_without_recording = false;
+
+ for(const auto& filter: stats){
+ filter_statistics(filter.second, memoryName,firstStartedRecording, firstStoppedRecording, &jsonData);
+ }
+
+ if(ended_without_recording){
+ return;
+ }
+
+ /*
+ if(sim_json_data.empty() || object_memory_json_data.empty()){
+ jsonData["Scene-Information for Simulation"] = load_scene_information("sim");
+ jsonData["Scene-Information for ObjectMemory"] = load_scene_information("om");
+ jsonData["Additional object in simulation"] = find_difference(jsonData["Scene-Information for ObjectMemory"], jsonData["Scene-Information for Simulation"]);
+ }else {
+ jsonData["Scene-Information for Simulation"] = sim_json_data;
+ jsonData["Scene-Information for ObjectMemory"] = object_memory_json_data;
+ }
+ */
+
+ std::string path = d_p.string();
+ path += "/tmp_wm_";
+ path += timeID;
+ path += ".json";
+ try{
+ std::ofstream outputFile;
+ outputFile.open(path);
+ if(outputFile.is_open()){
+ outputFile << jsonData.dump(4);
+ outputFile.close();
+ } else {
+ if(outputFile.fail()){
+ ARMARX_INFO << outputFile.rdstate();
+ outputFile.clear();
+ }
+ ARMARX_INFO << "File is not open";
+ }
+ } catch(...){
+ ARMARX_WARNING << "std::ofstream failed";
+ }
+
+ ARMARX_INFO << "Saving statistics completed";
+}
+
+nlohmann::json load_scene_information(std::string om_or_sim)
+{
+ std::string home_dir = getenv("HOME");
+ if(home_dir.empty()){
+ ARMARX_WARNING << "Trying to open home directory but env variable HOME is not set";
+ }
+ std::filesystem::path path = home_dir;
+ path /= "code";
+ path /= "h2t" ;
+ path /= "PriorKnowledgeData";
+ path /= "data";
+ path /= "PriorKnowledgeData";
+ path /= "scenes";
+ std::string name_for_current_scene = "ARMAR-III-kitchen"; //this can be changed later, maybe to something that indicates better that these are the changed files
+ std::string file_name = name_for_current_scene + "_" + om_or_sim + ".json";
+ path /= file_name;
+
+ std::ifstream ifs(path);
+ nlohmann::json json_data = nlohmann::json::parse(ifs);
+
+ return json_data;
+
+}
+
+nlohmann::json find_difference(nlohmann::json om, nlohmann::json sim)
+{
+ //the new object is at the last position of sim at the moment
+ auto objects = sim.at("objects");
+ int num_elements = objects.size();
+ auto object = objects[num_elements - 1];
+ return object;
+}
+
+/**
+ * @brief getStatisticsDirectory returns the path to where the statistics for this forgetting process
+ * should be saved
+ * @param memoryName name of the memory, e.g. VisionMemory
+ * @param exportName name of the export
+ * @param exportPath path to the export folder with date for this LTM
+ * @return path
+ */
+std::filesystem::path getStatisticsDirectory(
+ std::filesystem::path exportPath,
+ std::string exportName,
+ std::string memoryName)
+{
+ //path consists of exportPath/Date/exportName/memoryName with the date being the start date!
+ std::filesystem::path statisticsPath = exportPath;
+
+ //add parts to exportPath
+ statisticsPath /= exportName;
+ statisticsPath /= memoryName;
+
+ //add Statistics Directory:
+ statisticsPath /= "Statistics";
+
+ return statisticsPath;
+}
+
+void filter_statistics(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> statistics_processors,
+ std::string memoryName,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ nlohmann::json *statistics)
+{
+ //TODO: start time of processor should be when enabling recording for filters... not creation
+ //TODO: make sure order stays the way it was when added
+ nlohmann::json ltm_information;
+
+ for(const auto& statistics_processor: statistics_processors){
+ nlohmann::json processor_information;
+
+ //get start time of the processor:
+ auto time = std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.start_time);
+ auto t = localtime(&time);
+ std::stringstream ss;
+ ss << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ std::string timeStringStart = ss.str();
+ processor_information["Processor instance created at"] = timeStringStart;
+
+ //get end time of the processor:
+ auto timeEnd = std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.end_time);
+ auto t_end = localtime(&timeEnd);
+ std::stringstream s;
+ s << std::put_time(t_end, "%Y-%m-%d %H:%M:%S");
+ std::string timeStringEnd = s.str();
+ processor_information["Processor instance last used at"] = timeStringEnd;
+
+ //get time the processor ran for in seconds:
+ long seconds = timeEnd - time;
+ processor_information["Lifetime of processor in sec"] = seconds;
+
+ //actual runtime in first recording:
+ processor_information["Processor started at"] = firstStartedRecording.toDateTimeString();
+ processor_information["Processor stopped at"] = firstStoppedRecording.toDateTimeString();
+ auto runtime = firstStoppedRecording.toMilliSecondsSinceEpoch() - firstStartedRecording.toMilliSecondsSinceEpoch();
+ auto seconds_runtime = runtime / 1000.0;
+ processor_information["Duration of recording in seconds"] = seconds_runtime;
+
+ //add additional information:
+ processor_information["Additional information"] = statistics_processor.second.additional_info;
+
+ //add number of accepted elements if this is a filter:
+ processor_information["Accepted elements"] = statistics_processor.second.accepted;
+
+ //add number of rejected elements if this is a filter:
+ processor_information["Rejected elements"] = statistics_processor.second.rejected;
+
+ //add additional time needed:
+ processor_information["Additional time needed in sec"] = statistics_processor.second.additional_time.count();
+
+ //add importance or similarity type if this type of filter:
+ auto importance_type = statistics_processor.second.importance_type;
+ if (!importance_type.empty()){
+ processor_information["Importance type"] = importance_type;
+ }
+ auto similarity_type = statistics_processor.second.similarity_type;
+ if(similarity_type != aron::similarity::NDArraySimilarity::Type::NONE){
+ processor_information["Similarity type"] = std::to_string(similarity_type);
+ //add number of objects compared each time if similarity filter:
+ processor_information["Number of objects compared each time"] =
+ statistics_processor.second.number_of_compared_objects;
+ }
+
+ //add memory name as a sanity check:
+ processor_information["Memory name"] = memoryName;
+
+ // add information about this specific processor to the information about the ltm:
+ ltm_information[statistics_processor.first] = processor_information;
+ }
+
+
+ // add the ltm information to the statistics json object:
+ (*statistics)["Episodic Memory Information"] = ltm_information;
+}
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce929849a4d05cbd343b915d7bad6279aeb0e057
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Joana Plewnia ( uhfpm at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h>
+
+namespace armarx::armem::server::test
+{
+ void save_statistics(
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> stats,
+ std::map<std::string, armarx::core::time::DateTime> time_stats,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ std::filesystem::path exportPath = "/tmp",
+ std::string exportName ="MemoryExports",
+ std::string memoryName = "",
+ std::string sim_json_data = "",
+ std::string object_memory_json_data = ""
+ );
+
+ nlohmann::json load_scene_information(std::string om_or_sim);
+
+ nlohmann::json find_difference(nlohmann::json om, nlohmann::json sim);
+
+ std::filesystem::path getStatisticsDirectory(
+ std::filesystem::path exportPath = "/tmp",
+ std::string exportName ="MemoryExports",
+ std::string memoryName = "NoMemoryName");
+
+ nlohmann::json filter_statistics_comparison();
+
+ void filter_statistics(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> statistics_processors,
+ std::string memoryName,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ nlohmann::json* statistics);
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 6e3ef85cf5ef086450a9896003476d788bcfa69f..863fc5e6efcbad4b7661487d653113199cb4012b 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -21,7 +21,6 @@ set(LIB_FILES
aron_conversions.cpp
rw.cpp
-
data/variant/Variant.cpp
data/variant/detail/SpecializedVariant.cpp
data/variant/detail/ContainerVariant.cpp
diff --git a/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt b/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt
index 92661f2d2ecd6994a9978e6c8d1b2aad2ffb7e9a..f85c7d514e045c8a0dbd5a31a8b918bb5afafcdb 100644
--- a/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt
@@ -1,2 +1,50 @@
+set(LIB_NAME aronsimilarity)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
#add_subdirectory(data/aron)
#add_subdirectory(data/image)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
+
+set(LIBS
+ aron
+)
+
+set(LIB_FILES
+
+ data/image/NDArraySimilarity.cpp
+ data/image/mse.cpp
+ data/image/mae.cpp
+ data/image/chernoff.cpp
+ data/image/FloatSimilarity.cpp
+ cosine.cpp
+
+)
+
+set(LIB_HEADERS
+
+ data/image/NDArraySimilarity.h
+ data/image/mse.h
+ data/image/mae.h
+ data/image/chernoff.h
+ data/image/FloatSimilarity.h
+ cosine.h
+
+)
+
+
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
+)
+
+add_library(RobotAPI::aron::similarity ALIAS aronsimilarity)
diff --git a/source/RobotAPI/libraries/aron/similarity/cosine.cpp b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..396ea66133cf66e6dc1f268c8f3c5bc89c47de05
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
@@ -0,0 +1,72 @@
+#include "cosine.h"
+
+#include <SimoxUtility/algorithm/string.h>
+#include <cmath>
+
+namespace armarx::aron::similarity
+{
+ double
+ cosine::compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
+ {
+ //this does only work for RGB images not RGBDepth at the moment!
+ int width = p1->getShape().at(0);
+ int height = p1->getShape().at(1);
+ int dimensions = p1->getShape().at(2);
+ ARMARX_CHECK(dimensions == p2->getShape().at(2));
+ if(dimensions > 3){
+ ARMARX_INFO << "Trying to calculate cosine similarity for more than 3 channels, only first three channels will be looked at";
+ }
+ Eigen::MatrixXf m1r(width, height);
+ Eigen::MatrixXf m2r(width, height);
+ Eigen::MatrixXf m1g(width, height);
+ Eigen::MatrixXf m2g(width, height);
+ Eigen::MatrixXf m1b(width, height);
+ Eigen::MatrixXf m2b(width, height);
+
+ auto image1 = p1->getDataAsVector();
+ auto image2 = p2->getDataAsVector();
+
+
+ for(int x = 0; x < width; x++){
+ for(int y = 0; y < height; y++){
+ //R-value matices:
+ int index = x + width * (y + 0 * height);
+ double element1 = image1.at(index);
+ double element2 = image2.at(index);
+ m1r(x, y) = element1;
+ m2r(x, y) = element2;
+ //G-value matices:
+ index = x + width * (y + 1 * height);
+ element1 = image1.at(index);
+ element2 = image2.at(index);
+ m1g(x, y) = element1;
+ m2g(x, y) = element2;
+ //B-value matices:
+ index = x + width * (y + 2 * height);
+ element1 = image1.at(index);
+ element2 = image2.at(index);
+ m1b(x, y) = element1;
+ m2b(x, y) = element2;
+ }
+ }
+
+
+ double dotProductR = (m1r.array() * m2r.array()).sum();
+ double normProductR = m1r.norm() * m2r.norm();
+ double cosineR = 1.0 - (dotProductR / normProductR);
+ //ARMARX_INFO << VAROUT(cosineR);
+
+ double dotProductG = (m1g.array() * m2g.array()).sum();
+ double normProductG = m1g.norm() * m2g.norm();
+ double cosineG = 1.0 - (dotProductG / normProductG);
+ //ARMARX_INFO << VAROUT(cosineG);
+
+ double dotProductB = (m1b.array() * m2b.array()).sum();
+ double normProductB = m1b.norm() * m2b.norm();
+ double cosineB = 1.0 - (dotProductB / normProductB);
+ //ARMARX_INFO << VAROUT(cosineB);
+
+ return (cosineR + cosineG + cosineB)/ 3;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/cosine.h b/source/RobotAPI/libraries/aron/similarity/cosine.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a5e776ff4b0c13a2fd9c36d1c0a9dfc19b8f51a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/cosine.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::cosine
+{
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2724dc3402316b4bb0c261dd67de788dd6fd1b6d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
@@ -0,0 +1,33 @@
+#include "FloatSimilarity.h"
+#include <cmath>
+#include "ArmarXCore/core/logging/Logging.h"
+
+namespace armarx::aron::similarity{
+
+double FloatSimilarity::calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t)
+{
+ switch (t) {
+ case Type::MAE:
+ return calculateMAE(f1, f2);
+ case Type::MSE:
+ return calculateMSE(f1, f2);
+ default:
+ ARMARX_INFO << "Trying to calculate similarity with unknown similarity type";
+ return -1;
+ }
+}
+
+double FloatSimilarity::calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
+{
+
+ return std::abs(f1->getValue() - f2->getValue());
+}
+
+double FloatSimilarity::calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
+{
+ return std::pow(f1->getValue() - f2->getValue(), 2);
+}
+
+
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
new file mode 100644
index 0000000000000000000000000000000000000000..d325c6d61f2a5dc77ad53eb78a456da2dc07acf2
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <vector>
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
+
+namespace armarx::aron::similarity::FloatSimilarity{
+
+ enum Type {
+ MSE,
+ MAE,
+ NONE
+ };
+
+ double calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t);
+
+ double calculate_similarity_multi(std::vector<armarx::aron::data::FloatPtr>& images, armarx::aron::data::FloatPtr p, Type type);
+
+ double calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2);
+
+ double calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2);
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5fb38a7f8859e804e4ea8a71642402a08ce01e2b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
@@ -0,0 +1,57 @@
+#include "NDArraySimilarity.h"
+
+#include <cmath>
+#include "mse.h"
+#include "mae.h"
+#include "chernoff.h"
+#include "../../cosine.h"
+
+namespace armarx::aron::similarity{
+
+
+ double NDArraySimilarity::calculate_similarity(data::NDArrayPtr p1, data::NDArrayPtr p2, Type type)
+ {
+ switch(type){
+ case Type::MSE:
+ //ARMARX_INFO << "Calculate MSE";
+ return armarx::aron::similarity::mse::compute_similarity(p1, p2);
+ case Type::MAE:
+ return armarx::aron::similarity::mae::compute_similarity(p1, p2);
+ case Type::CHERNOFF:
+ return armarx::aron::similarity::chernoff::compute_similarity(p1, p2);
+ case Type::COSINE:
+ return armarx::aron::similarity::cosine::compute_similarity(p1, p2);
+ default:
+ ARMARX_WARNING << "Trying to calculate similarity with unspecified similarity measurement";
+ return -1;
+ }
+ }
+
+ std::string NDArraySimilarity::to_string(Type t)
+ {
+ switch(t){
+ case MSE:
+ return "MSE";
+ case MAE:
+ return "MAE";
+ case CHERNOFF:
+ return "Chernoff";
+ case COSINE:
+ return "Cosine";
+ default:
+ return "No similarity type information";
+ }
+ }
+
+ double NDArraySimilarity::calculate_similarity_multi(std::vector<data::NDArrayPtr> images, armarx::aron::data::NDArrayPtr p, Type type)
+ {
+ double sim = 0;
+ for(auto& image: images){
+ //ARMARX_INFO << "Before calculation";
+ sim += calculate_similarity(image, p, type);
+ //ARMARX_INFO << "Sim is currently: " << sim;
+ }
+ return sim / (images.size() + 1); // to average it over the distances, makes it easier to find good parameters
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
new file mode 100644
index 0000000000000000000000000000000000000000..70413392f2cc8f2ee50a2c78d019607c3376feef
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <vector>
+#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
+
+namespace armarx::aron::similarity::NDArraySimilarity{
+
+ enum Type {
+ MSE,
+ MAE,
+ CHERNOFF,
+ COSINE,
+ NONE
+ };
+
+ std::string to_string(Type t);
+
+ double calculate_similarity(armarx::aron::data::NDArrayPtr p1, armarx::aron::data::NDArrayPtr p2, Type type);
+
+ /**
+ * @brief calculate_similarity_multi compares the image p with all images from the images vector, the dissimilarity values are simply summed up
+ * @param images vector of images that are compared to p
+ * @param p main image that you want to know the dissimilarity from
+ * @param type Type of dissimilarity measure used
+ * @return dissimilarity
+ */
+ double calculate_similarity_multi(std::vector<armarx::aron::data::NDArrayPtr> images, armarx::aron::data::NDArrayPtr p, Type type);
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b4d26bc8dd4adb15a612fcc627eed41ed392cc1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
@@ -0,0 +1,180 @@
+#include "chernoff.h"
+
+#include <cmath>
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+
+namespace armarx::aron::similarity::chernoff
+{
+
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2){
+ //TODO: there seems to be an error that leads to the mean vectors always having the same
+ //value, which leads to the diff being zero which leads to distance being 0/inf/nan
+ //aron::data::NDArray image1 = *p1;
+ //aron::data::NDArray image2 = *p2;
+
+ //then normalize them to have values between 0 and 1
+ //TODO: do not rturn new array but change existing one
+ auto f = normalize_ndarray(p1, 255);
+ auto s = normalize_ndarray(p2, 255);
+ //ARMARX_INFO << VAROUT(f);
+ //ARMARX_INFO << VAROUT(s);
+ //then calculate the mean vectors
+ std::vector<double> mean_one = calculate_mean_values(f);
+ std::vector<double> mean_two = calculate_mean_values(s);
+ //ARMARX_INFO << VAROUT(mean_one);
+ //then calculate the covariance matrices
+ std::vector<std::vector<double>> cov_one = calculate_covariance_matrix(f, mean_one);
+ std::vector<std::vector<double>> cov_two = calculate_covariance_matrix(s, mean_two);
+ //then calculate the Bhattacharyya distance and return
+ double distance = calculate_bhattacharyya_distance(mean_one, mean_two, cov_one, cov_two);
+ ARMARX_INFO << "Chernoff distance: " << std::to_string(distance);
+ return distance;
+ }
+
+ std::vector<double> calculate_mean_values(data::NDArray array)
+ {
+ std::vector<double> mean(3, 0.0);
+ int width = array.getShape().at(0);
+ int height = array.getShape().at(1);
+ int colors = array.getShape().at(2);
+ auto data = array.getDataAsVector();
+
+ for (int row = 0; row < height; row++){
+ for (int col = 0; col < width; col++){
+ double red = data.at(row * width * colors + col * colors);
+ double green = data.at(row * width * colors + col * colors + 1);
+ double blue = data.at(row * width * colors + col * colors + 2);
+ mean[0] += red;
+ mean[1] += green;
+ mean[2] += blue;
+ }
+ }
+ //ARMARX_INFO << VAROUT(mean);
+ int numPixels = width * height; //only per color
+ mean[0] /= numPixels;
+ mean[1] /= numPixels;
+ mean[2] /= numPixels;
+
+ return mean;
+ }
+
+ data::NDArray normalize_ndarray(data::NDArrayPtr array, int j)
+ {
+ std::vector<unsigned char> data = array->getDataAsVector();
+ std::vector<unsigned char> new_data(data.size());
+
+ for(int i = 0; i < data.size(); i++){
+ double a = data.at(i);
+ double n = a/j;
+ new_data.at(i) = n;
+ }
+
+ armarx::aron::data::NDArray n(array->getShape(), array->getType(),new_data, array->getPath());
+ return n;
+ }
+
+ std::vector<std::vector<double> > calculate_covariance_matrix(data::NDArray array, std::vector<double> mean = std::vector<double>())
+ {
+ int width = array.getShape().at(0);
+ int height = array.getShape().at(1);
+ int colors = array.getShape().at(2);
+ auto data = array.getDataAsVector();
+
+ std::vector<double> mean_vec(3, 0.0);
+ if(mean.empty()){
+ mean_vec = calculate_mean_values(array);
+ } else {
+ mean_vec = mean;
+ }
+
+ int numPixels = width * height; //only per color
+
+ std::vector<std::vector<double>> covariance(3, std::vector<double>(3, 0.0));
+
+ for(int row = 0; row < height; row++){
+ for(int col = 0; col < width; col++){
+ double red = data.at(row * width * colors + col * colors);
+ double green = data.at(row * width * colors + col * colors + 1);
+ double blue = data.at(row * width * colors + col * colors + 2);
+
+ double redDeviation = red - mean[0];
+ double greenDeviation = green - mean[1];
+ double blueDeviation = blue - mean[2];
+
+ covariance[0][0] += redDeviation * redDeviation;
+ covariance[0][1] += redDeviation * greenDeviation;
+ covariance[0][2] += redDeviation * blueDeviation;
+ covariance[1][1] += greenDeviation * greenDeviation;
+ covariance[1][2] += greenDeviation * blueDeviation;
+ covariance[2][2] += blueDeviation * blueDeviation;
+ }
+ }
+
+ for(int i = 0; i < 3; i++){
+ for (int j = 0; j < i; j++){
+ covariance[i][j] /= numPixels;
+ covariance[j][i] = covariance[i][j];
+ }
+ covariance[i][i] /= numPixels;
+ }
+
+ return covariance;
+ }
+
+ double calculate_bhattacharyya_distance(std::vector<double> mean_one, std::vector<double> mean_two, std::vector<std::vector<double> > covariance_one, std::vector<std::vector<double> > covariance_two)
+ {
+ //first make mean vectors and covariance matrices in Eigen:: objects
+ Eigen::VectorXd meanOne(3);
+ meanOne << mean_one[0], mean_one[1], mean_one[2];
+ Eigen::VectorXd meanTwo(3);
+ meanTwo << mean_two[0], mean_two[1], mean_two[2];
+
+ //ARMARX_INFO << VAROUT(meanOne);
+ //ARMARX_INFO << VAROUT(meanTwo);
+
+ //these are column-major instead or row-major, but we have a quadratic, symmetric matrix, so it does not matter
+ //this conversion does not work correctly!!
+ //Eigen::MatrixXd cov_one = Eigen::Map<Eigen::MatrixXd>(covariance_one[0].data(), covariance_one.size(), covariance_one[0].size());
+ //Eigen::MatrixXd cov_two = Eigen::Map<Eigen::MatrixXd>(covariance_two[0].data(), covariance_two.size(), covariance_two[0].size());
+
+ Eigen::Matrix3d cov_one;
+ cov_one << covariance_one[0][0], covariance_one[0][1], covariance_one[0][2],
+ covariance_one[1][0], covariance_one[1][1], covariance_one[1][2],
+ covariance_one[2][0], covariance_one[2][1], covariance_one[2][2];
+
+ Eigen::Matrix3d cov_two;
+ cov_two << covariance_two[0][0], covariance_two[0][1], covariance_two[0][2],
+ covariance_two[1][0], covariance_two[1][1], covariance_two[1][2],
+ covariance_two[2][0], covariance_two[2][1], covariance_two[2][2];
+
+
+ //ARMARX_INFO << VAROUT(cov_one);
+
+ Eigen::VectorXd meanDiff = meanOne - meanTwo;
+ //ARMARX_INFO << VAROUT(meanDiff);
+
+ Eigen::MatrixXd sigma = 0.5 * (cov_one + cov_two);
+ Eigen::MatrixXd sigma_inverse = sigma.inverse();
+
+ double det_cov_one = cov_one.determinant();
+ double det_cov_two = cov_two.determinant();
+ double det_sigma = sigma.determinant();
+
+ //formula from https://www.kaggle.com/code/debanga/statistical-distances
+ double bigger = meanDiff.transpose() * (sigma_inverse) * meanDiff;
+ //ARMARX_INFO << "Bigger: " << std::to_string(bigger);
+ double term_one = 0.125 * bigger;
+ double sqr = std::sqrt(det_cov_one * det_cov_two);
+ //ARMARX_INFO << "Sqr: " << std::to_string(sqr);
+ double before_log = det_sigma / (sqr);
+ //ARMARX_INFO << "Before log: " << std::to_string(before_log);
+ double term_two = 0.5 * std::log(before_log);
+
+ double distance = term_one + term_two;
+
+ return distance;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c8dc173af489379e00435d9f1c98dda5e2e5f69
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Joana Plewnia ( uhfpm at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::chernoff
+{
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
+ armarx::aron::data::NDArray normalize_ndarray(armarx::aron::data::NDArrayPtr array, int i);
+
+ std::vector<double> calculate_mean_values(armarx::aron::data::NDArray array);
+
+ std::vector<std::vector<double>> calculate_covariance_matrix(armarx::aron::data::NDArray array,
+ std::vector<double> mean);
+
+ double calculate_bhattacharyya_distance(std::vector<double> mean_one, std::vector<double> mean_two,
+ std::vector<std::vector<double>> covariance_one,
+ std::vector<std::vector<double>> covariance_two);
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3be0d5ef20e5a3325c90a69dbbca0083c7ebfb89
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
@@ -0,0 +1,31 @@
+#include "mae.h"
+
+namespace armarx::aron::similarity::mae
+{
+
+ double compute_similarity(const data::NDArrayPtr p1, const data::NDArrayPtr p2)
+ {
+ double sum = 0;
+ int width = p1->getShape().at(0);
+ int height = p1->getShape().at(1);
+ int colors = p1->getShape().at(2);
+
+ auto first_image = p1->getDataAsVector();
+ auto second_image = p2->getDataAsVector();
+
+ ARMARX_CHECK(first_image.size() == second_image.size());
+
+ for (int w = 0; w < width; w++){
+ for (int h = 0; h < height; h++){
+ for (int c = 0; c < colors; c++){
+ int k = h * width * colors + w * colors + c;
+ sum += std::abs(first_image.at(k) - second_image.at(k));
+ }
+ }
+ }
+ return sum / (width * height * colors);
+ }
+
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mae.h b/source/RobotAPI/libraries/aron/similarity/data/image/mae.h
new file mode 100644
index 0000000000000000000000000000000000000000..cb68569c32f4a1ceaef11727ea85f6e5ea12ae7e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mae.h
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Joana Plewnia ( uhfpm at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::mae
+{
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
index 0f26147604663e1e752fa8d7a8498faae480d9b3..cb9dcf9d3f7ef6ca4ee31e514e3aa7c122605dee 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
@@ -1,12 +1,63 @@
#include "mse.h"
#include <SimoxUtility/algorithm/string.h>
+#include <cmath>
namespace armarx::aron::similarity
{
double
- mse::compute_similarity(const aron::data::NDArrayPtr& p1, const aron::data::NDArrayPtr& p2)
+ mse::compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
{
- return 0;
+ //ARMARX_INFO << "Begin MSE";
+ //auto start = std::chrono::high_resolution_clock::now();
+ double sum = 0;
+
+ //TODO: check shapes have same size
+ //ARMARX_INFO << "One";
+ if(p1 != nullptr && p2 != nullptr){
+ p1->getShortName();
+ } else {
+ if(p1 == nullptr){
+ ARMARX_INFO << "P1 is nullptr";
+ }else{
+ ARMARX_INFO << "P2 is Nullpointer";
+ }
+ }
+ //ARMARX_INFO << "Two";
+
+ //return 0;
+
+ std::vector<unsigned char> first_image = p1->getDataAsVector();
+ std::vector<unsigned char> second_image = p2->getDataAsVector();
+ int size = first_image.size();
+
+ //ARMARX_INFO << "Image size: " << std::to_string(int(first_image.size()));
+ //ARMARX_INFO << "Image size: " << std::to_string(int(second_image.size()));
+ ARMARX_CHECK(first_image.size() == second_image.size());
+
+ //auto start = std::chrono::high_resolution_clock::now();
+ int rolling_number = 1; //TODO: make sure that no elements will be left over
+
+ for(int i = 0; i < int(first_image.size()); i+= rolling_number){
+ //loop unrolling to shorten the needed computation time:
+ sum += std::pow(first_image.at(i) - second_image.at(i), 2);
+ /*
+ sum += std::pow(first_image.at(i + 1) - second_image.at(i + 1), 2);
+ sum += std::pow(first_image.at(i + 2) - second_image.at(i + 2), 2);
+ sum += std::pow(first_image.at(i + 3) - second_image.at(i + 3), 2);
+ */
+ }
+
+
+ //auto end = std::chrono::high_resolution_clock::now();
+ //auto additional = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+ //ARMARX_INFO << "Time with unroll factor " << rolling_number << " MSE: " << std::to_string(additional.count());
+ first_image.clear();
+ second_image.clear();
+
+ //ARMARX_INFO << "MSE is: " << std::to_string(sum / size);
+
+ return sum / (size);
}
+
} // namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mse.h b/source/RobotAPI/libraries/aron/similarity/data/image/mse.h
index 461768d99d4624a5a5d92d82ed749845ceef94f1..892a12dd06969404eba03fe21540d53ee4d3df81 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mse.h
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mse.h
@@ -29,5 +29,6 @@
namespace armarx::aron::similarity::mse
{
- double compute_similarity(const aron::data::NDArrayPtr& p1, const aron::data::NDArrayPtr& p2);
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
}