RoomEditor.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    navigation::ArmarXObjects::room_editor
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RoomEditor.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <string>
#include <utility>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/Interaction.h>
#include <RobotAPI/components/ArViz/Client/elements/Path.h>
 
namespace armarx::navigation::components::room_editor
{
 
    namespace
    {
        /// The minimum number of vertices a room polygon must retain.
        constexpr std::size_t MinPolygonSize = 3;
 
        const std::vector<std::string> VertexContextMenu{"Insert point after",
                                                         "Insert point before",
                                                         "Delete point"};
 
        const std::vector<std::string> EdgeContextMenu{"Cut edge here"};
 
        /// Human readable description of an interaction, for diagnosing the ArViz setup.
        std::string
        describe(const viz::InteractionFeedback& interaction)
        {
            std::string description = viz::toString(interaction.type());
 
            if (interaction.type() == viz::InteractionFeedbackType::Transform)
            {
                description += "(";
                description += interaction.isTransformBegin() ? "begin " : "";
                description += interaction.isTransformDuring() ? "during " : "";
                description += interaction.isTransformEnd() ? "end" : "";
                description += ")";
            }
 
            return description + " on `" + interaction.element() + "`";
        }
 
        Eigen::Vector3f
        to3D(const Eigen::Vector2f& point, float z)
        {
            return Eigen::Vector3f{point.x(), point.y(), z};
        }
 
        Eigen::Vector2f
        centroid(const std::vector<Eigen::Vector2f>& polygon)
        {
            Eigen::Vector2f sum = Eigen::Vector2f::Zero();
            for (const auto& point : polygon)
            {
                sum += point;
            }
            return sum / static_cast<float>(std::max<std::size_t>(polygon.size(), 1));
        }
 
        /// Rotates `polygon` by `angle` [rad] about `pivot`, then translates it by `translation`.
        std::vector<Eigen::Vector2f>
        applyRigidMotion(const std::vector<Eigen::Vector2f>& polygon,
                         const Eigen::Vector2f& pivot,
                         float angle,
                         const Eigen::Vector2f& translation)
        {
            const Eigen::Rotation2Df rotation(angle);
 
            std::vector<Eigen::Vector2f> result;
            result.reserve(polygon.size());
            for (const auto& point : polygon)
            {
                result.push_back(pivot + rotation * (point - pivot) + translation);
            }
            return result;
        }
    } // namespace
 
    RoomEditor::RoomEditor(viz::Client arviz, const Parameters& parameters) :
        arviz(std::move(arviz)), parameters(parameters)
    {
    }
 
    void
    RoomEditor::setRooms(const std::vector<EditableRoom>& rooms)
    {
        pendingTransform.reset();
        rooms_ = rooms;
    }
 
    const std::vector<EditableRoom>&
    RoomEditor::rooms() const
    {
        return rooms_;
    }
 
    void
    RoomEditor::setRoomChangedCallback(RoomChangedCallback callback)
    {
        roomChangedCallback = std::move(callback);
    }
 
    bool
    RoomEditor::isInteracting() const
    {
        return pendingTransform.has_value();
    }
 
    void
    RoomEditor::update(const viz::CommitResult& result, viz::StagedCommit& stage)
    {
        if (handleLayer.data_.name.empty())
        {
            // Nothing has been drawn yet, so there cannot be any interaction either.
            return;
        }
 
        stage.requestInteraction(handleLayer);
 
        for (const viz::InteractionFeedback& interaction : result.interactions())
        {
            if (interaction.layer() != handleLayer.data_.name)
            {
                continue;
            }
 
            handleInteraction(interaction, stage);
        }
    }
 
