Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New module: lanelet2_ml_converter #329

Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
66 commits
Select commit Hold shift + click to select a range
01805e6
initial commit lanelet2_map_learning module
immel-f Jul 18, 2023
4db0c18
bugfixes after cleanup
immel-f Jul 19, 2023
60ea2c6
fix dot visualization
immel-f Jul 20, 2023
3f6c08b
wip map graph data provider
immel-f Jul 20, 2023
39faaa0
wip graph conversion
immel-f Jul 24, 2023
8f757a5
wip polyline repr
immel-f Jul 25, 2023
0c2168c
wip node feature calculation
immel-f Jul 27, 2023
283c49f
draft lane lane graph
immel-f Aug 2, 2023
5155b53
wip lane to te graph
immel-f Aug 3, 2023
fb65688
draft lane to te graph
immel-f Aug 8, 2023
d3e3f34
wip partial rewrite new concept
immel-f Oct 30, 2023
8fe4100
wip partial rewrite, continue map feature classes
immel-f Oct 31, 2023
071a6b2
continue rewrite
immel-f Nov 7, 2023
d07a486
wip compound features
immel-f Nov 8, 2023
6aadd3f
further work map features
immel-f Nov 9, 2023
44d6194
first draft map features
immel-f Nov 22, 2023
1320216
wip MapData
immel-f Nov 23, 2023
aadf394
wip MapData and remove copy of lanelet2_routing classes
immel-f Nov 23, 2023
15cabc2
wip map data
immel-f Nov 24, 2023
1ac2c69
first draft compound feature calculation
immel-f Nov 29, 2023
d8cf015
draft, whole module builds again
immel-f Dec 1, 2023
95ccdd4
add unit test map
immel-f Dec 6, 2023
d80f461
add unit test map visualization file
immel-f Dec 6, 2023
025f234
add utils unit tests and fix bugs
immel-f Dec 7, 2023
3517b2d
unit tests for map features and fixes
immel-f Dec 7, 2023
835184f
wip MapData tests, add matplot++ for debug (remove later)
immel-f Dec 8, 2023
f314d4b
fix MapData
immel-f Dec 11, 2023
b809af7
add MapData Unit tests
immel-f Dec 12, 2023
24cb1d4
bugfix extract submap
immel-f Dec 13, 2023
742e47a
add associated ll ids
immel-f Dec 14, 2023
2f185fb
finish MapDataInterface unit tests
immel-f Dec 14, 2023
b04797a
wip serialization
immel-f Dec 15, 2023
03a8854
finish serialization, bugfixes
immel-f Dec 18, 2023
6bb3a84
wip python bindings
immel-f Dec 18, 2023
eeb1f1d
cont wip python bindings
immel-f Dec 20, 2023
dd8bb73
fix const ref properties
immel-f Dec 20, 2023
7ca574b
wip python bindings
immel-f Dec 21, 2023
7db76cd
wip python bindings
immel-f Jan 8, 2024
8a486e9
bugfixes python bindings
immel-f Jan 9, 2024
8cfe875
first finished version with python bindings; add transformation to ve…
immel-f Jan 10, 2024
dab156d
bugfixes
immel-f Jan 12, 2024
506a891
more bugfixes, now tested working on real map
immel-f Jan 15, 2024
63cd77d
fix python serialization, properly handle splitted cut lines
immel-f Jan 16, 2024
bf6d8c0
add following edges
immel-f Jan 17, 2024
d444210
add tracing pointMatrix -> cpdFeat
immel-f Jan 18, 2024
d9578d1
remove debug prints
immel-f Jan 18, 2024
65aac9e
fix path loop edge case, support multi file save and load
immel-f Jan 19, 2024
df54547
bugfix double cpdFeats
immel-f Jan 22, 2024
8763e4d
bugfix invalid feats
immel-f Jan 22, 2024
6e8af47
bugfix transform rotation
immel-f Jan 23, 2024
8beb2ca
better edge support
immel-f Jan 25, 2024
f914eb5
rename module
immel-f Jan 29, 2024
226073a
Merge branch 'fzi-forschungszentrum-informatik:master' into feature_m…
immel-f Jan 29, 2024
d255929
remove seq to support older eigen
immel-f Jan 29, 2024
b0d70a6
add missing virtual placeholder
immel-f Jan 29, 2024
f271a5d
add module to lint
immel-f Jan 29, 2024
df0d71d
add missing commands to conanfile
immel-f Jan 29, 2024
059cca1
add missing include
immel-f Jan 29, 2024
e0b63fc
add missing conan include
immel-f Jan 30, 2024
82bd169
Merge branch 'fzi-forschungszentrum-informatik:master' into feature_m…
immel-f Jan 30, 2024
2b050fb
fix conan build
immel-f Jan 30, 2024
e773b16
fix python test and dockerfile
immel-f Jan 30, 2024
54d7712
add numpy to dockerfile
immel-f Jan 30, 2024
ed33a0e
add doc and readme
immel-f Feb 1, 2024
aa3b023
add pre-release note
immel-f Feb 1, 2024
07bc92f
add download link
immel-f Feb 1, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
bugfixes python bindings
  • Loading branch information
