Editor.cpp

Go to the documentation of this file.

#include "Editor.h"
 
#include <utility>
 
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
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