Visualize gaussian translation in object memory

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

@@ -24,10 +24,17 @@ namespace armarx::armem::server::obj::instance
                        "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.");
+
+        defs->optional(gaussians, prefix + "gaussians",
+                       "Enable showing pose gaussians.");
+        defs->optional(gaussiansScale, prefix + "gaussiansScale",
+                       "Scaling of pose gaussians.");
+
         defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
                        "Prefer articulated object models if available.");
     }
@@ -164,6 +171,33 @@ namespace armarx::armem::server::obj::instance
         {
             layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
         }
+        if (gaussians and objectPose.objectPoseGlobalGaussian.has_value())
+        {
+            // Translation.
+            auto ellipsoid = objectPose.objectPoseGlobalGaussian->getTranslationEllipsoid();
+
+            layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
+                      .position(ellipsoid.center)
+                      .orientation(ellipsoid.orientation)
+                      .axisLengths(gaussiansScale * ellipsoid.size)
+                      .color(viz::Color::azure(255, 64))
+                      );
+
+            if (false)  // Arrows can be visualized for debugging.
+            {
+                for (int i = 0; i < 3; ++i)
+                {
+                    layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
+                              .fromTo(ellipsoid.center,
+                                      ellipsoid.center + gaussiansScale * ellipsoid.size(i)
+                                      * ellipsoid.orientation.col(i)
+                                      )
+                              .width(5)
+                              .color(viz::Color::orange())
+                              );
+                }
+            }
+        }
     }
 
 
@@ -177,6 +211,7 @@ namespace armarx::armem::server::obj::instance
         alpha.setValue(visu.alpha);
         alphaByConfidence.setValue(visu.alphaByConfidence);
         oobbs.setValue(visu.oobbs);
+
         objectFrames.setValue(visu.objectFrames);
         {
             float max = 10000;
@@ -185,6 +220,16 @@ namespace armarx::armem::server::obj::instance
             objectFramesScale.setSteps(int(10 * max));
             objectFramesScale.setValue(visu.objectFramesScale);
         }
+
+        gaussians.setValue(visu.gaussians);
+        {
+            float max = 10000;
+            gaussiansScale.setRange(0, max);
+            gaussiansScale.setDecimals(2);
+            gaussiansScale.setSteps(int(10 * max));
+            gaussiansScale.setValue(visu.gaussiansScale);
+        }
+
         useArticulatedModels.setValue(visu.useArticulatedModels);
 
         GridLayout grid;
@@ -199,9 +244,15 @@ namespace armarx::armem::server::obj::instance
         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++;
+
+        grid.add(Label("Gaussians"), {row, 0}).add(gaussians, {row, 1});
+        grid.add(Label("Scale:"), {row, 2}).add(gaussiansScale, {row, 3});
+        row++;
+
         grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
         row++;
 
@@ -216,8 +267,13 @@ namespace armarx::armem::server::obj::instance
         visu.alpha = alpha.getValue();
         visu.alphaByConfidence = alphaByConfidence.getValue();
         visu.oobbs = oobbs.getValue();
+
         visu.objectFrames = objectFrames.getValue();
         visu.objectFramesScale = objectFramesScale.getValue();
+
+        visu.gaussians = gaussians.getValue();
+        visu.gaussiansScale = gaussiansScale.getValue();
+
         visu.useArticulatedModels = useArticulatedModels.getValue();
     }
 

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

@@ -73,9 +73,13 @@ namespace armarx::armem::server::obj::instance
         float alpha = 1.0;
         bool alphaByConfidence = false;
         bool oobbs = false;
+
         bool objectFrames = false;
         float objectFramesScale = 1.0;
 
+        bool gaussians = false;
+        float gaussiansScale = 200.0;
+
         /// Prefer articulated models if available.
         bool useArticulatedModels = true;
 
@@ -92,9 +96,13 @@ namespace armarx::armem::server::obj::instance
             armarx::RemoteGui::Client::FloatSlider alpha;
             armarx::RemoteGui::Client::CheckBox alphaByConfidence;
             armarx::RemoteGui::Client::CheckBox oobbs;
+
             armarx::RemoteGui::Client::CheckBox objectFrames;
             armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
 
+            armarx::RemoteGui::Client::CheckBox gaussians;
+            armarx::RemoteGui::Client::FloatSpinBox gaussiansScale;
+
             armarx::RemoteGui::Client::CheckBox useArticulatedModels;