algoLib/sourceCode/SG_lineFeature.cpp

#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>

typedef struct
{
	int flag;
	int sPtIdx;  
	int ePtIdx;
	SVzNL3DPosition startPt;
	SVzNL3DPosition endPt;
}SSG_jump;

typedef struct
{
	int flag;
	int startPtIdx;
	int endPtIdx;
	SVzNL3DPosition startPt;
	SVzNL3DPosition endPt;
	SSG_jump maxJump;
}SSG_slope;

typedef struct
{
	int startPtIdx;
	int endPtIdx;
	double zDiff;
}SSG_zWin;

SSG_meanVar _computeMeanVar(double* data, int size)
{
	double sum_x = 0;
	double sum_square = 0;
	int num = 0;
	for (int i = 0; i < size; i++)
	{
		sum_x += data[i];
		sum_square += data[i] * data[i];
		num++;
	}
	SSG_meanVar a_meanVar = { 0, 0 };
	if (num > 0)
	{
		a_meanVar.mean = sum_x / num;
		a_meanVar.var = sum_square / num - a_meanVar.mean * a_meanVar.mean;
		if (a_meanVar.var < 0)
			a_meanVar.var = 0;
		else
			a_meanVar.var = sqrt(a_meanVar.var);
	}
	return a_meanVar;
}

void sg_lineDataSmoothing(
	std::vector<SVzNL3DPosition>& input,
	int smoothWin, 
	std::vector<SVzNL3DPosition>& output)
{
	output.resize(input.size()); // 预分配足够的空间
	std::copy(input.begin(), input.end(), output.begin());

	int size = (int)input.size();
	for (int i = 0; i < size; i++)
	{
		if (input[i].pt3D.z > 1e-4)
		{
			int n = 0;
			double sum_z = 0;
			for (int j = i-smoothWin / 2; j <= i+smoothWin / 2; j++)
			{
				if ((j >= 0) && (j < size))
				{
					if (input[j].pt3D.z > 1e-4)
					{
						sum_z += input[j].pt3D.z;
						n++;
					}
				}
			}
			output[i].pt3D.z = sum_z / n;
		}
	}
	return;
}

//对扫描线按分段进行平滑。
void sg_lineSegSmoothing(
	std::vector<SVzNL3DPosition>& input, 
	double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
	double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
	int smoothWin, 
	std::vector<SVzNL3DPosition>& output)
{
	//output.resize(input.size()); // 预分配足够的空间
	//std::copy(input.begin(), input.end(), output.begin());

	int dataSize = (int)input.size();
	//计算分段
	std::vector<std::vector<SVzNL3DPosition>> segs;
	std::vector<SVzNL3DPosition> a_seg;
	int segStart = -1;
	for (int i = 0; i < dataSize; i++)
	{
		SVzNL3DPosition a_pt = input[i];
		//seg判断
		if (a_pt.pt3D.z > 1e-4)
		{
			if (segStart < 0)
			{
				a_seg.push_back(a_pt);
				segStart = 1;
			}
			else  //检查两点距离
			{
				SVzNL3DPosition pre_pt = a_seg.back();
				double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
				double diff_y = abs(a_pt.pt3D.y - pre_pt.pt3D.y);
				if ((diff_y > seg_y_deltaTh) || (diff_z > seg_z_deltaTh))
				{
					segs.push_back(a_seg);
					a_seg.clear();
					a_seg.push_back(a_pt);
				}
				else
					a_seg.push_back(a_pt);
			}
		}
		else
			a_seg.push_back(a_pt);
	}
	//last
	if (a_seg.size() > 0)
		segs.push_back(a_seg);

	//分段平滑
	for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
	{
		std::vector<SVzNL3DPosition> seg_output;
		sg_lineDataSmoothing(segs[i],  smoothWin, seg_output);
		output.insert(output.end(), seg_output.begin(), seg_output.end());
	}
	return;
}

//滤除离群点：z跳变门限方法
void sg_lineDataRemoveOutlier(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	SSG_outlierFilterParam filterParam, 
	std::vector<SVzNL3DPosition>& filerData, 
	std::vector<int>& noisePts)
{
	filerData.resize(dataSize);

	std::vector< SSG_RUN> continueRuns;
	SSG_RUN a_run = {0, -1, 0}; //startIdx, len, lastIdx
	double pre_z = 0;
	for (int i = 0; i < dataSize; i++)
	{
if (i == 370)
	int kkk = 1;
		filerData[i] = lineData[i];
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = i;
			}
			else
			{
				double z_diff = abs(lineData[i].pt3D.z - pre_z);
				if (z_diff < filterParam.continuityTh)
				{
					a_run.len++;
					a_run.value = i;
				}
				else
				{
					continueRuns.push_back(a_run);
					a_run.start = i;
					a_run.len = 1;
					a_run.value = i;
				}
			}
			pre_z = lineData[i].pt3D.z;
		}
	}
	if(a_run.len > 0)
		continueRuns.push_back(a_run);

	for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++)
	{
		if (continueRuns[i].len < filterParam.outlierTh)  //噪声
		{
			for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
			{
				filerData[j].pt3D.z = 0;
				noisePts.push_back(j);//记录序号，用于在原始数据上去除噪声
			}
		}
	}
	return;
}

//滤除离群点：z跳变门限方法
void sg_lineDataRemoveOutlier_changeOriginData(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	SSG_outlierFilterParam filterParam)
{
	std::vector< SSG_RUN> continueRuns;
	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
	double pre_z = 0;
	for (int i = 0; i < dataSize; i++)
	{
		if (i == 370)
			int kkk = 1;
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = i;
			}
			else
			{
				double z_diff = abs(lineData[i].pt3D.z - pre_z);
				if (z_diff < filterParam.continuityTh)
				{
					a_run.len++;
					a_run.value = i;
				}
				else
				{
					continueRuns.push_back(a_run);
					a_run.start = i;
					a_run.len = 1;
					a_run.value = i;
				}
			}
			pre_z = lineData[i].pt3D.z;
		}
	}
	if (a_run.len > 0)
		continueRuns.push_back(a_run);

	for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++)
	{
		if (continueRuns[i].len < filterParam.outlierTh)  //噪声
		{
			for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
			{
				lineData[j].pt3D.z = 0;
			}
		}
	}
	return;
}

//滤除离群点：点距离方法
void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
{
	filerData.resize(dataSize);

	std::vector< SSG_RUN> continueRuns;
	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
	SVzNL3DPoint pre_pt = {0,0,0};
	for (int i = 0; i < dataSize; i++)
	{
		if (i == 370)
			int kkk = 1;
		filerData[i] = lineData[i];
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = i;
			}
			else
			{
#if 1
				double pt_dist = sqrt(pow(lineData[i].pt3D.x - pre_pt.x, 2) + 
									  pow(lineData[i].pt3D.y - pre_pt.y, 2) + 
									  pow(lineData[i].pt3D.z - pre_pt.z, 2));
				if (pt_dist < filterParam.continuityTh)
				{
					a_run.len++;
					a_run.value = i;
				}
				else
				{
					continueRuns.push_back(a_run);
					a_run.start = i;
					a_run.len = 1;
					a_run.value = i;
				}
#else
				double pt_dy = abs(lineData[i].pt3D.y - pre_pt.y);
				double pt_dz = abs(lineData[i].pt3D.z - pre_pt.z);
				if (pt_dy * 3 > pt_dz)
				{
					a_run.len++;
					a_run.value = i;
				}
				else
				{
					continueRuns.push_back(a_run);
					a_run.start = i;
					a_run.len = 1;
					a_run.value = i;
				}
#endif

			}
			pre_pt = lineData[i].pt3D;
		}
	}
	if (a_run.len > 0)
		continueRuns.push_back(a_run);

	for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++)
	{
		if (continueRuns[i].len < filterParam.outlierTh)  //噪声
		{
			for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
			{
				filerData[j].pt3D.z = 0;
				noisePts.push_back(j);//记录序号，用于在原始数据上去除噪声
			}
		}
	}
	return;
}

/// <summary>
/// 获取扫描线中的斜坡，并记录斜坡中的最大的相邻点之间的跳变。
/// 同时记录Ending。当没有中间袋子，只有左右袋子时，会有两组Ending。Ending之间的最小间距设置为袋子宽度的1/4
/// </summary>
/// <param name="lineData">扫描线数据，兼容栅格数据格式</param>
/// <param name="dataSize">扫描线数据长度</param>
/// <param name="validSlopeH">有效斜坡长度。小于此长度的斜坡被视为噪声</param>
/// <param name="slopes">输出检测到的斜坡</param>
void _getSlopes(SVzNL3DPosition* lineData, int dataSize, const double validSlopeH, double minEndingGap, 
	std::vector< SSG_slope>& slopes, std::vector<SVzNLRange>& endings)
{
	if (dataSize < 2)
		return;

	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	int nullPtNum = 0;
	SVzNL3DPosition* pre_data = NULL;
	SSG_slope a_slope = { 0, 0, 0, {0, {0,0,0}}, {0, {0,0,0}}, { 0, 0, 0, {0, {0,0,0}}, {0, {0,0,0}}} };
	for (int i = 0; i < dataSize; i++)
	{
if (i >= 317)
	int kkk = 1;
		SVzNL3DPosition* curr_data = &lineData[i];
		if (curr_data->pt3D.z < 1e-4)
		{
			nullPtNum++;
			if (i == dataSize - 1)  //最后一个
			{
				if (sEdgePtIdx >= 0)
				{
					SVzNLRange a_range = { sEdgePtIdx , eEdgePtIdx };
					endings.push_back(a_range);
				}
			}
			continue;
		}

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			nullPtNum = 0;
			continue;
		}

		if (i == dataSize - 1)  //最后一个
		{
			if (sEdgePtIdx >= 0)
			{
				SVzNLRange a_range = { sEdgePtIdx , i };
				endings.push_back(a_range);
			}
		}
		else
		{
			if (nullPtNum > 0)
			{
				double y_diff = abs(curr_data->pt3D.y - pre_data->pt3D.y);
				if ((y_diff > minEndingGap) && (sEdgePtIdx >= 0))
				{
					SVzNLRange a_range = { sEdgePtIdx , eEdgePtIdx };
					endings.push_back(a_range);
					sEdgePtIdx = i;
				}
				nullPtNum = 0;
			}
		}
		eEdgePtIdx = i;
		
		double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
		switch (_state)
		{
		case 0:  //初态
			if (z_diff > 0) //下降
			{
				a_slope.startPtIdx = pre_i;
				a_slope.endPtIdx = i;
				a_slope.startPt = *pre_data;
				a_slope.endPt = *curr_data;
				a_slope.maxJump.sPtIdx = pre_i;
				a_slope.maxJump.ePtIdx = i;
				a_slope.maxJump.startPt = *pre_data;
				a_slope.maxJump.endPt = *curr_data;
				_state = 2;
			}
			else if (z_diff < 0) //上升
			{
				a_slope.startPtIdx = pre_i;
				a_slope.endPtIdx = i;
				a_slope.startPt = *pre_data;
				a_slope.endPt = *curr_data;
				a_slope.maxJump.sPtIdx = pre_i;
				a_slope.maxJump.ePtIdx = i;
				a_slope.maxJump.startPt = *pre_data;
				a_slope.maxJump.endPt = *curr_data;
				_state = 1;
			}
			break;
		case 1: //上升
			if (z_diff > 0)   //下降
			{
				double height = a_slope.startPt.pt3D.z - a_slope.endPt.pt3D.z;
				if (height >= validSlopeH)
				{
					a_slope.flag = 0;
					a_slope.maxJump.flag = 0;
					slopes.push_back(a_slope);
				}

				a_slope.startPtIdx = pre_i;
				a_slope.endPtIdx = i;
				a_slope.startPt = *pre_data;
				a_slope.endPt = *curr_data;
				a_slope.maxJump.sPtIdx = pre_i;
				a_slope.maxJump.ePtIdx = i;
				a_slope.maxJump.startPt = *pre_data;
				a_slope.maxJump.endPt = *curr_data;
				_state = 2;
			}
			else
			{
				a_slope.endPtIdx = i;
				a_slope.endPt = *curr_data;
				double maxDelta = a_slope.maxJump.startPt.pt3D.z - a_slope.maxJump.endPt.pt3D.z;
				if (maxDelta < -z_diff)
				{
					a_slope.maxJump.sPtIdx = pre_i;
					a_slope.maxJump.ePtIdx = i;
					a_slope.maxJump.startPt = *pre_data;
					a_slope.maxJump.endPt = *curr_data;
				}
			}
			break;
		case 2: //下降
			if (z_diff < 0)   //  上升
			{
				double height = a_slope.endPt.pt3D.z - a_slope.startPt.pt3D.z;
				if (height >= validSlopeH)
				{
					a_slope.flag = 0;
					a_slope.maxJump.flag = 0;
					slopes.push_back(a_slope);
				}
				a_slope.startPtIdx = pre_i;
				a_slope.endPtIdx = i;
				a_slope.startPt = *pre_data;
				a_slope.endPt = *curr_data;
				a_slope.maxJump.sPtIdx = pre_i;
				a_slope.maxJump.ePtIdx = i;
				a_slope.maxJump.startPt = *pre_data;
				a_slope.maxJump.endPt = *curr_data;
				_state = 1;
			}
			else
			{
				a_slope.endPtIdx = i;
				a_slope.endPt = *curr_data;
				double maxDelta = a_slope.maxJump.endPt.pt3D.z - a_slope.maxJump.startPt.pt3D.z;
				if (maxDelta < z_diff)
				{
					a_slope.maxJump.sPtIdx = pre_i;
					a_slope.maxJump.ePtIdx = i;
					a_slope.maxJump.startPt = *pre_data;
					a_slope.maxJump.endPt = *curr_data;
				}
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//最后一个
	bool chkLast = true;
	if (slopes.size() > 0)
	{
		if (slopes.back().startPtIdx == a_slope.startPtIdx)
			chkLast = false;
	}
	if (true == chkLast)
	{
		if (_state == 1)//上升
		{
			double height = a_slope.startPt.pt3D.z - a_slope.endPt.pt3D.z;
			if (height >= validSlopeH)
			{
				a_slope.flag = 0;
				a_slope.maxJump.flag = 0;
				slopes.push_back(a_slope);
			}
		}
		else if (_state == 2)//下降
		{
			double height = a_slope.endPt.pt3D.z - a_slope.startPt.pt3D.z;
			if (height >= validSlopeH)
			{
				a_slope.flag = 0;
				a_slope.maxJump.flag = 0;
				slopes.push_back(a_slope);
			}
		}
	}

	return;
}

SVzNL3DPosition _getZNearestPt(SVzNL3DPosition* lineData, int startIdx, int endIdx, double nominalZ)
{
	SVzNL3DPosition rsltPt = { 0, {0,0,0} };
	double diffZ_min = -1;
	for (int i = startIdx; i <= endIdx; i++)
	{
		double diffZ = abs(nominalZ - lineData[i].pt3D.z);
		if (diffZ_min < 0)
		{
			diffZ_min = diffZ;
			rsltPt = lineData[i];
		}
		else
		{
			if (diffZ_min > diffZ)
			{
				diffZ_min = diffZ;
				rsltPt = lineData[i];
			}
		}
	}
	return rsltPt;
}
bool _chkVFeature(SVzNL3DPosition* lineData, int dataSize, SSG_slope* down_slope, SSG_slope* up_slope, const SSG_VFeatureParam valleyPara)
{
	bool isValidValley = false;
	//寻找最低点
	SVzNL3DPosition valleyPt;
	//检测两个Slope是否在Z向有重叠
	if ((down_slope->endPt.pt3D.z > up_slope->endPt.pt3D.z) &&
		(up_slope->startPt.pt3D.z > down_slope->startPt.pt3D.z))
	{
		if (down_slope->endPt.pt3D.z >= up_slope->startPt.pt3D.z) //down_slope的EndPt是最低点
		{
			valleyPt = down_slope->endPt;//down_slope的EndPt是最低点
			double chkZ_diff = abs(up_slope->startPt.pt3D.z - valleyPt.pt3D.z);
			//在dwon_slope中寻找与up_slope的startPt的z高度最接近的点
			SVzNL3DPosition counterPt = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, up_slope->startPt.pt3D.z);
			if (chkZ_diff < valleyPara.valleyMinH) //需要检测valleyMinH的宽度
			{
				SVzNL3DPosition minH_pt1 = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, down_slope->endPt.pt3D.z - valleyPara.valleyMinH/2);
				SVzNL3DPosition minH_pt2 = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, down_slope->endPt.pt3D.z - valleyPara.valleyMinH/2);
				double valleyW = abs(minH_pt1.pt3D.y - minH_pt2.pt3D.y);
				if (valleyW < valleyPara.valleyMaxW)
					isValidValley = true;
			}
			else//up_slope的最低点高于valleyMinH， 不需要检测valleyMinH的宽度
			{
				//检查Valley的宽高比
				//在up_slope中寻找与down_slope的endPt的z高度最接近的点
				double valleyW = abs(counterPt.pt3D.y - up_slope->startPt.pt3D.z);
				if (valleyW < valleyPara.valleyMaxW)
					isValidValley = true;
			}
		}
		else //up_slope的startPt是最低点
		{
			valleyPt = up_slope->startPt;//up_slope的startPt是最低点
			double chkZ_diff = abs(down_slope->endPt.pt3D.z - valleyPt.pt3D.z);
			//在up_slope中寻找与down_slope的endPt的z高度最接近的点
			SVzNL3DPosition counterPt = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, down_slope->endPt.pt3D.z);
			if (chkZ_diff < valleyPara.valleyMinH) //需要检测valleyMinH的宽度
			{
				SVzNL3DPosition minH_pt1 = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, up_slope->startPt.pt3D.z - valleyPara.valleyMinH);
				SVzNL3DPosition minH_pt2 = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, up_slope->startPt.pt3D.z - valleyPara.valleyMinH);
				double valleyW = abs(minH_pt1.pt3D.y - minH_pt2.pt3D.y);
				if (valleyW < valleyPara.valleyMaxW)
					isValidValley = true;
			}
			else//up_slope的最低点高于valleyMinH， 不需要检测valleyMinH的宽度
			{
				//检查Valley的宽高比
				//在up_slope中寻找与down_slope的endPt的z高度最接近的点
				double valleyW = abs(counterPt.pt3D.y - up_slope->startPt.pt3D.z);
				if (valleyW < valleyPara.valleyMaxW)
					isValidValley = true;
			}
		}
	}
	return isValidValley;
}

