From 489bd89459a1c31e0146b983c673ac90cc01369e Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Fri, 4 Oct 2019 22:44:06 -0400 Subject: [PATCH] template update rate --- launch/odometry_eval.launch | 1 + launch/reconstruction_eval.launch | 1 + launch/slam_eval.launch | 1 + launch/slam_eval_kitti.launch | 1 + launch/slam_eval_t265.launch | 1 + launch/slam_eval_tum.launch | 3 +- launch/tracking_eval.launch | 1 + src/feature/lk_tracker.cc | 75 ++++++++++++++++++++++++------- src/feature/lk_tracker.h | 8 +++- src/ros/tracking_eval.cc | 4 +- 10 files changed, 76 insertions(+), 20 deletions(-) diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index 3d28804..8ba926b 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -30,6 +30,7 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 + tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 pnp_inlier_threshold: 3.0 diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index 890d0e1..fe348b1 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -25,6 +25,7 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 + tracker_template_update_rate: 1 min_features_per_region: 10 max_features_per_region: 5000 max_reprojection_error: 5.0 diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch index ae2f1a1..cbb2440 100644 --- a/launch/slam_eval.launch +++ b/launch/slam_eval.launch @@ -35,6 +35,7 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 + tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 pnp_inlier_threshold: 3.0 diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch index 887aaad..19baca9 100644 --- a/launch/slam_eval_kitti.launch +++ b/launch/slam_eval_kitti.launch @@ -35,6 +35,7 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 + tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 pnp_inlier_threshold: 3.0 diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch index 4f0e253..f98ae02 100644 --- a/launch/slam_eval_t265.launch +++ b/launch/slam_eval_t265.launch @@ -35,6 +35,7 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 + tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 pnp_inlier_threshold: 3.0 diff --git a/launch/slam_eval_tum.launch b/launch/slam_eval_tum.launch index 5f031eb..a17c496 100644 --- a/launch/slam_eval_tum.launch +++ b/launch/slam_eval_tum.launch @@ -35,7 +35,8 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 1000000.0 - min_features_per_region: 100 + tracker_template_update_rate: 1 + min_features_per_region: 10 max_features_per_region: 1000 pnp_inlier_threshold: 3.0 pnp_iterations: 3000 diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch index 41533a8..d78fc97 100644 --- a/launch/tracking_eval.launch +++ b/launch/tracking_eval.launch @@ -23,6 +23,7 @@ tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 3.0 tracker_error_threshold: 20.0 + tracker_template_update_rate: 1 min_features_per_region: 10 max_features_per_region: 999999 diff --git a/src/feature/lk_tracker.cc b/src/feature/lk_tracker.cc index 901219e..ffda2f2 100644 --- a/src/feature/lk_tracker.cc +++ b/src/feature/lk_tracker.cc @@ -7,52 +7,65 @@ namespace omni_slam namespace feature { -LKTracker::LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh, const float err_thresh, const int term_count, const double term_eps) +LKTracker::LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh, const float err_thresh, const int template_update_rate, const int term_count, const double term_eps) : windowSize_(window_size / pow(2, num_scales), window_size / pow(2, num_scales)), numScales_(num_scales), errThresh_(err_thresh), deltaPixErrThresh_(delta_pix_err_thresh), termCrit_(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, term_count, term_eps), + templateUpdateRate_(template_update_rate), prevFrame_(nullptr) { } void LKTracker::Init(data::Frame &init_frame) { + frameNum_ = 0; + prevId_ = init_frame.GetID(); prevFrame_ = &init_frame; + prevTemplateId_ = init_frame.GetID(); + prevImg_ = init_frame.GetImage().clone(); + if (init_frame.HasStereoImage()) + { + prevStereoImg_ = init_frame.GetStereoImage().clone(); + } } int LKTracker::Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo) { - if (prevFrame_ == nullptr) + if (prevImg_.empty()) { return 0; } - bool prevCompressed = prevFrame_->IsCompressed(); bool curCompressed = cur_frame.IsCompressed(); - int prevId = prevFrame_->GetID(); std::vector pointsToTrack; std::vector origKpt; std::vector origInx; std::vector stereoPointsToTrack; std::vector stereoOrigKpt; std::vector stereoOrigInx; + std::vector results; + std::vector stereoResults; for (int i = 0; i < landmarks.size(); i++) { data::Landmark &landmark = landmarks[i]; - const data::Feature *feat = landmark.GetObservationByFrameID(prevId); - if (feat != nullptr) + const data::Feature *feat = landmark.GetObservationByFrameID(prevTemplateId_); + const data::Feature *featPrev = landmark.GetObservationByFrameID(prevId_); + if (feat != nullptr && featPrev != nullptr) { pointsToTrack.push_back(feat->GetKeypoint().pt); + results.push_back(featPrev->GetKeypoint().pt); origKpt.push_back(feat->GetKeypoint()); origInx.push_back(i); } - if (cur_frame.HasStereoImage() && prevFrame_->HasStereoImage()) + if (cur_frame.HasStereoImage() && !prevStereoImg_.empty()) { - const data::Feature *stereoFeat = landmark.GetStereoObservationByFrameID(prevId); - if (stereoFeat != nullptr) + const data::Feature *stereoFeat = landmark.GetStereoObservationByFrameID(prevTemplateId_); + const data::Feature *stereoFeatPrev = landmark.GetStereoObservationByFrameID(prevId_); + if (stereoFeat != nullptr && stereoFeatPrev != nullptr) { stereoPointsToTrack.push_back(stereoFeat->GetKeypoint().pt); + stereoResults.push_back(stereoFeatPrev->GetKeypoint().pt); stereoOrigKpt.push_back(stereoFeat->GetKeypoint()); stereoOrigInx.push_back(i); } @@ -60,19 +73,41 @@ int LKTracker::Track(std::vector &landmarks, data::Frame &cur_fr } if (pointsToTrack.size() == 0) { + prevId_ = cur_frame.GetID(); prevFrame_ = &cur_frame; + if (++frameNum_ % templateUpdateRate_ == 0) + { + prevTemplateId_ = cur_frame.GetID(); + prevImg_ = cur_frame.GetImage().clone(); + if (cur_frame.HasStereoImage()) + { + prevStereoImg_ = cur_frame.GetStereoImage().clone(); + } + } return 0; } - std::vector results; std::vector status; std::vector err; - std::vector stereoResults; std::vector stereoStatus; std::vector stereoErr; - cv::calcOpticalFlowPyrLK(prevFrame_->GetImage(), cur_frame.GetImage(), pointsToTrack, results, status, err, windowSize_, numScales_, termCrit_, 0); + if (prevId_ == prevTemplateId_) + { + cv::calcOpticalFlowPyrLK(prevImg_, cur_frame.GetImage(), pointsToTrack, results, status, err, windowSize_, numScales_, termCrit_, 0); + } + else + { + cv::calcOpticalFlowPyrLK(prevImg_, cur_frame.GetImage(), pointsToTrack, results, status, err, windowSize_, numScales_, termCrit_, cv::OPTFLOW_USE_INITIAL_FLOW); + } if (stereoPointsToTrack.size() > 0) { - cv::calcOpticalFlowPyrLK(prevFrame_->GetStereoImage(), cur_frame.GetStereoImage(), stereoPointsToTrack, stereoResults, stereoStatus, stereoErr, windowSize_, numScales_, termCrit_, 0); + if (prevId_ == prevTemplateId_) + { + cv::calcOpticalFlowPyrLK(prevStereoImg_, cur_frame.GetStereoImage(), stereoPointsToTrack, stereoResults, stereoStatus, stereoErr, windowSize_, numScales_, termCrit_, 0); + } + else + { + cv::calcOpticalFlowPyrLK(prevStereoImg_, cur_frame.GetStereoImage(), stereoPointsToTrack, stereoResults, stereoStatus, stereoErr, windowSize_, numScales_, termCrit_, cv::OPTFLOW_USE_INITIAL_FLOW); + } } errors.clear(); int numGood = 0; @@ -127,16 +162,22 @@ int LKTracker::Track(std::vector &landmarks, data::Frame &cur_fr landmark.AddStereoObservation(feat); } } - if (prevCompressed) - { - prevFrame_->CompressImages(); - } if (curCompressed) { cur_frame.CompressImages(); } + prevId_ = cur_frame.GetID(); prevFrame_ = &cur_frame; + if (++frameNum_ % templateUpdateRate_ == 0) + { + prevTemplateId_ = cur_frame.GetID(); + prevImg_ = cur_frame.GetImage().clone(); + if (cur_frame.HasStereoImage()) + { + prevStereoImg_ = cur_frame.GetStereoImage().clone(); + } + } return numGood; } diff --git a/src/feature/lk_tracker.h b/src/feature/lk_tracker.h index 571be96..2f7634a 100644 --- a/src/feature/lk_tracker.h +++ b/src/feature/lk_tracker.h @@ -16,7 +16,7 @@ namespace feature class LKTracker : public Tracker { public: - LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int term_count = 50, const double term_eps = 0.01); + LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int template_update_rate = 1, const int term_count = 50, const double term_eps = 0.01); void Init(data::Frame &init_frame); int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo = true); @@ -26,6 +26,12 @@ class LKTracker : public Tracker const int numScales_; const float errThresh_; const float deltaPixErrThresh_; + const int templateUpdateRate_; + int frameNum_{0}; + cv::Mat prevImg_; + cv::Mat prevStereoImg_; + int prevTemplateId_; + int prevId_; data::Frame *prevFrame_; }; diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index eb7ffeb..b855a4d 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -18,6 +18,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod string detectorType; int trackerWindowSize; int trackerNumScales; + int trackerTemplateUpdateRate; double fivePointThreshold; int fivePointRansacIterations; double trackerDeltaPixelErrorThresh; @@ -33,6 +34,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod this->nhp_.param("tracker_checker_iterations", fivePointRansacIterations, 1000); this->nhp_.param("tracker_delta_pixel_error_threshold", trackerDeltaPixelErrorThresh, 5.0); this->nhp_.param("tracker_error_threshold", trackerErrorThresh, 20.); + this->nhp_.param("tracker_template_update_rate", trackerTemplateUpdateRate, 1); this->nhp_.param("min_features_per_region", minFeaturesRegion, 5); this->nhp_.param("max_features_per_region", maxFeaturesRegion, 5000); this->nhp_.getParam("detector_parameters", detectorParams); @@ -47,7 +49,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod ROS_ERROR("Invalid feature detector specified"); } - unique_ptr tracker(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh)); + unique_ptr tracker(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh, trackerTemplateUpdateRate)); unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, 0, 0));