From 49bcb23bbd05b8a8d74a9f95b056699b99986448 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 3 Sep 2019 14:47:03 +0200
Subject: [PATCH] removing wrong landmarks and bug fixed in merge landmarks
---
src/landmark/landmark_polyline_2D.cpp | 9 ++++---
src/processor/polyline_2D_utils.cpp | 2 +-
.../processor_tracker_feature_polyline_2D.cpp | 27 ++++++++++++++++---
3 files changed, 30 insertions(+), 8 deletions(-)
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 1265a9a03..b6e7418c3 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -304,9 +304,9 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
}
WOLF_DEBUG("tryClose landmark ",id()," point ids: ");
- for (auto id : getValidIds())
- std::cout << id << " ";
- std::cout << std::endl;
+ //for (auto id : getValidIds())
+ // std::cout << id << " ";
+ //std::cout << std::endl;
std::vector<std::pair<int, int> > overlapped_ids;
@@ -424,7 +424,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
WOLF_DEBUG("The not defined extremes are overlapped",
"\n\tlast_proj_2_first_def: ", last_proj_2_first_def,
"\n\tfirst_proj_2_first_def: ", first_proj_2_first_def);
- overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getLastId()));
+ overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getPrevValidId(getLastId())));
+ overlapped_ids.push_back(std::pair<int,int>(getLastId(), getNextValidId(getFirstId())));
}
}
}
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index 20c8af784..853593d55 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -707,7 +707,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
// Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..)
matches.push_back(match);
- std::cout << "match added to list of matches!\n";
+ //std::cout << "match added to list of matches!\n";
}
}
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 2fe2ccff6..8c3fc0424 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -1049,6 +1049,22 @@ void ProcessorTrackerFeaturePolyline2D::postProcess()
params_tracker_feature_polyline_->match_landmark_overlap_n_min,
params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min);
}
+
+ // Remove "wrong" landmarks
+ LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
+ for (auto lmk_it = lmk_list.begin(); lmk_it != lmk_list.end(); lmk_it++)
+ if ((*lmk_it)->getType() == "POLYLINE 2D")
+ {
+ auto points_map = std::static_pointer_cast<LandmarkPolyline2D>(*lmk_it)->getPointStatePtrMap();
+
+ for (auto point_pair = points_map.begin(); point_pair != points_map.end(); point_pair++)
+ if (point_pair->second->getState().maxCoeff() > 1e6 || point_pair->second->getState().minCoeff() < -1e6)
+ {
+ (*lmk_it)->remove();
+ break;
+ }
+ }
+
//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_)");
}
@@ -1267,9 +1283,12 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
assert(lmk_match->feature_from_id_ == 0 || lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1);
// store in list
- lmk_matches[lmk_match->normalized_score_]= lmk_match;
- WOLF_DEBUG("match stored!");
- assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
+ if (lmk_match->check())
+ {
+ lmk_matches[lmk_match->normalized_score_]= lmk_match;
+ WOLF_DEBUG("match stored!");
+ assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
+ }
}
}
@@ -1396,6 +1415,8 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMatchPolyline2DPtr& lmk_match) const
{
//WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: ");
+ if (!lmk_match->check())
+ return false;
assert(lmk_match->check());
--
GitLab