Skip to content
Snippets Groups Projects
Commit 4b53de7f authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

additional parameter in computeBestSqDist

parent 07dbb86c
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
...@@ -72,7 +72,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir ...@@ -72,7 +72,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
const Scalar& max_sq_error, const Scalar& max_sq_error,
const Scalar& min_overlapping_dist, const Scalar& min_overlapping_dist,
const int& min_N_overlapped, 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, 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, const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
......
...@@ -170,7 +170,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir ...@@ -170,7 +170,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
const Scalar& sq_error_max, const Scalar& sq_error_max,
const Scalar& overlap_dist_min, const Scalar& overlap_dist_min,
const int& overlap_N_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; bool print = false;
if (print) if (print)
...@@ -218,7 +220,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir ...@@ -218,7 +220,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
sq_error_max, sq_error_max,
overlap_dist_min, overlap_dist_min,
overlap_N_min, overlap_N_min,
overlap_N_defined_min); overlap_N_defined_min,
both_exceeding_B_allowed_,
both_exceeding_A_allowed_);
// undo switching // undo switching
if (best_match != nullptr) if (best_match != nullptr)
{ {
...@@ -277,6 +281,12 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir ...@@ -277,6 +281,12 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
while (to_A > last_A && _closed_A) while (to_A > last_A && _closed_A)
to_A -= _points_A.cols(); 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) if (print)
{ {
std::cout << "\toffset " << offset << std::endl; std::cout << "\toffset " << offset << std::endl;
......
...@@ -1084,7 +1084,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli ...@@ -1084,7 +1084,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
1, 1,
params_tracker_feature_polyline_->match_landmark_overlap_dist_min, 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_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 else // not probabilistic match
{ {
...@@ -1103,7 +1105,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli ...@@ -1103,7 +1105,9 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
params_tracker_feature_polyline_->match_landmark_position_sq_norm_max, 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_dist_min,
params_tracker_feature_polyline_->match_landmark_overlap_n_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 //valid match
...@@ -1139,7 +1143,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli ...@@ -1139,7 +1143,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
// store in list // store in list
lmk_matches[lmk_match->normalized_score_]= lmk_match; lmk_matches[lmk_match->normalized_score_]= lmk_match;
WOLF_DEBUG("match stored!"); //WOLF_DEBUG("match stored!");
assert(lmk_match->check() && "tryMatchWithLandmark: wrong match"); assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
} }
} }
...@@ -1150,7 +1154,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli ...@@ -1150,7 +1154,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
*/ */
bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolyline2DPtr& _lmk_match) const 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_lmk = _lmk_match->pl_landmark_;
auto pl_ftr = _lmk_match->pl_feature_; auto pl_ftr = _lmk_match->pl_feature_;
...@@ -1180,12 +1184,12 @@ bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolylin ...@@ -1180,12 +1184,12 @@ bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolylin
LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeatureMatchPolyline2DPtr ftr_match, LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeatureMatchPolyline2DPtr ftr_match,
LandmarkMatchPolyline2DPtr lmk_match_last) const 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"); assert(lmk_match_last->check(false) && "input lmk_match_last wrong");
WOLF_DEBUG("INCOMING-LAST MATCH:"); //WOLF_DEBUG("INCOMING-LAST MATCH:");
ftr_match->print(); //ftr_match->print();
WOLF_DEBUG("LANDMARK-LAST MATCH:"); //WOLF_DEBUG("LANDMARK-LAST MATCH:");
lmk_match_last->print(); //lmk_match_last->print();
WOLF_WARN_COND((lmk_match_last->computeSqDistArray() >= 0.5).any(), WOLF_WARN_COND((lmk_match_last->computeSqDistArray() >= 0.5).any(),
"before concatenateFeatureLandmarkMatches too much error: ", "before concatenateFeatureLandmarkMatches too much error: ",
...@@ -1247,8 +1251,8 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature ...@@ -1247,8 +1251,8 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
assert(lmk_match_incoming->check(false) && "wrongly concatenated"); // check without partial match assert(lmk_match_incoming->check(false) && "wrongly concatenated"); // check without partial match
WOLF_DEBUG("LANDMARK-INCOMING MATCH:"); //WOLF_DEBUG("LANDMARK-INCOMING MATCH:");
lmk_match_incoming->print(); //lmk_match_incoming->print();
WOLF_WARN_COND((lmk_match_incoming->computeSqDistArray() >= 0.5).any(), WOLF_WARN_COND((lmk_match_incoming->computeSqDistArray() >= 0.5).any(),
"after concatenateFeatureLandmarkMatches too much error: ", "after concatenateFeatureLandmarkMatches too much error: ",
...@@ -1260,25 +1264,25 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature ...@@ -1260,25 +1264,25 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMatchPolyline2DPtr& lmk_match) const bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMatchPolyline2DPtr& lmk_match) const
{ {
WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: "); //WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: ");
assert(lmk_match->check()); assert(lmk_match->check());
if (lmk_match->getNDefinedPoints() > 0 && lmk_match->getNPoints() > 1) 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->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->pl_landmark_->computePointsGlobal(lmk_match->landmark_from_id_,lmk_match->landmark_to_id_),
lmk_match->fromDefined(), lmk_match->fromDefined(),
lmk_match->toDefined())); lmk_match->toDefined()));
WOLF_WARN_COND((T2pose2D(T_feature_landmark_prev)-T2pose2D(lmk_match->T_feature_landmark_)).head(2).norm() > 0.1 || // 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, // 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: ", // "tryUpdateMatchTransformation: lmk_match->T_feature_landmark_ changed significantly. \n\tPrev pose: ",
T2pose2D(T_feature_landmark_prev).transpose(), // T2pose2D(T_feature_landmark_prev).transpose(),
"\n\tUpdated pose:", // "\n\tUpdated pose:",
T2pose2D(lmk_match->T_feature_landmark_).transpose()); // T2pose2D(lmk_match->T_feature_landmark_).transpose());
WOLF_WARN_COND((lmk_match->computeSqDistArray() >= 0.5).any(), WOLF_WARN_COND((lmk_match->computeSqDistArray() >= 0.5).any(),
"after tryUpdateMatchTransformation too much error: ", "after tryUpdateMatchTransformation too much error: ",
...@@ -1286,7 +1290,7 @@ bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMat ...@@ -1286,7 +1290,7 @@ bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMat
return true; 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; return false;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment