bugfix compound instances

fzi-forschungszentrum-informatik · Feb 7, 2024 · 7b19474 · 7b19474
1 parent e1244fe
commit 7b19474
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 2 deletions.
diff --git a/README.md b/README.md
@@ -13,7 +13,7 @@ https://build.ros2.org/job/Hdev__lanelet2__ubuntu_jammy_amd64/lastBuild/)
 [![PyPI Downloads](https://img.shields.io/pypi/dm/lanelet2.svg?label=PyPI%20downloads)](
 https://pypi.org/project/lanelet2/)
 
-## New module :rocket: : `lanelet2_ml_converter`
+## New experimental module :test_tube: : `lanelet2_ml_converter`
 
 Convert Lanelet2 maps directly to instance labels for map perception and other map-based learning tasks! `lanelet2_ml_converter` provides local instance labels of various abstraction layers and representations accessible as numpy arrays from python. Check out the `lanelet2_ml_converter` module for more information and usage examples.
 

diff --git a/lanelet2_ml_converter/src/MapData.cpp b/lanelet2_ml_converter/src/MapData.cpp
@@ -226,6 +226,15 @@ std::map<Id, size_t>::const_iterator findFirstOccElement(const CompoundElsList&
   return searchMap.end();
 }
 
+bool hasElementNotInOther(const CompoundElsList& elsList1, const CompoundElsList& elsList2) {
+  for (const auto& el : elsList1.ids) {
+    if (std::find(elsList2.ids.begin(), elsList2.ids.end(), el) == elsList2.ids.end()) {
+      return true;
+    }
+  }
+  return false;
+}
+
 void insertAndCheckNewCompoundInstances(std::vector<CompoundElsList>& compFeats,
                                         const std::vector<CompoundElsList>& newCompFeats,
                                         std::map<Id, size_t>& elInsertIdx) {
@@ -237,11 +246,42 @@ void insertAndCheckNewCompoundInstances(std::vector<CompoundElsList>& compFeats,
         elInsertIdx[el] = compFeats.size() - 1;
       }
     } else if ((compFeats[firstOccIt->second].ids.size() < compEl.ids.size()) &&
-               compFeats[firstOccIt->second].type == compEl.type) {
+               compFeats[firstOccIt->second].type == compEl.type &&
+               !hasElementNotInOther(compFeats[firstOccIt->second], compEl)) {
       compFeats[firstOccIt->second] = compEl;
       for (const Id& el : compEl.ids) {
         elInsertIdx[el] = firstOccIt->second;
       }
+    } else if (compFeats[firstOccIt->second].type == compEl.type) {
+      std::vector<Id> leftoverIds;
+      std::vector<bool> leftoverInverted;
+      bool lastLeftover{false};
+      for (size_t i = 0; i < compEl.ids.size(); i++) {
+        if (!elInsertIdx.count(compEl.ids[i])) {
+          leftoverIds.push_back(compEl.ids[i]);
+          leftoverInverted.push_back(compEl.inverted[i]);
+          lastLeftover = true;
+        } else if (lastLeftover) {
+          CompoundElsList leftover(leftoverIds, leftoverInverted, compEl.type);
+          compFeats.push_back(leftover);
+          for (const Id& el : leftover.ids) {
+            elInsertIdx[el] = compFeats.size() - 1;
+          }
+          leftoverIds.clear();
+          leftoverInverted.clear();
+          lastLeftover = false;
+        }
+      }
+      if (lastLeftover) {
+        CompoundElsList leftover(leftoverIds, leftoverInverted, compEl.type);
+        compFeats.push_back(leftover);
+        for (const Id& el : leftover.ids) {
+          elInsertIdx[el] = compFeats.size() - 1;
+        }
+        leftoverIds.clear();
+        leftoverInverted.clear();
+        lastLeftover = false;
+      }
     }
   }
 }