Add interpolate in delay and Doppler

CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable(blah2
   ${PROJECT_SOURCE_DIR}/process/clutter/WienerHopf.cpp
   ${PROJECT_SOURCE_DIR}/process/detection/CfarDetector1D.cpp
   ${PROJECT_SOURCE_DIR}/process/detection/Centroid.cpp
+  ${PROJECT_SOURCE_DIR}/process/detection/Interpolate.cpp
   ${PROJECT_SOURCE_DIR}/data/IqData.cpp
   ${PROJECT_SOURCE_DIR}/data/Map.cpp
   ${PROJECT_SOURCE_DIR}/data/Detection.cpp

src/blah2.cpp

@@ -14,6 +14,7 @@
 #include <Map.h>
 #include <Detection.h>
 #include <Centroid.h>
+#include <Interpolate.h>
 #include <Timing.h>
 #include <sys/types.h>
 #include <getopt.h>
@@ -88,6 +89,7 @@ int main(int argc, char **argv)
   std::string mapJson, detectionJson;
   Detection *detection;
   Detection *detection1;
+  Detection *detection2;
 
   // setup fftw multithread
   if (fftw_init_threads() == 0)
@@ -156,6 +158,7 @@ int main(int argc, char **argv)
   tree["process"]["detection"]["minDelay"] >> minDelay;
   tree["process"]["detection"]["minDoppler"] >> minDoppler;
   CfarDetector1D *cfarDetector1D = new CfarDetector1D(pfa, nGuard, nTrain, minDelay, minDoppler);
+  Interpolate *interpolate = new Interpolate(true, true);
 
   // setup process centroid
   uint16_t nCentroid;
@@ -229,7 +232,9 @@ int main(int argc, char **argv)
 
           // detection process
           detection1 = cfarDetector1D->process(map);
-          detection = centroid->process(detection1);
+          detection2 = centroid->process(detection1);
+          detection = interpolate->process(detection2, map);
+          
           uint64_t t4 = current_time_us();
           double delta_t4 = (double)(t4-t3) / 1000;
           timing_name.push_back("detector");
@@ -258,6 +263,7 @@ int main(int argc, char **argv)
           }
           delete detection;
           delete detection1;
+          delete detection2;
           
           // output radar data timer
           uint64_t t5 = current_time_us();

src/data/Map.cpp

@@ -98,6 +98,20 @@ void Map<T>::print()
   }
 }
 
+template <class T>
+uint32_t Map<T>::doppler_hz_to_bin(double dopplerHz)
+{
+  for (size_t i = 0; i < doppler.size(); i++)
+  {
+    if (dopplerHz == doppler[i])
+    {
+      return (int) i;
+    }
+  }
+  std::cout << "HUGE FUCKUP GRR" << std::endl;
+  return 0;
+}
+
 template <class T>
 std::string Map<T>::to_json()
 {

src/data/Map.h

@@ -89,6 +89,11 @@ public:
   /// @return Void.
   void print();
 
+  /// @brief Convert a Doppler value from Hz to bins.
+  /// @param dopplerHz Doppler value (Hz).
+  /// @return dopplerBin Doppler value (bins).
+  uint32_t doppler_hz_to_bin(double dopplerHz);
+
   /// @brief Generate JSON of the map and metadata.
   /// @return JSON string.
   std::string to_json();

src/process/detection/Interpolate.cpp

@@ -0,0 +1,93 @@
+#include "Interpolate.h"
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <stdint.h>
+#include <algorithm>
+
+// constructor
+Interpolate::Interpolate(bool _doDelay, bool _doDoppler)
+{
+  // input
+  doDelay = _doDelay;
+  doDoppler = _doDoppler;
+}
+
+Interpolate::~Interpolate()
+{
+}
+
+Detection *Interpolate::process(Detection *x, Map<std::complex<double>> *y)
+{ 
+  // store detections temporarily
+  std::vector<double> delay, doppler, snr;
+  delay = x->get_delay();
+  doppler = x->get_doppler();
+  snr = x->get_snr();
+
+  // interpolate data
+  double intDelay, intDoppler, intSnrDelay, intSnrDoppler, intSnr[3];
+  std::vector<double> delay2, doppler2, snr2;
+  std::deque<int> indexDelay = y->delay;
+  std::deque<double> indexDoppler = y->doppler;
+
+  // loop over every detection
+  for (size_t i = 0; i < snr.size(); i++)
+  {
+    // initialise interpolated values for bool flags
+    intDelay = delay[i];
+    intDoppler = doppler[i];
+    intSnrDelay = snr[i];
+    intSnrDoppler = snr[i];
+    // interpolate in delay
+    if (doDelay)
+    {
+      // check not on boundary
+      if (delay[i] == indexDelay[0] || delay[i] == indexDelay.back())
+      {
+        continue;
+      }
+      intSnr[0] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]-1-indexDelay[0]]))-y->noisePower;
+      intSnr[1] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]-indexDelay[0]]))-y->noisePower;
+      intSnr[2] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]+1-indexDelay[0]]))-y->noisePower;
+      // check detection has peak SNR of neighbours
+      if (intSnr[1] < intSnr[0] || intSnr[1] < intSnr[2])
+      {
+          std::cout << "Detection dropped (SNR of peak lower)" << std::endl;
+          continue;
+      }
+      intDelay = (intSnr[0]-intSnr[2])/(2*(intSnr[0]-(2*intSnr[1])+intSnr[2]));
+      intSnrDelay = intSnr[1] - (((intSnr[0]-intSnr[2])*intDelay)/4);
+      intDelay = delay[i] + intDelay;
+    }
+    // interpolate in Doppler
+    if (doDoppler)
+    {
+      // check not on boundary
+      if (doppler[i] == indexDoppler[0] || doppler[i] == indexDoppler.back())
+      {
+        continue;
+      }
+      intSnr[0] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])-1][delay[i]-indexDelay[0]]))-y->noisePower;
+      intSnr[1] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]-indexDelay[0]]))-y->noisePower;
+      intSnr[2] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])+1][delay[i]-indexDelay[0]]))-y->noisePower;
+      // check detection has peak SNR of neighbours
+      if (intSnr[1] < intSnr[0] || intSnr[1] < intSnr[2])
+      {
+          continue;
+      }
+      intDoppler = (intSnr[0]-intSnr[2])/(2*(intSnr[0]-(2*intSnr[1])+intSnr[2]));
+      intSnrDelay = intSnr[1] - (((intSnr[0]-intSnr[2])*intDoppler)/4);
+      intDoppler = doppler[i] + ((indexDoppler[1]-indexDoppler[0])*intDoppler);
+    }
+    // store interpolated detections
+    delay2.push_back(intDelay);
+    doppler2.push_back(intDoppler);
+    snr2.push_back(std::max(std::max(intSnrDelay, intSnrDoppler), snr[i]));
+  }
+
+  // create detection
+  Detection *detection = new Detection(delay2, doppler2, snr2);
+
+  return detection;
+}

