- added hill climbing

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

gr-jay/lib/peak_detect_impl.cc

@@ -46,13 +46,13 @@ namespace gr {
 		, m_alpha_m(alpha_m)
 		, m_frameCount(0)
     {
-		m_pPeak = new float[vlen];
+		m_pPeakData = new float[vlen];
 		m_pX = new float[vlen];
 		m_pXs = new float[vlen];
 		m_pXm = new float[vlen];
 		m_pPeakPos = new int[vlen];
 		
-		memset(m_pPeak, 0, vlen*sizeof(int));
+		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));
@@ -71,7 +71,7 @@ namespace gr {
 		delete [] m_pXm;
 		delete [] m_pXs;
 		delete [] m_pX;
-		delete [] m_pPeak;
+		delete [] m_pPeakData;
     }
 
 	float peak_detect_impl::mean(float* pSrc, int length)
@@ -91,13 +91,22 @@ namespace gr {
 			pDst[i] = value;
 		}
 	}
+	int peak_detect_impl::threshold(float *pDst, float *pSrc, int length, float thresh)
+	{
+		for (int i=0; i < length; i++)
+		{
+			pDst[i] = (int)(pSrc[i] >= thresh);
+		}
 		
-	int peak_detect_impl::findPeakPos(int* pDst, float* pSrc, int length, float thresh)
+		return length;
+	}
+	
+	int peak_detect_impl::findPeakPos(int* pDst, float* pSrc, int length)
 	{
 		int count = 0;
 		for (int i=0; i < length; i++)
 		{
-			if (pSrc[i] > thresh)
+			if (pSrc[i] > 0)
 			{
 				pDst[count++] = i;
 			}
@@ -134,7 +143,6 @@ namespace gr {
 			for (int i = 0; i < nitems_per_block; i++)
 			{
 				float x = *(iptr++);
-				*(optr++) = x;
 				m_pX[i] = x;
 			}
 			
@@ -167,12 +175,58 @@ namespace gr {
 			// Peak detection
 			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_pPeak[i] = peak_dist;
+				m_pPeakData[i] = peak_dist;
+				*(optr++) = peak_dist;
 			}
 
 			// Create peak list
-			int numPeaks = findPeakPos(m_pPeakPos, m_pPeak, nitems_per_block, 10);
+			// Find peak candidates by thresholding
+			threshold(m_pPeakData, m_pPeakData, nitems_per_block, 20.f);
+			int numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
+			
+			// Search true maximum peak using hill climbing
+			int numPeaksLast = 0;
+			while(1)
+			{
+				int numPeaksClimbed = 0;
+				for (int i = 0; i < numPeaks; i++)
+				{
+					int pos = m_pPeakPos[i];
+					while(1)
+					{
+						float y0 = m_pX[pos];
+						int pos_p = std::min(nitems_per_block-1, pos+1);
+						int pos_n = std::max(0, pos-1);
+						if (m_pX[pos_p] > y0)
+						{
+							m_pPeakData[pos] = 0;
+							m_pPeakData[pos_n] = 0;
+							pos = pos_p;
+						}
+						else if (m_pX[pos_n] > y0)
+						{
+							m_pPeakData[pos] = 0;
+							m_pPeakData[pos_p] = 0;
+							pos = pos_n;
+						}
+						else
+						{
+							m_pPeakData[pos_n] = 0;
+							m_pPeakData[pos_p] = 0;
+							numPeaksClimbed++;
+							break;
+						}
+					}
+				}
+				if (numPeaksClimbed == numPeaksLast)
+				{
+					break;
+				}
+				numPeaksLast = numPeaksClimbed;
+			}
+			numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
 			for (int i = 0; i < numPeaks; i++)
 			{
 				// Create peak tags

gr-jay/lib/peak_detect_impl.h

@@ -32,7 +32,7 @@ namespace jay {
 		// Nothing to declare in this block.
 		float m_alpha_s;
 		float m_alpha_m;
-		float *m_pPeak;
+		float *m_pPeakData;
 		float *m_pX;
 		float *m_pXs;
 		float *m_pXm;
@@ -48,7 +48,9 @@ namespace jay {
 		int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
 		void fill(float *pDst, int length, float value);
 		float mean(float *pSrc, int length);
-		int findPeakPos(int *pDst, float *pSrc, int length, float thresh);
+		int threshold(float *pDst, float *pSrc, int length, float thresh);
+		int findPeakPos(int *pDst, float *pSrc, int length);
+		
     };
 
 } // namespace jay