diff --git a/gr-jay/lib/peak_detect_impl.cc b/gr-jay/lib/peak_detect_impl.cc index 19092c2..b523439 100644 --- a/gr-jay/lib/peak_detect_impl.cc +++ b/gr-jay/lib/peak_detect_impl.cc @@ -49,6 +49,9 @@ namespace gr { , m_alpha_s (alpha_s) , m_alpha_m(alpha_m) , m_frameCount(0) + , m_peakList(0) + , m_peakHistory(0) + , m_peak_id(0) { m_pPeakData = new float[vlen]; m_pX = new float[vlen]; @@ -56,7 +59,6 @@ namespace gr { m_pXm = new float[vlen]; m_pXd = new float[vlen]; m_pPeakPos = new int[vlen]; - m_pPeakPos_final = new int[vlen]; m_pPeakBox_rel = new float[vlen]; m_pPeakBox_abs = new float[vlen]; @@ -66,13 +68,15 @@ namespace gr { memset(m_pXm, 0, vlen*sizeof(float)); memset(m_pXd, 0, vlen*sizeof(float)); memset(m_pPeakPos, 0, vlen*sizeof(int)); - memset(m_pPeakPos_final, 0, vlen*sizeof(int)); memset(m_pPeakBox_rel, 0, vlen*sizeof(float)); memset(m_pPeakBox_abs, 0, vlen*sizeof(float)); std::stringstream str; str << name() << unique_id(); m_tag_id = pmt::string_to_symbol(str.str()); + + fprintf(stderr, "m_peakList.size = %u\n", m_peakHistory.size()); + fprintf(stderr, "m_peakList.max_size = %u\n", m_peakHistory.max_size()); } /* @@ -82,7 +86,6 @@ namespace gr { { delete [] m_pPeakBox_abs; delete [] m_pPeakBox_rel; - delete [] m_pPeakPos_final; delete [] m_pPeakPos; delete [] m_pXd; delete [] m_pXm; @@ -153,6 +156,7 @@ namespace gr { float xmean = mean(iptr, nitems_per_block); fill(m_pXs, nitems_per_block, xmean); fill(m_pXm, nitems_per_block, xmean); + m_peak_id = 0; } // Create input vector @@ -255,16 +259,12 @@ namespace gr { // ----------------------------------------------------------------- // Sort peaks // ----------------------------------------------------------------- - sort(m_pXd, m_pPeakPos, numPeaks); + sort(m_pXd, m_pPeakPos, numPeaks); // ----------------------------------------------------------------- - // Construct peak boxes + // Find peak boxes // ----------------------------------------------------------------- - int numPeaks_final = 0; - memset(m_pPeakPos_final, 0, nitems_per_block*sizeof(int)); - memcpy(m_pPeakBox_abs, m_pXm, nitems_per_block*sizeof(float)); - memset(m_pPeakBox_rel, 0, nitems_per_block*sizeof(float)); - + m_peakList.clear(); for (int i = 0; i < numPeaks; i++) { int pbh = 0; @@ -331,35 +331,80 @@ namespace gr { continue; } - // Use peak width for peak box - int pbox_start = std::max(0, pos-pbh); - int pbox_end = std::min(nitems_per_block-1, pos+pbh); - // Find intersection - float box_height_abs = m_pX[pos]; - float box_height_rel = m_pXd[pos]; - int does_intersect = 0; - for (int n=pbox_start; n <= pbox_end; n++) + Peak peak = { - if (m_pPeakBox_rel[n] > box_height_rel) + .id = m_peak_id, + .pos = (float)pos, + .half_width = (float)pbh, + .height_rel = m_pXd[pos], + .height_abs = m_pX[pos], + }; + int does_intersect = 0; + for (int j=0; j < m_peakList.size(); j++) + { + if (m_peakList[j].isInRange(peak)) { - does_intersect = 1; - break; + if (m_peakList[j].height_rel > peak.height_rel) + { + does_intersect = 1; + break; + } } } + if (!does_intersect) { - for (int n=pbox_start; n <= pbox_end; n++) - { - m_pPeakBox_abs[n] = box_height_abs; - m_pPeakBox_rel[n] = box_height_rel; - } - m_pPeakPos_final[numPeaks_final++] = pos; + m_peakList.push_back(peak); + m_peak_id++; } } // ----------------------------------------------------------------- - // Output peak box + // Peak management + // ----------------------------------------------------------------- + for (int i = 0; i < m_peakList.size(); i++) + { + Peak &peak = m_peakList[i]; + + bool found = false; + for (int j=0; j < m_peakHistory.size(); j++) + { + if (m_peakHistory[j].isInRange(peak)) + { + m_peakHistory[j].update(peak); + found = true; + break; + } + } + + if (!found) + { + m_peakHistory.push_back(peak); + } + } + + // ----------------------------------------------------------------- + // Create peak box data + // ----------------------------------------------------------------- + memcpy(m_pPeakBox_abs, m_pXm, nitems_per_block*sizeof(float)); + memset(m_pPeakBox_rel, 0, nitems_per_block*sizeof(float)); + for (int i = 0; i < m_peakHistory.size(); i++) + { + Peak &peak = m_peakHistory[i]; + // Use peak width for peak box + int pbox_start = std::max(0, (int)(peak.pos - peak.half_width + 0.5f)); + int pbox_end = std::min(nitems_per_block-1, (int)(peak.pos + peak.half_width + 0.5f)); + + for (int j=pbox_start; j <= pbox_end; j++) + { + m_pPeakBox_abs[j] = peak.height_abs; + m_pPeakBox_rel[j] = peak.height_rel; + } + } + + // ----------------------------------------------------------------- + // Output peak box data // ----------------------------------------------------------------- for (int i = 0; i < nitems_per_block; i++) { @@ -370,15 +415,16 @@ namespace gr { // ----------------------------------------------------------------- // Output tags // ----------------------------------------------------------------- - for (int i = 0; i < numPeaks_final; i++) + for (int i = 0; i < m_peakHistory.size(); i++) { // Create peak tags + Peak &peak = m_peakHistory[i]; pmt::pmt_t tag_key = pmt::string_to_symbol("id"); - pmt::pmt_t tag_value = pmt::from_long(i); - add_item_tag(DATA, nitems_written(DATA) + m_pPeakPos_final[i], tag_key, tag_value, m_tag_id); + pmt::pmt_t tag_value = pmt::from_long(peak.id); + add_item_tag(DATA, nitems_written(DATA) + peak.pos, tag_key, tag_value, m_tag_id); tag_key = pmt::string_to_symbol("pos"); - tag_value = pmt::from_long(m_pPeakPos_final[i]); - add_item_tag(DATA, nitems_written(DATA) + m_pPeakPos_final[i], tag_key, tag_value, m_tag_id); + tag_value = pmt::from_long(peak.pos); + add_item_tag(DATA, nitems_written(DATA) + peak.pos, tag_key, tag_value, m_tag_id); } m_frameCount++; diff --git a/gr-jay/lib/peak_detect_impl.h b/gr-jay/lib/peak_detect_impl.h index e8d40f5..e7f1ea6 100644 --- a/gr-jay/lib/peak_detect_impl.h +++ b/gr-jay/lib/peak_detect_impl.h @@ -28,6 +28,30 @@ namespace jay { class peak_detect_impl : public peak_detect { + struct Peak + { + uint64_t id; + float pos; + float half_width; + float height_rel; + float height_abs; + + bool isInRange(const Peak &other) + { + return (other.pos >= (pos - half_width)) and (other.pos <= (pos + half_width)); + } + + void update(const Peak &other) + { + float alpha = 0.5f; + float beta = 0.5f; + pos = alpha*pos + beta*other.pos; + half_width = alpha*half_width + beta*other.half_width; + height_rel = alpha*height_rel + beta*other.height_rel; + height_abs = alpha*height_abs + beta*other.height_abs; + } + }; + private: float m_framerate; @@ -42,11 +66,13 @@ namespace jay { float *m_pXm; float *m_pXd; int *m_pPeakPos; - int *m_pPeakPos_final; float *m_pPeakBox_rel; float *m_pPeakBox_abs; uint64_t m_frameCount; pmt::pmt_t m_tag_id; + std::vector m_peakList; + std::vector m_peakHistory; + uint64_t m_peak_id; public: peak_detect_impl(int vlen, float framerate, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m); @@ -60,7 +86,8 @@ namespace jay { int threshold(float *pDst, float *pSrc, int length, float thresh); int findPeakPos(int *pDst, float *pSrc, int length); - inline void sort(float const * const pData, int *pPos, int numPos) + template + inline void sort(T const * const pData, int *pPos, int numPos) { int n = numPos; do