Editor.cpp

Go to the documentation of this file.

#include "Editor.h"
 
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
#include <utility>
 
namespace armarx
{
    Editor::Editor(viz::Client& client,
                   Properties properties,
                   std::function<void(objpose::ProvidedObjectPoseSeq &)> pushToMemory,
                   std::function<objpose::ObjectPoseSeq(void)> pullFromMemory)
        : properties(std::move(properties))
        , client(client)
        , pushToMemory(std::move(pushToMemory))
        , pullFromMemory(std::move(pullFromMemory))
        , isCommitRequired(false)
        , isUpdateRequired(true)
        , isMemoryVizRequired(true)
        , isMetaVizRequired(true)
    {
        placeholderOptions.emplace_back("Remove");
        placeholderOptions.emplace_back(lineString);
 
        for (auto const& availableObject : this->properties.availableObjects)
        {
            placeholderOptions.push_back(armarx::fromIce(availableObject).str());
        }
    }
 
    void Editor::step()
    {
        viz::StagedCommit stage = client.stage();
 
        if (isUpdateRequired)
        {
            storedPoses = update();
 
            isUpdateRequired = false;
        }
 
        if (isResetRequired)
        {
            reset();
 
            isResetRequired = false;
        }
 
        if (isMemoryVizRequired)
        {
            visualizeMemory();
            stage.add(memoryLayer);
 
            isMemoryVizRequired = false;
        }
 
        if (isMetaVizRequired)
        {
            visualizeMeta();
            stage.add(metaLayer);
 
            isMetaVizRequired = false;
        }
 
        if (isCommitRequired)
        {
            commit();
 
            isCommitRequired = false;
        }
 
        observer.requestInteractions(stage);
 
        viz::CommitResult result = client.commit(stage);
 
        observer.process(result.interactions());
    }
 
    objpose::ObjectPoseSeq
    Editor::update()
    {
        objpose::ObjectPoseSeq newRequestedPoses = pullFromMemory();
 
        isMemoryVizRequired = true;
 
        return newRequestedPoses;
    }
 
    void Editor::reset()
    {
        changes.clear();
 
        isMemoryVizRequired = true;
    }
 
    void Editor::commit()
    {
        changes.moveNewObjectsTo(storedPoses);
 
        objpose::ProvidedObjectPoseSeq providingPoses;
        objpose::ObjectPoseSeq remainingPoses;
 
        remainingPoses.reserve(storedPoses.size());
        for (objpose::ObjectPose & current : storedPoses)
        {
            bool isChanged = changes.applyTo(current);
 
            if (isChanged)
            {
                providingPoses.push_back(current.toProvidedObjectPoseGlobal());
                objpose::ProvidedObjectPose& providing = providingPoses.back();
 
                providing.providerName = properties.providerName;
                providing.timestamp = DateTime::Now();
            }
 
            if (current.confidence > properties.confidenceThreshold)
            {
                remainingPoses.push_back(current);
            }
        }
 
        pushToMemory(providingPoses);
 
        changes.clear();
        storedPoses = remainingPoses;
 
        isMemoryVizRequired = true;
    }
 
    void Editor::visualizeMemory()
    {
        observer.clearObservedLayer(memoryLayer);
 
        for (objpose::ObjectPose & objectPose: storedPoses)
        {
            if (objectPose.confidence > properties.confidenceThreshold)
            {
                visualizeObject(objectPose);
            }
        }
 
        changes.visualizeNewObjects();
    }
 