double _computerMaxSlopeK(SVzNL3DPosition* lineData, int dataSize, SSG_jump maxJump, double zWinLen)
{
	//从maxJumPos向两端各延伸1cm，计算坡度
	double z0 = maxJump.startPt.pt3D.z;
	double z_start = z0;
	double y_start = maxJump.startPt.pt3D.y;
	double z1 = maxJump.endPt.pt3D.z;
	double z_end = z1;
	double y_end = maxJump.endPt.pt3D.y;
#if 1
	if (maxJump.sPtIdx > 0)
	{
		y_start = lineData[maxJump.sPtIdx-1].pt3D.y;
		z_start = lineData[maxJump.sPtIdx - 1].pt3D.z;
	}
	if (maxJump.ePtIdx < dataSize - 1)
	{
		y_end = lineData[maxJump.ePtIdx + 1].pt3D.y;
		z_end = lineData[maxJump.ePtIdx + 1].pt3D.z;
	}
#else
	//在窗口内计算坡度以排除最大下降点本身z波动的影响
	if (z0 < z1) //下降：对于L型斜坡，最大下降点应该在endPt端，再往下应该是平坦段，因此窗口要从startPt往前找。
	{
		double max_dz = 0;
		for (int i = maxJump.sPtIdx; i >= 0; i--)
		{
			if (lineData[i].pt3D.z > 1e-4)
			{
				double dz = abs(z0 - lineData[i].pt3D.z);
				if (max_dz < dz)
				{
					z_start = lineData[i].pt3D.z;
					y_start = lineData[i].pt3D.y;
					max_dz = dz;
				}
				if (dz > zWinLen)
					break;
			}
		}
	}
	else //上升：对于L型斜坡，最大下降点应该在startPt端，再往前应该是平坦段，因此窗口要从endPt往后找。
	{
		double max_dz = 0;
		for (int i = maxJump.ePtIdx; i < dataSize; i++)
		{
			if (lineData[i].pt3D.z > 1e-4)
			{
				double dz = abs(z0 - lineData[i].pt3D.z);
				if (max_dz < dz)
				{
					z_end = lineData[i].pt3D.z;
					y_end = lineData[i].pt3D.y;
					max_dz = dz;
				}
				if (dz > zWinLen)
					break;
			}
		}
	}
#endif
	return abs((z_start - z_end) / (y_start - y_end));
}

/// <summary>
/// 提取激光线上的L型跳变和V型跳变特征
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）提取扫描线中的Slope(超过门限高度的slope为有效slope）,局部最小值，以及局部Z跳变最大的点
/// （2）取窗口宽度内的最小值为有效最小值
/// （3）V:在最小值点两个Win宽度一半内检查有没有对称的Slope。
/// （4）L:在Slope中检测Z跳变最大的点，判断角度。
/// 
/// </summary>
void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, const SSG_slopeParam slopeParam, const SSG_VFeatureParam valleyPara, SSG_lineFeature* line_features)
{
	std::vector< SSG_slope> slopes;
	//提取扫描线中的Slope(超过门限高度的slope为有效slope）
	std::vector<SVzNLRange> endings;  //22024.12.16: 注意：当没有中间袋子，只有左右袋子时，有两组Ending。
	_getSlopes(lineData, dataSize, slopeParam.validSlopeH, slopeParam.minEndingGap, slopes, endings);
	
	//首先检查L型边界：本身具有足够坡度，与下一段形成明显坡度差
	//再检查是否可以形成V型特征
	for (int i = 0, i_max = (int)slopes.size(); i < i_max; i++)
	{
		SSG_slope* a_slope = &slopes[i];
		if (a_slope->flag > 0)
			continue;

		double curr_slopeH = a_slope->endPt.pt3D.z - a_slope->startPt.pt3D.z;
		double curr_maxJumpH = a_slope->maxJump.endPt.pt3D.z - a_slope->maxJump.startPt.pt3D.z;
		int curr_self_type = 0;
		SSG_basicFeature1D self_slope;
		memset(&self_slope, 0, sizeof(SSG_basicFeature1D));
		if (abs(curr_maxJumpH) > slopeParam.minLJumpH)
		{
			if (curr_maxJumpH > 0)  //下降
			{
				curr_self_type = LINE_FEATURE_L_JUMP_H2L;
				self_slope.featureType = LINE_FEATURE_L_JUMP_H2L;
				self_slope.jumpPos = a_slope->maxJump.startPt.pt3D;
				self_slope.jumpPos2D = { lineIdx, a_slope->maxJump.sPtIdx };
			}
			else
			{
				curr_self_type = LINE_FEATURE_L_JUMP_L2H;
				self_slope.featureType = LINE_FEATURE_L_JUMP_L2H;
				self_slope.jumpPos = a_slope->maxJump.endPt.pt3D;
				self_slope.jumpPos2D = { lineIdx,a_slope->maxJump.ePtIdx };
			}
		}
		else if (abs(curr_slopeH) > slopeParam.minLJumpH)
		{
			//计算最大坡度
			double maxSlopeK = _computerMaxSlopeK(lineData, dataSize, a_slope->maxJump, slopeParam.LSlopeZWin);
			//abs(slopeH / (a_slope->endPt.pt3D.y - a_slope->startPt.pt3D.y));
			if (maxSlopeK > 2) //对应63度，在此角度下，相邻点在1mm时，点的波动基本不会导致slope的反转
			{
				if (curr_slopeH > 0)  //下降
				{
					bool nearEdgeFlag = false;
					for (int m = 0; m < endings.size(); m++)
					{
						int gap_0 = abs(a_slope->endPtIdx - endings[m].nMin);
						int gap_1 = abs(a_slope->endPtIdx - endings[m].nMax);
						if ( (gap_0 <= 2) || (gap_1 <= 2))
						{
							nearEdgeFlag = true;
							break;
						}
					}
					if (false == nearEdgeFlag)
					{
						curr_self_type = LINE_FEATURE_L_SLOPE_H2L;
						self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
						self_slope.jumpPos = a_slope->maxJump.endPt.pt3D; //a_slope->endPt.pt3D;
						self_slope.jumpPos2D = { lineIdx, a_slope->maxJump.ePtIdx };//a_slope->endPtIdx };
					}
				}
				else
				{
					bool nearEdgeFlag = false;
					for (int m = 0; m < endings.size(); m++)
					{
						int gap_0 = abs(a_slope->startPtIdx - endings[m].nMin);
						int gap_1 = abs(a_slope->startPtIdx - endings[m].nMax);
						if ((gap_0 <= 2) || (gap_1 <= 2))
						{
							nearEdgeFlag = true;
							break;
						}
					}
					if (false == nearEdgeFlag)
					{
						curr_self_type = LINE_FEATURE_L_SLOPE_L2H;
						self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
						self_slope.jumpPos = a_slope->maxJump.startPt.pt3D; //a_slope->startPt.pt3D;
						self_slope.jumpPos2D = { lineIdx, a_slope->maxJump.sPtIdx }; //a_slope->startPtIdx };
					}
				}
			}
		}

		//考虑下一个特征，迭代检查
		int nxt_self_type = 0;
		int complex_type = 0; //复合特征
		SSG_basicFeature1D nxt_self_slope;
		memset(&nxt_self_slope, 0, sizeof(SSG_basicFeature1D));
		SSG_basicFeature1D a_valley;
		memset(&a_valley, 0, sizeof(SSG_basicFeature1D));
		if (i < i_max - 1)  //检查能否与下一个配对
		{
			SSG_slope* nxt_slope = &slopes[i + 1];
			double z_diff = a_slope->endPt.pt3D.z - a_slope->startPt.pt3D.z;
			double nxt_z_diff = nxt_slope->endPt.pt3D.z - nxt_slope->startPt.pt3D.z;
			double curr_dz = abs(z_diff);
			double nxt_dz = abs(nxt_z_diff);
			if ( (z_diff > 0) && (nxt_z_diff < 0) && (0 == nxt_slope->flag))  //下降slope
			{
				//检查是否可以配对
				bool isValley = _chkVFeature(lineData, dataSize, a_slope, nxt_slope, valleyPara);
				if (true == isValley) //&& (false == isLSlope))
				{
					a_slope->flag = 1;
					nxt_slope->flag = 1;
					complex_type = LINE_FEATURE_V_SLOPE;
					a_valley.featureType = LINE_FEATURE_V_SLOPE;
					if (a_slope->endPt.pt3D.z >= nxt_slope->startPt.pt3D.z)
					{
						a_valley.jumpPos = a_slope->endPt.pt3D;
						a_valley.jumpPos2D = { lineIdx, a_slope->endPtIdx };
					}
					else
					{
						a_valley.jumpPos = nxt_slope->startPt.pt3D;
						a_valley.jumpPos2D = { lineIdx, nxt_slope->startPtIdx };
					}
				}
			}
			if (complex_type > 0)//检查下一个slope的type
			{
				double nxt_slopeH = nxt_slope->endPt.pt3D.z - nxt_slope->startPt.pt3D.z;
				double nxt_maxJumpH = nxt_slope->maxJump.endPt.pt3D.z - nxt_slope->maxJump.startPt.pt3D.z;
				if (abs(nxt_maxJumpH) > slopeParam.minLJumpH)
				{
					if (nxt_maxJumpH > 0)  //下降
					{
						nxt_self_type = LINE_FEATURE_L_JUMP_H2L;
						nxt_self_slope.featureType = LINE_FEATURE_L_JUMP_H2L;
						nxt_self_slope.jumpPos = nxt_slope->maxJump.startPt.pt3D;
						nxt_self_slope.jumpPos2D = { lineIdx, nxt_slope->maxJump.sPtIdx };
					}
					else
					{
						nxt_self_type = LINE_FEATURE_L_JUMP_L2H;
						nxt_self_slope.featureType = LINE_FEATURE_L_JUMP_L2H;
						nxt_self_slope.jumpPos = nxt_slope->maxJump.endPt.pt3D;
						nxt_self_slope.jumpPos2D = { lineIdx,nxt_slope->maxJump.ePtIdx };
					}
				}
				else if (abs(nxt_slopeH) > slopeParam.minLJumpH)
				{
					//计算最大坡度
					double nxt_maxSlopeK = _computerMaxSlopeK(lineData, dataSize, nxt_slope->maxJump, slopeParam.LSlopeZWin);
					//abs(slopeH / (nxt_slope->endPt.pt3D.y - nxt_slope->startPt.pt3D.y));
					if (nxt_maxSlopeK > 2) //对应63度，在此角度下，相邻点在1mm时，点的波动基本不会导致slope的反转
					{
						if (nxt_slopeH > 0)  //下降
						{
							bool nearEdgeFlag = false;
							for (int m = 0; m < endings.size(); m++)
							{
								int gap_0 = abs(nxt_slope->endPtIdx - endings[m].nMin);
								int gap_1 = abs(nxt_slope->endPtIdx - endings[m].nMax);
								if ((gap_0 <= 2) || (gap_1 <= 2))
								{
									nearEdgeFlag = true;
									break;
								}
							}
							if (false == nearEdgeFlag)
							{
								nxt_self_type = LINE_FEATURE_L_SLOPE_H2L;
								nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
								nxt_self_slope.jumpPos = nxt_slope->maxJump.endPt.pt3D;//nxt_slope->endPt.pt3D;
								nxt_self_slope.jumpPos2D = { lineIdx, nxt_slope->maxJump.ePtIdx };//nxt_slope->endPtIdx };
							}
						}
						else
						{
							bool nearEdgeFlag = false;
							for (int m = 0; m < endings.size(); m++)
							{
								int gap_0 = abs(nxt_slope->startPtIdx - endings[m].nMin);
								int gap_1 = abs(nxt_slope->startPtIdx - endings[m].nMax);
								if ((gap_0 <= 2) || (gap_1 <= 2))
								{
									nearEdgeFlag = true;
									break;
								}
							}
							if (false == nearEdgeFlag)
							{
								nxt_self_type = LINE_FEATURE_L_SLOPE_L2H;
								nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
								nxt_self_slope.jumpPos = nxt_slope->maxJump.startPt.pt3D; //nxt_slope->startPt.pt3D;
								nxt_self_slope.jumpPos2D = { lineIdx, nxt_slope->maxJump.sPtIdx }; //nxt_slope->startPtIdx };
							}
						}
					}
				}

			}
		}

		//处理同时存在的情况
		if (complex_type == 0)
		{
			if (curr_self_type > 0) 
			{
				line_features->features.push_back(self_slope);
				//标记
				a_slope->flag = 1;
				if ((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))
					a_slope->maxJump.flag = 1;
			}
			if (nxt_self_type > 0)
			{
				line_features->features.push_back(nxt_self_slope);
				//标记
				slopes[i + 1].flag = 1;
				if ((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type))
					slopes[i + 1].maxJump.flag = 1;
			}
		}
		else  //complex_type > 0
		{
			const double sameFeatureDistTh = 5.0;
			const double slope_jump_th = 40.0;
			SSG_slope* nxt_slope = &slopes[i + 1];

			//标记
			a_slope->flag = 1;
			nxt_slope->flag = 1;
			if ((curr_self_type == 0) && (nxt_self_type == 0))
			{
				line_features->features.push_back(a_valley);
			}
			else if ((curr_self_type > 0) && (nxt_self_type == 0))
			{
				SVzNL3DPoint valley_lowest = a_valley.jumpPos;
				SVzNL3DPoint curr_slope_lowest;
				if (LINE_FEATURE_L_JUMP_H2L == curr_self_type)
					curr_slope_lowest = a_slope->maxJump.endPt.pt3D;
				else if (LINE_FEATURE_L_JUMP_L2H == curr_self_type)
					curr_slope_lowest = a_slope->maxJump.startPt.pt3D;
				else
					curr_slope_lowest = self_slope.jumpPos;
				double valley_slope_dist_0 = abs(valley_lowest.y - curr_slope_lowest.y);
				if (valley_slope_dist_0 > sameFeatureDistTh)  //同时存在
				{
					line_features->features.push_back(self_slope);
					line_features->features.push_back(a_valley);
				}
				else
				{
					//检查最高点高差。高差大于
					double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
					if (abs(highest_diff) < slope_jump_th) //小于40mm(袋子高度的1/4)，视为valley; 反之，视为Slope
					{
						if ((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))//JUMP优先
							line_features->features.push_back(self_slope);
						else
							line_features->features.push_back(a_valley);
					}
					else
						line_features->features.push_back(self_slope);
				}
			}
			else if ((curr_self_type == 0) && (nxt_self_type > 0))
			{
				SVzNL3DPoint valley_lowest = a_valley.jumpPos;
				SVzNL3DPoint nxt_slope_lowest;
				if (LINE_FEATURE_L_JUMP_H2L == nxt_self_type)
					nxt_slope_lowest = nxt_slope->maxJump.endPt.pt3D;
				else if (LINE_FEATURE_L_JUMP_L2H == nxt_self_type)
					nxt_slope_lowest = nxt_slope->maxJump.startPt.pt3D;
				else
					nxt_slope_lowest = nxt_self_slope.jumpPos;
				double valley_slope_dist_1 = abs(valley_lowest.y - nxt_slope_lowest.y);
				if (valley_slope_dist_1 > sameFeatureDistTh)  //同时存在
				{
					line_features->features.push_back(a_valley);
					line_features->features.push_back(nxt_self_slope);
				}
				else  // ((curr_self_type > 0) && (nxt_self_type > 0))
				{
					//检查最高点高差。高差大于
					double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
					if (abs(highest_diff) < slope_jump_th) //小于40mm(袋子高度的1/4)，视为valley; 反之，视为Slope
					{
						if ((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type))  //JUMP优先
							line_features->features.push_back(nxt_self_slope);
						else
							line_features->features.push_back(a_valley);
					}
					else
						line_features->features.push_back(nxt_self_slope);
				}
			}
			else 
			{
				SVzNL3DPoint valley_lowest = a_valley.jumpPos;
				SVzNL3DPoint curr_slope_lowest;
				if (LINE_FEATURE_L_JUMP_H2L == curr_self_type)
					curr_slope_lowest = a_slope->maxJump.endPt.pt3D;
				else if (LINE_FEATURE_L_JUMP_L2H == curr_self_type)
					curr_slope_lowest = a_slope->maxJump.startPt.pt3D;
				else
					curr_slope_lowest = self_slope.jumpPos;
				double valley_slope_dist_0 = abs(valley_lowest.y - curr_slope_lowest.y);

				SVzNL3DPoint nxt_slope_lowest;
				if (LINE_FEATURE_L_JUMP_H2L == nxt_self_type)
					nxt_slope_lowest = nxt_slope->maxJump.endPt.pt3D;
				else if (LINE_FEATURE_L_JUMP_L2H == nxt_self_type)
					nxt_slope_lowest = nxt_slope->maxJump.startPt.pt3D;
				else
					nxt_slope_lowest = nxt_self_slope.jumpPos;
				double valley_slope_dist_1= abs(valley_lowest.y - nxt_slope_lowest.y);

				if ( (valley_slope_dist_0 > sameFeatureDistTh) && (valley_slope_dist_1 > sameFeatureDistTh)) //同时存在
				{
					line_features->features.push_back(self_slope);
					line_features->features.push_back(a_valley);
					line_features->features.push_back(nxt_self_slope);
				}
				else if ((valley_slope_dist_0 > sameFeatureDistTh) && (valley_slope_dist_1 <= sameFeatureDistTh))
				{
					line_features->features.push_back(self_slope);
					if((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type))  //JUMP优先
						line_features->features.push_back(nxt_self_slope);
					else
					line_features->features.push_back(a_valley);
				}
				else if ((valley_slope_dist_0 <= sameFeatureDistTh) && (valley_slope_dist_1 > sameFeatureDistTh))
				{
					if((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))//JUMP优先
						line_features->features.push_back(self_slope);
					else
						line_features->features.push_back(a_valley);
					line_features->features.push_back(nxt_self_slope);
				}
				else  //合并为一个
				{
					//检查最高点高差。高差大于
					double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
					if (abs(highest_diff) < slope_jump_th) //小于40mm(袋子高度的1/4)，视为valley; 反之，视为Slope
					{
						line_features->features.push_back(a_valley);
					}
					else
					{
						if (highest_diff > 0)  //L2H slope
						{
							if (self_slope.jumpPos.z > nxt_self_slope.jumpPos.z)
							{
								self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
								line_features->features.push_back(self_slope);
							}
							else
							{
								nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
								line_features->features.push_back(nxt_self_slope);
							}
						}
						else //H2L slope
						{
							if (self_slope.jumpPos.z > nxt_self_slope.jumpPos.z)
							{
								self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
								line_features->features.push_back(self_slope);
							}
							else
							{
								nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
								line_features->features.push_back(nxt_self_slope);
							}
						}
					}
				}
			}
		}
	}
	//检查连续的JUMP是否可能合并
	int i = (int)line_features->features.size() - 2;
	while(i >= 0)
	{
		if (i < ((int)line_features->features.size() - 1))
		{
			SSG_basicFeature1D* curr_feature = &line_features->features[i];
			SSG_basicFeature1D* nxt_feature = &line_features->features[i + 1];
			if ((curr_feature->featureType == nxt_feature->featureType) &&
				((LINE_FEATURE_L_JUMP_H2L == curr_feature->featureType) ||
					(LINE_FEATURE_L_JUMP_L2H == curr_feature->featureType)))
			{
				//计算两个的距离
				double dist = abs(curr_feature->jumpPos.y - nxt_feature->jumpPos.y);
				if (dist < 20) //距离小于20mm，合并
				{
					if (LINE_FEATURE_L_JUMP_L2H)
						*curr_feature = *nxt_feature;
					line_features->features.erase(line_features->features.begin() + i + 1);
				}
			}
		}
		i--;
	}

	//添加开始和结束边界
	for (int i = 0; i < endings.size(); i++)
	{
		SSG_basicFeature1D an_edge;
		memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
		an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
		an_edge.jumpPos = lineData[endings[i].nMin].pt3D;
		an_edge.jumpPos2D = { lineIdx, endings[i].nMin };
		line_features->endings.push_back(an_edge);
		//line_features.insert(line_features.begin(), an_edge); //头部
		//尾部
		an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
		an_edge.jumpPos = lineData[endings[i].nMax].pt3D;
		an_edge.jumpPos2D = { lineIdx, endings[i].nMax };
		line_features->endings.push_back(an_edge);
	}

	return;
}

