diff --git a/demos/processor_visual_odometry.yaml b/demos/processor_visual_odometry.yaml index 4f19f6af07466e337b79a0bb27f77058ee139244..58d9160fbfcfff04ca7efc9ce75c9b0480933dda 100644 --- a/demos/processor_visual_odometry.yaml +++ b/demos/processor_visual_odometry.yaml @@ -67,5 +67,9 @@ max_nb_tracks: 100 # standard deviation of the pixel reprojection factor std_pix: 1 -# before creating a landmark, wait until the track is old enough -min_track_length_for_landmark: 3 +# Rules to create a new landmark from a track +lmk_creation: + # wait until the track is old enough + min_track_length_for_landmark: 3 + # enough pixel distance + min_pixel_dist: 0.0 diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h index 8af8156b1da6231ffaf8e4e78a3fc5a90d593a18..01228c448818874d228422caf363dc069b1b378f 100644 --- a/include/vision/capture/capture_image.h +++ b/include/vision/capture/capture_image.h @@ -76,7 +76,7 @@ class WKeyPoint // ID in a frame typedef std::unordered_map<size_t, WKeyPoint> KeyPointsMap; -// This maps the IDs of the Wolf KeyPoints that are tracked from a frame to the other. +// Map the IDs of the Wolf KeyPoints that are tracked from a frame to the other. // It takes the ID of a WKeyPoint and returns the ID of its track in another Frame. typedef std::unordered_map<size_t, size_t> TracksMap; diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h index 5368e965f2433b14498d7fcff23d70ed6fdddb65..63e2c92c075c66753fc8de154caad5787b5e958f 100644 --- a/include/vision/processor/processor_visual_odometry.h +++ b/include/vision/processor/processor_visual_odometry.h @@ -25,6 +25,7 @@ // wolf plugin includes +#include "vision/internal/config.h" #include "vision/math/pinhole_tools.h" #include "vision/sensor/sensor_camera.h" #include "vision/capture/capture_image.h" @@ -111,13 +112,19 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker, public Par } clahe; }; + struct LandmarkCreationParams + { + unsigned int min_track_length_for_landmark; + double min_pixel_dist; + }; + RansacParams ransac; KltParams klt; FastParams fast; GridParams grid; EqualizationParams equalization; + LandmarkCreationParams lmk_creation; double std_pix; - unsigned int min_track_length_for_landmark; ParamsProcessorVisualOdometry() = default; ParamsProcessorVisualOdometry(std::string _unique_name, const ParamsServer& _server): @@ -161,7 +168,9 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker, public Par grid.margin = _server.getParam<unsigned int>(prefix + _unique_name + "/grid/margin"); grid.separation = _server.getParam<unsigned int>(prefix + _unique_name + "/grid/separation"); - min_track_length_for_landmark = _server.getParam<unsigned int>(prefix + _unique_name + "/min_track_length_for_landmark"); + lmk_creation.min_track_length_for_landmark = _server.getParam<unsigned int>(prefix + _unique_name + "/lmk_creation/min_track_length_for_landmark"); + lmk_creation.min_pixel_dist = _server.getParam<double>(prefix + _unique_name + "/lmk_creation/min_pixel_dist"); + } std::string print() const override @@ -180,7 +189,8 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker, public Par + "grid.nbr_cells_v: " + std::to_string(grid.nbr_cells_v) + "\n" + "grid.margin: " + std::to_string(grid.margin) + "\n" + "grid.separation: " + std::to_string(grid.separation) + "\n" - + "min_track_length_for_landmark: " + std::to_string(min_track_length_for_landmark) + "\n"; + + "lmk_creation.min_track_length_for_landmark: " + std::to_string(lmk_creation.min_track_length_for_landmark) + "\n" + + "lmk_creation.min_pixel_dist: " + std::to_string(lmk_creation.min_pixel_dist) + "\n"; } }; @@ -330,7 +340,11 @@ class ProcessorVisualOdometry : public ProcessorTracker, public MotionProvider cv::Mat &E, VectorComposite &_pose_prev_curr); - /** \brief Tool to merge tracks + /** \brief Merge track maps between moments prev->curr and curr->next to give a track map between prev->next. + * + * \param tracks_prev_curr prev->curr track map + * \param tracks_curr_next curr->next track map + * \return merged track prev->next */ static TracksMap mergeTracks(const TracksMap& tracks_prev_curr, const TracksMap& tracks_curr_next); @@ -346,6 +360,11 @@ class ProcessorVisualOdometry : public ProcessorTracker, public MotionProvider const cv::Mat& _K, cv::Mat& cvMask); + /** \brief sequence of heuristics to decide if a track is worthy of becoming a landmark + * + */ + bool new_landmark_is_viable(int track_id); + private: void retainBest(std::vector<cv::KeyPoint> &_keypoints, int n); diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 2ca9d60decbae9b4fe73e2cf76818efbaaadb763..9bc899386f162921fd61cb66421b17f98795d2c1 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -67,16 +67,6 @@ void ProcessorVisualOdometry::configure(SensorBasePtr _sensor) params_visual_odometry_->grid.separation); } -TracksMap ProcessorVisualOdometry::mergeTracks(const TracksMap& tracks_prev_curr, const TracksMap& tracks_curr_next){ - TracksMap tracks_prev_next; - for (auto &match : tracks_prev_curr){ - if (tracks_curr_next.count(match.second)){ - tracks_prev_next[match.first] = tracks_curr_next.at(match.second); - } - } - return tracks_prev_next; -} - void ProcessorVisualOdometry::preProcess() { @@ -146,7 +136,9 @@ void ProcessorVisualOdometry::preProcess() // Outliers rejection with essential matrix cv::Mat E; VectorComposite pose_ol("PO", {3,4}); + int __attribute__((unused)) track_nb_before = tracks_origin_incoming.size(); bool success = filterWithEssential(mwkps_origin, mwkps_incoming, tracks_origin_incoming, E, pose_ol); + WOLF_DEBUG("After RANSAC: ", track_nb_before, "->", tracks_origin_incoming.size()) if (success){ // store result of recoverPose if RANSAC was handled well buffer_pose_cam_ol_.emplace(origin_ptr_->getTimeStamp(), std::make_shared<VectorComposite>(pose_ol)); @@ -226,6 +218,18 @@ void ProcessorVisualOdometry::preProcess() } +TracksMap ProcessorVisualOdometry::mergeTracks(const TracksMap& tracks_prev_curr, const TracksMap& tracks_curr_next){ + // merge tracks prev->curr and curr->next to get prev->next + TracksMap tracks_prev_next; + for (auto &match : tracks_prev_curr){ + if (tracks_curr_next.count(match.second)){ + tracks_prev_next[match.first] = tracks_curr_next.at(match.second); + } + } + return tracks_prev_next; +} + + unsigned int ProcessorVisualOdometry::processKnown() { @@ -304,6 +308,19 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features) } +bool ProcessorVisualOdometry::new_landmark_is_viable(int track_id) +{ + // a track should be old enough so that the keypoints can be considered stable (stable) + bool track_old_enough = track_matrix_.trackSize(track_id) >= params_visual_odometry_->lmk_creation.min_track_length_for_landmark; + + // Check if enough parallax has appeared while the camera moves through the scene so as the Landmark can be safely initialized. + // As a heuristic, we can check that the pixel distance between first detection time and current time is over a threshold. + double dist_pix = (track_matrix_.firstFeature(track_id)->getMeasurement() - track_matrix_.lastFeature(track_id)->getMeasurement()).norm(); + bool enough_parallax = dist_pix > params_visual_odometry_->lmk_creation.min_pixel_dist; + + return track_old_enough && enough_parallax; +} + void ProcessorVisualOdometry::establishFactors() { // Function only called when KF is created using last @@ -352,7 +369,7 @@ 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) + else if(new_landmark_is_viable(feat->trackId())) { WOLF_INFO(" NEW valid track \\o/") Track track_kf = track_matrix_.trackAtKeyframes(feat->trackId()); @@ -519,10 +536,12 @@ Eigen::Isometry3d ProcessorVisualOdometry::getTcw(TimeStamp _ts) const void ProcessorVisualOdometry::postProcess() { - // Delete tracks with no keyframes + + // Delete all dead tracks + auto snap_last = track_matrix_.snapshot(capture_image_last_); for (const auto& track_id : track_matrix_.trackIds()) { - if (track_matrix_.trackAtKeyframes(track_id).empty()) + if (snap_last.count(track_id) == 0) track_matrix_.remove(track_id); } @@ -655,6 +674,9 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev cv::Mat &_E, VectorComposite &_pose_prev_curr) { + // TODO: Check new RANSAC methods introduced in opencv 4.5.0 + // https://opencv.org/evaluating-opencvs-new-ransacs/ + // We need at least five tracks // TODO: this case is NOT handled by the rest of the algorithm currently if (_tracks_prev_curr.size() < 5) return false; @@ -683,12 +705,12 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev cv::Mat cvMask; // //////////////////////// - // // If we use rays then the + // // If we use rays then the camera matrix to pass is the identity // cv::Mat K = cv::Mat::eye(3,3,CV_32F); // double focal = (sen_cam_->getIntrinsicMatrix()(0,0) + // sen_cam_->getIntrinsicMatrix()(1,1)) / 2; - // // If using rays, thresh dived by the average focal + // // If using rays, thresh divided by the average focal // _E = cv::findEssentialMat(p2d_prev, // p2d_curr, // K, @@ -709,8 +731,9 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev // recover the pose relative pose if asked if (_pose_prev_curr.includesStructure("PO")){ - // modifies the mask of inliers -> maybe to change? - _pose_prev_curr = pose_from_essential_matrix(_E, p2d_prev, p2d_curr, Kcv_, cvMask); + // clone the mask not to change it. Otherwise it seems that the mask is reduced to all tracks being outliers... + cv::Mat cvMask_dummy = cvMask.clone(); + _pose_prev_curr = pose_from_essential_matrix(_E, p2d_prev, p2d_curr, Kcv_, cvMask_dummy); } // Let's remove outliers from tracksMap @@ -822,9 +845,7 @@ void ProcessorVisualOdometry::filter_last_incoming_tracks_from_ransac_result(con TracksMap& tracks_last_incoming_filtered, KeyPointsMap& mwkps_incoming_filtered) { /* - Use origin -> incoming track - O ---------------------> I - to filter last -> incoming track (and tracked keypoint map in incoming) + Filter last -> incoming track (and tracked keypoint map in incoming) L -------> I based of the tracks alive after RANSAC check on origin -> incoming O ---------------------> I diff --git a/test/processor_visual_odometry_gtest.yaml b/test/processor_visual_odometry_gtest.yaml index 784003cacaaf0a2c6cd36f717eb274d67b56f428..5cfe12ab88ab250538d53dbb65a69e3f87f774eb 100644 --- a/test/processor_visual_odometry_gtest.yaml +++ b/test/processor_visual_odometry_gtest.yaml @@ -52,5 +52,9 @@ max_nb_tracks: 30 # standard deviation of the pixel reprojection factor std_pix: 1 -# before creating a landmark, wait until the track is old enough -min_track_length_for_landmark: 20 +# Rules to create a new landmark from a track +lmk_creation: + # wait until the track is old enough + min_track_length_for_landmark: 20 + # enough pixel distance + min_pixel_dist: 0.0