feature region pruning

raphaelchang · Sep 20, 2019 · 3bb3dfc · 3bb3dfc
1 parent 2c340c5
commit 3bb3dfc
Show file tree

Hide file tree

Showing 11 changed files with 67 additions and 22 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
         </rosparam>
     </node>
 </launch>
diff --git 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<Feature>& 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
@@ -19,6 +19,7 @@ class Landmark
     Landmark();
     void AddObservation(Feature obs, bool compute_gnd = true);
     void AddStereoObservation(Feature obs);
+    void RemoveLastObservation();
     const std::vector<Feature>& GetObservations() const;
     std::vector<Feature>& GetObservations();
     const std::vector<Feature>& GetStereoObservations() const;

diff --git 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<feature::Detector> &detector, std::unique_ptr<feature::Tracker> &tracker, std::unique_ptr<odometry::FivePoint> &checker, int minFeaturesRegion)
+TrackingModule::TrackingModule(std::unique_ptr<feature::Detector> &detector, std::unique_ptr<feature::Tracker> &tracker, std::unique_ptr<odometry::FivePoint> &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<feature::Detector> &&detector, std::unique_ptr<feature::Tracker> &&tracker, std::unique_ptr<odometry::FivePoint> &&checker, int minFeaturesRegion)
-    : TrackingModule(detector, tracker, checker, minFeaturesRegion)
+TrackingModule::TrackingModule(std::unique_ptr<feature::Detector> &&detector, std::unique_ptr<feature::Tracker> &&tracker, std::unique_ptr<odometry::FivePoint> &&checker, int minFeaturesRegion, int maxFeaturesRegion)
+    : TrackingModule(detector, tracker, checker, minFeaturesRegion, maxFeaturesRegion)
 {
 }
 
@@ -40,29 +41,26 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
     }
 
     vector<double> trackErrors;
-    if (fivePointChecker_)
+    int tracks = tracker_->Track(landmarks_, *frames_.back(), trackErrors);
+    if (fivePointChecker_ && tracks > 0)
     {
-        std::vector<data::Landmark> landmarksTemp(landmarks_);
-        int tracks = tracker_->Track(landmarksTemp, *frames_.back(), trackErrors);
-        if (tracks > 0)
+        Matrix3d E;
+        std::vector<int> inlierIndices;
+        fivePointChecker_->Compute(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices);
+        std::unordered_set<int> inlierSet(inlierIndices.begin(), inlierIndices.end());
+        for (int i = 0; i < landmarks_.size(); i++)
         {
-            Matrix3d E;
-            std::vector<int> 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<data::Frame> &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<data::Frame> &frame)
 
     stats_.frameTrackCounts.emplace_back(vector<int>{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<data::Landmark>& TrackingModule::GetLandmarks()
 {
     return landmarks_;

diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h
@@ -28,8 +28,8 @@ class TrackingModule
         std::vector<double> failureRadDists;
     };
 
-    TrackingModule(std::unique_ptr<feature::Detector> &detector, std::unique_ptr<feature::Tracker> &tracker, std::unique_ptr<odometry::FivePoint> &checker, int minFeaturesRegion = 5);
-    TrackingModule(std::unique_ptr<feature::Detector> &&detector, std::unique_ptr<feature::Tracker> &&tracker, std::unique_ptr<odometry::FivePoint> &&checker, int minFeaturesRegion = 5);
+    TrackingModule(std::unique_ptr<feature::Detector> &detector, std::unique_ptr<feature::Tracker> &tracker, std::unique_ptr<odometry::FivePoint> &checker, int minFeaturesRegion = 5, int maxFeaturesRegion = 5000);
+    TrackingModule(std::unique_ptr<feature::Detector> &&detector, std::unique_ptr<feature::Tracker> &&tracker, std::unique_ptr<odometry::FivePoint> &&checker, int minFeaturesRegion = 5, int maxFeaturesRegion = 5000);
 
     void Update(std::unique_ptr<data::Frame> &frame);
     void Redetect();
@@ -57,6 +57,8 @@ class TrackingModule
         const double trackFade_{0.99};
     };
 
+    void Prune();
+
     std::shared_ptr<feature::Detector> detector_;
     std::shared_ptr<feature::Tracker> tracker_;
     std::shared_ptr<odometry::FivePoint> fivePointChecker_;
@@ -67,7 +69,9 @@ class TrackingModule
     const std::vector<double> rs_{0, 0.1, 0.2, 0.3, 0.4, 0.49};
     const std::vector<double> 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<std::pair<int, int>, int> regionCount_;
+    std::map<std::pair<int, int>, std::vector<data::Landmark*>> regionLandmarks_;
 
     Stats stats_;
     Visualization visualization_;

diff --git 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<true>(nh, nh_private), ReconstructionEval<true>(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<data::Frame> &&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<int> frameIds;
         frameIds.reserve(baSlidingWindow_);

diff --git a/src/ros/slam_eval.h b/src/ros/slam_eval.h
@@ -28,6 +28,7 @@ class SLAMEval : public OdometryEval<true>, ReconstructionEval<true>, 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
@@ -24,6 +24,7 @@ TrackingEval<Stereo>::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod
     double trackerErrorThresh;
     map<string, double> 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<Stereo>::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<feature::Detector> detector;
@@ -49,7 +51,7 @@ TrackingEval<Stereo>::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod
 
     unique_ptr<odometry::FivePoint> 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 <bool Stereo>