- added peak boxes

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

gr-jay/lib/peak_detect_impl.cc

@@ -50,12 +50,19 @@ namespace gr {
 		m_pX = new float[vlen];
 		m_pXs = new float[vlen];
 		m_pXm = new float[vlen];
+		m_pXd = new float[vlen];
 		m_pPeakPos = new int[vlen];
+		m_pPeakBox_rel = new float[vlen];
+		m_pPeakBox_abs = new float[vlen];
 		
 		memset(m_pPeakData, 0, vlen*sizeof(int));
 		memset(m_pX, 0, vlen*sizeof(float));
 		memset(m_pXs, 0, vlen*sizeof(float));
 		memset(m_pXm, 0, vlen*sizeof(float));
+		memset(m_pXd, 0, vlen*sizeof(float));
+		memset(m_pPeakPos, 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();
@@ -67,7 +74,10 @@ namespace gr {
      */
     peak_detect_impl::~peak_detect_impl()
     {
+		delete [] m_pPeakBox_abs;
+		delete [] m_pPeakBox_rel;
 		delete [] m_pPeakPos;
+		delete [] m_pXd;
 		delete [] m_pXm;
 		delete [] m_pXs;
 		delete [] m_pX;
@@ -176,14 +186,14 @@ namespace gr {
 			for (int i = 0; i < nitems_per_block; i++)
 			{
 				// Create peak distances by subtracting Xm
-				float peak_dist = m_pX[i]-m_pXm[i];
-				m_pPeakData[i] = peak_dist;
-				*(optr++) = peak_dist;
+				float Xd = m_pX[i]-m_pXm[i];
+				m_pXd[i] = Xd;
+				*(optr++) = Xd;
 			}
 
 			// Create peak list
 			// Find peak candidates by thresholding
-			threshold(m_pPeakData, m_pPeakData, nitems_per_block, 20.f);
+			threshold(m_pPeakData, m_pXd, nitems_per_block, 20.f);
 			int numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
 			
 			// Search true maximum peak using hill climbing
@@ -227,14 +237,132 @@ namespace gr {
 				numPeaksLast = numPeaksClimbed;
 			}
 			numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
+			
+			// Sort peaks
+			std::vector<int> peakPos;
+			for (int i = 0; i < numPeaks; i++)
+			{
+				peakPos.push_back(m_pPeakPos[i]);
+			}
+			
+			Compare cmp(m_pPeakData);
+			std::sort(peakPos.begin(), peakPos.end(), cmp);
+
+			
+			// Construct peak boxes
+			numPeaks = 0;
+			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 < peakPos.size(); i++)
+			{
+				int pbh = 0;
+				int pos = peakPos[i];
+				int left = pos;
+				int right = pos;
+				float nom = m_pXd[pos] - 10;
+				int left_found = 0;
+				int right_found = 0;
+				while(!(left_found & right_found))
+				{
+					// Determine peak width left from center (pos)
+					if (m_pXd[left] > nom)
+					{
+						if (left > 0)
+						{
+							left--;
+						}
+						else
+						{
+							break;
+						}
+					}
+					else
+					{
+						left_found = 1;
+					}
+					
+					// Determine peak width right from center (pos)
+					if (m_pXd[right] > nom)
+					{
+						if (right < (nitems_per_block-1))
+						{
+							right++;
+						}
+						else
+						{
+							break;
+						}
+					}
+					else
+					{
+						right_found = 1;
+					}
+					
+					int pbh_left = pos - left;
+					int pbh_right = right - pos;
+
+					// Take larger peak width
+					pbh = std::max(pbh_left, pbh_right);
+
+					// Ensure peak box is at least pb_min
+					pbh = std::max(pbh, PB_MIN);
+					
+					if (pbh > PB_MAX)
+					{
+						pbh = 0;
+						break;
+					}
+				}
+				
+				if (pbh == 0)
+				{
+					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++)
+				{
+					if (m_pPeakBox_rel[n] > box_height_rel)
+					{
+						does_intersect = 1;
+						break;
+					}
+				}
+				if (does_intersect == 0)
+				{
+					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[numPeaks++] = pos;
+					}
+				}
+			}
+			
+			// Output peak box
+			for (int i = 0; i < nitems_per_block; i++)
+			{
+				*(optr++) = m_pPeakBox_abs[i];
+			}
+
+			// Output tags
 			for (int i = 0; i < numPeaks; i++)
 			{
 				// Create peak tags
+//				pmt::pmt_t dict = pmt::make_dict();
+//				pmt::dict_add(&dict, &pmt::string_to_symbol("id"), &pmt::from_long(i));
 				pmt::pmt_t tag_key = pmt::string_to_symbol("peak");
 				pmt::pmt_t tag_value = pmt::from_long(m_pPeakPos[i]);
 				add_item_tag(0, nitems_written(0) + m_pPeakPos[i], tag_key, tag_value, m_tag_id);
 			}
-		
+
 			m_frameCount++;
 		}
 
@@ -242,6 +370,14 @@ namespace gr {
 		return noutput_items;
     }
 
+	int peak_detect_impl::cmp(const void *pA, const void *pB)
+	{
+		int a = *((int*)pA);
+		int b = *((int*)pB);
+		
+		return m_pPeakData[a] > m_pPeakData[b];
+	}
+
   } /* namespace jay */
 } /* namespace gr */
 

gr-jay/lib/peak_detect_impl.h

@@ -28,19 +28,39 @@ namespace jay {
 
     class peak_detect_impl : public peak_detect
     {
-		private:
-		// Nothing to declare in this block.
+	private:
+		struct Compare
+		{
+			float const *m_pData;
+			Compare(float const *pData)
+			: m_pData(pData)
+			{
+				
+			}
+			bool operator() (int i,int j)
+			{
+				return (m_pData[i] < m_pData[j]);
+			}
+		  
+		};
+		
 		float m_alpha_s;
 		float m_alpha_m;
 		float *m_pPeakData;
 		float *m_pX;
 		float *m_pXs;
 		float *m_pXm;
+		float *m_pXd;
 		int *m_pPeakPos;
+		float *m_pPeakBox_rel;
+		float *m_pPeakBox_abs;
 		uint64_t m_frameCount;
 		pmt::pmt_t m_tag_id;
 		
-		public:
+		const int PB_MIN = 5;
+		const int PB_MAX = 37;
+		
+	public:
 		peak_detect_impl(float alpha_s, float alpha_m, int vlen);
 		~peak_detect_impl();
 
@@ -51,6 +71,7 @@ namespace jay {
 		int threshold(float *pDst, float *pSrc, int length, float thresh);
 		int findPeakPos(int *pDst, float *pSrc, int length);
 		
+		int cmp(const void *a, const void *b);
     };
 
 } // namespace jay