Merge branch 'master' of gitlab.com:Simox/simox

CMakeLists.txt

@@ -5,6 +5,15 @@ PROJECT(Simox LANGUAGES CXX VERSION 2.3.74.0)
 MESSAGE (STATUS "** Simox version: ${Simox_VERSION}")
 INCLUDE(${Simox_SOURCE_DIR}/CMakeModules/SimoxMacros.cmake)
 SET(Simox_BOOST_VERSION 1.65.1)
+# If OS is Ubuntu 20, use another Boost version.
+execute_process(COMMAND lsb_release -cs
+    OUTPUT_VARIABLE RELEASE_CODENAME
+    OUTPUT_STRIP_TRAILING_WHITESPACE
+)
+if (RELEASE_CODENAME STREQUAL focal)
+    message(STATUS "Detected Ubuntu 20, using Boost 1.71.0")
+    SET(Simox_BOOST_VERSION 1.71.0)
+endif()
 #######################################################################################
 #global config
 # Set up build type

SimoxUtility/CMakeLists.txt

@@ -72,6 +72,9 @@ SET(SOURCES
     math/pose/invert.cpp
     math/pose/orthogonalize.cpp
 
+    math/periodic/periodic_diff.cpp
+    math/periodic/periodic_mean.cpp
+
     math/SoftMinMax.cpp
 
     shapes/AxisAlignedBoundingBox.cpp
@@ -109,46 +112,52 @@ SET(INCLUDES
 
     math/convert/rad_to_deg.h
     math/convert/deg_to_rad.h
-
     math/convert/aa_to_mat3f.h
     math/convert/aa_to_mat4f.h
     math/convert/aa_to_quat.h
     math/convert/aa_to_rpy.h
-
     math/convert/mat3f_to_aa.h
     math/convert/mat3f_to_mat4f.h
     math/convert/mat3f_to_quat.h
     math/convert/mat3f_to_rpy.h
-
     math/convert/mat4f_to_aa.h
     math/convert/mat4f_to_mat3f.h
     math/convert/mat4f_to_pos.h
     math/convert/mat4f_to_quat.h
     math/convert/mat4f_to_rpy.h
-
     math/convert/pos_aa_to_mat4f.h
     math/convert/pos_mat3f_to_mat4f.h
     math/convert/pos_quat_to_mat4f.h
     math/convert/pos_rpy_to_mat4f.h
     math/convert/pos_to_mat4f.h
-
     math/convert/quat_to_aa.h
     math/convert/quat_to_mat3f.h
     math/convert/quat_to_mat4f.h
     math/convert/quat_to_rpy.h
-
     math/convert/rpy_to_aa.h
     math/convert/rpy_to_mat3f.h
     math/convert/rpy_to_mat4f.h
     math/convert/rpy_to_quat.h
-
     math/convert/mat4f_to_xyyaw.h
     math/convert/xyyaw_to_mat4f.h
 
     math/distance/delta_angle.h
     math/distance/angle_between.h
 
-    math/periodic_clamp.h
+    math/mean.h
+
+    math/norm/norm_clamp.h
+    math/norm/norm_max.h
+    math/norm/norm_min.h
+
+    math/normal/orthogonal_vector.h
+    math/normal/normal_to_mat3.h
+    math/normal/normal_to_mat4.h
+
+    math/periodic_clamp.h  # Legacy header.
+    math/periodic/periodic_clamp.h
+    math/periodic/periodic_diff.h
+    math/periodic/periodic_mean.h
 
     math/pose/align_box_orientation.h
     math/pose/check_rotation_matrix.h
@@ -157,11 +166,15 @@ SET(INCLUDES
     math/pose/pose.h
     math/pose/transform.h
 
-    math/normal/orthogonal_vector.h
-    math/normal/normal_to_mat3.h
-    math/normal/normal_to_mat4.h
+    math/similarity/cosine_similarity.h
+    math/similarity/angular_similarity.h
+
+    math/sum.h
 
     math/SoftMinMax.h
+    math/rescale.h
+
+    math/zero.h
 
     meta/eigen/enable_if_compile_time_size.h
     meta/eigen/is_eigen_array.h

SimoxUtility/color/ColorMap.cpp

 #include "ColorMap.h"
 
+#include <SimoxUtility/math/rescale.h>
+
 
 namespace simox::color
 {
@@ -58,12 +60,10 @@ namespace simox::color
 
         if (_vmin || _vmax)
         {
-            // Native: [1 .. 2]
-            // Virtual: [100 .. 200]
+            // Original: [1 .. 2]
+            // Virtual:  [100 .. 200]
             // => Scale 150 to 1.5
-
-            value = (value - vmin()) / (vmax() - vmin());  // 150 -> 0.5
-            value = value * (original_vmax() - original_vmin()) + original_vmin(); // 0.5 -> 1.5
+            value = simox::math::rescale(value, vmin(), vmax(), original_vmin(), original_vmax());
         }
 
         // keys.size() >= 2

SimoxUtility/color/ColorMap.h

 #pragma once
 
+#include <algorithm>
 #include <initializer_list>
 #include <map>
 #include <string>
@@ -70,10 +71,13 @@ namespace simox::color
 
         /// The value corresponding to the bottom color.
         float vmin() const { return _vmin ? *_vmin : original_vmin(); }
-        void set_vmin(float vmin) { this->_vmin = vmin; }
         /// The value corresponding to the top color.
         float vmax() const { return _vmax ? *_vmax : original_vmax(); }
+
+        void set_vmin(float vmin) { this->_vmin = vmin; }
         void set_vmax(float vmax) { this->_vmax = vmax; }
+        void set_vmin(const std::vector<float>& values) { set_vmin(*std::max_element(values.begin(), values.end())); }
+        void set_vmax(const std::vector<float>& values) { set_vmax(*std::max_element(values.begin(), values.end())); }
 
         /// Sets the value limits, i.e. scales the color map to the range [vmin, vmax].
         void set_vlimits(float vmin, float vmax)
@@ -81,6 +85,12 @@ namespace simox::color
             set_vmin(vmin);
             set_vmax(vmax);
         }
+        void set_vlimits(const std::vector<float>& values)
+        {
+            const auto [min, max] = std::minmax_element(values.begin(), values.end());
+            set_vmin(*min);
+            set_vmax(*max);
+        }
 
 
         /// Get this colormap reversed (but defined in the same value range as this).

SimoxUtility/json/eigen_conversion.h

@@ -30,7 +30,6 @@
 namespace Eigen
 {
 
-
     // Eigen::MatrixBase (non-specialized).
 
     /// Writes the matrix as list of rows.

SimoxUtility/math.h

@@ -5,6 +5,13 @@
 #include "math/SoftMinMax.h"
 #include "math/convert.h"
 #include "math/distance.h"
+#include "math/mean.h"
+#include "math/norm.h"
 #include "math/normal.h"
+#include "math/periodic.h"
 #include "math/periodic_clamp.h"
 #include "math/pose.h"
+#include "math/rescale.h"
+#include "math/similarity.h"
+#include "math/sum.h"
+#include "math/zero.h"

SimoxUtility/math/mean.h

+#pragma once
+
+
+// STD/STL
+#include <iterator>
+#include <type_traits>
+
+// Simox
+#include <SimoxUtility/math/sum.h>
+
+
+namespace simox::math
+{
+
+    template <
+        typename iter_t,
+        typename element_t = typename std::iterator_traits<iter_t>::value_type>
+    element_t
+    mean(iter_t first, iter_t last)
+    {
+        return math::sum(first, last) * (1. / std::distance(first, last));
+    }
+
+
+    template <
+        typename container_t,
+        typename element_t =
+            typename std::iterator_traits<
+                decltype(std::begin(std::declval<container_t>()))>::value_type>
+    element_t
+    mean(const container_t& container)
+    {
+        return math::mean(std::begin(container), std::end(container));
+    }
+
+}

SimoxUtility/math/norm.h

+#pragma once
+
+// This file is generated!
+
+#include "norm/norm_clamp.h"
+#include "norm/norm_max.h"
+#include "norm/norm_min.h"

SimoxUtility/math/norm/norm_clamp.h

+#pragma once
+
+
+// Eigen
+#include <Eigen/Core>
+
+// Simox
+#include <SimoxUtility/math/norm/norm_max.h>
+#include <SimoxUtility/math/norm/norm_min.h>
+
+
+namespace simox::math
+{
+
+    template <int rows>
+    Eigen::Matrix<float, rows, 1>
+    norm_clamp(const Eigen::Matrix<float, rows, 1>& v, const float min, const float max)
+    {
+        if (v.norm() < min)
+        {
+            return v.normalized() * min;
+        }
+        else if (v.norm() > max)
+        {
+            return v.normalized() * max;
+        }
+
+        return v;
+    }
+
+}

SimoxUtility/math/norm/norm_max.h

+#pragma once
+
+
+// Eigen
+#include <Eigen/Core>
+
+
+namespace simox::math
+{
+
+    template <int rows>
+    Eigen::Matrix<float, rows, 1>
+    norm_max(const Eigen::Matrix<float, rows, 1>& v, const float max)
+    {
+        if (v.norm() > max)
+        {
+            return v.normalized() * max;
+        }
+
+        return v;
+    }
+
+}

SimoxUtility/math/norm/norm_min.h

+#pragma once
+
+
+// Eigen
+#include <Eigen/Core>
+
+
+namespace simox::math
+{
+
+    template <int rows>
+    Eigen::Matrix<float, rows, 1>
+    norm_min(const Eigen::Matrix<float, rows, 1>& v, const float min)
+    {
+        if (v.norm() < min)
+        {
+            return v.normalized() * min;
+        }
+
+        return v;
+    }
+
+}

SimoxUtility/math/periodic.h

+#pragma once
+
+// This file is generated!
+
+#include "periodic/periodic_clamp.h"
+#include "periodic/periodic_diff.h"
+#include "periodic/periodic_mean.h"

SimoxUtility/math/periodic/periodic_clamp.h

+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+namespace simox::math
+{
+    template<class T>
+    inline
+    std::enable_if_t<std::is_floating_point_v<T>, T>
+    periodic_clamp(T value, T periodLo, T periodHi)
+    {
+        const T dist = periodHi - periodLo;
+        return std::fmod(std::fmod(value - periodLo, dist) + dist, dist) + periodLo;
+    }
+}
+

SimoxUtility/math/periodic/periodic_diff.cpp

+#include "periodic_diff.h"
+
+
+namespace simox
+{
+
+    float math::periodic_diff(float a, float b, float periodLo, float periodHi)
+    {
+        return periodic_diff<float>(a, b, periodLo, periodHi);
+    }
+
+    double math::periodic_diff(double a, double b, double periodLo, double periodHi)
+    {
+        return periodic_diff<double>(a, b, periodLo, periodHi);
+    }
+
+}
+

SimoxUtility/math/periodic/periodic_diff.h

+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+#include <Eigen/Core>
+
+#include "periodic_clamp.h"
+
+
+namespace simox::math
+{
+
+    /**
+     * @brief Computes the periodic (cyclic, wrapped) difference `a - b`
+     * over the cyclic interval `[periodLo, periodHi]`.
+     */
+    template <class Float,
+              std::enable_if_t<std::is_floating_point_v<Float>, int> = 0>
+    inline
+    Float periodic_diff(Float a, Float b, Float periodLo, Float periodHi)
+    {
+        /* Python:
+        a, b = wrap_angles([a, b])
+        diffs = [a - b, (a + 2 * np.pi) - b, (a - 2 * np.pi) - b]
+        i_min = np.argmin(np.abs(diffs))
+        return diffs[int(i_min)]
+        */
+        a = periodic_clamp(a, periodLo, periodHi);
+        b = periodic_clamp(b, periodLo, periodHi);
+
+        const Float period = periodHi - periodLo;
+
+        Eigen::Array<Float, 3, 1> diffs = {
+            a - b,
+            (a + period) - b,
+            (a - period) - b
+        };
+
+        int i = 0;
+        diffs.abs().minCoeff(&i);
+        return diffs(i);
+    }
+
+
+    // Pre-compiled versions.
+
+    float periodic_diff(float a, float b, float periodLo, float periodHi);
+    double periodic_diff(double a, double b, double periodLo, double periodHi);
+
+}
+

SimoxUtility/math/periodic/periodic_mean.cpp

+#include "periodic_mean.h"
+
+
+namespace simox
+{
+
+    float math::periodic_mean(const std::vector<float>& samples, float periodLo, float periodHi)
+    {
+        return periodic_mean<float>(samples, periodLo, periodHi);
+    }
+    double math::periodic_mean(const std::vector<double>& samples, double periodLo, double periodHi)
+    {
+        return periodic_mean<double>(samples, periodLo, periodHi);
+    }
+
+}
+

SimoxUtility/math/periodic/periodic_mean.h

+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/math/rescale.h>
+
+#include "periodic_clamp.h"
+
+
+namespace simox::math
+{
+
+    /**
+     * @brief Computes the periodic (cyclic, wrapped) difference `a - b`
+     * over the cyclic interval `[periodLo, periodHi]`.
+     */
+    template <class Float,
+              std::enable_if_t<std::is_floating_point_v<Float>, int> = 0>
+    inline
+    Float periodic_mean(const std::vector<Float>& samples, Float periodLo, Float periodHi)
+    {
+        /* Python: (when samples in [0, 2*pi]
+        nominator = np.sum(np.sin(samples))
+        denominator = np.sum(np.cos(samples))
+        return np.arctan2(nominator, denominator)
+        */
+
+        Float nominator = 0;
+        Float denominator = 0;
+        for (auto sample : samples)
+        {
+            auto scaled = rescale(sample, periodLo, periodHi, Float(0), Float(2*M_PI));
+            nominator += std::sin(scaled);
+            denominator += std::cos(scaled);
+        }
+        Float result = std::atan2(nominator, denominator);
+        return rescale(result, Float(0), Float(2*M_PI), periodLo, periodHi);
+    }
+
+
+    // Pre-compiled versions.
+
+    float periodic_mean(const std::vector<float>& samples, float periodLo, float periodHi);
+    double periodic_mean(const std::vector<double>& samples, double periodLo, double periodHi);
+
+}
+

SimoxUtility/math/periodic_clamp.h

 #pragma once
 
-#include <cmath>
-#include <type_traits>
-
-namespace simox::math
-{
-    template<class T>
-    inline
-    std::enable_if_t<std::is_floating_point_v<T>, T>
-    periodic_clamp(T value, T periodLo, T periodHi)
-    {
-        const T dist = periodHi - periodLo;
-        return std::fmod(std::fmod(value - periodLo, dist) + dist, dist) + periodLo;
-    }
-}
-
+// Legacy header.
+#include "periodic/periodic_clamp.h"

SimoxUtility/math/rescale.h

+#pragma once
+
+#include <type_traits>
+
+
+namespace simox::math
+{
+
+    template <class Float,
+              std::enable_if_t<std::is_floating_point_v<Float>, int> = 0>
+    inline
+    /// Rescale a scalar `value` from interval `[from_lo, from_hi]` to `[to_lo, to_hi]`.
+    Float rescale(Float value, Float from_lo, Float from_hi, Float to_lo, Float to_hi)
+    {
+        // value: [from_lo, from_hi]
+        Float norm = (value - from_lo) / (from_hi - from_lo);  // [0, 1]
+        return norm * (to_hi - to_lo) + to_lo;  // [to_lo, to_hi]
+    }
+
+}
+

SimoxUtility/math/similarity.h

+#pragma once
+
+// This file is generated!
+
+#include "similarity/angular_similarity.h"
+#include "similarity/cosine_similarity.h"