#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 pntIdx;
	double forwardAngle;  //前向角
	double backwardAngle;  //后向角
	double corner; //拐角
	double forwardDiffZ;
	double backwardDiffZ;
}SSG_pntDirAngle;

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 = 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;
}

//滤除离群点：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 = 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 = 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 = 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 = 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 = 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;
	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);
	}

	//计算前向角和后向角
	std::vector< SSG_pntDirAngle> corners;
	corners.resize(vldPts.size());
	for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
	{
		//前向寻找
		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 = 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 = 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 = 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 < cornerPara.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;
		}
	}
	for (int i = 0, i_max = 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>
/// 提取激光线上的极值点（极大值点和极小值点）
/// 
/// </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 = 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 = 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;
}

/// <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>& line_features)
{
	if (lineIdx == 54)
		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 = 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=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 = segMax.size(); m < m_max; m++)
		{
			segMax[m].jumpPos2D.y += idx_1;
			localMax.push_back(segMax[m]);
		}
		for (int m = 0, m_max = 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 = 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 = 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 = 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 = 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进行配对
	for (int i = 0, i_max = 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.midY = lineData[a_feature.midPtIdx].pt3D.y;
					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 = preIdx; m <= postIdx; 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.midY = lineData[a_feature.midPtIdx].pt3D.y;
					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 = preIdx; m <= postIdx; m++)
						{
							if (0 == endingFlag[m])
								endingFlag[m] = 1;
						}
					}
				}
			}
		}
	}

	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;
}