immel-f committed Jan 9, 2024
commit 8a486e95a68dea480e8c49ba7b71fba925c302c3
16 changes: 8 additions & 8 deletions lanelet2_map_learning/include/lanelet2_map_learning/MapData.h
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,10 @@ class LaneData {
void updateAssociatedCpdFeatureIndices();

LineStringType getLineStringTypeFromId(Id id);
const LaneLineStringFeature& getLineStringFeatFromId(Id id);
LaneLineStringFeaturePtr getLineStringFeatFromId(Id id);
std::vector<internal::CompoundElsList> computeCompoundLeftBorders(const ConstLanelets& path);
std::vector<internal::CompoundElsList> computeCompoundRightBorders(const ConstLanelets& path);
CompoundLaneLineStringFeature computeCompoundCenterline(const ConstLanelets& path);
CompoundLaneLineStringFeaturePtr computeCompoundCenterline(const ConstLanelets& path);

LaneLineStringFeatures roadBorders_; // auxilliary features
LaneLineStringFeatures laneDividers_; // auxilliary features
Expand Down Expand Up @@ -103,16 +103,16 @@ class TEData {
};

template <typename T>
std::vector<T> getValidElements(const std::vector<T>& vec) {
std::vector<T> res;
std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](T el) { return el.valid(); });
std::vector<std::shared_ptr<T>> getValidElements(const std::vector<std::shared_ptr<T>>& vec) {
std::vector<std::shared_ptr<T>> res;
std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](std::shared_ptr<T> el) { return el->valid(); });
return res;
}

