diff --git a/CMakeLists.txt b/CMakeLists.txt
index b6539a1bab14ae553a09626abd5611f802d157d3..be988462333dfc5b87949f6e3a7b4e5520c180e6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -416,8 +416,6 @@ SET(HDRS_PROCESSOR
include/base/processor/processor_tracker.h
include/base/processor/processor_gnss_fix.h
include/base/processor/processor_gnss_single_diff.h
- include/base/processor/processor_tracker_feature_polyline.h
- include/base/processor/polyline_2D_utils.h
)
SET(HDRS_SENSOR
include/base/sensor/sensor_base.h
@@ -570,8 +568,6 @@ SET(SRCS_PROCESSOR
src/processor/processor_tracker_landmark.cpp
src/processor/processor_gnss_fix.cpp
src/processor/processor_gnss_single_diff.cpp
- src/processor/processor_tracker_feature_polyline.cpp
- src/processor/polyline_2D_utils.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_camera.cpp
@@ -623,6 +619,8 @@ IF (laser_scan_utils_FOUND)
include/base/processor/processor_polyline.h
include/base/processor/processor_tracker_feature_corner.h
include/base/processor/processor_tracker_landmark_corner.h
+ include/base/processor/processor_tracker_feature_polyline.h
+ include/base/processor/polyline_2D_utils.h
)
SET(HDRS_SENSOR ${HDRS_SENSOR}
include/base/sensor/sensor_laser_2D.h
@@ -634,6 +632,8 @@ IF (laser_scan_utils_FOUND)
src/processor/processor_polyline.cpp
src/processor/processor_tracker_feature_corner.cpp
src/processor/processor_tracker_landmark_corner.cpp
+ src/processor/processor_tracker_feature_polyline.cpp
+ src/processor/polyline_2D_utils.cpp
)
SET(SRCS_SENSOR ${SRCS_SENSOR}
src/sensor/sensor_laser_2D.cpp
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h
index 9d4e04c541c5f7533465b25df2151abaa9772966..94ee780210fd4035bd19c16752c1a9bce03647ac 100644
--- a/include/base/processor/processor_tracker_feature_polyline.h
+++ b/include/base/processor/processor_tracker_feature_polyline.h
@@ -171,14 +171,24 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
*/
virtual void postProcess() override;
- void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const;
+ void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
+ const FeaturePolyline2DPtr& _ftr_last,
+ const FeaturePolyline2DPtr& _ftr_incoming,
+ const Eigen::Matrix3s& _T_last_incoming_prior) const;
- void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const;
+ void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
+ const LandmarkPolyline2DPtr& _lmk_ptr,
+ const FeaturePolyline2DPtr& _feat_ptr,
+ const Eigen::Matrix3s& _T_feature_landmark_prior) const;
- bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr);
+ bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const;
void computeTransformations();
+ LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
+ FeatureMatchPolyline2DPtr ftr_match_incoming_last,
+ LandmarkMatchPolyline2DPtr lmk_match_last) const;
+
public:
/// @brief Factory method
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index e6f7c9a7a5572b0c1e2deb4bab5c8addcfdf3ff6..25a84f85f66b46581078a5efb7d1f516b66151f6 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -357,16 +357,11 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
return best_match;
}
-
MatchPolyline2DList computeBestRigidTrans(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& max_sq_error )
{
- //std::cout << "computeBestRigidTrans...\n";
- //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points\n";
- //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points\n";
- //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
- //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
+ std::cout << "computeBestRigidTrans...\n";
// ASSERTIONS if closed -> all points defined
assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
@@ -386,6 +381,17 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
*/
MatchPolyline2DList matches;
+ auto N_defined_points_A = _points_A.cols() - (_first_defined_A ? 0 : 1) - (_last_defined_A ? 0 : 1);
+ auto N_defined_points_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
+
+ // Not enough defined points
+ if (N_defined_points_A <= 1 || N_defined_points_B <= 1 )
+ return matches;
+
+ std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points (" << N_defined_points_A << " defined)\n";
+ std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points (" << N_defined_points_B << " defined)\n";
+ std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
+ std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
// ----------------- Call switching A<->B
if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
@@ -400,7 +406,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
{
std::swap(match->from_A_id_, match->from_B_id_);
std::swap(match->to_A_id_, match->to_B_id_);
- match->T_A_B_ = match->T_A_B_.inverse();
+ match->T_A_B_ = match->T_A_B_.inverse().eval();
}
return matches;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index d53c7831991e86c35d8d09a0967724c9007d7242..39528ef4299444cc759e0dfa6c6bc6f64793ab40 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -54,7 +54,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
WOLF_DEBUG("PT: FIRST_TIME_WITH_PACK");
PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
+ //kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
+ kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -131,7 +132,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
WOLF_DEBUG("PT: RUNNING_WITH_PACK");
PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
- kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+ //kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+ kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 91275c9f4945a15d222e0acd4817e115163627b7..94bd88049193d7f272d74789c1e03b0d249d21e6 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -45,10 +45,18 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
}
// Append all new Features to the incoming Captures' list of Features
+ WOLF_DEBUG("PTF ", getName(), ": ", incoming_ptr_->getFeatureList().size(), " in incoming_ptr_)");
+ WOLF_DEBUG("PTF ", getName(), ": ", new_features_incoming_.size(), " in new_features_incoming_)");
incoming_ptr_->addFeatureList(new_features_incoming_);
+ WOLF_DEBUG("PTF ", getName(), ": ", incoming_ptr_->getFeatureList().size(), " in incoming_ptr_)");
+ WOLF_DEBUG("PTF ", getName(), ": ", new_features_incoming_.size(), " in new_features_incoming_)");
// Append all new Features to the last Captures' list of Features
+ WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+ WOLF_DEBUG("PTF ", getName(), ": ", new_features_last_.size(), " in new_features_last_)");
last_ptr_->addFeatureList(new_features_last_);
+ WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+ WOLF_DEBUG("PTF ", getName(), ": ", new_features_last_.size(), " in new_features_last_)");
// return the number of new features detected in \b last
return n;
@@ -63,6 +71,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
if (!last_ptr_ || last_ptr_->getFeatureList().empty())
{
+ WOLF_DEBUG("PTF ", getName(), "::processKnown: null or empty last capture, returning");
return 0;
}
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp
index 29c688b9333f8484a1c1e61708a87bb3518d9252..b230252ebe874e21568015ae8d3d25f114ed46a3 100644
--- a/src/processor/processor_tracker_feature_polyline.cpp
+++ b/src/processor/processor_tracker_feature_polyline.cpp
@@ -27,7 +27,7 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
FeatureBaseList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences)
{
- WOLF_DEBUG("PTF ", getName(), "::trackFeatures", _features_last_in.size());
+ WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size());
if (_features_last_in.empty())
return 0;
@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n";
- // There is a landmark_match (i.e. it has not been recently created in processNew)
+ // If it has not been created in processNew: check/update/remove landmark match
if(find(new_features_last_.begin(), new_features_last_.end(), pl_last) == new_features_last_.end())
{
// the match with this last_ feature was removed after any updateMatchWithLandmark
@@ -74,34 +74,27 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
continue;
}
- auto lmk_match_last = landmark_match_map_[pl_last];
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_);
+ auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
// Landmark changed or was merged -> Redo match with last feature
- if (lmk_match_last->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
+ if (landmark_match_map_[pl_last]->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
{
- std::cout << "\t\tupdating match\n";
- if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last))
+ std::cout << "\t\tupdating match...\n";
+ if (!updateMatchWithLandmark(landmark_match_map_[pl_last],pl_lmk,pl_last))
+ {
+ // not successful update -> remove match and feature
+ std::cout << "\t\t\tNot success: removing feature " << pl_last->id() << " and landmark match\n";
+ landmark_match_map_.erase(pl_last);
+ last_ptr_->getFeatureList().remove(pl_last);
continue;
+ }
}
// removed -> not valid match with landmark
else if (pl_lmk->isRemoving())
continue;
- // Add the match to landmark_match_map
- auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
-
- lmk_match_incoming->feature_from_id_ = best_ftr_match->feature_incoming_from_id_;
- lmk_match_incoming->feature_to_id_ = best_ftr_match->feature_incoming_to_id_;
- // Not adding new points nor checks (done in correctFeatureDrift)
- lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + best_ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
- lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_ + best_ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_;
- lmk_match_incoming->landmark_ptr_ = pl_lmk;
- lmk_match_incoming->landmark_version_ = pl_lmk->getVersion();
- lmk_match_incoming->normalized_score_ = best_ftr_match->normalized_score_;
- lmk_match_incoming->T_feature_landmark_ = best_ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
-
- landmark_match_map_[pl_incoming] = lmk_match_incoming;
+ // Add the incoming match to landmark_match_map
+ landmark_match_map_[pl_incoming] = concatenateFeatureLandmarkMatches(pl_incoming, best_ftr_match, landmark_match_map_[pl_last]);
}
// TRACK valid
@@ -133,7 +126,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature)
{
- WOLF_DEBUG("PTF ", getName(), "::correctFeatureDrift: ");
+ //WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
// If incoming observed new landmark points & landmark has points
// Check if match is still valid and update the match
// TODO
@@ -147,7 +140,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _
bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
{
- WOLF_DEBUG("PTF ", getName(), "::voteForKeyFrame: ");
+ //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: ");
// TODO
/* IDEAS:
* - Any feature incoming derived too much from projected Landmark
@@ -160,7 +153,7 @@ bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _max_features)
{
- WOLF_DEBUG("PTF ", getName(), "::processNew: ");
+ WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
WOLF_DEBUG("Processing ", n, " new last features");
@@ -171,6 +164,7 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
// For each new feature: Either create a landmark or match with an existent landmark
+ LandmarkBaseList new_landmarks;
for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n);
ftr_it != last_ptr_->getFeatureList().end();
ftr_it++)
@@ -180,9 +174,10 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
WOLF_DEBUG("Processing feature: ", pl_ftr->id());
// Check matching with all existing polyline landmarks
- // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
for (auto lmk : getProblem()->getMapPtr()->getLandmarkList())
- if (lmk->getType() == "POLYLINE 2D")
+ // Check for polylines landmarks that hasn't been just created
+ if (lmk->getType() == "POLYLINE 2D" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end())
+ // 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);
// If not matched with any existing landmark: create a new one
@@ -206,6 +201,8 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
// Add the new landmark to the map
getProblem()->addLandmark(new_lmk_ptr);
+ // Store in new_landmarks
+ new_landmarks.push_back(new_lmk_ptr);
// store match in map
landmark_match_map_[pl_ftr] = new_lmk_match;
}
@@ -225,22 +222,32 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
}
}
}
+
+ // store in landmark_match_map_ the incoming features
+ for (auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n);
+ ftr_it != incoming_ptr_->getFeatureList().end();
+ ftr_it++)
+ {
+ auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+ assert(matches_last_from_incoming_.find(pl_incoming) != matches_last_from_incoming_.end() && "last-incoming match not found");
+ auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[pl_incoming]);
+ assert(landmark_match_map_.find(pl_ftr_match->feature_ptr_) != landmark_match_map_.end() && "last-lmk match not found");
+ landmark_match_map_[pl_incoming] = concatenateFeatureLandmarkMatches(pl_incoming,
+ pl_ftr_match,
+ landmark_match_map_[pl_ftr_match->feature_ptr_]);
+ }
return n;
}
unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned int& _max_new_features,
FeatureBaseList& _features_last_out)
{
- WOLF_DEBUG("PTF ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
+ WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
int prev_size = _features_last_out.size();
if (_features_last_out == new_features_last_)
{
- WOLF_DEBUG("_features_last_out:", _features_last_out.size(), " before splice");
- WOLF_DEBUG("all_features_last_:", all_features_last_.size(), " before splice");
_features_last_out.splice(_features_last_out.end(), all_features_last_);
- WOLF_DEBUG("_features_last_out:", _features_last_out.size(), " after splice");
- WOLF_DEBUG("all_features_last_:", all_features_last_.size(), " after splice");
// 0 means unlimited new features
if (_max_new_features != 0 && _features_last_out.size() > prev_size + _max_new_features)
@@ -260,7 +267,7 @@ unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned i
void ProcessorTrackerFeaturePolyline::establishConstraints()
{
- WOLF_DEBUG("PTF ", getName(), "::establishConstraints: ");
+ WOLF_DEBUG("PTFP ", getName(), "::establishConstraints: ");
unsigned int N_constraints = 0;
// Create a constraint for each match in last features
@@ -282,7 +289,7 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
// not successful update
if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr))
{
- WOLF_DEBUG("\tNot successful update");
+ std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n";
// remove from match map
landmark_match_map_.erase(*ftr_it);
// remove from last feature list
@@ -297,7 +304,7 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
WOLF_DEBUG("\tLandmark removed?");
if (pl_lmk->isRemoving())
{
- WOLF_DEBUG("\tLandmark was removed");
+ std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n";
// remove from match map
landmark_match_map_.erase(*ftr_it);
// remove from last feature list
@@ -364,9 +371,9 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
// point to point constraint
{
std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
- std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
- std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
+ //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
+ //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
+ //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
N_constraints++;
std::cout << "constraint added" << std::endl;
@@ -413,7 +420,7 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPt
LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _feature_ptr)
{
- WOLF_DEBUG("PTF ", getName(), "::createLandmark: ");
+ WOLF_DEBUG("PTFP ", getName(), "::createLandmark: ");
//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");
@@ -440,7 +447,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _
bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
{
- WOLF_DEBUG("PTF ", getName(), "::modifyLandmarkAndMatch: ");
+ WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
bool print = false;
@@ -486,6 +493,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
lmk_match->landmark_from_id_ = pl_lmk->getFirstId();
lmk_match->feature_from_id_ = 0;
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
modified = true;
//std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
//std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
@@ -514,6 +522,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
{
pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
modified = true;
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
}
}
assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
@@ -537,6 +546,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
lmk_match->landmark_to_id_ = pl_lmk->getLastId();
lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1;
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
modified = true;
//std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
//std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
@@ -564,6 +574,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
(pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
{
pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
+ lmk_match->landmark_version_ = pl_lmk->getVersion();
modified = true;
}
}
@@ -587,7 +598,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
void ProcessorTrackerFeaturePolyline::advanceDerived()
{
- WOLF_DEBUG("PTF ", getName(), "::advanceDerived: ");
+ WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: ");
// remove last features from landmark_match_map_
if (last_ptr_ != nullptr)
for (auto ftr : last_ptr_->getFeatureList())
@@ -596,17 +607,18 @@ void ProcessorTrackerFeaturePolyline::advanceDerived()
landmark_match_map_.erase(ftr);
}
- WOLF_DEBUG("removing ", all_features_last_.size() , " features in last");
+ WOLF_DEBUG("removing ", all_features_last_.size() , " features in all_features_last_");
all_features_last_.clear();
all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
- WOLF_DEBUG("last has ", all_features_last_.size() , " features (prev. incoming)");
+ if (last_ptr_)
+ WOLF_DEBUG("all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
ProcessorTrackerFeature::advanceDerived();
}
void ProcessorTrackerFeaturePolyline::resetDerived()
{
- WOLF_DEBUG("PTF ", getName(), "::resetDerived: ");
+ WOLF_DEBUG("PTFP ", getName(), "::resetDerived: ");
// remove last features from landmark_match_map_
if (last_ptr_ != nullptr)
for (auto ftr : last_ptr_->getFeatureList())
@@ -615,23 +627,25 @@ void ProcessorTrackerFeaturePolyline::resetDerived()
landmark_match_map_.erase(ftr);
}
- WOLF_DEBUG("removing ", all_features_last_.size() , " features in last");
+ WOLF_DEBUG("PTF ", getName(), ": ", "removing ", all_features_last_.size() , " features in all_features_last_");
all_features_last_.clear();
all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
- WOLF_DEBUG("last has ", all_features_last_.size() , " features (prev. incoming)");
+ WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
+ if (last_ptr_)
+ WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
ProcessorTrackerFeature::resetDerived();
}
void ProcessorTrackerFeaturePolyline::preProcess()
{
- WOLF_DEBUG("PTF ", getName(), "::extractPolylines: ");
+ WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: ");
// Extract all polylines from incoming_
// TODO: sort corners by bearing
std::list<laserscanutils::Polyline> polylines;
line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines);
- WOLF_DEBUG("Polylines extracted: ", polylines.size());
+ WOLF_DEBUG("PTFP ", getName(), " Polylines extracted: ", polylines.size());
for (auto&& pl : polylines)
{
@@ -644,11 +658,15 @@ void ProcessorTrackerFeaturePolyline::preProcess()
// compute transformations
if (last_ptr_ != nullptr && incoming_ptr_ != nullptr)
computeTransformations();
+
+ WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
+ if (last_ptr_)
+ WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
}
void ProcessorTrackerFeaturePolyline::postProcess()
{
- WOLF_DEBUG("PTF ", getName(), "::postProcess: ");
+ WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
// Try to close & classify modified landmarks
while (!modified_lmks_.empty())
{
@@ -687,53 +705,92 @@ void ProcessorTrackerFeaturePolyline::postProcess()
// Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
}
+ WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
+ if (last_ptr_)
+ WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
}
-void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I, const Eigen::Matrix3s& _T_last_incoming_prior) const
+void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
+ const FeaturePolyline2DPtr& _ftr_L,
+ const FeaturePolyline2DPtr& _ftr_I,
+ const Eigen::Matrix3s& _T_last_incoming_prior) const
{
- WOLF_DEBUG("PTF ", getName(), "::tryMatchWithFeature: ");
- WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose());
- WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose());
+ //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: ");
// compute best sq distance matching
- MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points
- _ftr_I->isFirstDefined(),
- _ftr_I->isLastDefined(),
- false, // <--feature is not closed for sure
- _ftr_L->getPoints(), // <--feature last points
- _ftr_L->isFirstDefined(),
- _ftr_L->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
- // valid match
- for (auto match : matches)
+ Eigen::MatrixXs last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+ MatchPolyline2DPtr best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
+ _ftr_I->isFirstDefined(),
+ _ftr_I->isLastDefined(),
+ false, // <--feature is not closed for sure
+ last_expected_points, // <--feature last points
+ _ftr_L->isFirstDefined(),
+ _ftr_L->isLastDefined(),
+ false, // <--feature is not closed for sure
+ params_tracker_feature_polyline_->match_feature_pose_sq_norm_th);
+ //valid match
+ if (best_match != nullptr)
{
- WOLF_DEBUG("match with pose ", T2pose2D(match->T_A_B_).transpose());
- Scalar sq_norm = T2pose2D(_T_last_incoming_prior * match->T_A_B_).squaredNorm();
-
- if (sq_norm > params_tracker_feature_polyline_->match_feature_pose_sq_norm_th)
- {
- WOLF_DEBUG("match with squared norm ", sq_norm, " discarded");
- continue;
- }
- WOLF_DEBUG("match with squared norm ", sq_norm, " stored!");
auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
// feature incoming point id's
- ftr_match->feature_incoming_from_id_ = match->from_A_id_;
- ftr_match->feature_incoming_to_id_ = match->to_A_id_;
+ ftr_match->feature_incoming_from_id_ = best_match->from_A_id_;
+ ftr_match->feature_incoming_to_id_ = best_match->to_A_id_;
// feature last point id's
- ftr_match->feature_last_from_id_ = match->from_B_id_;
- ftr_match->feature_last_to_id_ = match->to_B_id_;
+ ftr_match->feature_last_from_id_ = best_match->from_B_id_;
+ ftr_match->feature_last_to_id_ = best_match->to_B_id_;
// incoming last transformation
- ftr_match->T_incoming_last_ = match->T_A_B_;
+ //ftr_match->T_incoming_last_ = match->T_A_B_; unavailable data
// score
- ftr_match->normalized_score_ = match->normalized_score_;
+ ftr_match->normalized_score_ = best_match->normalized_score_;
// feature correspondence
ftr_match->feature_ptr_ = _ftr_L;
// store in list
- ftr_matches[sq_norm] = ftr_match;
+ ftr_matches[ftr_match->normalized_score_] = ftr_match;
+ WOLF_DEBUG("match stored!");
}
+
+// WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose());
+// WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose());
+// // compute best rigid transformations
+// MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points
+// _ftr_I->isFirstDefined(),
+// _ftr_I->isLastDefined(),
+// false, // <--feature is not closed for sure
+// _ftr_L->getPoints(), // <--feature last points
+// _ftr_L->isFirstDefined(),
+// _ftr_L->isLastDefined(),
+// false, // <--feature is not closed for sure
+// params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
+// // valid match
+// for (auto match : matches)
+// {
+// WOLF_DEBUG("match with pose ", T2pose2D(match->T_A_B_).transpose());
+// Scalar sq_norm = T2pose2D(_T_last_incoming_prior * match->T_A_B_).squaredNorm();
+//
+// if (sq_norm > params_tracker_feature_polyline_->match_feature_pose_sq_norm_th)
+// {
+// WOLF_DEBUG("match with squared norm ", sq_norm, " discarded");
+// continue;
+// }
+// WOLF_DEBUG("match with squared norm ", sq_norm, " stored!");
+//
+// auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
+// // feature incoming point id's
+// ftr_match->feature_incoming_from_id_ = match->from_A_id_;
+// ftr_match->feature_incoming_to_id_ = match->to_A_id_;
+// // feature last point id's
+// ftr_match->feature_last_from_id_ = match->from_B_id_;
+// ftr_match->feature_last_to_id_ = match->to_B_id_;
+// // incoming last transformation
+// ftr_match->T_incoming_last_ = match->T_A_B_;
+// // score
+// ftr_match->normalized_score_ = match->normalized_score_;
+// // feature correspondence
+// ftr_match->feature_ptr_ = _ftr_L;
+// // store in list
+// ftr_matches[sq_norm] = ftr_match;
+// }
}
void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
@@ -741,25 +798,22 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline
const FeaturePolyline2DPtr& _feat_ptr,
const Eigen::Matrix3s& _T_feature_landmark_prior) const
{
- WOLF_DEBUG("PTF ", getName(), "::tryMatchWithLandmark: ");
+ //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
+
// compute best sq distance matching
- MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates
- _lmk_ptr->isFirstDefined(),
- _lmk_ptr->isLastDefined(),
- _lmk_ptr->isClosed(),
- _feat_ptr->getPoints(), // <--feature points
- _feat_ptr->isFirstDefined(),
- _feat_ptr->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
- // valid match
- for (auto match : matches)
+ Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+ MatchPolyline2DPtr best_match = computeBestSqDist(lmk_expected_points, // <--landmark points in local coordinates
+ _lmk_ptr->isFirstDefined(),
+ _lmk_ptr->isLastDefined(),
+ _lmk_ptr->isClosed(),
+ _feat_ptr->getPoints(), // <--feature points
+ _feat_ptr->isFirstDefined(),
+ _feat_ptr->isLastDefined(),
+ false, // <--feature is not closed for sure
+ params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
+ //valid match
+ if (best_match != nullptr)
{
- Scalar sq_norm = T2pose2D(_T_feature_landmark_prior * match->T_A_B_).squaredNorm();
-
- if (sq_norm > params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th)
- continue;
-
auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
// landmark
@@ -767,36 +821,77 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline
lmk_match->landmark_version_=_lmk_ptr->getVersion();
// landmark point id's
std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
- lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
- lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_];
+ lmk_match->landmark_from_id_ = lmk_valid_ids[best_match->from_A_id_];
+ lmk_match->landmark_to_id_ = lmk_valid_ids[best_match->to_A_id_];
// feature point id's
- lmk_match->feature_from_id_ = match->from_B_id_;
- lmk_match->feature_to_id_ = match->to_B_id_;
+ lmk_match->feature_from_id_ = best_match->from_B_id_;
+ lmk_match->feature_to_id_ = best_match->to_B_id_;
// incoming last transformation
- lmk_match->T_feature_landmark_ = match->T_A_B_.inverse();
+ lmk_match->T_feature_landmark_ = best_match->T_A_B_.inverse().eval();
// score
- lmk_match->normalized_score_ = match->normalized_score_;
+ lmk_match->normalized_score_ = best_match->normalized_score_;
assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
// store in list
- lmk_matches[sq_norm]= lmk_match;
+ lmk_matches[lmk_match->normalized_score_]= lmk_match;
+ WOLF_DEBUG("match stored!");
}
+
+// // compute best rigid transformation matching
+// MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates
+// _lmk_ptr->isFirstDefined(),
+// _lmk_ptr->isLastDefined(),
+// _lmk_ptr->isClosed(),
+// _feat_ptr->getPoints(), // <--feature points
+// _feat_ptr->isFirstDefined(),
+// _feat_ptr->isLastDefined(),
+// false, // <--feature is not closed for sure
+// params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
+// // valid match
+// for (auto match : matches)
+// {
+// Scalar sq_norm = T2pose2D(_T_feature_landmark_prior * match->T_A_B_).squaredNorm();
+//
+// if (sq_norm > params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th)
+// continue;
+//
+// auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
+//
+// // landmark
+// lmk_match->landmark_ptr_=_lmk_ptr;
+// lmk_match->landmark_version_=_lmk_ptr->getVersion();
+// // landmark point id's
+// std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
+// lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
+// lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_];
+// // feature point id's
+// lmk_match->feature_from_id_ = match->from_B_id_;
+// lmk_match->feature_to_id_ = match->to_B_id_;
+//
+// // incoming last transformation
+// lmk_match->T_feature_landmark_ = match->T_A_B_.inverse().eval();
+// // score
+// lmk_match->normalized_score_ = match->normalized_score_;
+//
+// assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
+// assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
+//
+// // store in list
+// lmk_matches[sq_norm]= lmk_match;
+// }
}
-bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr)
+bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const
{
- WOLF_DEBUG("PTF ", getName(), "::updateMatchWithLandmark: ");
+ WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: ");
assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
// merged: update the pl_lmk pointer
if (_lmk_ptr->getMergedInLandmark() != nullptr)
- {
_lmk_ptr = _lmk_ptr->getMergedInLandmark();
- _lmk_match->landmark_ptr_ = _lmk_ptr;
- }
// try match with _feat_ptr again
LandmarkMatchPolyline2DScalarMap new_lmk_matches;
@@ -806,16 +901,34 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl
if (new_lmk_matches.empty())
{
_lmk_match = nullptr;
- landmark_match_map_.erase(_feat_ptr);
return false;
}
// valid match: update landmark match of last
_lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match
- landmark_match_map_[_feat_ptr] = _lmk_match;
return true;
}
+LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
+ FeatureMatchPolyline2DPtr ftr_match,
+ LandmarkMatchPolyline2DPtr lmk_match_last) const
+{
+ // Add the match to landmark_match_map
+ auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
+
+ lmk_match_incoming->feature_from_id_ = ftr_match->feature_incoming_from_id_;
+ lmk_match_incoming->feature_to_id_ = ftr_match->feature_incoming_to_id_;
+ // Not adding new points nor checks (done in correctFeatureDrift)
+ lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
+ lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_ + ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_;
+ lmk_match_incoming->landmark_ptr_ = lmk_match_last->landmark_ptr_;
+ lmk_match_incoming->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_)->getVersion();
+ lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_;
+ lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
+
+ return lmk_match_incoming;
+}
+
// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////