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 pntIdx;
	double forwardAngle;  //ǰ<><C7B0><EFBFBD><EFBFBD>
	double backwardAngle;  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double corner; //<2F>ս<EFBFBD>
	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()); // Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD>㹻<EFBFBD>Ŀռ<C4BF>
	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;
}

//<2F>˳<EFBFBD><CBB3><EFBFBD>Ⱥ<EFBFBD>㣺z<E3A3BA><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD>
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)  //<2F><><EFBFBD><EFBFBD>
		{
			for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
			{
				filerData[j].pt3D.z = 0;
				noisePts.push_back(j);//<2F><>¼<EFBFBD><C2BC><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			}
		}
	}
	return;
}

//<2F>˳<EFBFBD><CBB3><EFBFBD>Ⱥ<EFBFBD>㣺z<E3A3BA><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD>
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)  //<2F><><EFBFBD><EFBFBD>
		{
			for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
			{
				lineData[j].pt3D.z = 0;
			}
		}
	}
	return;
}

//<2F>˳<EFBFBD><CBB3><EFBFBD>Ⱥ<EFBFBD>㣺<EFBFBD><E3A3BA><EFBFBD><EFBFBD><EFBFBD>뷽<EFBFBD><EBB7BD>
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)  //<2F><><EFBFBD><EFBFBD>
		{
			for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
			{
				filerData[j].pt3D.z = 0;
				noisePts.push_back(j);//<2F><>¼<EFBFBD><C2BC><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			}
		}
	}
	return;
}

