more vector conversion methods

source/RobotAPI/libraries/aron/converter/common/VectorConverter.h

@@ -37,37 +37,93 @@ namespace armarx::aron::converter
 {
     class AronVectorConverter
     {
+    private:
+        template<typename T>
+        static std::vector<T> convert_to_1d_vector(const unsigned char* data, const int elements, const int offset, const int size)
+        {
+            ARMARX_CHECK_NOT_NULL(data);
+
+            std::vector<T> v(elements);
+            memcpy(v.data(), data, size);
+
+            return v;
+        }
+
     public:
         AronVectorConverter() = delete;
 
         template<typename T>
-        static std::vector<T> ConvertToVector(const data::NDArrayPtr& nav)
+        static std::vector<std::vector<T>> ConvertTo2DVector(const data::NDArray& nav)
         {
-            ARMARX_CHECK_NOT_NULL(nav);
+            const auto& dims = nav.getShape();
 
-            const auto& dims = nav->getShape();
+            if (dims.size() != 3)
+            {
+                throw error::AronException(__PRETTY_FUNCTION__, "The NDArray must have 3 dimensions.", nav.getPath());
+            }
 
-            if (dims.size() != 2)
+            if (dims.at(dims.size()-1) != sizeof(T))
             {
-                throw error::AronException(__PRETTY_FUNCTION__, "The NDArray must have two dimensions.", nav->getPath());
+                throw error::AronException(__PRETTY_FUNCTION__, "Last dimension of the array has to match the element size.", nav.getPath());
             }
 
-            if (dims.at(1) != sizeof(T))
+            const int one_row_size = dims.at(1) * dims.at(2);
+            std::vector<std::vector<T>> v(dims.at(0));
+            for (int i = 0; i < dims.at(0); i++)
             {
-                throw error::AronException(__PRETTY_FUNCTION__, "Dimension 1 of the array has to match the element size.", nav->getPath());
+                v[i] = convert_to_1d_vector<T>(nav.getData(), dims.at(1), i * one_row_size, one_row_size);
             }
 
-            const int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>());
+            return v;
+        }
+
+        template<typename T>
+        static std::vector<std::vector<T>> ConvertTo2DVector(const data::NDArrayPtr& nav)
+        {
+            ARMARX_CHECK_NOT_NULL(nav);
+
+            return ConvertTo2DVector<T>(*nav);
+        }
+
+        template<typename T>
+        static std::vector<T> ConvertTo1DVector(const data::NDArray& nav, const bool allowFlatten = false)
+        {
+            const auto& dims = nav.getShape();
+
+            if (!allowFlatten && dims.size() != 2)
+            {
+                throw error::AronException(__PRETTY_FUNCTION__, "The NDArray must have 2 dimensions.", nav.getPath());
+            }
 
-            std::vector<T> v(dims.at(0));
-            memcpy(v.data(), nav->getData(), size);
+            if (dims.at(dims.size()-1) != sizeof(T))
+            {
+                throw error::AronException(__PRETTY_FUNCTION__, "Last dimension of the array has to match the element size.", nav.getPath());
+            }
 
+            const int one_row_size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>());
+            std::vector<T> v = convert_to_1d_vector<T>(nav.getData(), dims.at(0), 0, one_row_size);
             return v;
         }
 
+        template<typename T>
+        static std::vector<T> ConvertTo1DVector(const data::NDArrayPtr& nav, const bool allowFlatten = false)
+        {
+            ARMARX_CHECK_NOT_NULL(nav);
 
+            return ConvertTo1DVector<T>(*nav, allowFlatten);
+        }
+
+        // alias
         template<typename T>
-        static data::NDArrayPtr ConvertFromVector(const std::vector<T>& data)
+        static std::vector<T> ConvertToVector(const data::NDArrayPtr& nav, const bool allowFlatten = false)
+        {
+            return ConvertTo1DVector<T>(nav, allowFlatten);
+        }
+
+
+        // Attention: If a vector was flattened, the reconstruction is flattened as well!
+        template<typename T>
+        static data::NDArrayPtr ConvertFrom1DVector(const std::vector<T>& data)
         {
             data::NDArrayPtr ndArr(new data::NDArray);
 
@@ -77,6 +133,13 @@ namespace armarx::aron::converter
             return ndArr;
         }
 
+        // alias
+        template<typename T>
+        static data::NDArrayPtr ConvertFromVector(const std::vector<T>& data)
+        {
+            return ConvertFrom1DVector<T>(data);
+        }
+
     };
 
 }  // namespace armarx::aron::converter