diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp index 696d2c0acde4091c6fe79e2bee11eb0f2468d64a..8aad5acc286b6bc2e2b1123a8edebd51421f8cac 100644 --- a/src/processor/polyline_2D_utils.cpp +++ b/src/processor/polyline_2D_utils.cpp @@ -409,7 +409,8 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir best_match->normalized_score_ = normalized_score; best_match->overlap_dist_ = overlap_dist; - best_match->print(); + if (print) + best_match->print(); } } @@ -541,7 +542,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool * 2. A closed: [-last_A, 0] */ - int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlap; + int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlap(0); last_A = _points_A.cols() - 1; last_B = _points_B.cols() - 1; diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index 4ac02519ca87fd08e10d1343bb2c888e4cd7b219..8bf1b9cb102b365b647b3fe25e97ff904079d61d 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -48,14 +48,14 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); FeatureMatchPolyline2DScalarMap best_matches; - std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n"; + // std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n"; // Check matching with all features in last // Store all matches consistent with T_last_incoming_prior in best_matches sorted by normalized score for (auto ftr_last_ : _features_in) tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior); - std::cout << "\t" << best_matches.size() << " matches with features last found\n"; + // std::cout << "\t" << best_matches.size() << " matches with features last found\n"; // Try the match for all candidates (sorted by normalized score, higher scores at the end!) for (auto best_ftr_match_pair_it = best_matches.rbegin(); best_ftr_match_pair_it != best_matches.rend(); best_ftr_match_pair_it++ ) @@ -135,8 +135,8 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP // match for this feature has been found matched = true; - std::cout << "Feature tracked!\n"; - best_ftr_match->print(); + // std::cout << "Feature tracked!\n"; + // best_ftr_match->print(); break; } @@ -147,7 +147,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP ftr_it++; } - WOLF_DEBUG(_feature_correspondences.size(), " features tracked"); + // WOLF_DEBUG(_feature_correspondences.size(), " features tracked"); return _feature_correspondences.size(); } @@ -175,8 +175,8 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr auto pl_lmk = lmk_inc_match->pl_landmark_; auto pl_inc = lmk_inc_match->pl_feature_; - WOLF_DEBUG("LANDMARK-INCOMING:"); - lmk_inc_match->print(); + // WOLF_DEBUG("LANDMARK-INCOMING:"); + // lmk_inc_match->print(); assert(lmk_inc_match->check(false) && "before correct drift, check not passed");//without checking partial match @@ -190,7 +190,7 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr // CORRECT DRIFT // feature landmark transformation - WOLF_DEBUG("try update match..."); + // WOLF_DEBUG("try update match..."); tryUpdateMatchTransformation(lmk_inc_match); // RETRY LANDMARK-MATCH IF TOO MUCH ERROR @@ -300,12 +300,12 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame() unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features) { - WOLF_DEBUG("PTFP ", getName(), "::processNew: "); + // WOLF_DEBUG("PTFP ", getName(), "::processNew: "); // PT::processNew() ========================================================== unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures() and trackFeatures() - WOLF_DEBUG("Processing ", n, " new last features"); + // WOLF_DEBUG("Processing ", n, " new last features"); // prior transformations Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity()); @@ -322,7 +322,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr); LandmarkMatchPolyline2DScalarMap best_lmk_matches; - WOLF_DEBUG("Processing feature: ", pl_ftr->id()); + // WOLF_DEBUG("Processing feature: ", pl_ftr->id()); // SEARCH: Check matching with all existing landmarks for (auto lmk : getProblem()->getMap()->getLandmarkList()) @@ -430,37 +430,37 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma for (auto ftr : _features_out) ftr->link(_capture); - WOLF_DEBUG(_features_out.size(), " were provided"); + // WOLF_DEBUG(_features_out.size(), " were provided"); return _features_out.size(); } void ProcessorTrackerFeaturePolyline2D::establishFactors() { - WOLF_DEBUG("PTFP ", getName(), "::establishFactors: "); + // WOLF_DEBUG("PTFP ", getName(), "::establishFactors: "); unsigned int N_factors = 0; // Create a factor for each match in last features FeatureBasePtrList last_features = last_ptr_->getFeatureList(); for (auto ftr : last_features) { - WOLF_DEBUG("\tLast feature: ", ftr->id()); + // WOLF_DEBUG("\tLast feature: ", ftr->id()); assert(landmark_match_map_.count(ftr) && "feature without landmark match in last features"); auto lmk_match = landmark_match_map_[ftr]; auto pl_lmk = lmk_match->pl_landmark_; auto pl_ftr = lmk_match->pl_feature_; assert(pl_ftr->id() == ftr->id()); - WOLF_DEBUG("\tLandmark: ", pl_lmk->id()); + // WOLF_DEBUG("\tLandmark: ", pl_lmk->id()); // LANDMARK CHANGED: update match - WOLF_DEBUG("\tLandmark changed?"); + // WOLF_DEBUG("\tLandmark changed?"); if (lmk_match->isDeprecated()) { - WOLF_DEBUG("\tLandmark changed or was merged"); + // WOLF_DEBUG("\tLandmark changed or was merged"); // not successful update if (!updateLandmarkMatch(lmk_match)) { - std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n"; + // std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n"; // remove from match map landmark_match_map_.erase(ftr); // remove feature @@ -472,10 +472,10 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() // LANDMARK REMOVED: remove match and feature else { - WOLF_DEBUG("\tLandmark removed?"); + // WOLF_DEBUG("\tLandmark removed?"); if (pl_lmk->isRemoving()) { - std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n"; + // std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n"; // remove from match map landmark_match_map_.erase(ftr); // remove feature @@ -485,72 +485,72 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() } // MODIFY LANDMARK (only for not closed and not recently emplaced) - WOLF_DEBUG("\tModifying.."); + // WOLF_DEBUG("\tModifying.."); if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0) // If modified, add lmk to modified_lmks_ list if (modifyLandmarkAndMatch(lmk_match)) modified_lmks_.push_back(pl_lmk); // ESTABLISH FACTORS - WOLF_DEBUG("\tEstablishing factors.."); + // WOLF_DEBUG("\tEstablishing factors.."); // checks assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); assert(lmk_match->check() && "Landmark didn't grow properly"); - std::cout << "Establishing factor between" << std::endl; - std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; - std::cout << "\tlandmark " << pl_lmk->id() << std::endl; - std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl; - std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl; - std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl; - std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl; + // std::cout << "Establishing factor between" << std::endl; + // std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; + // std::cout << "\tlandmark " << pl_lmk->id() << std::endl; + // std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl; + // std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl; + // std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl; + // std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl; // Factors for all feature points int lmk_point_id = lmk_match->landmark_from_id_; for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++) { - std::cout << "feature point " << ftr_point_id << std::endl; - std::cout << "landmark point " << lmk_point_id << std::endl; + // std::cout << "feature point " << ftr_point_id << std::endl; + // std::cout << "landmark point " << lmk_point_id << std::endl; // First not defined point if (ftr_point_id == 0 && !pl_ftr->isFirstDefined()) // first point to line factor { - std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl; + // std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl; assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly."); // emplace factor emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id)); N_factors++; - std::cout << "factor added" << std::endl; + // std::cout << "factor added" << std::endl; } // Last not defined point else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined()) // last point to line factor { - std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl; + // std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl; assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly."); // emplace factor emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id)); N_factors++; - std::cout << "factor added" << std::endl; + // std::cout << "factor added" << std::endl; } // Defined point else // point to point factor { - std::cout << "point-point: landmark point " << lmk_point_id << std::endl; + // std::cout << "point-point: landmark point " << lmk_point_id << std::endl; //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl; //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl; //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl; emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id); N_factors++; - std::cout << "factor added" << std::endl; + // std::cout << "factor added" << std::endl; } // Next lmk point @@ -558,7 +558,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() lmk_point_id=pl_lmk->getNextValidId(lmk_point_id); } } - std::cout << N_factors << " factors established" << std::endl; + // std::cout << N_factors << " factors established" << std::endl; } void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, @@ -595,7 +595,7 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePtr _feature_ptr) { - WOLF_DEBUG("PTFP ", getName(), "::emplaceLandmark: "); + // WOLF_DEBUG("PTFP ", getName(), "::emplaceLandmark: "); //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl; //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl; assert(_feature_ptr->getType() == "POLYLINE 2D"); @@ -607,8 +607,8 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt Eigen::MatrixXs points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() + t_world_sensor_last_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose(); - std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl; - std::cout << "Landmark global points: " << std::endl << points_global << std::endl; + // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl; + // std::cout << "Landmark global points: " << std::endl << points_global << std::endl; // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl; // Create new landmark @@ -624,10 +624,10 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match) { WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: "); - lmk_match->print(); + //lmk_match->print(); auto pl_lmk = lmk_match->pl_landmark_; auto pl_ftr = lmk_match->pl_feature_; - bool print = true; + bool print = false; bool modified = false; assert(lmk_match->check() && "Bad landmark match provided"); @@ -797,7 +797,7 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly void ProcessorTrackerFeaturePolyline2D::advanceDerived() { - WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: "); + //WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: "); // remove landmark matches with last features if (last_ptr_ != nullptr) for (auto ftr : last_ptr_->getFeatureList()) @@ -813,20 +813,20 @@ void ProcessorTrackerFeaturePolyline2D::advanceDerived() "any linked feature in untracked_features_incoming_"); // remove untracked features - WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_"); + //WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_"); untracked_features_last_.clear(); // move untracked features incoming to last untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_); - WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)"); - WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); + //WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)"); + //WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); ProcessorTrackerFeature::advanceDerived(); } void ProcessorTrackerFeaturePolyline2D::resetDerived() { - WOLF_DEBUG("PTFP ", getName(), "::resetDerived: "); + //WOLF_DEBUG("PTFP ", getName(), "::resetDerived: "); // remove landmark matches with last features if (last_ptr_ != nullptr) for (auto ftr : last_ptr_->getFeatureList()) @@ -842,20 +842,20 @@ void ProcessorTrackerFeaturePolyline2D::resetDerived() "any linked feature in untracked_features_incoming_"); // remove untracked features - WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_"); + //WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_"); untracked_features_last_.clear(); // move untracked features incoming to last untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_); - WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)"); - WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); + //WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)"); + //WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); ProcessorTrackerFeature::resetDerived(); } void ProcessorTrackerFeaturePolyline2D::preProcess() { - WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: "); + //WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: "); // Extract all polylines from incoming_ // TODO: sort corners by bearing std::list<laserscanutils::Polyline> polylines; @@ -877,13 +877,13 @@ void ProcessorTrackerFeaturePolyline2D::preProcess() if (last_ptr_ != nullptr && incoming_ptr_ != nullptr) computeTransformations(); - WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features"); - WOLF_DEBUG_COND(last_ptr_!=nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); + //WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features"); + //WOLF_DEBUG_COND(last_ptr_!=nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); } void ProcessorTrackerFeaturePolyline2D::postProcess() { - WOLF_DEBUG("PTFP ", getName(), "::postProcess: "); + //WOLF_DEBUG("PTFP ", getName(), "::postProcess: "); // Try to close & classify modified landmarks while (!modified_lmks_.empty()) { @@ -896,7 +896,7 @@ void ProcessorTrackerFeaturePolyline2D::postProcess() if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4) lmk_ptr->tryClassify(params_tracker_feature_polyline_->class_position_error_th, params_tracker_feature_polyline_->polyline_classes); // try close - if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 3 || lmk_ptr->getNPoints() >= 4)) + if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 4 || lmk_ptr->getNPoints() >= 5)) lmk_ptr->tryClose(params_tracker_feature_polyline_->class_position_error_th); } @@ -927,8 +927,8 @@ void ProcessorTrackerFeaturePolyline2D::postProcess() params_tracker_feature_polyline_->match_landmark_overlap_n_min, params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min); } - WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features"); - WOLF_DEBUG_COND(last_ptr_!=nullptr,"PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); + //WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features"); + //WOLF_DEBUG_COND(last_ptr_!=nullptr,"PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); } void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, @@ -936,7 +936,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline const FeaturePolyline2DPtr& _ftr_I, const Eigen::Matrix3s& _T_last_incoming_prior) const { - WOLF_DEBUG("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id()); + //WOLF_DEBUG("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id()); MatchPolyline2DPtr best_match = nullptr; @@ -1032,8 +1032,8 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline // store in list ftr_matches[ftr_match->normalized_score_] = ftr_match; - WOLF_DEBUG("match stored:"); - ftr_match->print(); + //WOLF_DEBUG("match stored:"); + //ftr_match->print(); assert(ftr_match->check() && "tryMatchWithFeature: wrong match"); } @@ -1044,7 +1044,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const { - WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: "); + //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: "); MatchPolyline2DPtr best_match = nullptr; @@ -1113,7 +1113,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli //valid match if (best_match != nullptr) { - WOLF_DEBUG("match found!"); + //WOLF_DEBUG("match found!"); auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>(); // landmark