implement bimap for internal_external state map

Get hashing for inverted lookups, but incur
structural overhead.

Whether this is worth it depends on the number of mapped interface states
and the number of pruning/reactivation requests.

Author: v4hn

core/include/moveit/task_constructor/container_p.h

@@ -42,6 +42,10 @@
 #include <moveit/macros/class_forward.h>
 #include "stage_p.h"
 
+#include <boost/bimap.hpp>
+#include <boost/bimap/unordered_set_of.hpp>
+#include <boost/bimap/unordered_multiset_of.hpp>
+
 #include <map>
 #include <climits>
 
@@ -118,7 +122,8 @@ class ContainerBasePrivate : public StagePrivate
 	InterfacePtr pendingForward() const { return pending_forward_; }
 
 	// map InterfaceStates from children to external InterfaceStates of the container
-	const auto& internalToExternalMap() const { return internal_to_external_; }
+	const auto& internalToExternalMap() const { return internal_external_.left; }
+	const auto& externalToInternalMap() const { return internal_external_.right; }
 
 protected:
 	ContainerBasePrivate(ContainerBase* me, const std::string& name);
@@ -135,14 +140,16 @@ class ContainerBasePrivate : public StagePrivate
 		child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
 	}
 
-	/// copy external_state to a child's interface and remember the link in internal_to map
+	/// copy external_state to a child's interface and remember the link in internal_external map
 	template <Interface::Direction>
 	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
 	/// lift solution from internal to external level
 	void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
 	                  const InterfaceState* internal_to);
 
-	auto& internalToExternalMap() { return internal_to_external_; }
+	/// protected writable overloads
+	inline auto& internalToExternalMap() { return internal_external_.left; }
+	inline auto& ExternalToInternalMap() { return internal_external_.right; }
 
 	// set in resolveInterface()
 	InterfaceFlags required_interface_;
@@ -151,7 +158,9 @@ class ContainerBasePrivate : public StagePrivate
 	container_type children_;
 
 	// map start/end states of children (internal) to corresponding states in our external interfaces
-	std::map<const InterfaceState*, InterfaceState*> internal_to_external_;
+	boost::bimap<boost::bimaps::unordered_set_of<const InterfaceState*>,
+	             boost::bimaps::unordered_multiset_of<const InterfaceState*>>
+	    internal_external_;
 
 	/* TODO: these interfaces don't need to be priority-sorted.
 	 * Introduce base class UnsortedInterface (which is a plain list) for this use case. */

core/src/container.cpp

@@ -109,20 +109,18 @@ void ContainerBasePrivate::compute() {
 template <Interface::Direction dir>
 void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
 	if (updated) {
-		// TODO(v4hn): This is inefficient, consider storing inverse mapping for each start/end pair in children
-		// for parallel containers there will be multiple internal states
-		std::for_each(internal_to_external_.begin(), internal_to_external_.end(), [&external](auto& p) {
-			if (p.second == &*external)
-				setStatus<dir>(p.first, external->priority().status());
-		});
+		auto internals{ externalToInternalMap().equal_range(&*external) };
+		for (auto& i = internals.first; i != internals.second; ++i) {
+			setStatus<dir>(i->second, external->priority().status());
+		}
 		return;
 	}
 
 	// create a clone of external state within target interface (child's starts() or ends())
 	auto internal = states_.insert(states_.end(), InterfaceState(*external));
 	target->add(*internal);
 	// and remember the mapping between them
-	internal_to_external_.insert(std::make_pair(&*internal, &*external));
+	internalToExternalMap().insert(std::make_pair(&*internal, &*external));
 }
 
 void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
@@ -131,12 +129,12 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
 
 	// map internal to external states
 	auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
-		auto it = internal_to_external_.find(internal);
-		if (it != internal_to_external_.end())
-			return it->second;
+		auto it = internalToExternalMap().find(internal);
+		if (it != internalToExternalMap().end())
+			return const_cast<InterfaceState*>(it->second);
 
 		InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
-		internal_to_external_.insert(std::make_pair(internal, external));
+		internalToExternalMap().insert(std::make_pair(internal, external));
 		created = true;
 		return external;
 	};
@@ -240,7 +238,7 @@ void ContainerBase::reset() {
 	impl->pending_backward_->clear();
 	impl->pending_forward_->clear();
 	// ... and state mapping
-	impl->internal_to_external_.clear();
+	impl->internalToExternalMap().clear();
 
 	// interfaces depend on children which might change
 	impl->required_interface_ = UNKNOWN;