template <typename T>
std::map<Id, T> getValidElements(const std::map<Id, T>& map) {
std::map<Id, T> res;
std::copy_if(map.begin(), map.end(), std::back_inserter(res), [](const auto& pair) { return pair.second.valid(); });
std::map<Id, std::shared_ptr<T>> getValidElements(const std::map<Id, std::shared_ptr<T>>& map) {
std::map<Id, std::shared_ptr<T>> res;
std::copy_if(map.begin(), map.end(), std::back_inserter(res), [](const auto& pair) { return pair.second->valid(); });
return res;
}

Expand Down
64 changes: 34 additions & 30 deletions lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h
Original file line number Diff line number Diff line change
Expand Up @@ -93,8 +93,9 @@ class LaneLineStringFeature : public LineStringFeature {
std::vector<Id> laneletIDs_;
};

using LaneLineStringFeatures = std::map<Id, LaneLineStringFeature>;
using LaneLineStringFeatureList = std::vector<LaneLineStringFeature>;
using LaneLineStringFeaturePtr = std::shared_ptr<LaneLineStringFeature>;
using LaneLineStringFeatures = std::map<Id, LaneLineStringFeaturePtr>;
using LaneLineStringFeatureList = std::vector<LaneLineStringFeaturePtr>;

class TEFeature : public LineStringFeature {
public:
Expand All @@ -118,8 +119,8 @@ class TEFeature : public LineStringFeature {
class LaneletFeature : public MapFeature {
public:
LaneletFeature() {}
LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary,
const LaneLineStringFeature& centerline, Id mapID);
LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary,
LaneLineStringFeaturePtr centerline, Id mapID);
LaneletFeature(const ConstLanelet& ll);
virtual ~LaneletFeature() noexcept = default;

Expand All @@ -128,18 +129,18 @@ class LaneletFeature : public MapFeature {

void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; }

const LaneLineStringFeature& leftBoundary() const { return leftBoundary_; }
const LaneLineStringFeature& rightBoundary() const { return rightBoundary_; }
const LaneLineStringFeature& centerline() const { return centerline_; }
LaneLineStringFeaturePtr leftBoundary() const { return leftBoundary_; }
LaneLineStringFeaturePtr rightBoundary() const { return rightBoundary_; }
LaneLineStringFeaturePtr centerline() const { return centerline_; }

template <class Archive>
friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat,
const unsigned int /*version*/);

private:
LaneLineStringFeature leftBoundary_;
LaneLineStringFeature rightBoundary_;
LaneLineStringFeature centerline_;
LaneLineStringFeaturePtr leftBoundary_;
LaneLineStringFeaturePtr rightBoundary_;
LaneLineStringFeaturePtr centerline_;
LaneletRepresentationType reprType_{LaneletRepresentationType::Centerline};
};

