Add full object ghost prediction visu

source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp

@@ -50,7 +50,7 @@ namespace armarx::armem::server::obj::instance
         defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
                        "Prefer articulated object models if available.");
 
-        defs->optional(showLinearPredictions, prefix + "predictions.linear.show",
+        defs->optional(showLinPredArrow, prefix + "predictions.linear.show",
                        "Show arrows linearly predicting object positions.");
         defs->optional(linearPredictionTimeOffsetSeconds, prefix + "predictions.linear.timeOffset",
                        "The offset (in seconds) to the current time to make predictions for.");
@@ -168,9 +168,9 @@ namespace armarx::armem::server::obj::instance
         const std::string key = id.str();
 
         const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+        std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+        std::optional<viz::Object> objectCache;
         {
-            std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
-
             bool done = false;
             if (useArticulatedModels && objectInfo)
             {
@@ -187,25 +187,25 @@ namespace armarx::armem::server::obj::instance
             }
             if (!done)
             {
-                viz::Object object(key);
-                object.pose(pose);
+                objectCache.emplace(key);
+                objectCache->pose(pose);
                 if (objectInfo)
                 {
-                    object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+                    objectCache->file(objectInfo->package(), objectInfo->simoxXML().relativePath);
                 }
                 else
                 {
-                    object.fileByObjectFinder(id);
+                    objectCache->fileByObjectFinder(id);
                 }
                 if (alphaByConfidence && objectPose.confidence < 1.0f)
                 {
-                    object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
+                    objectCache->overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
                 }
                 else if (alpha < 1)
                 {
-                    object.overrideColor(simox::Color::white().with_alpha(alpha));
+                    objectCache->overrideColor(simox::Color::white().with_alpha(alpha));
                 }
-                layer.add(object);
+                layer.add(objectCache.value());
             }
         }
 
@@ -277,7 +277,7 @@ namespace armarx::armem::server::obj::instance
                 }
             }
         }
-        if (showLinearPredictions)
+        if (objectCache && (showLinPredGhost || showLinPredArrow))
         {
             auto predictionResult = objpose::predictObjectPoseLinear(
                 poseHistory,
@@ -287,11 +287,23 @@ namespace armarx::armem::server::obj::instance
             {
                 auto predictedPose =
                     armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
-                Eigen::Vector3f predictedPosition = simox::math::position(
-                    inGlobalFrame ? predictedPose.objectPoseGlobal : predictedPose.objectPoseRobot);
-                layer.add(viz::Arrow(key + " Linear Prediction")
-                              .fromTo(simox::math::position(pose), predictedPosition)
-                              .color(viz::Color::blue()));
+                auto pose =
+                    inGlobalFrame ? predictedPose.objectPoseGlobal : predictedPose.objectPoseRobot;
+                if (showLinPredGhost)
+                {
+                    // Copy for safety - we don't want to accidentally modify the actual object
+                    viz::Object prediction(objectCache.value());
+                    prediction.pose(pose);
+                    prediction.overrideColor(simox::Color::white().with_alpha(linPredGhostAlpha));
+                    layer.add(prediction);
+                }
+                if (showLinPredArrow)
+                {
+                    Eigen::Vector3f predictedPosition = simox::math::position(pose);
+                    layer.add(viz::Arrow(key + " Linear Prediction")
+                                  .fromTo(simox::math::position(pose), predictedPosition)
+                                  .color(viz::Color::blue()));
+                }
             }
             else
             {
@@ -418,7 +430,7 @@ namespace armarx::armem::server::obj::instance
     {
         using namespace armarx::RemoteGui::Client;
 
-        show.setValue(visu.showLinearPredictions);
+        showPositionArrow.setValue(visu.showLinPredArrow);
         timeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
         timeOffsetSeconds.setRange(-1e6, 1e6);
         timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
@@ -431,7 +443,10 @@ namespace armarx::armem::server::obj::instance
         GridLayout grid;
         int row = 0;
 
-        grid.add(Label("Show"), {row, 0}).add(show, {row, 1});
+        grid.add(Label("Show Ghost"), {row, 0}).add(showGhost, {row, 1})
+            .add(Label("Alpha"), {row, 2}).add(ghostAlpha, {row, 3});
+        row++;
+        grid.add(Label("Show Position Arrow"), {row, 0}).add(showPositionArrow, {row, 1});
         row++;
         grid.add(Label("Time (seconds from now):"), {row, 0})
             .add(timeOffsetSeconds, {row, 1});
@@ -448,7 +463,9 @@ namespace armarx::armem::server::obj::instance
 
     void Visu::RemoteGui::LinearPredictions::update(Visu& visu)
     {
-        visu.showLinearPredictions = show.getValue();
+        visu.showLinPredGhost = showGhost.getValue();
+        visu.linPredGhostAlpha = ghostAlpha.getValue();
+        visu.showLinPredArrow = showPositionArrow.getValue();
         visu.linearPredictionTimeOffsetSeconds = timeOffsetSeconds.getValue();
         visu.linearPredictionTimeWindowSeconds = timeWindowSeconds.getValue();
     }

source/RobotAPI/libraries/armem_objects/server/instance/Visu.h

@@ -92,8 +92,10 @@ namespace armarx::armem::server::obj::instance
         /// Prefer articulated models if available.
         bool useArticulatedModels = true;
 
-        /// Linear prediction arrows for object positions.
-        bool showLinearPredictions = false;
+        /// Linear predictions for objects.
+        bool showLinPredGhost = false;
+        float linPredGhostAlpha = 0.7;
+        bool showLinPredArrow = false;
         float linearPredictionTimeOffsetSeconds = 1;
         float linearPredictionTimeWindowSeconds = 2;
 
@@ -133,7 +135,10 @@ namespace armarx::armem::server::obj::instance
 
             struct LinearPredictions
             {
-                armarx::RemoteGui::Client::CheckBox show;
+                armarx::RemoteGui::Client::CheckBox showGhost;
+                armarx::RemoteGui::Client::FloatSpinBox ghostAlpha;
+
+                armarx::RemoteGui::Client::CheckBox showPositionArrow;
 
                 armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
                 armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;