diff --git a/SimoxUtility/CMakeLists.txt b/SimoxUtility/CMakeLists.txt
index 0b14327c3f7dd1cb08879188f5355e0c898668eb..dd3b2d65174ce122bb0c2e9d761b9ec94c597351 100644
--- a/SimoxUtility/CMakeLists.txt
+++ b/SimoxUtility/CMakeLists.txt
@@ -81,6 +81,7 @@ SET(SOURCES
math/pose/check_rotation_matrix.cpp
math/pose/invert.cpp
math/pose/orthogonalize.cpp
+ math/pose/interpolate.cpp
math/statistics/Histogram1D.cpp
@@ -99,12 +100,13 @@ SET(INCLUDES
preprocessor/variadic_for_each.h
preprocessor/drop_front.h
- algorithm/for_each_if.h
+ algorithm/advanced.h
algorithm/apply.hpp
+ algorithm/contains.h
+ algorithm/for_each_if.h
+ algorithm/fuzzy_find.h
algorithm/get_map_keys_values.h
algorithm/minmax.h
- algorithm/advanced.h
- algorithm/fuzzy_find.h
algorithm/string/string_tools.h
algorithm/string/string_conversion.h
algorithm/string/string_conversion_eigen.h
@@ -220,6 +222,7 @@ SET(INCLUDES
math/pose/orthogonalize.h
math/pose/pose.h
math/pose/transform.h
+ math/pose/interpolate.h
math/similarity/cosine_similarity.h
math/similarity/angular_similarity.h
diff --git a/SimoxUtility/algorithm.h b/SimoxUtility/algorithm.h
index a4752d98709411b0351ec0376e155c5d794d88a6..bc7252a2cee84dc106b548b5a3e3025bdae0c14e 100644
--- a/SimoxUtility/algorithm.h
+++ b/SimoxUtility/algorithm.h
@@ -3,6 +3,7 @@
// This file is generated!
#include "algorithm/advanced.h"
+#include "algorithm/contains.h"
#include "algorithm/for_each_if.h"
#include "algorithm/fuzzy_find.h"
#include "algorithm/get_map_keys_values.h"
diff --git a/SimoxUtility/algorithm/contains.h b/SimoxUtility/algorithm/contains.h
new file mode 100644
index 0000000000000000000000000000000000000000..45516257dff2a166ed516779558e4498ef93954c
--- /dev/null
+++ b/SimoxUtility/algorithm/contains.h
@@ -0,0 +1,79 @@
+#pragma once
+
+#include <algorithm>
+#include <functional>
+#include <string>
+#include <map>
+
+
+namespace simox::alg
+{
+
+ // For overloads of `simox::alg::contains` taking `std::string`:
+ // #include <SimoxUtility/algorithm/string/string_tools.h>
+
+ // GENERAL CONTAINERS
+
+ /**
+ * Return true if `value` is an element of `container`, false otherwise.
+ */
+ template <class ContainerT, class ValueT>
+ bool contains(const ContainerT& container, const ValueT& value)
+ {
+ return std::find_if(container.begin(), container.end(), [&value](const auto& v)
+ {
+ return v == value;
+ }) != container.end();
+ }
+
+ /**
+ * Return true if `value` is an element of `container` (as indicated by `predicate`),
+ * false otherwise.
+ */
+ template <class ContainerT, class ValueT, class PredicateT>
+ bool contains(const ContainerT& container, const ValueT& value, const PredicateT& predicate)
+ {
+ return std::find_if(container.begin(), container.end(), [&value, &predicate](const auto& v)
+ {
+ return predicate(v, value);
+ }) != container.end();
+ }
+
+
+ // MAPS
+
+ /**
+ * Return true if `key` is a key in `map`, false otherwise.
+ */
+ template <class K, class V, template<class...> class MapT = std::map, class...Ts>
+ bool contains_key(const MapT<K, V, Ts...>& map, const K& key)
+ {
+ return map.count(key) > 0;
+ }
+
+ /**
+ * Return true if `value` is a value in `map`, false otherwise.
+ */
+ template <class K, class V, template<class...> class MapT = std::map, class...Ts>
+ bool contains_value(const MapT<K, V, Ts...>& map, const V& value)
+ {
+ return std::find_if(map.begin(), map.end(), [&value](const std::pair<K, V>& item)
+ {
+ return item.second == value;
+ }) != map.end();
+ }
+
+
+ // Overloads for string literals (which otherwise don't match the previous definition).
+ template <class V, template<class...> class MapT = std::map, class...Ts>
+ bool contains_key(const MapT<std::string, V, Ts...>& map, const std::string& key)
+ {
+ return map.count(key) > 0;
+ }
+ template <class K, template<class...> class MapT = std::map, class...Ts>
+ bool contains_value(const MapT<K, std::string, Ts...>& map, const std::string& value)
+ {
+ return contains_value<K, std::string>(map, value);
+ }
+
+}
diff --git a/SimoxUtility/algorithm/get_map_keys_values.h b/SimoxUtility/algorithm/get_map_keys_values.h
index eeb88a71f332e332523d1596fe1f387cc64f3c23..7d869b102c3aa8ef85ae369b0c1b165fcdeb32f7 100644
--- a/SimoxUtility/algorithm/get_map_keys_values.h
+++ b/SimoxUtility/algorithm/get_map_keys_values.h
@@ -112,6 +112,24 @@ namespace simox::alg
{
return get_value_ptrs(map);
}
+
+
+ /// Get a value from `map` if it exsits or a default value otherwise.
+ template <class K, class V, template<class...> class MapT = std::map, class...Ts>
+ V get_value_or_default(const MapT<K, V, Ts...>& map, const K& key, const V& default_value)
+ {
+ auto it = map.find(key);
+ return it != map.end() ? it->second : default_value;
+ }
+
+ // overload for `std::string` to match `char[]`.
+ template <class K, template<class...> class MapT = std::map, class...Ts>
+ std::string get_value_or_default(const MapT<K, std::string, Ts...>& map, const K& key, const std::string& default_value)
+ {
+ auto it = map.find(key);
+ return it != map.end() ? it->second : default_value;
+ }
+
}
diff --git a/SimoxUtility/algorithm/string/string_tools.h b/SimoxUtility/algorithm/string/string_tools.h
index 327ec2516c1e7c2273850bed8bc2a70c8b783e2f..404f2c038daade083ec565f9be894ba151959b09 100644
--- a/SimoxUtility/algorithm/string/string_tools.h
+++ b/SimoxUtility/algorithm/string/string_tools.h
@@ -44,6 +44,7 @@ namespace simox::alg
*/
std::string capitalize_words(const std::string& str);
+
void trim(std::string& str, const std::locale& locale = DEFAULT_LOCALE);
std::string trim_copy(const std::string& str, const std::locale& locale = DEFAULT_LOCALE);
@@ -52,6 +53,7 @@ namespace simox::alg
std::string trim_copy_if(const std::string& str, const std::string& trim = "\t ");
+
std::vector<std::string> split(const std::string& str, const std::string& splitBy = "\t ", bool trimElements = true,
bool removeEmptyElements = true, const std::locale& locale = DEFAULT_LOCALE);
@@ -62,17 +64,27 @@ namespace simox::alg
std::string join(const std::vector<std::string> vec, const std::string& delimiter = " ", bool trimElements = false,
bool ignoreEmptyElements = false, const std::locale& locale = DEFAULT_LOCALE);
+
std::string replace_first(std::string const& input, std::string const& search, std::string const& replace);
std::string replace_last(std::string const& input, std::string const& search, std::string const& replace);
std::string replace_all(std::string const& input, std::string const& search, std::string const& replace);
+
bool starts_with(std::string const& input, std::string const& search);
bool ends_with(std::string const& input, std::string const& search);
bool contains(const std::string& haystack, const std::string& needle);
+ inline bool contains(const std::string& haystack, const char* needle)
+ {
+ return contains(haystack, std::string(needle));
+ }
+ inline bool contains(const std::string& string, const char character)
+ {
+ return string.find(character) != std::string::npos;
+ }
template <typename IterT>
diff --git a/SimoxUtility/math/pose.h b/SimoxUtility/math/pose.h
index 3ec8afc2556e947326845580b200e37a06c1cbf5..003bff68627adecdf200b16a0300e509bc23bf66 100644
--- a/SimoxUtility/math/pose.h
+++ b/SimoxUtility/math/pose.h
@@ -4,6 +4,7 @@
#include "pose/align_box_orientation.h"
#include "pose/check_rotation_matrix.h"
+#include "pose/interpolate.h"
#include "pose/invert.h"
#include "pose/is_homogeneous_transform.h"
#include "pose/orthogonalize.h"
diff --git a/SimoxUtility/math/pose/interpolate.cpp b/SimoxUtility/math/pose/interpolate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61dec29db6f41490a909e3c65477a95c085a799d
--- /dev/null
+++ b/SimoxUtility/math/pose/interpolate.cpp
@@ -0,0 +1,25 @@
+#include "interpolate.h"
+
+
+namespace simox::math {
+
+
+Eigen::Affine3f interpolatePose(const Eigen::Affine3f &posePre, const Eigen::Affine3f &poseNext, float t) {
+
+ assert(0 <= t <= 1);
+
+ Eigen::Affine3f pose = Eigen::Affine3f::Identity();
+
+ pose.translation() = (1 - t) * posePre.translation() + t * poseNext.translation();
+
+ Eigen::Quaternionf rotPrev(posePre.linear().matrix());
+ Eigen::Quaternionf rotNext(poseNext.linear().matrix());
+
+ Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
+
+ pose.linear() = rotNew.toRotationMatrix();
+
+ return pose;
+}
+
+} // namespace simox::math
\ No newline at end of file
diff --git a/SimoxUtility/math/pose/interpolate.h b/SimoxUtility/math/pose/interpolate.h
new file mode 100644
index 0000000000000000000000000000000000000000..f31e8dbe730afa2ec933967cf5dd04bc73e6f75c
--- /dev/null
+++ b/SimoxUtility/math/pose/interpolate.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+namespace simox::math {
+
+Eigen::Affine3f interpolatePose(const Eigen::Affine3f &posePre, const Eigen::Affine3f &poseNext, float t);
+
+} // namespace simox::math
\ No newline at end of file
diff --git a/SimoxUtility/tests/algorithm/CMakeLists.txt b/SimoxUtility/tests/algorithm/CMakeLists.txt
index 6ec915c9b370c4725eb4a4d0aca3b4b5360ccb21..24614998a1993c6f9306581d40aea7f51d4b6afe 100644
--- a/SimoxUtility/tests/algorithm/CMakeLists.txt
+++ b/SimoxUtility/tests/algorithm/CMakeLists.txt
@@ -1,5 +1,6 @@
ADD_SU_TEST( apply )
+ADD_SU_TEST( contains )
ADD_SU_TEST( for_each_if )
ADD_SU_TEST( fuzzy_find )
ADD_SU_TEST( get_map_keys_values )
diff --git a/SimoxUtility/tests/algorithm/contains.cpp b/SimoxUtility/tests/algorithm/contains.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7c34cd83e6c2d3b6e4de38c8a99e7291d7b3ffe
--- /dev/null
+++ b/SimoxUtility/tests/algorithm/contains.cpp
@@ -0,0 +1,172 @@
+/**
+* @package SimoxUtility
+* @author Rainer Kartmann
+* @copyright 2021 Rainer Kartmann
+*/
+
+#define BOOST_TEST_MODULE SimoxUtility/algorithm/contains
+
+#include <boost/test/included/unit_test.hpp>
+
+#include <SimoxUtility/algorithm/contains.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <map>
+#include <set>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+
+namespace contains_test
+{
+ struct Fixture
+ {
+ const std::vector<std::string> string_vec { "one", "two", "three" };
+ const std::vector<int> int_vec { 0, -1, 10 };
+
+ const std::set<std::string> string_set { string_vec.begin(), string_vec.end() };
+ const std::set<int> int_set { int_vec.begin(), int_vec.end() };
+
+ const std::string string = "my/string";
+
+
+ std::map<std::string, int> m_si;
+ std::map<std::string, std::string> m_ss;
+ std::map<int, std::string> m_is;
+ std::map<int, int> m_ii;
+
+ std::unordered_map<std::string, int> um_si;
+ std::unordered_map<std::string, std::string> um_ss;
+ std::unordered_map<int, std::string> um_is;
+ std::unordered_map<int, int> um_ii;
+
+ Fixture()
+ {
+ fillMap(m_si);
+ fillMap(m_ss);
+ fillMap(m_is);
+ fillMap(m_ii);
+ fillMap(um_si);
+ fillMap(um_ss);
+ fillMap(um_is);
+ fillMap(um_ii);
+ }
+ ~Fixture()
+ {
+ }
+
+ template <class K, class V, template<class...> class MapT = std::map, class...Ts>
+ static MapT<K, V, Ts...> makeMap()
+ {
+ MapT<K, V, Ts...> map;
+ fillMap(map);
+ return map;
+ }
+ template <class K, class V, template<class...> class MapT = std::map, class...Ts>
+ static void fillMap(MapT<K, V, Ts...>& map)
+ {
+ for (int i = 1; i <= 3; ++i)
+ {
+ map[as<K>(i)] = as<V>(i);
+ }
+ }
+
+ template <class T>
+ static T as(int i)
+ {
+ if constexpr(std::is_same_v<T, int>)
+ {
+ return i;
+ }
+ else if constexpr(std::is_same_v<T, std::string>)
+ {
+ return std::to_string(i);
+ }
+ }
+ };
+}
+
+
+BOOST_FIXTURE_TEST_SUITE(contains_test, contains_test::Fixture)
+
+
+BOOST_AUTO_TEST_CASE(test_contains)
+{
+ BOOST_CHECK_EQUAL(simox::alg::contains(string_vec, "two"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(string_vec, "something"), false);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains(int_vec, -1), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(int_vec, 1000), false);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains(int_set, -1), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(int_set, 1000), false);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains(string_set, "two"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(string_set, "something"), false);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_string_contains)
+{
+ // Uses string_tools.h
+ // char
+ BOOST_CHECK_EQUAL(simox::alg::contains(string, '/'), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(string, ':'), false);
+
+ // single-character string
+ BOOST_CHECK_EQUAL(simox::alg::contains(string, "/"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(string, ":"), false);
+
+ // multi-character string
+ BOOST_CHECK_EQUAL(simox::alg::contains(string, "my"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains(string, "your"), false);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_map_contains)
+{
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_si, "2"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_ss, "2"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_si, "2"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_ss, "2"), true);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_si, "something"), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_ss, "something"), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_si, "something"), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_ss, "something"), false);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_is, 2), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_ii, 2), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_is, 2), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_ii, 2), true);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_is, -1), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(m_ii, -1), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_is, -1), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_key(um_ii, -1), false);
+
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_is, "2"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_ss, "2"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_is, "2"), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_ss, "2"), true);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_is, "something"), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_ss, "something"), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_is, "something"), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_ss, "something"), false);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_si, 2), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_ii, 2), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_si, 2), true);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_ii, 2), true);
+
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_si, -1), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(m_ii, -1), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_si, -1), false);
+ BOOST_CHECK_EQUAL(simox::alg::contains_value(um_ii, -1), false);
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/SimoxUtility/threads/CountingSemaphore.cpp b/SimoxUtility/threads/CountingSemaphore.cpp
index 9e6687e62c9900e841dc2f6b73dd9fe50b5b8812..2f6e616dda67661925905c85d95b47d2312975fb 100644
--- a/SimoxUtility/threads/CountingSemaphore.cpp
+++ b/SimoxUtility/threads/CountingSemaphore.cpp
@@ -7,14 +7,22 @@ namespace simox::threads
CountingSemaphore::CountingSemaphore()
{}
- CountingSemaphore::CountingSemaphore(unsigned int count) : _count(count)
+ CountingSemaphore::CountingSemaphore(unsigned int count) :
+ _count(count)
+ {}
+
+ CountingSemaphore::CountingSemaphore(unsigned int count, unsigned int maxCount) :
+ _count(count), _maxCount(maxCount)
{}
void CountingSemaphore::notify()
{
std::lock_guard<std::mutex> lock(_mutex);
- ++_count;
+ if (!_maxCount || _count < *_maxCount)
+ {
+ ++_count;
+ }
_condition.notify_one();
}
diff --git a/SimoxUtility/threads/CountingSemaphore.h b/SimoxUtility/threads/CountingSemaphore.h
index a7bc78f9059c7584b0f3cda781ca487e8d2ad726..e784b6b6737816148605bfb9bf7103c317e3f534 100644
--- a/SimoxUtility/threads/CountingSemaphore.h
+++ b/SimoxUtility/threads/CountingSemaphore.h
@@ -2,6 +2,7 @@
#include <condition_variable>
#include <mutex>
+#include <optional>
namespace simox::threads
@@ -14,33 +15,52 @@ namespace simox::threads
* Notifiying the semaphore increments the count and allows threads to enter.
* A thread can wait until it may enter. When it enters, it decrements
* the internal count.
+ * An optional max count can limit the value of count (useful e.g. when your
+ * buffer has a limited number of items).
*
- * Can be used e.g. in a Producer-Consumer pattern.
- * The producer signals new jobs via `notify()`, while the consumer waits
+ * A counting semaphore can be used e.g. in Producer-Consumer patterns.
+ * The producer signals new jobs/items via `notify()`, while the consumer waits
* for new jobs via `wait()`.
*/
class CountingSemaphore
{
public:
- /// Construct an initially blocking semaphore (initial count 0).
+ /// Construct an initially blocking semaphore (initial count 0) without max count.
CountingSemaphore();
- /// Construct a semaphore with the given count.
+ /**
+ * @brief Construct a semaphore with the given initial count without max count.
+ * @param count The initial count (0 to block initially).
+ */
CountingSemaphore(unsigned int count);
+ /**
+ * @brief Construct a semaphore with the given initial count and max count.
+ * @param count The initial count (0 to block initially).
+ * @param maxCount An optional count limit (1 for a binary semaphore).
+ */
+ CountingSemaphore(unsigned int count, unsigned int maxCount);
/**
* @brief Signal that one waiting thread may continue.
+ *
* Also known as `post()` or `signal()`.
+ * Increments the count by 1, if it is below the optional max count.
*/
void notify();
/**
* @brief Wait until a thread may enter.
+ *
+ * Decrements the count when resuming.
*/
void wait();
/**
- * @brief Try to enter. If the semaphore is currently blocking, return false.
+ * @brief Try to enter.
+ *
+ * If the semaphore is currently blocking, return false.
+ * If the semaphore is free (count > 0), decrement the count and return true.
+ *
* @return True if entering was successful, false if semaphore was blocking.
*/
bool try_wait();
@@ -56,6 +76,9 @@ namespace simox::threads
/// The current count. Waiting threads may enter when > 0.
unsigned int _count = 0;
+ /// An optional maximal count. All `notifiy()`s increasing the counter above maxCount are ignored.
+ std::optional<unsigned int> _maxCount = std::nullopt;
+
};
}