diff --git a/include/laser/processor/polyline_2D_utils.h b/include/laser/processor/polyline_2D_utils.h
index 62be7c00d128074128eeec92445a83846ef32384..769168d25f3d901e17e8634a0e36be7f348fbef1 100644
--- a/include/laser/processor/polyline_2D_utils.h
+++ b/include/laser/processor/polyline_2D_utils.h
@@ -71,8 +71,8 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
const Scalar& max_sq_error,
const Scalar& min_overlapping_dist,
- const int& min_N_overlapped,
- const int& min_N_defined_overlapped,
+ int min_N_overlapped,
+ int min_N_defined_overlapped,
bool both_exceeding_A_allowed_ = true,
bool both_exceeding_B_allowed_ = true);
diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 9ed7379c50cd0fe484e6b9e4970fd72f72bf4dab..e507d9a82b0e48da52b1272dfe6fbfe0ec048d63 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -252,6 +252,16 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
{
return untracked_features_last_;
}
+
+ const FeatureBasePtrList& getIncomingNewFeatures() const
+ {
+ return untracked_features_incoming_;
+ }
+
+ const FeatureBasePtrList& getIncomingKnownFeatures() const
+ {
+ return known_features_incoming_;
+ }
};
} /* namespace wolf */
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
index 0b2bb446741b79c23f5cc53ce731067fcdf37b02..034a6181b90835b425621e080d4ab372c6ad1ec1 100644
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ b/src/landmark/landmark_match_polyline_2D.cpp
@@ -172,6 +172,7 @@ 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 8aad5acc286b6bc2e2b1123a8edebd51421f8cac..f1dd2da92c28a0d125d4c66969d1bc0baa57f008 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -168,13 +168,13 @@ Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_au
MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
const Scalar& sq_error_max,
- const Scalar& overlap_dist_min,
- const int& overlap_N_min,
- const int& overlap_N_defined_min,
+ const Scalar& overlap_dist_ratio_min,
+ int overlap_N_min,
+ int overlap_N_defined_min,
bool both_exceeding_A_allowed_,
bool both_exceeding_B_allowed_)
{
- bool print = false;
+ bool print = true;
if (print)
{
std::cout << "computeBestSqDist...\n";
@@ -185,11 +185,22 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
std::cout << "\tsq_error_max: " << sq_error_max << std::endl;
- std::cout << "\toverlap_dist_min: " << overlap_dist_min << std::endl;
+ std::cout << "\toverlap_dist_ratio_min: " << overlap_dist_ratio_min << std::endl;
std::cout << "\toverlap_N_min: " << overlap_N_min << std::endl;
std::cout << "\toverlap_N_defined_min: " << overlap_N_defined_min << std::endl;
}
+ if (overlap_N_min < 2)
+ {
+ WOLF_WARN("overlap_N_min should be at least 2! It is a sufficient condition (if enough overlapping distance ratio)");
+ overlap_N_min = 2;
+ }
+ if (overlap_N_defined_min < 1)
+ {
+ WOLF_WARN("overlap_N_defined_min should be at least 1! It is a sufficient condition");
+ overlap_N_defined_min = 1;
+ }
+
// ASSERTIONS if closed -> all points defined
assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
assert((_closed_B &&_first_defined_B && _last_defined_B) || !_closed_B);
@@ -215,10 +226,10 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
{
//std::cout << "Auto-calling switching A<->B...\n";
- MatchPolyline2DPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
+ best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
_points_A, _first_defined_A, _last_defined_A, _closed_A,
sq_error_max,
- overlap_dist_min,
+ overlap_dist_ratio_min,
overlap_N_min,
overlap_N_defined_min,
both_exceeding_B_allowed_,
@@ -309,54 +320,58 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
bool to_B_defined = (to_B != 0 || _first_defined_B) && (to_B != last_B || _last_defined_B); //((to_B != 0 && to_B != last_B) || _last_defined_B);
// SUFFICIENT CONDITIONS
- // COND 1: sufficient overlapping points
- // COND 2: sufficient overlapping defined points
- // COND 3: sufficient overlapping distance
+ // COND 1: sufficient overlapping defined points
+ // COND 2: sufficient overlapping points (if no defined points, min overlapping distance)
int N_defined_overlap = std::max(0,N_overlap - (from_A_defined && from_B_defined ? 0 : 1) - (to_A_defined && to_B_defined ? 0 : 1));
- Scalar overlap_dist;
- if (N_overlap <= 1)
- overlap_dist = 0;
- else
- {
- Eigen::Vector2s p_from_A = _points_A.col(from_A).head(2);
- Eigen::Vector2s p_to_A = _points_A.col(to_A).head(2);
- Eigen::Vector2s p_from_B = _points_B.col(from_B).head(2);
- Eigen::Vector2s p_to_B = _points_B.col(to_B).head(2);
- Eigen::Vector2s vA = p_to_A - p_from_A;
- Eigen::Vector2s vB = p_to_B - p_from_B;
-
- // THE MINIMUM OF ALL 6 possible overlap distances
- Eigen::Vector6s overlap_dists;
- // 1: fromA - toA
- overlap_dists(0) = vA.norm();
- // 2: fromB - toB
- overlap_dists(1) = vB.norm();
- // 3: fromA - projected(toB)
- overlap_dists(2) = (vA.normalized()).dot(p_to_B - p_from_A);
- // 4: fromB - projected(toA)
- overlap_dists(3) = (vB.normalized()).dot(p_to_A - p_from_B);
- // 5: projected(fromB) - toA
- overlap_dists(4) = (-vA.normalized()).dot(p_from_B - p_to_A);
- // 6: projected(fromA) - toB
- overlap_dists(5) = (-vB.normalized()).dot(p_from_A - p_to_B);
-
- //std::cout << "\tdist2.mean() = " << dist2.mean() << std::endl;
- //std::cout << "\tmax_sq_error - dist2.mean() = " << max_sq_error - dist2.mean() << std::endl;
- if (print)
- std::cout << "\toverlap_dists = " << overlap_dists.transpose() << std::endl;
- overlap_dist = overlap_dists.minCoeff();
- }
-
- // if none of the sufficient conditions hold, continue
- if ( N_overlap < overlap_N_min &&
- N_defined_overlap < overlap_N_defined_min &&
- N_defined_overlap <= 0 && N_overlap < 2 &&
- overlap_dist < overlap_dist_min)
- {
- if (print)
- std::cout << "\t\tnone of the sufficient conditions hold\n";
- continue;
- }
+ if ( N_defined_overlap < overlap_N_defined_min &&
+ N_overlap < overlap_N_min )
+ {
+ if (print)
+ std::cout << "\t\tnone of the sufficient conditions hold\n";
+ continue;
+ }
+ // additional overlapping distance contidion for 2 not defined points
+ if (N_defined_overlap == 0)
+ {
+ Scalar overlap_dist;
+ WOLF_WARN_COND(N_overlap != 2, "THIS SHOULDN'T HAPPEN. \nN_defined_overlap: ", N_defined_overlap, "\nN_overlap: ", N_overlap, "\noverlap_N_defined_min: ", overlap_N_defined_min, "\noverlap_N_min: ", overlap_N_min)
+ assert(N_overlap == 2);
+ Eigen::Vector2s p_from_A = _points_A.col(from_A).head(2);
+ Eigen::Vector2s p_to_A = _points_A.col(to_A).head(2);
+ Eigen::Vector2s p_from_B = _points_B.col(from_B).head(2);
+ Eigen::Vector2s p_to_B = _points_B.col(to_B).head(2);
+ Eigen::Vector2s vA = p_to_A - p_from_A;
+ Eigen::Vector2s vB = p_to_B - p_from_B;
+
+ // THE MINIMUM OF ALL 6 possible overlap distances
+ Eigen::Vector6s overlap_dists;
+ // 1: fromA - toA
+ overlap_dists(0) = vA.norm();
+ // 2: fromB - toB
+ overlap_dists(1) = vB.norm();
+ // 3: fromA - projected(toB)
+ overlap_dists(2) = std::abs((vA.normalized()).dot(p_to_B - p_from_A));
+ // 4: fromB - projected(toA)
+ overlap_dists(3) = std::abs((vB.normalized()).dot(p_to_A - p_from_B));
+ // 5: projected(fromB) - toA
+ overlap_dists(4) = std::abs((-vA.normalized()).dot(p_from_B - p_to_A));
+ // 6: projected(fromA) - toB
+ overlap_dists(5) = std::abs((-vB.normalized()).dot(p_from_A - p_to_B));
+
+ //std::cout << "\tdist2.mean() = " << dist2.mean() << std::endl;
+ //std::cout << "\tmax_sq_error - dist2.mean() = " << max_sq_error - dist2.mean() << std::endl;
+ if (print)
+ std::cout << "\toverlap_dists ratios = " << (overlap_dists/std::min(vA.norm(),vB.norm())).transpose() << std::endl;
+ overlap_dist = overlap_dists.minCoeff();
+
+ // if necessary overlapping condition does not hold, continue
+ if (overlap_dist/std::min(vA.norm(),vB.norm()) < overlap_dist_ratio_min)
+ {
+ if (print)
+ std::cout << "\t\toverlapping distance ratio condition does not hold with 0 defined points overlapped\n";
+ continue;
+ }
+ }
assert(N_overlap <= _points_B.cols());
assert(N_overlap <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlap can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
@@ -407,13 +422,14 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
best_match->from_B_id_= from_B;
best_match->to_B_id_= to_B;
best_match->normalized_score_ = normalized_score;
- best_match->overlap_dist_ = overlap_dist;
+ best_match->overlap_dist_ = 0;//overlap_dist;
if (print)
best_match->print();
}
}
+ std::cout << "returning...\n";
return best_match;
}
@@ -421,6 +437,7 @@ 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());
@@ -468,7 +485,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 b4bcdabceafb59835fd6a7852acb6fb4b4e4d115..b45dc4fda249d3d115d186cf9d6ae7bbf45b8648 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -48,7 +48,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
FeatureMatchPolyline2DScalarMap best_matches;
- // std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
+ std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
// Check matching with all features in last
// Store all matches consistent with T_last_incoming_prior in best_matches sorted by normalized score
@@ -114,7 +114,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
// if concatenation did not succees -> next ftr match
if (pl_lmk_match_incoming == nullptr)
{
- WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming-last + last-landmark could not be concatenated into incoming-landmark");
+ 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";
@@ -135,8 +135,9 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
// match for this feature has been found
matched = true;
- // std::cout << "Feature tracked!\n";
- // best_ftr_match->print();
+
+ std::cout << "Feature tracked!\n";
+ best_ftr_match->print();
break;
}
@@ -155,7 +156,9 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature)
{
- WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
+ 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!!!
/** After finding a valid feature match last-incoming, we have to check that the concatenated match
* incoming-landmark is also valid.
@@ -174,6 +177,9 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
auto lmk_inc_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_incoming_feature]);
auto pl_lmk = lmk_inc_match->pl_landmark_;
auto pl_inc = lmk_inc_match->pl_feature_;
+ assert(pl_lmk != nullptr);
+ assert(pl_inc != nullptr);
+ assert(lmk_inc_match != nullptr);
// WOLF_DEBUG("LANDMARK-INCOMING:");
// lmk_inc_match->print();
@@ -183,7 +189,7 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
// COMPLETE PARTIAL MATCH
if (!tryCompletePartialMatch(lmk_inc_match))
{
- WOLF_WARN("correctFeatureDrift: tryCompletePartialMatch didn't success, removing..", "last ", last_ptr_->id(), " incoming ", incoming_ptr_->id(), " landmark ", pl_lmk->id());
+ WOLF_WARN("correctFeatureDrift: tryCompletePartialMatch 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;
}
@@ -191,7 +197,7 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
// 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());
+ WOLF_WARN("correctFeatureDrift: match didn't pass the check, removing..", "last feature ", _last_feature->id(), " incoming feature ", _incoming_feature->id(), " landmark ", pl_lmk->id());
landmark_match_map_.erase(_incoming_feature);
return false;
}
@@ -204,25 +210,39 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
// RETRY LANDMARK-MATCH IF TOO MUCH ERROR
if ((lmk_inc_match->computeSqDistArray() >= 0.5).any())
{
- WOLF_WARN("too much error: ", lmk_inc_match->computeSqDistArray().transpose(), " retrying landmark match" );
+ WOLF_DEBUG("too much error: ", lmk_inc_match->computeSqDistArray().transpose(), " retrying match landmark ", pl_lmk->id(), " with feature ", _incoming_feature->id() );
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())
{
- 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;
+ 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
+ Eigen::Matrix3s T_incoming_map_prior(Eigen::Matrix3s::Identity());
+ T_incoming_map_prior.topLeftCorner(2,2) = R_sensor_world_incoming_;
+ T_incoming_map_prior.topRightCorner(2,1) = t_sensor_world_incoming_;
+ 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
- else
- {
- lmk_inc_match = lmk_matches.rbegin()->second;
- landmark_match_map_[_incoming_feature] = lmk_inc_match;
- }
+ lmk_inc_match = lmk_matches.rbegin()->second;
+ landmark_match_map_[_incoming_feature] = lmk_inc_match;
}
+ WOLF_WARN("Validated match:");
+ lmk_inc_match->print();
+
assert(lmk_inc_match->check() && "after correct drift, check not passed"); // checking partial match
return true;
@@ -308,7 +328,7 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features)
{
- WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
+ WOLF_INFO("PTFP ", getName(), "::processNew: ");
// PT::processNew() ==========================================================
unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures() and trackFeatures()
@@ -335,7 +355,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
for (auto lmk : getProblem()->getMap()->getLandmarkList())
// Check only polyline landmarks that hasn't been just emplaced
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
+ // Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by squared distance
tryMatchWithLandmark(best_lmk_matches, std::static_pointer_cast<LandmarkPolyline2D>(lmk), pl_ftr, T_sensor_world_last);
// EMPLACE: If not matched with any existing landmark: emplace a new one
@@ -371,6 +391,9 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
landmark_match_map_[pl_ftr] = new_lmk_match;
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();
}
// MATCH
else
@@ -378,6 +401,9 @@ 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();
+
// store landmark candidates to be merged
if (best_lmk_matches.size() > 1)
{
@@ -406,6 +432,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();
assert(landmark_match_map_.count(pl_incoming) == 0);
landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
ftr_it++;
@@ -438,7 +466,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma
// link detected features (only created in preProcess())
for (auto ftr : _features_out)
- ftr->link(_capture);
+ {
+ WOLF_INFO("detect new feature in last: ", ftr->id());
+ ftr->link(_capture);
+ }
// WOLF_DEBUG(_features_out.size(), " were provided");
@@ -614,7 +645,7 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr
LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
- // WOLF_DEBUG("PTFP ", getName(), "::emplaceLandmark: ");
+ WOLF_DEBUG("PTFP ", getName(), "::emplaceLandmark: ");
//std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
//std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
assert(_feature_ptr->getType() == "POLYLINE 2D");
@@ -626,9 +657,9 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt
Eigen::MatrixXs points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() +
t_world_sensor_last_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
- // std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- // std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
+ //std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
+ //std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
+ //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << " "<< polyline_ptr->isLastDefined() << std::endl;
// Create new landmark
return LandmarkBase::emplace<LandmarkPolyline2D>(getProblem()->getMap(),
@@ -642,11 +673,11 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt
bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match)
{
- WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
+ WOLF_INFO("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
//lmk_match->print();
auto pl_lmk = lmk_match->pl_landmark_;
auto pl_ftr = lmk_match->pl_feature_;
- bool print = false;
+ bool print = true;
bool modified = false;
assert(lmk_match->check() && "Bad landmark match provided");
@@ -656,13 +687,14 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
if (print)
{
- std::cout << "landmark points" << std::endl << pl_lmk->computePointsGlobal() << std::endl;
- std::cout << "points_global " << std::endl << points_global << std::endl;
+ std::cout << "------ modifying landmark: " << pl_lmk->id() << "matched with feature " << pl_ftr->id() << std::endl;
+ std::cout << "\tlandmark points" << std::endl << pl_lmk->computePointsGlobal() << std::endl;
+ std::cout << "\tfeature points_global " << std::endl << points_global << std::endl;
//std::cout << "points_global2 " << std::endl << points_global2 << std::endl;
- std::cout << "landmark points local" << std::endl << lmk_match->T_feature_landmark_ * pl_lmk->computePointsGlobal() << std::endl;
- std::cout << "feature points " << std::endl << pl_ftr->getPoints() << std::endl;
- std::cout << "feature local global" << std::endl << lmk_match->T_feature_landmark_ * points_global << std::endl;
- std::cout << "lmk_match->T_feature_landmark_" << std::endl << lmk_match->T_feature_landmark_ << std::endl;
+ std::cout << "\tlandmark points local" << std::endl << lmk_match->T_feature_landmark_ * pl_lmk->computePointsGlobal() << std::endl;
+ std::cout << "\tfeature points " << std::endl << pl_ftr->getPoints() << std::endl;
+ //std::cout << "\tfeature local global" << std::endl << lmk_match->T_feature_landmark_ * points_global << std::endl;
+ std::cout << "\tlmk_match->T_feature_landmark_" << std::endl << lmk_match->T_feature_landmark_ << std::endl;
}
// MODIFY LANDMARK
@@ -972,7 +1004,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
const FeaturePolyline2DPtr& _ftr_I,
const Eigen::Matrix3s& _T_last_incoming_prior) const
{
- //WOLF_DEBUG("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id());
+ WOLF_INFO("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id());
MatchPolyline2DPtr best_match = nullptr;
@@ -1037,6 +1069,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
//valid match
if (best_match != nullptr)
{
+ WOLF_INFO("match found:");
auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
// feature pointers
@@ -1068,7 +1101,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
// store in list
ftr_matches[ftr_match->normalized_score_] = ftr_match;
- //WOLF_DEBUG("match stored:");
+ WOLF_INFO("match stored:");
//ftr_match->print();
assert(ftr_match->check() && "tryMatchWithFeature: wrong match");
}
@@ -1080,7 +1113,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
const FeaturePolyline2DPtr& _feat_ptr,
const Eigen::Matrix3s& _T_feature_landmark_prior) const
{
- //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
+ WOLF_INFO("PTFP ", getName(), "::tryMatchWithLandmark: ");
MatchPolyline2DPtr best_match = nullptr;
@@ -1143,13 +1176,13 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
params_tracker_feature_polyline_->match_landmark_overlap_n_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
+ false);// exceeding points in both extremes not allowed for feature
}
//valid match
if (best_match != nullptr)
{
- //WOLF_DEBUG("match found!");
+ WOLF_INFO("match found!");
auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
// landmark
@@ -1179,9 +1212,10 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
// store in list
lmk_matches[lmk_match->normalized_score_]= lmk_match;
- //WOLF_DEBUG("match stored!");
+ WOLF_INFO("match stored!");
assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
}
+ WOLF_INFO("end!");
}
/* updateLandmarkMatch
@@ -1190,7 +1224,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
*/
bool ProcessorTrackerFeaturePolyline2D::updateLandmarkMatch(LandmarkMatchPolyline2DPtr& _lmk_match) const
{
- //WOLF_DEBUG("PTFP ", getName(), "::updateLandmarkMatch: ");
+ WOLF_INFO("PTFP ", getName(), "::updateLandmarkMatch: ");
auto pl_lmk = _lmk_match->pl_landmark_;
auto pl_ftr = _lmk_match->pl_feature_;
@@ -1313,26 +1347,28 @@ bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMat
if (lmk_match->getNDefinedPoints() > 0 && lmk_match->getNPoints() > 1)
{
//auto T_feature_landmark_prev = lmk_match->T_feature_landmark_;
+ auto prev_sqdist = lmk_match->computeSqDistArray();
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: ",
- // lmk_match->computeSqDistArray().transpose() );
+ WOLF_WARN_COND(lmk_match->computeSqDistArray().maxCoeff() > prev_sqdist.maxCoeff() + 1e-9,
+ "after tryUpdateMatchTransformation more error! ",
+ "\nprev:", prev_sqdist.transpose(),
+ "\nnow: ", lmk_match->computeSqDistArray().transpose());
return true;
}
- //WOLF_DEBUG("tryUpdateMatchTransformation: Not enough points: ",lmk_match->getNPoints(), " (should be > 1) or defined points: ", lmk_match->getNDefinedPoints(), " (should be > 0)");
+ //WOLF_WARN("tryUpdateMatchTransformation: Not enough points: ",lmk_match->getNPoints(), " (should be > 1) or defined points: ", lmk_match->getNDefinedPoints(), " (should be > 0)");
return false;
}
@@ -1344,9 +1380,9 @@ void ProcessorTrackerFeaturePolyline2D::computeTransformations()
//WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
Eigen::Vector3s vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp());
- //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations:",
- // "\n\tvehicle_pose_incoming: ", vehicle_pose_incoming.transpose(),
- // "\n\tvehicle_pose_last: ", vehicle_pose_last.transpose());
+ WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations:",
+ "\n\tvehicle_pose_incoming: ", vehicle_pose_incoming.transpose(), " at ", incoming_ptr_->getTimeStamp(),
+ "\n\tvehicle_pose_last: ", vehicle_pose_last.transpose(), " at ", last_ptr_->getTimeStamp());
// world_robot
Eigen::Matrix2s R_world_robot_incoming = Eigen::Rotation2Ds(vehicle_pose_incoming(2)).matrix();