/// <summary>
/// 提取激光线上的拐点特征
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void sg_getLineCornerFeature(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	int lineIdx, 
	const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
	SSG_lineFeature* line_features)
{
	line_features->lineIdx = lineIdx;
	if (lineIdx == 538)
		int kkk = 1;
	//去除零点
	std::vector< SVzNL3DPosition> vldPts;
	std::vector<SSG_RUN> segs;
	//修改seg的定义。seg端点是两点间距离大于门限的点
	int segStart = -1, segEnd = -1;
	for (int i = 0; i < dataSize; i++)
	{
		if ( (lineIdx == 399)&&(i==568))
			int kkk = 1;
		SVzNL3DPosition a_pt = lineData[i];
		a_pt.nPointIdx = i;
		if (lineData[i].pt3D.z > 1e-4)
			vldPts.push_back(a_pt);

		//seg判断
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (segStart < 0)
				segStart = i;
			else  //检查两点距离
			{
				SVzNL3DPosition pre_pt = lineData[i-1];
				double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
				if (diff_z > cornerPara.minEndingGap_z)
				{
					SSG_RUN a_run;
					a_run.start = segStart;
					a_run.len = segEnd - segStart + 1;
					a_run.value = 1;
					segs.push_back(a_run);
					segStart = i;
				}
			}
			segEnd = i;
		}
		else
		{
			if (segStart >= 0)
			{
				SSG_RUN a_run;
				a_run.start = segStart;
				a_run.len = segEnd - segStart + 1;
				a_run.value = 1;
				segs.push_back(a_run);
				segStart = -1;
				segEnd = -1;
			}
		}
	}
	//last
	if (segStart >= 0)
	{
		SSG_RUN a_run;
		a_run.start = segStart;
		a_run.len = segEnd - segStart + 1;
		a_run.value = 1;
		segs.push_back(a_run);
	}

	//计算前向角和后向角
	std::vector< SSG_pntDirAngle> corners;
	corners.resize(vldPts.size());
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if ((lineIdx == 399) && (i == 419))
			int kkk = 1;
		//前向寻找
		int pre_i = -1;
		for (int j = i - 1; j >= 0; j--)
		{
			double dist = sqrt(	pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
								pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (dist >= cornerPara.scale)
			{
				pre_i = j;
				break;
			}
		}
		//后向寻找
		int post_i = -1;
		for (int j = i + 1; j < i_max; j++)
		{
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
				pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (dist >= cornerPara.scale)
			{
				post_i = j;
				break;
			}
		}
		//计算拐角
		if ((pre_i < 0) || (post_i < 0))
		{
			corners[i].pntIdx = -1;
			corners[i].forwardAngle = 0;
			corners[i].backwardAngle = 0;
			corners[i].corner = 0;
			corners[i].forwardDiffZ = 0;
			corners[i].backwardDiffZ = 0;
		}
		else
		{
			double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
			double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
			double forwardAngle = atan(tanValue_post) * 180.0 / PI;
			double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
			corners[i].pntIdx = i;
			corners[i].forwardAngle = forwardAngle;
			corners[i].backwardAngle = backwardAngle;
			corners[i].corner = -(forwardAngle - backwardAngle);  //图像坐标系与正常坐标系y方向相反，所以有“-”号
			corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
			corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
		}
	}

	//搜索拐角极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_pntDirAngle* pre_data = NULL;
	std::vector< SSG_pntDirAngle> cornerPeakP;
	std::vector< SSG_pntDirAngle> cornerPeakM;
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if (i == 275)
			int kkk = 1;
		SSG_pntDirAngle* curr_data = &corners[i];
		if (curr_data->pntIdx < 0)
		{
			if (i == i_max-1)  //最后一个
			{
				if (1 == _state)  //上升
				{
					cornerPeakP.push_back(corners[eEdgePtIdx]);
				}
				else if (2 == _state) //下降
				{
					cornerPeakM.push_back(corners[eEdgePtIdx]);
				}
			}
			continue;
		}

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double cornerDiff = curr_data->corner - pre_data->corner;
		switch (_state)
		{
		case 0:  //初态
			if (cornerDiff < 0) //下降
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (cornerDiff < 0)   //下降
			{
				cornerPeakP.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //下降
			if (cornerDiff > 0)   //  上升
			{
				cornerPeakM.push_back(*pre_data);
				_state = 1;
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//注意：最后一个不处理，为基座位置

	//极小值点（峰顶）
	//极值比较，在尺度窗口下寻找局部极值点
	double square_distTh = 4 * cornerPara.scale * cornerPara.scale;  //2倍的cornerScale。
	for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
	{
		if (cornerPeakP[i].corner < cornerPara.cornerTh)
			continue;

		bool isPeak = true;
		//向前搜索
		int cornerPtIdx = cornerPeakP[i].pntIdx;
		for (int j = i - 1; j >= 0; j--)
		{
			int prePtIdx = cornerPeakP[j].pntIdx;
			double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (cornerPeakP[i].corner < cornerPeakP[j].corner)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			cornerPtIdx = cornerPeakP[i].pntIdx;
			for (int j = i + 1; j < i_max; j++)
			{
				int postPtIdx = cornerPeakP[j].pntIdx;
				double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (cornerPeakP[i].corner < cornerPeakP[j].corner)
				{
					isPeak = false;
					break;
				}
			}
		}
		if (true == isPeak)
		{
			SSG_basicFeature1D a_feature;
			if( (cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2))
				a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
			else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2))
				a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
			else
				a_feature.featureType = LINE_FEATURE_CORNER_V;


			a_feature.jumpPos = vldPts[cornerPtIdx].pt3D;
			a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx };
			line_features->features.push_back(a_feature);
		}
	}

	//添加开始和结束边界
	//检查seg是否需要合并；向后合并
	for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++)
	{
		SSG_RUN* nxt_seg = &segs[i + 1];
		SSG_RUN* curr_seg = &segs[i];

		int idx_1 = curr_seg->start + curr_seg->len - 1;
		int idx_2 = nxt_seg->start;
		double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
		double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z);
		if ( (y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //合并
		{
			int idx_end = nxt_seg->start + nxt_seg->len - 1;
			nxt_seg->start = curr_seg->start;
			nxt_seg->len = idx_end - curr_seg->start + 1;
			curr_seg->value = 0;
		}
	}
	for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
	{
		if (0 == segs[i].value)  //被合并
			continue;
		
		int idx_1 = segs[i].start;
		int idx_2 = segs[i].start + segs[i].len - 1;

		SSG_basicFeature1D an_edge;
		memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
		an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
		an_edge.jumpPos = lineData[idx_1].pt3D;
		an_edge.jumpPos2D = { lineIdx, idx_1 };
		line_features->endings.push_back(an_edge);
		//line_features.insert(line_features.begin(), an_edge); //头部
		//尾部
		an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
		an_edge.jumpPos = lineData[idx_2].pt3D;
		an_edge.jumpPos2D = { lineIdx, idx_2 };
		line_features->endings.push_back(an_edge);
	}
	return;
}

