- introduced peak history

git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@463 b431acfa-c32f-4a4a-93f1-934dc6c82436

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<float>(m_pXd, m_pPeakPos, numPeaks);
 			
 			// -----------------------------------------------------------------
-			// Construct peak boxes
+			// Find peak boxes
 			// -----------------------------------------------------------------
-			int numPeaks_final = 0;
+			m_peakList.clear();
-			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));
-			
 			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];
+				Peak peak = 
-				float box_height_rel = m_pXd[pos];
-				int does_intersect = 0;
-				for (int n=pbox_start; n <= pbox_end; n++)
 				{
-					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))
+					{
+						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_peakList.push_back(peak);
-					{
+					m_peak_id++;
-						m_pPeakBox_abs[n] = box_height_abs;
-						m_pPeakBox_rel[n] = box_height_rel;
-					}
-					m_pPeakPos_final[numPeaks_final++] = pos;
 				}
 			}
 			
 			// -----------------------------------------------------------------
-			// 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);
+				pmt::pmt_t tag_value = pmt::from_long(peak.id);
-				add_item_tag(DATA, nitems_written(DATA) + m_pPeakPos_final[i], tag_key, tag_value, m_tag_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]);
+				tag_value = pmt::from_long(peak.pos);
-				add_item_tag(DATA, nitems_written(DATA) + m_pPeakPos_final[i], tag_key, tag_value, m_tag_id);
+				add_item_tag(DATA, nitems_written(DATA) + peak.pos, tag_key, tag_value, m_tag_id);
 			}
 
 			m_frameCount++;

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<Peak> m_peakList;
+		std::vector<Peak> 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 <typename T>
+		inline void sort(T const * const pData, int *pPos, int numPos)
 		{
 			int n = numPos;
 			do