From b6d7c54d85d9c817e72b0c7e156727bb0130e71c Mon Sep 17 00:00:00 2001
From: jean_patrick_mathes <uomnk@student.kit.edu>
Date: Wed, 28 Sep 2022 15:54:53 +0200
Subject: [PATCH] Add basic support for cloning and deleting objects
---
.../InteractiveMemoryEditor/Editor.cpp | 207 ++++++++++++++----
.../InteractiveMemoryEditor/Editor.h | 21 +-
2 files changed, 183 insertions(+), 45 deletions(-)
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
index b4a5195cd..882e575eb 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
@@ -28,7 +28,7 @@ namespace armarx
if (isPullRequired)
{
- requestedPoses = pull();
+ storedPoses = pull();
isPullRequired = false;
}
@@ -69,6 +69,7 @@ namespace armarx
objpose::ObjectPoseSeq newRequestedPoses = pullFromMemory();
changed.clear();
+ newPoses.clear();
isMemoryVizRequired = true;
@@ -77,28 +78,53 @@ namespace armarx
void Editor::push()
{
+ storedPoses.insert(storedPoses.begin(), newPoses.begin(), newPoses.end());
+ newPoses.clear();
+
objpose::ProvidedObjectPoseSeq providingPoses;
+ objpose::ObjectPoseSeq remainingPoses;
- for (const objpose::ObjectPose & requested : requestedPoses)
+ remainingPoses.reserve(storedPoses.size());
+ for (const objpose::ObjectPose & requested : storedPoses)
{
+ auto iterator = changed.find(requested.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ if (isChanged && iterator->second.kind == DELETE)
+ {
+ continue;
+ }
+
+ remainingPoses.push_back(requested);
+ objpose::ObjectPose& current = remainingPoses.back();
+
objpose::ProvidedObjectPose& providing = providingPoses.emplace_back();
- providing.providerName = requested.providerName;
+ providing.providerName = current.providerName;
providing.objectType = objpose::KnownObject;
- providing.objectID = requested.objectID;
+ providing.objectID = current.objectID;
- providing.objectPose = requested.objectPoseGlobal;
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ if (isChanged)
+ {
+ transform = iterator->second.transform;
+ }
+
+ current.objectPoseGlobal = transform * current.objectPoseGlobal;
+
+ providing.objectPose = current.objectPoseGlobal;
providing.objectPoseFrame = GlobalFrame;
- providing.objectPoseGaussian = requested.objectPoseGlobalGaussian;
+ providing.objectPoseGaussian = current.objectPoseGlobalGaussian;
- providing.confidence = requested.confidence;
+ providing.confidence = current.confidence;
providing.timestamp = DateTime::Now();
}
pushToMemory(providingPoses);
changed.clear();
+ storedPoses = remainingPoses;
isMemoryVizRequired = true;
}
@@ -107,48 +133,143 @@ namespace armarx
{
observer.clearObservedLayer(memoryLayer);
- for (objpose::ObjectPose & objectPose: requestedPoses)
+ for (objpose::ObjectPose & objectPose: storedPoses)
{
- bool isChanged = changed.find(objectPose.objectID.str()) != changed.end();
+ visualizeObject(objectPose);
+ }
- float alpha = isChanged ? 1.0 / 2 : 1.0;
+ for (objpose::ObjectPose & objectPose: newPoses)
+ {
+ visualizeObject(objectPose);
+ }
+ }
- viz::InteractionDescription interaction = viz::interaction().selection().transform().hideDuringTransform();
- if (isChanged)
+ void Editor::visualizeObject(objpose::ObjectPose &objectPose)
+ {
+ auto iterator = changed.find(objectPose.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ bool canMove = not isChanged or iterator->second.kind != DELETE;
+
+ size_t cloneIndex = std::numeric_limits<size_t>::max();
+ size_t deleteIndex = std::numeric_limits<size_t>::max();
+ size_t resetIndex = std::numeric_limits<size_t>::max();
+
+ size_t i = 0;
+
+ std::vector<std::string> options;
+ if (not isChanged or iterator->second.kind == MOVE)
+ {
+ options.emplace_back("Clone");
+ options.emplace_back("Delete");
+
+ cloneIndex = i++;
+ deleteIndex = i++;
+ }
+
+ if (isChanged)
+ {
+ options.emplace_back("Reset");
+
+ resetIndex = i++;
+ }
+
+ viz::InteractionDescription interaction = viz::interaction().selection();
+
+ if (canMove)
+ {
+ interaction.transform().hideDuringTransform();
+ }
+
+ if (not options.empty())
+ {
+ interaction.contextMenu(options);
+ }
+
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+
+ if (isChanged)
+ {
+ transform = changed[objectPose.objectID.str()].transform;
+ }
+
+ viz::Object object =
+ viz::Object(objectPose.objectID.str())
+ .pose(transform * objectPose.objectPoseGlobal)
+ .fileByObjectFinder(objectPose.objectID)
+ .enable(interaction);
+
+ if (isChanged)
+ {
+ const float alpha = 0.5;
+
+ switch (iterator->second.kind)
{
- interaction.contextMenu({"Reset"});
- }
+ case MOVE:
+ object.alpha(alpha);
+ break;
+
+ case CREATE:
+ object.overrideColor(simox::Color(0.0F, 1.0F, 0.0F, alpha));
+ break;
- observer.addObserved(
- memoryLayer,
- viz::Object(objectPose.objectID.str())
- .pose(objectPose.objectPoseGlobal)
- .fileByObjectFinder(objectPose.objectID)
- .alpha(alpha)
- .enable(interaction))
- .onContextMenu(0, [&]
- {
- auto iterator = changed.find(objectPose.objectID.str());
- if (iterator != changed.end())
- {
- Change change = iterator->second;
- changed.erase(iterator);
- objectPose.objectPoseGlobal = change.originalPose;
-
- isMemoryVizRequired = true;
- }
- })
- .onTransformEnd([&](const Eigen::Matrix4f& transform)
- {
- Change change = {.originalPose = objectPose.objectPoseGlobal};
- objectPose.objectPoseGlobal = transform * objectPose.objectPoseGlobal;
-
- // Emplace does not override when entry already exists in map.
- changed.emplace(objectPose.objectID.str(), change);
-
- isMemoryVizRequired = true;
- });
+ case DELETE:
+ object.overrideColor(simox::Color(1.0F, 0.0F, 0.0F, alpha));
+ }
}
+
+ observer.addObserved(memoryLayer, object)
+ .onContextMenu(cloneIndex, [&]
+ {
+ std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
+ std::chrono::system_clock::now().time_since_epoch()).count());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back(objectPose);
+ newPose.objectID = objectPose.objectID.withInstanceName(objectPose.objectID.instanceName() + suffix);
+
+ Change& change = changed[newPose.objectID.str()];
+ change.kind = CREATE;
+ change.transform = Eigen::Affine3f(Eigen::Translation3f(1000, 1000, 1000)).matrix();
+ change.iterator = std::prev(newPoses.end());
+
+ auto iterator = changed.find(objectPose.objectID.str());
+ if (iterator != changed.end())
+ {
+ change.transform *= iterator->second.transform;
+ }
+
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(deleteIndex, [&]
+ {
+ changed[objectPose.objectID.str()].kind = DELETE;
+
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(resetIndex, [&]
+ {
+ auto iterator = changed.find(objectPose.objectID.str());
+ if (iterator != changed.end())
+ {
+ Change& change = iterator->second;
+
+ if (change.kind == CREATE)
+ {
+ newPoses.erase(change.iterator);
+ }
+
+ changed.erase(iterator);
+
+ isMemoryVizRequired = true;
+ }
+ })
+ .onTransformEnd([&](const Eigen::Matrix4f& transform)
+ {
+ Change& change = changed[objectPose.objectID.str()];
+ change.transform = transform * change.transform;
+
+ isMemoryVizRequired = true;
+ });
}
void Editor::visualizeMeta()
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
index 7d5849b49..92ee3c6b3 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
@@ -1,5 +1,7 @@
#pragma once
+#include <list>
+
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
@@ -30,12 +32,25 @@ namespace armarx
InteractionObserver observer;
+ enum ChangeKind
+ {
+ MOVE,
+ CREATE,
+ DELETE
+ };
+
+ using ObjectPoseLst = std::list<objpose::ObjectPose>;
+
+ objpose::ObjectPoseSeq storedPoses;
+ ObjectPoseLst newPoses;
+
struct Change
{
- Eigen::Matrix4f originalPose;
+ ChangeKind kind = MOVE;
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ ObjectPoseLst::iterator iterator {};
};
- objpose::ObjectPoseSeq requestedPoses;
std::map<std::string, Change> changed;
bool isPushRequired;
@@ -50,5 +65,7 @@ namespace armarx
void push();
objpose::ObjectPoseSeq pull();
+
+ void visualizeObject(objpose::ObjectPose &objectPose);
};
}
--
GitLab