Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
 
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/aron/core/aron_conversions.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
 
namespace armarx::armem::server::obj::familiar_object_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(sizePixel, prefix + "sizePixel", "Pixel size of point cloud.");
 
        defs->optional(visualizePointCloud, prefix + "visualizePointCloud", "");
        defs->optional(visualizePose, prefix + "visualizePose", "");
        defs->optional(visualizeBoundingBox, prefix + "visualizeBoundingBox", "");
        defs->optional(visualizeLabels, prefix + "visualizeLabels", "");
 
        defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
        defs->optional(
            objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
        defs->optional(visualizationDuration,
                       prefix + "maxAgeSeconds",
                       "Maximum age in seconds for visualization.");
    }
 
    void
    Visu::visualizeFamiliarObjectsByProvider(
        const std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>&
            familiarObjectsByProvider)
    {
        const auto timestamp = armarx::Clock::Now();
 
        const auto isWithinTimeFrame =
            [&timestamp,
             this](const armarx::armem::arondto::FamiliarObjectInstance& instance) -> bool
        {
            const auto dt = timestamp - instance.timestamp;
            ARMARX_DEBUG << VAROUT(dt);
 
            return dt.toSecondsDouble() < visualizationDuration;
        };
 
        std::vector<viz::Layer> layers;
 
        for (const auto& [providerName, familiarObjects] : familiarObjectsByProvider)
        {
            auto layerPose = arviz.layer("familiar_objects/pose/" + providerName);
            auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
            auto layerLabels = arviz.layer("familiar_objects/labels/" + providerName);
            auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
 
            auto confidenceCmap = simox::color::cmaps::viridis();
            confidenceCmap.set_vlimits(0, 1);
 
            for (const auto& familiarObject : familiarObjects)
            {
                if (not isWithinTimeFrame(familiarObject))
                {
                    continue;
                }
 
                armarx::ObjectID objectId;
                fromAron(familiarObject.objectID, objectId);
 
                if (familiarObject.poseGlobal.has_value())
                {
                    if (visualizePose)
                    {
                        auto pose = viz::Pose(objectId.str()).pose(familiarObject.poseGlobal->pose);
                        layerPose.add(pose);
                    }
 
                    const int alpha = static_cast<int>(255 * this->alpha);
 
                    if (visualizePointCloud and not familiarObject.points.empty())
                    {
                        auto points = viz::PointCloud(objectId.str())
                                          .pointCloud(familiarObject.points)
                                          .pose(familiarObject.poseGlobal->pose);
 
                        points.pointSizeInPixels(sizePixel);
                        layerPointCloud.add(points);
                    }
 
                    if (visualizeLabels)
                    {
                        Eigen::Isometry3f pose{familiarObject.poseGlobal->pose};
 
                        // gently above object
                        // FIXME derive from bounding box
                        pose.translation().z() += 200;
 
                        const std::string name = familiarObject.objectID.className + "/" +
                                                 familiarObject.objectID.instanceName;
 
                        layerLabels.add(viz::Text(name).pose(pose).scale(20));
                    }
 
                    if (visualizeBoundingBox)
                    {
 
                        const Eigen::Isometry3f global_T_obj{familiarObject.poseGlobal->pose};
                        Eigen::Isometry3f obj_T_bb = Eigen::Isometry3f::Identity();
                        obj_T_bb.translation() = familiarObject.bounding_box.center;
 
                        if (familiarObject.boundingBoxOrientation.has_value())
                        {
                            obj_T_bb.linear() =
                                familiarObject.boundingBoxOrientation->toRotationMatrix();
                        }
 
                        const Eigen::Isometry3f global_T_bb = global_T_obj * obj_T_bb;
 
                        auto box = viz::Box(objectId.str());
                        box.pose(global_T_bb.matrix());
                        box.size(familiarObject.bounding_box.extents);
 
                        auto color = confidenceCmap.at(familiarObject.confidence);
                        color.a = alpha;
 
                        box.color(color);
                        layerBox.add(box);
                    }
                }
            }
 
            layers.push_back(layerPose);
            layers.push_back(layerPointCloud);
            layers.push_back(layerLabels);
            layers.push_back(layerBox);
        }
 
        arviz.commit(layers);
    }
 
 
} // namespace armarx::armem::server::obj::familiar_object_instance