diff --git a/src/module/reconstruction_module.cc b/src/module/reconstruction_module.cc index bec2701..dd09bc6 100644 --- a/src/module/reconstruction_module.cc +++ b/src/module/reconstruction_module.cc @@ -23,18 +23,18 @@ void ReconstructionModule::Update(std::vector &landmarks) triangulator_->Triangulate(landmarks); visualization_.Reserve(landmarks.size()); - for (int i = 0; i < lastLandmarksSize_; i++) + for (int i = lastLandmarksSize_; i < landmarks.size(); i++) + { + cv::Point2f pt = landmarks[i].GetObservations()[0].GetKeypoint().pt; + visualization_.AddPoint(pt); + } + for (int i = 0; i < landmarks.size(); i++) { if (landmarks[i].HasEstimatedPosition()) { visualization_.UpdatePoint(i, landmarks[i].GetEstimatedPosition()); } } - for (int i = lastLandmarksSize_; i < landmarks.size(); i++) - { - cv::Point2f pt = landmarks[i].GetObservations()[0].GetKeypoint().pt; - visualization_.AddPoint(pt); - } lastLandmarksSize_ = landmarks.size(); } diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index d8a7f09..cab740e 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -51,7 +51,7 @@ void TrackingModule::Update(std::unique_ptr &frame) int i = 0; int numGood = 0; - map, int> regionCount; + regionCount_.clear(); stats_.trackLengths.resize(landmarks_.size(), 0); for (data::Landmark& landmark : landmarks_) { @@ -66,7 +66,7 @@ void TrackingModule::Update(std::unique_ptr &frame) vector::const_iterator ti = upper_bound(ts_.begin(), ts_.end(), t); 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}]++; + regionCount_[{rinx, tinx}]++; Vector2d pixelGnd; if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd)) @@ -106,23 +106,28 @@ void TrackingModule::Update(std::unique_ptr &frame) } i++; } + + stats_.frameTrackCounts.emplace_back(vector{frameNum_, numGood}); + + (*next(frames_.rbegin()))->CompressImages(); + + frameNum_++; +} + +void TrackingModule::Redetect() +{ + int imsize = max(frames_.back()->GetImage().rows, frames_.back()->GetImage().cols); #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}) < minFeaturesRegion_) + if (regionCount_.find({i, j}) == regionCount_.end() || regionCount_.at({i, j}) < minFeaturesRegion_) { detector_->DetectInRadialRegion(*frames_.back(), landmarks_, rs_[i] * imsize, rs_[i+1] * imsize, ts_[j], ts_[j+1]); } } } - - stats_.frameTrackCounts.emplace_back(vector{frameNum_, numGood}); - - (*next(frames_.rbegin()))->CompressImages(); - - frameNum_++; } std::vector& TrackingModule::GetLandmarks() diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h index 5fa47bc..78ae27b 100644 --- a/src/module/tracking_module.h +++ b/src/module/tracking_module.h @@ -31,6 +31,7 @@ class TrackingModule TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, int minFeaturesRegion = 5); void Update(std::unique_ptr &frame); + void Redetect(); std::vector& GetLandmarks(); @@ -62,6 +63,7 @@ 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_; + std::map, int> regionCount_; Stats stats_; Visualization visualization_; diff --git a/src/ros/reconstruction_eval.cc b/src/ros/reconstruction_eval.cc index ce9e102..0e2f9be 100644 --- a/src/ros/reconstruction_eval.cc +++ b/src/ros/reconstruction_eval.cc @@ -43,8 +43,9 @@ void ReconstructionEval::InitPublishers() void ReconstructionEval::ProcessFrame(unique_ptr &&frame) { - TrackingEval::ProcessFrame(std::move(frame)); + trackingModule_->Update(frame); reconstructionModule_->Update(trackingModule_->GetLandmarks()); + trackingModule_->Redetect(); } void ReconstructionEval::Finish() diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index 16d726e..604a979 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -56,6 +56,7 @@ void TrackingEval::InitPublishers() void TrackingEval::ProcessFrame(unique_ptr &&frame) { trackingModule_->Update(frame); + trackingModule_->Redetect(); } void TrackingEval::GetResultsData(std::map>> &data)