LinearPredictionsVisu.cpp

Go to the documentation of this file.

#include "LinearPredictionsVisu.h"
 
#include <SimoxUtility/math/pose.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/ice_conversions.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
 
namespace armarx::armem::server::obj::instance::visu
{
 
    void
    LinearPredictions::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(
            showGhost, prefix + "showGhost", "Show ghosts at linearly predicted object poses.");
        defs->optional(ghostAlpha, prefix + "ghostAlpha", "Alpha of linear prediction ghosts.");
        defs->optional(
            showFrame, prefix + "showFrame", "Show frames at linearly predicted object poses.");
        defs->optional(showArrow,
                       prefix + "showArrow",
                       "Show arrows from current object poses to the linearly predicted ones.");
        defs->optional(timeOffsetSeconds,
                       prefix + "timeOffset",
                       "The offset (in seconds) to the current time to make predictions for.");
        defs->optional(timeWindowSeconds,
                       prefix + "timeWindow",
                       "The time window (in seconds) into the past to perform the regression on.");
    }
 
    bool
    LinearPredictions::showAny() const
    {
        return showGhost or showFrame or showArrow;
    }
 
    void
    LinearPredictions::draw(viz::Layer& layer,
                            std::function<viz::Object(const std::string& key)> makeObjectFn,
                            const objpose::ObjectPose& objectPose,
                            const std::map<DateTime, objpose::ObjectPose>& poseHistory,
                            bool inGlobalFrame) const
    {
        const std::string key = objectPose.objectID.str();
        const Eigen::Matrix4f pose =
            inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
 
        auto predictionResult = objpose::predictObjectPoseLinear(
            poseHistory, DateTime::Now() + Duration::SecondsDouble(timeOffsetSeconds), objectPose);
        if (predictionResult.success)
        {
            auto predictedObjectPose =
                armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
            const Eigen::Matrix4f& predictedPose = inGlobalFrame
                                                       ? predictedObjectPose.objectPoseGlobal
                                                       : predictedObjectPose.objectPoseRobot;
 
            if (showGhost)
            {
                layer.add(makeObjectFn(key + " Linear Prediction Ghost")
                              .pose(predictedPose)
                              .overrideColor(simox::Color::white().with_alpha(ghostAlpha)));
            }
            if (showFrame)
            {
                layer.add(viz::Pose(key + " Linear Prediction Pose").pose(predictedPose));
            }
            if (showArrow)
            {
                layer.add(
                    viz::Arrow(key + " Linear Prediction Arrw")
                        .fromTo(simox::math::position(pose), simox::math::position(predictedPose))
                        .width(10)
                        .color(viz::Color::azure()));
            }
        }
        else
        {
            ARMARX_INFO << deactivateSpam(60) << "Linear prediction for visualization failed: "
                        << predictionResult.errorMessage;
        }
    }
 
    void
    LinearPredictions::RemoteGui::setup(const LinearPredictions& data)
    {
        using namespace armarx::RemoteGui::Client;
 
        showGhost.setName("Show Ghost");
        showGhost.setValue(data.showGhost);
        showFrame.setValue(data.showFrame);
        showArrow.setValue(data.showArrow);
 
        ghostAlpha.setRange(0, 1);
        ghostAlpha.setValue(0.5);
 
        timeOffsetSeconds.setValue(data.timeOffsetSeconds);
        timeOffsetSeconds.setRange(-1e6, 1e6);
        timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
 
        timeWindowSeconds.setValue(data.timeWindowSeconds);
        timeWindowSeconds.setRange(0, 1e6);
        timeWindowSeconds.setSteps(2 * 1000 * 1000);
 
        GridLayout grid;
        int row = 0;
 
        HBoxLayout showBoxes({Label("Ghost"),
                              showGhost,
                              Label("    Frame"),
                              showFrame,
                              Label("    Arrow"),
                              showArrow,
                              Label("    Ghost Alpha"),
                              ghostAlpha});
        grid.add(showBoxes, {row, 0}, {1, 2});
        row++;
 
        grid.add(Label("Prediction time (sec from now):"), {row, 0})
            .add(timeOffsetSeconds, {row, 1});
        row++;
 
        grid.add(Label("Model time window (sec before now):"), {row, 0})
            .add(timeWindowSeconds, {row, 1});
        row++;
 
        group = GroupBox();
        group.setLabel("Linear Predictions");
        group.addChild(grid);
    }
 
    void
    LinearPredictions::RemoteGui::update(LinearPredictions& data)
    {
        data.showGhost = showGhost.getValue();
        data.ghostAlpha = ghostAlpha.getValue();
        data.showFrame = showFrame.getValue();
        data.showArrow = showArrow.getValue();
        data.timeOffsetSeconds = timeOffsetSeconds.getValue();
        data.timeWindowSeconds = timeWindowSeconds.getValue();
    }
 
} // namespace armarx::armem::server::obj::instance::visu