/// <summary>
/// <20><>ȡɨ<C8A1><C9A8><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>б<EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD>¼б<C2BC><D0B1><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>䡣
/// ͬʱ<CDAC><CAB1>¼Ending<6E><67><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD>ӣ<EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD><EFBFBD>Ҵ<EFBFBD><D2B4><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ending<6E><67>Ending֮<67><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ӿ<EFBFBD><D3BF>ȵ<EFBFBD>1/4
/// </summary>
/// <param name="lineData">ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD>դ<EFBFBD><D5A4><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ</param>
/// <param name="dataSize">ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD></param>
/// <param name="validSlopeH"><3E><>Чб<D0A7>³<EFBFBD><C2B3>ȡ<EFBFBD>С<EFBFBD>ڴ˳<DAB4><CBB3>ȵ<EFBFBD>б<EFBFBD>±<EFBFBD><C2B1><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD></param>
/// <param name="slopes"><3E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⵽<EFBFBD><E2B5BD>б<EFBFBD><D0B1></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)  //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
			{
				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)  //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
		{
			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:  //<2F><>̬
			if (z_diff > 0) //<2F>½<EFBFBD>
			{
				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) //<2F><><EFBFBD><EFBFBD>
			{
				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: //<2F><><EFBFBD><EFBFBD>
			if (z_diff > 0)   //<2F>½<EFBFBD>
			{
				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: //<2F>½<EFBFBD>
			if (z_diff < 0)   //  <20><><EFBFBD><EFBFBD>
			{
				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;
	}
	//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
	bool chkLast = true;
	if (slopes.size() > 0)
	{
		if (slopes.back().startPtIdx == a_slope.startPtIdx)
			chkLast = false;
	}
	if (true == chkLast)
	{
		if (_state == 1)//<2F><><EFBFBD><EFBFBD>
		{
			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)//<2F>½<EFBFBD>
		{
			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;
	//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD>
	SVzNL3DPosition valleyPt;
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Slope<70>Ƿ<EFBFBD><C7B7><EFBFBD>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>
	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<70><65>EndPt<50><74><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD>
		{
			valleyPt = down_slope->endPt;//down_slope<70><65>EndPt<50><74><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD>
			double chkZ_diff = abs(up_slope->startPt.pt3D.z - valleyPt.pt3D.z);
			//<2F><>dwon_slope<70><65>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD>up_slope<70><65>startPt<50><74>z<EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD>ĵ<EFBFBD>
			SVzNL3DPosition counterPt = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, up_slope->startPt.pt3D.z);
			if (chkZ_diff < valleyPara.valleyMinH) //<2F><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>valleyMinH<6E>Ŀ<EFBFBD><C4BF><EFBFBD>
			{
				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<70><65><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD>valleyMinH<6E><48> <20><><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>valleyMinH<6E>Ŀ<EFBFBD><C4BF><EFBFBD>
			{
				//<2F><><EFBFBD><EFBFBD>Valley<65>Ŀ<EFBFBD><C4BF>߱<EFBFBD>
				//<2F><>up_slope<70><65>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD>down_slope<70><65>endPt<50><74>z<EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD>ĵ<EFBFBD>
				double valleyW = abs(counterPt.pt3D.y - up_slope->startPt.pt3D.z);
				if (valleyW < valleyPara.valleyMaxW)
					isValidValley = true;
			}
		}
		else //up_slope<70><65>startPt<50><74><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD>
		{
			valleyPt = up_slope->startPt;//up_slope<70><65>startPt<50><74><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD>
			double chkZ_diff = abs(down_slope->endPt.pt3D.z - valleyPt.pt3D.z);
			//<2F><>up_slope<70><65>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD>down_slope<70><65>endPt<50><74>z<EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD>ĵ<EFBFBD>
			SVzNL3DPosition counterPt = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, down_slope->endPt.pt3D.z);
			if (chkZ_diff < valleyPara.valleyMinH) //<2F><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>valleyMinH<6E>Ŀ<EFBFBD><C4BF><EFBFBD>
			{
				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<70><65><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD>valleyMinH<6E><48> <20><><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>valleyMinH<6E>Ŀ<EFBFBD><C4BF><EFBFBD>
			{
				//<2F><><EFBFBD><EFBFBD>Valley<65>Ŀ<EFBFBD><C4BF>߱<EFBFBD>
				//<2F><>up_slope<70><65>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD>down_slope<70><65>endPt<50><74>z<EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD>ĵ<EFBFBD>
				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)
{
	//<2F><>maxJumPos<6F><73><EFBFBD><EFBFBD><EFBFBD>˸<EFBFBD><CBB8><EFBFBD><EFBFBD><EFBFBD>1cm<63><6D><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>
	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
	//<2F>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD>ڼ<EFBFBD><DABC><EFBFBD><EFBFBD>¶<EFBFBD><C2B6><EFBFBD><EFBFBD>ų<EFBFBD><C5B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD>㱾<EFBFBD><E3B1BE>z<EFBFBD><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>
	if (z0 < z1) //<2F>½<EFBFBD><C2BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>L<EFBFBD><4C>б<EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>endPt<50>ˣ<EFBFBD><CBA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>ƽ̹<C6BD>Σ<EFBFBD><CEA3><EFBFBD><EFBFBD>˴<EFBFBD><CBB4><EFBFBD>Ҫ<EFBFBD><D2AA>startPt<50><74>ǰ<EFBFBD>ҡ<EFBFBD>
	{
		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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>L<EFBFBD><4C>б<EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>startPt<50>ˣ<EFBFBD><CBA3><EFBFBD><EFBFBD><EFBFBD>ǰӦ<C7B0><D3A6><EFBFBD><EFBFBD>ƽ̹<C6BD>Σ<EFBFBD><CEA3><EFBFBD><EFBFBD>˴<EFBFBD><CBB4><EFBFBD>Ҫ<EFBFBD><D2AA>endPt<50><74><EFBFBD><EFBFBD><EFBFBD>ҡ<EFBFBD>
	{
		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>
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>L<EFBFBD><4C><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>V<EFBFBD><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// nPointIdx<64><78><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD><C2B6><EFBFBD><EFBFBD><EFBFBD>Feature<72><65><EFBFBD><EFBFBD>
/// <20>㷨<EFBFBD><E3B7A8><EFBFBD>̣<EFBFBD>
/// <20><>1<EFBFBD><31><EFBFBD><EFBFBD>ȡɨ<C8A1><C9A8><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>Slope(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸߶ȵ<DFB6>slopeΪ<65><CEAA>Чslope<70><65>,<2C>ֲ<EFBFBD><D6B2><EFBFBD>Сֵ<D0A1><D6B5><EFBFBD>Լ<EFBFBD><D4BC>ֲ<EFBFBD>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
/// <20><>2<EFBFBD><32>ȡ<EFBFBD><C8A1><EFBFBD>ڿ<EFBFBD><DABF><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>СֵΪ<D6B5><CEAA>Ч<EFBFBD><D0A7>Сֵ
/// <20><>3<EFBFBD><33>V:<3A><><EFBFBD><EFBFBD>Сֵ<D0A1><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Win<69><6E><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ڼ<EFBFBD><DABC><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>жԳƵ<D4B3>Slope<70><65>
/// <20><>4<EFBFBD><34>L:<3A><>Slope<70>м<EFBFBD><D0BC><EFBFBD>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ㣬<C4B5>жϽǶȡ<C7B6>
/// 
/// </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;
	//<2F><>ȡɨ<C8A1><C9A8><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>Slope(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸߶ȵ<DFB6>slopeΪ<65><CEAA>Чslope<70><65>
	std::vector<SVzNLRange> endings;  //22024.12.16: ע<>⣺<EFBFBD><E2A3BA>û<EFBFBD><C3BB><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD>ӣ<EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD><EFBFBD>Ҵ<EFBFBD><D2B4><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ending<6E><67>
	_getSlopes(lineData, dataSize, slopeParam.validSlopeH, slopeParam.minEndingGap, slopes, endings);
	
	//<2F><><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>L<EFBFBD>ͱ߽磺<DFBD><E7A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㹻<EFBFBD>¶ȣ<C2B6><C8A3><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>γ<EFBFBD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶Ȳ<C2B6>
	//<2F>ټ<EFBFBD><D9BC><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γ<EFBFBD>V<EFBFBD><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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)  //<2F>½<EFBFBD>
			{
				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)
		{
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>
			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) //<2F><>Ӧ63<36>ȣ<EFBFBD><C8A3>ڴ˽Ƕ<CBBD><C7B6>£<EFBFBD><C2A3><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>1mmʱ<6D><CAB1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᵼ<EFBFBD><E1B5BC>slope<70>ķ<EFBFBD>ת
			{
				if (curr_slopeH > 0)  //<2F>½<EFBFBD>
				{
					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 };
					}
				}
			}
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		int nxt_self_type = 0;
		int complex_type = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			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))  //<2F>½<EFBFBD>slope
			{
				//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				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)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>slope<70><65>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)  //<2F>½<EFBFBD>
					{
						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)
				{
					//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>
					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) //<2F><>Ӧ63<36>ȣ<EFBFBD><C8A3>ڴ˽Ƕ<CBBD><C7B6>£<EFBFBD><C2A3><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>1mmʱ<6D><CAB1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᵼ<EFBFBD><E1B5BC>slope<70>ķ<EFBFBD>ת
					{
						if (nxt_slopeH > 0)  //<2F>½<EFBFBD>
						{
							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 };
							}
						}
					}
				}

			}
		}

		//<2F><><EFBFBD><EFBFBD>ͬʱ<CDAC><CAB1><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>
		if (complex_type == 0)
		{
			if (curr_self_type > 0) 
			{
				line_features->features.push_back(self_slope);
				//<2F><><EFBFBD><EFBFBD>
				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);
				//<2F><><EFBFBD><EFBFBD>
				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];

			//<2F><><EFBFBD><EFBFBD>
			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)  //ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
				{
					line_features->features.push_back(self_slope);
					line_features->features.push_back(a_valley);
				}
				else
				{
					//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5>߲<DFB2>߲<EFBFBD><DFB2><EFBFBD><EFBFBD><EFBFBD>
					double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
					if (abs(highest_diff) < slope_jump_th) //С<><D0A1>40mm(<28><><EFBFBD>Ӹ߶ȵ<DFB6>1/4)<29><><EFBFBD><EFBFBD>Ϊvalley; <20><>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>ΪSlope
					{
						if ((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))//JUMP<4D><50><EFBFBD><EFBFBD>
							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)  //ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
				{
					line_features->features.push_back(a_valley);
					line_features->features.push_back(nxt_self_slope);
				}
				else  // ((curr_self_type > 0) && (nxt_self_type > 0))
				{
					//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5>߲<DFB2>߲<EFBFBD><DFB2><EFBFBD><EFBFBD><EFBFBD>
					double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
					if (abs(highest_diff) < slope_jump_th) //С<><D0A1>40mm(<28><><EFBFBD>Ӹ߶ȵ<DFB6>1/4)<29><><EFBFBD><EFBFBD>Ϊvalley; <20><>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>ΪSlope
					{
						if ((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type))  //JUMP<4D><50><EFBFBD><EFBFBD>
							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)) //ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
				{
					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<4D><50><EFBFBD><EFBFBD>
						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<4D><50><EFBFBD><EFBFBD>
						line_features->features.push_back(self_slope);
					else
						line_features->features.push_back(a_valley);
					line_features->features.push_back(nxt_self_slope);
				}
				else  //<2F>ϲ<EFBFBD>Ϊһ<CEAA><D2BB>
				{
					//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5>߲<DFB2>߲<EFBFBD><DFB2><EFBFBD><EFBFBD><EFBFBD>
					double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
					if (abs(highest_diff) < slope_jump_th) //С<><D0A1>40mm(<28><><EFBFBD>Ӹ߶ȵ<DFB6>1/4)<29><><EFBFBD><EFBFBD>Ϊvalley; <20><>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>Ϊ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);
							}
						}
					}
				}
			}
		}
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>JUMP<4D>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ܺϲ<DCBA>
	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)))
			{
				//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
				double dist = abs(curr_feature->jumpPos.y - nxt_feature->jumpPos.y);
				if (dist < 20) //<2F><><EFBFBD><EFBFBD>С<EFBFBD><D0A1>20mm<6D><6D><EFBFBD>ϲ<EFBFBD>
				{
					if (LINE_FEATURE_L_JUMP_L2H)
						*curr_feature = *nxt_feature;
					line_features->features.erase(line_features->features.begin() + i + 1);
				}
			}
		}
		i--;
	}

	//<2F><><EFBFBD>ӿ<EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>߽<EFBFBD>
	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); //ͷ<><CDB7>
		//β<><CEB2>
		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>
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵĹյ<C4B9><D5B5><EFBFBD><EFBFBD><EFBFBD>
/// nPointIdx<64><78><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD><C2B6><EFBFBD><EFBFBD><EFBFBD>Feature<72><65><EFBFBD><EFBFBD>
/// <20>㷨<EFBFBD><E3B7A8><EFBFBD>̣<EFBFBD>
/// <20><>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>Ǻͺ<C7BA><CDBA><EFBFBD><EFBFBD><EFBFBD>
/// <20><>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>սǣ<D5BD>˳ʱ<CBB3><CAB1>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>Ϊ<EFBFBD><CEAA>
/// <20><>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>սǵļ<C7B5><C4BC><EFBFBD>ֵ<EFBFBD><D6B5>
/// <20><>4<EFBFBD><34><EFBFBD>жϹս<CFB9><D5BD>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
/// </summary>
void sg_getLineCornerFeature(
	SVzNL3DPosition* lineData, 
	int dataSize, 
	int lineIdx, 
	const SSG_cornerParam cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
	SSG_lineFeature* line_features)
{
	line_features->lineIdx = lineIdx;
	if (lineIdx == 538)
		int kkk = 1;

	//ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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);
	}

	//<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>Ǻͺ<C7BA><CDBA><EFBFBD><EFBFBD><EFBFBD>
	std::vector< SSG_pntDirAngle> corners;
	corners.resize(vldPts.size());
	for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
	{
		//ǰ<><C7B0>Ѱ<EFBFBD><D1B0>
		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;
			}
		}
		//<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
		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;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD>ս<EFBFBD>
		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);  //ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵy<CFB5><79><EFBFBD><EFBFBD><EFBFBD>෴<EFBFBD><E0B7B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>-<2D><><EFBFBD><EFBFBD>
			corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
			corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
		}
	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD>սǼ<D5BD>ֵ
	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)  //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
			{
				if (1 == _state)  //<2F><><EFBFBD><EFBFBD>
				{
					cornerPeakP.push_back(corners[eEdgePtIdx]);
				}
				else if (2 == _state) //<2F>½<EFBFBD>
				{
					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:  //<2F><>̬
			if (cornerDiff < 0) //<2F>½<EFBFBD>
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //<2F><><EFBFBD><EFBFBD>
			{
				_state = 1;
			}
			break;
		case 1: //<2F><><EFBFBD><EFBFBD>
			if (cornerDiff < 0)   //<2F>½<EFBFBD>
			{
				cornerPeakP.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //<2F>½<EFBFBD>
			if (cornerDiff > 0)   //  <20><><EFBFBD><EFBFBD>
			{
				cornerPeakM.push_back(*pre_data);
				_state = 1;
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//ע<>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>

	//<2F><>Сֵ<D0A1>㣨<EFBFBD>嶥<EFBFBD><E5B6A5>
	//<2F><>ֵ<EFBFBD>Ƚϣ<C8BD><CFA3>ڳ߶ȴ<DFB6><C8B4><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD>Ҿֲ<D2BE><D6B2><EFBFBD>ֵ<EFBFBD><D6B5>
	double square_distTh = 4 * cornerPara.scale * cornerPara.scale;  //2<><32><EFBFBD><EFBFBD>cornerScale<6C><65>
	for (int i = 0, i_max = cornerPeakP.size(); i < i_max; i++)
	{
		if (cornerPeakP[i].corner < cornerPara.cornerTh)
			continue;

		bool isPeak = true;
		//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
				break;

			if (cornerPeakP[i].corner < cornerPeakP[j].corner)
			{
				isPeak = false;
				break;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
					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);
		}
	}

	//<2F><><EFBFBD>ӿ<EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>߽<EFBFBD>
	//<2F><><EFBFBD><EFBFBD>seg<65>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫ<EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϲ<EFBFBD>
	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)  //<2F>ϲ<EFBFBD>
		{
			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)  //<2F><><EFBFBD>ϲ<EFBFBD>
			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); //ͷ<><CDB7>
		//β<><CEB2>
		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>
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵļ<CFB5>ֵ<EFBFBD>㣨<EFBFBD><E3A3A8><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ͼ<EFBFBD>Сֵ<D0A1>㣩
/// 
/// </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)  //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
			{
				if ( 1 == _state )  //<2F><><EFBFBD><EFBFBD>
				{
					pkTop.push_back(lineData[eEdgePtIdx]);
				}
				else if (2 == _state) //<2F>½<EFBFBD>
				{
					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:  //<2F><>̬
			if (z_diff > 0) //<2F>½<EFBFBD>
			{
				_state = 2;
			}
			else if (z_diff < 0) //<2F><><EFBFBD><EFBFBD>
			{
				_state = 1;
			}
			break;
		case 1: //<2F><><EFBFBD><EFBFBD>
			if (z_diff > 0)   //<2F>½<EFBFBD>
			{
				pkTop.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //<2F>½<EFBFBD>
			if (z_diff < 0)   //  <20><><EFBFBD><EFBFBD>
			{
				pkBtm.push_back(*pre_data);
				_state = 1;
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//ע<>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>

	//<2F><>Сֵ<D0A1>㣨<EFBFBD>嶥<EFBFBD><E5B6A5>
	//<2F><>ֵ<EFBFBD>Ƚϣ<C8BD><CFA3>ڳ߶ȴ<DFB6><C8B4><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD>Ҿֲ<D2BE><D6B2><EFBFBD>ֵ<EFBFBD><D6B5>
	double square_distTh = scaleWin * scaleWin;
	for (int i = 0, i_max = pkTop.size(); i < i_max; i++)
	{
		bool isPeak = true;
		//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
				break;

			if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
			{
				isPeak = false;
				break;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
					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);
		}
	}
	//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD>㣨<EFBFBD>ȵף<C8B5>
	for (int i = 0, i_max = pkBtm.size(); i < i_max; i++)
	{
		bool isPeak = true;
		//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
				break;

			if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
			{
				isPeak = false;
				break;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
					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>
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣨Zֵ<5A><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>䣩
/// 
/// </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>
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// nPointIdx<64><78><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD><C2B6><EFBFBD><EFBFBD><EFBFBD>Feature<72><65><EFBFBD><EFBFBD>
/// <20>㷨<EFBFBD><E3B7A8><EFBFBD>̣<EFBFBD>
/// <20><>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>Ǻͺ<C7BA><CDBA><EFBFBD><EFBFBD><EFBFBD>
/// <20><>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>սǣ<D5BD>˳ʱ<CBB3><CAB1>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>Ϊ<EFBFBD><CEAA>
/// <20><>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>սǵļ<C7B5><C4BC><EFBFBD>ֵ<EFBFBD><D6B5>
/// <20><>4<EFBFBD><34><EFBFBD>жϹս<CFB9><D5BD>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
/// </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) //<2F><><EFBFBD><EFBFBD>holeMask<73><6B><EFBFBD>кϲ<D0BA>
{
	if (lineIdx == 194)
		int kkk = 1;
	//ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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);
	}
	//<2F><><EFBFBD><EFBFBD>seg<65>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫ<EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϲ<EFBFBD>
	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)  //<2F>ϲ<EFBFBD>
		{
			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;
		}
	}

	//<2F><>ȡ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)  //<2F><><EFBFBD>ϲ<EFBFBD>
			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]);
		}
	}

	//<2F><><EFBFBD><EFBFBD>slope<70><65>Z<EFBFBD>仯
	std::vector< SSG_zWin> slopes; //<2F><>¼<EFBFBD><C2BC><EFBFBD>ڳ<EFBFBD><DAB3><EFBFBD><EFBFBD>ڵ<EFBFBD>z<EFBFBD>仯
	slopes.resize(vldPts.size());
	for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
	{
		//<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
		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;
			}
		}
		//<2F><><EFBFBD><EFBFBD>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;
		}
	}

	//<2F><><EFBFBD><EFBFBD>zWin<69><6E>ֵ
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_zWin* pre_data = NULL;
	std::vector< SSG_zWin> zWinPeakP;  //<2F><><EFBFBD><EFBFBD>zWin
	std::vector< SSG_zWin> zWinPeakM;  //<2F><><EFBFBD><EFBFBD>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))  //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
			{
				if (1 == _state)  //<2F><><EFBFBD><EFBFBD>
				{
					if (pre_data->zDiff > 0)
						zWinPeakP.push_back(slopes[eEdgePtIdx]);
				}
				else if (2 == _state) //<2F>½<EFBFBD>
				{
					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:  //<2F><>̬
			if (zDiffDelta < 0) //<2F>½<EFBFBD>
			{
				if (curr_data->zDiff < 0)
				{
					maxPkMinus = curr_data;
					_state = 2;
				}
			}
			else if (zDiffDelta > 0) //<2F><><EFBFBD><EFBFBD>
			{
				if (curr_data->zDiff > 0)
				{
					maxPkPlus = curr_data;
					_state = 1;
				}
			}
			break;
		case 1: //<2F><><EFBFBD><EFBFBD>
			if (zDiffDelta < 0)   //<2F>½<EFBFBD>
			{
				if(maxPkPlus)
					zWinPeakP.push_back(*maxPkPlus);
				maxPkPlus = NULL;
				if (curr_data->zDiff < 0)
					maxPkMinus = curr_data;
				else
					maxPkMinus = NULL;
				_state = 2;
			}
			else
			{
				//<2F><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>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: //<2F>½<EFBFBD>
			if (zDiffDelta > 0)   //  <20><><EFBFBD><EFBFBD>
			{
				if (maxPkMinus)
					zWinPeakM.push_back(*maxPkMinus);
				maxPkMinus = NULL;
				if (curr_data->zDiff > 0)
					maxPkPlus = curr_data;
				else
					maxPkPlus = NULL;
				_state = 1;
			}
			else
			{
				//<2F><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>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;
	}

	//<2F><>ֵ<EFBFBD>Ƚϣ<C8BD><CFA3>ڳ߶ȴ<DFB6><C8B4><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD>Ҿֲ<D2BE><D6B2><EFBFBD>ֵ<EFBFBD><D6B5>
	std::vector<SSG_zWin> validZWinPeakP;
	double square_distTh = 4 * slopePara.LSlopeZWin * slopePara.LSlopeZWin;  //2<><32><EFBFBD><EFBFBD>cornerScale<6C><65>
	for (int i = 0, i_max = zWinPeakP.size(); i < i_max; i++)
	{
		if (zWinPeakP[i].zDiff < slopePara.validSlopeH)
			continue;

		bool isPeak = true;
		//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
				break;

			if (zWinPeakP[i].zDiff < zWinPeakP[j].zDiff)
			{
				isPeak = false;
				break;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
					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;
		}
	}

	//<2F><>ֵ<EFBFBD>Ƚϣ<C8BD><CFA3>ڳ߶ȴ<DFB6><C8B4><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD>Ҿֲ<D2BE><D6B2><EFBFBD>ֵ<EFBFBD><D6B5>
	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;
		//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
				break;

			if (zWinPeakM[i].zDiff > zWinPeakM[j].zDiff)
			{
				isPeak = false;
				break;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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)  //<2F><><EFBFBD><EFBFBD><EFBFBD>߶ȴ<DFB6><C8B4><EFBFBD>
					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;
		}
	}

	//<2F><>validCornerPeakP<6B><50><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector< SSG_featureSemiCircle> line_features;
	for (int i = 0, i_max = localMin.size(); i < i_max; i++)
	{
		int ptIdx = localMin[i].jumpPos2D.y;
		if (endingFlag[ptIdx] > 0)  //<2F>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			continue;

		//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		int preIdx = -1;
		for (int j = ptIdx - 1; j >= 0; j--)
		{
			if (endingFlag[j] > 0)
			{
				preIdx = j;
				break;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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))  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			{
				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))  || //<2F>еװ<D0B5><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				((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;
						}
					}
				}
			}
		}
	}

	//<2F><><EFBFBD>ݿ׶<DDBF>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>кϲ<D0BA>
	for (int i = 0, i_max = 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))
			{
				//<2F>ϲ<EFBFBD>
				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;
}