/// <summary>
/// 提取激光线上的拐点特征，水平端点视为Corner
/// 根据Seg进行corner提取
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void sg_getLineCornerFeature_BQ(
	SVzNL3DPosition* lineData,
	int dataSize,
	int lineIdx,
	double refSteppingZ,
	const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
	std::vector<SSG_basicFeature1D>& line_features)
{
	//去除零点
	std::vector< SVzNL3DPosition> vldPts;
	std::vector< int> vldPtSegIdx;
	std::vector<SSG_RUN> segs;
	std::vector<int> backIndexing;
	backIndexing.resize(dataSize);

	int runIdx = 1;
	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
	double pre_z = 0;
	double pre_y = 0;
	for (int i = 0; i < dataSize; i++)
	{
		if (i == 370)
			int kkk = 1;
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = i;
			}
			else
			{
				double z_diff = abs(lineData[i].pt3D.z - pre_z);
				double y_diff = abs(lineData[i].pt3D.y - pre_y);
				if ((z_diff < cornerPara.minEndingGap_z) && (y_diff < cornerPara.minEndingGap))
				{
					a_run.len = i - a_run.start + 1;
					a_run.value = i;
				}
				else
				{
					a_run.value = runIdx;
					runIdx++;
					segs.push_back(a_run);

					a_run.start = i;
					a_run.len = 1;
					a_run.value = i;
				}
			}
			int bIdx = (int)vldPts.size();
			backIndexing[i] = bIdx;
			vldPts.push_back(lineData[i]);
			vldPtSegIdx.push_back(runIdx);

			pre_z = lineData[i].pt3D.z;
			pre_y = lineData[i].pt3D.y;
		}
	}
	if (a_run.len > 0)
		segs.push_back(a_run);

	//将点置标志
	for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
	{
		int idx1 = segs[i].start;
		int idx2 = segs[i].start + segs[i].len - 1;
		lineData[idx1].nPointIdx |= 0x100000;
		lineData[idx2].nPointIdx |= 0x200000;
	}

	//计算前向角和后向角
	std::vector< SSG_pntDirAngle> corners;
	corners.resize(vldPts.size());
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if ((lineIdx == 399) && (i == 419))
			int kkk = 1;
		//前向寻找
		int pre_i = -1;
		for (int j = i - 1; j >= 0; j--)
		{
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
				pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if ( (dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j]))
			{
				pre_i = j;
				break;
			}
		}
		//后向寻找
		int post_i = -1;
		for (int j = i + 1; j < i_max; j++)
		{
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
				pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if ( (dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j]))
			{
				post_i = j;
				break;
			}
		}
		//计算拐角
		double tanValue_pre = 0;
		if (pre_i >= 0)
			tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
		double tanValue_post = 0;
		if (post_i >= 0)
			tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
		double forwardAngle = atan(tanValue_post) * 180.0 / PI;
		double backwardAngle = atan(tanValue_pre) * 180.0 / PI;

		if ((pre_i < 0) && (post_i < 0))
		{
			corners[i].pntIdx = -1;
			corners[i].forwardAngle = 0;
			corners[i].backwardAngle = 0;
			corners[i].corner = 0;
			corners[i].forwardDiffZ = 0;
			corners[i].backwardDiffZ = 0;
		}
		else
		{
			corners[i].pntIdx = i;
			corners[i].forwardAngle = forwardAngle;
			corners[i].backwardAngle = backwardAngle;
			if ((pre_i >= 0) && (post_i >= 0))
			{
				corners[i].corner = -(forwardAngle - backwardAngle);  //图像坐标系与正常坐标系y方向相反，所以有“-”号
				corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
				corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
			}
			else
			{
				corners[i].corner = 0;
				corners[i].forwardDiffZ = 0;
				corners[i].backwardDiffZ = 0;
			}
		}
	}

	//搜索拐角极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_pntDirAngle* pre_data = NULL;
	std::vector< SSG_pntDirAngle> cornerPeakP;
	std::vector< SSG_pntDirAngle> cornerPeakM;
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if (i == 275)
			int kkk = 1;
		SSG_pntDirAngle* curr_data = &corners[i];
		if (curr_data->pntIdx < 0)
		{
			if (i == i_max - 1)  //最后一个
			{
				if (1 == _state)  //上升
				{
					cornerPeakP.push_back(corners[eEdgePtIdx]);
				}
				else if (2 == _state) //下降
				{
					cornerPeakM.push_back(corners[eEdgePtIdx]);
				}
			}
			continue;
		}

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double cornerDiff = curr_data->corner - pre_data->corner;
		switch (_state)
		{
		case 0:  //初态
			if (cornerDiff < 0) //下降
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (cornerDiff < 0)   //下降
			{
				cornerPeakP.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //下降
			if (cornerDiff > 0)   //  上升
			{
				cornerPeakM.push_back(*pre_data);
				_state = 1;
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//注意：最后一个不处理，为基座位置

	//极小值点（峰顶）
	//极值比较，在尺度窗口下寻找局部极值点
	double square_distTh = 4 * cornerPara.scale * cornerPara.scale;  //2倍的cornerScale。
	for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
	{
		if (cornerPeakP[i].corner < cornerPara.cornerTh)
			continue;

		bool isPeak = true;
		//向前搜索
		int cornerPtIdx = cornerPeakP[i].pntIdx;
		for (int j = i - 1; j >= 0; j--)
		{
			int prePtIdx = cornerPeakP[j].pntIdx;
			double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (cornerPeakP[i].corner < cornerPeakP[j].corner)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			cornerPtIdx = cornerPeakP[i].pntIdx;
			for (int j = i + 1; j < i_max; j++)
			{
				int postPtIdx = cornerPeakP[j].pntIdx;
				double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (cornerPeakP[i].corner < cornerPeakP[j].corner)
				{
					isPeak = false;
					break;
				}
			}
		}
		if (true == isPeak)
		{
			//判断是否是跳变到水平平面上
			SSG_basicFeature1D a_feature;
			a_feature.featureType = 0;
			if (abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) 
				a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
			else if (abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) 
				a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
			if (a_feature.featureType > 0)
			{
				int idx = vldPts[cornerPtIdx].nPointIdx;
				lineData[idx].nPointIdx |= (a_feature.featureType << 16);
			}
		}
	}
	//添加Feature
	for (int i = 0; i < dataSize; i++)
	{
		SVzNL3DPosition a_pt = lineData[i];
		int flag = a_pt.nPointIdx >> 16;
		int endFlag = flag >> 4;
		flag &= 0x0f;

		if (flag > 0)
		{
			double diffZ = abs(a_pt.pt3D.z - refSteppingZ);
			if (diffZ < 10.0)
			{
				SSG_basicFeature1D a_feature;
				memset(&a_feature, 0, sizeof(SSG_basicFeature1D));
				a_feature.featureType = flag;
				a_feature.jumpPos = a_pt.pt3D;
				a_feature.jumpPos2D = { lineIdx, i };
				line_features.push_back(a_feature);
			}
		}
		else if (1 == endFlag)
		{
			int cornerIdx = backIndexing[i];
			if (abs(corners[cornerIdx].forwardAngle) < cornerPara.jumpCornerTh_1)
			{
				double diffZ = abs(a_pt.pt3D.z - refSteppingZ);
				if (diffZ < 10.0)
				{
					SSG_basicFeature1D a_feature;
					memset(&a_feature, 0, sizeof(SSG_basicFeature1D));
					a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
					a_feature.jumpPos = a_pt.pt3D;
					a_feature.jumpPos2D = { lineIdx, i };
					line_features.push_back(a_feature);
				}
			}
		}
		else if (2 == endFlag)
		{
			int cornerIdx = backIndexing[i];
			if (abs(corners[cornerIdx].backwardAngle) < cornerPara.jumpCornerTh_1)
			{
				double diffZ = abs(a_pt.pt3D.z - refSteppingZ);
				if (diffZ < 10.0)
				{
					SSG_basicFeature1D a_feature;
					memset(&a_feature, 0, sizeof(SSG_basicFeature1D));
					a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
					a_feature.jumpPos = a_pt.pt3D;
					a_feature.jumpPos2D = { lineIdx, i };
					line_features.push_back(a_feature);
				}
			}
		}
	}
	return;
}

bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) {
	return a.pntIdx < b.pntIdx;
}
/// <summary>
/// 提取激光线上的Jumping特征
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void sg_getLineJumpFeature_cornerMethod(
	SVzNL3DPosition* lineData,
	int dataSize,
	int lineIdx,
	const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
	std::vector< SSG_basicFeature1D>& jumpFeatures)
{
	//去除零点
	std::vector< SVzNL3DPosition> vldPts;
	std::vector<SSG_RUN> segs;
	//修改seg的定义。seg端点是两点间距离大于门限的点
	int segStart = -1, segEnd = -1;
	for (int i = 0; i < dataSize; i++)
	{
		SVzNL3DPosition a_pt = lineData[i];
		a_pt.nPointIdx = i;
		if (lineData[i].pt3D.z > 1e-4)
			vldPts.push_back(a_pt);

		//seg判断
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (segStart < 0)
				segStart = i;
			else  //检查两点距离
			{
				SVzNL3DPosition pre_pt = lineData[i - 1];
				double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
				if (diff_z > cornerPara.minEndingGap_z)
				{
					SSG_RUN a_run;
					a_run.start = segStart;
					a_run.len = segEnd - segStart + 1;
					a_run.value = 1;
					segs.push_back(a_run);
					segStart = i;
				}
			}
			segEnd = i;
		}
		else
		{
			if (segStart >= 0)
			{
				SSG_RUN a_run;
				a_run.start = segStart;
				a_run.len = segEnd - segStart + 1;
				a_run.value = 1;
				segs.push_back(a_run);
				segStart = -1;
				segEnd = -1;
			}
		}
	}
	//last
	if (segStart >= 0)
	{
		SSG_RUN a_run;
		a_run.start = segStart;
		a_run.len = segEnd - segStart + 1;
		a_run.value = 1;
		segs.push_back(a_run);
	}

	//计算前向角和后向角
	std::vector< SSG_pntDirAngle> corners;
	corners.resize(vldPts.size());
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if (i == 875)
			int kkk = 1;

		//前向寻找
		int pre_i = -1;
		for (int j = i - 1; j >= 0; j--)
		{
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
				pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (dist >= cornerPara.scale)
			{
				pre_i = j;
				break;
			}
		}
		//后向寻找
		int post_i = -1;
		for (int j = i + 1; j < i_max; j++)
		{
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
				pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (dist >= cornerPara.scale)
			{
				post_i = j;
				break;
			}
		}
		//计算拐角
		if ((pre_i < 0) || (post_i < 0))
		{
			corners[i].pntIdx = -1;
			corners[i].forwardAngle = 0;
			corners[i].backwardAngle = 0;
			corners[i].corner = 0;
			corners[i].forwardDiffZ = 0;
			corners[i].backwardDiffZ = 0;
			corners[i].forward_z = 0;
			corners[i].backward_z = 0;
		}
		else
		{
			double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
			double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
			double forwardAngle = atan(tanValue_post) * 180.0 / PI;  //沿序号增长方向
			double backwardAngle = atan(tanValue_pre) * 180.0 / PI;  //沿序号减小方向
			corners[i].pntIdx = i;
			corners[i].forwardAngle = forwardAngle;
			corners[i].backwardAngle = backwardAngle;
			corners[i].corner = -(forwardAngle - backwardAngle);  //图像坐标系与正常坐标系y方向相反，所以有“-”号
			corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
			corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
			corners[i].forward_z = vldPts[post_i].pt3D.z; //沿序号增长方向
			corners[i].backward_z = vldPts[pre_i].pt3D.z; //沿序号减小方向
		}
	}

	//搜索拐角极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_pntDirAngle* pre_data = NULL;
	std::vector< SSG_pntDirAngle> cornerPeakP;
	std::vector< SSG_pntDirAngle> cornerPeakM;
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if (i == 275)
			int kkk = 1;
		SSG_pntDirAngle* curr_data = &corners[i];
		if (curr_data->pntIdx < 0)
		{
			if (i == i_max - 1)  //最后一个
			{
				if (1 == _state)  //上升
				{
					cornerPeakP.push_back(corners[eEdgePtIdx]);
				}
				else if (2 == _state) //下降
				{
					cornerPeakM.push_back(corners[eEdgePtIdx]);
				}
			}
			continue;
		}

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double cornerDiff = curr_data->corner - pre_data->corner;
		switch (_state)
		{
		case 0:  //初态
			if (cornerDiff < 0) //下降
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (cornerDiff < 0)   //下降
			{
				cornerPeakP.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //下降
			if (cornerDiff > 0)   //  上升
			{
				cornerPeakM.push_back(*pre_data);
				_state = 1;
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//注意：最后一个不处理，为基座位置

	//极小值点（峰顶）
	//极值比较，在尺度窗口下寻找局部极值点
	std::vector< SSG_pntDirAngle> cornerFeatures;
	double square_distTh = 4 * cornerPara.scale * cornerPara.scale;  //2倍的cornerScale。
	for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
	{
		if (cornerPeakP[i].corner < cornerPara.cornerTh)
			continue;

		bool isPeak = true;
		//向前搜索
		int cornerPtIdx = cornerPeakP[i].pntIdx;
		for (int j = i - 1; j >= 0; j--)
		{
			int prePtIdx = cornerPeakP[j].pntIdx;
			double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (cornerPeakP[i].corner < cornerPeakP[j].corner)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			cornerPtIdx = cornerPeakP[i].pntIdx;
			for (int j = i + 1; j < i_max; j++)
			{
				int postPtIdx = cornerPeakP[j].pntIdx;
				double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (cornerPeakP[i].corner < cornerPeakP[j].corner)
				{
					isPeak = false;
					break;
				}
			}
		}
		if (true == isPeak)
		{
			if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2))
			{
				cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_HR;
				cornerFeatures.push_back(cornerPeakP[i]);
			}
			else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2))
			{
				cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_FH;
				cornerFeatures.push_back(cornerPeakP[i]);
			}
		}
	}
	for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++)
	{
		if (abs(cornerPeakM[i].corner) < cornerPara.cornerTh)
			continue;

		bool isPeak = true;
		//向前搜索
		int cornerPtIdx = cornerPeakM[i].pntIdx;
		for (int j = i - 1; j >= 0; j--)
		{
			int prePtIdx = cornerPeakM[j].pntIdx;
			double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner))
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			cornerPtIdx = cornerPeakM[i].pntIdx;
			for (int j = i + 1; j < i_max; j++)
			{
				int postPtIdx = cornerPeakM[j].pntIdx;
				double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner))
				{
					isPeak = false;
					break;
				}
			}
		}
		if (true == isPeak)
		{
			if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2))
			{
				cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_HF;
				cornerFeatures.push_back(cornerPeakM[i]);
			}
			else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2))
			{
				cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_RH;
				cornerFeatures.push_back(cornerPeakM[i]);
			}
		}
	}

	//排序
	std::sort(cornerFeatures.begin(), cornerFeatures.end(), compareByIdx);

	//进行组合获得Jump
	for (int i = 0, i_max = (int)cornerFeatures.size(); i < i_max-1; i++)
	{
		if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HR)
		{
			if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_RH)
			{
				int pntIdx_1 = cornerFeatures[i].pntIdx;
				int pntIdx_2 = cornerFeatures[i+1].pntIdx;
				//计算距离和跳变高度
				double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y);
				double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z);
				if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z))
				{
					SSG_basicFeature1D a_feature;
					a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
					a_feature.jumpPos = vldPts[pntIdx_1].pt3D;
					a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_1].nPointIdx };
					a_feature.featureValue = cornerFeatures[i].backward_z;
					jumpFeatures.push_back(a_feature);
				}
			}
		}
		else if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HF)
		{
			if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_FH)
			{
				int pntIdx_1 = cornerFeatures[i].pntIdx;
				int pntIdx_2 = cornerFeatures[i + 1].pntIdx;
				//计算距离和跳变高度
				double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y);
				double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z);
				if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z))
				{
					SSG_basicFeature1D a_feature;
					a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
					a_feature.jumpPos = vldPts[pntIdx_2].pt3D;
					a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_2].nPointIdx };
					a_feature.featureValue = cornerFeatures[i + 1].forward_z;
					jumpFeatures.push_back(a_feature);
				}
			}
		}
	}
	return;
}

