Improve depth image visualization by calibrating the colormap

source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp

@@ -16,6 +16,7 @@
 
 #include <SimoxUtility/algorithm/string.h>
 #include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/SoftMinMax.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
@@ -278,6 +279,26 @@ namespace armarx::armem::gui::instance
                     {
                         this->showImageView(path.value());
                     });
+
+                    try
+                    {
+                        aron::datanavigator::NavigatorPtr element = currentInstance->data()->navigateAbsolute(path.value());
+                        if (auto imageData = aron::datanavigator::NDArrayNavigator::DynamicCast(element))
+                        {
+                            const std::vector<int> shape = imageData->getDimensions();
+                            if (std::find(shape.begin(), shape.end(), 0) != shape.end())
+                            {
+                                viewAction->setText(viewAction->text() + " (image is empty)");
+                                viewAction->setEnabled(false);
+                            }
+                        }
+                    }
+                    catch (const aron::error::AronException&)
+                    {
+                    }
+                    catch (const armarx::LocalException&)
+                    {
+                    }
                 }
             }
             break;
@@ -427,8 +448,9 @@ namespace armarx::armem::gui::instance
     }
 
 
-    static
-    QImage convertDepth32ToRGB32(const aron::datanavigator::NDArrayNavigator& aron)
+
+    QImage InstanceView::ImageView::convertDepth32ToRGB32(
+            const aron::datanavigator::NDArrayNavigator& aron)
     {
         const std::vector<int> shape = aron.getDimensions();
         ARMARX_CHECK_EQUAL(shape.size(), 3);
@@ -442,11 +464,18 @@ namespace armarx::armem::gui::instance
         QImage image(cols, rows, QImage::Format::Format_RGB32);
         const float* data = reinterpret_cast<float*>(aron.getData());
 
-        // Find data range and adapt cmap.
-        simox::ColorMap cmap = simox::color::cmaps::plasma();
-        float min = data[0];
-        float max = data[0];
+        auto updateLimits = [](float value, Limits& limits)
+        {
+            if (value > 0)  // Exclude 0 from normalization (it may be only background)
+            {
+                limits.min = std::min(limits.min, value);
+            }
+            limits.max = std::max(limits.max, value);
+        };
 
+        // Find data range and adapt cmap.
+        Limits limits;
+        if (limitsHistory.empty())
         {
             const float* sourceRow = data;
             for (int row = 0; row < rows; ++row)
@@ -454,13 +483,28 @@ namespace armarx::armem::gui::instance
                 for (int col = 0; col < cols; ++col)
                 {
                     float value = sourceRow[col];
-                    min = std::min(min, value);
-                    max = std::max(max, value);
+                    updateLimits(value, limits);
                 }
                 sourceRow += cols;
             }
-            cmap.set_vlimits(min, max);
+            cmap.set_vlimits(limits.min, limits.max);
         }
+        // Only do it at the beginning and stop after enough samples were collected.
+        else if (limitsHistory.size() < limitsHistoryMaxSize)
+        {
+            simox::math::SoftMinMax softMin(0.25, limitsHistory.size());
+            simox::math::SoftMinMax softMax(0.25, limitsHistory.size());
+
+            for (auto& l : limitsHistory)
+            {
+                softMin.add(l.min);
+                softMax.add(l.max);
+            }
+
+            cmap.set_vlimits(softMin.getSoftMin(), softMax.getSoftMax());
+        }
+
+        // Update image
         {
             const float* sourceRow = data;
 
@@ -472,16 +516,24 @@ namespace armarx::armem::gui::instance
                 for (int col = 0; col < cols; ++col)
                 {
                     float value = sourceRow[col];
-                    simox::Color color = cmap(value);
+                    simox::Color color = value <= 0
+                            ? simox::Color::white()
+                            : cmap(value);
                     targetRow[col*4 + 0] = color.b;
                     targetRow[col*4 + 1] = color.g;
                     targetRow[col*4 + 2] = color.r;
                     targetRow[col*4 + 3] = color.a;
+
+                    updateLimits(value, limits);
                 }
                 sourceRow += cols;
                 targetRow += bytesPerLine;
             }
         }
+        if (limitsHistory.size() < limitsHistoryMaxSize)
+        {
+            limitsHistory.push_back(limits);
+        }
 
         return image;
     }
@@ -548,6 +600,7 @@ namespace armarx::armem::gui::instance
         {
         }
 
+        bool clearLimitsHistory = true;
         std::optional<QImage> image;
         if (pixelType)
         {
@@ -559,7 +612,8 @@ namespace armarx::armem::gui::instance
                     break;
 
                 case ImagePixelType::Depth32:
-                    image = convertDepth32ToRGB32(*imageData);
+                    image = imageView->convertDepth32ToRGB32(*imageData);
+                    clearLimitsHistory = false;
                     break;
             }
         }
@@ -590,10 +644,17 @@ namespace armarx::armem::gui::instance
         title << "Image element '" << imageView->elementPath.toString() << "'";  // of entity instance " << currentInstance->id();
         imageView->setTitle(QString::fromStdString(title.str()));
         imageView->view->setImage(image.value());
+
+        if (clearLimitsHistory)
+        {
+            imageView->limitsHistory.clear();
+        }
     }
 
 
-    InstanceView::ImageView::ImageView()
+    InstanceView::ImageView::ImageView() :
+        cmap(simox::color::cmaps::plasma().reversed()),
+        limitsHistoryMaxSize(32)
     {
         setLayout(new QHBoxLayout());
         int margin = 2;

source/RobotAPI/libraries/armem_gui/instance/InstanceView.h

 #pragma once
 
 #include <optional>
+#include <deque>
 
 #include <QWidget>
 #include <QGroupBox>
 
+#include <SimoxUtility/color/ColorMap.h>
+
 #include <ArmarXCore/core/logging/Logging.h>
 
 #include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
@@ -104,10 +107,26 @@ namespace armarx::armem::gui::instance
         public:
             ImageView();
 
+            QImage convertDepth32ToRGB32(const aron::datanavigator::NDArrayNavigator& aron);
+
             instance::ImageView* view;
             aron::Path elementPath;
 
             WidgetsWithToolbar* toolbar;
+
+
+            struct Limits
+            {
+                float min = std::numeric_limits<float>::max();
+                float max = -std::numeric_limits<float>::max();
+            };
+
+            /// Color map to visualize depth images.
+            simox::ColorMap cmap;
+            /// History over first n extremal depth values used to calibrate the colormap.
+            std::deque<Limits> limitsHistory;
+            /// In this context, n.
+            const size_t limitsHistoryMaxSize;
         };
         ImageView* imageView = nullptr;