diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h index e7acfc0a354713bc2f408effb4907f6576a15d58..a97ac9840b29f40c3638289c90b39614669c8856 100644 --- a/src/constraint_AHP.h +++ b/src/constraint_AHP.h @@ -185,18 +185,14 @@ inline bool ConstraintAHP::operator ()(const T* const _current_frame_p, } inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr, - const LandmarkAHPPtr& _lmk_ahp_ptr, - const ProcessorBasePtr& _processor_ptr, - bool _apply_loss_function, - ConstraintStatus _status) + const LandmarkAHPPtr& _lmk_ahp_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + ConstraintStatus _status) { // construct constraint ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status); - // link it to wolf tree <-- these pointers cannot be set at construction time - _lmk_ahp_ptr->getAnchorFrame()->addConstrainedBy(ctr_ahp); - _lmk_ahp_ptr->addConstrainedBy(ctr_ahp); - return ctr_ahp; } diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp index 049e0bf5cd760c15ba3e3f1f8988a5c716bf3c3a..0ce945f30c65152485f07a74a2fe7549937b0286 100644 --- a/src/processor_tracker_feature.cpp +++ b/src/processor_tracker_feature.cpp @@ -148,9 +148,20 @@ void ProcessorTrackerFeature::establishConstraints() FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first; FeatureBasePtr feature_in_last = pair_trkid_pair.second.second; - auto constraint = createConstraint(feature_in_last, feature_in_origin); - feature_in_last ->addConstraint(constraint); - feature_in_origin->addConstrainedBy(constraint); + auto ctr_ptr = createConstraint(feature_in_last, feature_in_origin); + feature_in_last ->addConstraint(ctr_ptr); + feature_in_origin->addConstrainedBy(ctr_ptr); + + if (ctr_ptr != nullptr) // constraint links + { + FrameBasePtr frm = ctr_ptr->getFrameOtherPtr(); + if (frm) + frm->addConstrainedBy(ctr_ptr); + CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr(); + if (cap) + cap->addConstrainedBy(ctr_ptr); + } + WOLF_DEBUG( "Constraint: track: " , feature_in_last->trackId(), " origin: " , feature_in_origin->id() , diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h index e51bd731282fa81c25d059ee23b8810041244ded..fc35c5465326d31eeba705523430ace7f3fda131 100644 --- a/src/processor_tracker_feature.h +++ b/src/processor_tracker_feature.h @@ -114,11 +114,11 @@ class ProcessorTrackerFeature : public ProcessorTracker virtual unsigned int processKnown(); /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \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 * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_correspondences) = 0; + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0; /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. * \param _origin_feature input feature in origin capture tracked @@ -153,7 +153,7 @@ class ProcessorTrackerFeature : public ProcessorTracker * * The function sets the member new_features_last_, the list of newly detected features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) = 0; + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) = 0; /** \brief Create a new constraint and link it to the wolf tree * \param _feature_ptr pointer to the parent Feature diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp index 5876bdd2ba39e0427f0b86b61d33f23a7e736ede..387333fd7151c0d59fc89cf3124ead333f098a52 100644 --- a/src/processor_tracker_feature_corner.cpp +++ b/src/processor_tracker_feature_corner.cpp @@ -70,14 +70,14 @@ void ProcessorTrackerFeatureCorner::advanceDerived() corners_last_ = std::move(corners_incoming_); } -unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _feature_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _features_last_in, + FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { - std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl; + std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl; Eigen::Vector3s expected_feature_pose; - for (auto feat_in_ptr : _feature_list_in) + for (auto feat_in_ptr : _features_last_in) { expected_feature_pose = R_current_prev_ * feat_in_ptr->getMeasurement().head<3>() + t_current_prev_; @@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0})); // move matched feature to list - _feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it); + _features_incoming_out.splice(_features_incoming_out.end(), corners_incoming_, feat_out_it); std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl; } @@ -99,7 +99,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& } } - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureCorner::voteForKeyFrame() @@ -107,11 +107,11 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th; } -unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { // in corners_last_ remain all not tracked corners - _feature_list_out = std::move(corners_last_); - return _feature_list_out.size(); + _features_incoming_out = std::move(corners_last_); + return _features_incoming_out.size(); } ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr _feature_ptr, diff --git a/src/processor_tracker_feature_corner.h b/src/processor_tracker_feature_corner.h index 93c4c7add4e1e2b7e6cc1b022dffa92a7a5370ea..efc40a1451b6015d86ce06e2538474bd0ae425a3 100644 --- a/src/processor_tracker_feature_corner.h +++ b/src/processor_tracker_feature_corner.h @@ -83,11 +83,11 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature void advanceDerived(); /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \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 * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -116,7 +116,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp index b0679cf6ac236e21684933c3601ea7e2ca574fe5..dc81126f93f78ef01930150ae69fadf802c173ab 100644 --- a/src/processor_tracker_feature_dummy.cpp +++ b/src/processor_tracker_feature_dummy.cpp @@ -13,13 +13,13 @@ namespace wolf unsigned int ProcessorTrackerFeatureDummy::count_ = 0; -unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _feature_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _features_last_in, + FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { - WOLF_INFO("tracking " , _feature_list_in.size() , " features..."); + WOLF_INFO("tracking " , _features_last_in.size() , " features..."); - for (auto feat_in : _feature_list_in) + for (auto feat_in : _features_last_in) { if (++count_ % 3 == 2) // lose one every 3 tracks { @@ -28,14 +28,14 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& else { FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance())); - _feature_list_out.push_back(ftr); - _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); + _features_incoming_out.push_back(ftr); + _feature_correspondences[_features_incoming_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" ); } } - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureDummy::voteForKeyFrame() @@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe; } -unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { WOLF_INFO("Detecting " , _max_features , " new features..." ); @@ -60,14 +60,14 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1))); - _feature_list_out.push_back(ftr); + _features_incoming_out.push_back(ftr); WOLF_INFO("feature " , ftr->id() , " detected!" ); } - WOLF_INFO(_feature_list_out.size() , " features detected!"); + WOLF_INFO(_features_incoming_out.size() , " features detected!"); - return _feature_list_out.size(); + return _features_incoming_out.size(); } ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr, diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h index 09ae7df58dc6b2ca628086a952ace56d72ae7702..3c163d276c282c38b69691b2fa8e0924b38fb3e8 100644 --- a/src/processor_tracker_feature_dummy.h +++ b/src/processor_tracker_feature_dummy.h @@ -32,11 +32,11 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature unsigned int n_feature_; /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \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 * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -65,7 +65,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/src/processor_tracker_feature_image.cpp b/src/processor_tracker_feature_image.cpp index 397a39cf6e3cbf111dbecabc5041e047ae0637cb..1a860c2170a547a03b8851b38ad9514698e77080 100644 --- a/src/processor_tracker_feature_image.cpp +++ b/src/processor_tracker_feature_image.cpp @@ -135,14 +135,14 @@ void ProcessorTrackerFeatureImage::postProcess() { } -unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) { KeyPointVector candidate_keypoints; cv::Mat candidate_descriptors; DMatchVector cv_matches; - for (auto feature_base_ptr : _feature_list_in) + for (auto feature_base_ptr : _features_last_in) { FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr); @@ -167,7 +167,7 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& (candidate_descriptors.row(cv_matches[0].trainIdx)), Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var); incoming_point_ptr->setIsKnown(feature_ptr->isKnown()); - _feature_list_out.push_back(incoming_point_ptr); + _features_incoming_out.push_back(incoming_point_ptr); _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score})); } @@ -185,8 +185,8 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& tracker_roi_.pop_back(); } } -// std::cout << "TrackFeatures - Number of Features tracked: " << _feature_list_out.size() << std::endl; - return _feature_list_out.size(); +// std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl; + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) @@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori } } -unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) { cv::Rect roi; KeyPointVector new_keypoints; @@ -277,7 +277,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& new_descriptors.row(index), Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var); point_ptr->setIsKnown(false); - _feature_list_out.push_back(point_ptr); + _features_incoming_out.push_back(point_ptr); active_search_ptr_->hitCell(new_keypoints[0]); diff --git a/src/processor_tracker_feature_image.h b/src/processor_tracker_feature_image.h index c8eeaa4bedfec9d0e2ba3e9fb404749e2df334f9..792c5d64527e7c6308fd071383be2ea22b82785b 100644 --- a/src/processor_tracker_feature_image.h +++ b/src/processor_tracker_feature_image.h @@ -91,7 +91,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature image_last_ = image_incoming_; } - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -119,7 +119,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature * * \return The number of detected Features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out); virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp index e68101044926a208ea031fbf005421b62c1c26c8..8e3f5d0d91f55d82a04e8fe8e93965906185daff 100644 --- a/src/processor_tracker_landmark.cpp +++ b/src/processor_tracker_landmark.cpp @@ -116,15 +116,6 @@ void ProcessorTrackerLandmark::createNewLandmarks() unsigned int ProcessorTrackerLandmark::processKnown() { - - //std::cout << "ProcessorTrackerLandmark::processKnown:" << std::endl; - //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl; - //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl; - //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl; - //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl; - // Find landmarks in incoming_ptr_ FeatureBaseList known_features_list_incoming; unsigned int n = findLandmarks(getProblem()->getMapPtr()->getLandmarkList(), @@ -132,22 +123,12 @@ unsigned int ProcessorTrackerLandmark::processKnown() // Append found incoming features incoming_ptr_->addFeatureList(known_features_list_incoming); - //std::cout << "end of processKnown:" << std::endl; - //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl; - //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl; - //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl; - //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl; return n; - } void ProcessorTrackerLandmark::establishConstraints() { - - //std::cout << "\tfeatures:" << last_ptr_->getFeatureList().size() << std::endl; - //std::cout << "\tcorrespondences: " << matches_landmark_from_last_.size() << std::endl; + // Loop all features in last_ptr_ for (auto last_feature : last_ptr_->getFeatureList()) { auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_; @@ -160,6 +141,9 @@ void ProcessorTrackerLandmark::establishConstraints() FrameBasePtr frm = ctr_ptr->getFrameOtherPtr(); if (frm) frm->addConstrainedBy(ctr_ptr); + CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr(); + if (cap) + cap->addConstrainedBy(ctr_ptr); } } } diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h index 479d0d8ea68622a2b4c924425c698534a37daf5d..bae94066b540cc1a566e9e9fe5c146262797c8bf 100644 --- a/src/processor_tracker_landmark.h +++ b/src/processor_tracker_landmark.h @@ -94,31 +94,24 @@ class ProcessorTrackerLandmark : public ProcessorTracker /** \brief Tracker function * \return The number of successful tracks. * + * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map. + * * This is the tracker function to be implemented in derived classes. * It operates on the \b incoming capture pointed by incoming_ptr_. * - * This should do one of the following, depending on the design of the tracker: - * - Track Features against other Features in the \b origin Capture. Tips: - * - An intermediary step of matching against Features in the \b last Capture makes tracking easier. - * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Constraints in \b last. - * - If required, correct the drift by re-comparing against the Features in origin. - * - The Constraints in \b last need to be transferred to \b incoming (moved, not copied). - * - * The function must generate the necessary Features in the \b incoming Capture, - * of the correct type, derived from FeatureBase. - * - * It must also generate the constraints, of the correct type, derived from ConstraintBase - * (through ConstraintAnalytic or ConstraintSparse). + * The function must populate the list of Features in the \b incoming Capture. + * Features need to be of the correct type, derived from FeatureBase. */ virtual unsigned int processKnown(); /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, - LandmarkMatchMap& _feature_landmark_correspondences) = 0; + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, + FeatureBaseList& _features_incoming_out, + LandmarkMatchMap& _feature_landmark_correspondences) = 0; /** \brief Vote for KeyFrame generation * @@ -147,7 +140,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) = 0; + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) = 0; /** \brief Creates a landmark for each of new_features_last_ **/ diff --git a/src/processor_tracker_landmark_corner.cpp b/src/processor_tracker_landmark_corner.cpp index c69b9964ed844402ba33f0781dfeeecb68ec7f9e..e687e5fcdb63d9e865b2b816a2cc69f05e33c48d 100644 --- a/src/processor_tracker_landmark_corner.cpp +++ b/src/processor_tracker_landmark_corner.cpp @@ -375,11 +375,11 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f _feature_ptr->getMeasurement()(3)); } -unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { // already computed since each scan is computed in preprocess() - _feature_list_out = std::move(corners_last_); - return _feature_list_out.size(); + _features_incoming_out = std::move(corners_last_); + return _features_incoming_out.size(); } ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(FeatureBasePtr _feature_ptr, diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h index 56619fcf1bb724b12beab03077c786a1016c6f1e..bb9eff8db34f6f46dcdd4c34d15ea7487ff1e74c 100644 --- a/src/processor_tracker_landmark_corner.h +++ b/src/processor_tracker_landmark_corner.h @@ -120,11 +120,11 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark } /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -146,7 +146,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); /** \brief Create one landmark * diff --git a/src/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp index c4a45acb259199c97018a20beccced79974dcea9..e806c23e5948de12922d86d6db58ff2b1be61b02 100644 --- a/src/processor_tracker_landmark_dummy.cpp +++ b/src/processor_tracker_landmark_dummy.cpp @@ -26,16 +26,16 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy() // } -unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmark_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmarks_in, + FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences) { std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl; - std::cout << "\t\t" << _landmark_list_in.size() << " landmarks..." << std::endl; + std::cout << "\t\t" << _landmarks_in.size() << " landmarks..." << std::endl; // loosing the track of the first 2 features auto landmarks_lost = 0; - for (auto landmark_in_ptr : _landmark_list_in) + for (auto landmark_in_ptr : _landmarks_in) { if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_) { @@ -44,15 +44,15 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList } else { - _feature_list_out.push_back(std::make_shared<FeatureBase>( + _features_incoming_out.push_back(std::make_shared<FeatureBase>( "POINT IMAGE", landmark_in_ptr->getDescriptor(), Eigen::MatrixXs::Identity(1,1))); - _feature_landmark_correspondences[_feature_list_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1); + _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1); std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl; } } - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() @@ -63,7 +63,7 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < 4; } -unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl; @@ -71,11 +71,11 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int for (unsigned int i = 1; i <= _max_features; i++) { n_feature_++; - _feature_list_out.push_back( + _features_incoming_out.push_back( std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1))); - std::cout << "\t\tfeature " << _feature_list_out.back()->getMeasurement() << " detected!" << std::endl; + std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl; } - return _feature_list_out.size(); + return _features_incoming_out.size(); } LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr) diff --git a/src/processor_tracker_landmark_dummy.h b/src/processor_tracker_landmark_dummy.h index 6df2393fd897bad27b1fd5398bdef10d289fcc0f..410b9c72c129ee2403b318bd6701f1d85b1f7cdc 100644 --- a/src/processor_tracker_landmark_dummy.h +++ b/src/processor_tracker_landmark_dummy.h @@ -31,11 +31,11 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark virtual void postProcess(); // implemented /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -56,7 +56,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); /** \brief Create one landmark * diff --git a/src/processor_tracker_landmark_image.cpp b/src/processor_tracker_landmark_image.cpp index 00d90d7ff63464d075c46812275b2b3f83280245..da3f1c7b499deb2e1ecfd73a03aafa1a83586bb1 100644 --- a/src/processor_tracker_landmark_image.cpp +++ b/src/processor_tracker_landmark_image.cpp @@ -144,8 +144,8 @@ void ProcessorTrackerLandmarkImage::postProcess() feat_lmk_found_.clear(); } -unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmark_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmarks_in, + FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences) { KeyPointVector candidate_keypoints; @@ -154,7 +154,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList Eigen::VectorXs current_state = getProblem()->getState(incoming_ptr_->getTimeStamp()); - for (auto landmark_in_ptr : _landmark_list_in) + for (auto landmark_in_ptr : _landmarks_in) { // project landmark into incoming capture @@ -194,7 +194,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList incoming_point_ptr->setScore(normalized_score); incoming_point_ptr->setExpectation(pixel); - _feature_list_out.push_back(incoming_point_ptr); + _features_incoming_out.push_back(incoming_point_ptr); _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score); @@ -215,9 +215,9 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList // else // std::cout << "NOT in the image" << std::endl; } -// std::cout << "\tNumber of Features tracked: " << _feature_list_out.size() << std::endl; - landmarks_tracked_ = _feature_list_out.size(); - return _feature_list_out.size(); +// std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl; + landmarks_tracked_ = _features_incoming_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerLandmarkImage::voteForKeyFrame() @@ -226,7 +226,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame() // return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe; } -unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { cv::Rect roi; KeyPointVector new_keypoints; @@ -261,7 +261,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int point_ptr->setIsKnown(false); point_ptr->setTrackId(point_ptr->id()); point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y)); - _feature_list_out.push_back(point_ptr); + _features_incoming_out.push_back(point_ptr); active_search_ptr_->hitCell(new_keypoints[0]); diff --git a/src/processor_tracker_landmark_image.h b/src/processor_tracker_landmark_image.h index 8d8b8fd425eddf59be0272304a81c623ed571fa1..6d86249e7bd10f151c074c96166ff15f2a730441 100644 --- a/src/processor_tracker_landmark_image.h +++ b/src/processor_tracker_landmark_image.h @@ -112,11 +112,11 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark //Pure virtual /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -136,7 +136,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark * * \return The number of detected Features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out); /** \brief Create one landmark * diff --git a/src/processor_tracker_landmark_polyline.h b/src/processor_tracker_landmark_polyline.h index 56d7bad9e3fe6d838901a1d70ebe0dd929a69986..3c4c6903171739283dff0cc47fe69fc1a824e304 100644 --- a/src/processor_tracker_landmark_polyline.h +++ b/src/processor_tracker_landmark_polyline.h @@ -132,11 +132,11 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark void resetDerived(); /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -158,7 +158,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); /** \brief Creates a landmark for each of new_features_last_ **/ @@ -213,11 +213,11 @@ inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(Proces { } -inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { // already computed since each scan is computed in preprocess() - _feature_list_out = std::move(polylines_last_); - return _feature_list_out.size(); + _features_incoming_out = std::move(polylines_last_); + return _features_incoming_out.size(); } inline void ProcessorTrackerLandmarkPolyline::advanceDerived() diff --git a/src/processors/processor_tracker_feature_trifocal.cpp b/src/processors/processor_tracker_feature_trifocal.cpp index 13ea679d23797827cdd522587f9d05aae6c2dfff..7ffb993755e37f242c9ab8df551fdb29f417d2d2 100644 --- a/src/processors/processor_tracker_feature_trifocal.cpp +++ b/src/processors/processor_tracker_feature_trifocal.cpp @@ -137,7 +137,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con } -unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) { // // DEBUG ===================================== // clock_t debug_tStart; @@ -177,7 +177,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i capture_last_->descriptors_.row(index_last), Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std); - _feature_list_out.push_back(last_point_ptr); + _features_incoming_out.push_back(last_point_ptr); // hit cell to acknowledge there's a tracked point in that cell capture_last_->grid_features_->hitTrackingCell(kp_last); @@ -195,17 +195,17 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i break; // There are no empty cells } - WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _feature_list_out.size() ); + WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _features_incoming_out.size() ); // // DEBUG // debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC; // WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_); // WOLF_TRACE("======== ========= ========="); - return _feature_list_out.size(); + return _features_incoming_out.size(); } -unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches) +unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) { // // DEBUG ===================================== // clock_t debug_tStart; @@ -214,7 +214,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis // WOLF_TRACE("======== TrackFeatures ========="); // // =========================================== - for (auto feature_base_last_ : _feature_list_in) + for (auto feature_base_last_ : _features_last_in) { FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_); @@ -236,7 +236,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis capture_incoming_->descriptors_.row(index_kp_incoming), Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std); - _feature_list_out.push_back(incoming_point_ptr); + _features_incoming_out.push_back(incoming_point_ptr); _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_last_, capture_incoming_->matches_normalized_scores_.at(index_kp_incoming)})); @@ -252,9 +252,9 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis // WOLF_TRACE("--> TIME: track: ",debug_comp_time_); // WOLF_TRACE("======== ========= ========="); - WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _feature_list_out.size() ); + WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _features_incoming_out.size() ); - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) diff --git a/src/processors/processor_tracker_feature_trifocal.h b/src/processors/processor_tracker_feature_trifocal.h index e31ee6f5f439d8cc51eec4ebad2250ff94c6d8fb..9065c718cffa4d0ebca5166391ca30730442e560 100644 --- a/src/processors/processor_tracker_feature_trifocal.h +++ b/src/processors/processor_tracker_feature_trifocal.h @@ -62,11 +62,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature virtual void configure(SensorBasePtr _sensor) override; /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \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 * \param _feature_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches) override; + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) override; /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. * \param _origin_feature input feature in origin capture tracked @@ -92,7 +92,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature * * The function sets the member new_features_last_, the list of newly detected features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) override; + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) override; /** \brief Create a new constraint and link it to the wolf tree * \param _feature_ptr pointer to the parent Feature