v0.7.12 增加茎节peakfind功能

ReadMe.txt

@@ -96,4 +96,5 @@ v0.7.7
 v0.7.8 修改接穗抓点靠上一点
 v0.7.9 修改接穗抓点靠上一点，增加了grab_offset，用来在找到茎节后的上下偏移得到抓取点，+值向上偏，-值向下偏
 v0.7.10 增加茎节点位置绘制输出；茎节判别仅按dx方向大小判断（dz误差较大）
-v0.7.11 增加棋盘格识别，用于标定辅助
+v0.7.11 增加棋盘格识别，用于标定辅助
+v0.7.12 增加茎节peakfind功能

gcv_conf.yml

@@ -83,11 +83,17 @@ conf_parameters:
    sc_grab_ymax: 45
    sc_grab_zmin: 300
    sc_grab_zmax: 400
+   #sc_grab_xmin: -30
+   #sc_grab_xmax: 170
+   #sc_grab_ymin: -10
+   #sc_grab_ymax: 100
+   #sc_grab_zmin: 420
+   #sc_grab_zmax: 550   
    sc_grab_stem_diameter: 5.0
    sc_grab_y_opt: -20
    sc_grab_seedling_dist: 40.0
    sc_grab_stem_min_pts: 45
-   sc_grab_ror_ratio: 0.8
+   sc_grab_ror_ratio: 0.85
    sc_grab_offset: -5.0
    
 

grab_point_rs.cpp

@@ -24,6 +24,7 @@
 
 #include "grab_point_rs.h"
 #include "utils.h"
+#include "peak_finder.h"
 
 #define PI std::acos(-1)
 
