diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index cbc7babade827f66d0ef0681ef9541c67738deed..547a7126ea6287fe58e49b8e306d1a743df92e61 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -170,9 +170,10 @@ namespace armarx
pose.objectPoseGaussian->mean = pose.objectPose;
pose.objectPoseGaussian->covariance = Eigen::Matrix6f::Identity();
// Translational (co)variance.
- pose.objectPoseGaussian->covariance.diagonal()(0) = 1.0 + 1.0 * std::sin(t - i);
- pose.objectPoseGaussian->covariance.diagonal()(1) = 3.0 + 3.0 * std::sin(t - i);
- pose.objectPoseGaussian->covariance.diagonal()(2) = 2.0 + 2.0 * std::sin(t - i);
+ const float posVar = 10 + 10 * std::sin(t - i);
+ pose.objectPoseGaussian->covariance.diagonal()(0) = 1.0 * posVar;
+ pose.objectPoseGaussian->covariance.diagonal()(1) = 3.0 * posVar;
+ pose.objectPoseGaussian->covariance.diagonal()(2) = 2.0 * posVar;
if (i % 2 == 1)
{
Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
@@ -181,9 +182,10 @@ namespace armarx
}
// Rotational (co)variance.
- pose.objectPoseGaussian->covariance.diagonal()(3) = 1.0 + 1.0 * std::sin(t - i);
- pose.objectPoseGaussian->covariance.diagonal()(4) = 2.0 + 2.0 * std::sin(t - i);
- pose.objectPoseGaussian->covariance.diagonal()(5) = 3.0 + 3.0 * std::sin(t - i);
+ const float oriVar = (M_PI_4) + (M_PI_4) * std::sin(t - i);
+ pose.objectPoseGaussian->covariance.diagonal()(3) = 1.0 * oriVar;
+ pose.objectPoseGaussian->covariance.diagonal()(4) = 4.0 * oriVar;
+ pose.objectPoseGaussian->covariance.diagonal()(5) = 2.0 * oriVar;
pose.confidence = 0.75 + 0.25 * std::sin(t - i);
pose.timestamp = TimeUtil::GetTime();
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
index ff509c2aec8792df2fb4614fb2b0c8d0f0410575..0bd3a723ee0b460ebbf6c399fb5a9ee6cc037132 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
@@ -25,17 +25,15 @@ namespace armarx::objpose
* ( x y z ) = u
* ( )
*
- * Singular vectors are sorted by the singular values:
- * ( s_x )
- * ( s_y ) = sv, (s_x >= s_y >= s_z)
- * ( s_z )
+ * Eigen vectors are sorted by the singular values:
+ * ( e_x )
+ * ( e_y ) = ev, (e_x >= e_y >= e_z)
+ * ( e_z )
*/
result.orientation = evec.determinant() > 0 ? evec : - evec;
result.size = eval;
- // Normalize singular values
- // result.size /= result.size.sum();
// Handle very small values.
result.size = result.size.cwiseMax(Eigen::Vector3f::Constant(minSize));
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index a08a726ffbca6bac929ac6a1ceb00ad16eebcf7e..18a46aa2058eb5f033c277599f0059b062b9beb0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -1,6 +1,7 @@
#include "Visu.h"
#include <SimoxUtility/math/pose.h>
+#include <SimoxUtility/math/rescale.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -30,10 +31,17 @@ namespace armarx::armem::server::obj::instance
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(gaussiansPosition, prefix + "gaussians.position",
+ "Enable showing pose gaussians (position part).");
+ defs->optional(gaussiansPositionScale, prefix + "gaussians.positionScale",
+ "Scaling of pose gaussians (position part).");
+
+ defs->optional(gaussiansOrientation, prefix + "gaussians.position",
+ "Enable showing pose gaussians (orientation part).");
+ defs->optional(gaussiansOrientationScale, prefix + "gaussians.positionScale",
+ "Scaling of pose gaussians (orientation part).");
+ defs->optional(gaussiansOrientationDisplaced, prefix + "gaussians.positionDisplaced",
+ "Displace center orientation (co)variance circle arrows along their rotation axis.");
defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
"Prefer articulated object models if available.");
@@ -171,29 +179,62 @@ namespace armarx::armem::server::obj::instance
{
layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
}
- if (gaussians and objectPose.objectPoseGlobalGaussian.has_value())
+ if (objectPose.objectPoseGlobalGaussian.has_value() and (gaussiansPosition or gaussiansOrientation))
{
- // 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))
- );
+ const objpose::PoseManifoldGaussian& gaussian = objectPose.objectPoseGlobalGaussian.value();
+ objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getTranslationEllipsoid();
- if (false) // Arrows can be visualized for debugging.
+ if (gaussiansPosition)
+ {
+ layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
+ .position(ellipsoid.center)
+ .orientation(ellipsoid.orientation)
+ .axisLengths(gaussiansPositionScale * ellipsoid.size)
+ .color(viz::Color::azure(255, 32))
+ );
+
+ if (false) // Arrows can be visualized for debugging.
+ {
+ for (int i = 0; i < 3; ++i)
+ {
+ Eigen::Vector3i color = Eigen::Vector3i::Zero();
+ color(i) = 255;
+
+ layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
+ .fromTo(ellipsoid.center,
+ ellipsoid.center + gaussiansPositionScale * ellipsoid.size(i)
+ * ellipsoid.orientation.col(i)
+ )
+ .width(5)
+ .color(simox::Color(color))
+ );
+ }
+ }
+ }
+ if (gaussiansOrientation)
{
+ const float pi = static_cast<float>(M_PI);
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())
+ const Eigen::Vector3f scaledRotationAxis = gaussian.covariance.block<3, 1>(3, 3+i);
+ const float angle = scaledRotationAxis.norm();
+ const Eigen::Vector3f axis =
+ simox::math::orientation(gaussian.mean)
+ * (scaledRotationAxis / angle);
+
+ Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ color(i) = 255;
+ color(3) = 64;
+
+ layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
+ .position(gaussiansOrientationDisplaced
+ ? ellipsoid.center + gaussiansOrientationScale * axis
+ : ellipsoid.center)
+ .radius(gaussiansOrientationScale)
+ .normal(axis)
+ .completion(simox::math::rescale(angle, 0.f, pi * 2, 0.f, 1.f))
+ .width(simox::math::rescale(angle, 0.f, pi * 2, 2.f, 7.f))
+ .color(simox::Color(color))
);
}
}
@@ -212,23 +253,23 @@ namespace armarx::armem::server::obj::instance
alphaByConfidence.setValue(visu.alphaByConfidence);
oobbs.setValue(visu.oobbs);
- objectFrames.setValue(visu.objectFrames);
+ auto initScale = [](FloatSpinBox& scale, float value, float stepResolution)
{
float max = 10000;
- objectFramesScale.setRange(0, max);
- objectFramesScale.setDecimals(2);
- objectFramesScale.setSteps(int(10 * max));
- objectFramesScale.setValue(visu.objectFramesScale);
- }
+ scale.setRange(0, max);
+ scale.setDecimals(2);
+ scale.setSteps(int(stepResolution * max));
+ scale.setValue(value);
+ };
+ objectFrames.setValue(visu.objectFrames);
+ initScale(objectFramesScale, visu.objectFramesScale, 10);
- gaussians.setValue(visu.gaussians);
- {
- float max = 10000;
- gaussiansScale.setRange(0, max);
- gaussiansScale.setDecimals(2);
- gaussiansScale.setSteps(int(10 * max));
- gaussiansScale.setValue(visu.gaussiansScale);
- }
+ gaussians.position.setValue(visu.gaussiansPosition);
+ initScale(gaussians.positionScale, visu.gaussiansPositionScale, 10);
+
+ gaussians.orientation.setValue(visu.gaussiansOrientation);
+ initScale(gaussians.orientationScale, visu.gaussiansOrientationScale, 0.5);
+ gaussians.orientationDisplaced.setValue(visu.gaussiansOrientationDisplaced);
useArticulatedModels.setValue(visu.useArticulatedModels);
@@ -249,17 +290,42 @@ namespace armarx::armem::server::obj::instance
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});
+ gaussians.setup(visu);
+ grid.add(gaussians.group, {row, 0}, {1, 4});
row++;
+
grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
row++;
+ group = GroupBox();
group.setLabel("Visualization");
group.addChild(grid);
}
+
+ void Visu::RemoteGui::Gaussians::setup(const Visu& data)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ GridLayout grid;
+ int row = 0;
+
+ grid.add(Label("Position"), {row, 0}).add(position, {row, 1});
+ grid.add(Label("Scale:"), {row, 2}).add(positionScale, {row, 3});
+ row++;
+
+ grid.add(Label("Orientation"), {row, 0}).add(orientation, {row, 1});
+ grid.add(Label("Scale:"), {row, 2}).add(orientationScale, {row, 3});
+ grid.add(Label("Displaced"), {row, 4}).add(orientationDisplaced, {row, 5});
+ row++;
+
+ group = GroupBox();
+ group.setLabel("Pose Gaussians");
+ group.addChild(grid);
+ }
+
+
void Visu::RemoteGui::update(Visu& visu)
{
visu.enabled = enabled.getValue();
@@ -271,8 +337,12 @@ namespace armarx::armem::server::obj::instance
visu.objectFrames = objectFrames.getValue();
visu.objectFramesScale = objectFramesScale.getValue();
- visu.gaussians = gaussians.getValue();
- visu.gaussiansScale = gaussiansScale.getValue();
+ visu.gaussiansPosition = gaussians.position.getValue();
+ visu.gaussiansPositionScale = gaussians.positionScale.getValue();
+
+ visu.gaussiansOrientation = gaussians.orientation.getValue();
+ visu.gaussiansOrientationScale = gaussians.orientationScale.getValue();
+ visu.gaussiansOrientationDisplaced = gaussians.orientationDisplaced.getValue();
visu.useArticulatedModels = useArticulatedModels.getValue();
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 086d560e1346827f5ac45633b3d83def752d8400..01c584d6f66cd10b75bf07f00925890f8c598d09 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -77,8 +77,11 @@ namespace armarx::armem::server::obj::instance
bool objectFrames = false;
float objectFramesScale = 1.0;
- bool gaussians = false;
- float gaussiansScale = 200.0;
+ bool gaussiansPosition = true;
+ float gaussiansPositionScale = 1.0;
+ bool gaussiansOrientation = false;
+ float gaussiansOrientationScale = 100;
+ bool gaussiansOrientationDisplaced = true;
/// Prefer articulated models if available.
bool useArticulatedModels = true;
@@ -100,8 +103,20 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::CheckBox objectFrames;
armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
- armarx::RemoteGui::Client::CheckBox gaussians;
- armarx::RemoteGui::Client::FloatSpinBox gaussiansScale;
+ struct Gaussians
+ {
+ armarx::RemoteGui::Client::CheckBox position;
+ armarx::RemoteGui::Client::FloatSpinBox positionScale;
+
+ armarx::RemoteGui::Client::CheckBox orientation;
+ armarx::RemoteGui::Client::FloatSpinBox orientationScale;
+ armarx::RemoteGui::Client::CheckBox orientationDisplaced;
+
+ armarx::RemoteGui::Client::GroupBox group;
+
+ void setup(const Visu& data);
+ };
+ Gaussians gaussians;
armarx::RemoteGui::Client::CheckBox useArticulatedModels;