//手套的环的特征：在扫描线的Peak点的前后一定范围内，存在前向角从正到负的变化范围超过150度（门限值）
void wd_getLineGloveArcs(
	std::vector<SVzNL3DPosition>& lineData,
	int lineIdx,
	const SSG_gloveArcParam arcPara, 
	std::vector<SSG_basicFeature1D>& gloveArcs)
{
	//去除零点
	std::vector< SVzNL3DPosition> vldPts;
	int dataSize = (int)lineData.size();
	if (dataSize < 10)  //点太少，无法检出特征
		return; 
	for (int i = 0; i < dataSize; i++)
	{
		if ((lineIdx == 399) && (i == 568))
			int kkk = 1;
		SVzNL3DPosition a_pt = lineData[i];
		a_pt.nPointIdx = i;
		if (lineData[i].pt3D.z > 1e-4)
			vldPts.push_back(a_pt);
	}
	
	//在同一尺度下计算前向角和后向角，以及极大值点。
	std::vector< SSG_pntDirAngle> corners;  //逐点计算前向角
	corners.resize(vldPts.size());
	std::vector< int> localMax; //寻找极大值点
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if ((lineIdx == 0) && (i == 2585))
			int kkk = 1;

		double pre_stepDist = 0;
		//前向寻找
		bool isMax = true;
		int pre_i = -1;
		for (int j = i - 1; j >= 0; j--)
		{
			if ((true == isMax) &&(vldPts[i].pt3D.z <= vldPts[j].pt3D.z))
				isMax = false;
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (j == (i - 1))
				pre_stepDist = dist;
			if (dist >= arcPara.scale_angle)
			{
				pre_i = j;
				break;
			}
		}
		//后向寻找
		double post_stepDist = 0;
		int post_i = -1;
		for (int j = i + 1; j < i_max; j++)
		{
			if (( true == isMax) && (vldPts[i].pt3D.z < vldPts[j].pt3D.z))
				isMax = false;
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (j == (i + 1))
				post_stepDist = dist;
			if (dist >= arcPara.scale_angle)
			{
				post_i = j;
				break;
			}
		}
		if (true == isMax)
			localMax.push_back(i);

		//计算拐角
		if ((pre_i < 0) || (post_i < 0))
		{
			corners[i].pntIdx = -1;
			corners[i].forwardAngle = 0;
			corners[i].backwardAngle = 0;
			corners[i].corner = 0;
			corners[i].forwardDiffZ = 0;
			corners[i].backwardDiffZ = 0;
			corners[i].pre_stepDist = pre_stepDist;
			corners[i].post_stepDist = post_stepDist;
		}
		else
		{
			double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
			double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
			double forwardAngle = atan(tanValue_post) * 180.0 / PI;
			double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
			corners[i].pntIdx = i;
			corners[i].forwardAngle = forwardAngle;
			corners[i].backwardAngle = backwardAngle;
			corners[i].corner = (forwardAngle - backwardAngle);  
			corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
			corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
			corners[i].pre_stepDist = pre_stepDist;
			corners[i].post_stepDist = post_stepDist;
		}
	}

	//在极大值前后搜索
	for (int i = 0, i_max = (int)localMax.size(); i < i_max; i++)
	{
		if (i == 31)
			int kkk = 1;
		int peakIdx = localMax[i];
		
		//向前搜索正的最大前向角
		double pre_sumLen = 0;
		double pre_angleMax = 0;
		for (int j = peakIdx - 1; j >= 0; j--)
		{
			if (pre_angleMax < corners[j].forwardAngle)
				pre_angleMax = corners[j].forwardAngle;
			pre_sumLen += corners[j].post_stepDist;
			if (pre_sumLen >= arcPara.scale_corner)
				break;
		}
		//向后搜索负的最大前向角
		double post_angleMax = corners[peakIdx].forwardAngle;
		double post_sumLen = corners[peakIdx].post_stepDist;
		for (int j = peakIdx + 1; j < (int)vldPts.size(); j++)
		{
			post_sumLen += corners[j].post_stepDist;
			if (post_sumLen >= arcPara.scale_corner)
				break;
			if (post_angleMax > corners[j].forwardAngle)
				post_angleMax = corners[j].forwardAngle;
		}
		//计算最大转角
		double totalTurn = pre_angleMax - post_angleMax;
		if (totalTurn >= arcPara.cornerTh)  //有效ARC
		{
			SSG_basicFeature1D a_feature;
			a_feature.featureType = LINE_FEATURE_PEAK_TOP;
			a_feature.jumpPos = vldPts[peakIdx].pt3D;
			a_feature.jumpPos2D = { lineIdx, vldPts[peakIdx].nPointIdx };
			a_feature.featureValue = totalTurn;
			gloveArcs.push_back(a_feature);
		}
	}
	return;
}

/// <summary>
/// 提取激光线上的极值点（极大值点和极小值点）
/// 
/// </summary>
void sg_getLineLocalPeaks(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	int lineIdx, 
	const double scaleWin, 
	std::vector< SSG_basicFeature1D>& localMax, 
	std::vector< SSG_basicFeature1D>& localMin)
{
	if (dataSize < 2)
		return;

	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SVzNL3DPosition* pre_data = NULL;
	std::vector< SVzNL3DPosition> pkTop;
	std::vector< SVzNL3DPosition> pkBtm;
	for (int i = 0; i < dataSize; i++)
	{
		lineData[i].nPointIdx = i;
		SVzNL3DPosition* curr_data = &lineData[i];
		if (curr_data->pt3D.z < 1e-4)
		{
			if (i == dataSize - 1)  //最后一个
			{
				if ( 1 == _state )  //上升
				{
					pkTop.push_back(lineData[eEdgePtIdx]);
				}
				else if (2 == _state) //下降
				{
					pkBtm.push_back(lineData[eEdgePtIdx]);
				}
			}
			continue;
		}

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
		switch (_state)
		{
		case 0:  //初态
			if (z_diff > 0) //下降
			{
				_state = 2;
			}
			else if (z_diff < 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (z_diff > 0)   //下降
			{
				pkTop.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //下降
			if (z_diff < 0)   //  上升
			{
				pkBtm.push_back(*pre_data);
				_state = 1;
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//注意：最后一个不处理，为基座位置

	//极小值点（峰顶）
	//极值比较，在尺度窗口下寻找局部极值点
	double square_distTh = scaleWin * scaleWin;
	for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
	{
		bool isPeak = true;
		//向前搜索
		for (int j = i - 1; j >= 0; j--)
		{
			double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			for (int j = i + 1; j < i_max; j++)
			{
				double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
				{
					isPeak = false;
					break;
				}
			}
		}
		if (true == isPeak)
		{
			SSG_basicFeature1D a_feature;
			a_feature.featureType = LINE_FEATURE_PEAK_TOP;
			a_feature.jumpPos = pkTop[i].pt3D;
			a_feature.jumpPos2D = { lineIdx, pkTop[i].nPointIdx };
			localMin.push_back(a_feature);
		}
	}
	//极大值点（谷底）
	for (int i = 0, i_max = (int)pkBtm.size(); i < i_max; i++)
	{
		bool isPeak = true;
		//向前搜索
		for (int j = i - 1; j >= 0; j--)
		{
			double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			for (int j = i + 1; j < i_max; j++)
			{
				double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
				{
					isPeak = false;
					break;
				}
			}
		}
		if (true == isPeak)
		{
			SSG_basicFeature1D a_feature;
			a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM;
			a_feature.jumpPos = pkBtm[i].pt3D;
			a_feature.jumpPos2D = { lineIdx, pkBtm[i].nPointIdx };
			localMax.push_back(a_feature);
		}
	}

	return;
}

#if 0
typedef struct
{
	double holeLen;
	double holeCenterY;
	double holeCenterX;
	int holeSIdx;
	int holeEIdx;
}_HoleInfo;

typedef struct
{
	double maxZ;
	double minZ;
	double pk_y;
	double pk_x;
	int pkIdx;
}_Feature;
//对调平后的直线寻找突起。采用分段法寻找。
void sg_getFlatLineLocalPeaks_vector(
	std::vector<SVzNL3DPosition>& lineData,
	int lineIdx,
	const double scaleWin,
	const double minPkHeighth,
	const double holeR,
	std::vector< SSG_basicFeature1D>& localMax)
{
	int dataSize = lineData.size();
	if (dataSize < 2)
		return;

	double maxZ = 0;
	double subScale = scaleWin / 3.0;
	std::vector<_Feature> subFeatures;
	int maxIdx = -1;
	int minIdx = -1;
	bool startVld = false;
	double startY = 0;
	int preVldPt = 0;
	int holePtNum = 0;
	_HoleInfo a_hole = { 0.0, 0.0, 0.0, -1, -1 };
	for (int i = 0; i < dataSize; i++)
	{
		lineData[i].nPointIdx = 0;
		if ((lineIdx == 1219) && (i == 2722))
			int kkk = 1;

		if (lineData[i].pt3D.z < 1e-4)
		{
			if (true == startVld)
				holePtNum++;
		}
		else
		{
			if (maxZ < lineData[i].pt3D.z)
				maxZ = lineData[i].pt3D.z;

			if (false == startVld)
			{
				startVld = true;
				startY = lineData[i].pt3D.y;
			}
			//检查孔洞
			if ((holePtNum > 0) && (preVldPt >= 0))
			{
				double holeLen = lineData[i].pt3D.y - lineData[preVldPt].pt3D.y;
				if (holeLen > holeR / 3)
				{
					_HoleInfo currHole;
					currHole.holeLen = holeLen;
					currHole.holeCenterY = (lineData[i].pt3D.y + lineData[preVldPt].pt3D.y) / 2;
					currHole.holeCenterX = (lineData[i].pt3D.x + lineData[preVldPt].pt3D.x) / 2;
					currHole.holeSIdx = preVldPt;
					currHole.holeEIdx = i;
				
					if (a_hole.holeSIdx >= 0) 
					{
						//检查孔洞是否可以合并，如果不能合并，则取大的孔洞
						double hole_dist = currHole.holeCenterY - a_hole.holeCenterY;
						if (hole_dist < holeR * 2)
						{
							a_hole.holeEIdx = currHole.holeEIdx;
							a_hole.holeLen = lineData[a_hole.holeEIdx].pt3D.y - lineData[a_hole.holeSIdx].pt3D.y;
							a_hole.holeCenterY = (lineData[a_hole.holeEIdx].pt3D.y + lineData[a_hole.holeSIdx].pt3D.y) / 2;
						}
						else if (a_hole.holeLen < currHole.holeLen)
							a_hole = currHole;

					}
					else
						a_hole = currHole;
				}
				//重新检查孔洞
				holePtNum = 0;
			}

			double diffY = lineData[i].pt3D.y - startY;
			if (diffY >= subScale)  //分段检查
			{
				if (a_hole.holeSIdx >= 0)
				{
					_Feature a_subFeature;
					a_subFeature.minZ = -1;
					a_subFeature.maxZ = -1;
					a_subFeature.pk_y = a_hole.holeCenterY;
					a_subFeature.pk_x = a_hole.holeCenterX;
					a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2;
					subFeatures.push_back(a_subFeature);
				}
				else if (maxIdx >= 0)
				{
					double depth = lineData[maxIdx].pt3D.z - lineData[minIdx].pt3D.z;
					if (depth > minPkHeighth / 2)
					{
						_Feature a_subFeature;
						a_subFeature.minZ = lineData[minIdx].pt3D.z;
						a_subFeature.maxZ = lineData[maxIdx].pt3D.z;
						a_subFeature.pk_x = lineData[maxIdx].pt3D.x;
						a_subFeature.pk_y = lineData[maxIdx].pt3D.y;
						a_subFeature.pkIdx = maxIdx;
						subFeatures.push_back(a_subFeature);
					}
				}
				//分段重新开始
				maxIdx = -1;
				minIdx = -1;
				startY = lineData[i].pt3D.y;
				a_hole = { 0.0, 0.0, 0.0, -1, -1 };
			}

			if (maxIdx < 0)
				maxIdx = i;
			else
			{
				if (lineData[maxIdx].pt3D.z < lineData[i].pt3D.z)
					maxIdx = i;
			}
			if (minIdx < 0)
				minIdx = i;
			else
			{
				if (lineData[minIdx].pt3D.z > lineData[i].pt3D.z)
					minIdx = i;
			}

			//处理最后一个数据
			if (i == dataSize - 1)
			{
				if (a_hole.holeSIdx >= 0)
				{
					_Feature a_subFeature;
					a_subFeature.minZ = -1;
					a_subFeature.maxZ = -1;
					a_subFeature.pk_x = a_hole.holeCenterX;
					a_subFeature.pk_y = a_hole.holeCenterY;
					a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2;
					subFeatures.push_back(a_subFeature);
				}
				else if (maxIdx >= 0)
				{
					_Feature a_subFeature;
					a_subFeature.minZ = lineData[minIdx].pt3D.z;
					a_subFeature.maxZ = lineData[maxIdx].pt3D.z;
					a_subFeature.pk_x = lineData[maxIdx].pt3D.x;
					a_subFeature.pk_y = lineData[maxIdx].pt3D.y;
					a_subFeature.pkIdx = maxIdx;
					subFeatures.push_back(a_subFeature);
				}
			}
			preVldPt = i;
		}
	}

	for (int i = 0, i_max = subFeatures.size(); i < i_max; i++)
	{
		_Feature* currFeature = &subFeatures[i];
		if (currFeature->pkIdx < 0)
			continue;
		if (currFeature->maxZ < 0)  // 孔洞
		{
			bool isCombined = false;
			if (i < i_max - 1) //向后合并
			{
				_Feature* nxtFeature = &subFeatures[i + 1];
				//检查孔洞是否可以合并，如果不能合并，则取大的孔洞
				double hole_dist = nxtFeature->pk_y - currFeature->pk_y;
				if (hole_dist < holeR*2)  //合并
				{
					if (nxtFeature->maxZ < 0)
					{
						nxtFeature->pkIdx = (currFeature->pkIdx + nxtFeature->pkIdx) / 2;
						nxtFeature->pk_x = (currFeature->pk_x + nxtFeature->pk_x) / 2;
						nxtFeature->pk_y = (currFeature->pk_y + nxtFeature->pk_y) / 2;
					}
					currFeature->pkIdx = -1; //合并标记
					isCombined = true;
				}
			}
			if (false == isCombined)
			{
				//生成特征
				SSG_basicFeature1D a_feature;
				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
				a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx };
				a_feature.jumpPos.x = currFeature->pk_x;
				a_feature.jumpPos.y = currFeature->pk_y;
				a_feature.jumpPos.z = maxZ;
				localMax.push_back(a_feature);
				lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
				lineData[currFeature->pkIdx].pt3D = a_feature.jumpPos;
			}
		}
		else
		{
			bool isBigger_1 = true; //比前一个大
			bool isBigger_2 = true; //比后一个大
			double minZ = currFeature->minZ;

			if (i > 0)
			{
				_Feature* preFeature = &subFeatures[i - 1];
				if (preFeature->maxZ < 0)
				{
					if(preFeature->pkIdx >= 0)
						isBigger_1 = false;
				}
				else
				{
					if (currFeature->minZ > preFeature->minZ)
						minZ = preFeature->minZ;
					if (currFeature->maxZ < preFeature->maxZ)
						isBigger_1 = false;
				}
			}
			if (i < i_max - 1)
			{
				_Feature* nxtFeature = &subFeatures[i + 1];
				if (nxtFeature->maxZ < 0)
				{
					double hole_dist = nxtFeature->pk_y - currFeature->pk_y;
					if (hole_dist >= holeR * 2)
						isBigger_2 = false;
					else
						nxtFeature->pkIdx = -1; //后面的孔被合并
				}
				else
				{
					if (currFeature->minZ > nxtFeature->minZ)
						minZ = nxtFeature->minZ;
					if (currFeature->maxZ < nxtFeature->maxZ)
						isBigger_2 = false;
				}
			}
			if ((true == isBigger_1) && (true == isBigger_2))
			{
				double z_diff = currFeature->maxZ - minZ;
				if (z_diff > minPkHeighth)  //合格的突起
				{
					SSG_basicFeature1D a_feature;
					a_feature.featureType = LINE_FEATURE_PEAK_TOP;
					a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx };
					a_feature.jumpPos = lineData[currFeature->pkIdx].pt3D;
					localMax.push_back(a_feature);
					lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
				}
			}
		}
	}
	return;
}
#else
//对调平后的直线寻找突起。
//根据斜率寻找：寻找跳变、斜坡、Gap，进行配对
typedef struct
{
	int type;
	int ptIdxS;
	int ptIdxE;
	int sIdx;
	int eIdx;
	SVzNL3DPoint start;
	SVzNL3DPoint end;
	double delta_y;
	double delta_z;
	double k;
}_FeatureInfo;

int _findSegMaxZPt(
	std::vector<SVzNL3DPosition>& lineData, 
	int searchStart, 
	bool searchToHead, 
	double searchWin, 
	double* meanZ,
	bool* foundJump)
{
	int dataSize = (int)lineData.size();
	double start_y = lineData[searchStart].pt3D.y;
	double start_z = lineData[searchStart].pt3D.z;
	if (start_z < 1e-4)
		return -1; 

	double max_z = start_z;
	int max_idx = searchStart;
	double sum_z = max_z;
	int sum_num = 1;
	int step = searchToHead == true ? -1 : 1;
	int idx = searchStart;
	while (1)
	{
		idx += step;
		if ((idx <= 0) || (idx >= dataSize))
			break;

		if (lineData[idx].nPointIdx == LINE_FEATURE_NUM)
		{
			*foundJump = true;
			break;
		}
		double pt_y = lineData[idx].pt3D.y;
		double pt_z = lineData[idx].pt3D.z;
		if (pt_z > 1e-4)
		{
			double dist = abs(pt_y - start_y);
			if (dist > searchWin)
			{
				*meanZ = sum_z / sum_num;
				return max_idx;
			}
			else
			{
				sum_z += pt_z;
				sum_num++;
				if (max_z < pt_z)
				{
					max_z = pt_z;
					max_idx = idx;
				}
			}

		}
	}
	return -1;
}

void sg_getFlatLineLocalPeaks_vector(
	std::vector<SVzNL3DPosition>& lineData,
	int lineIdx,
	const double scaleWin,
	const double minPkHeighth,
	const double holeR,
	std::vector< SSG_basicFeature1D>& localMax)
{
	int dataSize = (int)lineData.size();
	if (dataSize < 2)
		return;

	//加速，进行抽样
	//抽样的点中去除空点
	double slopeZDeltaTH = 0.2; //slope的z变化门限，大于此门限为有限slope
	int dwnSample = 1;
	int sampleNum = dataSize / dwnSample;
	std::vector<SVzNL3DPosition> sampleData;
	for (int i = 0; i < sampleNum; i++)
	{
		SVzNL3DPosition a_sample = lineData[i * dwnSample];
		a_sample.nPointIdx = i * dwnSample;
		if(a_sample.pt3D.z > 1e-4)
			sampleData.push_back(a_sample);
	}

	sampleNum = (int)sampleData.size();
	std::vector< _FeatureInfo> allSubFeatures;
	allSubFeatures.resize(sampleNum);
	for (int i = 0; i < sampleNum; i++)  //初始化
		allSubFeatures[i].type = LINE_FEATURE_UNDEF;

	int segEnd = 0;
	for (int i = 0; i < sampleNum-1; i++)
	{
		SVzNL3DPosition* samplePtr = &sampleData[i];
		if ((lineIdx == 0) && (i == 2290))
			int kkk = 1;
		//检查相邻两点是否为跳变或gap
		SVzNL3DPosition* nxtSamplePtr = &sampleData[i+1];
		//double d_delta = sqrt(pow(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y, 2) + pow(nxtSamplePtr->pt3D.z - samplePtr->pt3D.z, 2));
		double y_delta = abs(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y);
		double z_delta = nxtSamplePtr->pt3D.z - samplePtr->pt3D.z;
		if ((y_delta > holeR / 2) || (abs(z_delta) > minPkHeighth))
		{
			_FeatureInfo a_subFeature;
			a_subFeature.delta_y = y_delta;
			a_subFeature.delta_z = z_delta;
			a_subFeature.k = z_delta / y_delta;
			a_subFeature.ptIdxS = i;
			a_subFeature.ptIdxE = i+1;
			a_subFeature.sIdx = sampleData[i].nPointIdx;
			a_subFeature.eIdx = sampleData[i + 1].nPointIdx;
			a_subFeature.start = samplePtr->pt3D;
			a_subFeature.end = nxtSamplePtr->pt3D;
			if(z_delta > 0)
				a_subFeature.type = LINE_FEATURE_L_JUMP_L2H;
			else
				a_subFeature.type = LINE_FEATURE_L_JUMP_H2L;
			allSubFeatures[i] = a_subFeature;
			segEnd = i + 1;
		}
		else
		{
			//寻找尺度端点
			for (int j = segEnd; j < sampleNum; j++)
			{
				SVzNL3DPosition* postSamplePtr = &sampleData[j];
				double deltaY = abs(postSamplePtr->pt3D.y - samplePtr->pt3D.y);
				if (deltaY >= scaleWin) 
				{
					double deltaZ = postSamplePtr->pt3D.z - samplePtr->pt3D.z;
					if (abs(deltaZ) > slopeZDeltaTH)
						int kkk = 1;
					_FeatureInfo a_subFeature;
					a_subFeature.delta_y = deltaY;
					a_subFeature.delta_z = deltaZ;
					a_subFeature.k = deltaZ / deltaY;
					a_subFeature.ptIdxS = i;
					a_subFeature.ptIdxE = j;
					a_subFeature.sIdx = sampleData[i].nPointIdx;
					a_subFeature.eIdx = sampleData[j].nPointIdx;
					a_subFeature.start = samplePtr->pt3D;
					a_subFeature.end = postSamplePtr->pt3D;
					if ( (abs(a_subFeature.k) > 0.5) && (abs(deltaZ) > slopeZDeltaTH))
					{
						if (deltaZ > 0)
							a_subFeature.type = LINE_FEATURE_L_SLOPE_L2H;
						else
							a_subFeature.type = LINE_FEATURE_L_SLOPE_H2L;
					}
					else
						a_subFeature.type = LINE_FEATURE_UNDEF;
					allSubFeatures[i] = a_subFeature;
					segEnd = j;
					break;
				}
			}
		}
	}


	//处理JUMP, 将相邻的LINE_FEATURE_L_SLOPE_L2H无效
	for (int i = 0; i < sampleNum; i++)
	{
		if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)
		{
			//将相邻的LINE_FEATURE_L_SLOPE_L2H无效
			for (int j = i + 1; j < sampleNum; j++)
			{
				if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_L2H)
					allSubFeatures[j].type = LINE_FEATURE_UNDEF;
				else
					break;
			}
		}
		else if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L)
		{
			//将相邻的LINE_FEATURE_L_SLOPE_L2H无效
			for (int j = i - 1; j >= 0; j--)
			{
				if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_H2L)
					allSubFeatures[j].type = LINE_FEATURE_UNDEF;
				else
					break;
			}
		}
	}