    void
    RoomEditor::handleInteraction(const viz::InteractionFeedback& interaction,
                                  viz::StagedCommit& stage)
    {
        // Dragging produces a lot of intermediate events, so only the interesting ones are
        // logged at INFO. If nothing shows up here at all while dragging in ArViz, the
        // interaction never reached this component (see the note in `Component::run()`).
        const bool isDragUpdate = interaction.type() == viz::InteractionFeedbackType::Transform and
                                  not interaction.isTransformEnd();
        if (isDragUpdate)
        {
            ARMARX_VERBOSE << "Interaction: " << describe(interaction);
        }
        else
        {
            ARMARX_INFO << "Interaction: " << describe(interaction);
        }
 
        const auto it = handleByElement.find(interaction.element());
        if (it == handleByElement.end())
        {
            ARMARX_VERBOSE << "Interaction with unknown element `" << interaction.element() << "`.";
            return;
        }
 
        const Handle handle = it->second;
 
        switch (interaction.type())
        {
            case viz::InteractionFeedbackType::Transform:
            {
                const Eigen::Matrix4f transform = interaction.transformation();
                const Eigen::Vector3f translation = transform.block<3, 1>(0, 3);
 
                // The interaction is restricted to the XY plane, so the z component is dropped.
                if (handle.type == Handle::Type::Move)
                {
                    const Eigen::Matrix3f rotation = transform.block<3, 3>(0, 0);
                    const float angle = std::atan2(rotation(1, 0), rotation(0, 0));
 
                    pendingTransform = PendingTransform{.element = interaction.element(),
                                                        .handle = handle,
                                                        .rotation = angle,
                                                        .translation = translation.head<2>()};
                }
                else
                {
                    pendingTransform =
                        PendingTransform{.element = interaction.element(),
                                         .handle = handle,
                                         .position = handlePosition(handle) + translation.head<2>()};
                }
 
                if (interaction.isTransformEnd())
                {
                    applyPendingTransform(stage);
                }
                else
                {
                    // Live preview of the polygon while dragging.
                    redrawRooms(stage);
                }
            }
            break;
 
            case viz::InteractionFeedbackType::Deselect:
            {
                // Depending on the ArViz GUI, the drag may only be concluded by a deselect.
                if (pendingTransform.has_value() and
                    pendingTransform->element == interaction.element())
                {
                    applyPendingTransform(stage);
                }
            }
            break;
 
            case viz::InteractionFeedbackType::ContextMenuChosen:
            {
                handleContextMenu(handle, interaction.chosenContextMenuEntry(), stage);
            }
            break;
 
            default:
                // Nothing to do for the remaining interaction types.
                break;
        }
    }
 
    void
    RoomEditor::applyPendingTransform(viz::StagedCommit& stage)
    {
        if (not pendingTransform.has_value())
        {
            return;
        }
 
        const PendingTransform pending = *pendingTransform;
        pendingTransform.reset();
 
        EditableRoom& editable = rooms_.at(pending.handle.roomIndex);
        std::vector<Eigen::Vector2f>& polygon = editable.room.polygon;
 
        if (pending.handle.type == Handle::Type::Move)
        {
            polygon = applyRigidMotion(
                polygon, centroid(polygon), pending.rotation, pending.translation);
 
            ARMARX_INFO << "Moved room `" << editable.room.name << "` by ["
                        << pending.translation.x() << ", " << pending.translation.y()
                        << "] and rotated it by " << pending.rotation << " rad.";
 
            notifyRoomChanged(pending.handle.roomIndex);
            redrawAll(stage);
            return;
        }
 
        if (pending.handle.pointIndex >= polygon.size())
        {
            ARMARX_WARNING << "The edited polygon changed underneath the interaction. "
                              "Discarding the transformation.";
            redrawAll(stage);
            return;
        }
 
        if (pending.handle.type == Handle::Type::Vertex)
        {
            polygon.at(pending.handle.pointIndex) = pending.position;
 
            ARMARX_INFO << "Moved point " << pending.handle.pointIndex << " of room `"
                        << editable.room.name << "` to [" << pending.position.x() << ", "
                        << pending.position.y() << "].";
        }
        else
        {
            polygon.insert(polygon.begin() +
                               static_cast<std::ptrdiff_t>(pending.handle.pointIndex) + 1,
                           pending.position);
 
            ARMARX_INFO << "Cut edge " << pending.handle.pointIndex << " of room `"
                        << editable.room.name << "` at [" << pending.position.x() << ", "
                        << pending.position.y() << "].";
        }
 
        notifyRoomChanged(pending.handle.roomIndex);
        redrawAll(stage);
    }
 
