diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index 072505f..9d2ba00 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -28,6 +28,7 @@ tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 min_features_per_region: 100 + max_features_per_region: 1000 pnp_inlier_threshold: 3.0 pnp_iterations: 3000 bundle_adjustment_max_iterations: 1000 diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index 662a3d9..686002b 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -25,6 +25,7 @@ tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 min_features_per_region: 10 + max_features_per_region: 5000 max_reprojection_error: 5.0 min_triangulation_angle: 3.0 bundle_adjustment_max_iterations: 1000 diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch index 6c2e25a..59d8744 100644 --- a/launch/slam_eval.launch +++ b/launch/slam_eval.launch @@ -31,6 +31,7 @@ tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 min_features_per_region: 100 + max_features_per_region: 1000 pnp_inlier_threshold: 3.0 pnp_iterations: 3000 max_reprojection_error: 5.0 @@ -40,6 +41,7 @@ bundle_adjustment_logging: true bundle_adjustment_num_threads: 20 local_bundle_adjustment_window: 0 + local_bundle_adjustment_interval: 0 stereo_matcher_window_size: 256 stereo_matcher_num_scales: 5 stereo_matcher_error_threshold: 20 diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch index f75348a..41533a8 100644 --- a/launch/tracking_eval.launch +++ b/launch/tracking_eval.launch @@ -24,6 +24,7 @@ tracker_delta_pixel_error_threshold: 3.0 tracker_error_threshold: 20.0 min_features_per_region: 10 + max_features_per_region: 999999 diff --git a/src/data/landmark.cc b/src/data/landmark.cc index f88b409..0662927 100644 --- a/src/data/landmark.cc +++ b/src/data/landmark.cc @@ -46,6 +46,13 @@ void Landmark::AddStereoObservation(Feature obs) stereoObs_.push_back(obs); } +void Landmark::RemoveLastObservation() +{ + int id = obs_.back().GetFrame().GetID(); + obs_.pop_back(); + idToIndex_.erase(id); +} + const std::vector& Landmark::GetObservations() const { return obs_; @@ -77,7 +84,7 @@ const int Landmark::GetFirstFrameID() const { if (obs_.size() > 0) { - return obs_[0].GetFrame().GetID(); + return obs_.front().GetFrame().GetID(); } return -1; } diff --git a/src/data/landmark.h b/src/data/landmark.h index c8b9f6d..d023834 100644 --- a/src/data/landmark.h +++ b/src/data/landmark.h @@ -19,6 +19,7 @@ class Landmark Landmark(); void AddObservation(Feature obs, bool compute_gnd = true); void AddStereoObservation(Feature obs); + void RemoveLastObservation(); const std::vector& GetObservations() const; std::vector& GetObservations(); const std::vector& GetStereoObservations() const; diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index b5c5a4f..0127aa9 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -11,16 +11,17 @@ namespace omni_slam namespace module { -TrackingModule::TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion) +TrackingModule::TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion, int maxFeaturesRegion) : detector_(std::move(detector)), tracker_(std::move(tracker)), fivePointChecker_(std::move(checker)), - minFeaturesRegion_(minFeaturesRegion) + minFeaturesRegion_(minFeaturesRegion), + maxFeaturesRegion_(maxFeaturesRegion) { } -TrackingModule::TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, std::unique_ptr &&checker, int minFeaturesRegion) - : TrackingModule(detector, tracker, checker, minFeaturesRegion) +TrackingModule::TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, std::unique_ptr &&checker, int minFeaturesRegion, int maxFeaturesRegion) + : TrackingModule(detector, tracker, checker, minFeaturesRegion, maxFeaturesRegion) { } @@ -40,29 +41,26 @@ void TrackingModule::Update(std::unique_ptr &frame) } vector trackErrors; - if (fivePointChecker_) + int tracks = tracker_->Track(landmarks_, *frames_.back(), trackErrors); + if (fivePointChecker_ && tracks > 0) { - std::vector landmarksTemp(landmarks_); - int tracks = tracker_->Track(landmarksTemp, *frames_.back(), trackErrors); - if (tracks > 0) + Matrix3d E; + std::vector inlierIndices; + fivePointChecker_->Compute(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices); + std::unordered_set inlierSet(inlierIndices.begin(), inlierIndices.end()); + for (int i = 0; i < landmarks_.size(); i++) { - Matrix3d E; - std::vector inlierIndices; - fivePointChecker_->Compute(landmarksTemp, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices); - for (int inx : inlierIndices) + if (landmarks_[i].IsObservedInFrame(frames_.back()->GetID()) && inlierSet.find(i) == inlierSet.end()) { - landmarks_[inx].AddObservation(*landmarksTemp[inx].GetObservationByFrameID(frames_.back()->GetID())); + landmarks_[i].RemoveLastObservation(); } } } - else - { - tracker_->Track(landmarks_, *frames_.back(), trackErrors); - } int i = 0; int numGood = 0; regionCount_.clear(); + regionLandmarks_.clear(); stats_.trackLengths.resize(landmarks_.size(), 0); for (data::Landmark& landmark : landmarks_) { @@ -78,6 +76,7 @@ void TrackingModule::Update(std::unique_ptr &frame) int rinx = min((int)(ri - rs_.begin()), (int)(rs_.size() - 1)) - 1; int tinx = min((int)(ti - ts_.begin()), (int)(ts_.size() - 1)) - 1; regionCount_[{rinx, tinx}]++; + regionLandmarks_[{rinx, tinx}].push_back(&landmark); const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID()); Vector2d pixelGnd; @@ -124,6 +123,8 @@ void TrackingModule::Update(std::unique_ptr &frame) stats_.frameTrackCounts.emplace_back(vector{frameNum_, numGood}); + Prune(); + (*next(frames_.rbegin()))->CompressImages(); frameNum_++; @@ -145,6 +146,29 @@ void TrackingModule::Redetect() } } +void TrackingModule::Prune() +{ + #pragma omp parallel for collapse(2) + for (int i = 0; i < rs_.size() - 1; i++) + { + for (int j = 0; j < ts_.size() - 1; j++) + { + if (regionCount_.find({i, j}) != regionCount_.end() && regionCount_.at({i, j}) > maxFeaturesRegion_) + { + sort(regionLandmarks_.at({i, j}).begin(), regionLandmarks_.at({i, j}).end(), + [](const data::Landmark *a, const data::Landmark *b) -> bool + { + return a->GetNumObservations() > b->GetNumObservations(); + }); + for (int c = 0; c < regionCount_.at({i, j}) - maxFeaturesRegion_; c++) + { + regionLandmarks_.at({i, j})[c]->RemoveLastObservation(); + } + } + } + } +} + std::vector& TrackingModule::GetLandmarks() { return landmarks_; diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h index 41eef62..bc0b0b7 100644 --- a/src/module/tracking_module.h +++ b/src/module/tracking_module.h @@ -28,8 +28,8 @@ class TrackingModule std::vector failureRadDists; }; - TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion = 5); - TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, std::unique_ptr &&checker, int minFeaturesRegion = 5); + TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion = 5, int maxFeaturesRegion = 5000); + TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, std::unique_ptr &&checker, int minFeaturesRegion = 5, int maxFeaturesRegion = 5000); void Update(std::unique_ptr &frame); void Redetect(); @@ -57,6 +57,8 @@ class TrackingModule const double trackFade_{0.99}; }; + void Prune(); + std::shared_ptr detector_; std::shared_ptr tracker_; std::shared_ptr fivePointChecker_; @@ -67,7 +69,9 @@ class TrackingModule const std::vector rs_{0, 0.1, 0.2, 0.3, 0.4, 0.49}; const std::vector ts_{-M_PI, -3 * M_PI / 4, -M_PI / 2, -M_PI / 4, 0, M_PI / 4, M_PI / 2, 3 * M_PI / 4, M_PI}; int minFeaturesRegion_; + int maxFeaturesRegion_; std::map, int> regionCount_; + std::map, std::vector> regionLandmarks_; Stats stats_; Visualization visualization_; diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc index 1b75b89..f2cfaef 100644 --- a/src/ros/slam_eval.cc +++ b/src/ros/slam_eval.cc @@ -18,6 +18,7 @@ SLAMEval::SLAMEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_priv : OdometryEval(nh, nh_private), ReconstructionEval(nh, nh_private), StereoEval(nh, nh_private) { this->nhp_.param("local_bundle_adjustment_window", baSlidingWindow_, 0); + this->nhp_.param("local_bundle_adjustment_interval", baSlidingInterval_, 0); } void SLAMEval::InitPublishers() @@ -32,7 +33,7 @@ void SLAMEval::ProcessFrame(unique_ptr &&frame) trackingModule_->Update(frame); odometryModule_->Update(trackingModule_->GetLandmarks(), *trackingModule_->GetFrames().back()); reconstructionModule_->Update(trackingModule_->GetLandmarks()); - if (baSlidingWindow_ > 0 && (frameNum_ + 1) % baSlidingWindow_ == 0) + if (baSlidingWindow_ > 0 && baSlidingInterval_ > 0 && (frameNum_ + 1) % baSlidingInterval_ == 0) { std::vector frameIds; frameIds.reserve(baSlidingWindow_); diff --git a/src/ros/slam_eval.h b/src/ros/slam_eval.h index 1b6cd01..ad762fe 100644 --- a/src/ros/slam_eval.h +++ b/src/ros/slam_eval.h @@ -28,6 +28,7 @@ class SLAMEval : public OdometryEval, ReconstructionEval, StereoEval void Visualize(cv_bridge::CvImagePtr &base_img, cv_bridge::CvImagePtr &base_stereo_img); int baSlidingWindow_; + int baSlidingInterval_; int frameNum_{0}; }; diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index f2ddeff..f08a808 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -24,6 +24,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod double trackerErrorThresh; map detectorParams; int minFeaturesRegion; + int maxFeaturesRegion; this->nhp_.param("detector_type", detectorType, string("GFTT")); this->nhp_.param("tracker_window_size", trackerWindowSize, 128); @@ -33,6 +34,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod this->nhp_.param("tracker_delta_pixel_error_threshold", trackerDeltaPixelErrorThresh, 5.0); this->nhp_.param("tracker_error_threshold", trackerErrorThresh, 20.); this->nhp_.param("min_features_per_region", minFeaturesRegion, 5); + this->nhp_.param("max_features_per_region", maxFeaturesRegion, 5000); this->nhp_.getParam("detector_parameters", detectorParams); unique_ptr detector; @@ -49,7 +51,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold)); - trackingModule_.reset(new module::TrackingModule(detector, tracker, checker, minFeaturesRegion)); + trackingModule_.reset(new module::TrackingModule(detector, tracker, checker, minFeaturesRegion, maxFeaturesRegion)); } template