Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <math.h>
 
#include <cstddef>
#include <map>
#include <optional>
#include <string>
#include <vector>
 
#include <range/v3/algorithm/contains.hpp>
#include <range/v3/view/map.hpp>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
 
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
#include "RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h"
#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
 
namespace armarx::armem::server::obj::instance
{
 
    void
    Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(enabled, prefix + "enabled", "Enable or disable visualization of objects.");
        defs->optional(minConfidence,
                       prefix + "minConfidence",
                       "Min confidence to visualize objects. If decay is enabled, "
                       "`decay.minConfidence` will be used instead.");
        defs->optional(colModelsEnabled,
                       prefix + "colModelsEnabled",
                       "Enable or disable visualization of collision models of objects.");
        defs->optional(frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
        defs->optional(inGlobalFrame,
                       prefix + "inGlobalFrame",
                       "If true, show global poses. If false, show poses in robot frame.");
        defs->optional(alpha, prefix + "alpha", "Alpha of objects (1 = solid, 0 = transparent).");
        defs->optional(alphaByConfidence,
                       prefix + "alphaByConfidence",
                       "If true, use the pose confidence as alpha (if < 1.0).");
        defs->optional(oobbs, prefix + "oobbs", "Enable showing oriented bounding boxes.");
 
        defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
        defs->optional(
            objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
 
        gaussians.defineProperties(defs, prefix + "gaussians.");
 
        defs->optional(useArticulatedModels,
                       prefix + "useArticulatedModels",
                       "Prefer articulated object models if available.");
 
        linearPredictions.defineProperties(defs, prefix + "predictions.linear.");
    }
 
    void
    Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(
            position, prefix + "position", "Enable showing pose gaussians (position part).");
        defs->optional(
            positionScale, prefix + "positionScale", "Scaling of pose gaussians (position part).");
 
        defs->optional(
            orientation, prefix + "position", "Enable showing pose gaussians (orientation part).");
        defs->optional(orientationScale,
                       prefix + "positionScale",
                       "Scaling of pose gaussians (orientation part).");
        defs->optional(
            orientationDisplaced,
            prefix + "positionDisplaced",
            "Displace center orientation (co)variance circle arrows along their rotation axis.");
    }
 
    std::vector<viz::Layer>
    Visu::visualizeCommit(
        const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
        const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
            poseHistories,
        const ObjectFinder& objectFinder) const
    {
        std::vector<viz::Layer> layers;
        for (const auto& [name, poses] : objectPoses)
        {
            if (name.empty())
            {
                continue;
            }
 
            auto poseHistoryMap = poseHistories.find(name);
            if (poseHistoryMap != poseHistories.end())
            {
                layers.push_back(
                    visualizeProvider(name, poses, poseHistoryMap->second, objectFinder));
            }
            else
            {
                layers.push_back(visualizeProvider(name, poses, {}, objectFinder));
            }
        }
        return layers;
    }
 
