diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index 89510368848d3f29b49219ae62a5699428086627..8a7a329753ee84ac1cccf4f70e552f4a7f4f1b1b 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -269,15 +269,6 @@ 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())
{
throw error::InvalidArgument(
@@ -286,7 +277,6 @@ namespace armarx::armem::base
"Cannot add an EntityInstance because its index is too big.");
}
- */
int index = instance.index();
//int index = static_cast<int>(this->_container.size());
EntityInstanceT& added = this->_container.emplace_back(std::move(instance));
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 1dbb15e5cbcb3e1d5275fcfc9ea174a684c0f7fb..d9c6a40aa7aac9b325ad9f21672b8b1e5226d041 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -16,6 +16,8 @@
#include "query_proc/ltm/disk/ltm.h"
#include "query_proc/wm/wm.h"
+#include <thread>
+
namespace armarx::armem::server
{
@@ -31,6 +33,64 @@ namespace armarx::armem::server
this->memoryListenerTopic = memoryListener;
}
+ void MemoryToIceAdapter::addSecondLTM(ltm::Memory *longtermMemory)
+ {
+ std::string name = longtermMemory->getExportName();
+ if(name.find("MemoryExport2") != std::string::npos){
+ this->secondLTM = longtermMemory;
+ ARMARX_INFO << "added second longterm Memory";
+ } else if (name.find("MemoryExport3") != std::string::npos){
+ this->thirdLTM = longtermMemory;
+ ARMARX_INFO << "added third LTM";
+ } else if (name.find("MemoryExport4") != std::string::npos){
+ this->fourthLTM = longtermMemory;
+ ARMARX_INFO << "added fourth LTM";
+ } else if (name.find("MemoryExport5") != std::string::npos){
+ this->fiftLTM = longtermMemory;
+ ARMARX_INFO << "added fifth LTM";
+ } else if (name.find("MemoryExport6") != std::string::npos){
+ this->sixthLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 6";
+ } else if (name.find("MemoryExport7") != std::string::npos){
+ this->seventhLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 7";
+ } else if (name.find("MemoryExport8") != std::string::npos){
+ this->eightLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 8";
+ } else if (name.find("MemoryExport9") != std::string::npos){
+ this->ninthLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 9";
+ } else if (name.find("MemoryExport10") != std::string::npos){
+ this->tenthLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 10";
+ } else if (name.find("MemoryExport11") != std::string::npos){
+ this->eleventhLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 11";
+ } /*else if (name.find("MemoryExport12") != std::string::npos){
+ this->twelvethLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 12";
+ } else if (name.find("MemoryExport13") != std::string::npos){
+ this->thirteenthLTM= longtermMemory;
+ ARMARX_INFO << "added LTM 13";
+ } else if (name.find("MemoryExport14") != std::string::npos){
+ this->fourteenthLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 14";
+ } else if (name.find("MemoryExport15") != std::string::npos){
+ this->fifteenthLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 15";
+ } else if (name.find("MemoryExport16") != std::string::npos){
+ this->sixteenthLTM = longtermMemory;
+ ARMARX_INFO << "added LTM 16";
+ } */else {
+ ARMARX_WARNING << "trying to add a LTM to the MemoryToIceAdapter with an export name "
+ "that does not correspond to a predefined LTM. Currently supported "
+ "LTMs are MemoryExport2, MemoryExport3, MemoryExport4, MemoryExport5 to 16. "
+ "You tried to add " << name;
+ }
+
+
+ }
+
// WRITING
data::AddSegmentResult
MemoryToIceAdapter::addSegment(const data::AddSegmentInput& input, bool addCoreSegments)
@@ -166,7 +226,7 @@ namespace armarx::armem::server
armem::CommitResult
MemoryToIceAdapter::_commit(const armem::Commit& commit, bool locking)
- {
+ {
ARMARX_TRACE;
std::vector<data::MemoryID> updatedIDs;
const bool publishUpdates = bool(memoryListenerTopic);
@@ -189,12 +249,10 @@ 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);
@@ -203,6 +261,114 @@ namespace armarx::armem::server
longtermMemory->store(m);
}
+
+ if(secondLTM->isRecording()){
+ armem::wm::Memory mem(secondLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ secondLTM->store(mem);
+ }
+
+ if(thirdLTM->isRecording()){
+ armem::wm::Memory mem(thirdLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ thirdLTM->store(mem);
+ }
+
+ if(fourthLTM->isRecording()){
+ armem::wm::Memory mem(fourthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ fourthLTM->store(mem);
+ }
+
+ if(fiftLTM->isRecording()){
+ armem::wm::Memory mem(fiftLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ fiftLTM->store(mem);
+ }
+
+ if(sixthLTM->isRecording()){
+ armem::wm::Memory mem(sixthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ sixthLTM->store(mem);
+ }
+
+ if(seventhLTM->isRecording()){
+ armem::wm::Memory mem(seventhLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ seventhLTM->store(mem);
+ }
+
+ if(eightLTM->isRecording()){
+ armem::wm::Memory mem(eightLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ eightLTM->store(mem);
+ }
+
+ if(ninthLTM->isRecording()){
+ armem::wm::Memory mem(ninthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ ninthLTM->store(mem);
+ }
+
+ if(tenthLTM->isRecording()){
+ armem::wm::Memory mem(tenthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ tenthLTM->store(mem);
+ }
+
+ if(eleventhLTM->isRecording()){
+ armem::wm::Memory mem(eleventhLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ eleventhLTM->store(mem);
+ }
+ /*
+
+ if(twelvethLTM->isRecording()){
+ armem::wm::Memory mem(twelvethLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ twelvethLTM->store(mem);
+ }
+
+ if(thirteenthLTM->isRecording()){
+ armem::wm::Memory mem(thirteenthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ thirteenthLTM->store(mem);
+ }
+
+ if(fourteenthLTM->isRecording()){
+ armem::wm::Memory mem(fourteenthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ fourteenthLTM->store(mem);
+ }
+
+ if(fifteenthLTM->isRecording()){
+ armem::wm::Memory mem(fifteenthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ fifteenthLTM->store(mem);
+ }
+
+ if(sixteenthLTM->isRecording()){
+ armem::wm::Memory mem(sixteenthLTM->name());
+ armem::wm::toMemory(mem, updateResult.removedSnapshots);
+
+ sixteenthLTM->store(mem);
+ }
+ */
+
if (publishUpdates)
{
data::MemoryID& id = updatedIDs.emplace_back();
@@ -224,6 +390,9 @@ namespace armarx::armem::server
result.success = false;
result.errorMessage = e.what();
}
+ catch (...){
+ ARMARX_INFO << "Error during LTM consollidation";
+ }
}
if (publishUpdates)
@@ -355,7 +524,30 @@ 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!
+ std::vector<armarx::armem::server::ltm::Memory*> ltms;
+ ltms.push_back(longtermMemory);
+ ltms.push_back(secondLTM);
+ ltms.push_back(thirdLTM);
+ ltms.push_back(fourthLTM);
+ ltms.push_back(fiftLTM);
+ ltms.push_back(sixthLTM);
+ ltms.push_back(seventhLTM);
+ ltms.push_back(eightLTM);
+ ltms.push_back(ninthLTM);
+ ltms.push_back(tenthLTM);
+ ltms.push_back(eleventhLTM);
+ /*
+ ltms.push_back(twelvethLTM);
+ ltms.push_back(thirteenthLTM);
+ ltms.push_back(fourteenthLTM);
+ ltms.push_back(fifteenthLTM);
+ ltms.push_back(sixteenthLTM);
+ */
+ for(int i = 0; i < static_cast<int>(ltms.size()); i++){
+ ltms.at(i)->startRecording();
+ }
+ //longtermMemory->startRecording(); // TODO: config and execution time!
+
dto::StartRecordResult ret;
ret.success = true;
@@ -369,7 +561,39 @@ namespace armarx::armem::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(longtermMemory);
ARMARX_IMPORTANT << "Disabling the recording of memory " << longtermMemory->id().str();
- longtermMemory->stopRecording();
+ //all LTMs will be disabled:
+ std::vector<armarx::armem::server::ltm::Memory*> ltms;
+ ltms.push_back(longtermMemory);
+ ltms.push_back(secondLTM);
+ ltms.push_back(thirdLTM);
+ ltms.push_back(fourthLTM);
+ ltms.push_back(fiftLTM);
+ ltms.push_back(sixthLTM);
+ ltms.push_back(seventhLTM);
+ ltms.push_back(eightLTM);
+ ltms.push_back(ninthLTM);
+ ltms.push_back(tenthLTM);
+ ltms.push_back(eleventhLTM);
+ /*
+ ltms.push_back(twelvethLTM);
+ ltms.push_back(thirteenthLTM);
+ ltms.push_back(fourteenthLTM);
+ ltms.push_back(fifteenthLTM);
+ ltms.push_back(sixteenthLTM);
+ */
+ std::vector<std::thread> threads;
+ for(int i = 0; i < static_cast<int>(ltms.size()); ++i){
+ threads.push_back(std::thread([i, <ms]{
+ ltms.at(i)->stopRecording();
+ }));
+ }
+
+ for(auto& thread: threads){
+ thread.join();
+ }
+
+ ARMARX_INFO << "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 a451054a2fb667e99c9472d847c86e3e2cb48dea..8185bea42392bbf4bdbc7eeff7d3e5b9ac46f5d7 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -25,6 +25,8 @@ namespace armarx::armem::server
void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
+ void addSecondLTM(server::ltm::Memory* longtermMemory);
+
// WRITING
data::AddSegmentResult addSegment(const data::AddSegmentInput& input,
@@ -62,6 +64,23 @@ namespace armarx::armem::server
public:
server::wm::Memory* workingMemory;
server::ltm::Memory* longtermMemory;
+ server::ltm::Memory* secondLTM;
+ server::ltm::Memory* thirdLTM;
+ server::ltm::Memory* fourthLTM;
+ server::ltm::Memory* fiftLTM;
+ server::ltm::Memory* sixthLTM;
+ server::ltm::Memory* seventhLTM;
+ server::ltm::Memory* eightLTM;
+ server::ltm::Memory* ninthLTM;
+ server::ltm::Memory* tenthLTM;
+ server::ltm::Memory* eleventhLTM;
+ /*
+ server::ltm::Memory* twelvethLTM;
+ server::ltm::Memory* thirteenthLTM;
+ server::ltm::Memory* fourteenthLTM;
+ server::ltm::Memory* fifteenthLTM;
+ server::ltm::Memory* sixteenthLTM;
+ */
client::MemoryListenerInterfacePrx memoryListenerTopic;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
index 71cc8baa0b3ff6ac7d442c185d02cd95e717082f..c4e33dbd4dfe5c10fb7b8e08dcd748acf83d10c1 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -228,7 +228,7 @@ namespace armarx::armem::server::ltm
processors);
c.store(prov);
- statistics.recordedProviderSegments++;
+ //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 3336f93a346076dc53a397a0b4ed3755a7343014..ac9433dfb78be5a973925feb9c917a67699f1d7e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
@@ -39,28 +39,9 @@ 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 */
-
- //ARMARX_INFO << VAROUT(getFullPath());
-
if (fullPathExists())
{
- //ARMARX_INFO << VAROUT(getAllDirectories());
-
for (const auto& d : getAllDirectories()) // days
{
if (!util::fs::detail::isDateString(d.filename()))
@@ -80,8 +61,6 @@ namespace armarx::armem::server::ltm
// continue;
//}
- //ARMARX_INFO << VAROUT(util::fs::getAllDirectories(d));
-
for (const auto& s : util::fs::getAllDirectories(d)) // seconds
{
@@ -94,8 +73,6 @@ namespace armarx::armem::server::ltm
continue;
}
- //ARMARX_INFO << VAROUT(util::fs::getAllDirectories(s));
-
for (const auto& us : util::fs::getAllDirectories(s)) // microseconds
{
if (!util::fs::detail::isNumberString(us.filename()))
@@ -107,8 +84,6 @@ namespace armarx::armem::server::ltm
continue;
}
- //ARMARX_INFO << "id of EntitySnapshot: " << (id().withTimestamp(timeFromStringMicroSeconds(us.filename()))).timestampStr();
-
EntitySnapshot c(
getMemoryBasePath(),
@@ -212,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())
{
@@ -430,8 +396,6 @@ namespace armarx::armem::server::ltm
void
Entity::_store(const armem::wm::Entity& c)
{
-
- //ARMARX_INFO << deactivateSpam() << "_store entity " << c.id();
std::lock_guard l(ltm_mutex);
if (id().entityName.empty())
{
@@ -463,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;
@@ -472,16 +435,17 @@ namespace armarx::armem::server::ltm
for (auto& f : processors->snapFilters)
{
- //ARMARX_INFO << "checking filter";
+
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";
}
}
+
c.store(snap);
statistics.recordedSnapshots++;
});
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index 75241f49427b3aa24abab81bb8641b0e04e55e68..8a344c29aa0d88d87eb2be389d9fc0d73874bc16 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -193,18 +193,14 @@ namespace armarx::armem::server::ltm
auto metadataAron = std::make_shared<aron::data::Dict>();
to_aron(metadataAron, dataAron, e);
- //auto deep_copy_dataAron = dataAron->clone(); //this is only a fix for now, I need to find out what is actually happening
-
- //ARMARX_INFO << VAROUT(dataAron->getElements());
- //ARMARX_INFO << VAROUT(deep_copy_dataAron->getElements());
+ auto deep_copy_dataAron = dataAron->clone(); //this is only a fix for now, I need to find out what is actually happening
// 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(deep_copy_dataAron);
for (const auto& [memberName, var] : dataExt.extraction)
{
@@ -216,17 +212,14 @@ namespace armarx::armem::server::ltm
ensureFileExists(filename, true);
writeDataToFile(filename, memberDataVec);
- }
+ }
- dataAron = dataExt.dataWithoutExtraction;
- }
+ deep_copy_dataAron = dataExt.dataWithoutExtraction;
- //ARMARX_INFO << VAROUT(dataAron->getElements());
- //ARMARX_INFO << VAROUT(deep_copy_dataAron->getElements());
+ }
// convert dict and metadata
auto [dataVec, dataVecModeSuffix] = dictConverter.convert(dataAron);
- //auto [dataVec, dataVecModeSuffix] = dictConverter.convert(deep_copy_dataAron);
auto [metadataVec, metadataVecModeSuffix] = dictConverter.convert(metadataAron);
ARMARX_CHECK_EMPTY(dataVecModeSuffix);
ARMARX_CHECK_EMPTY(metadataVecModeSuffix);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
index 544765d0243df5c62fb7808fd92a9233a970872d..5f007c02889573f023e3ecbbcf861ca28da19218 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
@@ -41,6 +41,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
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 3bdeae55b35a02085f2aee55b72aa1c5abeb8282..377d94984ce4ae81ecb884162d8c01435eb47290 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
@@ -31,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:
@@ -73,8 +77,13 @@ 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();
enabled = true;
+ if(statistics.firstStart){
+ statistics.firstStarted = armarx::core::time::DateTime::Now();
+ statistics.firstStart = false;
+ }
this->_enable();
}
@@ -82,8 +91,13 @@ 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();
}
@@ -137,13 +151,13 @@ namespace armarx::armem::server::ltm::detail
void
store(const armem::wm::Memory& memory)
{
- ARMARX_INFO << deactivateSpam() << "Memory stored";
TIMING_START(LTM_Memory_Append);
+
for (auto& f : processors->memFilters)
{
if (!f->accept(memory))
{
- ARMARX_INFO
+ ARMARX_INFO << deactivateSpam()
<< "Ignoring to put a Memory into the LTM because it got filtered.";
return;
}
@@ -183,15 +197,15 @@ namespace armarx::armem::server::ltm::detail
startRecording()
{
statistics.lastEnabled = armarx::core::time::DateTime::Now();
+
enable();
}
void
stopRecording()
{
+
disable();
- //This can hold information abut the filter statistics, if the LTM recording is properly ended
- //getFilterStatistics();
}
bool
@@ -219,11 +233,10 @@ namespace armarx::armem::server::ltm::detail
getFilterStatistics(){
ARMARX_INFO << "Trying to save statistics";
auto stats = processors->getSnapshotFilterStatistics();
- test::save_statistics(stats, this->name());
return stats;
}
- /// get level name
+ /// get level name1
static std::string
getLevelName()
{
@@ -262,8 +275,7 @@ namespace armarx::armem::server::ltm::detail
{
bool enabled_on_startup = true;
std::string configuration_on_startup =
- "{"
- " \"PngConverter\": {}"
+ "{ \"SnapshotFrequencyFilter\": {\"WaitingTimeInMsForFilter\" : 3000}"
"}";
} p;
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..579e43019af6b96f525c9410b959498dba8e567d 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -6,6 +6,9 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <chrono>
+#include <thread>
+
namespace armarx::armem::server::ltm::detail::mixin
{
template <class _CoreSegmentT>
@@ -87,7 +90,14 @@ namespace armarx::armem::server::ltm::detail::mixin
return;
}
- this->directlyStore(*to_store);
+ while(storeFlag.test_and_set(std::memory_order_acquire)){
+ std::this_thread::yield();
+ }
+ std::thread storingThread([&]{
+ this->directlyStore(*to_store);
+ storeFlag.clear(std::memory_order_release);
+ });
+ storingThread.detach();
}
/// configuration
@@ -97,6 +107,7 @@ namespace armarx::armem::server::ltm::detail::mixin
if (json.find("BufferedMemory.storeFrequency") != json.end())
{
storeFrequency = json.at("BufferedMemory.storeFrequency");
+ ARMARX_INFO << VAROUT(storeFrequency);
}
}
@@ -116,6 +127,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/processors/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
index e4fe1786631f6679c822a0d76972dece4ac14e56..9f7b2c970191e0d9a2af3177e483e8b24de6f499 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
@@ -11,6 +11,7 @@ namespace armarx::armem::server::ltm::processor::filter
bool
SnapshotSimilarityFilter::accept(const armem::wm::EntitySnapshot& e)
{
+ ARMARX_INFO << "Checking snapshot similarity";
auto start = std::chrono::high_resolution_clock::now();
double similarity = -10;
@@ -22,7 +23,14 @@ namespace armarx::armem::server::ltm::processor::filter
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 = data->at(key)->getDescriptor();
+ 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);
@@ -43,29 +51,45 @@ namespace armarx::armem::server::ltm::processor::filter
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;
}
- std::vector<armarx::aron::data::NDArrayPtr> lastCommitImages = images.at(images.size() - 1);
- ARMARX_CHECK(lastCommitImages.size() == images_snapshot.size());
- for(int i= 0; i < images_snapshot.size(); i++){
- armarx::aron::data::NDArrayPtr p1 = lastCommitImages.at(i);
- armarx::aron::data::NDArrayPtr p2 = images_snapshot.at(i);
- float distance = aron::similarity::NDArraySimilarity::calculate_similarity(p1, p2, this->similarity_type);
- distances.insert(distances.end(), distance);
- }
- /*
- std::vector<armarx::aron::data::FloatPtr> lastCommitFloats = floats.at(floats.size()- 1);
- ARMARX_CHECK(lastCommitFloats.size() == floats_snapshot.size());
- for(int i= 0; i < floats_snapshot.size(); i++){
- armarx::aron::data::FloatPtr f1 = lastCommitFloats.at(i);
- armarx::aron::data::FloatPtr f2 = floats_snapshot.at(i);
- float distance = aron::similarity::FloatSimilarity::calculate_similarity(f1, f2, this->float_similarity_type);
- distances.insert(distances.end(), distance);
+ 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)
}
- */
+ 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;
+ ARMARX_INFO << VAROUT(index);
+ 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;
@@ -86,9 +110,7 @@ namespace armarx::armem::server::ltm::processor::filter
accept = (sum_distances > this->threshold);
}
-
if(accept){
- //ARMARX_INFO << "Accept because " << std::to_string(sum_distances) << " distance";
images.pop_front(); //delete first element
images.push_back(images_snapshot);
this->stats.accepted += 1;
@@ -97,14 +119,12 @@ namespace armarx::armem::server::ltm::processor::filter
stats.end_time = end;
return true;
} else {
- //ARMARX_INFO << "Not accepted because " << std::to_string(sum_distances) << " distance";
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)
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 a9a2dd8b9561fb08c4bcf0c2d7a0c985a8f647bd..19bfb3c3242386018c517d358c13fb8c4fe675ef 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,61 +1,46 @@
#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;
- bool
- SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot& e)
- {
- ARMARX_INFO << deactivateSpam() << "Checking Snapshot frequency filter";
- auto start = std::chrono::high_resolution_clock::now();
- auto entityID = e.id().getEntityID();
- auto genMs = e.time().toMilliSecondsSinceEpoch();
+ 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
+ }
- long lastMs = 0;
- if (timestampLastCommitInMs.count(entityID) > 0)
- {
- lastMs = timestampLastCommitInMs.at(entityID);
- }
+ 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();
+ }
+ });
+ 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;
- auto end = std::chrono::high_resolution_clock::now();
- stats.end_time = end;
- stats.additional_time += (end - start);
- stats.accepted += 1;
- return true;
- }
- auto end = std::chrono::high_resolution_clock::now();
- stats.end_time = end;
- stats.additional_time += (end - start);
- stats.rejected += 1;
- return false;
+ if(instances_accepted){
+ lastTime = current;
+ this->stats.accepted += 1;
+ } else {
+ this->stats.rejected += 1;
+ }
+ return instances_accepted;
}
void
@@ -63,17 +48,19 @@ namespace armarx::armem::server::ltm::processor::filter
{
if (json.find(PARAM_WAITING_TIME) != json.end())
{
- waitingTimeInMs = json.at(PARAM_WAITING_TIME);
- ARMARX_INFO << VAROUT(waitingTimeInMs);
+ 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;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //just for testing
}
SnapshotFilter::FilterStatistics SnapshotFrequencyFilter::getFilterStatistics()
{
- return stats;
+ stats.end_time = std::chrono::high_resolution_clock::now();
+ return this->stats;
}
std::string SnapshotFrequencyFilter::getName()
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 ef46182b4080d81af20ae2a80b2e8f762f72e4bf..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,51 +1,28 @@
#pragma once
-#include <map>
-
// Base Class
#include "../Filter.h"
-#include <chrono>
-
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;
FilterStatistics getFilterStatistics() override;
std::string getName() override;
- public:
- int waitingTimeInMs = -1;
-
private:
- std::map<MemoryID, long> timestampLastCommitInMs;
- FilterStatistics stats;
+ 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
index ca331dc177f131ebed33a98174afde7d719de375..f044009517deb10bf3a747c957e3590c70f70342 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
@@ -9,8 +9,10 @@ bool SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot &e)
auto start = std::chrono::high_resolution_clock::now();
bool instances_accepted = false;
double t = this->threshold;
- e.forEachInstance([this, &instances_accepted](armem::wm::EntityInstance& i){
- instances_accepted = important(i);
+ 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
+ }
});
auto end = std::chrono::high_resolution_clock::now();
stats.end_time = end;
@@ -66,7 +68,6 @@ bool SnapshotImportanceFilter::important(armem::wm::EntityInstance &i)
}
} else if (this->type == ImportanceType::ACCESSES){
auto a = i.metadata().numAccessed;
- ARMARX_INFO << "Number of accesses: " << a << " compared to threshold of: " << this->threshold;
if (a > this->threshold){
return true;
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 1390209ac53286177ff93f1ff81d6c3271646c69..bdb594c021543129047bfd3fc82769d6b76bcc6b 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,8 +43,45 @@ namespace armarx::armem::server::plugins
workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
}
- // stuff for ltm
+ // stuff for ltm
+ this->iceAdapter.addSecondLTM(&secondLTM);
+ this->iceAdapter.addSecondLTM(&thirdLTM);
+ this->iceAdapter.addSecondLTM(&fourthLTM);
+ this->iceAdapter.addSecondLTM(&fiftLTM);
+ this->iceAdapter.addSecondLTM(&sixthLTM);
+ this->iceAdapter.addSecondLTM(&seventhLTM);
+ this->iceAdapter.addSecondLTM(&eightLTM);
+ this->iceAdapter.addSecondLTM(&ninthLTM);
+ this->iceAdapter.addSecondLTM(&tenthLTM);
+ this->iceAdapter.addSecondLTM(&eleventhLTM);
+ /*
+ this->iceAdapter.addSecondLTM(&twelvethLTM);
+ this->iceAdapter.addSecondLTM(&thirteenthLTM);
+ this->iceAdapter.addSecondLTM(&fourteenthLTM);
+ this->iceAdapter.addSecondLTM(&fifteenthLTM);
+ this->iceAdapter.addSecondLTM(&sixteenthLTM);
+ */
+
+
longtermMemory.createPropertyDefinitions(properties, prefix + "ltm.");
+ secondLTM.createPropertyDefinitions(properties, prefix + "ltm2.");
+ thirdLTM.createPropertyDefinitions(properties, prefix + "ltm3.");
+ fourthLTM.createPropertyDefinitions(properties, prefix + "ltm4.");
+ fiftLTM.createPropertyDefinitions(properties, prefix + "ltm5.");
+ sixthLTM.createPropertyDefinitions(properties, prefix + "ltm6.");
+ seventhLTM.createPropertyDefinitions(properties, prefix + "ltm7.");
+ eightLTM.createPropertyDefinitions(properties, prefix + "ltm8.");
+ ninthLTM.createPropertyDefinitions(properties, prefix + "ltm9.");
+ tenthLTM.createPropertyDefinitions(properties, prefix + "ltm10.");
+ eleventhLTM.createPropertyDefinitions(properties, prefix + "ltm11.");
+ /*
+ twelvethLTM.createPropertyDefinitions(properties, prefix + "ltm12.");
+ thirteenthLTM.createPropertyDefinitions(properties, prefix + "ltm13.");
+ fourteenthLTM.createPropertyDefinitions(properties, prefix + "ltm14.");
+ fifteenthLTM.createPropertyDefinitions(properties, prefix + "ltm15.");
+ sixteenthLTM.createPropertyDefinitions(properties, prefix + "ltm16.");
+ */
+
}
void
@@ -57,6 +96,12 @@ namespace armarx::armem::server::plugins
void
Plugin::postOnInitComponent()
{
+ ARMARX_INFO << VAROUT(secondLTM.p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(longtermMemory.p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(thirdLTM.p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(fourthLTM.p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(fiftLTM.p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(tenthLTM.p.configuration_on_startup);
Component& parent = this->parent<Component>();
// activate LTM
@@ -64,12 +109,63 @@ namespace armarx::armem::server::plugins
if (not workingMemory.id().memoryName.empty())
{
longtermMemory.setMemoryID(workingMemory.id());
+ secondLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ thirdLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ fourthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ fiftLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ sixthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ seventhLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ eightLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ ninthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ tenthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ eleventhLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ /*
+ twelvethLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ thirteenthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ fourteenthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ fifteenthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ sixteenthLTM.setMemoryID(MemoryID(workingMemory.id().memoryName));
+ */
}
else
{
longtermMemory.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ secondLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ thirdLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ fourthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ fiftLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ sixthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ seventhLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ eightLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ ninthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ tenthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ eleventhLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ /*
+ twelvethLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ thirteenthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ fourteenthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ fifteenthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ sixteenthLTM.setMemoryID(MemoryID(parent.getDefaultName(), ""));
+ */
}
longtermMemory.configure();
+ secondLTM.configure();
+ thirdLTM.configure();
+ fourthLTM.configure();
+ fiftLTM.configure();
+ sixthLTM.configure();
+ seventhLTM.configure();
+ eightLTM.configure();
+ ninthLTM.configure();
+ tenthLTM.configure();
+ eleventhLTM.configure();
+ /*
+ twelvethLTM.configure();
+ thirteenthLTM.configure();
+ fourteenthLTM.configure();
+ fifteenthLTM.configure();
+ sixteenthLTM.configure();
+ */
initialized = true;
}
@@ -93,14 +189,150 @@ namespace armarx::armem::server::plugins
void
Plugin::preOnDisconnectComponent()
{
- longtermMemory.getFilterStatistics(); //does not work correctly at the moment
+ ARMARX_INFO << "Preparing to save statistics for " << this->workingMemory.name();
+ try{
+ auto first_stats = longtermMemory.getFilterStatistics();
+ auto stats = secondLTM.getFilterStatistics();
+ auto thirdStats = thirdLTM.getFilterStatistics();
+ auto fourthStats = fourthLTM.getFilterStatistics();
+ auto fiftStats = fiftLTM.getFilterStatistics();
+ auto stats6 = sixthLTM.getFilterStatistics();
+ auto stats7 = seventhLTM.getFilterStatistics();
+ auto stats8 = eightLTM.getFilterStatistics();
+ auto stats9 = ninthLTM.getFilterStatistics();
+ auto stats10 = tenthLTM.getFilterStatistics();
+ auto stats11 = eleventhLTM.getFilterStatistics();
+ /*
+ auto stats12 = twelvethLTM.getFilterStatistics();
+ auto stats13 = thirteenthLTM.getFilterStatistics();
+ auto stats14 = fourteenthLTM.getFilterStatistics();
+ auto stats15 = fifteenthLTM.getFilterStatistics();
+ auto stats16 = sixteenthLTM.getFilterStatistics();
+ */
+ try{
+ std::map<std::string, armarx::core::time::DateTime> times;
+ times["Started LTM1"] = longtermMemory.getStatistics().firstStarted;
+ times["Stopped LTM1"] = longtermMemory.getStatistics().firstStopped;
+ times["Started LTM2"] = secondLTM.getStatistics().firstStarted;
+ times["Stopped LTM2"] = secondLTM.getStatistics().firstStopped;
+ times["Started LTM3"] = thirdLTM.getStatistics().firstStarted;
+ times["Stopped LTM3"] = thirdLTM.getStatistics().firstStopped;
+ times["Started LTM4"] = fourthLTM.getStatistics().firstStarted;
+ times["Stopped LTM4"] = fourthLTM.getStatistics().firstStopped;
+ times["Started LTM5"] = fiftLTM.getStatistics().firstStarted;
+ times["Stopped LTM5"] = fiftLTM.getStatistics().firstStopped;
+ times["Started LTM6"] = sixthLTM.getStatistics().firstStarted;
+ times["Stopped LTM6"] = sixthLTM.getStatistics().firstStopped;
+ times["Started LTM7"] = seventhLTM.getStatistics().firstStarted;
+ times["Stopped LTM7"] = seventhLTM.getStatistics().firstStopped;
+ times["Started LTM8"] = eightLTM.getStatistics().firstStarted;
+ times["Stopped LTM8"] = eightLTM.getStatistics().firstStopped;
+ times["Started LTM9"] = ninthLTM.getStatistics().firstStarted;
+ times["Stopped LTM9"] = ninthLTM.getStatistics().firstStopped;
+ times["Started LTM10"] = tenthLTM.getStatistics().firstStarted;
+ times["Stopped LTM10"] = tenthLTM.getStatistics().firstStopped;
+ times["Started LTM11"] = eleventhLTM.getStatistics().firstStarted;
+ times["Stopped LTM11"] = eleventhLTM.getStatistics().firstStopped;
+ /*
+ times["Started LTM12"] = twelvethLTM.getStatistics().firstStarted;
+ times["Stopped LTM12"] = twelvethLTM.getStatistics().firstStopped;
+ times["Started LTM13"] = thirteenthLTM.getStatistics().firstStarted;
+ times["Stopped LTM13"] = thirteenthLTM.getStatistics().firstStopped;
+ times["Started LTM14"] = fourteenthLTM.getStatistics().firstStarted;
+ times["Stopped LTM14"] = fourteenthLTM.getStatistics().firstStopped;
+ times["Started LTM15"] = fifteenthLTM.getStatistics().firstStarted;
+ times["Stopped LTM15"] = fifteenthLTM.getStatistics().firstStopped;
+ times["Started LTM16"] = sixteenthLTM.getStatistics().firstStarted;
+ times["Stopped LTM16"] = sixteenthLTM.getStatistics().firstStopped;
+ */
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> information;
+ information["LTM1"] = first_stats;
+ if(stats.size() > 0){
+ information["LTM2"] = stats;
+ }
+ if(thirdStats.size() > 0){
+ information["LTM3"] = thirdStats;
+ }
+ if(fourthStats.size() > 0){
+ information["LTM4"] = fourthStats;
+ }
+ if(fiftStats.size() > 0){
+ information["LTM5"] = fiftStats;
+ }
+ if(stats6.size() > 0){
+ information["LTM6"] = stats6;
+ }
+ if(stats7.size() > 0){
+ information["LTM7"] = stats7;
+ }
+ if(stats8.size() > 0){
+ information["LTM8"] = stats8;
+ }
+ if(stats9.size() > 0){
+ information["LTM9"] = stats9;
+ }
+ if(stats10.size() > 0){
+ information["LTM10"] = stats10;
+ }
+ if(stats11.size() > 0){
+ information["LTM11"] = stats11;
+ }
+ /*
+ if(stats12.size() > 0){
+ information["LTM12"] = stats12;
+ }
+ if(stats13.size() > 0){
+ information["LTM13"] = stats13;
+ }
+ if(stats14.size() > 0){
+ information["LTM14"] = stats14;
+ }
+ if(stats15.size() > 0){
+ information["LTM15"] = stats15;
+ }
+ if(stats16.size() > 0){
+ information["LTM16"] = stats16;
+ }
+ */
+ test::save_statistics(information, times, secondLTM.name());
+ } catch(...){
+ ARMARX_INFO << "Something went wrong after getting the statistics";
+ }
+ } catch (...){
+ ARMARX_INFO << "Something went wrong with the statistics saving process";
+ }
+
+ statistics_saved = true;
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
removeServer();
}
+
+ }
+
+ void Plugin::preOnExitComponent()
+ {
+ //leads to issues right now, so deactrivated until I can find the issue
+ /**
+ if(!statistics_saved){ //sometimes the disconnection of the component does not happen correctly, but it will be exited later
+ auto first_stats = longtermMemory.getFilterStatistics();
+ auto stats = secondLTM.getFilterStatistics();
+ ARMARX_INFO << VAROUT(stats.at("SnapshotSimilarityFilter").rejected);
+ std::map<std::string, armarx::core::time::DateTime> times;
+ times["Started LTM1"] = longtermMemory.getStatistics().firstStarted;
+ times["Stopped LTM1"] = longtermMemory.getStatistics().firstStopped;
+ times["Started LTM2"] = secondLTM.getStatistics().firstStarted;
+ times["Stopped LTM2"] = secondLTM.getStatistics().firstStopped;
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> information;
+ information["LTM1"] = first_stats;
+ information["LTM2"] = stats;
+ test::save_statistics(information,times, secondLTM.name());
+ ARMARX_INFO << "Statistics saved";
+ }
+ */
}
void
@@ -118,6 +350,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());
@@ -141,6 +374,8 @@ namespace armarx::armem::server::plugins
result.errorMessage = e.what();
ARMARX_WARNING << e.what();
}
+
+
return result;
}
@@ -156,6 +391,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..10c3a73ee4a7ceb24fedb7a420388ce80840b46d 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -33,6 +33,7 @@ namespace armarx::armem::server::plugins
virtual void postOnInitComponent() override;
virtual void postOnConnectComponent() override;
virtual void preOnDisconnectComponent() override;
+ virtual void preOnExitComponent() override;
public:
@@ -78,6 +79,24 @@ namespace armarx::armem::server::plugins
/// A manager class for the ltm. It internally holds a normal wm instance as a cache.
server::ltm::Memory longtermMemory;
+ server::ltm::Memory secondLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport2", "LTM2");
+ server::ltm::Memory thirdLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport3", "LTM3");
+ server::ltm::Memory fourthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport4", "LTM4");
+ server::ltm::Memory fiftLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport5", "LTM5");
+ server::ltm::Memory sixthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport6", "LTM6");
+ server::ltm::Memory seventhLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport7", "LTM7");
+ server::ltm::Memory eightLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport8", "LTM8");
+ server::ltm::Memory ninthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport9", "LTM9");
+ server::ltm::Memory tenthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport10", "LTM10");
+ server::ltm::Memory eleventhLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport11", "LTM11");
+ /*
+ server::ltm::Memory twelvethLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport12", "LTM12");
+ server::ltm::Memory thirteenthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport13", "LTM13");
+ server::ltm::Memory fourteenthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport14", "LTM14");
+ server::ltm::Memory fifteenthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport15", "LTM15");
+ server::ltm::Memory sixteenthLTM = server::ltm::Memory(std::filesystem::path("/tmp"), {}, "MemoryExport16", "LTM16");
+
+ */
private:
@@ -85,6 +104,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
index eef5de1817a028c69518483286ab172b00135060..9fafdf7c5b66b21da9c5c7cbc21fb5373d2da30c 100644
--- a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
@@ -8,9 +8,11 @@
namespace armarx::armem::server::test
{
-void save_statistics(std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> stats, std::string memoryName, std::string sim_json_data, std::string object_memory_json_data)
+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, std::string memoryName, std::string sim_json_data, std::string object_memory_json_data)
{
- //ARMARX_INFO << "Saving statistics";
+ ARMARX_INFO << "Saving statistics";
+
+
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);
@@ -18,56 +20,67 @@ void save_statistics(std::map<std::string, ltm::processor::SnapshotFilter::Filte
auto month = time->tm_mon;
auto day = time->tm_mday;
- std::filesystem::path d_p = "Experiments";
- d_p /= memoryName;
- if(!std::filesystem::is_directory(d_p)){
- std::filesystem::create_directory(d_p);
- }
- d_p /= std::to_string(month);
- if(!std::filesystem::is_directory(d_p)){
- std::filesystem::create_directory(d_p);
- }
- d_p /= std::to_string(day);
- if(!std::filesystem::is_directory(d_p)){
- std::filesystem::create_directory(d_p);
- }
+ std::filesystem::path homePath = std::getenv("HOME");
+
+ std::filesystem::path d_p = homePath / "code" / "Experiments";
+
+ try{
+ d_p = d_p / memoryName / std::to_string(month) / std::to_string(day);
- ARMARX_INFO << "Experiments will be saved at: " << d_p.string();
+ std::filesystem::create_directories(d_p);
- for(auto f : stats){
- ARMARX_INFO << f.first << ": " << f.second.additional_time.count() << " sec; " << f.second.accepted << " accepted";
+ ARMARX_INFO << "Experiments will be saved at: " << d_p.string();
+ } catch(...){
+ ARMARX_WARNING << "Creating needed directories for saving statistics did not work";
}
+
+
nlohmann::json jsonData;
bool ended_without_recording = false;
-
- for(const auto& pair: stats){
- if(pair.second.accepted + pair.second.rejected < 1){
- ended_without_recording = true;
- ARMARX_INFO << "Trying to not add JSON file because accepted + rejected < 1";
+ for(const auto& filter: stats){
+ nlohmann::json info;
+ for(const auto& pair: filter.second){
+ if(pair.second.accepted + pair.second.rejected < 1){
+ //ARMARX_INFO << VAROUT(pair.second.accepted);
+ 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;
}
- 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;
- jsonData[pair.first] = information;
+ jsonData[filter.first] = info;
+ info.clear();
}
if(ended_without_recording){
@@ -112,6 +125,8 @@ void save_statistics(std::map<std::string, ltm::processor::SnapshotFilter::Filte
} catch(...){
ARMARX_WARNING << "std::ofstream failed";
}
+
+ ARMARX_INFO << "Saving statistics completed";
}
nlohmann::json load_scene_information(std::string om_or_sim)
diff --git a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
index 0cc2a8de0b57e09b5c537dc4c428ec3d5e100c60..384d898de223a7fb60c37bd22420d4985e2959aa 100644
--- a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
@@ -30,7 +30,7 @@
namespace armarx::armem::server::test
{
- void save_statistics(std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> stats, std::string memoryName = "", std::string sim_json_data = "", std::string object_memory_json_data = "");
+ 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, std::string memoryName = "", std::string sim_json_data = "", std::string object_memory_json_data = "");
nlohmann::json load_scene_information(std::string om_or_sim);
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
index bc7a38d7314b4cf135c18336289eb17474f3e6ed..5fb38a7f8859e804e4ea8a71642402a08ce01e2b 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
@@ -43,15 +43,15 @@ namespace armarx::aron::similarity{
}
}
- double NDArraySimilarity::calculate_similarity_multi(std::vector<data::NDArrayPtr>& images, armarx::aron::data::NDArrayPtr& p, Type type)
+ 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";
+ //ARMARX_INFO << "Before calculation";
sim += calculate_similarity(image, p, type);
- ARMARX_INFO << "Sim is currently: " << sim;
+ //ARMARX_INFO << "Sim is currently: " << sim;
}
- return 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
index 11236a40353a0a37dcbecd44e9f7eb491df6fd81..70413392f2cc8f2ee50a2c78d019607c3376feef 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
@@ -24,6 +24,6 @@ namespace armarx::aron::similarity::NDArraySimilarity{
* @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);
+ 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
index 2a71dd0a9f1ec4fbeb8bc75aa6c8b821e70d147f..0b4d26bc8dd4adb15a612fcc627eed41ed392cc1 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
@@ -138,17 +138,15 @@ namespace armarx::aron::similarity::chernoff
//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::Matrix<double, 3, 3> 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::Matrix<double, 3, 3> 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]}
- };
+ 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);