From 4df60c401541726bfb8557b99b2cd4398b420fff Mon Sep 17 00:00:00 2001
From: jean_patrick_mathes <uomnk@student.kit.edu>
Date: Wed, 28 Sep 2022 17:01:06 +0200
Subject: [PATCH] Refactor change state management
---
.../InteractiveMemoryEditor/Editor.cpp | 293 +++++++++++-------
.../InteractiveMemoryEditor/Editor.h | 58 +++-
2 files changed, 220 insertions(+), 131 deletions(-)
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
index 882e575eb..3528d1455 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
@@ -68,8 +68,7 @@ namespace armarx
{
objpose::ObjectPoseSeq newRequestedPoses = pullFromMemory();
- changed.clear();
- newPoses.clear();
+ changes.clear();
isMemoryVizRequired = true;
@@ -78,40 +77,26 @@ namespace armarx
void Editor::push()
{
- storedPoses.insert(storedPoses.begin(), newPoses.begin(), newPoses.end());
- newPoses.clear();
+ changes.moveNewObjectsTo(storedPoses);
objpose::ProvidedObjectPoseSeq providingPoses;
objpose::ObjectPoseSeq remainingPoses;
remainingPoses.reserve(storedPoses.size());
- for (const objpose::ObjectPose & requested : storedPoses)
+ for (objpose::ObjectPose & current : storedPoses)
{
- auto iterator = changed.find(requested.objectID.str());
- bool isChanged = iterator != changed.end();
-
- if (isChanged && iterator->second.kind == DELETE)
+ bool keep = changes.applyTo(current);
+ if (not keep)
{
continue;
}
- remainingPoses.push_back(requested);
- objpose::ObjectPose& current = remainingPoses.back();
-
objpose::ProvidedObjectPose& providing = providingPoses.emplace_back();
providing.providerName = current.providerName;
providing.objectType = objpose::KnownObject;
providing.objectID = current.objectID;
- Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
- if (isChanged)
- {
- transform = iterator->second.transform;
- }
-
- current.objectPoseGlobal = transform * current.objectPoseGlobal;
-
providing.objectPose = current.objectPoseGlobal;
providing.objectPoseFrame = GlobalFrame;
@@ -119,11 +104,13 @@ namespace armarx
providing.confidence = current.confidence;
providing.timestamp = DateTime::Now();
+
+ remainingPoses.push_back(current);
}
pushToMemory(providingPoses);
- changed.clear();
+ changes.clear();
storedPoses = remainingPoses;
isMemoryVizRequired = true;
@@ -138,136 +125,60 @@ namespace armarx
visualizeObject(objectPose);
}
- for (objpose::ObjectPose & objectPose: newPoses)
- {
- visualizeObject(objectPose);
- }
+ changes.visualizeNewObjects(this);
}
- void Editor::visualizeObject(objpose::ObjectPose &objectPose)
+ 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++;
- }
+ VisualizationDescription description = changes.buildVisualizationDescription(objectPose);
viz::InteractionDescription interaction = viz::interaction().selection();
- if (canMove)
+ if (description.allowTransforming)
{
interaction.transform().hideDuringTransform();
}
- if (not options.empty())
+ if (not description.options.empty())
{
- interaction.contextMenu(options);
- }
-
- Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
-
- if (isChanged)
- {
- transform = changed[objectPose.objectID.str()].transform;
+ interaction.contextMenu(description.options);
}
viz::Object object =
viz::Object(objectPose.objectID.str())
- .pose(transform * objectPose.objectPoseGlobal)
+ .pose(description.transform * objectPose.objectPoseGlobal)
.fileByObjectFinder(objectPose.objectID)
.enable(interaction);
- if (isChanged)
+ if (description.alpha.has_value())
{
- const float alpha = 0.5;
-
- switch (iterator->second.kind)
- {
- case MOVE:
- object.alpha(alpha);
- break;
-
- case CREATE:
- object.overrideColor(simox::Color(0.0F, 1.0F, 0.0F, alpha));
- break;
+ object.alpha(description.alpha.value());
+ }
- case DELETE:
- object.overrideColor(simox::Color(1.0F, 0.0F, 0.0F, alpha));
- }
+ if (description.color.has_value())
+ {
+ object.overrideColor(description.color.value());
}
observer.addObserved(memoryLayer, object)
- .onContextMenu(cloneIndex, [&]
+ .onContextMenu(description.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;
- }
-
+ changes.cloneObject(objectPose);
isMemoryVizRequired = true;
})
- .onContextMenu(deleteIndex, [&]
+ .onContextMenu(description.deleteIndex, [&]
{
- changed[objectPose.objectID.str()].kind = DELETE;
-
+ changes.deleteObject(objectPose);
isMemoryVizRequired = true;
})
- .onContextMenu(resetIndex, [&]
+ .onContextMenu(description.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;
- }
+ changes.resetObject(objectPose);
+ isMemoryVizRequired = true;
})
.onTransformEnd([&](const Eigen::Matrix4f& transform)
{
- Change& change = changed[objectPose.objectID.str()];
- change.transform = transform * change.transform;
-
+ changes.moveObject(objectPose, transform);
isMemoryVizRequired = true;
});
}
@@ -297,4 +208,150 @@ namespace armarx
isMetaVizRequired = true;
});
}
+
+ void Editor::ChangeState::clear()
+ {
+ changed.clear();
+ newPoses.clear();
+ }
+
+ void Editor::ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
+ {
+ seq.insert(seq.begin(), newPoses.begin(), newPoses.end());
+ newPoses.clear();
+ }
+
+ bool Editor::ChangeState::applyTo(objpose::ObjectPose& pose)
+ {
+ auto iterator = changed.find(pose.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ if (isChanged && iterator->second.kind == DELETE)
+ {
+ return false;
+ }
+
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ if (isChanged)
+ {
+ transform = iterator->second.transform;
+ }
+
+ pose.objectPoseGlobal = transform * pose.objectPoseGlobal;
+
+ return true;
+ }
+
+ void Editor::ChangeState::visualizeNewObjects(Editor* editor)
+ {
+ for (objpose::ObjectPose & objectPose: newPoses)
+ {
+ editor->visualizeObject(objectPose);
+ }
+ }
+
+ Editor::VisualizationDescription Editor::ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
+ {
+ VisualizationDescription description;
+
+ auto iterator = changed.find(object.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ ChangeKind kind = MOVE;
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+ kind = change.kind;
+ }
+
+ description.allowTransforming = kind != DELETE;
+
+ size_t currentIndex = 0;
+
+ if (kind == MOVE)
+ {
+ description.options.emplace_back("Clone");
+ description.options.emplace_back("Delete");
+
+ description.cloneIndex = currentIndex++;
+ description.deleteIndex = currentIndex++;
+ }
+
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+
+ description.options.emplace_back("Reset");
+ description.resetIndex = currentIndex++;
+
+ description.transform = change.transform;
+
+ const float alpha = 0.5;
+
+ switch (kind)
+ {
+ case MOVE:
+ description.alpha = alpha;
+ break;
+
+ case CREATE:
+ description.color = simox::Color(0.0F, 1.0F, 0.0F, alpha);
+ break;
+
+ case DELETE:
+ description.color = simox::Color(1.0F, 0.0F, 0.0F, alpha);
+ break;
+ }
+ }
+
+ return description;
+ }
+
+ void Editor::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());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back(object);
+ newPose.objectID = object.objectID.withInstanceName(object.objectID.instanceName() + suffix);
+
+ Change& clonedChange = changed[newPose.objectID.str()];
+ clonedChange.kind = CREATE;
+ clonedChange.transform = Eigen::Affine3f(Eigen::Translation3f(1000, 1000, 1000)).matrix();
+ clonedChange.iterator = std::prev(newPoses.end());
+
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, originalChange] = *iterator;
+ clonedChange.transform *= originalChange.transform;
+ }
+ }
+
+ void Editor::ChangeState::deleteObject(const objpose::ObjectPose& object)
+ {
+ changed[object.objectID.str()].kind = DELETE;
+ }
+
+ void Editor::ChangeState::resetObject(const objpose::ObjectPose& object)
+ {
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, change] = *iterator;
+
+ if (change.kind == CREATE)
+ {
+ newPoses.erase(change.iterator);
+ }
+
+ changed.erase(iterator);
+ }
+ }
+
+ void Editor::ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
+ {
+ Change& change = changed[object.objectID.str()];
+ change.transform = transform * change.transform;
+ }
} // namespace armarx
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
index 92ee3c6b3..cd2c5cf54 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
@@ -32,26 +32,58 @@ namespace armarx
InteractionObserver observer;
- enum ChangeKind
+ objpose::ObjectPoseSeq storedPoses;
+
+ struct VisualizationDescription
{
- MOVE,
- CREATE,
- DELETE
- };
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ bool allowTransforming = true;
- using ObjectPoseLst = std::list<objpose::ObjectPose>;
+ std::vector<std::string> options {};
+ std::optional<float> alpha {};
+ std::optional<simox::Color> color {};
- objpose::ObjectPoseSeq storedPoses;
- ObjectPoseLst newPoses;
+ 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();
+ };
- struct Change
+ class ChangeState
{
- ChangeKind kind = MOVE;
- Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
- ObjectPoseLst::iterator iterator {};
+ public:
+ void clear();
+ void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
+ bool applyTo(objpose::ObjectPose& pose);
+ void visualizeNewObjects(Editor* editor);
+ VisualizationDescription buildVisualizationDescription(objpose::ObjectPose& object);
+
+ void cloneObject(objpose::ObjectPose const& object);
+ 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 {};
+ };
+
+ ObjectPoseLst newPoses;
+ std::map<std::string, Change> changed;
};
- std::map<std::string, Change> changed;
+ ChangeState changes;
bool isPushRequired;
bool isPullRequired;
--
GitLab