Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
 
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.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(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)
        {
            if(objectPoses.at(i).providerName.empty())
            {
                ARMARX_INFO << "Object pose provider not set!";
                continue;
            }
 
            visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
                                objectPoses.at(i),
                                poseHistories.at(i),
                                objectFinder);
        }
        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)
        {
            
            auto poseHistoryMap = poseHistories.find(id);
            if (poseHistoryMap != poseHistories.end())
            {
                visualizeObjectPose(
                    getLayer(pose.providerName, stage), pose, poseHistoryMap->second, objectFinder);
            }
            else
            {
                visualizeObjectPose(getLayer(pose.providerName, stage), pose, {}, objectFinder);
            }
        }
        return simox::alg::get_values(stage);
    }
 
 
    viz::Layer& Visu::getLayer(
        const std::string& providerName,
        std::map<std::string, viz::Layer>& stage) const
    {
        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);
        }
        return layer;
    }
 
    void Visu::visualizeObjectPose(
        viz::Layer& layer,
        const objpose::ObjectPose& objectPose,
        const std::map<DateTime, objpose::ObjectPose>& poseHistory,
        const ObjectFinder& objectFinder) 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);
 
                    layer.add(robot);
                    done = true;
                }
            }
            if (not done)
            {
                viz::Object object = makeObject(key).pose(pose);
                if (alphaByConfidence && objectPose.confidence < 1.0f)
                {
                    object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
                }
                else if (alpha < 1)
                {
                    object.overrideColor(simox::Color::white().with_alpha(alpha));
                }
                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);
    }
 
}