Merge branch 'master' of github.com:ctu-mrs/uvdar_core

ctu-mrs · Mar 27, 2024 · 7f43d11 · 7f43d11
2 parents 4d49d11 + 43651fd
commit 7f43d11
Show file tree

Hide file tree

Showing 8 changed files with 35 additions and 45 deletions.
diff --git a/include/ht4dbt/ht4d.cpp b/include/ht4dbt/ht4d.cpp
@@ -36,6 +36,7 @@ HT4DBlinkerTracker::HT4DBlinkerTracker(
     int i_yaw_steps,
     int i_max_pixel_shift,
     cv::Size i_im_res,
+    int i_allowed_BER_per_seq,
     int i_nullify_radius,
     int i_reasonable_radius,
     double i_framerate) {
@@ -45,10 +46,11 @@ HT4DBlinkerTracker::HT4DBlinkerTracker(
   yaw_steps_      = i_yaw_steps;
   total_steps_    = pitch_steps_*yaw_steps_;
   framerate_     = i_framerate;
+  allowed_BER_per_seq_ = i_allowed_BER_per_seq;
   max_pixel_shift_ = i_max_pixel_shift;
   frame_scale_    = round(3 * framerate_ / 10);
   mask_width_     = 1 + 2 * max_pixel_shift_ * (frame_scale_ - 1);
-
+  
   double weight_coeff;
   if (CONSTANT_NEWER){
     weight_coeff = 0.625;
@@ -138,7 +140,7 @@ void HT4DBlinkerTracker::resetToZero(unsigned char *input, int steps) {
 
 void HT4DBlinkerTracker::setSequences(std::vector<std::vector<bool>> i_sequences){
   sequences_ = i_sequences;
-  matcher_ = std::make_unique<SignalMatcher>(sequences_);
+  matcher_ = std::make_unique<SignalMatcher>(sequences_, allowed_BER_per_seq_);
 }
 
 void HT4DBlinkerTracker::setDebug(bool i_debug, bool i_vis_debug) {

diff --git a/include/ht4dbt/ht4d.h b/include/ht4dbt/ht4d.h
@@ -45,6 +45,7 @@ class HT4DBlinkerTracker {
       int i_yaw_steps,
       int i_max_pixel_shift,
       cv::Size i_im_res,
+      int i_allowed_BER_per_seq,
       int i_nullify_radius,
       int i_reasonable_radius,
       double i_framerate);
@@ -322,6 +323,7 @@ class HT4DBlinkerTracker {
   int          reasonable_radius_;
   double       framerate_;
   int          max_pixel_shift_;
+  int          allowed_BER_per_seq_;
 
   cv::Size     im_res_;
   unsigned int im_area_;

diff --git a/include/ht4dbt/ht4d_cpu.cpp b/include/ht4dbt/ht4d_cpu.cpp
@@ -8,10 +8,10 @@ HT4DBlinkerTrackerCPU::HT4DBlinkerTrackerCPU (
     int i_yaw_steps,
     int i_max_pixel_shift,
     cv::Size i_im_res,
+    int i_allowed_BER_per_seq,
     int i_nullify_radius,
     int i_reasonable_radius,
-    double i_framerate) : HT4DBlinkerTracker(i_mem_steps, i_pitch_steps, i_yaw_steps, i_max_pixel_shift, i_im_res, i_nullify_radius,
-    i_reasonable_radius, i_framerate) {
+    double i_framerate) : HT4DBlinkerTracker(i_mem_steps, i_pitch_steps, i_yaw_steps, i_max_pixel_shift, i_im_res, i_allowed_BER_per_seq, i_nullify_radius, i_reasonable_radius, i_framerate) {
   std::cout << "Initiating HT4DBlinkerTrackerCPU..." << std::endl;
 
   hough_space_ = new unsigned int[im_area_ * pitch_steps_ * yaw_steps_];

diff --git a/include/ht4dbt/ht4d_cpu.h b/include/ht4dbt/ht4d_cpu.h
@@ -14,6 +14,7 @@ class HT4DBlinkerTrackerCPU : public HT4DBlinkerTracker {
       int i_yaw_steps,
       int i_max_pixel_shift,
       cv::Size i_im_res,
+      int i_allowed_BER_per_seq,
       int i_nullify_radius = 8,
       int i_reasonable_radius = 6,
       double i_framerate = 72);

diff --git a/include/ht4dbt/ht4d_gpu.cpp b/include/ht4dbt/ht4d_gpu.cpp
@@ -8,9 +8,10 @@ HT4DBlinkerTrackerGPU::HT4DBlinkerTrackerGPU (
     int i_yaw_steps,
     int i_max_pixel_shift,
     cv::Size i_im_res,
+    int i_allowed_BER_per_seq,
     int i_nullify_radius,
     int i_reasonable_radius,
-    double i_framerate) : HT4DBlinkerTracker(i_mem_steps, i_pitch_steps, i_yaw_steps, i_max_pixel_shift, i_im_res, i_nullify_radius,
+    double i_framerate) : HT4DBlinkerTracker(i_mem_steps, i_pitch_steps, i_yaw_steps, i_max_pixel_shift, i_im_res, i_allowed_BER_per_seq, i_nullify_radius,
     i_reasonable_radius, i_framerate) {
   std::cout << "Initiating HT4DBlinkerTrackerGPU..." << std::endl;
 

diff --git a/include/ht4dbt/ht4d_gpu.h b/include/ht4dbt/ht4d_gpu.h
@@ -26,6 +26,7 @@ class HT4DBlinkerTrackerGPU : public HT4DBlinkerTracker {
       int i_yaw_steps,
       int i_max_pixel_shift,
       cv::Size i_im_res,
+      int i_allowed_BER_per_seq,
       int i_nullify_radius = 8,
       int i_reasonable_radius = 6,
       double i_framerate = 72);

diff --git a/include/signal_matcher/signal_matcher.h b/include/signal_matcher/signal_matcher.h
@@ -4,25 +4,18 @@
 #define MATCH_ERROR_THRESHOLD 0
 /* #define MATCH_ERROR_THRESHOLD 1 */
 
-#include <iostream>
+#include <optional>
+
 namespace uvdar {
 
   class SignalMatcher{
     public:
-      SignalMatcher(std::vector<std::vector<bool>> i_sequences, int i_allowed_BER_per_seq_){
+      SignalMatcher(std::vector<std::vector<bool>> i_sequences, const int i_allowed_BER_per_seq, const std::optional<bool>& i_using_ami = std::nullopt){
 
-        allowed_BER_per_seq_ = i_allowed_BER_per_seq_;
-        sequences_ = i_sequences;
-        sequence_size_ = sequences_.at(0).size();
-        for (auto &curr_seq : sequences_){
-          auto curr_seq_copy = curr_seq;
-          // append the original signal at the end and delete last Bit e.g. 0,1 -> 0,1,0 
-          curr_seq.insert(curr_seq.end(),curr_seq_copy.begin(),curr_seq_copy.end()-1);
-        }
-      }
+        if(i_using_ami) using_ami = i_using_ami.value();
+        else using_ami = false; 
 
-      SignalMatcher(std::vector<std::vector<bool>> i_sequences){
-        //TODO sanitation
+        allowed_BER_per_seq_ = i_allowed_BER_per_seq;
         sequences_ = i_sequences;
         sequence_size_ = sequences_.at(0).size();
         for (auto &curr_seq : sequences_){
@@ -33,18 +26,26 @@ namespace uvdar {
       }
 
       int matchSignal(std::vector<bool> i_signal){
+
+        if(using_ami){
+          int valid_size = this->check_seq_size(i_signal);
+          if(valid_size != 1){
+            return valid_size;
+          }
+        }
+
         for (int s=0; s<(int)(sequences_.size()); s++){ // check sequences
           for (int i=0; i<sequence_size_; i++){ //slide along the duplicated sequence
             int match_errors = 0;
             for (int j=0; j<(int)(i_signal.size()); j++){ //iterate over signal
               if (sequences_.at(s).at(i+j) != i_signal.at(j)){
                 match_errors++;
               }
-              if (match_errors > MATCH_ERROR_THRESHOLD) {//TODO make settable
+              if (match_errors > allowed_BER_per_seq_) {
                 break;
               }
             }
-            if (match_errors <= MATCH_ERROR_THRESHOLD){
+            if (match_errors <= allowed_BER_per_seq_){
               return s;
             }
           }
@@ -53,38 +54,19 @@ namespace uvdar {
 
       }
 
-      int matchSignalWithCrossCorr(std::vector<bool> i_signal){
+    private:
+
+      int check_seq_size(const std::vector<bool> i_signal){
 
         if ((int)i_signal.size() == 0){
           return -1;
         }
 
-        if ((int)i_signal.size() < 3 || (int)i_signal.size() < sequence_size_){
+        if ((int)i_signal.size() < sequence_size_){
           return -3;
         }
-
-        for (int s=0; s<(int)(sequences_.size()); s++){ // check sequences
-          for (int i=0; i<sequence_size_; i++){ //slide along the duplicated sequence
-            int match_errors = 0;
-            for (int j=0; j<(int)(i_signal.size()); j++){ //iterate over signal
-              if (sequences_.at(s).at(i+j) != i_signal.at(j)){
-                match_errors++;
-              }
-              if (match_errors > allowed_BER_per_seq_) {//TODO make settable
-                break;
-              }
-            }
-            if (match_errors <= allowed_BER_per_seq_){
-              return s;
-            }
-          }
-        }
-        return -1;
+        return 1;
       }
-
-
-    private:
-
 
   /**
    * Atrributes
@@ -93,6 +75,7 @@ namespace uvdar {
   std::vector<std::vector<bool>> sequences_;
   int sequence_size_;
   int allowed_BER_per_seq_ = 0;
+  bool using_ami = true;
 
   };
 }

diff --git a/src/blink_processor.cpp b/src/blink_processor.cpp
@@ -460,7 +460,7 @@ namespace uvdar{
     for (size_t i = 0; i < _points_seen_topics_.size(); ++i) {
       ht4dbt_trackers_.push_back(
         std::make_shared<HT4DBlinkerTrackerCPU>(
-            _accumulator_length_, _pitch_steps_, _yaw_steps_, _max_pixel_shift_, cv::Size(0, 0), _nullify_radius_, _reasonable_radius_
+            _accumulator_length_, _pitch_steps_, _yaw_steps_, _max_pixel_shift_, cv::Size(0, 0), _allowed_BER_per_seq_,  _nullify_radius_, _reasonable_radius_
           )
         );
       ht4dbt_trackers_.back()->setDebug(_debug_, _visual_debug_);