Skip to content
Snippets Groups Projects
Commit 31d15fa1 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

ProcessorLandmarkExternal less verbose and cleanup

parent b24a8cdd
No related branches found
No related tags found
3 merge requests!476spdlog version upgrade,!473Rerefactor,!472Merge ProcessorLandmarkExternal
Pipeline #17594 canceled
Stage: license
    Stage: build_and_test
      Stage: deploy_plugins
        Stage: final_plugins
          Stage: deploy_ros
            Hide whitespace changes
            Inline Side-by-side
            src/processor/processor_landmark_external.cpp
            + 119
            − 189
            View file @ 31d15fa1
            ......@@ -87,9 +87,9 @@ void ProcessorLandmarkExternal::preProcess()
            // add new feature
            new_features_incoming_.push_back(ftr);
            }
            WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ",
            new_features_incoming_.size(),
            " features in incoming capture");
            WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
            new_features_incoming_.size(),
            " features in incoming capture");
            }
            unsigned int ProcessorLandmarkExternal::processKnown()
            ......@@ -100,7 +100,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            // Track features from last_ptr_ to incoming_ptr_
            if (not last_ptr_) return 0;
            WOLF_INFO("Searching ", known_features_last_.size(), " tracked features...");
            WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
            auto pose_sen = getSensor()->getState("PO");
            auto pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
            auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
            ......@@ -109,30 +109,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            {
            auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
            bool matched = false;
            WOLF_INFO("Tracking feature last: ",
            feat_lmk_last->id(),
            " - ID: ",
            feat_lmk_last->getExternalId(),
            " - TYPE: ",
            feat_lmk_last->getExternalType(),
            " meas: ",
            feat_lmk_last->getMeasurement().transpose());
            // auto last_global_meas = (dim_ == 2 ? Eigen::VectorXd(pose_last.at('P') +
            // Rotation2Dd(pose_last.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_last->getMeasurement().head<2>()))
            // : Eigen::VectorXd(pose_last.at('P') +
            // Quaterniond(Vector4d(pose_last.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_last->getMeasurement().head<3>())));
            // WOLF_INFO("pose_last: ", pose_last);
            // WOLF_INFO("pose_sen: ", pose_sen);
            // auto last_global_meas =
            // pose_last.at('P') +
            // Rotation2Dd(pose_last.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>());
            // WOLF_INFO("Last global meas: ", last_global_meas.transpose());
            WOLF_DEBUG("Tracking feature last: ",
            feat_lmk_last->id(),
            " - ID: ",
            feat_lmk_last->getExternalId(),
            " - TYPE: ",
            feat_lmk_last->getExternalType(),
            " meas: ",
            feat_lmk_last->getMeasurement().transpose());
            // First we try to match by EXTERNAL_ID
            if (feat_lmk_last->getExternalId() != -1)
            ......@@ -142,32 +126,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            {
            auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
            WOLF_INFO("Feature incoming candidate (by ID): ",
            feat_lmk_incoming->id(),
            " - ID: ",
            feat_lmk_incoming->getExternalId(),
            " - TYPE: ",
            feat_lmk_incoming->getExternalType(),
            " meas: ",
            feat_lmk_incoming->getMeasurement().transpose());
            // auto global_meas =
            // (dim_ == 2
            // ? Eigen::VectorXd(pose_incoming.at('P') +
            // Rotation2Dd(pose_incoming.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_incoming->getMeasurement().head<2>()))
            // : Eigen::VectorXd(
            // pose_incoming.at('P') +
            // Quaterniond(Vector4d(pose_incoming.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_incoming->getMeasurement().head<3>())));
            // auto global_meas =
            // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_incoming->getMeasurement().head<2>());
            // WOLF_INFO("pose_incoming: ", pose_incoming);
            // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
            WOLF_DEBUG("Feature incoming candidate (by ID): ",
            feat_lmk_incoming->id(),
            " - ID: ",
            feat_lmk_incoming->getExternalId(),
            " - TYPE: ",
            feat_lmk_incoming->getExternalType(),
            " meas: ",
            feat_lmk_incoming->getMeasurement().transpose());
            // MATCH NECESSARY CONDITIONS:
            // 1. Same EXTERNAL_ID
            ......@@ -177,16 +143,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            (feat_lmk_incoming->getExternalType() == -1 or // cond 2
            feat_lmk_last->getExternalType() == -1 or //
            feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
            detectionDistance(
            feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
            params_tfle_->match_dist_th_id)
            detectionDistance( // cond 3
            feat_lmk_last,
            feat_lmk_incoming,
            pose_last,
            pose_incoming,
            pose_sen) < params_tfle_->match_dist_th_id)
            {
            WOLF_INFO("Feature last: ",
            feat_lmk_last->id(),
            " matched with feature ",
            feat_lmk_incoming->id(),
            " with landmark ID: ",
            feat_lmk_last->landmarkId());
            WOLF_DEBUG("Feature last: ",
            feat_lmk_last->id(),
            " matched with feature ",
            feat_lmk_incoming->id(),
            " with landmark ID: ",
            feat_lmk_last->landmarkId());
            matched = true;
            // set LANDMARK_ID if defined
            ......@@ -217,32 +186,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            while (feature_incoming_it != new_features_incoming_.end())
            {
            auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
            WOLF_INFO("Feature incoming candidate (by TYPE): ",
            feat_lmk_incoming->id(),
            " - ID: ",
            feat_lmk_incoming->getExternalId(),
            " - TYPE: ",
            feat_lmk_incoming->getExternalType(),
            " meas: ",
            feat_lmk_incoming->getMeasurement().transpose());
            // auto global_meas =
            // (dim_ == 2
            // ? Eigen::VectorXd(pose_incoming.at('P') +
            // Rotation2Dd(pose_incoming.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_incoming->getMeasurement().head<2>()))
            // : Eigen::VectorXd(
            // pose_incoming.at('P') +
            // Quaterniond(Vector4d(pose_incoming.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_incoming->getMeasurement().head<3>())));
            // auto global_meas =
            // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_incoming->getMeasurement().head<2>());
            // WOLF_INFO("pose_incoming: ", pose_incoming);
            // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
            WOLF_DEBUG("Feature incoming candidate (by TYPE): ",
            feat_lmk_incoming->id(),
            " - ID: ",
            feat_lmk_incoming->getExternalId(),
            " - TYPE: ",
            feat_lmk_incoming->getExternalType(),
            " meas: ",
            feat_lmk_incoming->getMeasurement().transpose());
            // MATCH NECESSARY CONDITIONS:
            // 1. Compatible EXTERNAL_ID (either not defined or same)
            ......@@ -252,16 +203,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            feat_lmk_last->getExternalId() == -1 or //
            feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
            feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType() and // cond 2
            detectionDistance(
            feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
            params_tfle_->match_dist_th_type)
            detectionDistance( // cond 3
            feat_lmk_last,
            feat_lmk_incoming,
            pose_last,
            pose_incoming,
            pose_sen) < params_tfle_->match_dist_th_type)
            {
            WOLF_INFO("Feature last: ",
            feat_lmk_last->id(),
            " matched with feature ",
            feat_lmk_incoming->id(),
            " with landmark ID: ",
            feat_lmk_last->landmarkId());
            WOLF_DEBUG("Feature last: ",
            feat_lmk_last->id(),
            " matched with feature ",
            feat_lmk_incoming->id(),
            " with landmark ID: ",
            feat_lmk_last->landmarkId());
            matched = true;
            // set LANDMARK_ID last -> incoming
            ......@@ -295,32 +249,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            {
            auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
            WOLF_INFO("Feature incoming candidate (by distance): ",
            feat_lmk_incoming->id(),
            " - ID: ",
            feat_lmk_incoming->getExternalId(),
            " - TYPE: ",
            feat_lmk_incoming->getExternalType(),
            " meas: ",
            feat_lmk_incoming->getMeasurement().transpose());
            // auto global_meas =
            // (dim_ == 2
            // ? Eigen::VectorXd(pose_incoming.at('P') +
            // Rotation2Dd(pose_incoming.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_incoming->getMeasurement().head<2>()))
            // : Eigen::VectorXd(
            // pose_incoming.at('P') +
            // Quaterniond(Vector4d(pose_incoming.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_incoming->getMeasurement().head<3>())));
            // auto global_meas =
            // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
            // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            // feat_lmk_incoming->getMeasurement().head<2>());
            // WOLF_INFO("pose_incoming: ", pose_incoming);
            // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
            WOLF_DEBUG("Feature incoming candidate (by distance): ",
            feat_lmk_incoming->id(),
            " - ID: ",
            feat_lmk_incoming->getExternalId(),
            " - TYPE: ",
            feat_lmk_incoming->getExternalType(),
            " meas: ",
            feat_lmk_incoming->getMeasurement().transpose());
            // MATCH NECESSARY CONDITIONS:
            // 1. Compatible EXTERNAL_ID (either not defined or same)
            ......@@ -335,12 +271,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
            params_tfle_->match_dist_th_unknown)
            {
            WOLF_INFO("Feature last: ",
            feat_lmk_last->id(),
            " matched with feature ",
            feat_lmk_incoming->id(),
            " with landmark ID: ",
            feat_lmk_last->landmarkId());
            WOLF_DEBUG("Feature last: ",
            feat_lmk_last->id(),
            " matched with feature ",
            feat_lmk_incoming->id(),
            " with landmark ID: ",
            feat_lmk_last->landmarkId());
            matched = true;
            // set LANDMARK_ID last -> incoming
            ......@@ -369,7 +305,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            }
            WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked.");
            }
            WOLF_INFO("Tracked ", known_features_incoming_.size(), " features.");
            WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
            // Add new features (not tracked) as known features
            WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
            ......@@ -467,7 +403,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
            {
            auto track_ids_last = track_matrix_.trackIds(last_ptr_);
            WOLF_INFO("Active feature tracks: ", track_ids_last.size());
            WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
            // number of tracks longer than track_length_th
            auto n_tracks = 0;
            ......@@ -483,55 +419,49 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
            // Necessary condition: active valid tracks
            bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
            WOLF_INFO("vote_min_features: ",
            vote_min_features,
            " - Active feature tracks longer than ",
            params_tfle_->track_length_th,
            ": ",
            n_tracks,
            " (should be equal or bigger than ",
            params_tfle_->min_features_for_keyframe,
            ")");
            WOLF_DEBUG("vote_min_features: ",
            vote_min_features,
            " - Active feature tracks longer than ",
            params_tfle_->track_length_th,
            ": ",
            n_tracks,
            " (should be equal or bigger than ",
            params_tfle_->min_features_for_keyframe,
            ")");
            bool vote_new_features(true), vote_time_span(true);
            if (vote_min_features and origin_ptr_)
            {
            // Sufficient condition: new valid tracks
            vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
            WOLF_INFO("vote_new_features: ",
            vote_new_features,
            " - n_new_tracks = ",
            n_new_tracks,
            " (should be equal or bigger than ",
            params_tfle_->new_features_for_keyframe,
            ")");
            WOLF_DEBUG("vote_new_features: ",
            vote_new_features,
            " - n_new_tracks = ",
            n_new_tracks,
            " (should be equal or bigger than ",
            params_tfle_->new_features_for_keyframe,
            ")");
            // Sufficient condition: time span
            vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
            WOLF_INFO("vote_time_span: ",
            vote_time_span,
            " - time_span = ",
            last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
            " (should be bigger than ",
            params_tfle_->time_span,
            ")");
            WOLF_DEBUG("vote_time_span: ",
            vote_time_span,
            " - time_span = ",
            last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
            " (should be bigger than ",
            params_tfle_->time_span,
            ")");
            }
            bool vote = vote_min_features and (vote_new_features or vote_time_span);
            WOLF_INFO((vote ? "Vote " : "Do NOT vote "), "for KF");
            WOLF_DEBUG((vote ? "Vote " : "Do NOT vote "), "for KF");
            return vote;
            }
            void ProcessorLandmarkExternal::establishFactors()
            {
            WOLF_INFO("establishFactors");
            // if (origin_ptr_ == last_ptr_) return;
            // // reset n_tracks_origin_
            // lmks_ids_origin_.clear();
            // will emplace a factor (and landmark if needed) for each known feature in last with long tracks
            FactorBasePtrList fac_list;
            auto track_ids_last = track_matrix_.trackIds(last_ptr_);
            ......@@ -542,19 +472,19 @@ void ProcessorLandmarkExternal::establishFactors()
            {
            auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
            WOLF_INFO("Feature ",
            feature->id(),
            ": ID: ",
            feature->landmarkId(),
            " EXTERNAL_ID: ",
            feature->getExternalId(),
            " TYPE: ",
            feature->getExternalType());
            WOLF_DEBUG("Feature ",
            feature->id(),
            ": ID: ",
            feature->landmarkId(),
            " EXTERNAL_ID: ",
            feature->getExternalId(),
            " TYPE: ",
            feature->getExternalType());
            // not enough long track
            WOLF_INFO_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
            "Track NOT long enough ",
            track_matrix_.trackSize(track_id));
            WOLF_DEBUG_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
            "Track NOT long enough ",
            track_matrix_.trackSize(track_id));
            if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue;
            // Landmark match
            ......@@ -563,7 +493,7 @@ void ProcessorLandmarkExternal::establishFactors()
            {
            // LOOP CLOSURE
            // By ID
            WOLF_INFO("Searching Loop closure by ID...");
            WOLF_DEBUG("Searching Loop closure by ID...");
            if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1)
            {
            auto lmk_list = getProblem()->getMap()->getLandmarkList();
            ......@@ -576,16 +506,16 @@ void ProcessorLandmarkExternal::establishFactors()
            detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id)
            {
            lmk = lmk_ext;
            WOLF_INFO("Found loop closure by EXTERNAL_ID with ", lmk->id());
            WOLF_DEBUG("Found loop closure by EXTERNAL_ID with ", lmk->id());
            break;
            }
            WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
            "Landmark with EXTERNAL_ID found but not matched due to distance: ",
            detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
            WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
            "Landmark with EXTERNAL_ID found but not matched due to distance: ",
            detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
            }
            }
            // By TYPE
            WOLF_INFO("Searching Loop closure by TYPE...");
            WOLF_DEBUG("Searching Loop closure by TYPE...");
            if (not lmk and params_tfle_->close_loops_by_type and feature->getExternalType() != -1)
            {
            auto lmk_list = getProblem()->getMap()->getLandmarkList();
            ......@@ -598,12 +528,12 @@ void ProcessorLandmarkExternal::establishFactors()
            detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type)
            {
            lmk = lmk_ext;
            WOLF_INFO("Found loop closure by TYPE with ", lmk->id());
            WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id());
            break;
            }
            WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
            "Landmark with TYPE found but not matched due to distance: ",
            detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
            WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
            "Landmark with TYPE found but not matched due to distance: ",
            detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
            }
            // update Landmark EXTERNAL_ID (if available)
            ......@@ -614,17 +544,17 @@ void ProcessorLandmarkExternal::establishFactors()
            if (not lmk)
            {
            lmk = emplaceLandmark(feature);
            WOLF_INFO("Emplaced new landmark ", lmk->id());
            WOLF_DEBUG("Emplaced new landmark ", lmk->id());
            }
            // set LANDMARK_ID in all features of the track
            for (auto feat_pair : track_matrix_.track(track_id))
            {
            feat_pair.second->setLandmarkId(lmk->id());
            WOLF_INFO("Setting landmark id in feature ",
            feat_pair.second->id(),
            " landmark_id: ",
            feat_pair.second->landmarkId());
            WOLF_DEBUG("Setting landmark id in feature ",
            feat_pair.second->id(),
            " landmark_id: ",
            feat_pair.second->landmarkId());
            }
            }
            // landmarkId already set
            ......@@ -651,7 +581,7 @@ void ProcessorLandmarkExternal::establishFactors()
            }
            }
            WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
            WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
            }
            FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
            ......
            0% or .
            You are about to add 0 people to the discussion. Proceed with caution.
            Finish editing this message first!
            Please register or to comment