Extractor Code Cleanup v3

utiasASRL · Dec 12, 2024 · c22d1b3 · c22d1b3
1 parent fb7d713
commit c22d1b3
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 9 deletions.
diff --git a/main/src/vtr_radar/include/vtr_radar/detector/detector.hpp b/main/src/vtr_radar/include/vtr_radar/detector/detector.hpp
@@ -43,10 +43,9 @@ template <class PointT>
 class KStrongest : public Detector<PointT> {
  public:
   KStrongest() = default;
-  KStrongest(int kstrong, double threshold, double static_threshold, double minr,
+  KStrongest(int kstrong, double static_threshold, double minr,
              double maxr, double range_offset)
       : kstrong_(kstrong),
-        threshold_(threshold),
         static_threshold_(static_threshold),
         minr_(minr),
         maxr_(maxr),
@@ -59,7 +58,6 @@ class KStrongest : public Detector<PointT> {
 
  private:
   int kstrong_ = 10;
-  double threshold_ = 1.5;
   double static_threshold_ = 0.22;
   double minr_ = 2.0;
   double maxr_ = 100.0;

diff --git a/.../vtr_radar/include/vtr_radar/modules/preprocessing/extraction/radar_extraction_module.hpp b/.../vtr_radar/include/vtr_radar/modules/preprocessing/extraction/radar_extraction_module.hpp
@@ -45,7 +45,6 @@ class RadarExtractionModule : public tactic::BaseModule {
     // kstrongest
     struct {
       int kstrong = 10;
-      double threshold = 0.5;
       double static_threshold = 0.22;
     } kstrongest;
     // cen2018
@@ -80,7 +79,7 @@ class RadarExtractionModule : public tactic::BaseModule {
       int guard = 2;
       double threshold = 1.0;
       double threshold2 = 0.5;
-      double threshold3 = 0.09;
+      double threshold3 = 0.22;
     } modified_cacfar;
     // cago_cfar
     struct {

diff --git a/main/src/vtr_radar/src/modules/preprocessing/extraction/radar_extraction_module.cpp b/main/src/vtr_radar/src/modules/preprocessing/extraction/radar_extraction_module.cpp
@@ -103,7 +103,6 @@ auto RadarExtractionModule::Config::fromROS(
   config->save_pointcloud_overlay = node->declare_parameter<bool>(param_prefix + ".save_pointcloud_overlay", config->save_pointcloud_overlay);
 
   config->kstrongest.kstrong = node->declare_parameter<int>(param_prefix + ".kstrongest.kstrong", config->kstrongest.kstrong);
-  config->kstrongest.threshold = node->declare_parameter<double>(param_prefix + ".kstrongest.threshold", config->kstrongest.threshold);
   config->kstrongest.static_threshold = node->declare_parameter<double>(param_prefix + ".kstrongest.static_threshold", config->kstrongest.static_threshold);
 
   config->cen2018.zq = node->declare_parameter<double>(param_prefix + ".cen2018.zq", config->cen2018.zq);
@@ -230,9 +229,8 @@ void RadarExtractionModule::run_(QueryCache &qdata0, OutputCache &,
                  raw_point_cloud);
   } else if (config_->detector == "kstrongest") {
     KStrongest detector = KStrongest<PointWithInfo>(
-        config_->kstrongest.kstrong, config_->kstrongest.threshold,
-        config_->kstrongest.static_threshold, config_->minr, config_->maxr,
-        config_->range_offset);
+        config_->kstrongest.kstrong, config_->kstrongest.static_threshold,
+        config_->minr, config_->maxr, config_->range_offset);
     detector.run(fft_scan, radar_resolution, azimuth_times, azimuth_angles,
                  raw_point_cloud);
   } else if (config_->detector == "oscfar") {

diff --git a/main/src/vtr_tactic/src/tactic.cpp b/main/src/vtr_tactic/src/tactic.cpp
@@ -87,6 +87,8 @@ void Tactic::addRun(const bool) {
   graph_->addRun();
   // re-initialize the run
   first_frame_ = true;
+
+  // \todo change to VertexId::Invalid():
   current_vertex_id_ = VertexId((uint64_t)-1);
   // re-initialize the localization chain (path will be added later)
   chain_->reset();
@@ -172,6 +174,7 @@ bool Tactic::routeCompleted() const {
   const auto T_leaf_target_matrix = T_leaf_target.matrix();
   const auto angle = atan2(T_leaf_target_matrix(1, 0), T_leaf_target_matrix(0, 0));
 
+  CLOG(DEBUG, "tactic.eop") << "Translation: " << translation;
   if (chain_->trunkSequenceId() < (chain_->sequence().size() - 2)) {
     return false;
   }