src/process/detection/Interpolate.h

@@ -1,39 +1,37 @@
 /// @file Interpolate.h
 /// @class Interpolate
-/// @brief A class to remove duplicate target detections.
-/// @details If detection SNR is larger than neighbours, then remove.
+/// @brief A class to interpolate detection data using a quadratic curve.
+/// @details Interpolate in delay and Doppler. If 2 points either side have a higher SNR, then remove detection.
+/// References:
+/// - https://ccrma.stanford.edu/~jos/sasp/Quadratic_Interpolation_Spectral_Peaks.html
+/// - Fundamentals of Signal Processing (2nd), Richards, Section 5.3.6
 /// @author 30hours
-/// @todo Still a bug where sometimes 2 consecutive range detections get through.
+/// @todo Should I remove the detection pointer? Also on Centroid.
 
-#ifndef CENTROID_H
-#define CENTROID_H
+#ifndef INTERPOLATE_H
+#define INTERPOLATE_H
 
 #include <Map.h>
 #include <Detection.h>
-#include <stdint.h>
 
-class Centroid
+class Interpolate
 {
 private:
-  /// @brief Number of delay bins to check.
-  int8_t nDelay;
+  /// @brief True if interpolating over delay.
+  bool doDelay;
 
-  /// @brief Number of Doppler bins to check.
-  int8_t nDoppler;
-
-  /// @brief Doppler resolution to convert Hz to bins (Hz).
-  double resolutionDoppler;
+  /// @brief True if interpolating over Doppler.
+  bool doDoppler;
 
   /// @brief Pointer to detection data to store result.
   Detection *detection;
 
 public:
   /// @brief Constructor.
-  /// @param nDelay Number of delay bins to check.
-  /// @param nDoppler Number of Doppler bins to check.
-  /// @param resolutionDoppler Doppler resolution to convert Hz to bins (Hz).
+  /// @param doDelay True if interpolating over delay.
+  /// @param doDoppler True if interpolating over Doppler.
   /// @return The object.
-  Interpolate(int8_t nDelay, int8_t nDoppler, double resolutionDoppler);
+  Interpolate(bool doDelay, bool doDoppler);
 
   /// @brief Destructor.
   /// @return Void.
@@ -41,8 +39,8 @@ public:
 
   /// @brief Implement the 1D CFAR detector.
   /// @param x Detections from the 1D CFAR detector.
-  /// @return Centroided detections.
-  Detection *process(Detection *x);
+  /// @return Interpolated detections.
+  Detection *process(Detection *x, Map<std::complex<double>> *y);
 };
 
 #endif