- refactored

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

gr-jay/lib/peak_detect_impl.cc

@@ -53,23 +53,19 @@ namespace gr {
 		, m_peakHistory(0)
 		, m_peak_id(0)
     {
-		m_pPeakData = new float[vlen];
 		m_pX = new float[vlen];
 		m_pXs = new float[vlen];
 		m_pXm = new float[vlen];
 		m_pXd = new float[vlen];
 		m_pXds = 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_pXds, 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));
 		
@@ -86,13 +82,11 @@ namespace gr {
     {
 		delete [] m_pPeakBox_abs;
 		delete [] m_pPeakBox_rel;
-		delete [] m_pPeakPos;
 		delete [] m_pXds;
 		delete [] m_pXd;
 		delete [] m_pXm;
 		delete [] m_pXs;
 		delete [] m_pX;
-		delete [] m_pPeakData;
     }
 
 	float peak_detect_impl::mean(float* pSrc, int length)
@@ -112,27 +106,18 @@ namespace gr {
 			pDst[i] = value;
 		}
 	}
-	int peak_detect_impl::threshold(float *pDst, float *pSrc, int length, float thresh)
+
+	std::vector<int> peak_detect_impl::findPeakPos(float* pSrc, int length, float thresh)
 	{
+		std::vector<int> result;
 		for (int i=0; i < length; i++)
 		{
-			pDst[i] = (int)(pSrc[i] >= thresh);
+			if (pSrc[i] > thresh)
-		}
-		
-		return length;
-	}
-	
-	int peak_detect_impl::findPeakPos(int* pDst, float* pSrc, int length)
 			{
-		int count = 0;
+				result.push_back(i);
-		for (int i=0; i < length; i++)
-		{
-			if (pSrc[i] > 0)
-			{
-				pDst[count++] = i;
 			}
 		}
-		return count;
+		return result;
 	}
 
     int
