From 8cd162f05f39802df1b1521c1f56b8cfbbb980a0 Mon Sep 17 00:00:00 2001 From: Mederic Fourmy <mederic.fourmy@gmail.com> Date: Mon, 6 Jun 2022 17:07:28 +0200 Subject: [PATCH] Add a break, some name changes and comments --- .../processor/processor_visual_odometry.h | 13 ++++++++++- src/processor/processor_visual_odometry.cpp | 22 +++++++++---------- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h index cd110482f..a7cde6c38 100644 --- a/include/vision/processor/processor_visual_odometry.h +++ b/include/vision/processor/processor_visual_odometry.h @@ -250,9 +250,20 @@ class ProcessorVisualOdometry : public ProcessorTracker * \brief Emplace a landmark corresponding to a track and initialize it with triangulation. * \param _feature_ptr a pointer to the feature used to create the new landmark * \return a pointer to the created landmark + * + * Implementation: Use rays of features detections in last frame and create a landmark at 1 meter (arbitrary) */ - LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr); + LandmarkBasePtr emplaceLandmarkNaive(FeatureBasePtr _feature_ptr); + /** + * \brief Emplace a landmark corresponding to a track and initialize it with triangulation. + * \param _feature_ptr a pointer to the feature used to create the new landmark + * \return a pointer to the created landmark. If null, the triangulation failed due to low parallax + * + * Implementation: try to triangulate a new landmark based on previous frames estimates. + * Apply a numerical test to asses if parallax is enough. + */ + LandmarkBasePtr emplaceLandmarkTriangulation(FeatureBasePtr _feature_ptr); /** \brief Advance the incoming Capture to become the last. * diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 0291290f2..cdefc35d3 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -231,11 +231,11 @@ unsigned int ProcessorVisualOdometry::processKnown() size_t id_feat_last = feat_pi_last->getKeyPoint().getId(); // if this feature id is in the last->incoming tracks of capture incoming, the track is continued - // otherwise we store the pair as a newly detected track (for processNew) + // the matched pairs are stored in tracks_map_li_matched_ which is used in processNew to select the point that have NOT been match as "new" TracksMap tracks_map_li = capture_image_incoming_->getTracksPrev(); if (tracks_map_li.count(id_feat_last)){ - // WOLF_DEBUG("A corresponding track has been found for id_feat_last ", id_feat_last ); auto kp_track_li = tracks_map_li.find(id_feat_last); + // create a feature using the corresponding WKeyPoint contained in incoming (hence the "second") auto feat_inco = FeatureBase::emplace<FeaturePointImage>( capture_image_incoming_, @@ -267,10 +267,8 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features) // So we need to reset the origin tracks of incoming used in preProcess so that they correspond to the future origin (currently last) capture_image_incoming_->setTracksOrigin(capture_image_incoming_->getTracksPrev()); - // We have matched the tracks in the track matrix with the last->incoming tracks - // stored in the TracksMap from getTracksPrev() + // We have matched the tracks in the track matrix with the last->incoming tracks stored in the TracksMap from getTracksPrev() // Now we need to add new tracks in the track matrix for the NEW tracks. - // // Use book-keeping prepared in processKnown: the TracksMap that have been matched were stored in tracks_map_li_matched_ // and here add tracks only for those that have not been matched @@ -296,9 +294,6 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features) } - - - void ProcessorVisualOdometry::establishFactors() { // Function only called when KF is created using last @@ -308,6 +303,8 @@ void ProcessorVisualOdometry::establishFactors() // using triangulation as a prior, using previous KF current estimates. Create a KF-Lmk factor for all these KFs. // For bookkeeping, define the landmark id as the track id. + WOLF_INFO(" establishFactors") + std::list<FeatureBasePtr> tracks_snapshot_last = track_matrix_.snapshotAsList(last_ptr_); if(tracks_snapshot_last.empty()) @@ -322,10 +319,13 @@ void ProcessorVisualOdometry::establishFactors() // verify if a landmark is associated to this track (BAD implementation) LandmarkBasePtr associated_lmk = nullptr; + + // OPTIM: try to reverse iterate the map for (auto lmk: getProblem()->getMap()->getLandmarkList()) { if (lmk->id() == feat_pi->trackId()){ associated_lmk = lmk; + break; } } @@ -344,8 +344,8 @@ void ProcessorVisualOdometry::establishFactors() // 2) create landmark if track is not associated with one and has enough length else if(track_matrix_.trackSize(feat->trackId()) >= params_visual_odometry_->min_track_length_for_landmark) { - // std::cout << "NEW valid track \\o/" << std::endl; - LandmarkBasePtr lmk = emplaceLandmark(feat_pi); + WOLF_INFO(" NEW valid track \\o/") + LandmarkBasePtr lmk = emplaceLandmarkNaive(feat_pi); lmk->setId(feat_pi->trackId()); // Add factors from all KFs of this track to the new lmk @@ -364,7 +364,7 @@ void ProcessorVisualOdometry::establishFactors() return; } -LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat) +LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkNaive(FeatureBasePtr _feat) { // Taken from processor_bundle_adjustment // Initialize the landmark in its ray (based on pixel meas) and using a arbitrary distance -- GitLab