From e1fbef6b293ea8e14f070b1a6435372c4af78140 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 6 Oct 2022 14:57:55 +0200
Subject: [PATCH] Refactoring, make placeholders yellow and not shifted in Z
---
.../InteractiveMemoryEditor/Editor.cpp | 64 +++---
.../InteractiveMemoryEditor/Editor.h | 217 ++++++++++--------
.../InteractiveMemoryEditor.cpp | 1 -
3 files changed, 152 insertions(+), 130 deletions(-)
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
index 42924eedd..8b085c75d 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
@@ -238,7 +238,7 @@ namespace armarx
viz::Box box = viz::Box("placeholder_" + std::to_string(id))
.set(placeholder.box.transformed(placeholder.transform))
- .color(simox::Color::blue())
+ .color(simox::Color::yellow(255, 128))
.enable(interaction);
auto& observation = observer.addObserved(metaLayer, box)
@@ -267,19 +267,19 @@ namespace armarx
}
}
- void Editor::ChangeState::clear()
+ void ChangeState::clear()
{
changed.clear();
newPoses.clear();
}
- void Editor::ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
+ void ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
{
seq.insert(seq.begin(), newPoses.begin(), newPoses.end());
newPoses.clear();
}
- bool Editor::ChangeState::applyTo(objpose::ObjectPose& pose)
+ bool ChangeState::applyTo(objpose::ObjectPose& pose)
{
auto iterator = changed.find(pose.objectID.str());
bool isChanged = iterator != changed.end();
@@ -299,7 +299,7 @@ namespace armarx
return isChanged;
}
- void Editor::ChangeState::visualizeNewObjects()
+ void ChangeState::visualizeNewObjects()
{
for (objpose::ObjectPose & objectPose: newPoses)
{
@@ -307,7 +307,7 @@ namespace armarx
}
}
- Editor::VisualizationDescription Editor::ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
+ VisualizationDescription ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
{
VisualizationDescription description;
@@ -364,7 +364,7 @@ namespace armarx
description.options.emplace_back("Create Placeholder");
description.prototypeIndex = currentIndex++;
- description.options.emplace_back(lineString);
+ description.options.emplace_back(Editor::lineString);
currentIndex++;
description.options.emplace_back("Commit All Changes");
@@ -379,7 +379,7 @@ namespace armarx
return description;
}
- void Editor::ChangeState::cloneObject(objpose::ObjectPose const& object)
+ void ChangeState::cloneObject(objpose::ObjectPose const& object)
{
std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
std::chrono::system_clock::now().time_since_epoch()).count());
@@ -399,7 +399,8 @@ namespace armarx
Change& clonedChange = changed[newPose.objectID.str()];
clonedChange.kind = CREATE;
- clonedChange.transform = Eigen::Affine3f(Eigen::Translation3f(offset, offset, offset)).matrix();
+ // Heuristic: Don't shift in Z direction to ease creation in a horizontal plane.
+ clonedChange.transform = Eigen::Affine3f(Eigen::Translation3f(offset, offset, 0)).matrix();
clonedChange.iterator = std::prev(newPoses.end());
auto iterator = changed.find(object.objectID.str());
@@ -410,7 +411,7 @@ namespace armarx
}
}
- void Editor::ChangeState::createObject(const std::string &objectID, const Eigen::Matrix4f &pose)
+ void ChangeState::createObject(const std::string &objectID, const Eigen::Matrix4f &pose)
{
std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
std::chrono::system_clock::now().time_since_epoch()).count());
@@ -428,7 +429,7 @@ namespace armarx
newPose.confidence = 1;
newPose.timestamp = DateTime::Now();
- std::optional<armarx::ObjectInfo> info = editor->properties.getObjectInfo(id);
+ std::optional<armarx::ObjectInfo> info = editor->objectFinder.findObject(id);
if (info.has_value())
{
newPose.localOOBB = info->loadOOBB();
@@ -445,12 +446,12 @@ namespace armarx
createdChange.iterator = std::prev(newPoses.end());
}
- void Editor::ChangeState::deleteObject(const objpose::ObjectPose& object)
+ void ChangeState::deleteObject(const objpose::ObjectPose& object)
{
changed[object.objectID.str()].kind = DELETE;
}
- void Editor::ChangeState::resetObject(const objpose::ObjectPose& object)
+ void ChangeState::resetObject(const objpose::ObjectPose& object)
{
auto iterator = changed.find(object.objectID.str());
if (iterator != changed.end())
@@ -466,13 +467,13 @@ namespace armarx
}
}
- void Editor::ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
+ void ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
{
Change& change = changed[object.objectID.str()];
change.transform = transform * change.transform;
}
- Eigen::Matrix4f Editor::ChangeState::getTransform(const objpose::ObjectPose &object)
+ Eigen::Matrix4f ChangeState::getTransform(const objpose::ObjectPose &object)
{
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
@@ -486,48 +487,47 @@ namespace armarx
return transform;
}
- void Editor::PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
+ void PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
{
size_t id = getID();
- auto& [placeholder, isActive] = placeholders[id];
- placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
- isActive = true;
+ auto& entry = placeholders[id];
+ entry.placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
+ entry.isActive = true;
}
- void Editor::PlaceholderState::visualizePlaceholders()
+ void PlaceholderState::visualizePlaceholders()
{
for (size_t id = 0; id < placeholders.size(); id++)
{
- auto& [placeholder, isActive] = placeholders[id];
+ auto& entry = placeholders[id];
- if (isActive)
+ if (entry.isActive)
{
- editor->visualizePlaceholder(placeholder, id);
+ editor->visualizePlaceholder(entry.placeholder, id);
}
}
}
- void Editor::PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
+ void PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
{
- auto& [placeholder, isActive] = placeholders[id];
+ auto& placeholder = placeholders[id].placeholder;
placeholder.transform = transform * placeholder.transform;
}
- void Editor::PlaceholderState::removePlaceholder(size_t id)
+ void PlaceholderState::removePlaceholder(size_t id)
{
- auto& [placeholder, isActive] = placeholders[id];
- isActive = false;
+ placeholders[id].isActive = false;
unusedIDs.push(id);
}
- size_t Editor::PlaceholderState::getID()
+ size_t PlaceholderState::getID()
{
if (unusedIDs.empty())
{
size_t id = placeholders.size();
- placeholders.emplace_back(std::make_pair(Placeholder(), false));
+ placeholders.push_back({Placeholder(), false});
return id;
}
@@ -536,9 +536,9 @@ namespace armarx
return id;
}
- void Editor::PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
+ void PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
{
- auto& [placeholder, isActive] = placeholders[id];
+ auto& placeholder = placeholders[id].placeholder;
Eigen::Matrix4f pose = placeholder.box.transformed(placeholder.transform).transformation_centered();
changeState.createObject(objectID, pose);
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
index 6ec576d19..4f5a1f1b9 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
@@ -11,6 +11,107 @@
namespace armarx
{
+
+ class Editor;
+
+
+ struct VisualizationDescription
+ {
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ bool allowTransforming = true;
+
+ std::vector<std::string> options {};
+ std::optional<float> alpha {};
+ std::optional<simox::Color> color {};
+
+ // Context menu indices.
+ 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 prototypeIndex = std::numeric_limits<size_t>::max();
+ size_t commitIndex = std::numeric_limits<size_t>::max();
+ size_t updateIndex = std::numeric_limits<size_t>::max();
+ size_t resetAllIndex = std::numeric_limits<size_t>::max();
+ };
+
+
+
+ class ChangeState
+ {
+ public:
+ explicit ChangeState(Editor* editor) : editor(editor) {} ;
+
+ void clear();
+ void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
+ bool applyTo(objpose::ObjectPose& pose);
+ void visualizeNewObjects();
+ VisualizationDescription buildVisualizationDescription(objpose::ObjectPose& object);
+ Eigen::Matrix4f getTransform(objpose::ObjectPose const& object);
+
+ void cloneObject(objpose::ObjectPose const& object);
+ void createObject(std::string const& objectID, Eigen::Matrix4f const& pose);
+ void deleteObject(objpose::ObjectPose const& object);
+ void resetObject(objpose::ObjectPose const& object);
+ void moveObject(objpose::ObjectPose const& object, Eigen::Matrix4f const& transform);
+
+ private:
+ enum ChangeKind
+ {
+ MOVE,
+ CREATE,
+ DELETE
+ };
+
+ using ObjectPoseList = std::list<objpose::ObjectPose>;
+
+ struct Change
+ {
+ ChangeKind kind = MOVE;
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ ObjectPoseList::iterator iterator {};
+ };
+
+ Editor* editor;
+
+ ObjectPoseList newPoses;
+ std::map<std::string, Change> changed;
+ };
+
+
+
+ class PlaceholderState
+ {
+ public:
+ explicit PlaceholderState(Editor* editor) : editor(editor) {} ;
+
+ struct Placeholder
+ {
+ simox::OrientedBoxf box;
+ Eigen::Matrix4f transform;
+ };
+
+ void addPlaceholder(simox::OrientedBoxf box);
+ void visualizePlaceholders();
+ void movePlaceholder(size_t id, Eigen::Matrix4f const& transform);
+ void removePlaceholder(size_t id);
+ void specifyObject(size_t id, std::string const& objectID, ChangeState& changeState);
+
+ private:
+ size_t getID();
+
+ Editor* editor;
+
+ std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIDs;
+ struct PlaceholderEntry
+ {
+ Placeholder placeholder;
+ bool isActive;
+ };
+ std::vector<PlaceholderEntry> placeholders;
+ };
+
+
+
class Editor
{
public:
@@ -22,15 +123,29 @@ namespace armarx
float const& confidenceThreshold;
std::vector<data::ObjectID> const& availableObjects;
- std::function<std::optional<armarx::ObjectInfo>(armarx::ObjectID const&)> getObjectInfo;
};
+
+ public:
+
explicit Editor(viz::Client& client,
Properties properties,
std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory,
std::function<objpose::ObjectPoseSeq(void)> pullFromMemory);
void step();
+ private:
+
+ void visualizeMemory();
+ void visualizeMeta();
+
+ void commit();
+ objpose::ObjectPoseSeq update();
+ void reset();
+
+ void visualizeObject(objpose::ObjectPose &objectPose);
+ void visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id);
+
private:
static constexpr const char* memoryLayerName = "Memory";
static constexpr const char* metaLayerName = "Meta";
@@ -39,6 +154,7 @@ namespace armarx
std::vector<std::string> placeholderOptions;
Properties properties;
+ ObjectFinder objectFinder;
viz::Client& client;
const std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory;
@@ -51,93 +167,7 @@ namespace armarx
objpose::ObjectPoseSeq storedPoses;
- struct VisualizationDescription
- {
- Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
- bool allowTransforming = true;
-
- std::vector<std::string> options {};
- std::optional<float> alpha {};
- std::optional<simox::Color> color {};
-
- 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 prototypeIndex = std::numeric_limits<size_t>::max();
- size_t commitIndex = std::numeric_limits<size_t>::max();
- size_t updateIndex = std::numeric_limits<size_t>::max();
- size_t resetAllIndex = std::numeric_limits<size_t>::max();
- };
-
- class ChangeState
- {
- public:
- explicit ChangeState(Editor* editor) : editor(editor) {} ;
-
- void clear();
- void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
- bool applyTo(objpose::ObjectPose& pose);
- void visualizeNewObjects();
- VisualizationDescription buildVisualizationDescription(objpose::ObjectPose& object);
- Eigen::Matrix4f getTransform(objpose::ObjectPose const& object);
-
- void cloneObject(objpose::ObjectPose const& object);
- void createObject(std::string const& objectID, Eigen::Matrix4f const& pose);
- void deleteObject(objpose::ObjectPose const& object);
- void resetObject(objpose::ObjectPose const& object);
- void moveObject(objpose::ObjectPose const& object, Eigen::Matrix4f const& transform);
-
- private:
- enum ChangeKind
- {
- MOVE,
- CREATE,
- DELETE
- };
-
- using ObjectPoseLst = std::list<objpose::ObjectPose>;
-
- struct Change
- {
- ChangeKind kind = MOVE;
- Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
- ObjectPoseLst::iterator iterator {};
- };
-
- Editor* editor;
-
- ObjectPoseLst newPoses;
- std::map<std::string, Change> changed;
- };
-
- ChangeState changes{this};
-
- class PlaceholderState
- {
- public:
- explicit PlaceholderState(Editor* editor) : editor(editor) {} ;
-
- struct Placeholder
- {
- simox::OrientedBoxf box;
- Eigen::Matrix4f transform;
- };
-
- void addPlaceholder(simox::OrientedBoxf box);
- void visualizePlaceholders();
- void movePlaceholder(size_t id, Eigen::Matrix4f const& transform);
- void removePlaceholder(size_t id);
- void specifyObject(size_t id, std::string const& objectID, ChangeState& changeState);
-
- private:
- size_t getID();
-
- Editor* editor;
-
- std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIDs;
- std::vector<std::pair<Placeholder, bool>> placeholders;
- };
-
+ ChangeState changes{this};
PlaceholderState placeholders{this};
bool isCommitRequired;
@@ -146,14 +176,7 @@ namespace armarx
bool isMemoryVizRequired;
bool isMetaVizRequired;
- void visualizeMemory();
- void visualizeMeta();
-
- void commit();
- objpose::ObjectPoseSeq update();
- void reset();
-
- void visualizeObject(objpose::ObjectPose &objectPose);
- void visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id);
+ friend ChangeState;
+ friend PlaceholderState;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp
index 554d552d1..c6b602aec 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp
@@ -74,7 +74,6 @@ namespace armarx
.objectScaling = objectScaling,
.confidenceThreshold = confidenceThreshold,
.availableObjects = providerInfo.supportedObjects,
- .getObjectInfo = [this](armarx::ObjectID const& id) { return objectFinder.findObject(id); }
};
Editor editor(arviz, properties,
--
GitLab