//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>鳤<EFBFBD><E9B3A4>ʱʹ<CAB1>ý<EFBFBD><C3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ϳ<EFBFBD><CDBF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>z<EFBFBD><7A><EFBFBD><EFBFBD>ΪJump<6D><70><EFBFBD><EFBFBD>True<75><65>Jump<6D><70><EFBFBD><EFBFBD>Seedǰһ<C7B0><D2BB><EFBFBD><EFBFBD>z<EFBFBD>ı仯<C4B1><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;  //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>0
	*maxDeltaZ = 0;
	*endingIdx = -1;//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чֵ<D0A7><D6B5><EFBFBD><EFBFBD>ʾû<CABE><C3BB><EFBFBD>㹻<EFBFBD><E3B9BB><EFBFBD><EFBFBD>
	if (seedIdx == 0)  //<2F>޷<EFBFBD><DEB7><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		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;//û<><C3BB><EFBFBD>㹻<EFBFBD><E3B9BB><EFBFBD><EFBFBD>
	return false;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>鳤<EFBFBD><E9B3A4>ʱʹ<CAB1>ý<EFBFBD><C3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ϳ<EFBFBD><CDBF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>z<EFBFBD><7A><EFBFBD><EFBFBD>ΪJump<6D><70><EFBFBD><EFBFBD>True<75><65>Jump<6D><70><EFBFBD><EFBFBD>Seedǰһ<C7B0><D2BB><EFBFBD><EFBFBD>z<EFBFBD>ı仯<C4B1><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;  //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>0
	*maxDeltaZ = 0;
	*endingIdx = -1;//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чֵ<D0A7><D6B5><EFBFBD><EFBFBD>ʾû<CABE><C3BB><EFBFBD>㹻<EFBFBD><E3B9BB><EFBFBD><EFBFBD>
	if (seedIdx == dataSize-1)  //<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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;//<2F><>ʾû<CABE><C3BB><EFBFBD>㹻<EFBFBD><E3B9BB><EFBFBD><EFBFBD>
	return false;
}

