diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 56ccd7f01fce1ffd02cd535c8181ad4afacdb073..9ed7379c50cd0fe484e6b9e4970fd72f72bf4dab 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -67,6 +67,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
FeatureBasePtrList untracked_features_incoming_, untracked_features_last_;
LandmarkMatchPolyline2DMap landmark_match_map_;
LandmarkPolyline2DPtrList modified_lmks_;
+ std::set<LandmarkBasePtr> new_landmarks_;
std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
index add363ba36bf6a26c5191bfff801d867ff565ecb..0b2bb446741b79c23f5cc53ce731067fcdf37b02 100644
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ b/src/landmark/landmark_match_polyline_2D.cpp
@@ -45,7 +45,7 @@ bool LandmarkMatchPolyline2D::isDeprecated() const
{
assert(pl_landmark_ != nullptr);
- return landmark_version_ != pl_landmark_->getVersion() || pl_landmark_->getMergedInLandmark() != nullptr;
+ return pl_landmark_->isRemoving() || landmark_version_ != pl_landmark_->getVersion() || pl_landmark_->getMergedInLandmark() != nullptr;
}
bool LandmarkMatchPolyline2D::isPartialFront() const
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index ce4164acecde747c6249961c06a4dbf1138880e9..eb1d75fb5944b2df43b42460413d8c8902bc83a3 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -1000,6 +1000,7 @@ void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list,
// 2 by 2
LandmarkPolyline2DPtr remaining_lmk = _lmk_list.front();
_lmk_list.pop_front();
+ assert(!remaining_lmk->isRemoving());
LandmarkPolyline2DPtr lmk_1, lmk_2, merged_lmk;
int remaining_from, remaining_to, merged_from, merged_to;
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 4fea24612b2c726dcbf5bf05574210f155a26201..b4bcdabceafb59835fd6a7852acb6fb4b4e4d115 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -28,7 +28,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences)
{
- WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_in.size());
+ WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_in.size());
if (_features_in.empty())
return 0;
@@ -85,7 +85,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
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";
+ 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;
@@ -94,7 +94,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
// Landmark removed -> not valid match with landmark
else if (pl_lmk->isRemoving())
{
- //std::cout << "\t\t\tRemoved landmark, removing last feature " << pl_last->id() << " and removing landmark match\n";
+ 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;
@@ -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();
}
@@ -155,7 +155,7 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature)
{
- WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
+ WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
/** After finding a valid feature match last-incoming, we have to check that the concatenated match
* incoming-landmark is also valid.
@@ -183,14 +183,22 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
// COMPLETE PARTIAL MATCH
if (!tryCompletePartialMatch(lmk_inc_match))
{
- WOLF_WARN("correctFeatureDrift: tryCompletePartialMatch didn't success");
+ WOLF_WARN("correctFeatureDrift: tryCompletePartialMatch didn't success, removing..", "last ", last_ptr_->id(), " incoming ", incoming_ptr_->id(), " landmark ", pl_lmk->id());
+ landmark_match_map_.erase(_incoming_feature);
+ return false;
+ }
+
+ // FILTER OUT matches that does not pass check
+ if (!lmk_inc_match->check())
+ {
+ WOLF_WARN("correctFeatureDrift: match didn't pass the check, removing..", "last ", last_ptr_->id(), " incoming ", incoming_ptr_->id(), " landmark ", pl_lmk->id());
landmark_match_map_.erase(_incoming_feature);
return false;
}
// 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
@@ -203,7 +211,7 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
// not succeed
if (lmk_matches.empty())
{
- WOLF_WARN("correctFeatureDrift: tryMatchWithLandmark didn't success");
+ WOLF_WARN("correctFeatureDrift: tryMatchWithLandmark didn't success, removing..", "last ", last_ptr_->id(), " incoming ", incoming_ptr_->id(), " landmark ", pl_lmk->id());
landmark_match_map_.erase(_incoming_feature);
return false;
}
@@ -300,12 +308,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());
@@ -314,7 +322,6 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
// MATCH or EMPLACE LANDMARKS ==========================================================
// For each new last feature: Either emplace a landmark or match with an existent landmark
- LandmarkBasePtrList new_landmarks;
for (auto ftr : new_features_last_)
{
// not matched feature
@@ -327,7 +334,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
// 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" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end())
+ if (lmk->getType() == "POLYLINE 2D" && new_landmarks_.count(lmk) == 0)
// Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by difference from T_sensor_world_last
tryMatchWithLandmark(best_lmk_matches, std::static_pointer_cast<LandmarkPolyline2D>(lmk), pl_ftr, T_sensor_world_last);
@@ -358,9 +365,12 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
assert((new_lmk_match->computeSqDistArray() < Constants::EPS).all());
// Store in new_landmarks
- new_landmarks.push_back(new_lmk_ptr);
+ new_landmarks_.insert(new_lmk_ptr);
+
// store match in map
landmark_match_map_[pl_ftr] = new_lmk_match;
+
+ WOLF_DEBUG("New landmark ", new_lmk_ptr->id(), " created. Matching with feature ", pl_ftr->id());
}
// MATCH
else
@@ -417,7 +427,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out)
{
- WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", untracked_features_last_.size(), " in untracked_features_last_)");
+ WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", untracked_features_last_.size(), " in untracked_features_last_)");
// move all already created features (in preProcess())
_features_out = std::move(untracked_features_last_);
@@ -437,59 +447,66 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma
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());
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("Establishing factors for last feature: ", ftr->id(), " and landmark ", pl_lmk->id());
- // LANDMARK CHANGED: update match
- // WOLF_DEBUG("\tLandmark changed?");
+ // DEPRECATED MATCH: update match
if (lmk_match->isDeprecated())
{
- WOLF_DEBUG("\tLandmark changed or was merged, trying to update...");
+ WOLF_DEBUG("\tLandmark was changed, merged or removed, trying to update...");
// not successful update
if (!updateLandmarkMatch(lmk_match))
{
- WOLF_DEBUG("\tUpdate didn't succeess, removing feature...");
+ WOLF_DEBUG("\tUpdate didn't success, removing feature...");
// remove from match map
landmark_match_map_.erase(ftr);
// remove feature
ftr->remove();
continue;
}
+ else // landmark could be merged into another one, update the pointer
+ pl_lmk = lmk_match->pl_landmark_;
}
- // LANDMARK REMOVED: remove match and feature
- else
- {
- // WOLF_DEBUG("\tLandmark removed?");
- if (pl_lmk->isRemoving())
- {
- WOLF_DEBUG("\tLandmark was removed, removing feature...");
- // remove from match map
- landmark_match_map_.erase(ftr);
- // remove feature
- ftr->remove();
- continue;
- }
+ // CHECK
+ if (!lmk_match->check())
+ {
+ WOLF_DEBUG("\tMatch check not passed, removing feature...");
+ // remove from match map
+ landmark_match_map_.erase(ftr);
+ // remove feature
+ ftr->remove();
+ continue;
}
// MODIFY LANDMARK (only for not closed and not recently emplaced)
// WOLF_DEBUG("\tModifying..");
- if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
- // If modified, add lmk to modified_lmks_ list
+ if (!pl_lmk->isClosed() && new_landmarks_.count(pl_lmk) == 0)
+ {
+ // If modified, add lmk to modified_lmks_ list
if (modifyLandmarkAndMatch(lmk_match))
- modified_lmks_.push_back(pl_lmk);
+ modified_lmks_.push_back(pl_lmk);
+
+ //std::cout << "after modifyLandmarkAndMatchlandmark " << pl_lmk->id() << (pl_lmk->isClosed() ? " is closed" : " is new") << " matched with feature " << ftr->id() << std::endl;
+
+ 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");
+ }
+ //else
+ // std::cout << "landmark " << pl_lmk->id() << (pl_lmk->isClosed() ? " is closed" : " is new") << " matched with feature " << ftr->id() << std::endl;
// ESTABLISH FACTORS
// WOLF_DEBUG("\tEstablishing factors..");
@@ -549,6 +566,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
//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);
+ assert(pl_lmk->getHits() != 0);
N_factors++;
// std::cout << "factor added" << std::endl;
}
@@ -557,6 +575,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
if (ftr_point_id < pl_ftr->getNPoints()-1)
lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
}
+ WOLF_DEBUG("landmark ", pl_lmk->id(), " has now ", pl_lmk->getHits(), " hits");
}
// std::cout << N_factors << " factors established" << std::endl;
}
@@ -623,7 +642,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt
bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match)
{
- WOLF_DEBUG("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_;
@@ -786,7 +805,7 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
//if (print)
// lmk_match->print();
-//
+
// WOLF_ERROR_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
// "landmark not properly aligned with feature: ",
// lmk_match->computeSqDistArray().transpose() );
@@ -797,7 +816,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())
@@ -821,12 +840,15 @@ void ProcessorTrackerFeaturePolyline2D::advanceDerived()
//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_)");
+ // clear new_landmarks_
+ new_landmarks_.clear();
+
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())
@@ -850,6 +872,9 @@ void ProcessorTrackerFeaturePolyline2D::resetDerived()
//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_)");
+ // clear new_landmarks_
+ new_landmarks_.clear();
+
ProcessorTrackerFeature::resetDerived();
}
@@ -883,10 +908,11 @@ void ProcessorTrackerFeaturePolyline2D::preProcess()
void ProcessorTrackerFeaturePolyline2D::postProcess()
{
- //WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
+ WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
// Try to close & classify modified landmarks
while (!modified_lmks_.empty())
{
+ WOLF_DEBUG("classifying and closing modified landmarks");
auto lmk_ptr = modified_lmks_.front();
modified_lmks_.pop_front();
@@ -903,20 +929,30 @@ void ProcessorTrackerFeaturePolyline2D::postProcess()
// Try to merge landmarks
while (!merge_candidates_list_.empty())
{
+ WOLF_DEBUG("merging landmarks...");
// load next list of candidates
LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
merge_candidates_list_.pop_front();
// change already merged lmks with the corresponding remaining lmk
- for (auto&& lmk : merge_candidates)
- if (lmk->getMergedInLandmark() != nullptr)
- lmk = lmk->getMergedInLandmark();
+ auto lmk_it = merge_candidates.begin();
+ while (lmk_it != merge_candidates.end())
+ {
+ if ((*lmk_it)->isRemoving())
+ {
+ lmk_it = merge_candidates.erase(lmk_it);
+ continue;
+ }
+ if ((*lmk_it)->getMergedInLandmark() != nullptr)
+ (*lmk_it) = (*lmk_it)->getMergedInLandmark();
+ lmk_it++;
+ }
// remove repeated lmks
merge_candidates.sort();
merge_candidates.unique();
- if (merge_candidates.size() == 1)
+ if (merge_candidates.size() < 2)
continue;
// Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
@@ -1162,6 +1198,12 @@ bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolylin
// merged: update the pl_lmk pointer
if (pl_lmk->getMergedInLandmark() != nullptr)
pl_lmk = pl_lmk->getMergedInLandmark();
+ // removed: not valid match
+ else if (pl_lmk->isRemoving())
+ {
+ WOLF_DEBUG("updateLandmarkMatch: landmark was removed, returning false");
+ return false;
+ }
// try match with _feat_ptr again
LandmarkMatchPolyline2DScalarMap new_lmk_matches;
@@ -1191,9 +1233,9 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
//WOLF_DEBUG("LANDMARK-LAST MATCH:");
//lmk_match_last->print();
- WOLF_WARN_COND((lmk_match_last->computeSqDistArray() >= 0.5).any(),
- "before concatenateFeatureLandmarkMatches too much error: ",
- lmk_match_last->computeSqDistArray().transpose() );
+ //WOLF_WARN_COND((lmk_match_last->computeSqDistArray() >= 0.5).any(),
+ // "before concatenateFeatureLandmarkMatches too much error: ",
+ // lmk_match_last->computeSqDistArray().transpose() );
//assert((lmk_match_last->computeSqDistArray() < 0.5).all());
auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
@@ -1254,9 +1296,9 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
//WOLF_DEBUG("LANDMARK-INCOMING MATCH:");
//lmk_match_incoming->print();
- WOLF_WARN_COND((lmk_match_incoming->computeSqDistArray() >= 0.5).any(),
- "after concatenateFeatureLandmarkMatches too much error: ",
- lmk_match_incoming->computeSqDistArray().transpose() );
+ //WOLF_WARN_COND((lmk_match_incoming->computeSqDistArray() >= 0.5).any(),
+ // "after concatenateFeatureLandmarkMatches too much error: ",
+ // lmk_match_incoming->computeSqDistArray().transpose() );
//assert((lmk_match_incoming->computeSqDistArray() < 0.5).all());
return lmk_match_incoming;
@@ -1284,9 +1326,9 @@ bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMat
// "\n\tUpdated pose:",
// T2pose2D(lmk_match->T_feature_landmark_).transpose());
- WOLF_WARN_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
- "after tryUpdateMatchTransformation too much error: ",
- lmk_match->computeSqDistArray().transpose() );
+ //WOLF_WARN_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
+ // "after tryUpdateMatchTransformation too much error: ",
+ // lmk_match->computeSqDistArray().transpose() );
return true;
}