#if 0
	std::vector< _FeatureInfo> subFeatures;
	for (int i = 0; i < sampleNum; i++)
	{
		if (allSubFeatures[i].type != LINE_FEATURE_UNDEF)
			subFeatures.push_back(allSubFeatures[i]);
	}
	_FeatureInfo a_nullFeature;
	memset(&a_nullFeature, 0, sizeof(_FeatureInfo));
	subFeatures.push_back(a_nullFeature);  //在结尾补上一个空，序列处理时会正确处理最后一个
	int subfeatureSize = subFeatures.size();
#endif
	//将合格的subFeature挑选出来
	int pre_type = LINE_FEATURE_UNDEF;
	_FeatureInfo a_vldFeature;
	std::vector< _FeatureInfo> vldFeatures;
	for (int i = 0; i < sampleNum; i++)
	{
		if (pre_type == LINE_FEATURE_UNDEF)
		{
			if ( (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) ||
				(allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H))
			{
				vldFeatures.push_back(allSubFeatures[i]);
				pre_type = LINE_FEATURE_UNDEF;
			}
			else
			{
				a_vldFeature = allSubFeatures[i];
				pre_type = allSubFeatures[i].type;
			}
		}
		else
		{
			if (allSubFeatures[i].type == LINE_FEATURE_UNDEF)
			{
				vldFeatures.push_back(a_vldFeature);
			}
			else
			{
				if (((a_vldFeature.k < 0) && (allSubFeatures[i].k < 0)) ||
					((a_vldFeature.k > 0) && (allSubFeatures[i].k > 0)))
				{
					if (abs(a_vldFeature.k) < abs(allSubFeatures[i].k))
						a_vldFeature = allSubFeatures[i];
				}
				else
				{
					vldFeatures.push_back(a_vldFeature);
					a_vldFeature = allSubFeatures[i];
				}
			}
			pre_type = allSubFeatures[i].type;
		}
	}

	//配对，生成特征
	//优先对Jump进行配对
	std::vector<SSG_basicFeature1D> featureBuffer;
	featureBuffer.resize(vldFeatures.size());
	for (int i = 0; i < vldFeatures.size(); i++) //初始化
		featureBuffer[i].featureType = LINE_FEATURE_UNDEF;
	for (int i = 0, i_max = (int)vldFeatures.size(); i < i_max; i++)
	{
		if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_H2L)
		{
			//判断z的幅度
			if (abs(vldFeatures[i].delta_z) < minPkHeighth)  //没有起伏，可以匹配后面的JUMP
			{
				//生成特征
				SSG_basicFeature1D a_feature;
				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
				a_feature.jumpPos2D = { lineIdx, centerIdx };
				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
				int start = vldFeatures[i].sIdx;
				int end = vldFeatures[i].eIdx;
				//尝试匹配后面的JUMP
				if (i < i_max - 1)
				{
					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
						(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
					{
						//判断距离
						double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
						if (dist < holeR) //合并
						{
							centerIdx = (vldFeatures[i].sIdx + vldFeatures[i+1].eIdx) / 2;
							a_feature.jumpPos2D = { lineIdx, centerIdx };
							a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i+1].end.x) / 2;
							a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i+1].end.y) / 2;
							a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i+1].end.z) / 2;
							vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
							end = vldFeatures[i+1].eIdx;
						}
					}
				}
				featureBuffer[i] = a_feature;
				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
				lineData[end].nPointIdx = LINE_FEATURE_NUM;
			}
			else //有明显下降，只能匹配后面没有起伏的JUMP
			{
				//生成特征
				SSG_basicFeature1D a_feature;
				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
				a_feature.jumpPos2D = { lineIdx, centerIdx };
				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
				int start = vldFeatures[i].sIdx;
				int end = vldFeatures[i].eIdx;
				if (i < i_max - 1)
				{
					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
						(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
					{
						if (abs(vldFeatures[i + 1].delta_z) < minPkHeighth)  //只匹配没有起伏的
						{
							//判断距离
							double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
							if (dist < holeR) //合并
							{
								centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
								a_feature.jumpPos2D = { lineIdx, centerIdx };
								a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
								a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
								a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
								vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
								end = vldFeatures[i + 1].eIdx;
							}
						}
					}
				}
				featureBuffer[i] = a_feature;
				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
				lineData[end].nPointIdx = LINE_FEATURE_NUM;
			}
		}
		else if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)
		{
			//判断z的幅度
			if (abs(vldFeatures[i].delta_z) < minPkHeighth) //没有起伏，可以匹配后面的JUMP
			{
				//直接匹配后面的JUMP
				//生成特征
				SSG_basicFeature1D a_feature;
				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
				a_feature.jumpPos2D = { lineIdx, centerIdx };
				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
				int start = vldFeatures[i].sIdx;
				int end = vldFeatures[i].eIdx;
				//尝试匹配后面的JUMP
				if (i < i_max - 1)
				{
					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
						(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
					{
						//判断距离
						double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
						if (dist < holeR) //合并
						{
							centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
							a_feature.jumpPos2D = { lineIdx, centerIdx };
							a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
							a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
							a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
							vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
							end = vldFeatures[i + 1].eIdx;
						}
					}
				}
				featureBuffer[i] = a_feature;
				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
				lineData[end].nPointIdx = LINE_FEATURE_NUM;
			}
			else //有起伏的，可以匹配后面的下降JUMP和没有起伏的JUMP和SLOPE
			{
				//生成特征
				SSG_basicFeature1D a_feature;
				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
				a_feature.jumpPos2D = { lineIdx, centerIdx };
				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
				int start = vldFeatures[i].sIdx;
				int end = vldFeatures[i].eIdx;
				if (i < i_max - 1)
				{
					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
						((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H) && (abs(vldFeatures[i+1].delta_z) < minPkHeighth)) ||
						(vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L))
					{
						//判断距离
						double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
						if (dist < holeR) //合并
						{
							centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
							a_feature.jumpPos2D = { lineIdx, centerIdx };
							a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
							a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
							a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
							vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
							end = vldFeatures[i + 1].eIdx;
						}
						
					}
				}
				featureBuffer[i] = a_feature;
				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
				lineData[end].nPointIdx = LINE_FEATURE_NUM;
			}
		}
	}
	//对SLOPE进行配对
	for (int i = 0, i_max = (int)vldFeatures.size(); i < i_max; i++)
	{
		if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_L2H)
		{
			if (i < i_max - 1)
			{
				double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
				if ((dist < holeR) && (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L))
				{
					//生成特征
					SSG_basicFeature1D a_feature;
					a_feature.featureType = LINE_FEATURE_PEAK_TOP;
					int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
					a_feature.jumpPos2D = { lineIdx, centerIdx };
					a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
					a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
					a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
					vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
					featureBuffer[i] = a_feature;
				}
				else
				{
					SSG_basicFeature1D a_feature;
					a_feature.featureType = LINE_FEATURE_PEAK_TOP;
#if 0
					int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
					a_feature.jumpPos2D = { lineIdx, centerIdx };
					a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
					a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
					a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
#endif
					//在HoleR范围内寻找Peak点
					double mean_z = -1;
					bool foundJump = false;
					int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR*1.5, &mean_z, &foundJump);
					//反向寻找是否有Jump
					double meanZ_inv = -1;
					bool foundJump_inv = false;
					int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR * 1.5, &meanZ_inv, &foundJump_inv);
					if ( (pkIdx >= 0)&&(false == foundJump_inv))
					{
						double mean_depth = mean_z - vldFeatures[i].start.z;
						if (pkIdx_inv >= 0)
							mean_depth = mean_z - meanZ_inv;
						if (mean_depth >= slopeZDeltaTH/2)
						{
							a_feature.jumpPos2D = { lineIdx, pkIdx };
							a_feature.jumpPos = lineData[pkIdx].pt3D;
							featureBuffer[i] = a_feature;
						}
					}
					
				}
			}
		}
		else if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_H2L)
		{
			//在HoleR范围内寻找Peak点
			SSG_basicFeature1D a_feature;
			a_feature.featureType = LINE_FEATURE_PEAK_TOP;
#if 0
			int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
			a_feature.jumpPos2D = { lineIdx, centerIdx };
			a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
			a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
			a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
#endif
			//在HoleR范围内寻找Peak点
			double mean_z = -1;
			bool foundJump = false;
			int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR*1.5, &mean_z, &foundJump);
			//反向寻找是否有Jump
			double meanZ_inv = -1;
			bool foundJump_inv = false;
			int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR * 1.5, &meanZ_inv, &foundJump_inv);
			if ((pkIdx >= 0) && (false == foundJump_inv))
			{
				double mean_depth = mean_z - vldFeatures[i].start.z;
				if (pkIdx_inv >= 0)
					mean_depth = mean_z - meanZ_inv;
				if (mean_depth >= slopeZDeltaTH/2)
				{
					a_feature.jumpPos2D = { lineIdx, pkIdx };
					a_feature.jumpPos = lineData[pkIdx].pt3D;
					featureBuffer[i] = a_feature;
				}
			}
		}

	}
	for (int i = 0, i_max = (int)vldFeatures.size(); i < i_max; i++)
	{
		if (featureBuffer[i].featureType != LINE_FEATURE_UNDEF)
		{
			localMax.push_back(featureBuffer[i]);
			if (lineData[featureBuffer[i].jumpPos2D.y].pt3D.z < 1e-4)
				lineData[featureBuffer[i].jumpPos2D.y].pt3D = featureBuffer[i].jumpPos;
			lineData[featureBuffer[i].jumpPos2D.y].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
		}
	}
	return;
}
#endif

