Show eigen matrix data in Instance View

source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp

@@ -4,6 +4,8 @@
 
 #include <SimoxUtility/algorithm/string.h>
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 #include <RobotAPI/libraries/aron/core/Exception.h>
 #include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
@@ -22,6 +24,14 @@ namespace armarx::aron
         return v.value.str();
     }
 
+
+    TypedDataDisplayVisitor::TypedDataDisplayVisitor() :
+        coeffSep("  "),
+        eigenIof(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", "")
+    {
+    }
+
+
     bool TypedDataDisplayVisitor::visitEnter(DictTypeNavigator&, DictDataNavigator& data)
     {
         value << DataDisplayVisitor::getValue(data);
@@ -54,12 +64,14 @@ namespace armarx::aron
 
     bool TypedDataDisplayVisitor::visit(DoubleTypeNavigator&, DoubleDataNavigator& data)
     {
+        setStreamPrecision();
         value << DataDisplayVisitor::getValue(data);
         return false;
     }
 
     bool TypedDataDisplayVisitor::visit(FloatTypeNavigator&, FloatDataNavigator& data)
     {
+        setStreamPrecision();
         value << DataDisplayVisitor::getValue(data);
         return false;
     }
@@ -85,20 +97,45 @@ namespace armarx::aron
     bool TypedDataDisplayVisitor::visit(TimeTypeNavigator&, LongDataNavigator& data)
     {
         armem::Time time = armem::Time::microSeconds(data.getValue());
-        armem::toDateTimeMilliSeconds(time);
         value << armem::toDateTimeMilliSeconds(time);
         return false;
     }
 
-    bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data)
+
+    template <typename ScalarT>
+    void TypedDataDisplayVisitor::processEigenMatrix(EigenMatrixTypeNavigator& type, NDArrayDataNavigator& data)
     {
-        value << DataDisplayVisitor::getValue(data);
+        Eigen::Map<Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic>> m(reinterpret_cast<ScalarT*>(data.getData()), type.getRows(), type.getCols());
+        value << m.format(eigenIof);
+    }
+
+    bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator& t, NDArrayDataNavigator& data)
+    {
+        if (std::max(t.getRows(), t.getCols()) > 10)
+        {
+            // Just show the shape.
+            value << DataDisplayVisitor::getValue(data);
+        }
+        else if (data.getType() == "float")
+        {
+            setStreamPrecision();
+            processEigenMatrix<float>(t, data);
+        }
+        else if (data.getType() == "double")
+        {
+            setStreamPrecision();
+            processEigenMatrix<double>(t, data);
+        }
+        else
+        {
+            value << DataDisplayVisitor::getValue(data);
+        }
         return false;
     }
 
     bool TypedDataDisplayVisitor::visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data)
     {
-        value << DataDisplayVisitor::getValue(data);
+        processEigenQuaternion(data);
         return false;
     }
 
@@ -123,25 +160,35 @@ namespace armarx::aron
     bool TypedDataDisplayVisitor::visit(PoseTypeNavigator&, NDArrayDataNavigator& data)
     {
         const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
-        value << std::setprecision(2) << std::fixed;
-        value << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+        setStreamPrecision();
+        value << pose.format(eigenIof);
         return false;
     }
 
     bool TypedDataDisplayVisitor::visit(PositionTypeNavigator&, NDArrayDataNavigator& data)
     {
         const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
-        value << std::setprecision(2) << std::fixed;
-        value << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+        setStreamPrecision();
+        value << pos.format(eigenIof);
         return false;
     }
 
     bool TypedDataDisplayVisitor::visit(OrientationTypeNavigator&, NDArrayDataNavigator& data)
+    {
+        processEigenQuaternion(data);
+        return false;
+    }
+
+    void TypedDataDisplayVisitor::processEigenQuaternion(visitor::TypedDataVisitor::NDArrayDataNavigator& data)
     {
         const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
-        value << std::setprecision(2) << std::fixed;
+        setStreamPrecision();
         value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
-        return false;
+    }
+
+    void TypedDataDisplayVisitor::setStreamPrecision()
+    {
+        value << std::setprecision(2) << std::fixed;
     }
 
 }

source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h

@@ -3,6 +3,8 @@
 #include <sstream>
 #include <string>
 
+#include <Eigen/Core>
+
 #include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
 
 
@@ -18,6 +20,9 @@ namespace armarx::aron
 
     public:
 
+        TypedDataDisplayVisitor();
+
+
         std::stringstream value;
 
 
@@ -49,7 +54,17 @@ namespace armarx::aron
 
     protected:
 
-        std::string coeffSep = "  ";
+        std::string coeffSep;
+        Eigen::IOFormat eigenIof;
+
+
+    private:
+
+        template <typename ScalarT>
+        void processEigenMatrix(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data);
+        void processEigenQuaternion(NDArrayDataNavigator& data);
+
+        void setStreamPrecision();
 
     };