From 4b53de7f683f23c9bd020d02df4a27016ceabe01 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 10 Jul 2019 12:24:15 +0200
Subject: [PATCH] additional parameter in computeBestSqDist
---
include/laser/processor/polyline_2D_utils.h | 4 +-
src/processor/polyline_2D_utils.cpp | 14 +++++-
.../processor_tracker_feature_polyline_2D.cpp | 44 ++++++++++---------
3 files changed, 39 insertions(+), 23 deletions(-)
diff --git a/include/laser/processor/polyline_2D_utils.h b/include/laser/processor/polyline_2D_utils.h
index c48b12db8..62be7c00d 100644
--- a/include/laser/processor/polyline_2D_utils.h
+++ b/include/laser/processor/polyline_2D_utils.h
@@ -72,7 +72,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
const Scalar& max_sq_error,
const Scalar& min_overlapping_dist,
const int& min_N_overlapped,
- const int& min_N_defined_overlapped );
+ const int& min_N_defined_overlapped,
+ bool both_exceeding_A_allowed_ = true,
+ bool both_exceeding_B_allowed_ = true);
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/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index 5e0c7d5d9..696d2c0ac 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -170,7 +170,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
const Scalar& sq_error_max,
const Scalar& overlap_dist_min,
const int& overlap_N_min,
- const int& overlap_N_defined_min)
+ const int& overlap_N_defined_min,
+ bool both_exceeding_A_allowed_,
+ bool both_exceeding_B_allowed_)
{
bool print = false;
if (print)
@@ -218,7 +220,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
sq_error_max,
overlap_dist_min,
overlap_N_min,
- overlap_N_defined_min);
+ overlap_N_defined_min,
+ both_exceeding_B_allowed_,
+ both_exceeding_A_allowed_);
// undo switching
if (best_match != nullptr)
{
@@ -277,6 +281,12 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
while (to_A > last_A && _closed_A)
to_A -= _points_A.cols();
+ // exceeding points for both extremes allowed?
+ if (!both_exceeding_A_allowed_ && from_A != 0 && to_A != last_A)
+ continue;
+ if (!both_exceeding_B_allowed_ && from_B != 0 && to_B != last_B)
+ continue;
+
if (print)
{
std::cout << "\toffset " << offset << std::endl;
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index b8d1331f8..4ac02519c 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -1084,7 +1084,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
1,
params_tracker_feature_polyline_->match_landmark_overlap_dist_min,
params_tracker_feature_polyline_->match_landmark_overlap_n_min,
- params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min);
+ params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min,
+ true, // partial match lmk allowed
+ false);// partial match ftr allowed
}
else // not probabilistic match
{
@@ -1103,7 +1105,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
params_tracker_feature_polyline_->match_landmark_position_sq_norm_max,
params_tracker_feature_polyline_->match_landmark_overlap_dist_min,
params_tracker_feature_polyline_->match_landmark_overlap_n_min,
- params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min);
+ params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min,
+ true, // exceeding points in both extremes allowed for landmark
+ false);// exceeding points in both extremes allowed for feature
}
//valid match
@@ -1139,7 +1143,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
// store in list
lmk_matches[lmk_match->normalized_score_]= lmk_match;
- WOLF_DEBUG("match stored!");
+ //WOLF_DEBUG("match stored!");
assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
}
}
@@ -1150,7 +1154,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
*/
bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolyline2DPtr& _lmk_match) const
{
- WOLF_DEBUG("PTFP ", getName(), "::updateLandmarkMatch: ");
+ //WOLF_DEBUG("PTFP ", getName(), "::updateLandmarkMatch: ");
auto pl_lmk = _lmk_match->pl_landmark_;
auto pl_ftr = _lmk_match->pl_feature_;
@@ -1180,12 +1184,12 @@ bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolylin
LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeatureMatchPolyline2DPtr ftr_match,
LandmarkMatchPolyline2DPtr lmk_match_last) const
{
- WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
+ //WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
assert(lmk_match_last->check(false) && "input lmk_match_last wrong");
- WOLF_DEBUG("INCOMING-LAST MATCH:");
- ftr_match->print();
- WOLF_DEBUG("LANDMARK-LAST MATCH:");
- lmk_match_last->print();
+ //WOLF_DEBUG("INCOMING-LAST MATCH:");
+ //ftr_match->print();
+ //WOLF_DEBUG("LANDMARK-LAST MATCH:");
+ //lmk_match_last->print();
WOLF_WARN_COND((lmk_match_last->computeSqDistArray() >= 0.5).any(),
"before concatenateFeatureLandmarkMatches too much error: ",
@@ -1247,8 +1251,8 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
assert(lmk_match_incoming->check(false) && "wrongly concatenated"); // check without partial match
- WOLF_DEBUG("LANDMARK-INCOMING MATCH:");
- lmk_match_incoming->print();
+ //WOLF_DEBUG("LANDMARK-INCOMING MATCH:");
+ //lmk_match_incoming->print();
WOLF_WARN_COND((lmk_match_incoming->computeSqDistArray() >= 0.5).any(),
"after concatenateFeatureLandmarkMatches too much error: ",
@@ -1260,25 +1264,25 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMatchPolyline2DPtr& lmk_match) const
{
- WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: ");
+ //WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: ");
assert(lmk_match->check());
if (lmk_match->getNDefinedPoints() > 0 && lmk_match->getNPoints() > 1)
{
- auto T_feature_landmark_prev = lmk_match->T_feature_landmark_;
+ //auto T_feature_landmark_prev = lmk_match->T_feature_landmark_;
lmk_match->T_feature_landmark_ = pose2T2D(computeRigidTrans(lmk_match->pl_feature_->getPoints().middleCols(lmk_match->feature_from_id_,lmk_match->getNPoints()),
lmk_match->pl_landmark_->computePointsGlobal(lmk_match->landmark_from_id_,lmk_match->landmark_to_id_),
lmk_match->fromDefined(),
lmk_match->toDefined()));
- WOLF_WARN_COND((T2pose2D(T_feature_landmark_prev)-T2pose2D(lmk_match->T_feature_landmark_)).head(2).norm() > 0.1 ||
- abs(pi2pi((T2pose2D(T_feature_landmark_prev)-T2pose2D(lmk_match->T_feature_landmark_))(2))) > 0.1,
- "tryUpdateMatchTransformation: lmk_match->T_feature_landmark_ changed significantly. \n\tPrev pose: ",
- T2pose2D(T_feature_landmark_prev).transpose(),
- "\n\tUpdated pose:",
- T2pose2D(lmk_match->T_feature_landmark_).transpose());
+// WOLF_WARN_COND((T2pose2D(T_feature_landmark_prev)-T2pose2D(lmk_match->T_feature_landmark_)).head(2).norm() > 0.1 ||
+// abs(pi2pi((T2pose2D(T_feature_landmark_prev)-T2pose2D(lmk_match->T_feature_landmark_))(2))) > 0.1,
+// "tryUpdateMatchTransformation: lmk_match->T_feature_landmark_ changed significantly. \n\tPrev pose: ",
+// T2pose2D(T_feature_landmark_prev).transpose(),
+// "\n\tUpdated pose:",
+// T2pose2D(lmk_match->T_feature_landmark_).transpose());
WOLF_WARN_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
"after tryUpdateMatchTransformation too much error: ",
@@ -1286,7 +1290,7 @@ bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMat
return true;
}
- WOLF_DEBUG("tryUpdateMatchTransformation: Not enough points: ",lmk_match->getNPoints(), " (should be > 1) or defined points: ", lmk_match->getNDefinedPoints(), " (should be > 0)");
+ //WOLF_DEBUG("tryUpdateMatchTransformation: Not enough points: ",lmk_match->getNPoints(), " (should be > 1) or defined points: ", lmk_match->getNDefinedPoints(), " (should be > 0)");
return false;
}
--
GitLab