    void Editor::visualizeObject(objpose::ObjectPose& objectPose)
    {
        VisualizationDescription description = changes.buildVisualizationDescription(objectPose);
 
        viz::InteractionDescription interaction = viz::interaction().selection();
 
        if (description.allowTransforming)
        {
            interaction.transform().hideDuringTransform();
        }
 
        if (not description.options.empty())
        {
            interaction.contextMenu(description.options);
        }
 
        viz::Object object =
                viz::Object(objectPose.objectID.str())
                .pose(description.transform * objectPose.objectPoseGlobal)
                .scale(properties.objectScaling)
                .fileByObjectFinder(objectPose.objectID)
                .enable(interaction);
 
        if (description.alpha.has_value())
        {
            object.alpha(description.alpha.value());
        }
 
        if (description.color.has_value())
        {
            object.overrideColor(description.color.value());
        }
 
        observer.addObserved(memoryLayer, object)
            .onContextMenu(description.cloneIndex, [this, &objectPose]
            {
                changes.cloneObject(objectPose);
                isMemoryVizRequired = true;
            })
            .onContextMenu(description.deleteIndex, [this, &objectPose]
            {
                changes.deleteObject(objectPose);
                isMemoryVizRequired = true;
            })
            .onContextMenu(description.resetIndex, [this, &objectPose]
            {
                changes.resetObject(objectPose);
                isMemoryVizRequired = true;
            })
            .onContextMenu(description.prototypeIndex, [this, &objectPose]
            {
                const float defaultExtents = 100;
                simox::OrientedBoxf box(objectPose.objectPoseGlobal, Eigen::Vector3f(defaultExtents, defaultExtents, defaultExtents));
 
                box = objectPose.oobbGlobal().value_or(box);
                box = box.transformed(changes.getTransform(objectPose));
 
                placeholders.addPlaceholder(box);
                isMetaVizRequired = true;
            })
            .onContextMenu(description.commitIndex, [this]
            {
                isCommitRequired = true;
            })
            .onContextMenu(description.updateIndex, [this]
            {
                isUpdateRequired = true;
            })
            .onContextMenu(description.resetAllIndex, [this]
            {
                isResetRequired = true;
            })
            .onTransformEnd([this, &objectPose](const Eigen::Matrix4f& transform)
            {
                changes.moveObject(objectPose, transform);
                isMemoryVizRequired = true;
            });
    }
 
    void Editor::visualizeMeta()
    {
        observer.clearObservedLayer(metaLayer);
 
        placeholders.visualizePlaceholders();
    }
 