Expand All @@ -152,7 +153,7 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature {
virtual ~CompoundLaneLineStringFeature() noexcept = default;
bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override;

const LaneLineStringFeatureList& features() const { return individualFeatures_; }
LaneLineStringFeatureList features() const { return individualFeatures_; }
const std::vector<double>& pathLengthsRaw() const { return pathLengthsRaw_; }
const std::vector<double>& pathLengthsProcessed() const { return pathLengthsProcessed_; }
const std::vector<bool>& processedFeaturesValid() const { return processedFeaturesValid_; }
Expand All @@ -169,30 +170,33 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature {
double validLengthThresh_{0.3};
};

using CompoundLaneLineStringFeatureList = std::vector<CompoundLaneLineStringFeature>;
using TEFeatures = std::map<Id, TEFeature>;
using LaneletFeatures = std::map<Id, LaneletFeature>;
using CompoundLaneLineStringFeaturePtr = std::shared_ptr<CompoundLaneLineStringFeature>;
using TEFeaturePtr = std::shared_ptr<TEFeature>;
using LaneletFeaturePtr = std::shared_ptr<LaneletFeature>;
using CompoundLaneLineStringFeatureList = std::vector<CompoundLaneLineStringFeaturePtr>;
using TEFeatures = std::map<Id, TEFeaturePtr>;
using LaneletFeatures = std::map<Id, LaneletFeaturePtr>;

template <class T>
MatrixXd getFeatureVectorMatrix(const std::map<Id, T>& mapFeatures, bool onlyPoints, bool pointsIn2d) {
std::vector<T> featList;
MatrixXd getFeatureVectorMatrix(const std::map<Id, std::shared_ptr<T>>& mapFeatures, bool onlyPoints, bool pointsIn2d) {
std::vector<std::shared_ptr<T>> featList;
for (const auto& pair : mapFeatures) {
featList.push_back(pair.second);
}
return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d);
}

template <class T>
MatrixXd getFeatureVectorMatrix(const std::vector<T>& mapFeatures, bool onlyPoints, bool pointsIn2d) {
MatrixXd getFeatureVectorMatrix(const std::vector<std::shared_ptr<T>>& mapFeatures, bool onlyPoints, bool pointsIn2d) {
if (mapFeatures.empty()) {
throw std::runtime_error("Empty mapFeatures vector supplied!");
}
std::vector<VectorXd> featureVectors;
for (const auto& feat : mapFeatures) {
if (!feat.valid()) {
if (!feat->valid()) {
throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!");
}
featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d));
featureVectors.push_back(feat->computeFeatureVector(onlyPoints, pointsIn2d));
}
if (std::adjacent_find(featureVectors.begin(), featureVectors.end(),
[](const VectorXd& v1, const VectorXd& v2) { return v1.size() != v2.size(); }) ==
Expand All @@ -209,47 +213,47 @@ MatrixXd getFeatureVectorMatrix(const std::vector<T>& mapFeatures, bool onlyPoin
}

template <class T>
std::vector<MatrixXd> getPointsMatrices(const std::map<Id, T>& mapFeatures, bool pointsIn2d) {
std::vector<T> featList;
std::vector<MatrixXd> getPointsMatrices(const std::map<Id, std::shared_ptr<T>>& mapFeatures, bool pointsIn2d) {
std::vector<std::shared_ptr<T>> featList;
for (const auto& pair : mapFeatures) {
featList.push_back(pair.second);
}
return getPointsMatrices(featList, pointsIn2d);
}

template <class T>
std::vector<MatrixXd> getPointsMatrices(const std::vector<T>& mapFeatures, bool pointsIn2d) {
std::vector<MatrixXd> getPointsMatrices(const std::vector<std::shared_ptr<T>>& mapFeatures, bool pointsIn2d) {
if (mapFeatures.empty()) {
throw std::runtime_error("Empty mapFeatures vector supplied!");
}
std::vector<MatrixXd> pointMatrices;
for (const auto& feat : mapFeatures) {
if (!feat.valid()) {
if (!feat->valid()) {
throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!");
}
pointMatrices.push_back(feat.pointMatrix(pointsIn2d));
pointMatrices.push_back(feat->pointMatrix(pointsIn2d));
}
return pointMatrices;
}

template <typename T>
bool processFeatures(std::map<Id, T>& featMap, const OrientedRect& bbox, const ParametrizationType& paramType,
int32_t nPoints) {
bool processFeatures(std::map<Id, std::shared_ptr<T>>& featMap, const OrientedRect& bbox,
const ParametrizationType& paramType, int32_t nPoints) {
bool allValid = true;
for (auto& feat : featMap) {
if (!feat.second.process(bbox, paramType, nPoints)) {
if (!feat.second->process(bbox, paramType, nPoints)) {
allValid = false;
}
}
return allValid;
}

template <typename T>
bool processFeatures(std::vector<T>& featVec, const OrientedRect& bbox, const ParametrizationType& paramType,
int32_t nPoints) {
bool processFeatures(std::vector<std::shared_ptr<T>>& featVec, const OrientedRect& bbox,
const ParametrizationType& paramType, int32_t nPoints) {
bool allValid = true;
for (auto& feat : featVec) {
if (!feat.process(bbox, paramType, nPoints)) {
if (!feat->process(bbox, paramType, nPoints)) {
allValid = false;
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <boost/serialization/map.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>

#include "MapData.h"
Expand Down
77 changes: 39 additions & 38 deletions lanelet2_map_learning/src/MapData.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,20 +23,20 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap,
if (isRoadBorder(ll.leftBound3d())) {
LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.leftBound3d().id());
if (itRoadBd != roadBorders_.end()) {
itRoadBd->second.addLaneletID(ll.id());
itRoadBd->second->addLaneletID(ll.id());
} else {
roadBorders_.insert(
{ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(),
bdTypeToEnum(ll.leftBound3d()), ll.id())});
roadBorders_.insert({ll.leftBound3d().id(), std::make_shared<LaneLineStringFeature>(
ll.leftBound3d().basicLineString(), ll.leftBound3d().id(),
bdTypeToEnum(ll.leftBound3d()), ll.id())});
}
} else {
LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.leftBound3d().id());
if (itLaneBd != laneDividers_.end()) {
itLaneBd->second.addLaneletID(ll.id());
itLaneBd->second->addLaneletID(ll.id());
} else {
laneDividers_.insert(
{ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(),
bdTypeToEnum(ll.leftBound3d()), ll.id())});
laneDividers_.insert({ll.leftBound3d().id(), std::make_shared<LaneLineStringFeature>(
ll.leftBound3d().basicLineString(), ll.leftBound3d().id(),
bdTypeToEnum(ll.leftBound3d()), ll.id())});
}
}

Expand All @@ -55,20 +55,20 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap,
if (isRoadBorder(ll.rightBound3d())) {
LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.rightBound3d().id());
if (itRoadBd != roadBorders_.end()) {
itRoadBd->second.addLaneletID(ll.id());
itRoadBd->second->addLaneletID(ll.id());
} else {
roadBorders_.insert(
{ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(),
bdTypeToEnum(ll.rightBound3d()), ll.id())});
roadBorders_.insert({ll.rightBound3d().id(), std::make_shared<LaneLineStringFeature>(
ll.rightBound3d().basicLineString(), ll.rightBound3d().id(),
bdTypeToEnum(ll.rightBound3d()), ll.id())});
}
} else {
LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.rightBound3d().id());
if (itLaneBd != laneDividers_.end()) {
itLaneBd->second.addLaneletID(ll.id());
itLaneBd->second->addLaneletID(ll.id());
} else {
laneDividers_.insert(
{ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(),
bdTypeToEnum(ll.rightBound3d()), ll.id())});
laneDividers_.insert({ll.rightBound3d().id(), std::make_shared<LaneLineStringFeature>(
ll.rightBound3d().basicLineString(), ll.rightBound3d().id(),
bdTypeToEnum(ll.rightBound3d()), ll.id())});
}
}
Optional<ConstLanelet> rightLL = localSubmapGraph->right(ll);
Expand All @@ -82,11 +82,12 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap,
void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap,
lanelet::routing::RoutingGraphConstPtr localSubmapGraph) {
for (const auto& ll : localSubmap->laneletLayer) {
const LaneLineStringFeature& leftBoundary = getLineStringFeatFromId(ll.leftBound().id());
const LaneLineStringFeature& rightBoundary = getLineStringFeatFromId(ll.rightBound().id());
LaneLineStringFeature centerline{ll.centerline3d().basicLineString(), ll.centerline3d().id(),
LineStringType::Centerline, ll.id()};
laneletFeatures_.insert({ll.id(), LaneletFeature(leftBoundary, rightBoundary, centerline, ll.id())});
LaneLineStringFeaturePtr leftBoundary = getLineStringFeatFromId(ll.leftBound().id());
LaneLineStringFeaturePtr rightBoundary = getLineStringFeatFromId(ll.rightBound().id());
LaneLineStringFeaturePtr centerline = std::make_shared<LaneLineStringFeature>(
ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline, ll.id());
laneletFeatures_.insert(
{ll.id(), std::make_shared<LaneletFeature>(leftBoundary, rightBoundary, centerline, ll.id())});
}
}

Expand All @@ -111,17 +112,17 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) {
LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id);
LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id);
if (itLaneBd != laneDividers_.end()) {
bdType = itLaneBd->second.type();
bdType = itLaneBd->second->type();
} else if (itRoadBd != roadBorders_.end()) {
bdType = itRoadBd->second.type();
bdType = itRoadBd->second->type();
} else {
throw std::runtime_error("Lanelet boundary " + std::to_string(id) +
" is neither a road border nor a lane divider!");
}
return bdType;
}

const LaneLineStringFeature& LaneData::getLineStringFeatFromId(Id id) {
LaneLineStringFeaturePtr LaneData::getLineStringFeatFromId(Id id) {
LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id);
LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id);
if (itLaneBd != laneDividers_.end()) {
Expand Down Expand Up @@ -172,13 +173,13 @@ std::vector<CompoundElsList> LaneData::computeCompoundRightBorders(const ConstLa
return compoundBorders;
}

CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLanelets& path) {
CompoundLaneLineStringFeaturePtr LaneData::computeCompoundCenterline(const ConstLanelets& path) {
LaneLineStringFeatureList compoundCenterlines;
for (const auto& ll : path) {
compoundCenterlines.push_back(
LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline, ll.id()));
compoundCenterlines.push_back(std::make_shared<LaneLineStringFeature>(ll.centerline3d().basicLineString(), ll.id(),
LineStringType::Centerline, ll.id()));
}
return CompoundLaneLineStringFeature(compoundCenterlines, LineStringType::Centerline);
return std::make_shared<CompoundLaneLineStringFeature>(compoundCenterlines, LineStringType::Centerline);
}

std::map<Id, size_t>::const_iterator findFirstOccElement(const CompoundElsList& elsList,
Expand Down Expand Up @@ -241,39 +242,39 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap,
for (const auto& compFeat : compoundedBordersAndDividers) {
LaneLineStringFeatureList toBeCompounded;
for (const auto& el : compFeat.ids) {
LaneLineStringFeature cmpdFeat = getLineStringFeatFromId(el);
LaneLineStringFeaturePtr cmpdFeat = getLineStringFeatFromId(el);
toBeCompounded.push_back(cmpdFeat);
}
LineStringType cmpdType = toBeCompounded.front().type();
LineStringType cmpdType = toBeCompounded.front()->type();
if (cmpdType == LineStringType::RoadBorder) {
compoundRoadBorders_.push_back(CompoundLaneLineStringFeature(toBeCompounded, cmpdType));
compoundRoadBorders_.push_back(std::make_shared<CompoundLaneLineStringFeature>(toBeCompounded, cmpdType));
} else {
compoundLaneDividers_.push_back(CompoundLaneLineStringFeature(toBeCompounded, cmpdType));
compoundLaneDividers_.push_back(std::make_shared<CompoundLaneLineStringFeature>(toBeCompounded, cmpdType));
}
}
}

void LaneData::updateAssociatedCpdFeatureIndices() {
for (size_t i = 0; i < compoundRoadBorders_.size(); i++) {
const auto& cpdFeat = compoundRoadBorders_[i];
for (const auto& indFeat : cpdFeat.features()) {
for (const auto& id : indFeat.laneletIDs()) {
for (const auto& indFeat : cpdFeat->features()) {
for (const auto& id : indFeat->laneletIDs()) {
associatedCpdRoadBorderIndices_[id].push_back(i);
}
}
}
for (size_t i = 0; i < compoundLaneDividers_.size(); i++) {
const auto& cpdFeat = compoundLaneDividers_[i];
for (const auto& indFeat : cpdFeat.features()) {
for (const auto& id : indFeat.laneletIDs()) {
for (const auto& indFeat : cpdFeat->features()) {
for (const auto& id : indFeat->laneletIDs()) {
associatedCpdLaneDividerIndices_[id].push_back(i);
}
}
}
for (size_t i = 0; i < compoundCenterlines_.size(); i++) {
const auto& cpdFeat = compoundCenterlines_[i];
for (const auto& indFeat : cpdFeat.features()) {
for (const auto& id : indFeat.laneletIDs()) {
for (const auto& indFeat : cpdFeat->features()) {
for (const auto& id : indFeat->laneletIDs()) {
associatedCpdCenterlineIndices_[id].push_back(i);
}
}
Expand Down
Loading