@@ -212,19 +197,19 @@ namespace gr {
 			// Create peak list
 			// -----------------------------------------------------------------
 			// Find peak candidates by thresholding
-			threshold(m_pPeakData, m_pXds, nitems_per_block, m_peak_height_min);
+			std::vector<int> peakPos = findPeakPos(m_pXds, nitems_per_block, m_peak_height_min);
-			int numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
 			
 			// -----------------------------------------------------------------
 			// Search true maximum peak using hill climbing
 			// -----------------------------------------------------------------
+			std::vector<int> newPeakPos;
 			int numPeaksLast = 0;
 			while(1)
 			{
 				int numPeaksClimbed = 0;
-				for (int i = 0; i < numPeaks; i++)
+				for (int i = 0; i < peakPos.size(); i++)
 				{
-					int pos = m_pPeakPos[i];
+					int pos = peakPos[i];
 					while(1)
 					{
 						float y0 = m_pXs[pos];
@@ -232,20 +217,15 @@ namespace gr {
 						int pos_n = std::max(0, pos-1);
 						if (m_pXs[pos_p] > y0)
 						{
-							m_pPeakData[pos] = 0;
-							m_pPeakData[pos_n] = 0;
 							pos = pos_p;
 						}
 						else if (m_pXs[pos_n] > y0)
 						{
-							m_pPeakData[pos] = 0;
-							m_pPeakData[pos_p] = 0;
 							pos = pos_n;
 						}
 						else
 						{
-							m_pPeakData[pos_n] = 0;
+							addToList(newPeakPos, pos);
-							m_pPeakData[pos_p] = 0;
 							numPeaksClimbed++;
 							break;
 						}
@@ -257,21 +237,21 @@ namespace gr {
 				}
 				numPeaksLast = numPeaksClimbed;
 			}
-			numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
+			peakPos = newPeakPos;
 			
 			// -----------------------------------------------------------------
 			// Sort peaks
 			// -----------------------------------------------------------------
-			sort<float>(m_pXds, m_pPeakPos, numPeaks);
+			sort<float>(m_pXds, peakPos);
 			
 			// -----------------------------------------------------------------
 			// Find peak boxes
 			// -----------------------------------------------------------------
 			m_peakList.clear();
-			for (int i = 0; i < numPeaks; i++)
+			for (int i = 0; i < peakPos.size(); i++)
 			{
 				int pbh = 0;
-				int pos = m_pPeakPos[i];
+				int pos = peakPos[i];
 				int left = pos;
 				int right = pos;
 				float nom = 10;
@@ -334,15 +314,7 @@ namespace gr {
 					continue;
 				}
 				
-				Peak peak = 
+				Peak peak(m_peak_id, pos, pbh, m_pXds[pos], m_pXs[pos]);
-				{
-					.id = m_peak_id,
-					.pos = (float)pos,
-					.half_width = (float)pbh,
-					.height_rel = m_pXds[pos],
-					.height_abs = m_pXs[pos],
-					.isConfirmed = true,
-				};
 				
 				// Intersection check
 				bool intersection = false;
@@ -382,7 +354,7 @@ namespace gr {
 				
 				if (!found)
 				{
-					peak.isConfirmed = 1;
+					peak.isValid = 1;
 					temp.push_back(peak);
 					fprintf(stderr, "New Peak #%u at %d\n", (int)peak.id, (int)peak.pos);
 				}
@@ -391,7 +363,7 @@ namespace gr {
 			for (int j=0; j < m_peakHistory.size(); j++)
 			{
 				Peak &peak = m_peakHistory[j];
-				if (!peak.isConfirmed)
+				if (!peak.isValid)
 				{
 					fprintf(stderr, "Lost Peak #%u at %d\n", (int)peak.id, (int)peak.pos);
 				}
@@ -408,8 +380,8 @@ namespace gr {
 			{
 				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_start = std::max(0, peak.left());
-				int pbox_end = std::min(nitems_per_block-1, (int)(peak.pos + peak.half_width + 0.5f));
+				int pbox_end = std::min(nitems_per_block-1, peak.right());
 				
 				for (int j=pbox_start; j <= pbox_end; j++)
 				{
@@ -437,7 +409,7 @@ namespace gr {
 				pmt::pmt_t tag_key = pmt::string_to_symbol(std::string("pos_id_") + std::to_string(peak.id));
 				pmt::pmt_t tag_value = pmt::from_long(peak.pos);
 				add_item_tag(DATA, nitems_written(DATA) + k, tag_key, tag_value, m_tag_id);
-				peak.isConfirmed = false;
+				peak.isValid = false;
 			}
 
 			m_frameCount++;

gr-jay/lib/peak_detect_impl.h

@@ -30,16 +30,31 @@ namespace jay {
     {
 		struct Peak
 		{
+			Peak()
+			: Peak(0, 0, 0, 0.0, 0.0)
+			{
+				
+			}
+			Peak(uint64_t _id, int _pos, int _half_width, float _height_rel, float _height_abs)
+			: id(_id)
+			, pos(_pos)
+			, half_width(_half_width)
+			, height_rel(_height_rel)
+			, height_abs(_height_abs)
+			, isValid(true)
+			{
+				
+			}
 			uint64_t id;
-			float pos;
+			int pos;
-			float half_width;
+			int half_width;
 			float height_rel;
 			float height_abs;
-			bool isConfirmed;
+			bool isValid;
 			
 			bool isInRange(const Peak &other)
 			{
-				return (other.pos >= (pos - half_width)) and (other.pos <= (pos + half_width));
+				return (other.pos >= left()) and (other.pos <= right());
 			}
 			
 			const Peak& update(const Peak &other)
@@ -50,26 +65,61 @@ namespace jay {
 				half_width = alpha*half_width + beta*other.half_width;
 				height_rel = std::max(height_rel, other.height_rel);
 				height_abs = std::max(height_abs, other.height_abs);
-				isConfirmed = true;
+				isValid = true;
 				return *this;
 			}
+			
+			int left()
+			{
+				return pos - half_width;
+			}
+			
+			int right()
+			{
+				return pos + half_width;
+			}
 		};
 		
 	private:
 	
+		Peak peakCheck(float const *pData, Peak const &peak, float thresh)
+		{
+			Peak result;
+			
+	
+			return result;
+		}
+		
+		bool addToList(std::vector<int>&list, int pos)
+		{
+			bool found = false;
+			for (std::vector<int>::iterator it = list.begin(); it != list.end(); it++)
+			{
+				if (*it == pos)
+				{
+					found = true;
+					break;
+				}
+			}
+			
+			if (!found)
+			{
+				list.push_back(pos);
+			}
+			return !found;
+		}
+		
 		float m_framerate;
 		float m_peak_height_min;
 		int m_peak_width_min;
 		int m_peak_width_max;
 		float m_alpha_s;
 		float m_alpha_m;
-		float *m_pPeakData;
 		float *m_pX;
 		float *m_pXs;
 		float *m_pXm;
 		float *m_pXd;
 		float *m_pXds;
-		int *m_pPeakPos;
 		float *m_pPeakBox_rel;
 		float *m_pPeakBox_abs;
 		uint64_t m_frameCount;
@@ -88,29 +138,36 @@ namespace jay {
 	private:
 		void fill(float *pDst, int length, float value);
 		float mean(float *pSrc, int length);
-		int threshold(float *pDst, float *pSrc, int length, float thresh);
+		std::vector<int> findPeakPos(float* pSrc, int length, float thresh);
-		int findPeakPos(int *pDst, float *pSrc, int length);
 		
 		template <typename T>
-		inline void sort(T const * const pData, int *pPos, int numPos)
+		inline bool cmp(const T a, const T b)
 		{
-			int n = numPos;
+			return (a < b);
+		}
+		
+		template <typename T>
+		void sort(T const * const pData, std::vector<int>&pos)
+		{
+			int n = pos.size();
 			do
 			{
 				int newn = 1;
 				for (int i=0; i < (n-1); i++)
 				{
-					if (pData[pPos[i]] < pData[pPos[i+1]])
+					if (cmp(pData[pos[i]], pData[pos[i+1]]))
 					{
-						int temp = pPos[i+1];
+						int temp = pos[i+1];
-						pPos[i+1] = pPos[i];
+						pos[i+1] = pos[i];
-						pPos[i] = temp;
+						pos[i] = temp;
 						newn = i+1;
 					} // ende if
 				} // ende for
 				n = newn;
 			} while (n > 1);
 		}
+		
+		
     };
 
 } // namespace jay