/// <summary>
/// ʹ<><CAB9>ģ<EFBFBD>巨<EFBFBD><E5B7A8>ȡֱ<C8A1><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD>deltaZ<61><5A>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>С<EFBFBD>ķ<EFBFBD>Χ<EFBFBD>ڣ<EFBFBD><DAA3>յ<EFBFBD><D5B5>Ҳ<EFBFBD>deltaY<61><59>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>С<EFBFBD>ķ<EFBFBD>Χ<EFBFBD><CEA7>
/// </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;

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>ļ<EFBFBD><C4BC>鳤<EFBFBD>Ƚ<EFBFBD><C8BD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>ж<EFBFBD>
		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);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ǣ<EFBFBD>ˮƽ-<2D><><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD>-ˮƽ<CBAE><C6BD>ˮƽ-<2D><><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD>-ˮƽ
		if ((endingIdx_pre < 0) || (endingIdx_post < 0))  //û<><C3BB><EFBFBD>㹻<EFBFBD><E3B9BB><EFBFBD><EFBFBD>
			continue;

		if (true == isJump_pre)
			maxDeltaY_pre = 0;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
		if (true == isJump_post)
			maxDeltaY_post = 0;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
		
		if ((maxDeltaZ_pre < maxDelta) && (maxDeltaY_post < maxDelta))  //<2F><><EFBFBD><EFBFBD>ΪHX<48>ͣ<EFBFBD>ˮƽ-<2D><>ֱ<EFBFBD><D6B1>
		{
			if (lineData[endingIdx_post].pt3D.z > lineData[i].pt3D.z)  //<2F>½<EFBFBD>
			{
				//ʹ<><CAB9>templatePara_HX<48>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				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;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				if (true == isJump_post)
					maxDeltaY_post = 0;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				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  //<2F><><EFBFBD><EFBFBD>
			{
				//ʹ<><CAB9>templatePara_HX<48>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				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;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				if (true == isJump_post)
					maxDeltaY_post = 0;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				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))  //<2F><><EFBFBD><EFBFBD>ΪXH<58>ͣ<EFBFBD><CDA3><EFBFBD>ֱ-ˮƽ<CBAE><C6BD>
		{
			if (lineData[i].pt3D.z > lineData[endingIdx_pre].pt3D.z)  //<2F>½<EFBFBD>
			{
				//ʹ<><CAB9>templatePara_XH<58>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				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;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				if (true == isJump_post)
					maxDeltaY_post = 0;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				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//<2F><><EFBFBD><EFBFBD>
			{
				//ʹ<><CAB9>templatePara_XH<58>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				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;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				if (true == isJump_post)
					maxDeltaY_post = 0;  //ʵ<>ʿ<EFBFBD><CABF>ܻ<EFBFBD><DCBB>ܴ<EFBFBD>
				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;
				}
			}
		}
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD>ǽ<EFBFBD><C7BD>д<EFBFBD><D0B4><EFBFBD>
	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;  //<2F><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
			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;//ȡ<>е<EFBFBD>
					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;
}