/// <summary>
/// 提取激光线上的下跳变点（Z值变大的跳变）
/// 
/// </summary>
void sg_getLineDownJumps(
	SVzNL3DPosition* lineData,
	int dataSize,
	int lineIdx,
	const double jumpTh,
	std::vector< SSG_basicFeature1D>& downJumps)
{
	if (dataSize < 2)
		return;

	int pre_i = -1;
	SVzNL3DPosition* pre_data = NULL;
	for (int i = 0; i < dataSize; i++)
	{
		if (i == 830)
			int kkk = 1;
		lineData[i].nPointIdx = i;
		SVzNL3DPosition* curr_data = &lineData[i];
		if (curr_data->pt3D.z < 1e-4)
		{
			continue;
		}

		if (NULL == pre_data)
		{
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
		if (z_diff > jumpTh)
		{
			SSG_basicFeature1D a_feature;
			a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
			a_feature.jumpPos = lineData[pre_i].pt3D;
			a_feature.jumpPos2D = { lineIdx, lineData[pre_i].nPointIdx };
			downJumps.push_back(a_feature);
		}
		pre_data = curr_data;
		pre_i = i;
	}
	return;
}

int _getSegPeak(SVzNL3DPosition* lineData, int segStartIdx, int segEndIdx)
{
	double pkZ = -1;
	int pkZIdx = -1;
	for (int i = segStartIdx; i <= segEndIdx; i++)
	{
		if (lineData[i].pt3D.z < 1e-4)
			continue;

		if (pkZ < 0)
		{
			pkZ = lineData[i].pt3D.z;
			pkZIdx = i;
		}
		else
		{
			if (pkZ > lineData[i].pt3D.z)
			{
				pkZ = lineData[i].pt3D.z;
				pkZIdx = i;
			}
		}
	}
	return pkZIdx;
}

int _getYNearestPtIdx(SVzNL3DPosition* lineData, int segStartIdx, int segEndIdx, double midY)
{
	double minDiff = -1;
	int pkZIdx = -1;
	for (int i = segStartIdx; i <= segEndIdx; i++)
	{
		if (lineData[i].pt3D.z < 1e-4)
			continue;

		double yDiff = abs(lineData[i].pt3D.y - midY);

		if (minDiff < 0)
		{
			minDiff = yDiff;
			pkZIdx = i;
		}
		else
		{
			if (minDiff > yDiff)
			{
				minDiff = yDiff;
				pkZIdx = i;
			}
		}
	}
	return pkZIdx;
}

/// <summary>
/// 提取激光线上的圆柱形特征
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void sg_getLineUpperSemiCircleFeature(
	SVzNL3DPosition* lineData,
	int dataSize,
	int lineIdx,
	const double sieveDiameter,
	const SSG_slopeParam slopePara,
	std::vector< SSG_featureSemiCircle>& result_features,
	cv::Mat& holeMask) //根据holeMask进行合并
{
	if (lineIdx == 194)
		int kkk = 1;
	//去除零点
	std::vector< SVzNL3DPosition> vldPts;
	std::vector<SSG_RUN> segs;
	int segStart = -1, segEnd = -1;
	for (int i = 0; i < dataSize; i++)
	{
		SVzNL3DPosition a_pt = lineData[i];
		a_pt.nPointIdx = i;
		if (lineData[i].pt3D.z > 1e-4)
		{
			if (segStart < 0)
				segStart = i;
			segEnd = i;
			vldPts.push_back(a_pt);
		}
		else
		{
			if (segStart >= 0)
			{
				SSG_RUN a_run;
				a_run.start = segStart;
				a_run.len = segEnd - segStart + 1;
				a_run.value = 1;
				segs.push_back(a_run);
				segStart = -1;
				segEnd = -1;
			}
		}
	}
	//last
	if (segStart >= 0)
	{
		SSG_RUN a_run;
		a_run.start = segStart;
		a_run.len = segEnd - segStart + 1;
		a_run.value = 1;
		segs.push_back(a_run);
	}
	//检查seg是否需要合并；向后合并
	for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++)
	{
		SSG_RUN* nxt_seg = &segs[i + 1];
		SSG_RUN* curr_seg = &segs[i];

		int idx_1 = curr_seg->start + curr_seg->len - 1;
		int idx_2 = nxt_seg->start;
		double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
		if (y_diff < slopePara.minEndingGap)  //合并
		{
			int idx_end = nxt_seg->start + nxt_seg->len - 1;
			nxt_seg->start = curr_seg->start;
			nxt_seg->len = idx_end - curr_seg->start + 1;
			curr_seg->value = 0;
		}
	}

	//获取localPeak
	std::vector< SSG_basicFeature1D> localMax;
	std::vector< SSG_basicFeature1D> localMin;
	std::vector<int> endingFlag;
	endingFlag.resize(dataSize);
	for (int i = 0; i < dataSize; i++)
		endingFlag[i] = 0;
	for (int i = 0,i_max= (int)segs.size(); i < i_max; i++)
	{
		if (0 == segs[i].value)  //被合并
			continue;

		int idx_1 = segs[i].start;
		int idx_2 = segs[i].start + segs[i].len - 1;
		endingFlag[idx_1] = 2;
		endingFlag[idx_2] = 3;

		std::vector< SSG_basicFeature1D> segMax;
		std::vector< SSG_basicFeature1D> segMin;
		sg_getLineLocalPeaks(
			&lineData[idx_1],
			segs[i].len,
			lineIdx,
			slopePara.LSlopeZWin,
			segMax,
			segMin);
		for (int m = 0, m_max = (int)segMax.size(); m < m_max; m++)
		{
			segMax[m].jumpPos2D.y += idx_1;
			localMax.push_back(segMax[m]);
		}
		for (int m = 0, m_max = (int)segMin.size(); m < m_max; m++)
		{
			segMin[m].jumpPos2D.y += idx_1;
			localMin.push_back(segMin[m]);
		}
	}

	//计算slope的Z变化
	std::vector< SSG_zWin> slopes; //记录窗口长度内的z变化
	slopes.resize(vldPts.size());
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		//后向寻找
		int post_i = -1;
		for (int j = i + 1; j < i_max; j++)
		{
			double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
				pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
			if (dist >= slopePara.LSlopeZWin)
			{
				post_i = j;
				break;
			}
		}
		//计算slope
		if (post_i < 0)
		{
			slopes[i].startPtIdx = i;
			slopes[i].endPtIdx = -1;
			slopes[i].zDiff = 0;
		}
		else
		{
			slopes[i].startPtIdx = i;
			slopes[i].endPtIdx = post_i;
			slopes[i].zDiff = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
		}
	}

	//搜索zWin极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_zWin* pre_data = NULL;
	std::vector< SSG_zWin> zWinPeakP;  //正的zWin
	std::vector< SSG_zWin> zWinPeakM;  //负的zWin
	SSG_zWin* maxPkPlus = NULL;
	SSG_zWin* maxPkMinus = NULL;
	for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
	{
		if (i == 275)
			int kkk = 1;
		SSG_zWin* curr_data = &slopes[i];
		if ( curr_data->endPtIdx < 0)
		{
			if ( (i == i_max - 1) && (pre_data != NULL))  //最后一个
			{
				if (1 == _state)  //上升
				{
					if (pre_data->zDiff > 0)
						zWinPeakP.push_back(slopes[eEdgePtIdx]);
				}
				else if (2 == _state) //下降
				{
					if (pre_data->zDiff < 0)
						zWinPeakM.push_back(slopes[eEdgePtIdx]);
				}
			}
			continue;
		}

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double zDiffDelta = curr_data->zDiff - pre_data->zDiff;
		switch (_state)
		{
		case 0:  //初态
			if (zDiffDelta < 0) //下降
			{
				if (curr_data->zDiff < 0)
				{
					maxPkMinus = curr_data;
					_state = 2;
				}
			}
			else if (zDiffDelta > 0) //上升
			{
				if (curr_data->zDiff > 0)
				{
					maxPkPlus = curr_data;
					_state = 1;
				}
			}
			break;
		case 1: //上升
			if (zDiffDelta < 0)   //下降
			{
				if(maxPkPlus)
					zWinPeakP.push_back(*maxPkPlus);
				maxPkPlus = NULL;
				if (curr_data->zDiff < 0)
					maxPkMinus = curr_data;
				else
					maxPkMinus = NULL;
				_state = 2;
			}
			else
			{
				//记录最大slope
				if (curr_data->zDiff > 0)
				{
					if (NULL == maxPkPlus)
						maxPkPlus = curr_data;
					else
					{
						if(maxPkPlus->zDiff < curr_data->zDiff)
							maxPkPlus = curr_data;
					}
				}
			}
			break;
		case 2: //下降
			if (zDiffDelta > 0)   //  上升
			{
				if (maxPkMinus)
					zWinPeakM.push_back(*maxPkMinus);
				maxPkMinus = NULL;
				if (curr_data->zDiff > 0)
					maxPkPlus = curr_data;
				else
					maxPkPlus = NULL;
				_state = 1;
			}
			else
			{
				//记录最大slope
				if (curr_data->zDiff < 0)
				{
					if (NULL == maxPkMinus)
						maxPkMinus = curr_data;
					else
					{
						if (maxPkMinus->zDiff > curr_data->zDiff)
							maxPkMinus = curr_data;
					}
				}
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}

	//极值比较，在尺度窗口下寻找局部极值点
	std::vector<SSG_zWin> validZWinPeakP;
	double square_distTh = 4 * slopePara.LSlopeZWin * slopePara.LSlopeZWin;  //2倍的cornerScale。
	for (int i = 0, i_max = (int)zWinPeakP.size(); i < i_max; i++)
	{
		if (zWinPeakP[i].zDiff < slopePara.validSlopeH)
			continue;

		bool isPeak = true;
		//向前搜索
		int currPtIdx = zWinPeakP[i].startPtIdx;
		for (int j = i - 1; j >= 0; j--)
		{
			int prePtIdx = zWinPeakP[j].startPtIdx;
			double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (zWinPeakP[i].zDiff < zWinPeakP[j].zDiff)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			currPtIdx = zWinPeakP[i].startPtIdx;
			for (int j = i + 1; j < i_max; j++)
			{
				int postPtIdx = zWinPeakP[j].startPtIdx;
				double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (zWinPeakP[i].zDiff < zWinPeakP[j].zDiff)
				{
					isPeak = false;
					break;
				}
			}
		}

		if (true == isPeak)
		{
			SSG_zWin vldPk;
			vldPk.startPtIdx = vldPts[zWinPeakP[i].startPtIdx].nPointIdx;
			vldPk.endPtIdx = vldPts[zWinPeakP[i].endPtIdx].nPointIdx;
			vldPk.zDiff = zWinPeakP[i].zDiff;
			validZWinPeakP.push_back(vldPk);
			//endingFlag[vldPk.startPtIdx] = 4;
		}
	}

	//极值比较，在尺度窗口下寻找局部极值点
	std::vector<SSG_zWin> validZWinPeakM;
	for (int i = 0, i_max = (int)zWinPeakM.size(); i < i_max; i++)
	{
		if (zWinPeakM[i].zDiff > -slopePara.validSlopeH)
			continue;

		bool isPeak = true;
		//向前搜索
		int currPtIdx = zWinPeakM[i].startPtIdx;
		for (int j = i - 1; j >= 0; j--)
		{
			int prePtIdx = zWinPeakM[j].startPtIdx;
			double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
			if (dist > square_distTh)  //超出尺度窗口
				break;

			if (zWinPeakM[i].zDiff > zWinPeakM[j].zDiff)
			{
				isPeak = false;
				break;
			}
		}
		//向后搜索
		if (true == isPeak)
		{
			currPtIdx = zWinPeakM[i].startPtIdx;
			for (int j = i + 1; j < i_max; j++)
			{
				int postPtIdx = zWinPeakM[j].startPtIdx;
				double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
				if (dist > square_distTh)  //超出尺度窗口
					break;

				if (zWinPeakM[i].zDiff > zWinPeakM[j].zDiff)
				{
					isPeak = false;
					break;
				}
			}
		}

		if (true == isPeak)
		{
			SSG_zWin vldPk;
			vldPk.startPtIdx = vldPts[zWinPeakM[i].startPtIdx].nPointIdx;
			vldPk.endPtIdx = vldPts[zWinPeakM[i].endPtIdx].nPointIdx;
			vldPk.zDiff = zWinPeakM[i].zDiff;
			validZWinPeakM.push_back(vldPk);
			//endingFlag[vldPk.endPtIdx] = 5;
		}
	}

	//对validCornerPeakP进行配对
	std::vector< SSG_featureSemiCircle> line_features;
	for (int i = 0, i_max = (int)localMin.size(); i < i_max; i++)
	{
		int ptIdx = localMin[i].jumpPos2D.y;
		if (endingFlag[ptIdx] > 0)  //已经被处理
			continue;

		//向前搜索
		int preIdx = -1;
		for (int j = ptIdx - 1; j >= 0; j--)
		{
			if (endingFlag[j] > 0)
			{
				preIdx = j;
				break;
			}
		}
		//身后搜索
		int postIdx = -1;
		for (int j = ptIdx + 1; j < dataSize; j++)
		{
			if (endingFlag[j] > 0)
			{
				postIdx = j;
				break;
			}
		}
		if ((preIdx >= 0) && (postIdx >= 0))
		{
			int preFlag = endingFlag[preIdx];
			int postFlag = endingFlag[postIdx];
			if ((2 == preFlag ) && (3 == postFlag))  //段落情况，筛网悬空会出现这种情况
			{
				double yDist = abs(lineData[preIdx].pt3D.y - lineData[postIdx].pt3D.y);
				if (yDist < sieveDiameter * 4)  //valid
				{
					SSG_featureSemiCircle a_feature;
					a_feature.flag = 0;
					a_feature.startPtIdx = preIdx;
					a_feature.endPtIdx = postIdx;
					double midY = (lineData[preIdx].pt3D.y + lineData[postIdx].pt3D.y) / 2.0;
					a_feature.width = yDist;
					a_feature.lineIdx = lineIdx;
					a_feature.midPtIdx = _getYNearestPtIdx(lineData, preIdx, postIdx, midY);
					a_feature.midPt = lineData[a_feature.midPtIdx].pt3D;
					if (a_feature.midPtIdx >= 0)
					{
						int pkIdx = _getSegPeak(lineData, preIdx, postIdx);
						a_feature.pkHeight = lineData[pkIdx].pt3D.z;
						line_features.push_back(a_feature);
						for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
						{
							if (0 == endingFlag[m])
								endingFlag[m] = 1;
						}
					}
				}
			}
			else if ( ((5 == preFlag) && (4 == postFlag))  || //有底板情况，筛网放置在一个平面上会出现这种情况
				((2 == preFlag) && (4 == postFlag)) ||
				((5 == preFlag) && (3 == postFlag)))
			{
				double yDist = abs(lineData[preIdx].pt3D.y - lineData[postIdx].pt3D.y);
				if (yDist < sieveDiameter * 4)  //valid
				{
					SSG_featureSemiCircle a_feature;
					a_feature.flag = 0;
					a_feature.startPtIdx = preIdx;
					a_feature.endPtIdx = postIdx;
					a_feature.width = abs(lineData[preIdx].pt3D.y - lineData[postIdx].pt3D.y);
					a_feature.lineIdx = lineIdx;
					double midY = (lineData[preIdx].pt3D.y + lineData[postIdx].pt3D.y) / 2.0;
					a_feature.midPtIdx = _getYNearestPtIdx(lineData, preIdx, postIdx, midY);
					a_feature.midPt = lineData[a_feature.midPtIdx].pt3D;
					if (a_feature.midPtIdx >= 0)
					{
						int pkPtIdx = _getSegPeak(lineData, preIdx, postIdx);
						a_feature.pkHeight = lineData[pkPtIdx].pt3D.z;
						line_features.push_back(a_feature);
						for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
						{
							if (0 == endingFlag[m])
								endingFlag[m] = 1;
						}
					}
				}
			}
		}
	}

	//根据孔洞目标进行合并
	for (int i = 0, i_max = (int)line_features.size(); i < i_max; i++)
	{
		if (FEATURE_FLAG_INVALID == line_features[i].flag)
			continue;

		if (i < i_max - 1)
		{
			int idx1 = line_features[i].endPtIdx + 1;
			int holeId1 = holeMask.at<int>(idx1, lineIdx);
			int idx2 = line_features[i + 1].startPtIdx - 1;
			int holeId2 = holeMask.at<int>(idx2, lineIdx);
			if( (holeId1 > 0) && (holeId1 == holeId2))
			{
				//合并
				line_features[i].endPtIdx = line_features[i + 1].endPtIdx;
				double midY = (lineData[line_features[i].startPtIdx].pt3D.y + lineData[line_features[i].endPtIdx].pt3D.y) / 2;
				line_features[i].midPtIdx = _getYNearestPtIdx(lineData, line_features[i].startPtIdx, line_features[i].endPtIdx, midY);
				line_features[i].midPt = lineData[line_features[i].midPtIdx].pt3D;
				line_features[i].width = abs(lineData[line_features[i].startPtIdx].pt3D.y - lineData[line_features[i].endPtIdx].pt3D.y);
				int pkPtIdx = _getSegPeak(lineData, line_features[i].startPtIdx, line_features[i].endPtIdx);
				line_features[i].pkHeight = lineData[pkPtIdx].pt3D.z;
				result_features.push_back(line_features[i]);
				line_features[i + 1].flag = FEATURE_FLAG_INVALID;
			}
			else
			{
				result_features.push_back(line_features[i]);
			}
		}
	}
	return;
}

//计算检查长度时使用街区距离以避免太多的平方和开方运算
//如果z方向为Jump返回True。Jump定义Seed前一个点z的变化大于门限
bool _getPtPreMaxDelta(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	int seedIdx, 
	const double chkLen, 
	double* maxDeltaY, 
	double* maxDeltaZ,
	int* endingIdx)
{
	double seedY = lineData[seedIdx].pt3D.y;
	double seedZ = lineData[seedIdx].pt3D.z;
	*maxDeltaY = 0;  //初始化成0
	*maxDeltaZ = 0;
	*endingIdx = -1;//初始化成无效值，表示没有足够长度
	if (seedIdx == 0)  //无法向前检查
		return false;

	double max_dy = -1;
	double max_dz = -1;
	for (int i = seedIdx - 1; i >= 0; i--)
	{
		SVzNL3DPoint* a_pt = &(lineData[i].pt3D);
		if (a_pt->z > 1e-4)
		{
			double dy = abs(a_pt->y - seedY);
			double dz = abs(a_pt->z - seedZ);
			double dist = dy + dz;

			if (dist > chkLen)
			{
				*endingIdx = i;
				*maxDeltaY = max_dy;
				*maxDeltaZ = max_dz;
				if (i == seedIdx - 1)
					return true;
				else
					return false;
			}
			if (max_dy < 0)
			{
				max_dy = dy;
				max_dz = dz;
			}
			else
			{
				max_dy = max_dy < dy ? dy : max_dy;
				max_dz = max_dz < dz ? dz : max_dz;
			}
		}
	}
	*endingIdx = -1;//没有足够长度
	return false;
}

//计算检查长度时使用街区距离以避免太多的平方和开方运算
//如果z方向为Jump返回True。Jump定义Seed前一个点z的变化大于门限
bool _getPtPostMaxDelta(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	int seedIdx, 
	const double chkLen, 
	double* maxDeltaY, 
	double* maxDeltaZ,
	int* endingIdx)
{
	double seedY = lineData[seedIdx].pt3D.y;
	double seedZ = lineData[seedIdx].pt3D.z;
	*maxDeltaY = 0;  //初始化成0
	*maxDeltaZ = 0;
	*endingIdx = -1;//初始化成无效值，表示没有足够长度
	if (seedIdx == dataSize-1)  //无法向后检查
		return false;


	double max_dy = -1;
	double max_dz = -1;
	for (int i = seedIdx + 1; i < dataSize; i++)
	{
		SVzNL3DPoint* a_pt = &(lineData[i].pt3D);
		if (a_pt->z > 1e-4)
		{
			double dy = abs(a_pt->y - seedY);
			double dz = abs(a_pt->z - seedZ);
			double dist = dy + dz;
			if (dist > chkLen)
			{
				*endingIdx = i;
				*maxDeltaY = max_dy;
				*maxDeltaZ = max_dz;
				if (i == seedIdx - 1)
					return true;
				else
					return false;
			}
			if (max_dy < 0)
			{
				max_dy = dy;
				max_dz = dz;
			}
			else
			{
				max_dy = max_dy < dy ? dy : max_dy;
				max_dz = max_dz < dz ? dz : max_dz;
			}
		}
	}
	*endingIdx = -1;//表示没有足够长度
	return false;
}

/// <summary>
/// 使用模板法提取直角特征
/// 水平向下直角特征：拐点左侧deltaZ在一个很小的范围内；拐点右侧deltaY在一个很小的范围内
/// </summary>
/// <param name="lineData"></param>
/// <param name="dataSize"></param>
/// <param name="lineIdx"></param>
/// <param name="slopeParam"></param>
/// <param name="valleyPara"></param>
/// <param name="line_features"></param>
void sg_getLineRigthAngleFeature(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	int lineIdx, 
	const SSG_lineRightAngleParam templatePara_HF,
	const SSG_lineRightAngleParam templatePara_FH,
	const SSG_lineRightAngleParam templatePara_HR, 
	const SSG_lineRightAngleParam templatePara_RH,
	SSG_lineFeature* line_features)
{
	double chkLen = templatePara_HF.H_len > templatePara_HF.V_len ? templatePara_HF.V_len : templatePara_HF.H_len;
	chkLen = chkLen > templatePara_FH.H_len ? templatePara_FH.H_len : chkLen;
	chkLen = chkLen > templatePara_FH.V_len ? templatePara_FH.V_len : chkLen;
	chkLen = chkLen > templatePara_HR.H_len ? templatePara_HR.H_len : chkLen;
	chkLen = chkLen > templatePara_HR.V_len ? templatePara_HR.V_len : chkLen;
	chkLen = chkLen > templatePara_RH.H_len ? templatePara_RH.H_len : chkLen;
	chkLen = chkLen > templatePara_RH.V_len ? templatePara_RH.V_len : chkLen;

	double maxDelta = templatePara_HF.maxDelta < templatePara_FH.maxDelta ? templatePara_FH.maxDelta : templatePara_HF.maxDelta;
	maxDelta = maxDelta < templatePara_HR.maxDelta ? templatePara_HR.maxDelta : maxDelta;
	maxDelta = maxDelta < templatePara_RH.maxDelta ? templatePara_RH.maxDelta : maxDelta;

	std::vector<int> types;
	std::vector<int> jumpFlags;
	types.resize(dataSize);
	jumpFlags.resize(dataSize);
	for (int i = 0; i < dataSize; i++)
	{
if (i == 98)
	int kkk = 1;
		types[i] = 0;
		jumpFlags[i] = 0;
		double maxDeltaY_pre = 0;
		double maxDeltaZ_pre = 0;
		int endingIdx_pre = -1;
		double maxDeltaY_post = 0;
		double maxDeltaZ_post = 0;
		int endingIdx_post = -1;
		if (lineData[i].pt3D.z < 1e-4)
			continue;

		//首先以最小的检查长度进行直角判断
		bool isJump_pre = _getPtPreMaxDelta(
			lineData, 
			dataSize, 
			i, 
			chkLen,
			&maxDeltaY_pre, 
			&maxDeltaZ_pre, 
			&endingIdx_pre);

		bool isJump_post = _getPtPostMaxDelta(
			lineData, 
			dataSize, 
			i, 
			chkLen, 
			&maxDeltaY_post, 
			&maxDeltaZ_post, 
			&endingIdx_post);
		//共四种直角：水平-向下；向下-水平；水平-向上；向上-水平
		if ((endingIdx_pre < 0) || (endingIdx_post < 0))  //没有足够长度
			continue;

		if (true == isJump_pre)
			maxDeltaY_pre = 0;  //实际可能会很大
		if (true == isJump_post)
			maxDeltaY_post = 0;  //实际可能会很大
		
		if ((maxDeltaZ_pre < maxDelta) && (maxDeltaY_post < maxDelta))  //可能为HX型（水平-垂直）
		{
			if (lineData[endingIdx_post].pt3D.z > lineData[i].pt3D.z)  //下降
			{
				//使用templatePara_HX的参数重新进行迭代计算
				isJump_pre = _getPtPreMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_HF.H_len,
					&maxDeltaY_pre,
					&maxDeltaZ_pre,
					&endingIdx_pre);
				isJump_post = _getPtPostMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_HF.V_len,
					&maxDeltaY_post,
					&maxDeltaZ_post,
					&endingIdx_post);

				if (true == isJump_pre)
					maxDeltaY_pre = 0;  //实际可能会很大
				if (true == isJump_post)
					maxDeltaY_post = 0;  //实际可能会很大
				if ((maxDeltaZ_pre < templatePara_HF.maxDelta) && (maxDeltaY_post < templatePara_HF.maxDelta))
				{
					if ((true == isJump_pre) || (true == isJump_post))
						jumpFlags[i] = 1;
					types[i] = LINE_FEATURE_RIGHT_ANGLE_HF;
				}
			}
			else  //上升
			{
				//使用templatePara_HX的参数重新进行迭代计算
				isJump_pre = _getPtPreMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_HR.H_len,
					&maxDeltaY_pre,
					&maxDeltaZ_pre,
					&endingIdx_pre);
				isJump_post = _getPtPostMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_HR.V_len,
					&maxDeltaY_post,
					&maxDeltaZ_post,
					&endingIdx_post);

				if (true == isJump_pre)
					maxDeltaY_pre = 0;  //实际可能会很大
				if (true == isJump_post)
					maxDeltaY_post = 0;  //实际可能会很大
				if ((maxDeltaZ_pre < templatePara_HF.maxDelta) && (maxDeltaY_post < templatePara_HF.maxDelta))
				{
					if ((true == isJump_pre) || (true == isJump_post))
						jumpFlags[i] = 1;
					types[i] = LINE_FEATURE_RIGHT_ANGLE_HR;;
				}
			}
		}
		else if ((maxDeltaY_pre < maxDelta) && (maxDeltaZ_post < maxDelta))  //可能为XH型（垂直-水平）
		{
			if (lineData[i].pt3D.z > lineData[endingIdx_pre].pt3D.z)  //下降
			{
				//使用templatePara_XH的参数重新进行迭代计算
				isJump_pre = _getPtPreMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_FH.V_len,
					&maxDeltaY_pre,
					&maxDeltaZ_pre,
					&endingIdx_pre);
				isJump_post = _getPtPostMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_FH.H_len,
					&maxDeltaY_post,
					&maxDeltaZ_post,
					&endingIdx_post);

				if (true == isJump_pre)
					maxDeltaY_pre = 0;  //实际可能会很大
				if (true == isJump_post)
					maxDeltaY_post = 0;  //实际可能会很大
				if ((maxDeltaY_pre < templatePara_FH.maxDelta) && (maxDeltaZ_post < templatePara_FH.maxDelta))
				{
					if ((true == isJump_pre) || (true == isJump_post))
						jumpFlags[i] = 1;
					types[i] = LINE_FEATURE_RIGHT_ANGLE_FH;
				}
			}
			else//上升
			{
				//使用templatePara_XH的参数重新进行迭代计算
				isJump_pre = _getPtPreMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_RH.V_len,
					&maxDeltaY_pre,
					&maxDeltaZ_pre,
					&endingIdx_pre);
				isJump_post = _getPtPostMaxDelta(
					lineData,
					dataSize,
					i,
					templatePara_RH.H_len,
					&maxDeltaY_post,
					&maxDeltaZ_post,
					&endingIdx_post);

				if (true == isJump_pre)
					maxDeltaY_pre = 0;  //实际可能会很大
				if (true == isJump_post)
					maxDeltaY_post = 0;  //实际可能会很大
				if ((maxDeltaY_pre < templatePara_RH.maxDelta) && (maxDeltaZ_post < templatePara_RH.maxDelta))
				{
					if ((true == isJump_pre) || (true == isJump_post))
						jumpFlags[i] = 1;
					types[i] = LINE_FEATURE_RIGHT_ANGLE_RH;
				}
			}
		}
	}
	//对连续的同类型直角进行处理
	int i = 0;
	int pre_type = -1;
	while(i < dataSize)
	{
		int sIdx = i;
		int eIdx = i;
		if (types[i] != pre_type)
		{
			int sIdx = i;
			int eIdx = i;
			int jumpPos = jumpFlags[i] == 1 ? i : -1;  //跳变位置
			for (int j = i; j < dataSize; j++)
			{
				if (types[j] != types[i])
					break;
				else
				{
					eIdx = j;
					if (jumpFlags[j] == 1)
						jumpPos = j;
				}
			}
			if (types[i] > 0)
			{
				SSG_basicFeature1D a_rightAngle;
				a_rightAngle.featureType = types[i];
				if (jumpPos >= 0)
				{
					a_rightAngle.jumpPos2D = { lineIdx, jumpPos };
					a_rightAngle.jumpPos = lineData[jumpPos].pt3D;
				}
				else
				{
					int pos = (sIdx + eIdx) / 2;//取中点
					if (lineData[pos].pt3D.z < 1e-4)
					{
						int w = pos - sIdx;
						for (int m = 1; m <= w; m++)
						{
							if (lineData[pos + m].pt3D.z > 1e-4)
							{
								pos = pos + m;
								break;
							}
							else if (lineData[pos - m].pt3D.z > 1e-4)
							{
								pos = pos - m;
								break;
							}
						}
					}
					a_rightAngle.jumpPos2D = { lineIdx, pos };
					a_rightAngle.jumpPos = lineData[pos].pt3D;
				}
				line_features->features.push_back(a_rightAngle);
			}
			pre_type = types[i];
			i = eIdx + 1;
		}
		else
			i++;
	}
	return;
}