    void
    RoomEditor::handleContextMenu(const Handle& handle, int entry, viz::StagedCommit& stage)
    {
        // A context menu action always supersedes a (possibly) ongoing drag.
        pendingTransform.reset();
 
        EditableRoom& editable = rooms_.at(handle.roomIndex);
        std::vector<Eigen::Vector2f>& polygon = editable.room.polygon;
 
        if (handle.pointIndex >= polygon.size())
        {
            ARMARX_WARNING << "The edited polygon changed underneath the interaction. "
                              "Ignoring the context menu action.";
            return;
        }
 
        const auto insertAt = [&polygon](std::size_t index, const Eigen::Vector2f& point)
        { polygon.insert(polygon.begin() + static_cast<std::ptrdiff_t>(index), point); };
 
        if (handle.type == Handle::Type::Edge)
        {
            // The edge handle has a single entry: cut the edge at its midpoint.
            const Eigen::Vector2f midpoint = handlePosition(handle);
            insertAt(handle.pointIndex + 1, midpoint);
 
            ARMARX_INFO << "Cut edge " << handle.pointIndex << " of room `" << editable.room.name
                        << "`.";
        }
        else
        {
            const std::size_t i = handle.pointIndex;
            const std::size_t n = polygon.size();
 
            switch (static_cast<ContextMenuEntry>(entry))
            {
                case ContextMenuEntry::InsertPointAfter:
                {
                    const Eigen::Vector2f newPoint =
                        0.5F * (polygon.at(i) + polygon.at((i + 1) % n));
                    insertAt(i + 1, newPoint);
 
                    ARMARX_INFO << "Inserted a point after point " << i << " of room `"
                                << editable.room.name << "`.";
                }
                break;
 
                case ContextMenuEntry::InsertPointBefore:
                {
                    const Eigen::Vector2f newPoint =
                        0.5F * (polygon.at(i) + polygon.at((i + n - 1) % n));
                    insertAt(i, newPoint);
 
                    ARMARX_INFO << "Inserted a point before point " << i << " of room `"
                                << editable.room.name << "`.";
                }
                break;
 
                case ContextMenuEntry::DeletePoint:
                {
                    if (n <= MinPolygonSize)
                    {
                        ARMARX_WARNING << "Refusing to delete point " << i << " of room `"
                                       << editable.room.name << "`: a polygon needs at least "
                                       << MinPolygonSize << " points.";
                        return;
                    }
 
                    polygon.erase(polygon.begin() + static_cast<std::ptrdiff_t>(i));
 
                    ARMARX_INFO << "Deleted point " << i << " of room `" << editable.room.name
                                << "`.";
                }
                break;
 
                default:
                {
                    ARMARX_WARNING << "Unknown context menu entry " << entry << ".";
                    return;
                }
            }
        }
 
        notifyRoomChanged(handle.roomIndex);
        redrawAll(stage);
    }
 
    void
    RoomEditor::redrawAll(viz::StagedCommit& stage)
    {
        handleByElement.clear();
        handleLayer = arviz.layer(parameters.layerPrefix + "_Handles");
 
        for (std::size_t roomIndex = 0; roomIndex < rooms_.size(); ++roomIndex)
        {
            const EditableRoom& editable = rooms_.at(roomIndex);
            const std::vector<Eigen::Vector2f>& polygon = editable.room.polygon;
 
            const float z = editable.room.zFloor + parameters.handleZOffset;
 
            for (std::size_t i = 0; i < polygon.size(); ++i)
            {
                const Handle handle{
                    .type = Handle::Type::Vertex, .roomIndex = roomIndex, .pointIndex = i};
                const std::string name = elementName(handle);
                handleByElement[name] = handle;
 
                handleLayer.add(viz::Sphere(name)
                                    .position(to3D(polygon.at(i), z))
                                    .radius(parameters.vertexHandleRadius)
                                    .colorGlasbeyLUT(roomIndex)
                                    .enable(viz::interaction()
                                                .translation(viz::AXES_XY)
                                                .contextMenu(VertexContextMenu)));
            }
 
            // One handle per edge, sitting on its midpoint. The last edge closes the polygon.
            if (polygon.size() >= 2)
            {
                for (std::size_t i = 0; i < polygon.size(); ++i)
                {
                    const Handle handle{
                        .type = Handle::Type::Edge, .roomIndex = roomIndex, .pointIndex = i};
                    const std::string name = elementName(handle);
                    handleByElement[name] = handle;
 
                    handleLayer.add(viz::Box(name)
                                        .position(to3D(handlePosition(handle), z))
                                        .size(parameters.edgeHandleSize)
                                        .color(viz::Color::gray(160))
                                        .enable(viz::interaction()
                                                    .translation(viz::AXES_XY)
                                                    .contextMenu(EdgeContextMenu)));
                }
            }
 
            // One handle in the room center to translate/rotate the whole room in the XY plane.
            if (polygon.size() >= MinPolygonSize)
            {
                const Handle handle{
                    .type = Handle::Type::Move, .roomIndex = roomIndex, .pointIndex = 0};
                const std::string name = elementName(handle);
                handleByElement[name] = handle;
 
                handleLayer.add(viz::Cylinder(name)
                                    .position(to3D(handlePosition(handle), z))
                                    .radius(parameters.moveHandleRadius)
                                    .height(parameters.moveHandleHeight)
                                    .color(viz::Color::gray(200))
                                    .enable(viz::interaction()
                                                .translation(viz::AXES_XY)
                                                .rotation(viz::AXES_Z)));
            }
        }
 
        stage.add(handleLayer);
 
        redrawRooms(stage);
    }
 
