diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index 5cdfc2f764851b07de5d481751f18bacb0806595..0ceaa6cb15961743a7e603a2940ad8ac33824e60 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -34,7 +34,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP // return 0; // TRACK FEATURES ================================================== - if (_features_in.empty()) + if (!_features_in.empty()) { // prior transformations Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity()); @@ -51,7 +51,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); FeatureMatchPolyline2DScalarMap best_matches; - WOLF_INFO("----tracking feature incoming ", pl_incoming->id(), " with last features"); + WOLF_DEBUG("----tracking feature incoming ", pl_incoming->id(), " with last features"); // Check matching with all features in last // Store all matches consistent with T_last_incoming_prior in best_matches sorted by normalized score @@ -61,8 +61,6 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP pl_incoming, T_last_incoming_prior); - std::cout << best_matches.size() << " matches with features last\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++ ) { @@ -142,7 +140,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP // match for this feature has been found matched = true; - WOLF_WARN("Tracked feature"); + WOLF_DEBUG("Tracked feature"); //best_ftr_match->print(); break; } @@ -164,13 +162,13 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP T_sensor_world_incoming.topLeftCorner(2,2) = R_sensor_world_incoming_; T_sensor_world_incoming.topRightCorner(2,1) = t_sensor_world_incoming_; // iterate over all untracked_features_incoming_ looking for existing landmarks - ftr_it = untracked_features_incoming_.begin(); + auto ftr_it = untracked_features_incoming_.begin(); while (ftr_it != untracked_features_incoming_.end()) { auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); LandmarkMatchPolyline2DScalarMap best_lmk_matches; - WOLF_INFO("----tracking feature incoming ", pl_incoming->id(), " with landmarks"); + WOLF_DEBUG("----tracking feature incoming ", pl_incoming->id(), " with landmarks"); // SEARCH: Check matching with all existing landmarks for (auto lmk : getProblem()->getMap()->getLandmarkList()) @@ -185,8 +183,8 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP // store best match (highest score) in map landmark_match_map_[pl_incoming] = best_lmk_matches.rbegin()->second; - WOLF_INFO("Tracked landmark"); - landmark_match_map_[pl_incoming]->print(); + WOLF_DEBUG("Tracked landmark"); + //landmark_match_map_[pl_incoming]->print(); // store landmark candidates to be merged if (best_lmk_matches.size() > 1) @@ -280,7 +278,7 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr // not succeed if (lmk_matches.empty()) { - WOLF_WARN("correctFeatureDrift: after retrying tryMatchWithLandmark with concatenated transformation didn't success, trying with SLAM transformation..", "last feature ", _last_feature->id(), " incoming feature ", _incoming_feature->id(), " landmark ", pl_lmk->id()); + WOLF_DEBUG("correctFeatureDrift: after retrying tryMatchWithLandmark with concatenated transformation didn't success, trying with SLAM transformation..", "last feature ", _last_feature->id(), " incoming feature ", _incoming_feature->id(), " landmark ", pl_lmk->id()); // try with SLAM transformation Eigen::Matrix3s T_incoming_map_prior(Eigen::Matrix3s::Identity()); @@ -983,18 +981,13 @@ void ProcessorTrackerFeaturePolyline2D::preProcess() auto points = pl.points_; points.bottomRows(1) = Eigen::MatrixXs::Ones(1,points.cols()); untracked_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(points, pl.covs_, pl.first_defined_, pl.last_defined_)); - WOLF_INFO("new polyline in untracked_features_incoming_: ",untracked_features_incoming_.back()->id()); + WOLF_DEBUG("new polyline in untracked_features_incoming_: ",untracked_features_incoming_.back()->id()); } // compute transformations if (last_ptr_ != nullptr && incoming_ptr_ != nullptr) computeTransformations(); - std::cout << "untracked_features_incoming: "; - for (auto ftr : untracked_features_incoming_) - std::cout << ftr->id() << " "; - std::cout << std::endl; - //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_)"); } @@ -1176,7 +1169,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const { - WOLF_INFO("tryMatchWithLandmark: feature ", _feat_ptr->id(), " landmark ", _lmk_ptr->id()); + WOLF_DEBUG("tryMatchWithLandmark: feature ", _feat_ptr->id(), " landmark ", _lmk_ptr->id()); MatchPolyline2DPtr best_match = nullptr;