diff --git a/include/laser/processor/polyline_2D_utils.h b/include/laser/processor/polyline_2D_utils.h
index 769168d25f3d901e17e8634a0e36be7f348fbef1..9a8aba9b2e6ec66215621de79a6fac8b5c84bff1 100644
--- a/include/laser/processor/polyline_2D_utils.h
+++ b/include/laser/processor/polyline_2D_utils.h
@@ -74,7 +74,8 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
int min_N_overlapped,
int min_N_defined_overlapped,
bool both_exceeding_A_allowed_ = true,
- bool both_exceeding_B_allowed_ = true);
+ bool both_exceeding_B_allowed_ = true,
+ bool print = false);
Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
index 034a6181b90835b425621e080d4ab372c6ad1ec1..0b2bb446741b79c23f5cc53ce731067fcdf37b02 100644
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ b/src/landmark/landmark_match_polyline_2D.cpp
@@ -172,7 +172,6 @@ bool LandmarkMatchPolyline2D::check(bool check_partial_match) const
Eigen::Array<Scalar,Eigen::Dynamic,1> LandmarkMatchPolyline2D::computeSqDistArray() const
{
- std::cout << "LandmarkMatchPolyline2D::computeSqDistArray\n";
assert(pl_feature_ != nullptr);
assert(pl_landmark_ != nullptr);
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index f1dd2da92c28a0d125d4c66969d1bc0baa57f008..20c8af784aec91143da632f590fe0a7a9bb1889b 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -172,9 +172,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
int overlap_N_min,
int overlap_N_defined_min,
bool both_exceeding_A_allowed_,
- bool both_exceeding_B_allowed_)
+ bool both_exceeding_B_allowed_,
+ bool print)
{
- bool print = true;
if (print)
{
std::cout << "computeBestSqDist...\n";
@@ -429,7 +429,6 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
}
}
- std::cout << "returning...\n";
return best_match;
}
@@ -437,7 +436,6 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
const int& last_A, const int& N_overlap )
{
- std::cout << "computeSqDist\n";
// POINT-POINT DISTANCES for all overlap points (in case of closed A in two parts)
// Fill with B points
Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlap).array());
@@ -485,7 +483,7 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
to_A_defined,
to_B_defined);
}
- std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+ //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
return dist2;
}
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index b45dc4fda249d3d115d186cf9d6ae7bbf45b8648..5cdfc2f764851b07de5d481751f18bacb0806595 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -30,123 +30,186 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
{
WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_in.size());
- if (_features_in.empty())
- return 0;
-
- // prior transformations
- Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity());
- T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
- T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
-
- // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: untracked_features_incoming_)
- auto ftr_it = untracked_features_incoming_.begin();
-
- // iterate over all untracked_features_incoming_
- while (ftr_it != untracked_features_incoming_.end())
- {
- bool matched = false;
- auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- FeatureMatchPolyline2DScalarMap best_matches;
+ //
+ // return 0;
+
+ // TRACK FEATURES ==================================================
+ if (_features_in.empty())
+ {
+ // prior transformations
+ Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity());
+ T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
+ T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
+
+ // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: untracked_features_incoming_)
+ auto ftr_it = untracked_features_incoming_.begin();
+
+ // iterate over all untracked_features_incoming_ looking for last features in _feature_in
+ while (ftr_it != untracked_features_incoming_.end())
+ {
+ bool matched = false;
+ auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+ FeatureMatchPolyline2DScalarMap best_matches;
- std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
+ WOLF_INFO("----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
- for (auto ftr_last_ : _features_in)
- tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior);
+ // 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 << 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++ )
- {
- auto best_ftr_match = best_ftr_match_pair_it->second;
- auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_);
+ // 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++ )
+ {
+ auto best_ftr_match = best_ftr_match_pair_it->second;
+ auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_);
+
+ //std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << " with score " << best_ftr_match->normalized_score_ << std::endl;
+
+ // VALIDATE LAST-LANDMARK --------------------------------------------------------------
+ // only if last feature not recently emplaced in processNew
+ if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
+ {
+ // Removed match
+ if(landmark_match_map_.count(pl_last) == 0)
+ {
+ std::cout << "\t\tremoved feature last\n";
+ continue;
+ }
+
+ auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
+
+ // Landmark changed or merged -> update match with last feature
+ if (landmark_match_map_[pl_last]->isDeprecated())
+ {
+ //std::cout << "\t\tupdating match last-landmark...\n";
+ if (!updateLandmarkMatch(landmark_match_map_[pl_last]))
+ {
+ // not successful update -> remove match and feature
+ WOLF_WARN("Not success: removing last feature ", pl_last->id(), " and removing landmark match");
+ landmark_match_map_.erase(pl_last);
+ pl_last->remove();
+ continue;
+ }
+ }
+ // Landmark removed -> not valid match with landmark
+ else if (pl_lmk->isRemoving())
+ {
+ WOLF_WARN("Removed landmark ", pl_lmk->id(), " removing last feature ", pl_last->id(), " and removing landmark match");
+ landmark_match_map_.erase(pl_last);
+ pl_last->remove();
+ continue;
+ }
+
+ }
+ //std::cout << "\t\tlast-landmark still valid...\n";
+
+ // INCOMING-LANDMARK --------------------------------------------------------------
+ // only if there is a landmark, so if last feature is not in new_features_last_
+ if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
+ {
+ // try to concatenate landmark match
+ auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(best_ftr_match,
+ landmark_match_map_[pl_last]);
+
+ // if concatenation did not succees -> next ftr match
+ if (pl_lmk_match_incoming == nullptr)
+ {
+ WOLF_WARN("PTFP ", getName(), "::trackFeatures: incoming-last + last-landmark could not be concatenated into incoming-landmark");
+ continue;
+ }
+ //std::cout << "\t\tlast-incoming valid, storing...\n";
+
+ // Add the incoming match to landmark_match_map
+ landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
+ }
+
+ // STORE INCOMING-LAST --------------------------------------------------------------
+ // add best match to feature-feature correspondences
+ _feature_correspondences[pl_incoming] = best_ftr_match;
+
+ // add feature to list of tracked features
+ _features_out.push_back(pl_incoming);
+
+ // link the incoming eature with the capture (since it was just created in preProcess())
+ pl_incoming->link(_capture);
+
+ // match for this feature has been found
+ matched = true;
+
+ WOLF_WARN("Tracked feature");
+ //best_ftr_match->print();
+ break;
+ }
- //std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << " with score " << best_ftr_match->normalized_score_ << std::endl;
+ // next feature
+ if (matched)
+ ftr_it = untracked_features_incoming_.erase(ftr_it); // remove feature from untracked features list
+ else
+ ftr_it++;
+ }
+ }
+
+ // TESTING ********************************************************
+ // TRACK FEATURES ==================================================
+ if (!getProblem()->getMap()->getLandmarkList().empty())
+ {
+ // prior transformations
+ Eigen::Matrix3s T_sensor_world_incoming(Eigen::Matrix3s::Identity());
+ 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();
+ while (ftr_it != untracked_features_incoming_.end())
+ {
+ auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+ LandmarkMatchPolyline2DScalarMap best_lmk_matches;
- // VALIDATE LAST-LANDMARK --------------------------------------------------------------
- // only if last feature not recently emplaced in processNew
- if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
- {
- // Removed match
- if(landmark_match_map_.count(pl_last) == 0)
- {
- //std::cout << "\t\tremoved feature last\n";
- continue;
- }
-
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
-
- // Landmark changed or merged -> update match with last feature
- if (landmark_match_map_[pl_last]->isDeprecated())
- {
- //std::cout << "\t\tupdating match last-landmark...\n";
- if (!updateLandmarkMatch(landmark_match_map_[pl_last]))
- {
- // not successful update -> remove match and feature
- std::cout << "\t\t\tNot success: removing last feature " << pl_last->id() << " and removing landmark match\n";
- landmark_match_map_.erase(pl_last);
- pl_last->remove();
- continue;
- }
- }
- // Landmark removed -> not valid match with landmark
- else if (pl_lmk->isRemoving())
- {
- std::cout << "\t\t\tRemoved landmark " << pl_lmk->id() << " removing last feature " << pl_last->id() << " and removing landmark match\n";
- landmark_match_map_.erase(pl_last);
- pl_last->remove();
- continue;
- }
+ WOLF_INFO("----tracking feature incoming ", pl_incoming->id(), " with landmarks");
- }
- //std::cout << "\t\tlast-landmark still valid...\n";
+ // SEARCH: Check matching with all existing landmarks
+ for (auto lmk : getProblem()->getMap()->getLandmarkList())
+ // Check only polyline landmarks that hasn't been just emplaced
+ if (lmk->getType() == "POLYLINE 2D")
+ // Store all matches consistent with T_sensor_world_incoming in best_lmk_matches sorted by squared distance
+ tryMatchWithLandmark(best_lmk_matches, std::static_pointer_cast<LandmarkPolyline2D>(lmk), pl_incoming, T_sensor_world_incoming);
- // INCOMING-LANDMARK --------------------------------------------------------------
- // only if there is a landmark, so if last feature is not in new_features_last_
- if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
- {
- // try to concatenate landmark match
- auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(best_ftr_match,
- landmark_match_map_[pl_last]);
-
- // if concatenation did not succees -> next ftr match
- if (pl_lmk_match_incoming == nullptr)
- {
- WOLF_WARN("PTFP ", getName(), "::trackFeatures: incoming-last + last-landmark could not be concatenated into incoming-landmark");
- continue;
- }
- //std::cout << "\t\tlast-incoming valid, storing...\n";
-
- // Add the incoming match to landmark_match_map
- landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
- }
-
- // STORE INCOMING-LAST --------------------------------------------------------------
- // add best match to feature-feature correspondences
- _feature_correspondences[pl_incoming] = best_ftr_match;
+ // Matches!
+ if (!best_lmk_matches.empty())
+ {
+ // store best match (highest score) in map
+ landmark_match_map_[pl_incoming] = best_lmk_matches.rbegin()->second;
- // add feature to list of tracked features
- _features_out.push_back(pl_incoming);
+ WOLF_INFO("Tracked landmark");
+ landmark_match_map_[pl_incoming]->print();
- // link the incoming eature with the capture (since it was just created in preProcess())
- pl_incoming->link(_capture);
+ // store landmark candidates to be merged
+ if (best_lmk_matches.size() > 1)
+ {
+ LandmarkPolyline2DPtrList merge_candidates;
+ for (auto lmk_match_pair_it : best_lmk_matches)
+ merge_candidates.push_back(lmk_match_pair_it.second->pl_landmark_);
+ merge_candidates_list_.push_back(merge_candidates);
+ }
- // match for this feature has been found
- matched = true;
+ // link the incoming feature with the capture (since it was just created in preProcess())
+ pl_incoming->link(_capture);
- std::cout << "Feature tracked!\n";
- best_ftr_match->print();
- break;
- }
+ // new track
+ track_matrix_.newTrack(pl_incoming);
- // next feature
- if (matched)
- ftr_it = untracked_features_incoming_.erase(ftr_it); // remove feature from untracked features list
- else
- ftr_it++;
- }
+ // remove feature from untracked features list
+ ftr_it = untracked_features_incoming_.erase(ftr_it);
+ }
+ else
+ ftr_it++;
+ }
+ }
WOLF_DEBUG(_feature_correspondences.size(), " features tracked");
return _feature_correspondences.size();
@@ -156,9 +219,8 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature)
{
- WOLF_WARN("PTFP ", getName(), "::correctFeatureDrift: incoming feature ", _incoming_feature->id());
- WOLF_WARN("PTFP ", getName(), "::correctFeatureDrift: last feature ", _last_feature->id());
- //FIXME: sometimes last_feature is nullptr!!!
+ WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: incoming feature ", _incoming_feature->id());
+ WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: last feature ", _last_feature->id());
/** After finding a valid feature match last-incoming, we have to check that the concatenated match
* incoming-landmark is also valid.
@@ -214,11 +276,10 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
LandmarkMatchPolyline2DScalarMap lmk_matches;
// try with concatenated transformation
tryMatchWithLandmark(lmk_matches, pl_lmk, pl_inc, lmk_inc_match->T_feature_landmark_);
- std::cout << "again here...\n";
+
// not succeed
if (lmk_matches.empty())
{
- std::cout << "here...!?\n";
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());
// try with SLAM transformation
@@ -228,20 +289,18 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
tryMatchWithLandmark(lmk_matches, pl_lmk, pl_inc, T_incoming_map_prior);
if (lmk_matches.empty())
{
- std::cout << "here...?\n";
WOLF_WARN("correctFeatureDrift: after retrying tryMatchWithLandmark didn't success, removing..", "last feature ", _last_feature->id(), " incoming feature ", _incoming_feature->id(), " landmark ", pl_lmk->id());
landmark_match_map_.erase(_incoming_feature);
return false;
}
}
- std::cout << "and here...\n";
// override if succed
lmk_inc_match = lmk_matches.rbegin()->second;
landmark_match_map_[_incoming_feature] = lmk_inc_match;
}
- WOLF_WARN("Validated match:");
- lmk_inc_match->print();
+ WOLF_DEBUG("Validated match:");
+ //lmk_inc_match->print();
assert(lmk_inc_match->check() && "after correct drift, check not passed"); // checking partial match
@@ -256,14 +315,14 @@ bool ProcessorTrackerFeaturePolyline2D::tryCompletePartialMatch(LandmarkMatchPol
// FRONT POINTS
while (lmk_inc_match->isPartialFront())
{
- WOLF_DEBUG("Partial match in front...");
+ //WOLF_INFO("Partial match in front...");
+ //lmk_inc_match->print();
auto exceeding_inc = lmk_inc_match->feature_from_id_-1;
auto exceeding_lmk = pl_lmk->getPrevValidId(lmk_inc_match->landmark_from_id_);
// check good overlap
- Eigen::Vector3s lmk_point_expected(Eigen::Vector3s::Ones());
- lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
+ Eigen::Vector3s lmk_point_expected = lmk_inc_match->T_feature_landmark_ * pl_lmk->computePointsGlobal(exceeding_lmk,exceeding_lmk);
Eigen::Vector3s ftr_point = pl_inc->getPoints().col(exceeding_inc);
Scalar sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
@@ -284,14 +343,14 @@ bool ProcessorTrackerFeaturePolyline2D::tryCompletePartialMatch(LandmarkMatchPol
// BACK POINTS
while (lmk_inc_match->isPartialBack())
{
- WOLF_DEBUG("Partial match in back...");
+ //WOLF_INFO("Partial match in back...");
+ //lmk_inc_match->print();
auto exceeding_inc = lmk_inc_match->feature_to_id_+1;
auto exceeding_lmk = pl_lmk->getNextValidId(lmk_inc_match->landmark_to_id_);
// check good overlap
- Eigen::Vector3s lmk_point_expected(Eigen::Vector3s::Ones());
- lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
+ Eigen::Vector3s lmk_point_expected = lmk_inc_match->T_feature_landmark_ * pl_lmk->computePointsGlobal(exceeding_lmk,exceeding_lmk);
Eigen::Vector3s ftr_point = pl_inc->getPoints().col(exceeding_inc);
Scalar sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
@@ -328,7 +387,7 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features)
{
- WOLF_INFO("PTFP ", getName(), "::processNew: ");
+ WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
// PT::processNew() ==========================================================
unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures() and trackFeatures()
@@ -392,8 +451,8 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
WOLF_DEBUG("New landmark ", new_lmk_ptr->id(), " created. Matching with feature ", pl_ftr->id());
- WOLF_WARN("New match, last with new landmark:");
- new_lmk_match->print();
+ //WOLF_WARN("New match, last with new landmark:");
+ //new_lmk_match->print();
}
// MATCH
else
@@ -401,8 +460,8 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
// store best match (highest score) in map
landmark_match_map_[pl_ftr] = best_lmk_matches.rbegin()->second;
- WOLF_WARN("New match, last with existing landmark:");
- landmark_match_map_[pl_ftr]->print();
+ //WOLF_DEBUG("New match, last with existing landmark:");
+ //landmark_match_map_[pl_ftr]->print();
// store landmark candidates to be merged
if (best_lmk_matches.size() > 1)
@@ -432,8 +491,8 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
// Matched
if (pl_lmk_match_incoming != nullptr)
{
- WOLF_WARN("New match, incoming with new landmark:");
- pl_lmk_match_incoming->print();
+ WOLF_DEBUG("New match, incoming with new landmark:");
+ //pl_lmk_match_incoming->print();
assert(landmark_match_map_.count(pl_incoming) == 0);
landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
ftr_it++;
@@ -466,10 +525,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma
// link detected features (only created in preProcess())
for (auto ftr : _features_out)
- {
- WOLF_INFO("detect new feature in last: ", ftr->id());
ftr->link(_capture);
- }
// WOLF_DEBUG(_features_out.size(), " were provided");
@@ -673,11 +729,11 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt
bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match)
{
- WOLF_INFO("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
+ WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
//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");
@@ -927,13 +983,18 @@ 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_DEBUG("new polyline detected: points\n",pl.points_);
+ WOLF_INFO("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_)");
}
@@ -1004,7 +1065,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
const FeaturePolyline2DPtr& _ftr_I,
const Eigen::Matrix3s& _T_last_incoming_prior) const
{
- WOLF_INFO("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id());
+ WOLF_DEBUG("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id());
MatchPolyline2DPtr best_match = nullptr;
@@ -1063,13 +1124,15 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
params_tracker_feature_polyline_->match_feature_position_sq_norm_max,
params_tracker_feature_polyline_->match_feature_overlap_dist_min,
params_tracker_feature_polyline_->match_feature_overlap_n_min,
- params_tracker_feature_polyline_->match_feature_overlap_n_defined_min);
+ params_tracker_feature_polyline_->match_feature_overlap_n_defined_min,
+ true,true, // exceeding points allowed for both features
+ false); //print
}
//valid match
if (best_match != nullptr)
{
- WOLF_INFO("match found:");
+ WOLF_DEBUG("match found:");
auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
// feature pointers
@@ -1101,7 +1164,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
// store in list
ftr_matches[ftr_match->normalized_score_] = ftr_match;
- WOLF_INFO("match stored:");
+ //WOLF_INFO("match stored:");
//ftr_match->print();
assert(ftr_match->check() && "tryMatchWithFeature: wrong match");
}
@@ -1113,7 +1176,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
const FeaturePolyline2DPtr& _feat_ptr,
const Eigen::Matrix3s& _T_feature_landmark_prior) const
{
- WOLF_INFO("PTFP ", getName(), "::tryMatchWithLandmark: ");
+ WOLF_INFO("tryMatchWithLandmark: feature ", _feat_ptr->id(), " landmark ", _lmk_ptr->id());
MatchPolyline2DPtr best_match = nullptr;
@@ -1182,7 +1245,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
//valid match
if (best_match != nullptr)
{
- WOLF_INFO("match found!");
+ WOLF_DEBUG("match found!");
auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
// landmark
@@ -1212,10 +1275,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
// store in list
lmk_matches[lmk_match->normalized_score_]= lmk_match;
- WOLF_INFO("match stored!");
+ WOLF_DEBUG("match stored!");
assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
}
- WOLF_INFO("end!");
}
/* updateLandmarkMatch
@@ -1224,7 +1286,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
*/
bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolyline2DPtr& _lmk_match) const
{
- WOLF_INFO("PTFP ", getName(), "::updateLandmarkMatch: ");
+ WOLF_DEBUG("PTFP ", getName(), "::updateLandmarkMatch: ");
auto pl_lmk = _lmk_match->pl_landmark_;
auto pl_ftr = _lmk_match->pl_feature_;