    std::vector<viz::Layer>
    Visu::visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
                          const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
                          const ObjectFinder& objectFinder) const
    {
        std::map<std::string, viz::Layer> stage;
        for (size_t i = 0; i < objectPoses.size(); ++i)
        {
            const std::string providerName = objectPoses.at(i).providerName;
            if (providerName.empty())
            {
                ARMARX_INFO << "Object pose provider not set!";
                continue;
            }
 
            visualizeObjectPose(getLayer(providerName, stage),
                                objectPoses.at(i),
                                poseHistories.at(i),
                                objectFinder,
                                false);
 
            if (colModelsEnabled)
            {
                visualizeObjectPose(getLayer(providerName + "_col", stage),
                                    objectPoses.at(i),
                                    poseHistories.at(i),
                                    objectFinder,
                                    true);
            }
        }
        return simox::alg::get_values(stage);
    }
 
    std::vector<viz::Layer>
    Visu::visualizeCommit(
        const objpose::ObjectPoseMap& objectPoses,
        const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
        const ObjectFinder& objectFinder) const
    {
        std::map<std::string, viz::Layer> stage;
        for (const auto& [id, pose] : objectPoses)
        {
            if (pose.providerName.empty())
            {
                ARMARX_WARNING << "The object " << QUOTED(id)
                               << " does not have a `providerName` set";
                continue;
            }
 
            auto poseHistoryMap = poseHistories.find(id);
            if (poseHistoryMap != poseHistories.end())
            {
                visualizeObjectPose(getLayer(pose.providerName, stage),
                                    pose,
                                    poseHistoryMap->second,
                                    objectFinder,
                                    false);
            }
            else
            {
                visualizeObjectPose(
                    getLayer(pose.providerName, stage), pose, {}, objectFinder, false);
            }
 
            // visualize collision models
            if (colModelsEnabled)
            {
                if (poseHistoryMap != poseHistories.end())
                {
                    visualizeObjectPose(getLayer(pose.providerName + "_col", stage),
                                        pose,
                                        poseHistoryMap->second,
                                        objectFinder,
                                        true);
                }
                else
                {
                    visualizeObjectPose(
                        getLayer(pose.providerName + "_col", stage), pose, {}, objectFinder, true);
                }
            }
        }
        return simox::alg::get_values(stage);
    }
 
    viz::Layer&
    Visu::getLayer(const std::string& providerName, std::map<std::string, viz::Layer>& stage) const
    {
        ARMARX_CHECK_NOT_EMPTY(providerName)
            << "`providerName` used as layer name: must not be empty";
        auto it = stage.find(providerName);
        if (it == stage.end())
        {
            it = stage.emplace(providerName, arviz.layer(providerName)).first;
        }
        return it->second;
    }
 
    viz::Layer
    Visu::visualizeProvider(
        const std::string& providerName,
        const objpose::ObjectPoseSeq& objectPoses,
        const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
        const ObjectFinder& objectFinder) const
    {
        viz::Layer layer = arviz.layer(providerName);
        for (size_t i = 0; i < poseHistories.size(); ++i)
        {
            visualizeObjectPose(layer, objectPoses.at(i), poseHistories.at(i), objectFinder, false);
 
            if (colModelsEnabled)
            {
                visualizeObjectPose(
                    layer, objectPoses.at(i), poseHistories.at(i), objectFinder, true);
            }
        }
        return layer;
    }
 
    void
    Visu::visualizeObjectPose(viz::Layer& layer,
                              const objpose::ObjectPose& objectPose,
                              const std::map<DateTime, objpose::ObjectPose>& poseHistory,
                              const ObjectFinder& objectFinder,
                              const bool useColModel) const
    {
        const bool show = objectPose.confidence >= minConfidence;
        if (not show)
        {
            return;
        }
        const armarx::ObjectID id = objectPose.objectID;
        const std::string key = id.str();
 
        const Eigen::Matrix4f pose =
            inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
        std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
 
        auto makeObject = [&objectInfo, &id](const std::string& key)
        {
            viz::Object object(key);
            if (objectInfo)
            {
                object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
            }
            else
            {
                object.fileByObjectFinder(id);
            }
            return object;
        };
 
        bool hasObject = false;
        {
 
            bool done = false;
            if (useArticulatedModels and objectInfo)
            {
                if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
                {
                    viz::Robot robot(key);
                    robot.pose(pose);
                    robot.file(model->package, model->package + "/" + model->relativePath);
                    robot.joints(objectPose.objectJointValues);
 
                    if (useColModel)
                    {
                        robot.useCollisionModel();
                        // robot.overrideColor(colVisColor);
                    }
 
                    layer.add(robot);
                    done = true;
                }
            }
            if (not done)
            {
                viz::Object object = makeObject(key).pose(pose);
                if (alphaByConfidence && objectPose.confidence < 1.0f)
                {
                    object.alpha(objectPose.confidence);
                }
                else if (alpha < 1)
                {
                    object.alpha(alpha);
                }
 
                if (useColModel)
                {
                    object.useCollisionModel();
                    // object.overrideColor(colVisColor);
                }
 
 
                layer.add(object);
                hasObject = true;
            }
        }
 
        if (oobbs and objectPose.localOOBB)
        {
            const simox::OrientedBoxf oobb =
                inGlobalFrame ? objectPose.oobbGlobal().value() : objectPose.oobbRobot().value();
            layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
        }
        if (objectFrames)
        {
            layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
        }
        if (gaussians.showAny() and
            (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value()
                           : objectPose.objectPoseRobotGaussian.has_value()))
        {
            gaussians.draw(layer, objectPose, inGlobalFrame);
        }
        if (linearPredictions.showAny() and hasObject)
        {
            linearPredictions.draw(layer, makeObject, objectPose, poseHistory, inGlobalFrame);
        }
    }
 
    bool
    Visu::Gaussians::showAny() const
    {
        return position or orientation;
    }
 
    void
    Visu::Gaussians::draw(viz::Layer& layer,
                          const objpose::ObjectPose& objectPose,
                          bool inGlobalFrame) const
    {
        const std::string key = objectPose.objectID.str();
 
        const objpose::PoseManifoldGaussian& gaussian =
            inGlobalFrame ? objectPose.objectPoseGlobalGaussian.value()
                          : objectPose.objectPoseRobotGaussian.value();
        objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
 
        if (position)
        {
            layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
                          .position(ellipsoid.center)
                          .orientation(ellipsoid.orientation)
                          .axisLengths(positionScale * ellipsoid.size)
                          .color(viz::Color::azure(255, 32)));
 
            if (false) // Arrows can be visualized for debugging.
            {
                for (int i = 0; i < 3; ++i)
                {
                    Eigen::Vector3i color = Eigen::Vector3i::Zero();
                    color(i) = 255;
 
                    layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
                                  .fromTo(ellipsoid.center,
                                          ellipsoid.center + positionScale * ellipsoid.size(i) *
                                                                 ellipsoid.orientation.col(i))
                                  .width(5)
                                  .color(simox::Color(color)));
                }
            }
        }
        if (orientation)
        {
            const float pi = static_cast<float>(M_PI);
            for (int i = 0; i < 3; ++i)
            {
                const bool global = true;
                Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
 
                Eigen::Vector4i color = Eigen::Vector4i::Zero();
                color(i) = 255;
                color(3) = 64;
 
                layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
                              .position(orientationDisplaced
                                            ? ellipsoid.center + orientationScale * rot.axis()
                                            : ellipsoid.center)
                              .normal(rot.axis())
                              .radius(orientationScale)
                              .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
                              .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
                              .color(simox::Color(color)));
            }
        }
    }
 
    void
    Visu::RemoteGui::setup(const Visu& visu)
    {
        using namespace armarx::RemoteGui::Client;
 
        enabled.setValue(visu.enabled);
        inGlobalFrame.setValue(visu.inGlobalFrame);
        alpha.setRange(0, 1.0);
        alpha.setValue(visu.alpha);
        alphaByConfidence.setValue(visu.alphaByConfidence);
        oobbs.setValue(visu.oobbs);
 
        auto initScale = [](FloatSpinBox& scale, float value, float stepResolution)
        {
            float max = 10000;
            scale.setRange(0, max);
            scale.setDecimals(2);
            scale.setSteps(int(stepResolution * max));
            scale.setValue(value);
        };
        objectFrames.setValue(visu.objectFrames);
        initScale(objectFramesScale, visu.objectFramesScale, 10);
 
        gaussians.position.setValue(visu.gaussians.position);
        initScale(gaussians.positionScale, visu.gaussians.positionScale, 10);
 
        gaussians.orientation.setValue(visu.gaussians.orientation);
        initScale(gaussians.orientationScale, visu.gaussians.orientationScale, 0.5);
        gaussians.orientationDisplaced.setValue(visu.gaussians.orientationDisplaced);
 
        useArticulatedModels.setValue(visu.useArticulatedModels);
 
 
        GridLayout grid;
        int row = 0;
        grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
        row++;
        grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
        row++;
        grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
        row++;
        grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
        row++;
        grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
        row++;
 
        grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
        grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
        row++;
 
        gaussians.setup(visu);
        grid.add(gaussians.group, {row, 0}, {1, 4});
        row++;
 
        grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
        row++;
 
        linearPredictions.setup(visu.linearPredictions);
        grid.add(linearPredictions.group, {row, 0}, {1, 4});
        row++;
 
        group = GroupBox();
        group.setLabel("Visualization");
        group.addChild(grid);
    }
 
    void
    Visu::RemoteGui::Gaussians::setup(const Visu& data)
    {
        using namespace armarx::RemoteGui::Client;
 
        GridLayout grid;
        int row = 0;
 
        grid.add(Label("Position"), {row, 0}).add(position, {row, 1});
        grid.add(Label("Scale:"), {row, 2}).add(positionScale, {row, 3});
        row++;
 
        grid.add(Label("Orientation"), {row, 0}).add(orientation, {row, 1});
        grid.add(Label("Scale:"), {row, 2}).add(orientationScale, {row, 3});
        grid.add(Label("Displaced"), {row, 4}).add(orientationDisplaced, {row, 5});
        row++;
 
        group = GroupBox();
        group.setLabel("Pose Gaussians");
        group.addChild(grid);
    }
 
    void
    Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
    {
        data.position = position.getValue();
        data.positionScale = positionScale.getValue();
 
        data.orientation = orientation.getValue();
        data.orientationScale = orientationScale.getValue();
        data.orientationDisplaced = orientationDisplaced.getValue();
    }
 
    void
    Visu::RemoteGui::update(Visu& visu)
    {
        visu.enabled = enabled.getValue();
        visu.inGlobalFrame = inGlobalFrame.getValue();
        visu.alpha = alpha.getValue();
        visu.alphaByConfidence = alphaByConfidence.getValue();
        visu.oobbs = oobbs.getValue();
 
        visu.objectFrames = objectFrames.getValue();
        visu.objectFramesScale = objectFramesScale.getValue();
 
        gaussians.update(visu.gaussians);
 
        visu.useArticulatedModels = useArticulatedModels.getValue();
 
        linearPredictions.update(visu.linearPredictions);
    }
 
    std::vector<viz::Layer>
    Visu::visualizeCommit(
        const std::map<std::string, objpose::ObjectPoseMap>& objectPosesByProvider,
        const std::map<std::string, std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>>&
            poseHistoriesByProvider,
        const ObjectFinder& objectFinder) const
    {
        std::map<std::string, viz::Layer> stage;
 
        ARMARX_VERBOSE << "Visualizing commit";
 
        const auto availableProvidersInPoseHistories = ranges::views::keys(poseHistoriesByProvider);
 
        for (const auto& [provider, objectPoses] : objectPosesByProvider)
        {
            for (const auto& [id, pose] : objectPoses)
            {
                if (pose.providerName.empty())
                {
                    ARMARX_WARNING << "The object " << QUOTED(id)
                                   << " does not have a `providerName` set";
                    continue;
                }
 
                const auto visualizeObjPose =
                    [&](const std::map<DateTime, objpose::ObjectPose>& poseHistory)
                {
                    visualizeObjectPose(
                        getLayer(pose.providerName, stage), pose, poseHistory, objectFinder, false);
                };
 
                if (ranges::contains(availableProvidersInPoseHistories, provider))
                {
                    const auto& poseHistories = poseHistoriesByProvider.at(provider);
 
                    auto poseHistoryMap = poseHistories.find(id);
                    if (poseHistoryMap != poseHistories.end())
                    {
                        visualizeObjPose(poseHistoryMap->second);
                    }
                    else
                    {
                        visualizeObjPose({});
                    }
                }
                else
                {
                    visualizeObjPose({});
                }
 
                // visualize collision models
                if (colModelsEnabled)
                {
                    const auto visualizeObjColPose =
                        [&](const std::map<DateTime, objpose::ObjectPose>& poseHistory)
                    {
                        visualizeObjectPose(getLayer(pose.providerName + "_col", stage),
                                            pose,
                                            poseHistory,
                                            objectFinder,
                                            true);
                    };
 
 
                    if (ranges::contains(availableProvidersInPoseHistories, provider))
                    {
                        const auto& poseHistories = poseHistoriesByProvider.at(provider);
                        auto poseHistoryMap = poseHistories.find(id);
 
                        if (poseHistoryMap != poseHistories.end())
                        {
                            visualizeObjColPose(poseHistoryMap->second);
                        }
                        else
                        {
                            visualizeObjColPose({});
                        }
                    }
                    else
                    {
                        visualizeObjColPose({});
                    }
                }
            }
        }
 
 
        // independent of the provider, we want visualize the most recent poses of each object instance in a layer "latest"
        {
            // get latest pose for each object (by iterating over all providers and their poses)
            objpose::ObjectPoseMap latestObjectPoses;
            {
                for (const auto& [provider, objectPoses] : objectPosesByProvider)
                {
                    for (const auto& [id, pose] : objectPoses)
                    {
 
                        // if the object is not already in the map, add it
                        if (latestObjectPoses.count(id) == 0)
                        {
                            latestObjectPoses.emplace(id, pose);
                        }
                        else
                        {
                            // the object is already in the map
                            auto& latestObjectPose = latestObjectPoses.at(id);
 
                            // update pose if it is more recent
                            if (pose.timestamp > latestObjectPose.timestamp)
                            {
                                latestObjectPose = pose;
                            }
                        }
                    }
                }
            }
 
            // visualize latest poses
            {
                auto& layer = getLayer("latest", stage);
 
                for (const auto& [id, pose] : latestObjectPoses)
                {
                    visualizeObjectPose(layer, pose, {}, objectFinder, false);
                }
            }
        }
 
        return simox::alg::get_values(stage);
    }
} // namespace armarx::armem::server::obj::instance