    void Editor::visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id)
    {
        viz::InteractionDescription interaction = viz::interaction()
                .selection().transform().hideDuringTransform().contextMenu(placeholderOptions);
 
        viz::Box box = viz::Box("placeholder_" + std::to_string(id))
                        .set(placeholder.box.transformed(placeholder.transform))
                        .color(simox::Color::yellow(255, 128))
                        .enable(interaction);
 
        auto& observation = observer.addObserved(metaLayer, box)
                .onContextMenu(0, [this, id]()
                {
                    placeholders.removePlaceholder(id);
                    isMetaVizRequired = true;
                })
                .onTransformEnd([this, id](Eigen::Matrix4f const& transform)
                {
                    placeholders.movePlaceholder(id, transform);
                    isMetaVizRequired = true;
                });
 
        for (size_t index = 2; index < placeholderOptions.size(); index++)
        {
            std::string const& object = placeholderOptions[index];
 
            observation.onContextMenu(index, [this, id, &object]
            {
               placeholders.specifyObject(id, object, changes);
               
               isMetaVizRequired = true;
               isMemoryVizRequired = true;
            });
        }
    }
 
    void ChangeState::clear()
    {
        changed.clear();
        newPoses.clear();
    }
 
    void ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
    {
        seq.insert(seq.begin(), newPoses.begin(), newPoses.end());
        newPoses.clear();
    }
 
    bool ChangeState::applyTo(objpose::ObjectPose& pose)
    {
        auto iterator = changed.find(pose.objectID.str());
        bool isChanged = iterator != changed.end();
 
        if (isChanged)
        {
            auto& [name, change] = *iterator;
 
            if (change.kind == DELETE)
            {
                pose.confidence = 0;
            }
 
            pose.objectPoseGlobal = change.transform * pose.objectPoseGlobal;
        }
 
        return isChanged;
    }
 
    void ChangeState::visualizeNewObjects()
    {
        for (objpose::ObjectPose & objectPose: newPoses)
        {
            editor->visualizeObject(objectPose);
        }
    }
 
    VisualizationDescription 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.cloneIndex = currentIndex++;
 
            description.options.emplace_back("Delete");
            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;
            }
        }
 
        description.options.emplace_back("Create Placeholder");
        description.prototypeIndex = currentIndex++;
 
        description.options.emplace_back(Editor::lineString);
        currentIndex++;
 
        description.options.emplace_back("Commit All Changes");
        description.commitIndex = currentIndex++;
 
        description.options.emplace_back("Update Unchanged");
        description.updateIndex = currentIndex++;
 
        description.options.emplace_back("Reset All");
        description.resetAllIndex = currentIndex; // ++
 
        return description;
    }
 
    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());
 
        objpose::ObjectPose& newPose = newPoses.emplace_back(object);
        newPose.objectID = object.objectID.withInstanceName(object.objectID.instanceName() + suffix);
 
        const float minOffset = 100;
        float offset = minOffset;
        if (object.localOOBB.has_value())
        {
            Eigen::Vector3f size = object.localOOBB.value().corner_max() - object.localOOBB.value().corner_min();
            float objectOffset = size.maxCoeff() / 2;
 
            offset = std::max(minOffset, objectOffset);
        }
 
        Change& clonedChange = changed[newPose.objectID.str()];
        clonedChange.kind = CREATE;
        // 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());
        if (iterator != changed.end())
        {
            auto& [name, originalChange] = *iterator;
            clonedChange.transform *= originalChange.transform;
        }
    }
    
    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());
 
        objpose::ObjectPose& newPose = newPoses.emplace_back();
 
        armarx::ObjectID id(objectID);
        newPose.objectID = id.withInstanceName(id.instanceName() + suffix);
 
        newPose.providerName = editor->properties.providerName;
        newPose.objectType = objpose::ObjectType::KnownObject;
        newPose.isStatic = true;
 
        newPose.objectPoseGlobal = pose;
        newPose.confidence = 1;
        newPose.timestamp = DateTime::Now();
 
        std::optional<armarx::ObjectInfo> info = editor->objectFinder.findObject(id);
        if (info.has_value())
        {
            newPose.localOOBB = info->loadOOBB();
 
            if (newPose.localOOBB.has_value())
            {
                newPose.objectPoseGlobal = pose * newPose.localOOBB->transformation_centered().inverse();
            }
        }
 
        Change& createdChange = changed[newPose.objectID.str()];
        createdChange.kind = CREATE;
        createdChange.transform = Eigen::Matrix4f::Identity();
        createdChange.iterator = std::prev(newPoses.end());
    }
 
    void ChangeState::deleteObject(const objpose::ObjectPose& object)
    {
        changed[object.objectID.str()].kind = DELETE;
    }
 
    void 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 ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
    {
        Change& change = changed[object.objectID.str()];
        change.transform = transform * change.transform;
    }
 
    Eigen::Matrix4f ChangeState::getTransform(const objpose::ObjectPose &object)
    {
        Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
 
        auto iterator = changed.find(object.objectID.str());
        if (iterator != changed.end())
        {
            auto& [name, change] = *iterator;
            transform = change.transform;
        }
 
        return transform;
    }
 
    void PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
    {
        size_t id = getID();
 
        auto& entry = placeholders[id];
        entry.placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
        entry.isActive = true;
    }
 
    void PlaceholderState::visualizePlaceholders()
    {
        for (size_t id = 0; id < placeholders.size(); id++)
        {
            auto& entry = placeholders[id];
 
            if (entry.isActive)
            {
                editor->visualizePlaceholder(entry.placeholder, id);
            }
        }
    }
 
    void PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
    {
        auto& placeholder = placeholders[id].placeholder;
        placeholder.transform = transform * placeholder.transform;
    }
 
    void PlaceholderState::removePlaceholder(size_t id)
    {
        placeholders[id].isActive = false;
 
        unusedIDs.push(id);
    }
 
    size_t PlaceholderState::getID()
    {
        if (unusedIDs.empty())
        {
            size_t id = placeholders.size();
            placeholders.push_back({Placeholder(), false});
            return id;
        }
 
        size_t id = unusedIDs.top();
        unusedIDs.pop();
        return id;
    }
 
    void PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
    {
        auto& placeholder = placeholders[id].placeholder;
 
        Eigen::Matrix4f pose = placeholder.box.transformed(placeholder.transform).transformation_centered();
        changeState.createObject(objectID, pose);
        
        removePlaceholder(id);
    }
}  // namespace armarx