diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch index 2345d5f..6c2e25a 100644 --- a/launch/slam_eval.launch +++ b/launch/slam_eval.launch @@ -39,6 +39,7 @@ bundle_adjustment_loss_coefficient: 0.05 bundle_adjustment_logging: true bundle_adjustment_num_threads: 20 + local_bundle_adjustment_window: 0 stereo_matcher_window_size: 256 stereo_matcher_num_scales: 5 stereo_matcher_error_threshold: 20 diff --git a/src/module/reconstruction_module.cc b/src/module/reconstruction_module.cc index 276e342..e7a51ef 100644 --- a/src/module/reconstruction_module.cc +++ b/src/module/reconstruction_module.cc @@ -43,9 +43,9 @@ void ReconstructionModule::Update(std::vector &landmarks) lastLandmarksSize_ = landmarks.size(); } -void ReconstructionModule::BundleAdjust(std::vector &landmarks) +void ReconstructionModule::BundleAdjust(std::vector &landmarks, const std::vector &frame_ids) { - bundleAdjuster_->Optimize(landmarks); + bundleAdjuster_->Optimize(landmarks, frame_ids); for (int i = 0; i < lastLandmarksSize_; i++) { @@ -56,6 +56,12 @@ void ReconstructionModule::BundleAdjust(std::vector &landmarks) } } +void ReconstructionModule::BundleAdjust(std::vector &landmarks) +{ + std::vector temp; + BundleAdjust(landmarks, temp); +} + ReconstructionModule::Stats& ReconstructionModule::GetStats() { return stats_; diff --git a/src/module/reconstruction_module.h b/src/module/reconstruction_module.h index 9c67095..26a1696 100644 --- a/src/module/reconstruction_module.h +++ b/src/module/reconstruction_module.h @@ -30,6 +30,7 @@ class ReconstructionModule ReconstructionModule(std::unique_ptr &&triangulator, std::unique_ptr &&bundle_adjuster); void Update(std::vector &landmarks); + void BundleAdjust(std::vector &landmarks, const std::vector &frame_ids); void BundleAdjust(std::vector &landmarks); Stats& GetStats(); diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc index 3107860..17257dc 100644 --- a/src/optimization/bundle_adjuster.cc +++ b/src/optimization/bundle_adjuster.cc @@ -23,7 +23,7 @@ BundleAdjuster::BundleAdjuster(int max_iterations, double loss_coeff, int num_th solverOptions_.logging_type = log ? ceres::PER_MINIMIZER_ITERATION : ceres::SILENT; } -bool BundleAdjuster::Optimize(std::vector &landmarks) +bool BundleAdjuster::Optimize(std::vector &landmarks, const std::vector &frame_ids) { std::vector landmarkEstimates; landmarkEstimates.reserve(3 * landmarks.size()); @@ -32,9 +32,32 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) ceres::LossFunction *loss_function = new ceres::HuberLoss(lossCoeff_); for (const data::Landmark &landmark : landmarks) { + if (frame_ids.size() > 0) + { + bool observed = false; + for (int id : frame_ids) + { + if (landmark.IsObservedInFrame(id)) + { + observed = true; + break; + } + } + if (!observed) + { + continue; + } + } bool hasEstCameraPoses = false; for (const data::Feature &feature : landmark.GetObservations()) { + if (frame_ids.size() > 0) + { + if (std::find(frame_ids.begin(), frame_ids.end(), feature.GetFrame().GetID()) == frame_ids.end()) + { + continue; + } + } if (feature.GetFrame().HasEstimatedPose() && feature.GetFrame().IsEstimatedByLandmark(landmark.GetID())) { hasEstCameraPoses = true; @@ -63,6 +86,13 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) } for (const data::Feature &feature : landmark.GetObservations()) { + if (frame_ids.size() > 0) + { + if (std::find(frame_ids.begin(), frame_ids.end(), feature.GetFrame().GetID()) == frame_ids.end()) + { + continue; + } + } if (!feature.GetFrame().HasEstimatedPose() && feature.GetFrame().HasPose()) { if (!landmark.HasEstimatedPosition()) @@ -154,9 +184,32 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) int inx = 0; for (data::Landmark &landmark : landmarks) { + if (frame_ids.size() > 0) + { + bool observed = false; + for (int id : frame_ids) + { + if (landmark.IsObservedInFrame(id)) + { + observed = true; + break; + } + } + if (!observed) + { + continue; + } + } bool hasEstCameraPoses = false; for (const data::Feature &feature : landmark.GetObservations()) { + if (frame_ids.size() > 0) + { + if (std::find(frame_ids.begin(), frame_ids.end(), feature.GetFrame().GetID()) == frame_ids.end()) + { + continue; + } + } if (feature.GetFrame().HasEstimatedPose() && feature.GetFrame().IsEstimatedByLandmark(landmark.GetID())) { hasEstCameraPoses = true; @@ -181,8 +234,15 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) const Matrix pose = util::TFUtil::QuaternionTranslationToPoseMatrix(quat, t); frame.second->SetEstimatedInversePose(pose); } + problem_.reset(new ceres::Problem()); return true; } +bool BundleAdjuster::Optimize(std::vector &landmarks) +{ + std::vector tmp; + Optimize(landmarks, tmp); +} + } } diff --git a/src/optimization/bundle_adjuster.h b/src/optimization/bundle_adjuster.h index 3713a82..a8ffa71 100644 --- a/src/optimization/bundle_adjuster.h +++ b/src/optimization/bundle_adjuster.h @@ -14,6 +14,7 @@ class BundleAdjuster public: BundleAdjuster(int max_iterations = 500, double loss_coeff = 0.1, int num_threads = 1, bool log = false); + bool Optimize(std::vector &landmarks, const std::vector &frame_ids); bool Optimize(std::vector &landmarks); private: diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc index 876b8cb..1b75b89 100644 --- a/src/ros/slam_eval.cc +++ b/src/ros/slam_eval.cc @@ -17,6 +17,7 @@ namespace ros SLAMEval::SLAMEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) : OdometryEval(nh, nh_private), ReconstructionEval(nh, nh_private), StereoEval(nh, nh_private) { + this->nhp_.param("local_bundle_adjustment_window", baSlidingWindow_, 0); } void SLAMEval::InitPublishers() @@ -31,8 +32,23 @@ 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) + { + std::vector frameIds; + frameIds.reserve(baSlidingWindow_); + for (auto it = trackingModule_->GetFrames().rbegin(); it != trackingModule_->GetFrames().rend(); ++it) + { + frameIds.push_back((*it)->GetID()); + if (frameIds.size() >= baSlidingWindow_) + { + break; + } + } + reconstructionModule_->BundleAdjust(trackingModule_->GetLandmarks(), frameIds); + } trackingModule_->Redetect(); stereoModule_->Update(*trackingModule_->GetFrames().back(), trackingModule_->GetLandmarks()); + frameNum_++; } void SLAMEval::Finish() diff --git a/src/ros/slam_eval.h b/src/ros/slam_eval.h index 3e43a29..1b6cd01 100644 --- a/src/ros/slam_eval.h +++ b/src/ros/slam_eval.h @@ -26,6 +26,9 @@ class SLAMEval : public OdometryEval, ReconstructionEval, StereoEval void GetResultsData(std::map>> &data); void Visualize(cv_bridge::CvImagePtr &base_img); void Visualize(cv_bridge::CvImagePtr &base_img, cv_bridge::CvImagePtr &base_stereo_img); + + int baSlidingWindow_; + int frameNum_{0}; }; }