From f9ff78626e693c4189623f6e0e448010264f58ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Thu, 13 Jun 2019 11:49:33 +0200 Subject: [PATCH 1/9] Compiling --- include/laser/capture/capture_laser_2D.h | 2 +- .../processor_tracker_feature_polyline_2D.h | 82 +++++++--- src/landmark/landmark_polyline_2D.cpp | 148 ++++++++---------- .../processor_tracker_feature_polyline_2D.cpp | 120 +++++++------- 4 files changed, 194 insertions(+), 158 deletions(-) diff --git a/include/laser/capture/capture_laser_2D.h b/include/laser/capture/capture_laser_2D.h index 03faa523d..4a439dae6 100644 --- a/include/laser/capture/capture_laser_2D.h +++ b/include/laser/capture/capture_laser_2D.h @@ -8,8 +8,8 @@ class SensorLaser2D; } //wolf includes -#include "core/capture/capture_base.h" #include "laser/sensor/sensor_laser_2D.h" +#include "core/capture/capture_base.h" //laserscanutils includes #include "laser_scan_utils/laser_scan.h" diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h index 802634edd..5a9a360da 100644 --- a/include/laser/processor/processor_tracker_feature_polyline_2D.h +++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h @@ -75,13 +75,18 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature virtual void configure(SensorBasePtr _sensor) override {}; protected: - /** \brief Track provided features from \b last to \b incoming - * \param _features_last_in input list of features in \b last to track - * \param _features_incoming_out returned list of features found in \b incoming + + /** \brief Track provided features in \b _capture + * \param _features_in input list of features in \b last to track + * \param _capture the capture in which the _features_in should be searched + * \param _features_out returned list of features found in \b _capture * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr + * + * \return the number of features tracked */ - virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, - FeatureBasePtrList& _features_incoming_out, + virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, FeatureMatchMap& _feature_correspondences) override; /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -109,48 +114,70 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature /** \brief Detect new Features * \param _max_features maximum number of features detected (-1: unlimited. 0: none) - * \param _features_last_out The list of detected Features. + * \param _capture The capture in which the new features should be detected. + * \param _features_out The list of detected Features in _capture. * \return The number of detected Features. * * This function detects Features that do not correspond to known Features/Landmarks in the system. * - * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, + * The function is called in processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ virtual unsigned int detectNewFeatures(const int& _max_new_features, - FeatureBasePtrList& _features_last_out) override; + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out) override; - /** \brief Create a new factor and link it to the wolf tree + /** \brief Emplaces a new factor * \param _feature_ptr pointer to the parent Feature * \param _feature_other_ptr pointer to the other feature constrained. * * Implement this method in derived classes. * - * This function creates a factor of the appropriate type for the derived processor. + * This function emplaces a factor of the appropriate type for the derived processor. */ - virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, - FeatureBasePtr _feature_other_ptr) override { return nullptr; }; + virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override { return nullptr; }; /** \brief Establish factors between features in Captures \b last and \b origin */ - virtual void establishFactors() override; - + virtual void establishFactors() override; + + /** \brief Emplaces a new point to line factor + * \param _polyline_feature pointer to the feature + * \param _polyline_landmark pointer to the landmark + * \param _ftr_point_id index of the feature point + * \param _lmk_point_id intex of the landmark point + * \param _lmk_prev_point_id index of the landmark previous point + */ void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, - LandmarkPolyline2DPtr _polyline_landmark, - const int& _ftr_point_id, - const int& _lmk_point_id, - const int& _lmk_prev_point_id); - + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id, + const int& _lmk_prev_point_id); + + /** \brief Emplaces a new point to point factor + * \param _polyline_feature pointer to the feature + * \param _polyline_landmark pointer to the landmark + * \param _ftr_point_id index of the feature point + * \param _lmk_point_id intex of the landmark point + */ void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature, - LandmarkPolyline2DPtr _polyline_landmark, - const int& _ftr_point_id, - const int& _lmk_point_id); + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id); - /** \brief create a landmark from a feature + /** \brief emplace a landmark corresponding to a new feature * */ - virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr); + virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr); + /** \brief Modify a landmark and a landmark match according to a matched feature + * \param lmk_match landmark match + * \param pl_ftr feature + * + * Grow and/or define the extremes the landmark, according to the feature. + * + * \return if any modification was performed + */ bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr); /** \brief advance pointers @@ -187,6 +214,13 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature void computeTransformations(); + /** \brief Concatenate feature_incoming-feature_last match with feature_last-landmark match + * \param pl_incoming polyline feature + * \param ftr_match_incoming_last match feature_incoming-feature_last + * \param lmk_match_last match feature_last-landmark + * + * \return the concatenated match: feature_incoming-landmark + */ LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming, FeatureMatchPolyline2DPtr ftr_match_incoming_last, LandmarkMatchPolyline2DPtr lmk_match_last) const; diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp index ab06878f1..05569121e 100644 --- a/src/landmark/landmark_polyline_2D.cpp +++ b/src/landmark/landmark_polyline_2D.cpp @@ -138,7 +138,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de point_state_ptr_map_[++last_id_] = new_sb_ptr; if (getProblem()) - getProblem()->addStateBlock(new_sb_ptr); + getProblem()->notifyStateBlock(new_sb_ptr,ADD); last_defined_ = _defined; } @@ -152,7 +152,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de point_state_ptr_map_[--first_id_] = new_sb_ptr; if (getProblem()) - getProblem()->addStateBlock(new_sb_ptr); + getProblem()->notifyStateBlock(new_sb_ptr,ADD); first_defined_ = _defined; } @@ -190,7 +190,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne point_state_ptr_map_[++last_id_] = new_sb_ptr; if (getProblem()) - getProblem()->addStateBlock(new_sb_ptr); + getProblem()->notifyStateBlock(new_sb_ptr,ADD); } last_defined_ = _defined; } @@ -206,7 +206,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne point_state_ptr_map_[--first_id_] = new_sb_ptr; if (getProblem()) - getProblem()->addStateBlock(new_sb_ptr); + getProblem()->notifyStateBlock(new_sb_ptr,ADD); } first_defined_ = _defined; } @@ -447,23 +447,26 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) // Change factors from remove_state to remain_state FactorBasePtrList old_factors_list = getConstrainedByList(); //std::cout << "changing " << old_factors_list.size() << " factors." << std::endl; - FactorBasePtr new_fac_ptr = nullptr; for (auto fac_ptr : old_factors_list) { - FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr); - FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); + auto fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr); + auto fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); if (fac_point_ptr) { // If landmark point constrained -> new factor if (fac_point_ptr->getLandmarkPointId() == _remove_id) { - new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_ptr->getProcessor(), - fac_point_ptr->getFeaturePointId(), - _remain_id, - fac_point_ptr->getApplyLossFunction(), - fac_point_ptr->getStatus()); + // emplace new factor + FactorBase::emplace<FactorPoint2D>(fac_ptr->getFeature(), + std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_ptr->getProcessor(), + fac_point_ptr->getFeaturePointId(), + _remain_id, + fac_point_ptr->getApplyLossFunction(), + fac_point_ptr->getStatus()); + // remove old factor + fac_ptr->remove(); } } else if (fac_point_line_ptr) @@ -471,26 +474,34 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) // If landmark point constrained -> new factor if (fac_point_line_ptr->getLandmarkPointId() == _remove_id) { - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_line_ptr->getProcessor(), - fac_point_line_ptr->getFeaturePointId(), - _remain_id, - fac_point_line_ptr->getLandmarkPointAuxId(), - fac_point_line_ptr->getApplyLossFunction(), - fac_point_line_ptr->getStatus()); + // emplace new factor + FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), + std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_line_ptr->getProcessor(), + fac_point_line_ptr->getFeaturePointId(), + _remain_id, + fac_point_line_ptr->getLandmarkPointAuxId(), + fac_point_line_ptr->getApplyLossFunction(), + fac_point_line_ptr->getStatus()); + // remove old factor + fac_ptr->remove(); } // If landmark point is aux point -> new factor else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id) { - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_line_ptr->getProcessor(), - fac_point_line_ptr->getFeaturePointId(), - fac_point_line_ptr->getLandmarkPointId(), - _remain_id, - fac_point_line_ptr->getApplyLossFunction(), - fac_point_line_ptr->getStatus()); + // emplace new factor + FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), + std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_line_ptr->getProcessor(), + fac_point_line_ptr->getFeaturePointId(), + fac_point_line_ptr->getLandmarkPointId(), + _remain_id, + fac_point_line_ptr->getApplyLossFunction(), + fac_point_line_ptr->getStatus()); + // remove old factor + fac_ptr->remove(); } } else @@ -498,22 +509,6 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) WOLF_ERROR("polyline factor of unknown type"); throw std::runtime_error ("polyline factor of unknown type"); } - - // If new factor - if (new_fac_ptr) - { - //std::cout << "created new factor: " << new_fac_ptr->id() << std::endl; - //std::cout << "deleting factor: " << fac_ptr->id() << std::endl; - - // add new factor - fac_ptr->getFeature()->addFactor(new_fac_ptr); - addConstrainedBy(new_fac_ptr); - - // remove factor - fac_ptr->remove(); - - new_fac_ptr = nullptr; - } } // If removed was a not defined extreme, set defined extreme @@ -524,7 +519,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) // Remove remove_state if (getProblem() != nullptr) - getProblem()->removeStateBlock(remove_state); + getProblem()->notifyStateBlock(remove_state, REMOVE); //std::cout << "state removed " << std::endl; // remove element from deque @@ -821,14 +816,13 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, // ------------------------- COPY FACTORS FactorBasePtrList old_factors_list = _merged_lmk->getConstrainedByList(); //std::cout << "\tchanging " << old_factors_list.size() << " factors." << std::endl; - FactorBasePtr new_fac_ptr = nullptr; for (auto fac_ptr : old_factors_list) { //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOther()->id() << std::endl; assert(fac_ptr->getLandmarkOther() == _merged_lmk); - FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr); - FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); + auto fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr); + auto fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); // point 2 point if (fac_point_ptr) @@ -836,13 +830,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, //std::cout << "\t\tpoint-point factor to id: " << fac_point_ptr->getLandmarkPointId() << std::endl; assert(merged_id_to_id.find(fac_point_ptr->getLandmarkPointId()) != merged_id_to_id.end()); - new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_ptr->getProcessor(), - fac_point_ptr->getFeaturePointId(), - merged_id_to_id[fac_point_ptr->getLandmarkPointId()], - fac_point_ptr->getApplyLossFunction(), - fac_point_ptr->getStatus()); + // emplace new factor + FactorBase::emplace<FactorPoint2D>(fac_ptr->getFeature(), + std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_ptr->getProcessor(), + fac_point_ptr->getFeaturePointId(), + merged_id_to_id[fac_point_ptr->getLandmarkPointId()], + fac_point_ptr->getApplyLossFunction(), + fac_point_ptr->getStatus()); + // remove old factor + fac_ptr->remove(); } // point 2 line @@ -852,14 +850,18 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointId()) != merged_id_to_id.end()); assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointAuxId()) != merged_id_to_id.end()); - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_line_ptr->getProcessor(), - fac_point_line_ptr->getFeaturePointId(), - merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()], - merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()], - fac_point_line_ptr->getApplyLossFunction(), - fac_point_line_ptr->getStatus()); + // emplace new factor + FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), + std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_line_ptr->getProcessor(), + fac_point_line_ptr->getFeaturePointId(), + merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()], + merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()], + fac_point_line_ptr->getApplyLossFunction(), + fac_point_line_ptr->getStatus()); + // remove old factor + fac_ptr->remove(); } else @@ -867,18 +869,6 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, WOLF_ERROR("polyline factor of unknown type"); throw std::runtime_error ("polyline factor of unknown type"); } - // Establish new factor - assert(new_fac_ptr != nullptr); - //std::cout << "\t\tcreated new factor: " << new_fac_ptr->id() << std::endl; - //std::cout << "\t\tdeleting factor: " << fac_ptr->id() << std::endl; - - // add new factor - fac_ptr->getFeature()->addFactor(new_fac_ptr); - addConstrainedBy(new_fac_ptr); - - // remove factor - fac_ptr->remove(); - //std::cout << "\t\tdeleted\n"; } // ------------------------- REMOVE _merged_lmk @@ -892,7 +882,7 @@ void LandmarkPolyline2D::registerNewStateBlocks() LandmarkBase::registerNewStateBlocks(); if (getProblem()) for (auto state_it : point_state_ptr_map_) - getProblem()->addStateBlock(state_it.second); + getProblem()->notifyStateBlock(state_it.second, ADD); } void LandmarkPolyline2D::removeStateBlocks() @@ -901,7 +891,7 @@ void LandmarkPolyline2D::removeStateBlocks() while (sbp_pair != point_state_ptr_map_.end()) { if (getProblem()) - getProblem()->removeStateBlock(sbp_pair->second); + getProblem()->notifyStateBlock(sbp_pair->second, REMOVE); sbp_pair = point_state_ptr_map_.erase(sbp_pair); } diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index 46f839049..a314cc1fa 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -23,13 +23,14 @@ ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D() { } -unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_last_in, - FeatureBasePtrList& _features_incoming_out, - FeatureMatchMap& _feature_correspondences) +unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, + FeatureMatchMap& _feature_correspondences) { - WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size()); + WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_in.size()); - if (_features_last_in.empty()) + if (_features_in.empty()) return 0; // prior transformations @@ -51,7 +52,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP // 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) + for (auto ftr_last_ : _features_in) tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior); std::cout << "\t" << best_matches.size() << " matches with features last found\n"; @@ -113,7 +114,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP _feature_correspondences[pl_incoming] = best_ftr_match; // add feature to list of tracked features - _features_incoming_out.push_back(pl_incoming); + _features_out.push_back(pl_incoming); + + // link the feature with the capture (since they weren't emplaced in preProcess()) + pl_incoming->link(_capture); // match for this feature has been found matched = true; @@ -164,7 +168,7 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame() unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features) { WOLF_DEBUG("PTFP ", getName(), "::processNew: "); - unsigned int n = ProcessorTrackerFeature::processNew(_max_features); + unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures WOLF_DEBUG("Processing ", n, " new last features"); @@ -194,7 +198,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu if (best_lmk_matches.empty()) { // create a landmark - auto new_lmk_ptr = createLandmark(pl_ftr); + auto new_lmk_ptr = emplaceLandmark(pl_ftr); // Add a new match to landmark_match_map_ auto new_lmk_match = std::make_shared<LandmarkMatchPolyline2D>(); @@ -209,8 +213,6 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu 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 in new_landmarks new_landmarks.push_back(new_lmk_ptr); // store match in map @@ -262,17 +264,25 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu } unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _max_new_features, - FeatureBasePtrList& _features_last_out) + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out) { WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)"); - _features_last_out = std::move(all_features_last_); - if (_max_new_features != -1 && _features_last_out.size() > _max_new_features) - _features_last_out.resize(_max_new_features); + // move all already created features (in preProcess()) + _features_out = std::move(all_features_last_); + + // resize to max_new_features + if (_max_new_features != -1 && _features_out.size() > _max_new_features) + _features_out.resize(_max_new_features); + + // link detected features (only created in preProcess()) + for (auto ftr : _features_out) + ftr->link(_capture); - WOLF_DEBUG(_features_last_out.size(), " were provided"); + WOLF_DEBUG(_features_out.size(), " were provided"); - return _features_last_out.size(); + return _features_out.size(); } void ProcessorTrackerFeaturePolyline2D::establishFactors() @@ -281,14 +291,14 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() unsigned int N_factors = 0; // Create a factor for each match in last features - auto ftr_it = last_ptr_->getFeatureList().begin(); - while (ftr_it != last_ptr_->getFeatureList().end()) + FeatureBasePtrList last_features = last_ptr_->getFeatureList(); + for (auto ftr : last_features) { - WOLF_DEBUG("\tLast feature: ", (*ftr_it)->id()); - assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features"); - auto lmk_match = landmark_match_map_[*ftr_it]; + WOLF_DEBUG("\tLast feature: ", ftr->id()); + assert(landmark_match_map_.find(ftr) != landmark_match_map_.end() && "feature without landmark match in last features"); + auto lmk_match = landmark_match_map_[ftr]; auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_); - auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); + auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr); WOLF_DEBUG("\tLandmark: ", pl_lmk->id()); // LANDMARK CHANGED: update match @@ -301,9 +311,9 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() { 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 - ftr_it = last_ptr_->getFeatureList().erase(ftr_it); + landmark_match_map_.erase(ftr); + // remove feature + ftr->remove(); continue; } } @@ -316,9 +326,9 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() { 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 - ftr_it = last_ptr_->getFeatureList().erase(ftr_it); + landmark_match_map_.erase(ftr); + // remove feature + ftr->remove(); continue; } } @@ -330,7 +340,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() if (modifyLandmarkAndMatch(lmk_match, pl_ftr)) modified_lmks_.push_back(pl_lmk); - // ESTABLISH CONSTRAINTS + // ESTABLISH FACTORS WOLF_DEBUG("\tEstablishing factors.."); // checks assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); @@ -396,26 +406,25 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() if (ftr_point_id < pl_ftr->getNPoints()-1) lmk_point_id=pl_lmk->getNextValidId(lmk_point_id); } - // next ftr - ftr_it++; } std::cout << N_factors << " factors established" << std::endl; } void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, - LandmarkPolyline2DPtr _polyline_landmark, - const int& _ftr_point_id, - const int& _lmk_point_id, - const int& _lmk_prev_point_id) + 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 -------------------- - FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id); - - // ADD CONSTRAINT -------------------- - _polyline_feature->addFactor(new_fac); - _polyline_landmark->addConstrainedBy(new_fac); + FactorBasePtr new_fac = FactorBase::emplace<FactorPointToLine2D>(_polyline_feature, + _polyline_feature, + _polyline_landmark, + shared_from_this(), + _ftr_point_id, + _lmk_point_id, + _lmk_prev_point_id); } void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature, @@ -423,15 +432,17 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr const int& _ftr_point_id, const int& _lmk_point_id) { - // CREATE CONSTRAINT -------------------- - FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id); - - // ADD CONSTRAINT -------------------- - _polyline_feature->addFactor(new_fac); - _polyline_landmark->addConstrainedBy(new_fac); + assert(_polyline_landmark->isValidId(_lmk_point_id)); + + FactorBasePtr new_fac = FactorBase::emplace<FactorPoint2D>(_polyline_feature, + _polyline_feature, + _polyline_landmark, + shared_from_this(), + _ftr_point_id, + _lmk_point_id); } -LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr) +LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePtr _feature_ptr) { WOLF_DEBUG("PTFP ", getName(), "::createLandmark: "); //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl; @@ -450,11 +461,12 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl; // Create new landmark - return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), - std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), - points_global, - polyline_ptr->isFirstDefined(), - polyline_ptr->isLastDefined()); + return LandmarkBase::emplace<LandmarkPolyline2D>(getProblem()->getMap(), + std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), + std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), + points_global, + polyline_ptr->isFirstDefined(), + polyline_ptr->isLastDefined()); //std::cout << "done" << std::endl; } -- GitLab From 813b51f123658480ae40cb5e7bdcb8502ccbc40a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 10:06:29 +0200 Subject: [PATCH 2/9] improved check() in LandmarkMatchPolyline2D --- .../landmark/landmark_match_polyline_2D.h | 2 +- src/landmark/landmark_match_polyline_2D.cpp | 61 +++++++++++++------ 2 files changed, 44 insertions(+), 19 deletions(-) diff --git a/include/laser/landmark/landmark_match_polyline_2D.h b/include/laser/landmark/landmark_match_polyline_2D.h index 4e846418e..12e716a49 100644 --- a/include/laser/landmark/landmark_match_polyline_2D.h +++ b/include/laser/landmark/landmark_match_polyline_2D.h @@ -26,7 +26,7 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch int landmark_version_; Eigen::Matrix3s T_feature_landmark_; - bool check() const; + bool check(FeatureBasePtr ftr) const; }; }//end namespace diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp index 318255a73..f490eb455 100644 --- a/src/landmark/landmark_match_polyline_2D.cpp +++ b/src/landmark/landmark_match_polyline_2D.cpp @@ -7,35 +7,59 @@ #include "laser/landmark/landmark_match_polyline_2D.h" #include "laser/landmark/landmark_polyline_2D.h" +#include "laser/feature/feature_polyline_2D.h" namespace wolf { -bool LandmarkMatchPolyline2D::check() const +bool LandmarkMatchPolyline2D::check(FeatureBasePtr ftr) const { - LandmarkPolyline2DPtr pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_); + auto pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_); + auto pl_ftr = std::dynamic_pointer_cast<FeaturePolyline2D>(ftr); + + // landmark polyline type if (pl_lmk == nullptr) { WOLF_ERROR("LandmarkMatchPolyline2D landmark is not polyline 2D"); return false; } - if (!pl_lmk->isClosed()) + // feature polyline type + if (pl_ftr == nullptr) { - if (landmark_from_id_ > landmark_to_id_) - { - WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk"); - return false; - } - if (landmark_from_id_ < pl_lmk->getFirstId()) - { - WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk"); - return false; - } - if (landmark_to_id_ > pl_lmk->getLastId()) - { - WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk"); - return false; - } + WOLF_ERROR("LandmarkMatchPolyline2D feature is not polyline 2D"); + return false; + } + // Check idxs + if (!pl_lmk->isClosed() && landmark_from_id_ > landmark_to_id_) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk"); + return false; + } + if (!pl_lmk->isClosed() && landmark_from_id_ < pl_lmk->getFirstId()) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk"); + return false; + } + if (!pl_lmk->isClosed() && landmark_to_id_ > pl_lmk->getLastId()) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk"); + return false; + } + if (feature_from_id_ != 0 && feature_to_id_ != pl_ftr->getNPoints()-1) + { + WOLF_ERROR("LandmarkMatchPolyline2D feature_from_id_ != 0 && feature_to_id_ != pl_ftr->getNPoints()-1 partial match"); + return false; } + if (feature_from_id_ != 0 && landmark_from_id_ != pl_lmk->getFirstId()) + { + WOLF_ERROR("LandmarkMatchPolyline2D feature_from_id_ != 0 && landmark_from_id_ != pl_lmk->getFirstId() partial match"); + return false; + } + if (feature_to_id_ != pl_ftr->getNPoints()-1 && landmark_to_id_ != pl_lmk->getLastId()) + { + WOLF_ERROR("LandmarkMatchPolyline2D feature_to_id_ != pl_ftr->getNPoints()-1 && landmark_to_id_ != pl_lmk->getLastId() partial match"); + return false; + } + // correspondences int lmk_id = landmark_from_id_; int ftr_id = feature_from_id_; while (1) @@ -60,6 +84,7 @@ bool LandmarkMatchPolyline2D::check() const ftr_id++; } } + return true; } -- GitLab From 9f24255625c813003de843a8e080295006c9a088 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 10:07:12 +0200 Subject: [PATCH 3/9] removed first_id_ and last_id_, using only getFirstId() and getLastId() --- include/laser/landmark/landmark_polyline_2D.h | 14 +-- src/landmark/landmark_polyline_2D.cpp | 108 +++++++----------- 2 files changed, 49 insertions(+), 73 deletions(-) diff --git a/include/laser/landmark/landmark_polyline_2D.h b/include/laser/landmark/landmark_polyline_2D.h index 16dcdb9c5..665c1150a 100644 --- a/include/laser/landmark/landmark_polyline_2D.h +++ b/include/laser/landmark/landmark_polyline_2D.h @@ -77,8 +77,6 @@ class LandmarkPolyline2D : public LandmarkBase std::map<int,StateBlockPtr> point_state_ptr_map_; ///< polyline points state blocks bool first_defined_; ///< Wether the first point is an extreme of a line or the line may continue bool last_defined_; ///< Wether the last point is an extreme of a line or the line may continue - int first_id_; - int last_id_; bool closed_; ///< Wether the polyline is closed or not PolylineRectangularClass classification_; ///< The classification of the landmark int version_; ///< Integer increased each time a modification in landmark occurs (added points, merged points, closed, classified) @@ -299,21 +297,21 @@ inline int LandmarkPolyline2D::getLastId() const { inline int LandmarkPolyline2D::getNextValidId(const int& i) const { - assert(!(i == last_id_ && !closed_) && "Calling getNextValidId of last_id in an open landmark"); + assert(!(i == getLastId() && !closed_) && "Calling getNextValidId of last_id in an open landmark"); assert(isValidId(i) && "Calling getNextValidId of an invalid id"); - if (i == last_id_ && closed_) - return first_id_; + if (i == getLastId() && closed_) + return getFirstId(); return std::next(point_state_ptr_map_.find(i))->first; } inline int LandmarkPolyline2D::getPrevValidId(const int& i) const { - assert(!(i == first_id_ && !closed_) && "Calling getPrevValidId of first_id in an open landmark"); + assert(!(i == getFirstId() && !closed_) && "Calling getPrevValidId of first_id in an open landmark"); assert(isValidId(i) && "Calling getPrevValidId of an invalid id"); - if (i == first_id_ && closed_) - return last_id_; + if (i == getFirstId() && closed_) + return getLastId(); return std::prev(point_state_ptr_map_.find(i))->first; } diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp index 05569121e..a36666765 100644 --- a/src/landmark/landmark_polyline_2D.cpp +++ b/src/landmark/landmark_polyline_2D.cpp @@ -37,10 +37,7 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt if (!last_defined_) lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.rbegin())->second)); - first_id_ = getFirstId(); - last_id_ = getLastId(); - - //std::cout << "LandmarkPolyline2D " << id() << " created. First: " << first_id_ << " last: "<< last_id_ << std::endl; + //std::cout << "LandmarkPolyline2D " << id() << " created. First: " << getFirstId() << " last: "<< getLastId() << std::endl; } LandmarkPolyline2D::~LandmarkPolyline2D() @@ -135,7 +132,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de (!_defined ? std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) : nullptr)); - point_state_ptr_map_[++last_id_] = new_sb_ptr; + point_state_ptr_map_[getLastId()+1]= new_sb_ptr; if (getProblem()) getProblem()->notifyStateBlock(new_sb_ptr,ADD); @@ -149,7 +146,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de (!_defined ? std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) : nullptr)); - point_state_ptr_map_[--first_id_] = new_sb_ptr; + point_state_ptr_map_[getFirstId()-1] = new_sb_ptr; if (getProblem()) getProblem()->notifyStateBlock(new_sb_ptr,ADD); @@ -159,9 +156,6 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de // new version version_++; - - assert(getFirstId() == first_id_); - assert(getLastId() == last_id_); } void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined, @@ -187,7 +181,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne (i == _points.cols()-1 && !_defined ? std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) : nullptr)); - point_state_ptr_map_[++last_id_] = new_sb_ptr; + point_state_ptr_map_[getLastId()+1] = new_sb_ptr; if (getProblem()) getProblem()->notifyStateBlock(new_sb_ptr,ADD); @@ -203,7 +197,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne (i == 0 && !_defined ? std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) : nullptr)); - point_state_ptr_map_[--first_id_] = new_sb_ptr; + point_state_ptr_map_[getFirstId()-1] = new_sb_ptr; if (getProblem()) getProblem()->notifyStateBlock(new_sb_ptr,ADD); @@ -213,10 +207,6 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne // new version version_++; - - //std::cout << "final number of points: " << point_state_ptr_vector_.size() << std::endl; - assert(getFirstId() == first_id_); - assert(getLastId() == last_id_); } void LandmarkPolyline2D::defineFirst() @@ -275,8 +265,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) std::vector<std::pair<int, int> > overlapped_ids; // Check first defined point against the rest defined points - int first_point_id = (first_defined_ ? first_id_ : getNextValidId(first_id_)); - int with_point_id = (last_defined_ ? last_id_ : getPrevValidId(last_id_)); + int first_point_id = (first_defined_ ? getFirstId() : getNextValidId(getFirstId())); + int with_point_id = (last_defined_ ? getLastId() : getPrevValidId(getLastId())); int last_with_id = first_point_id; // limit for avoiding more than one overlapping per defined point (full overlapping of defined points) bool found = false; @@ -312,10 +302,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) } // overlapped points from corresponding first point match to match with last point - while (with_point_id <= last_id_) + while (with_point_id <= getLastId()) { overlapped_ids.push_back(std::pair<int,int>(first_point_id, with_point_id)); - if (with_point_id == last_id_) + if (with_point_id == getLastId()) break; first_point_id = getNextValidId(first_point_id); with_point_id = getNextValidId(with_point_id); @@ -330,14 +320,14 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) Scalar sq_dist=1e6; // 2 defined points: point-point - if ( (overlapped_ids[i].first != first_id_ || first_defined_) && - (overlapped_ids[i].second != last_id_ || last_defined_) ) + if ( (overlapped_ids[i].first != getFirstId() || first_defined_) && + (overlapped_ids[i].second != getLastId() || last_defined_) ) sq_dist = (getPointVector(overlapped_ids[i].first)-getPointVector(overlapped_ids[i].second)).squaredNorm(); // defined with not defined: not defined point-segment - else if (overlapped_ids[i].first != first_id_ || first_defined_) + else if (overlapped_ids[i].first != getFirstId() || first_defined_) sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].second), getPointVector(overlapped_ids[i].first), getPointVector(getNextValidId(overlapped_ids[i].first))); // not defined with defined: not defined point-segment - else if (overlapped_ids[i].second != last_id_ || last_defined_) + else if (overlapped_ids[i].second != getLastId() || last_defined_) sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].first),getPointVector(overlapped_ids[i].second), getPointVector(getPrevValidId(overlapped_ids[i].second))); else throw std::runtime_error("when trying to close, overlapping two not defined points in a landmark"); @@ -345,13 +335,13 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) if (sq_dist > _dist_tol*_dist_tol) { WOLF_TRACE("\nBad overlapping: ", sq_dist, - "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""), - "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : "")); + "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""), + "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : "")); return false; } WOLF_TRACE("\nGood overlapping: ", sq_dist, - "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""), - "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : "")); + "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""), + "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : "")); } } } @@ -363,10 +353,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) WOLF_TRACE("No overlapping found with defined points, trying with not defined extremes"); // Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold - Eigen::Vector2s first_not_def = getPointVector(first_id_); - Eigen::Vector2s last_not_def = getPointVector(last_id_); - Eigen::Vector2s first_def = getPointVector(getNextValidId(first_id_)); - Eigen::Vector2s last_def = getPointVector(getPrevValidId(last_id_)); + Eigen::Vector2s first_not_def = getPointVector(getFirstId()); + Eigen::Vector2s last_not_def = getPointVector(getLastId()); + Eigen::Vector2s first_def = getPointVector(getNextValidId(getFirstId())); + Eigen::Vector2s last_def = getPointVector(getPrevValidId(getLastId())); if(sqDistPoint2Segment(first_not_def, last_def, first_def) < _dist_tol*_dist_tol && sqDistPoint2Segment(last_not_def, last_def, first_def) < _dist_tol*_dist_tol) @@ -388,7 +378,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) WOLF_TRACE("The not defined extremes are overlapped", "\n\tlast_proj_2_first_def: ", last_proj_2_first_def, "\n\tfirst_proj_2_first_def: ", first_proj_2_first_def); - overlapped_ids.push_back(std::pair<int,int>(first_id_, last_id_)); + overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getLastId())); } } } @@ -402,7 +392,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) while (!overlapped_ids.empty()) { // not defined points are merged into defined ones - if ((overlapped_ids.back().first == first_id_ && !first_defined_) || (overlapped_ids.back().first == last_id_ && !last_defined_)) + if ((overlapped_ids.back().first == getFirstId() && !first_defined_) || (overlapped_ids.back().first == getLastId() && !last_defined_)) mergePoints(overlapped_ids.back().first,overlapped_ids.back().second); else mergePoints(overlapped_ids.back().second,overlapped_ids.back().first); @@ -413,9 +403,6 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol) WOLF_TRACE("Landmark ",id()," was closed."); - assert(getFirstId() == first_id_); - assert(getLastId() == last_id_); - return true; } return false; @@ -439,7 +426,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) assert(!(_remain_id == getLastId() && !last_defined_) && "in merging points, the remaining point must be defined"); assert(!(_remain_id == getFirstId() && !first_defined_) && "in merging points, the remaining point must be defined"); - //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << first_id_ << " to " << getLastId() << ")" << std::endl; + //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << getFirstId() << " to " << getLastId() << ")" << std::endl; StateBlockPtr remove_state = getPointStateBlock(_remove_id); //std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl; @@ -512,9 +499,9 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) } // If removed was a not defined extreme, set defined extreme - if (_remove_id == first_id_ && !first_defined_) + if (_remove_id == getFirstId() && !first_defined_) first_defined_ = true; - if (_remove_id == last_id_ && !last_defined_) + if (_remove_id == getLastId() && !last_defined_) last_defined_ = true; // Remove remove_state @@ -526,15 +513,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) point_state_ptr_map_.erase(_remove_id); //std::cout << "state removed from point vector " << std::endl; - // reset first_id_ & last_id_ - first_id_ = getFirstId(); - last_id_ = getLastId(); - // new version version_++; - - assert(getFirstId() == first_id_); - assert(getLastId() == last_id_); } bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes) @@ -628,7 +608,7 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli if (getNPoints() < 4) { addPoint(Eigen::Vector2s::Zero(), true, true); - points_ids.push_back(last_id_); + points_ids.push_back(getLastId()); //std::cout << "ADDING POINT\n"; //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl; } @@ -642,14 +622,14 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli { //std::cout << "4 DEFINED POINTS\n"; if (!last_defined_) - mergePoints(first_id_, getPrevValidId(last_id_)); + mergePoints(getFirstId(), getPrevValidId(getLastId())); else - mergePoints(first_id_, last_id_); + mergePoints(getFirstId(), getLastId()); } else // define { defineFirst(); - points_ids.push_back(first_id_); + points_ids.push_back(getFirstId()); } //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl; } @@ -662,12 +642,12 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli if (getNDefinedPoints() == 4) // merge { //std::cout << "4 DEFINED POINTS\n"; - mergePoints(last_id_, first_id_); + mergePoints(getLastId(), getFirstId()); } else // define { defineLast(); - points_ids.push_back(last_id_); // it is points_ids[3] + points_ids.push_back(getLastId()); // it is points_ids[3] } //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl; } @@ -675,8 +655,6 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli assert(getNDefinedPoints() == 4 && "classified landmark must have 4 defined points"); assert(getNPoints() == 4 && "classified landmark must have 4 points"); assert(points_ids.size() == 4 && "classified landmark must have 4 points"); - assert(getFirstId() == first_id_); - assert(getLastId() == last_id_); } // Close @@ -749,17 +727,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, // ADD FRONT POINTS if (_merged_from > _merged_lmk->getFirstId()) { - assert(_from == first_id_); + assert(_from == getFirstId()); for (int merged_id = _merged_lmk->getPrevValidId(_merged_from); ; merged_id = _merged_lmk->getPrevValidId(merged_id)) { //std::cout << "adding front point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl; - //std::cout << "Until current first point: " << first_id_ << "\n"; + //std::cout << "Until current first point: " << getFirstId() << "\n"; // Add point addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined() ,false); //std::cout << "added\n"; - //std::cout << "adding correspondence" << merged_id << " with " << first_id_ << "\n"; - merged_id_to_id[merged_id] = first_id_; + //std::cout << "adding correspondence" << merged_id << " with " << getFirstId() << "\n"; + merged_id_to_id[merged_id] = getFirstId(); //std::cout << "added correspondence\n"; // exit loop @@ -770,17 +748,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, // ADD BACK POINTS if (_merged_to < _merged_lmk->getLastId()) { - assert(_to == last_id_); + assert(_to == getLastId()); for (int merged_id = _merged_lmk->getNextValidId(_merged_to); ; merged_id = _merged_lmk->getNextValidId(merged_id)) { //std::cout << "adding back point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl; - //std::cout << "Until current last point: " << last_id_ << "\n"; + //std::cout << "Until current last point: " << getLastId() << "\n"; // Add point addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined() ,true); //std::cout << "added\n"; - //std::cout << "adding correspondence" << merged_id << " with " << last_id_ << "\n"; - merged_id_to_id[merged_id] = last_id_; + //std::cout << "adding correspondence" << merged_id << " with " << getLastId() << "\n"; + merged_id_to_id[merged_id] = getLastId(); //std::cout << "added correspondence\n"; // exit loop @@ -799,9 +777,9 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, merged_id_to_id[merged_id] = id; // Set Defined extremes if it's the case - if (id == first_id_ && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined())) + if (id == getFirstId() && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined())) defineFirst(); - if (id == last_id_ && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined())) + if (id == getLastId() && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined())) defineLast(); // exit loop @@ -945,7 +923,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const YAML::Node node = LandmarkBase::saveToYaml(); // Then add specific things - node["first_id"] = first_id_; + node["first_id"] = getFirstId(); node["first_defined"] = first_defined_; node["last_defined"] = last_defined_; node["classification_type"] = static_cast<int>(classification_.type); -- GitLab From 12ef59da6bee95ecdb802096dda60913a920c077 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 10:11:50 +0200 Subject: [PATCH 4/9] changed name all_features by untracked_features --- .../processor_tracker_feature_polyline_2D.h | 4 +- .../processor_tracker_feature_polyline_2D.cpp | 65 ++++++++++++------- 2 files changed, 42 insertions(+), 27 deletions(-) diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h index 5a9a360da..918a17961 100644 --- a/include/laser/processor/processor_tracker_feature_polyline_2D.h +++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h @@ -52,7 +52,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_; laserscanutils::LineFinderIterative line_finder_; - FeatureBasePtrList all_features_incoming_, all_features_last_; + FeatureBasePtrList untracked_features_incoming_, untracked_features_last_; LandmarkMatchPolyline2DMap landmark_match_map_; LandmarkPolyline2DPtrList modified_lmks_; std::list<LandmarkPolyline2DPtrList> merge_candidates_list_; @@ -234,7 +234,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature const FeatureBasePtrList& getLastNewFeatures() const { - return all_features_last_; + return untracked_features_last_; } }; diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index a314cc1fa..dea8f4e34 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -38,11 +38,11 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_; T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_; - // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: all_features_incoming_) - auto ftr_it = all_features_incoming_.begin(); + // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: untracked_features_incoming_) + auto ftr_it = untracked_features_incoming_.begin(); - // iterate over all polylines features - while (ftr_it != all_features_incoming_.end()) + // iterate over all untracked_features_incoming_ + while (ftr_it != untracked_features_incoming_.end()) { bool matched = false; auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); @@ -127,7 +127,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP // next feature if (matched) - ftr_it = all_features_incoming_.erase(ftr_it); // remove from all_features_incoming + ftr_it = untracked_features_incoming_.erase(ftr_it); // remove feature from untracked features list else ftr_it++; } @@ -267,10 +267,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out) { - WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)"); + WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", untracked_features_last_.size(), " in untracked_features_last_)"); // move all already created features (in preProcess()) - _features_out = std::move(all_features_last_); + _features_out = std::move(untracked_features_last_); // resize to max_new_features if (_max_new_features != -1 && _features_out.size() > _max_new_features) @@ -659,11 +659,20 @@ void ProcessorTrackerFeaturePolyline2D::advanceDerived() landmark_match_map_.erase(ftr); } - 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_); - if (last_ptr_) - WOLF_DEBUG("all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)"); + // check all not tracked features are not linked + assert(std::all_of(untracked_features_last_.begin(), untracked_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) && + "any linked feature in untracked_features_last_"); + assert(std::all_of(untracked_features_incoming_.begin(), untracked_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) && + "any linked feature in untracked_features_incoming_"); + + // remove untracked features + WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_"); + untracked_features_last_.clear(); + + // move untracked features incoming to last + untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_); + WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)"); + WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); ProcessorTrackerFeature::advanceDerived(); } @@ -679,12 +688,20 @@ void ProcessorTrackerFeaturePolyline2D::resetDerived() landmark_match_map_.erase(ftr); } - 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("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_)"); + // check all not tracked features are not linked + assert(std::all_of(untracked_features_last_.begin(), untracked_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) && + "any linked feature in untracked_features_last_"); + assert(std::all_of(untracked_features_incoming_.begin(), untracked_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) && + "any linked feature in untracked_features_incoming_"); + + // remove untracked features + WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_"); + untracked_features_last_.clear(); + + // move untracked features incoming to last + untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_); + WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)"); + WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); ProcessorTrackerFeature::resetDerived(); } @@ -703,7 +720,7 @@ void ProcessorTrackerFeaturePolyline2D::preProcess() { //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ ); //std::cout << "covs: " << std::endl << pl.covs_ << std::endl; - all_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_)); + untracked_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_)); //WOLF_DEBUG("new polyline detected: "); } @@ -711,9 +728,8 @@ void ProcessorTrackerFeaturePolyline2D::preProcess() 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_)"); + WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features"); + WOLF_DEBUG_COND(last_ptr_!=nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); } void ProcessorTrackerFeaturePolyline2D::postProcess() @@ -757,9 +773,8 @@ void ProcessorTrackerFeaturePolyline2D::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_)"); + WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features"); + WOLF_DEBUG_COND(last_ptr_!=nullptr,"PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); } void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, -- GitLab From cafd6d3d957f0fa092ae8665eda36ecda13be4ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 10:15:15 +0200 Subject: [PATCH 5/9] adapted to emplace --- .../processor_tracker_feature_polyline_2D.cpp | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index dea8f4e34..15e6a3001 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -84,9 +84,9 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP 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"; + std::cout << "\t\t\tNot success: removing last feature " << pl_last->id() << " and removing landmark match\n"; landmark_match_map_.erase(pl_last); - last_ptr_->getFeatureList().remove(pl_last); + pl_last->remove(); continue; } } @@ -98,25 +98,27 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming, best_ftr_match, landmark_match_map_[pl_last]); - // Add the incoming match to landmark_match_map - if (pl_lmk_match_incoming != nullptr) - landmark_match_map_[pl_incoming] = pl_lmk_match_incoming; - else + + // if concatenation did not succees -> next ftr match + if (pl_lmk_match_incoming == nullptr) { WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming track lost common points with landmark"); continue; } + + // Add the incoming match to landmark_match_map + landmark_match_map_[pl_incoming] = pl_lmk_match_incoming; } // TRACK valid std::cout << "\t\tvalid match, storing...\n"; - // add best match to match_map _feature_correspondences[feature_out_ptr] = feature_in_ptr + // add best match to feature-feature correspondences _feature_correspondences[pl_incoming] = best_ftr_match; // add feature to list of tracked features _features_out.push_back(pl_incoming); - // link the feature with the capture (since they weren't emplaced in preProcess()) + // link the incoming eature with the capture (since it was just created in preProcess()) pl_incoming->link(_capture); // match for this feature has been found @@ -168,7 +170,7 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame() unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features) { WOLF_DEBUG("PTFP ", getName(), "::processNew: "); - unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures + unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures() and trackFeatures() WOLF_DEBUG("Processing ", n, " new last features"); @@ -177,13 +179,11 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu 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 each new last feature: Either create a landmark or match with an existent landmark LandmarkBasePtrList new_landmarks; - for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n); - ftr_it != last_ptr_->getFeatureList().end(); - ftr_it++) + for (auto ftr : new_features_last_) { - auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); + auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr); LandmarkMatchPolyline2DScalarMap best_lmk_matches; WOLF_DEBUG("Processing feature: ", pl_ftr->id()); @@ -236,13 +236,13 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu } // store in landmark_match_map_ the incoming features - auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n); - while (ftr_it != incoming_ptr_->getFeatureList().end()) + auto ftr_it = new_features_incoming_.begin(); + while (ftr_it != new_features_incoming_.end()) { 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"); + assert(matches_last_from_incoming_.count(*ftr_it) && "last-incoming match not found"); + auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[*ftr_it]); + assert(landmark_match_map_.count(pl_ftr_match->feature_ptr_) && "last-lmk match not found"); auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming, pl_ftr_match, landmark_match_map_[pl_ftr_match->feature_ptr_]); @@ -255,9 +255,9 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu { WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming track lost common points with landmark"); matches_last_from_incoming_.erase(pl_incoming); - // move feature from known to all features - all_features_incoming_.push_back(pl_incoming); - ftr_it = incoming_ptr_->getFeatureList().erase(ftr_it); + // remove feature (unlink not allowed, we could create a new one via copy..) + pl_incoming->remove(); + ftr_it = new_features_incoming_.erase(ftr_it); } } return n; -- GitLab From 4c9b8aac74fa8f333a77baac3cf2c59acb51e365 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 10:16:12 +0200 Subject: [PATCH 6/9] more asserts, debuging comments and documentation --- .../processor_tracker_feature_polyline_2D.cpp | 119 ++++++++++-------- 1 file changed, 68 insertions(+), 51 deletions(-) diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index 15e6a3001..ce350fc5f 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -65,11 +65,11 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n"; - // 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()) + // check/update/remove landmark match (only if not recently created in processNew) + if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last)) { - // the match with this last_ feature was removed after any updateMatchWithLandmark - if(landmark_match_map_.find(pl_last) == landmark_match_map_.end()) + // Check if the match with this last_ feature was removed after any updateMatchWithLandmark + if(landmark_match_map_.count(pl_last) == 0) { std::cout << "\t\tremoved feature last\n"; continue; @@ -77,10 +77,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP 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 + // Landmark changed or was merged -> update match with last feature if (landmark_match_map_[pl_last]->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr) { - std::cout << "\t\tupdating match...\n"; + std::cout << "\t\tupdating match last-landmark...\n"; if (!updateMatchWithLandmark(landmark_match_map_[pl_last],pl_lmk,pl_last)) { // not successful update -> remove match and feature @@ -295,7 +295,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() for (auto ftr : last_features) { WOLF_DEBUG("\tLast feature: ", ftr->id()); - assert(landmark_match_map_.find(ftr) != landmark_match_map_.end() && "feature without landmark match in last features"); + assert(landmark_match_map_.count(ftr) && "feature without landmark match in last features"); auto lmk_match = landmark_match_map_[ftr]; auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_); auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr); @@ -305,7 +305,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() WOLF_DEBUG("\tLandmark changed?"); if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr) { - WOLF_DEBUG("\tLandmark changed"); + WOLF_DEBUG("\tLandmark changed or was merged"); // not successful update if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr)) { @@ -346,7 +346,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors() 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() && "Landmark didn't grow properly"); - assert(lmk_match->check() && "Landmark didn't grow properly"); + assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly"); std::cout << "Establishing factor between" << std::endl; std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; @@ -474,7 +474,6 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly { WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: "); auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_); - bool print = true; bool modified = false; @@ -486,7 +485,11 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly 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 << "\tpoints " << pl_lmk->getNPoints() << " ( "; + std::vector<int> lmk_ids = pl_lmk->getValidIds(); + for (auto id : lmk_ids) + std::cout << id << " "; + std::cout << ")" << 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; @@ -494,6 +497,11 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly 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; } + assert(lmk_match->landmark_version_ == pl_lmk->getVersion() && "Different landmark version"); + assert(lmk_match->check(pl_ftr) && "Bad landmark match provided"); + assert((lmk_match->landmark_from_id_ == pl_lmk->getFirstId() || lmk_match->feature_from_id_ == 0) && "Partial match"); + assert((lmk_match->landmark_to_id_ == pl_lmk->getLastId() || lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "Partial match"); + //Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() + // t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose(); Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints(); @@ -531,8 +539,8 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly // checks assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); - assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); - assert(lmk_match->check() && "Landmark didn't grow properly"); + assert(lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && "Landmark didn't grow properly"); + assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly"); } // -----------------CHANGE Front----------------- // Change first not defined point if landmark first not defined matched with first feature point that: @@ -540,13 +548,6 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly // 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 @@ -554,15 +555,23 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly (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()) { + 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; + } + pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined()); modified = true; lmk_match->landmark_version_ = pl_lmk->getVersion(); - } - // checks - assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); - assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); - assert(lmk_match->check() && "Landmark didn't grow properly"); + // checks + assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); + assert(lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && "Landmark didn't grow properly"); + assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly"); + } } // -----------------GROW Back----------------- // Add new back points (if any feature point not matched) @@ -594,10 +603,9 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl; } // checks - 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() && "Landmark didn't grow properly"); - assert(lmk_match->check() && "Landmark didn't grow properly"); + assert(lmk_match->landmark_to_id_ == pl_lmk->getLastId() && "Landmark didn't grow properly"); + assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly"); } // -----------------CHANGE Back----------------- // Change last not defined point if landmark last not defined matched with last feature point that: @@ -605,13 +613,6 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly // 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 @@ -619,18 +620,29 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly (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()) { + 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; + } + pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined()); lmk_match->landmark_version_ = pl_lmk->getVersion(); modified = true; - } - // checks - 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() && "Landmark didn't grow properly"); - assert(lmk_match->check() && "Landmark didn't grow properly"); + // checks + assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); + assert(lmk_match->landmark_to_id_ == pl_lmk->getLastId() && "Landmark didn't grow properly"); + assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly"); + } } + // all checks + 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() && "Landmark didn't grow properly"); + assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly (should have stopped in a previous assert)"); if (print) { @@ -651,11 +663,11 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly void ProcessorTrackerFeaturePolyline2D::advanceDerived() { WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: "); - // remove last features from landmark_match_map_ + // remove landmark matches with last features if (last_ptr_ != nullptr) for (auto ftr : last_ptr_->getFeatureList()) { - assert(landmark_match_map_.find(ftr) != landmark_match_map_.end()); + assert(landmark_match_map_.count(ftr)); landmark_match_map_.erase(ftr); } @@ -680,11 +692,11 @@ void ProcessorTrackerFeaturePolyline2D::advanceDerived() void ProcessorTrackerFeaturePolyline2D::resetDerived() { WOLF_DEBUG("PTFP ", getName(), "::resetDerived: "); - // remove last features from landmark_match_map_ + // remove landmark matches with last features if (last_ptr_ != nullptr) for (auto ftr : last_ptr_->getFeatureList()) { - assert(landmark_match_map_.find(ftr) != landmark_match_map_.end()); + assert(landmark_match_map_.count(ftr)); landmark_match_map_.erase(ftr); } @@ -824,6 +836,10 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline ftr_match->normalized_score_ = best_match->normalized_score_; // feature correspondence ftr_match->feature_ptr_ = _ftr_L; + + assert(ftr_match->feature_incoming_from_id_ == 0 || ftr_match->feature_last_from_id_ == 0); + assert(ftr_match->feature_incoming_to_id_ == _ftr_I->getNPoints() -1 || ftr_match->feature_incoming_to_id_ == _ftr_L->getNPoints() -1); + // store in list ftr_matches[ftr_match->normalized_score_] = ftr_match; WOLF_DEBUG("match stored!"); @@ -927,11 +943,12 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed()); assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed()); + assert(lmk_match->feature_from_id_ == 0 || lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1); // store in list lmk_matches[lmk_match->normalized_score_]= lmk_match; WOLF_DEBUG("match stored!"); - assert(lmk_match->check() && "tryMatchWithLandmark: wrong match"); + assert(lmk_match->check(_feat_ptr) && "tryMatchWithLandmark: wrong match"); } // // compute best rigid transformation matching @@ -982,7 +999,7 @@ bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPol LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const { - assert(_lmk_match->check() && "updateMatchWithLandmark: input _lmk_match wrong"); + assert(_lmk_match->check(_feat_ptr) && "updateMatchWithLandmark: input _lmk_match wrong"); WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: "); assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id()); @@ -1002,8 +1019,8 @@ bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPol } // valid match: update landmark match of last - assert(_lmk_match->check() && "updateMatchWithLandmark: wrongly updated"); - _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match + assert(_lmk_match->check(_feat_ptr) && "updateMatchWithLandmark: wrongly updated"); + _lmk_match = new_lmk_matches.begin()->second; // the first is the one with smallest difference from movement of the old match return true; } @@ -1011,7 +1028,7 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature FeatureMatchPolyline2DPtr ftr_match, LandmarkMatchPolyline2DPtr lmk_match_last) const { - assert(lmk_match_last->check() && "input lmk_match_last wrong"); + assert(lmk_match_last->check(ftr_match->feature_ptr_) && "input lmk_match_last wrong"); WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: "); std::cout << "ftr_match:"; std::cout << "\n\tincoming_from: " << ftr_match->feature_incoming_from_id_; @@ -1075,7 +1092,7 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature 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_; - assert(lmk_match_incoming->check() && "lmk_match_incoming wrongly concatenated"); + assert(lmk_match_incoming->check(ftr_match->feature_ptr_) && "lmk_match_incoming wrongly concatenated"); return lmk_match_incoming; } -- GitLab From 71724149c0fd2c9c66602c540f3abf6db800275b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 10:16:39 +0200 Subject: [PATCH 7/9] developing correctFeatureDrift --- .../processor_tracker_feature_polyline_2D.cpp | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index ce350fc5f..b6440603d 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -143,9 +143,30 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr FeatureBasePtr _incoming_feature) { //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 + /** After finding a valid feature match last-incoming, we have to check that the concatenated match + * incoming-landmark is also valid. + * + * We will only have to check IN BOTH EXTREMES if: + * 1. There are exceeding points in incoming-last match. + * AND + * 2. Landmark has exceeding points in the same extreme OR is closed. + * + * It this is the case, we have to check that the exceeding points match with the corresponding existing landmark points. + */ + + assert(matches_last_from_incoming_.count(_incoming_feature) != 0); + assert(landmark_match_map_.count(_last_feature) != 0); + + auto ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[_incoming_feature]); + auto lmk_last_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_last_feature]); + auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[_last_feature]->landmark_ptr_); + + // FRONT POINTS + if (ftr_match->feature_incoming_from_id_ != 0 && + (lmk_last_match->landmark_from_id_ > pl_lmk->getFirstId() || pl_lmk->isClosed())) + { + + } // int& new_first_points = best_ftr_match->feature_incoming_from_id_; // int new_last_points = pl_incoming->getNPoints() - best_ftr_match->feature_incoming_to_id_ - 1; -- GitLab From 826df69d1a5619724eb42d11849187d5c3725dc3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 15:02:51 +0200 Subject: [PATCH 8/9] implementing correctFeatureDrift --- .../processor_tracker_feature_polyline_2D.cpp | 36 +++++++++++++------ 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index b6440603d..b28e0e43e 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -155,22 +155,38 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr */ assert(matches_last_from_incoming_.count(_incoming_feature) != 0); - assert(landmark_match_map_.count(_last_feature) != 0); + assert(landmark_match_map_.count(_incoming_feature) != 0); - auto ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[_incoming_feature]); - auto lmk_last_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_last_feature]); - auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[_last_feature]->landmark_ptr_); + auto lmk_inc_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_incoming_feature]); + auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_inc_match->landmark_ptr_); // FRONT POINTS - if (ftr_match->feature_incoming_from_id_ != 0 && - (lmk_last_match->landmark_from_id_ > pl_lmk->getFirstId() || pl_lmk->isClosed())) + while (lmk_inc_match->feature_from_id_ != 0 && + (lmk_inc_match->landmark_from_id_ > pl_lmk->getFirstId() || pl_lmk->isClosed())) { + auto exceeding_inc = lmk_inc_match->feature_from_id_-1; + auto exceeding_lmk = pl_lmk->getPrevValidId(lmk_inc_match->landmark_from_id_); - } + // check good overlapping + Eigen::Vector3s lmk_point_expected(Eigen::Vector3s::Ones()); + lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk); + Eigen::Vector3s ftr_point = std::static_pointer_cast<FeaturePolyline2D>(_incoming_feature)->getPoints().col(exceeding_inc); + + Scalar sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm(); -// int& new_first_points = best_ftr_match->feature_incoming_from_id_; -// int new_last_points = pl_incoming->getNPoints() - best_ftr_match->feature_incoming_to_id_ - 1; -// int overlapping = best_ftr_match->feature_incoming_to_id_ - best_ftr_match->feature_incoming_from_id_; + // exceeding point not matched -> return not succeed + if (sq_dist > params_tracker_feature_polyline_->match_landmark_position_sq_norm_th) + { + WOLF_WARN("correctFeatureDrift: any point didn't match while checking incoming-lmk front exceeding point: feature point ", exceeding_inc, " - landmark point ", exceeding_lmk); + return false; + } + // add exceeding point to the landmark match + else + { + lmk_inc_match->feature_from_id_ = exceeding_inc; + lmk_inc_match->landmark_from_id_ = exceeding_lmk; + } + } return true; } -- GitLab From 9a82e395f4288f4410ed33a8b9afc97f90ee80db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 21 Jun 2019 15:03:01 +0200 Subject: [PATCH 9/9] probabilistic matching --- .../processor_tracker_feature_polyline_2D.h | 8 +- .../processor_tracker_feature_polyline_2D.cpp | 130 +++++++++++++++--- 2 files changed, 113 insertions(+), 25 deletions(-) diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h index 918a17961..9d4b448a4 100644 --- a/include/laser/processor/processor_tracker_feature_polyline_2D.h +++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h @@ -37,9 +37,11 @@ typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScala struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature { 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 match_feature_position_sq_norm_th; + Scalar match_feature_orientation_sq_norm_th; + Scalar match_landmark_position_sq_norm_th; + Scalar match_landmark_orientation_sq_norm_th; + bool use_probabilistic_match; Scalar class_position_error_th; unsigned int new_features_th; Scalar loop_time_th; diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index b28e0e43e..d2d71a9a8 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -833,17 +833,60 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline { //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: "); - // compute best sq distance matching - 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); + MatchPolyline2DPtr best_match = nullptr; + + if (params_tracker_feature_polyline_->use_probabilistic_match) + { + // Compute Covariance of projected points (only considering noise in T_last_incoming) and computing Jacobian using the mean of the points + Eigen::Vector2s mean_last_points = _ftr_L->getPoints().rowwise().mean().head(2); + Eigen::Vector2s J_th; + Scalar cos_th = _T_last_incoming_prior(0,0); + Scalar sin_th = _T_last_incoming_prior(1,0); + J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1), + cos_th * mean_last_points(0) - sin_th * mean_last_points(1); + Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_th + + J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_th*J_th.transpose(); + + // sq root of inverse + Eigen::Matrix2s sqrtOmega = Sigma.llt().matrixL().inverse(); + + // last expected points + Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints(); + + // Shape points in the equiprobabilistic space + Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_shaped = last_expected_points; + Eigen::Matrix<Scalar, 3, Eigen::Dynamic> inc_shaped = _ftr_I->getPoints(); + last_shaped.topRows<2>() = sqrtOmega*last_shaped.topRows<2>(); + inc_shaped.topRows<2>() = sqrtOmega*inc_shaped.topRows<2>(); + + // compute best sq Mahalanobis distance matching + best_match = computeBestSqDist(inc_shaped, // <--feature incoming points + _ftr_I->isFirstDefined(), + _ftr_I->isLastDefined(), + false, // <--feature is not closed for sure + last_shaped, // <--feature last points + _ftr_L->isFirstDefined(), + _ftr_L->isLastDefined(), + false, // <--feature is not closed for sure + 1); + } + else // not probabilistic match + { + // last expected points + Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints(); + + // compute best sq Mahalanobis distance matching + 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_position_sq_norm_th); + } + //valid match if (best_match != nullptr) { @@ -932,17 +975,60 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli { //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: "); - // compute best sq distance matching - 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); + MatchPolyline2DPtr best_match = nullptr; + + if (params_tracker_feature_polyline_->use_probabilistic_match) + { + // Compute Covariance of projected points (only considering noise in _T_feature_landmark_prior) and computing Jacobian using the mean of the points + Eigen::Vector2s mean_last_points = _lmk_ptr->computePointsGlobal().rowwise().mean().head(2); + Eigen::Vector2s J_th; + Scalar cos_th = _T_feature_landmark_prior(0,0); + Scalar sin_th = _T_feature_landmark_prior(1,0); + J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1), + cos_th * mean_last_points(0) - sin_th * mean_last_points(1); + Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_th + + J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_th*J_th.transpose(); + + // sq root of inverse + Eigen::Matrix2s sqrtOmega = Sigma.llt().matrixL().inverse(); + + // landmark expected points + Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal(); + + // Shape points in the equiprobabilistic space + Eigen::Matrix<Scalar, 3, Eigen::Dynamic> lmk_shaped = lmk_expected_points; + Eigen::Matrix<Scalar, 3, Eigen::Dynamic> feat_shaped = _feat_ptr->getPoints(); + lmk_shaped.topRows<2>() = sqrtOmega*lmk_shaped.topRows<2>(); + feat_shaped.topRows<2>() = sqrtOmega*feat_shaped.topRows<2>(); + + // compute best sq Mahalanobis distance matching + best_match = computeBestSqDist(lmk_shaped, // <--landmark points in local coordinates + _lmk_ptr->isFirstDefined(), + _lmk_ptr->isLastDefined(), + _lmk_ptr->isClosed(), + feat_shaped, // <--feature last points + _feat_ptr->isFirstDefined(), + _feat_ptr->isLastDefined(), + false, // <--feature is not closed for sure + 1); + } + else // not probabilistic match + { + // landmark expected points + Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal(); + + // compute best sq distance matching + 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) { -- GitLab