@@ -2309,7 +2310,7 @@ void CRootStockGrabPoint::line_filter(
 	)
 	{
 		valid_line_index.clear();
-		float th_ratio = 2.5;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
+		float th_ratio = 1.5;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
 		pt.x = -1000.0;
 		pt.y = -1000.0;
 		pt.z = -1000.0;
@@ -2395,6 +2396,13 @@ void CRootStockGrabPoint::line_filter(
 		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
 		int max_pos_ref = max_pos;
 
+		//peak finder
+		std::vector<int> peak_indices;
+		findPeaks(stem_width, peak_indices, false);
+		if(peak_indices.size() > 0) {
+			max_pos = peak_indices[0];
+			max_pos_ref = max_pos;
+		}
 		float max_val = stem_width.at(max_pos);
 		float th = mu + th_ratio * stdv;
 		if(m_dtype == 0){

graft_cv_api.cpp

@@ -20,7 +20,7 @@ extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.7.11";
+	char *g_version_str = "0.7.12";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";	
@@ -147,7 +147,8 @@ namespace graft_cv
 		g_sola_grab_rs.set_image_saver(&g_pImStore);
 		g_sola_grab_sc.set_image_saver(&g_pImStore);
 		g_sola_reid_rs.set_image_saver(&g_pImStore);
-		g_sola_reid_sc.set_image_saver(&g_pImStore);		
+		g_sola_reid_sc.set_image_saver(&g_pImStore);	
+		g_chessboard.set_image_saver(&g_pImStore);
 		return 0;
 	}
 

peak_finder.cpp

@@ -0,0 +1,257 @@
+#include "peak_finder.h"
+
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+namespace graft_cv {
+	void diff(std::vector<float> in, std::vector<float>& out)
+	{
+		out = std::vector<float>(in.size() - 1);
+
+		for (int i = 1; i<in.size(); ++i)
+			out[i - 1] = in[i] - in[i - 1];
+	}
+
+	void vectorElementsProduct(std::vector<float> a, std::vector<float> b, std::vector<float>& out)
+	{
+		out = std::vector<float>(a.size());
+
+		for (int i = 0; i<a.size(); ++i)
+			out[i] = a[i] * b[i];
+	}
+
+	void findIndicesLessThan(std::vector<float> in, float threshold, std::vector<int>& indices)
+	{
+		for (int i = 0; i<in.size(); ++i)
+			if (in[i]<threshold)
+				indices.push_back(i + 1);
+	}
+
+	void selectElementsFromIndices(std::vector<float> in, std::vector<int> indices, std::vector<float>& out)
+	{
+		for (int i = 0; i<indices.size(); ++i)
+			out.push_back(in[indices[i]]);
+	}
+
+	void selectElementsFromIndices(std::vector<int> in, std::vector<int> indices, std::vector<int>& out)
+	{
+		for (int i = 0; i<indices.size(); ++i)
+			out.push_back(in[indices[i]]);
+	}
+
+	void signVector(std::vector<float> in, std::vector<int>& out)
+	{
+		out = std::vector<int>(in.size());
+
+		for (int i = 0; i<in.size(); ++i)
+		{
+			if (in[i]>0)
+				out[i] = 1;
+			else if (in[i]<0)
+				out[i] = -1;
+			else
+				out[i] = 0;
+		}
+	}
+
+	void scalarProduct(float scalar, std::vector<float> in, std::vector<float>& out)
+	{
+		out = std::vector<float>(in.size());
+
+		for (int i = 0; i<in.size(); ++i)
+			out[i] = scalar * in[i];
+	}
+
+	void findPeaks(std::vector<float> x0, std::vector<int>& peakInds, bool includeEndpoints, float extrema)
+	{
+		int minIdx = distance(x0.begin(), min_element(x0.begin(), x0.end()));
+		int maxIdx = distance(x0.begin(), max_element(x0.begin(), x0.end()));
+
+		float sel = (x0[maxIdx] - x0[minIdx]) / 4.0;
+		int len0 = x0.size();
+
+		scalarProduct(extrema, x0, x0);
+
+		std::vector<float> dx;
+		diff(x0, dx);
+		replace(dx.begin(), dx.end(), 0.0f, - EPS);
+		std::vector<float> dx0(dx.begin(), dx.end() - 1);
+		std::vector<float> dx0_1(dx.begin() + 1, dx.end());
+		std::vector<float> dx0_2;
+
+		vectorElementsProduct(dx0, dx0_1, dx0_2);
+
+		std::vector<int> ind;
+		findIndicesLessThan(dx0_2, 0, ind); // Find where the derivative changes sign	
+		std::vector<float> x;
+		float leftMin;
+		int minMagIdx;
+		float minMag;
+
+		if (includeEndpoints)
+		{
+			//x = [x0(1);x0(ind);x0(end)];	
+			selectElementsFromIndices(x0, ind, x);
+			x.insert(x.begin(), x0[0]);
+			x.insert(x.end(), x0[x0.size() - 1]);
+			//ind = [1;ind;len0];
+			ind.insert(ind.begin(), 1);
+			ind.insert(ind.end(), len0);
+			minMagIdx = distance(x.begin(), std::min_element(x.begin(), x.end()));
+			minMag = x[minMagIdx];
+			//std::cout<<"Hola"<<std::endl;
+			leftMin = minMag;
+		}
+		else
+		{
+			selectElementsFromIndices(x0, ind, x);
+			if (x.size()>2)
+			{
+				minMagIdx = distance(x.begin(), std::min_element(x.begin(), x.end()));
+				minMag = x[minMagIdx];
+				leftMin = x[0]<x0[0] ? x[0] : x0[0];
+			}
+		}
+
+		int len = x.size();
+
+		if (len>2)
+		{
+			float tempMag = minMag;
+			bool foundPeak = false;
+			int ii;
+
+			if (includeEndpoints)
+			{
+				// Deal with first point a little differently since tacked it on
+				// Calculate the sign of the derivative since we tacked the first
+				//  point on it does not neccessarily alternate like the rest.
+				std::vector<float> xSub0(x.begin(), x.begin() + 3);//tener cuidado subvector
+				std::vector<float> xDiff;//tener cuidado subvector
+				diff(xSub0, xDiff);
+
+				std::vector<int> signDx;
+				signVector(xDiff, signDx);
+
+				if (signDx[0] <= 0) // The first point is larger or equal to the second
+				{
+					if (signDx[0] == signDx[1]) // Want alternating signs
+					{
+						x.erase(x.begin() + 1);
+						ind.erase(ind.begin() + 1);
+						len = len - 1;
+					}
+				}
+				else // First point is smaller than the second
+				{
+					if (signDx[0] == signDx[1]) // Want alternating signs
+					{
+						x.erase(x.begin());
+						ind.erase(ind.begin());
+						len = len - 1;
+					}
+				}
+			}
+
+			//Skip the first point if it is smaller so we always start on maxima
+			if (x[0] >= x[1])
+				ii = 0;
+			else
+				ii = 1;
+
+			//Preallocate max number of maxima
+			float maxPeaks = ceil((float)len / 2.0);
+			std::vector<int> peakLoc(maxPeaks, 0);
+			std::vector<float> peakMag(maxPeaks, 0.0);
+			int cInd = 1;
+			int tempLoc;
+
+			while (ii < len)
+			{
+				ii = ii + 1;//This is a peak
+							//Reset peak finding if we had a peak and the next peak is bigger
+							//than the last or the left min was small enough to reset.
+				if (foundPeak)
+				{
+					tempMag = minMag;
+					foundPeak = false;
+				}
+
+				//Found new peak that was lager than temp mag and selectivity larger
+				//than the minimum to its left.
+
+				if (x[ii - 1] > tempMag && x[ii - 1] > leftMin + sel)
+				{
+					tempLoc = ii - 1;
+					tempMag = x[ii - 1];
+				}
+
+				//Make sure we don't iterate past the length of our vector
+				if (ii == len)
+					break; //We assign the last point differently out of the loop
+
+				ii = ii + 1; // Move onto the valley
+
+							 //Come down at least sel from peak
+				if (!foundPeak && tempMag > sel + x[ii - 1])
+				{
+					foundPeak = true; //We have found a peak
+					leftMin = x[ii - 1];
+					peakLoc[cInd - 1] = tempLoc; // Add peak to index
+					peakMag[cInd - 1] = tempMag;
+					cInd = cInd + 1;
+				}
+				else if (x[ii - 1] < leftMin) // New left minima
+					leftMin = x[ii - 1];
+
+			}
+
+			// Check end point
+			if (includeEndpoints)
+			{
+				if (x[x.size() - 1] > tempMag && x[x.size() - 1] > leftMin + sel)
+				{
+					peakLoc[cInd - 1] = len - 1;
+					peakMag[cInd - 1] = x[x.size() - 1];
+					cInd = cInd + 1;
+				}
+				else if (!foundPeak && tempMag > minMag)// Check if we still need to add the last point
+				{
+					peakLoc[cInd - 1] = tempLoc;
+					peakMag[cInd - 1] = tempMag;
+					cInd = cInd + 1;
+				}
+			}
+			else if (!foundPeak)
+			{
+				float minAux = x0[x0.size() - 1]<x[x.size() - 1] ? x0[x0.size() - 1] : x[x.size() - 1];
+				if (x[x.size() - 1] > tempMag && x[x.size() - 1] > leftMin + sel)
+				{
+					peakLoc[cInd - 1] = len - 1;
+					peakMag[cInd - 1] = x[x.size() - 1];
+					cInd = cInd + 1;
+				}
+				else if (!tempMag >  minAux + sel)// Check if we still need to add the last point
+				{
+					peakLoc[cInd - 1] = tempLoc;
+					peakMag[cInd - 1] = tempMag;
+					cInd = cInd + 1;
+				}
+			}
+
+			//Create output
+			if (cInd > 0)
+			{
+				std::vector<int> peakLocTmp(peakLoc.begin(), peakLoc.begin() + cInd - 1);
+				selectElementsFromIndices(ind, peakLocTmp, peakInds);
+			}
+
+		}
+		//else
+		//{
+		//input signal length <= 2
+		//}
+	}
+}

peak_finder.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <vector>
+
+namespace graft_cv {
+	const float EPS = 2.2204e-16f;
+
+	void diff(std::vector<float> in, std::vector<float>& out);
+
+	void vectorElementsProduct(std::vector<float> a, std::vector<float> b, std::vector<float>& out);
+
+	void findIndicesLessThan(std::vector<float> in, float threshold, std::vector<int>& indices);
+
+	void selectElementsFromIndices(std::vector<float> in, std::vector<int> indices, std::vector<float>& out);
+
+	void selectElementsFromIndices(std::vector<int> in, std::vector<int> indices, std::vector<int>& out);
+
+	void signVector(std::vector<float> in, std::vector<int>& out);
+
+	void scalarProduct(float scalar, std::vector<float> in, std::vector<float>& out);
+
+	/*
+	Inputs
+	x0: input signal
+	extrema: 1 if maxima are desired, -1 if minima are desired
+	includeEndpoints - If true the endpoints will be included as possible extrema otherwise they will not be included
+	Output
+	peakInds: Indices of peaks in x0
+	*/
+	void findPeaks(std::vector<float> x0, std::vector<int>& peakInds, bool includeEndpoints = true, float extrema = 1);
+
+}