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/ice_conversions.h>
#include <ArmarXCore/core/time/TimeUtil.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();
    }
 
}