diff --git a/CMakeLists.txt b/CMakeLists.txt
index c6e0000f09b4f33d171452c1cf93755cd6735a58..d76d259b303c034477695255eaea250a96b4b845 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -619,17 +619,21 @@ IF (laser_scan_utils_FOUND)
include/base/capture/capture_laser_2D.h
)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/base/processor/processor_polyline.h
include/base/processor/processor_tracker_feature_corner.h
include/base/processor/processor_tracker_landmark_corner.h
)
SET(HDRS_SENSOR ${HDRS_SENSOR}
include/base/sensor/sensor_laser_2D.h
)
- SET(SRCS ${SRCS}
- src/sensor/sensor_laser_2D.cpp
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_polyline.cpp
src/processor/processor_tracker_feature_corner.cpp
src/processor/processor_tracker_landmark_corner.cpp
)
+ SET(SRCS_SENSOR ${SRCS_SENSOR}
+ src/sensor/sensor_laser_2D.cpp
+ )
ENDIF(laser_scan_utils_FOUND)
IF (raw_gps_utils_FOUND)
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index 62624e9cac7fcaa474dd731452c38141e97690ce..db93b3319a929aff2b6addfcbade9640690521f7 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -212,7 +212,7 @@ class LandmarkPolyline2D : public LandmarkBase
YAML::Node saveToYaml() const;
- static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_tol);
+ static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol);
};
inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock()
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h
index f822aea4a5eb90a0efeace61d1d176d56d4e2040..f5059b7e0f6c5b86a8746e6492733b4822a785bc 100644
--- a/include/base/processor/processor_tracker_feature_polyline.h
+++ b/include/base/processor/processor_tracker_feature_polyline.h
@@ -31,22 +31,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline);
typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList;
typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList;
+typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
+typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_alignment_position_sq_norm_th;
- Scalar match_pose_sq_norm_th;
- Scalar class_position_error_th;
- unsigned int new_features_th;
- Scalar loop_time_th;
- std::vector<PolylineRectangularClass> polyline_classes;
-
- // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
- //Scalar aperture_error_th_;// = 20.0 * M_PI / 180.; //20 degrees
- //Scalar angular_error_th_;// = 10.0 * M_PI / 180.; //10 degrees;
- //Scalar position_error_th_;// = 1;
- //Scalar min_features_ratio_th_;// = 0.5;
+ laserscanutils::LineFinderIterativeParams line_finder_params;
+ Scalar match_alignment_position_sq_norm_th;
+ Scalar match_landmark_pose_sq_norm_th;
+ Scalar match_feature_pose_sq_norm_th;
+ Scalar class_position_error_th;
+ unsigned int new_features_th;
+ Scalar loop_time_th;
+ std::vector<PolylineRectangularClass> polyline_classes;
};
class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
@@ -57,6 +54,8 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
FeatureBaseList all_features_incoming_, all_features_last_;
LandmarkMatchPolyline2DMap landmark_match_map_;
+ LandmarkPolyline2DList modified_lmks_;
+ std::list<LandmarkPolyline2DList> merge_candidates_list_;
Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
@@ -130,24 +129,28 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr){ return nullptr; };
- /** \brief Create a new constraint and link it to the wolf tree
- * \param _feature_ptr pointer to the feature
- * \param _landmark_ptr pointer to the landmark.
- *
- * This function creates a constraint of the appropriate type for the derived processor.
- */
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
- LandmarkBasePtr _landmark_ptr) const;
-
/** \brief Establish constraints between features in Captures \b last and \b origin
*/
virtual void establishConstraints();
+ void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+ LandmarkPolyline2DPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id,
+ const int& _lmk_prev_point_id);
+
+ void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+ LandmarkPolyline2DPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id);
+
/** \brief create a landmark from a feature
*
*/
virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+ bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr);
+
/** \brief advance pointers
*
*/
@@ -169,17 +172,19 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
virtual void postProcess() override;
FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
+ void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const;
// FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
// const FeatureMatchPolyline2DPtr& _match_B) const;
LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) 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, const Eigen::Matrix3s& T_landmark_feature_prior);
+ bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr);
void computeTransformations();
- void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
+ //void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
public:
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 69e75eb51ee0d98a3587a066e9a4b44416ed2aaa..b8d8fdc6190a613f266f71c02fa38223292102fe 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -967,7 +967,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
return node;
}
-void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_tol)
+void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol)
{
std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n";
assert(_lmk_list.size() >= 2);
@@ -993,7 +993,7 @@ void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_to
Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal();
Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal();
MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
- points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _dist_tol*_dist_tol);
+ points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _sq_dist_tol);
// match
if ( match )
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp
index daba299b4b0f06fbd0d9a3dd61b417bbf094e4cd..c9ec60595ab287116aa5abb27544369e11832b20 100644
--- a/src/processor/processor_tracker_feature_polyline.cpp
+++ b/src/processor/processor_tracker_feature_polyline.cpp
@@ -32,10 +32,6 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
- Eigen::Matrix3s T_world_sensor_last(Eigen::Matrix3s::Identity());
- T_world_sensor_last.topLeftCorner(2,2) = R_world_sensor_last_;
- T_world_sensor_last.topRightCorner(2,1) = t_world_sensor_last_;
-
// ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: all_features_incoming_)
auto ftr_it = all_features_incoming_.begin();
@@ -44,24 +40,16 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
{
bool matched = false;
auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
- std::map<Scalar,FeatureMatchPolyline2DPtr> best_matches;
+ FeatureMatchPolyline2DScalarMap best_matches;
// Check matching with all features in last
+ // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
for (auto ftr_last_ : _features_last_in)
- {
- auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(ftr_last_);
- auto pair_matches = tryMatchWithFeature(pl_last,pl_incoming);
-
- // store sorted by difference from movement prior (only the close enough ones)
- for (auto match : pair_matches)
- if (T2pose2D(T_last_incoming_prior * match->T_incoming_last_).squaredNorm() < params_tracker_feature_polyline_->match_pose_sq_norm_th)
- best_matches[T2pose2D(T_last_incoming_prior * match->T_incoming_last_).squaredNorm()] = match;
- }
+ tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior);
// Try the match for all candidates (sorted by difference from movement prior)
for (auto best_ftr_match_pair : best_matches)
{
-
auto best_ftr_match = best_ftr_match_pair.second;
auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_);
auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
@@ -74,9 +62,10 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
// Landmark changed or was merged -> Redo match with last feature
if (lmk_match_last->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
- if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last,T_world_sensor_last))
+ {
+ if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last))
continue;
-
+ }
// removed -> not valid match with landmark
else if (pl_lmk->isRemoving())
continue;
@@ -149,9 +138,9 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
// prior transformations
- Eigen::Matrix3s T_world_sensor_last(Eigen::Matrix3s::Identity());
- T_world_sensor_last.topLeftCorner(2,2) = R_world_sensor_last_;
- T_world_sensor_last.topRightCorner(2,1) = t_world_sensor_last_;
+ Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity());
+ T_sensor_world_last.topLeftCorner(2,2) = R_sensor_world_last_;
+ 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
for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n);
@@ -159,52 +148,53 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
ftr_it++)
{
auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+ LandmarkMatchPolyline2DScalarMap best_lmk_matches;
- // Search matches with all existing landmarks
- LandmarkMatchPolyline2DPtr best_lmk_match = nullptr;
- Scalar best_sq_norm = params_tracker_feature_polyline_->match_pose_sq_norm_th;
+ // 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")
- continue;
-
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk);
- auto lmk_matches = tryMatchWithLandmark(pl_lmk,pl_ftr);
-
- // store sorted by difference from movement prior (only the close enough ones)
- for (auto lmk_match : lmk_matches)
- if (T2pose2D(T_world_sensor_last * lmk_match->T_feature_landmark_).squaredNorm() < best_sq_norm)
- {
- best_lmk_match = lmk_match;
- best_sq_norm = T2pose2D(T_world_sensor_last * lmk_match->T_feature_landmark_).squaredNorm();
- }
- }
+ if (lmk->getType() == "POLYLINE 2D")
+ 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
- if (best_lmk_match == nullptr)
+ if (best_lmk_matches.empty())
{
// create a landmark
auto new_lmk_ptr = createLandmark(pl_ftr);
// Add a new match to landmark_match_map_
- best_lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
- best_lmk_match->landmark_ptr_ = new_lmk_ptr;
- best_lmk_match->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getVersion();
- best_lmk_match->feature_from_id_= 0; // all points match
- best_lmk_match->landmark_from_id_ = 0; // all points match
- best_lmk_match->feature_to_id_= pl_ftr->getNPoints()-1; // all points match
- best_lmk_match->landmark_to_id_ = pl_ftr->getNPoints()-1; // all points match
- best_lmk_match->normalized_score_ = 1.0; // max score
- best_lmk_match->T_feature_landmark_ = Eigen::Matrix3s::Identity();
- best_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
- best_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
+ auto new_lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
+ new_lmk_match->landmark_ptr_ = new_lmk_ptr;
+ new_lmk_match->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getVersion();
+ new_lmk_match->feature_from_id_= 0; // all points match
+ new_lmk_match->landmark_from_id_ = 0; // all points match
+ new_lmk_match->feature_to_id_= pl_ftr->getNPoints()-1; // all points match
+ new_lmk_match->landmark_to_id_ = pl_ftr->getNPoints()-1; // all points match
+ new_lmk_match->normalized_score_ = 1.0; // max score
+ new_lmk_match->T_feature_landmark_ = Eigen::Matrix3s::Identity();
+ new_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
+ new_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
// Add the new landmark to the map
getProblem()->addLandmark(new_lmk_ptr);
+ // store match in map
+ landmark_match_map_[pl_ftr] = new_lmk_match;
+ }
+ // If matched
+ else
+ {
+ // store best match (lowest difference from prior) in map
+ landmark_match_map_[pl_ftr] = best_lmk_matches.begin()->second;
+
+ // store landmark candidates to be merged
+ if (best_lmk_matches.size() > 1)
+ {
+ LandmarkPolyline2DList merge_candidates;
+ for (auto lmk_match_pair_it : best_lmk_matches)
+ merge_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_pair_it.second->landmark_ptr_));
+ merge_candidates_list_.push_back(merge_candidates);
+ }
}
-
- // store match in map
- landmark_match_map_[pl_ftr] = best_lmk_match;
}
return n;
}
@@ -232,43 +222,144 @@ unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned i
return 0;
}
-ConstraintBasePtr ProcessorTrackerFeaturePolyline::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) const
-{
- assert(landmark_match_map_.find(_feature_ptr) != landmark_match_map_.end() && "feature without landmark match");
-
- auto match = landmark_match_map_.at(_feature_ptr);
-
-
- // TODO
- return nullptr;
-}
-
void ProcessorTrackerFeaturePolyline::establishConstraints()
{
+ unsigned int N_constraints = 0;
+
// Create a constraint for each match in last features
- for (auto ftr : last_ptr_->getFeatureList())
+ auto ftr_it = last_ptr_->getFeatureList().begin();
+ while (ftr_it != last_ptr_->getFeatureList().end())
{
- assert(landmark_match_map_.find(ftr) != landmark_match_map_.end() && "feature without landmark match in last features");
- auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[ftr]->landmark_ptr_);
- auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr);
- auto lmk_match = landmark_match_map_[ftr];
+ assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features");
+ auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[*ftr_it]->landmark_ptr_);
+ auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+ auto lmk_match = landmark_match_map_[*ftr_it];
- // CHECK IF LANDMARK CHANGED
+ // LANDMARK CHANGED: update match
if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
{
- // TODO
+ // not successful update
+ if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr))
+ {
+ // remove from match map
+ landmark_match_map_.erase(*ftr_it);
+ // remove from last feature list
+ ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
+ continue;
+ }
+ }
+
+ // LANDMARK REMOVED: remove match and feature
+ else if (pl_lmk->isRemoving())
+ {
+ // remove from match map
+ landmark_match_map_.erase(*ftr_it);
+ // remove from last feature list
+ ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
+ continue;
}
// MODIFY LANDMARK (only for not closed and not recently created)
if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
+ // If modified, add lmk to modified_lmks_ list
+ if (modifyLandmarkAndMatch(lmk_match, pl_ftr))
+ modified_lmks_.push_back(pl_lmk);
+
+ // ESTABLISH CONSTRAINTS
+ assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + lmk_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
+ //std::cout << "Establishing constraint between" << std::endl;
+ //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
+ //std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
+ //std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
+ //std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
+ //std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
+ //std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
+
+ // Constraints for all feature points
+ int lmk_point_id = lmk_match->landmark_from_id_;
+ for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
{
- // TODO
+ //std::cout << "feature point " << ftr_point_id << std::endl;
+ //std::cout << "landmark point " << lmk_point_id << std::endl;
+
+ // First not defined point
+ if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
+ // first point to line constraint
+ {
+ //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
+ assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
+
+ // emplace constraint
+ emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
+ N_constraints++;
+ //std::cout << "constraint added" << std::endl;
+ }
+
+ // Last not defined point
+ else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
+ // last point to line constraint
+ {
+ //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
+ assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
+
+ // emplace constraint
+ emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
+ N_constraints++;
+ //std::cout << "constraint added" << std::endl;
+ }
+
+ // Defined point
+ else
+ // 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;
+ emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
+ N_constraints++;
+ //std::cout << "constraint added" << std::endl;
+ }
+
+ // Next lmk point
+ if (ftr_point_id < pl_ftr->getNPoints()-1)
+ lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
}
-
- auto ctr_ptr = createConstraint(ftr, pl_lmk);
- ftr->addConstraint(ctr_ptr);
- pl_lmk->addConstrainedBy(ctr_ptr);
+ // next ftr
+ ftr_it++;
}
+ //std::cout << N_constraints << " constraints established, trying to close and classify " << modified_lmks.size() << std::endl;
+}
+
+void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+ LandmarkPolyline2DPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id,
+ const int& _lmk_prev_point_id)
+{
+ assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
+
+ // CREATE CONSTRAINT --------------------
+ ConstraintBasePtr new_ctr = std::make_shared<ConstraintPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
+
+ // ADD CONSTRAINT --------------------
+ _polyline_feature->addConstraint(new_ctr);
+ _polyline_landmark->addConstrainedBy(new_ctr);
+}
+
+void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+ LandmarkPolyline2DPtr _polyline_landmark,
+ const int& _ftr_point_id,
+ const int& _lmk_point_id)
+{
+ // CREATE CONSTRAINT --------------------
+ ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
+
+ // ADD CONSTRAINT --------------------
+ _polyline_feature->addConstraint(new_ctr);
+ _polyline_landmark->addConstrainedBy(new_ctr);
}
LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _feature_ptr)
@@ -298,6 +389,152 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _
//std::cout << "done" << std::endl;
}
+bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
+{
+ auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
+
+ bool print = false;
+ bool modified = false;
+
+ if (print)
+ {
+ std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
+ std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
+ std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
+ std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
+ t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
+
+ // MODIFY LANDMARK
+ // -----------------GROW Front-----------------
+ // Add new front points (if any feature point not matched)
+ if ( lmk_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
+ {
+ assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
+ if (print)
+ {
+ std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
+ std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ pl_lmk->addPoints(points_global, // points matrix in global coordinates
+ lmk_match->feature_from_id_-1, // last feature point index to be added
+ pl_ftr->isFirstDefined(), // is defined
+ false); // front
+
+ lmk_match->landmark_from_id_ = pl_lmk->getFirstId();
+ lmk_match->feature_from_id_ = 0;
+ modified = true;
+ //std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ //std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ //std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ //std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ // -----------------CHANGE Front-----------------
+ // Change first not defined point if landmark first not defined matched with first feature point that:
+ // a. is defined
+ // b. would extend the line (check if angle is bigger than 90º)
+ else if (lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
+ {
+ if (print)
+ {
+ std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
+ std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
+ }
+ if (// case a
+ pl_ftr->isFirstDefined() ||
+ // case b
+ (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))).squaredNorm() >
+ (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
+ (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
+ {
+ pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
+ modified = true;
+ }
+ }
+ assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ // -----------------GROW Back-----------------
+ // Add new back points (if any feature point not matched)
+ if (lmk_match->feature_to_id_ < pl_ftr->getNPoints()-1)
+ {
+ assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
+ if (print)
+ {
+ std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
+ std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl;
+ }
+ pl_lmk->addPoints(points_global, // points matrix in global coordinates
+ lmk_match->feature_to_id_+1, // last feature point index to be added
+ pl_ftr->isLastDefined(), // is defined
+ true); // back
+
+ lmk_match->landmark_to_id_ = pl_lmk->getLastId();
+ lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1;
+ modified = true;
+ //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
+ //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
+ //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
+ //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
+ }
+ // -----------------CHANGE Back-----------------
+ // Change last not defined point if landmark last not defined matched with last feature point that:
+ // a. is defined
+ // b. would extend the line (check if angle is bigger than 90º)
+ else if (lmk_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
+ {
+ if (print)
+ {
+ std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
+ std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
+ std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
+ }
+ if (// case a
+ pl_ftr->isLastDefined() ||
+ // case b
+ (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))).squaredNorm() >
+ (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
+ (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());
+ modified = true;
+ }
+ }
+ assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+
+ if (print)
+ {
+ std::cout << "MODIFIED LANDMARK" << std::endl;
+ std::cout << "feature " << pl_ftr->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_ftr->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
+ std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
+ std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
+ std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
+ std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
+ }
+
+ return modified;
+}
+
ProcessorBasePtr ProcessorTrackerFeaturePolyline::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr)
@@ -364,7 +601,44 @@ void ProcessorTrackerFeaturePolyline::preProcess()
void ProcessorTrackerFeaturePolyline::postProcess()
{
+ // Try to close & classify modified landmarks
+ while (!modified_lmks_.empty())
+ {
+ auto lmk_ptr = modified_lmks_.front();
+ modified_lmks_.pop_front();
+
+ assert(!lmk_ptr->isRemoving());
+
+ // try classify
+ if (lmk_ptr->getClassification().type == UNCLASSIFIED && lmk_ptr->getNDefinedPoints() >= 3 && lmk_ptr->getNDefinedPoints() <= 4)
+ lmk_ptr->tryClassify(params_tracker_feature_polyline_->class_position_error_th, params_tracker_feature_polyline_->polyline_classes);
+ // try close
+ if (!lmk_ptr->isClosed() && (lmk_ptr->getNDefinedPoints() >= 3 || lmk_ptr->getNPoints() >= 4))
+ lmk_ptr->tryClose(params_tracker_feature_polyline_->class_position_error_th);
+ }
+
+ // Try to merge landmarks
+ while (!merge_candidates_list_.empty())
+ {
+ // load next list of candidates
+ LandmarkPolyline2DList merge_candidates = std::move(merge_candidates_list_.front());
+ merge_candidates_list_.pop_front();
+ // change already merged lmks with the corresponding remaining lmk
+ for (auto&& lmk : merge_candidates)
+ if (lmk->getMergedInLandmark() != nullptr)
+ lmk = lmk->getMergedInLandmark();
+
+ // remove repeated lmks
+ merge_candidates.sort();
+ merge_candidates.unique();
+
+ if (merge_candidates.size() == 1)
+ continue;
+
+ // 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);
+ }
}
FeatureMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I) const
@@ -537,6 +811,44 @@ FeatureMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithFeature(
*/
}
+void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I, const Eigen::Matrix3s& _T_last_incoming_prior) const
+{
+ // 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)
+ {
+ 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)
+ continue;
+
+ 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;
+ }
+}
+
//wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A, const FeatureMatchPolyline2DPtr& _match_B) const
//{
// assert(_match_A != nullptr || _match_B != nullptr);
@@ -601,7 +913,53 @@ LandmarkMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithLandmar
return lmk_matches;
}
-bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& T_landmark_feature_prior)
+void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const
+{
+ // 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)
+ {
+ 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();
+ // 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)
{
assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
@@ -615,15 +973,18 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl
// try match with _feat_ptr again
auto new_lmk_matches = tryMatchWithLandmark(_lmk_ptr,_feat_ptr);
- // choose the one with smallest difference from movement prior
+ // choose the one with smallest difference from movement of the old match
LandmarkMatchPolyline2DPtr best_lmk_match = nullptr;
- Scalar best_sq_norm = params_tracker_feature_polyline_->match_pose_sq_norm_th;
+ Scalar best_sq_norm = params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th;
for (auto new_lmk_match : new_lmk_matches)
- if (T2pose2D(T_landmark_feature_prior * new_lmk_match->T_feature_landmark_).squaredNorm() < best_sq_norm)
+ {
+ Scalar sq_norm = T2pose2D(_lmk_match->T_feature_landmark_.inverse() * new_lmk_match->T_feature_landmark_).squaredNorm();
+ if (sq_norm < best_sq_norm)
{
best_lmk_match = new_lmk_match;
- best_sq_norm = T2pose2D(T_landmark_feature_prior * new_lmk_match->T_feature_landmark_).squaredNorm();
+ best_sq_norm = sq_norm;
}
+ }
_lmk_match = best_lmk_match;
@@ -642,7 +1003,7 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl
// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void ProcessorTrackerFeaturePolyline::expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const
+/*void ProcessorTrackerFeaturePolyline::expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const
{
assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerFeaturePolyline::expectedFeature: Bad landmark type");
LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
@@ -655,7 +1016,7 @@ void ProcessorTrackerFeaturePolyline::expectedFeatureLast(LandmarkBasePtr _landm
expected_feature_.topRows(2) = R_sensor_world_last_ * (expected_feature_.topRows(2).colwise() - t_world_sensor_last_);
expected_feature_cov_ = pl_lmk->computePointsCovGlobal();
// TODO Rotate covariances
-}
+}*/
void ProcessorTrackerFeaturePolyline::computeTransformations()
{