    void
    RoomEditor::redrawRooms(viz::StagedCommit& stage)
    {
        roomLayer = arviz.layer(parameters.layerPrefix + "_Rooms");
 
        for (std::size_t roomIndex = 0; roomIndex < rooms_.size(); ++roomIndex)
        {
            const EditableRoom& editable = rooms_.at(roomIndex);
            const std::vector<Eigen::Vector2f> polygon = previewPolygon(roomIndex);
 
            if (polygon.size() < 2)
            {
                continue;
            }
 
            const std::string prefix = editable.providerSegmentName + "/" + editable.room.name;
            const float z = editable.room.zFloor;
 
            viz::Path outline(prefix + "/outline");
            for (const auto& point : polygon)
            {
                outline.addPoint(to3D(point, z));
            }
            // Close the outline.
            outline.addPoint(to3D(polygon.front(), z));
            outline.colorGlasbeyLUT(roomIndex).width(parameters.lineWidth);
            roomLayer.add(outline);
 
            if (polygon.size() >= MinPolygonSize)
            {
                viz::Polygon area(prefix + "/polygon");
                for (const auto& point : polygon)
                {
                    area.addPoint(to3D(point, z));
                }
                area.colorGlasbeyLUT(roomIndex, 60);
                area.lineColorGlasbeyLUT(roomIndex);
                area.lineWidth(0);
                roomLayer.add(area);
            }
 
            roomLayer.add(viz::Text(prefix + "/label")
                              .text(editable.room.name)
                              .position(to3D(centroid(polygon), z))
                              .scale(parameters.labelScale));
        }
 
        stage.add(roomLayer);
    }
 
    std::vector<Eigen::Vector2f>
    RoomEditor::previewPolygon(std::size_t roomIndex) const
    {
        std::vector<Eigen::Vector2f> polygon = rooms_.at(roomIndex).room.polygon;
 
        if (not pendingTransform.has_value() or pendingTransform->handle.roomIndex != roomIndex)
        {
            return polygon;
        }
 
        const Handle& handle = pendingTransform->handle;
 
        if (handle.type == Handle::Type::Move)
        {
            return applyRigidMotion(polygon,
                                    centroid(polygon),
                                    pendingTransform->rotation,
                                    pendingTransform->translation);
        }
 
        if (handle.pointIndex >= polygon.size())
        {
            return polygon;
        }
 
        if (handle.type == Handle::Type::Vertex)
        {
            polygon.at(handle.pointIndex) = pendingTransform->position;
        }
        else
        {
            polygon.insert(polygon.begin() + static_cast<std::ptrdiff_t>(handle.pointIndex) + 1,
                           pendingTransform->position);
        }
 
        return polygon;
    }
 
    Eigen::Vector2f
    RoomEditor::handlePosition(const Handle& handle) const
    {
        const std::vector<Eigen::Vector2f>& polygon = rooms_.at(handle.roomIndex).room.polygon;
 
        if (handle.type == Handle::Type::Move)
        {
            return centroid(polygon);
        }
 
        if (handle.type == Handle::Type::Vertex)
        {
            return polygon.at(handle.pointIndex);
        }
 
        const std::size_t next = (handle.pointIndex + 1) % polygon.size();
        return 0.5F * (polygon.at(handle.pointIndex) + polygon.at(next));
    }
 
    std::string
    RoomEditor::elementName(const Handle& handle) const
    {
        const EditableRoom& editable = rooms_.at(handle.roomIndex);
        const std::string prefix = editable.providerSegmentName + "/" + editable.room.name;
 
        if (handle.type == Handle::Type::Move)
        {
            return prefix + "/move";
        }
 
        const std::string suffix = handle.type == Handle::Type::Vertex ? "/vertex_" : "/edge_";
 
        return prefix + suffix + std::to_string(handle.pointIndex);
    }
 
    void
    RoomEditor::notifyRoomChanged(std::size_t roomIndex) const
    {
        if (roomChangedCallback)
        {
            roomChangedCallback(rooms_.at(roomIndex));
        }
    }
 
} // namespace armarx::navigation::components::room_editor