From 38e3ddb02d5a22685b3ff92f4eb55a329fd6c06c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 14 Dec 2021 14:07:43 +0100
Subject: [PATCH] Move main math code to PoseManifoldGaussian and fix it
---
.../ArmarXObjects/PoseManifoldGaussian.cpp | 38 ++++++++++++
.../ArmarXObjects/PoseManifoldGaussian.h | 24 +++++++-
.../libraries/armem_objects/CMakeLists.txt | 2 -
.../RobotAPI/libraries/armem_objects/visu.cpp | 61 -------------------
.../RobotAPI/libraries/armem_objects/visu.h | 30 ---------
5 files changed, 61 insertions(+), 94 deletions(-)
delete mode 100644 source/RobotAPI/libraries/armem_objects/visu.cpp
delete mode 100644 source/RobotAPI/libraries/armem_objects/visu.h
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
index cdc43f287..ff509c2ae 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
@@ -1,7 +1,45 @@
#include "PoseManifoldGaussian.h"
+#include <SimoxUtility/math/pose/pose.h>
+
namespace armarx::objpose
{
+ PoseManifoldGaussian::Ellipsoid
+ PoseManifoldGaussian::getTranslationEllipsoid() const
+ {
+ const float minSize = 0.005;
+
+ const Eigen::Matrix3f covarianceTranslation = this->covariance.block<3, 3>(0, 0);
+
+ const Eigen::EigenSolver<Eigen::Matrix3f> eigen(covarianceTranslation);
+ const Eigen::Vector3f eval = eigen.eigenvalues().real();
+ const Eigen::Matrix3f evec = eigen.eigenvectors().real();
+
+ Ellipsoid result;
+ result.center = simox::math::position(this->mean);
+
+ /* Eigen values are local x, y, and z of ellipsoid:
+ * ( )
+ * ( 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 )
+ */
+ 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));
+
+ return result;
+ }
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
index d32127935..ee508f425 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
@@ -7,14 +7,36 @@ namespace armarx::objpose
{
/**
- * @brief The covariance (matrices) of a pose covariance.
+ * @brief A "gaussian" distribution in pose space (i.e. pose manifold).
+ *
+ * The mean is a specific pose, represented as 4x4 homogeneous matrix.
+ *
+ * The covariance is a 6x6 covariance matrix. The first 3 dimensions
+ * represent the position (translation) part, the second 3 dimensions
+ * represent the orientation (rotation) part.
+ * Note that the lower left and upper right 3x3 parts of the covariance
+ * matrix can be non-zero.
*/
struct PoseManifoldGaussian
{
+ public:
Eigen::Matrix4f mean = Eigen::Matrix4f::Identity();
Eigen::Matrix<float, 6, 6> covariance = Eigen::Matrix<float, 6, 6>::Identity();
+
+ public:
+
+ struct Ellipsoid
+ {
+ Eigen::Vector3f center;
+ Eigen::Matrix3f orientation;
+ Eigen::Vector3f size;
+ };
+
+ Ellipsoid
+ getTranslationEllipsoid() const;
+
};
}
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index bf27ebe8c..88fd8cf80 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -20,7 +20,6 @@ armarx_add_library(
HEADERS
aron_conversions.h
aron_forward_declarations.h
- visu.h
SceneSnapshot.h
@@ -48,7 +47,6 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
- visu.cpp
SceneSnapshot.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/visu.cpp b/source/RobotAPI/libraries/armem_objects/visu.cpp
deleted file mode 100644
index cf9ea6709..000000000
--- a/source/RobotAPI/libraries/armem_objects/visu.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-#include "visu.h"
-
-#include <SimoxUtility/math/pose/pose.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
-
-
-namespace armarx::armem
-{
-
- viz::Ellipsoid
- obj::visPositionCovariance(
- const std::string& name,
- const objpose::PoseManifoldGaussian& poseGaussian
- )
- {
- obj::EllipsoidOrientationAndScale oriAndScale = getEllipsoidOrientationAndScale(poseGaussian);
-
- return viz::Ellipsoid{name}
- .position(simox::math::position(poseGaussian.mean))
- .orientation(oriAndScale.orientation)
- .color(viz::Color(0,200,255,125))
- .axisLengths(scale_factor * scale)
- }
-
- obj::EllipsoidOrientationAndScale
- obj::getEllipsoidOrientationAndScale(
- const armarx::objpose::PoseManifoldGaussian& poseGaussian
- )
- {
- // Get covariance matrix for translation
- auto covarianceTranslation = poseGaussian.covariance.block<3, 3>(0, 0);
-
- Eigen::JacobiSVD<Eigen::Matrix3f> jacobiOfCovariance(
- covarianceTranslation, Eigen::ComputeThinU | Eigen::ComputeThinV);
- // jacobiOfCovariance.singularValues(): 3x1 matrix
- // jacobiOfCovariance.matrixU(): 3x3 matrix
- // jacobiOfCovariance.matrixV(): 3x3 matrix
-
-
- obj::EllipsoidOrientationAndScale result;
-
- // Get singular vectors
- result.orientation = Eigen::Quaterniond::FromTwoVectors(
- jacobiOfCovariance.matrixU().col(0), // Eigenvector 1
- jacobiOfCovariance.matrixU().col(1) // Eigenvector 2
- );
-
- result.scale = jacobiOfCovariance.singularValues();
-
- // Normalize singular values
- result.scale /= result.scale.sum();
- // Handle very small values.
- result.scale = result.scale.cwiseMax(Eigen::Vector3f::Constant(0.005));
-
- return result;
- }
-
-
-}
-
diff --git a/source/RobotAPI/libraries/armem_objects/visu.h b/source/RobotAPI/libraries/armem_objects/visu.h
deleted file mode 100644
index 89d70342e..000000000
--- a/source/RobotAPI/libraries/armem_objects/visu.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-#include <Eigen/Geometry>
-
-#include <RobotAPI/components/ArViz/Client/Elements.h>
-#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
-
-namespace armarx::armem::obj
-{
-
- viz::Ellipsoid
- visPositionCovariance(
- const std::string& name,
- const objpose::PoseManifoldGaussian& poseGaussian
- );
-
-
- struct EllipsoidOrientationAndScale
- {
- Eigen::Quaternionf orientation;
- Eigen::Vector3f scale;
- };
-
- EllipsoidOrientationAndScale
- getEllipsoidOrientationAndScale(
- const objpose::PoseManifoldGaussian& poseGaussian
- );
-}
-
--
GitLab