algoLib/sourceCode/wheelArchHeigthMeasure.cpp

#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "wheelArchHeigthMeasure_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>

//version 1.0.0 : base version release to customer
//version 1.1.0 : <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><C3BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD> 
//version 1.2.0 : <20><><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת180<38><30> 
//version 1.3.0 : <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>ʹ<EFBFBD>ù̶<C3B9><CCB6><EFBFBD><EFBFBD>޵<EFBFBD><DEB5><EFBFBD><EFBFBD><EFBFBD>
//version 1.3.1 : <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ڵĺ<DAB5><C4BA><EFBFBD> 
//version 1.3.2 : <20><><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƸĽ<C6B8><C4BD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>ǿ³<C7BF><C2B3><EFBFBD><EFBFBD> 
//version 1.3.3 : <20><>ü<EFBFBD><C3BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>˸Ľ<CBB8><C4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿<EFBFBD><CBBF>ܵ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//version 1.3.4 : <20><>ü<EFBFBD><C3BC><EFBFBD><EFBFBD><EFBFBD>ĸ߶ȼ<DFB6><C8BC>㷽<EFBFBD><E3B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD>߶Ȳ<DFB6><C8B2>޸<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::string	m_strVersion = "1.3.4";
const char* wd_wheelArchHeigthMeasureVersion(void)
{
	return m_strVersion.c_str();
}

//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>е<EFBFBD><D0B5><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ҫ<EFBFBD>Ե<EFBFBD><D4B5><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>գ<EFBFBD><D5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara wd_horizonCamera_getGroundCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_HCameraVScan_getGroundCalibPara(scanLines);
}

//<2F><><EFBFBD><EFBFBD>ˮƽʱ<C6BD><CAB1>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_horizonCamera_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	HCamera_lineDataRT_vector(a_line, camPoseR, groundH);
}

SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
{
	SVzNL3DPoint _r_pt;
	_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
	_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
	_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
	return _r_pt;
}

bool compareByPtSize(const SWD_clustersInfo& a, const SWD_clustersInfo& b) {
	return a.ptSize > b.ptSize;
}

//<2F><>ȡ<EFBFBD><C8A1>ü<EFBFBD><C3BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¶˵<C2B6>
void _getArcEndings(std::vector< std::vector<SVzNL3DPosition>>& scanLines, const int cluster_arc_id, std::vector<SVzNL2DPoint>& contourPts, double maxZ)
{
	int lineNum = (int)scanLines.size();
	
	for (int i = 0; i < lineNum; i++)
	{
		int ptNum = (int)scanLines[i].size();
		int lastIdx = -1;
		for (int j = 0; j < ptNum; j++)
		{
			if ((i == 380) && (j > 288))
				int kkk = 1;
			if ( (scanLines[i][j].nPointIdx == cluster_arc_id) && (scanLines[i][j].pt3D.z >1e-4) && (scanLines[i][j].pt3D.z < maxZ))
				lastIdx = j;
		}
		if (lastIdx >= 0)
		{
			SVzNL2DPoint a_pt = { i, lastIdx};
			contourPts.push_back(a_pt);
		}
	}
}

//<2F><>ȡ<EFBFBD><C8A1>ü<EFBFBD><C3BC><EFBFBD>ӵ㣬<D3B5><E3A3AC><EFBFBD><EFBFBD>Ϊy<CEAA><79>С<EFBFBD>Ķ˵<C4B6>
int _getArcEndingsCenterPos(std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector<SVzNL2DPoint>& contourPts)
{
	double minY = DBL_MAX;
	int seedPosIdx = 0;
	for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
	{
		SVzNL2DPoint& a_pos = contourPts[i];
		double y = scanLines[a_pos.x][a_pos.y].pt3D.y;
		if ((minY > y) && (scanLines[a_pos.x][a_pos.y].pt3D.z > 1e-4))
		{
			seedPosIdx = i;
			minY = y;
		}
	}
	return seedPosIdx;
}

//ʹ<>ö˵<C3B6>ֱ<EFBFBD>ߣ<EFBFBD><DFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㵽ֱ<E3B5BD>ߵľ<DFB5><C4BE>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޵ķָ<C4B7>
int computeMaxDistPos(
	int ptStartIdx, int ptEndIdx,
	std::vector< SVzNL3DPosition>& lineData)
{
	SVzNL3DPoint pt1 = lineData[ptStartIdx].pt3D;
	SVzNL3DPoint pt2 = lineData[ptEndIdx].pt3D;
	if ((pt1.z < 1e-4) || (pt2.z < 1e-4))
		return -1;
	double _a, _b, _c;
	compute2ptLine_2(
		pt1.y, pt1.z,
		pt2.y, pt2.z,
		&_a, &_b, &_c);
	//compute2ptLine(pt1, pt2, &_a, &_b, &_c);
	double denominator = sqrt(_a * _a + _b * _b);
	//<2F><>һ<EFBFBD><D2BB>
	_a = _a / denominator;
	_b = _b / denominator;
	_c = _c / denominator;

	double maxDist = 0;
	int maxPos = 0;
	for (int i = ptStartIdx; i <= ptEndIdx; i++)
	{
		SVzNL3DPoint a_pt = lineData[i].pt3D;
		if (a_pt.z > 1e-4)
		{
			double dist = abs(a_pt.y * _a + a_pt.z * _b + _c);
			if (maxDist < dist)
			{
				maxDist = dist;
				maxPos = i;
			}
		}
	}
	return maxPos;
}

void _extractArcFittingPts(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines, 
	std::vector<SVzNL2DPoint>& contourPts, 
	double chkWin, 
	int seedPosIdx, 
	std::vector< SVzNL2DPoint>& arcFittingPos)
{
	int size = (int)contourPts.size();
	arcFittingPos.push_back(contourPts[seedPosIdx]);
	SVzNL3DPoint seedPt = scanLines[contourPts[seedPosIdx].x][contourPts[seedPosIdx].y].pt3D;
	for (int i = seedPosIdx - 1; i >= 0; i--)
	{
		SVzNL2DPoint chkPos = contourPts[i];
		SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
		double len = sqrt(pow(chkPt.x - seedPt.x, 2) + pow(chkPt.y - seedPt.y, 2));
		if (len > chkWin)
			break;
		arcFittingPos.insert(arcFittingPos.begin(), chkPos);
	}
	for (int i = seedPosIdx + 1; i < size; i++)
	{
		SVzNL2DPoint chkPos = contourPts[i];
		SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
		double len = sqrt(pow(chkPt.x - seedPt.x, 2) + pow(chkPt.y - seedPt.y, 2));
		if (len > chkWin)
			break;
		arcFittingPos.push_back(chkPos);
	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD>Ķ˵㣬<CBB5><E3A3AC><EFBFBD><EFBFBD>20mm
	double chkLen = 20;
	for (int i = 0, i_max = (int)arcFittingPos.size(); i < i_max; i++)
	{
		SVzNL2DPoint chkPos = arcFittingPos[i];
		SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
		int endIdx = chkPos.y;
		int startIdx = endIdx;
		for (int m = endIdx - 1; m >= 0; m--)
		{
			SVzNL3DPoint a_pt = scanLines[chkPos.x][m].pt3D;
			if (a_pt.z > 1e-4)
			{
				double len = sqrt(pow(a_pt.y - chkPt.y, 2) + pow(a_pt.z - chkPt.z, 2));
				if (len > chkLen)
					break;
				startIdx = m;
			}
		}
		if (startIdx != endIdx)
		{
			int maxPos = computeMaxDistPos(startIdx, endIdx, scanLines[chkPos.x]);
			SVzNL3DPoint max_pt = scanLines[chkPos.x][maxPos].pt3D;
			double len1 = sqrt(pow(max_pt.y - chkPt.y, 2) + pow(max_pt.z - chkPt.z, 2));
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Σ<EFBFBD><CEA3><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD>ҵ㵽<D2B5>ױߵľ<DFB5><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>
			for (int m = maxPos - 1; m >= 0; m--)
			{
				SVzNL3DPoint a_pt = scanLines[chkPos.x][m].pt3D;
				if (a_pt.z > 1e-4)
				{
					double len = sqrt(pow(a_pt.y - max_pt.y, 2) + pow(a_pt.z - max_pt.z, 2));
					if (len > len1)
						break;
					startIdx = m;
				}
			}
			maxPos = computeMaxDistPos(startIdx, endIdx, scanLines[chkPos.x]);
			arcFittingPos[i].y = maxPos;
		}
	}

}

SVzNL3DPoint _getWheelArcFittingPoint(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const int maskID,
	std::vector< SVzNL2DPoint>& fittingPos,
	SVzNL2DPoint& fittingPosition)
{
	//upWheel, <20><>XYƽ<59><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>
	std::vector<cv::Point2d> fittingPoints;
	for (int i = 0, i_max = (int)fittingPos.size(); i < i_max; i++)
	{
		int lineIdx = fittingPos[i].x;
		int ptIdx = fittingPos[i].y;
		cv::Point2d a_pt = { scanLines[lineIdx][ptIdx].pt3D.x,scanLines[lineIdx][ptIdx].pt3D.y };
		scanLines[lineIdx][ptIdx].nPointIdx = maskID;
		fittingPoints.push_back(a_pt);
	}

	double a, b, c, mse, max_err;
	bool fitResult = leastSquareParabolaFitEigen(
		fittingPoints, a, b, c, mse, max_err);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>϶<EFBFBD><CFB6><EFBFBD>
	SVzNL3DPoint fitPt;
	fitPt.x = -b / (2 * a);
	fitPt.y = (4 * a * c - b * b) / (4 * a);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
	SVzNL2DPoint subOptiPtPos = { 0,0 };
	double minLen = DBL_MAX;
	for (int i = 0, i_max = (int)fittingPos.size(); i < i_max; i++)
	{
		int lineIdx = fittingPos[i].x;
		int ptIdx = fittingPos[i].y;
		cv::Point2d a_pt = { scanLines[lineIdx][ptIdx].pt3D.x,scanLines[lineIdx][ptIdx].pt3D.y };
		double len = sqrt(pow(fitPt.x - a_pt.x, 2) + pow(fitPt.y - a_pt.y, 2));
		if (minLen > len)
		{
			subOptiPtPos = fittingPos[i];
			minLen = len;
		}
	}
	fittingPosition = subOptiPtPos;
	fitPt.z = scanLines[subOptiPtPos.x][subOptiPtPos.y].pt3D.z;
	return fitPt;
}


bool wd_wheelPresenseDetection(
	std::vector<std::vector< SVzNL3DPosition>>& scanLines,
	const SVzNL3DRangeD wheelRoi3d)
{
	int lines = (int)scanLines.size();
	int validPtNum = 0;
	for (int line = 0; line < lines; line++)
	{
		std::vector< SVzNL3DPosition>& a_line = scanLines[line];
		int ptNum = (int)a_line.size();
		for (int i = 0; i < ptNum; i++)
		{
			SVzNL3DPoint& a_pt3D = a_line[i].pt3D;
			if ( (a_pt3D.x > wheelRoi3d.xRange.min) && (a_pt3D.x < wheelRoi3d.xRange.max)&&
				(a_pt3D.y > wheelRoi3d.yRange.min) && (a_pt3D.y < wheelRoi3d.yRange.max)&&
				(a_pt3D.z > wheelRoi3d.zRange.min) && (a_pt3D.z < wheelRoi3d.zRange.max) && (a_pt3D.z> 1e-4))
			{
				validPtNum++;
			}
		}
	}
	if (validPtNum > 1000)
		return true;
	else
		return false;

}

void _lineProc_getWheelEnding(
	std::vector< SVzNL3DPosition>& lineData,
	int lineIdx,
	std::vector<SWDIndexingVzPoint>& line_upEnding,
	std::vector<SWDIndexingVzPoint>& line_downEnding,
	double scale_segGap = 10.0,
	double scale_endingAngleLen = 25.0, //<2F>˵㷽<CBB5><E3B7BD><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD>߶<EFBFBD>
	double wheel_endingAngleTh = 30)
{
	int dataSize = (int)lineData.size();
	std::vector<SSG_RUN> segs;
	SSG_RUN a_run = { 0, -1, 0, }; //startIdx, len, lastIdx
	SVzNL3DPosition preData = { -1, {0, 0, 0} };
	for (int i = 0; i < dataSize; i++)
	{
		if ( (lineData[i].pt3D.z > 1e-4) && (lineData[i].nPointIdx == 1)) //wheel
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = i;
			}
			else
			{
				double dist = sqrt(pow(lineData[i].pt3D.y - preData.pt3D.y, 2) + pow(lineData[i].pt3D.z - preData.pt3D.z, 2));
				if (dist < scale_segGap)
				{
					a_run.len = i - a_run.start + 1;
					a_run.value = i;
				}
				else
				{
					segs.push_back(a_run);

					a_run.start = i;
					a_run.len = 1;
					a_run.value = i;
				}
			}
			preData = lineData[i];
		}
	}
	if (a_run.len > 0)
		segs.push_back(a_run);

	//<2F><><EFBFBD>μ<EFBFBD><CEBC><EFBFBD><EFBFBD>˵㷽<CBB5><E3B7BD><EFBFBD><EFBFBD>
	for (int i = 0; i < (int)segs.size(); i++)
	{
		SSG_RUN& a_seg = segs[i];
		int sPtIdx = a_seg.start;
		int ePtIdx = a_seg.value;
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷽<EFBFBD><E3B7BD><EFBFBD><EFBFBD>
		for (int j = sPtIdx; j < ePtIdx; j++)
		{
			if (lineData[j].pt3D.z > 1e-4)
			{
				double dist = sqrt(pow(lineData[j].pt3D.y - lineData[sPtIdx].pt3D.y, 2) +
					pow(lineData[j].pt3D.z - lineData[sPtIdx].pt3D.z, 2));
				if (dist > scale_endingAngleLen)
				{
					double tanValue_post = (lineData[j].pt3D.z - lineData[sPtIdx].pt3D.z) / abs(lineData[j].pt3D.y - lineData[sPtIdx].pt3D.y);
					double forwardAngle = atan(tanValue_post) * 180.0 / PI;
					double corner = -(forwardAngle - 0);
					if (corner > wheel_endingAngleTh)
					{
						SWDIndexingVzPoint a_upEndig;
						a_upEndig.lineIdx = lineIdx;
						a_upEndig.ptIdx = sPtIdx;
						a_upEndig.point = lineData[sPtIdx].pt3D;
						line_upEnding.push_back(a_upEndig);
					}
					break;
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD>յ㷽<D5B5><E3B7BD><EFBFBD><EFBFBD>
		for (int j = ePtIdx; j > sPtIdx; j--)
		{
			if (lineData[j].pt3D.z > 1e-4)
			{
				double dist = sqrt(pow(lineData[ePtIdx].pt3D.y - lineData[j].pt3D.y, 2) +
								pow(lineData[ePtIdx].pt3D.z - lineData[j].pt3D.z, 2));
				if (dist > scale_endingAngleLen)
				{
					double tanValue_pre = (lineData[ePtIdx].pt3D.z - lineData[j].pt3D.z) / abs(lineData[ePtIdx].pt3D.y - lineData[j].pt3D.y);
					double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
					double corner = -(0 - backwardAngle);
					if (corner > wheel_endingAngleTh)
					{
						SWDIndexingVzPoint a_downEndig;
						a_downEndig.lineIdx = lineIdx;
						a_downEndig.ptIdx = ePtIdx;
						a_downEndig.point = lineData[ePtIdx].pt3D;
						line_downEnding.push_back(a_downEndig);
					}
					break;
				}
			}
		}
	}
}

typedef struct
{
	int treeState;
	int treeType;
	int sLineIdx;
	int eLineIdx;
	SSG_ROIRectD roi;
	std::vector< SWDIndexingVzPoint> treeNodes;
}SSG_endingTree;

#define _TREE_STATE_UNDEF	0
#define _TREE_STATE_ALIVE	1
#define _TREE_STATE_DEAD		2
//<2F><>feature<72><65>trees<65><73>Ѱ<EFBFBD>Һ<EFBFBD><D2BA>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>к<EFBFBD><D0BA>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬 <20><><EFBFBD><EFBFBD>false
//û<><C3BB>ʹ<EFBFBD><CAB9>ȫƥ<C8AB>䡣һ<E4A1A3><D2BB>featureһ<65><D2BB><EFBFBD><EFBFBD>ƥ<EFBFBD><C6A5><EFBFBD>ϣ<EFBFBD>ƥ<EFBFBD><C6A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɡ<EFBFBD>û<EFBFBD><C3BB>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƥ<EFBFBD>䡣
bool _endingFeatureGrowing(SWDIndexingVzPoint& a_feature, std::vector<SSG_endingTree>& trees, SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
	{
		SSG_endingTree& a_tree = trees[i];
		if (_TREE_STATE_DEAD == a_tree.treeState)
			continue;
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		SWDIndexingVzPoint last_node = a_tree.treeNodes.back();
		if (last_node.lineIdx == a_feature.lineIdx)  //xΪlineIdx<64><78>ͬһ<CDAC><D2BB>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD>ϵIJ<CFB5><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			continue;

		//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		double y_diff = abs(a_feature.point.y - last_node.point.y);
		double z_diff = abs(a_feature.point.z - last_node.point.z);
		int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
		double x_diff = abs(a_feature.point.x - last_node.point.x);
		if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
			((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
		{
			a_tree.eLineIdx = a_feature.lineIdx;
			a_tree.treeNodes.push_back(a_feature);
			return true;
		}
	}
	return false;
}
void _getWheelEndingGrowingTrees(
	std::vector<std::vector<SWDIndexingVzPoint>>& endings,
	std::vector<SSG_endingTree>& trees,
	SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = (int)endings.size(); i < i_max; i++)
	{
		std::vector<SWDIndexingVzPoint>& line_endings = endings[i];
		if (line_endings.size() > 0)
		{
			for (int j = 0, j_max = (int)line_endings.size(); j < j_max; j++)
			{
				SWDIndexingVzPoint& an_ending = line_endings[j];
				bool isMatched = _endingFeatureGrowing(an_ending, trees, growParam);
				if (false == isMatched)
				{
					//<2F>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
					SSG_endingTree a_newTree;
					a_newTree.treeNodes.push_back(an_ending);
					a_newTree.treeState = _TREE_STATE_ALIVE;
					a_newTree.treeType = 0;
					a_newTree.sLineIdx = an_ending.lineIdx;
					a_newTree.eLineIdx = an_ending.lineIdx;
					trees.push_back(a_newTree);
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>١<EFBFBD>
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>Ϊ1<CEAA><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD>
		int lineIdx = i;
		int m_max = (int)trees.size();
		for (int m = m_max - 1; m >= 0; m--)  //<2F>Ӻ<EFBFBD><D3BA><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɾ<EFBFBD><C9BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>ѭ<EFBFBD><D1AD>
		{
			if (_TREE_STATE_ALIVE == trees[m].treeState)
			{
				int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx);
				if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
					(i == i_max - 1))
				{
					trees[m].treeState = _TREE_STATE_DEAD;
					SWDIndexingVzPoint first_node = trees[m].treeNodes[0];
					SWDIndexingVzPoint last_node = trees[m].treeNodes.back();
					double len = sqrt(pow(first_node.point.x - last_node.point.x, 2) + pow(first_node.point.y - last_node.point.y, 2));
					if (len <= growParam.minVTypeTreeLen)
						trees.erase(trees.begin() + m);				
				}
			}
		}
	}
}

double _getTopY(SSG_endingTree& wheedEdge)
{
	double minY = DBL_MAX;
	for (int i = 0; i < (int)wheedEdge.treeNodes.size(); i++)
		minY = minY > wheedEdge.treeNodes[i].point.y ? wheedEdge.treeNodes[i].point.y : minY;
	return minY;
}

double _getBtmY(SSG_endingTree& wheedEdge)
{
	double maxY = DBL_MIN;
	for (int i = 0; i < (int)wheedEdge.treeNodes.size(); i++)
		maxY = maxY < wheedEdge.treeNodes[i].point.y ? wheedEdge.treeNodes[i].point.y : maxY;
	return maxY;
}

bool _compareByLineIdx(SSG_endingTree& a, SSG_endingTree& b)
{
	return a.sLineIdx < b.sLineIdx;
}

void _genEdgeContinuousPos(SSG_endingTree&edgeTree, std::vector<SVzNL2DPoint>& edgePos)
{
	for (int i = 0; i < (int)edgeTree.treeNodes.size(); i++)
	{
		SVzNL2DPoint a_pos = { edgeTree.treeNodes[i].lineIdx, edgeTree.treeNodes[i].ptIdx };
		if (edgePos.size() == 0)
			edgePos.push_back(a_pos);
		else
		{
			SVzNL2DPoint last_pos = edgePos.back();
			if ((a_pos.x - last_pos.x) == 1)
				edgePos.push_back(a_pos);
			else  //<2F><>Ҫ<EFBFBD><D2AA>ֵ
			{
				for (int j = last_pos.x + 1; j < a_pos.x; j++)
				{
					int meany = (last_pos.y + a_pos.y) / 2;
					SVzNL2DPoint new_pos = {j, meany};
					edgePos.push_back(new_pos);
				}
				edgePos.push_back(a_pos);
			}
		}
	}
}

//<2F><>ü<EFBFBD>߶Ȳ<DFB6><C8B2><EFBFBD>
WD_wheelArchInfo wd_wheelArchHeigthMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_lineSegParam lineSegPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSG_planeCalibPara groundCalibPara,
	int* errCode)
{
	*errCode = 0;

	//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
	//ʹ<>ó߶<C3B3>1ȷ<31><C8B7><EFBFBD><EFBFBD>̥<EFBFBD><CCA5><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ص<EFBFBD>
	double scale_segGap = 10.0;
	double scale_endingAngleLen = 25.0; //<2F>˵㷽<CBB5><E3B7BD><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD>߶<EFBFBD>
	double wheel_endingAngleTh = 25;
	double arcScale_1 = 10.0;
	double arcScale_2 = 5.0;
	double computeLen = 150.0; 
	int safeGuardLen = 20;

	WD_wheelArchInfo result;
	memset(&result, 0, sizeof(WD_wheelArchInfo));

	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		if (linePtNum != (int)lineData.size())
			isGridData = false;
		//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
	}

	//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
	std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
	hLines_raw.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		hLines_raw[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
			hLines_raw[j][line] = scanLines[line][j];
			hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 974)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
		//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
		int ptNum = (int)lineData.size();
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
	}

	//<2F><><EFBFBD>࣬<EFBFBD><E0A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>̥
	std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
	std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
	featureInfoMask.resize(lineNum);
	feature3DInfo.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		featureInfoMask[i].resize(linePtNum);
		feature3DInfo[i].resize(linePtNum);
	}
	//<2F><><EFBFBD><EFBFBD>Mask
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			if (scanLines[line][ptIdx].pt3D.z > 1e-4)
			{
				SSG_featureClusteringInfo a_mask;
				memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
				a_mask.featurType = 1;
				a_mask.lineIdx = line;
				a_mask.ptIdx = ptIdx;
				featureInfoMask[line][ptIdx] = a_mask;
				feature3DInfo[line][ptIdx] = scanLines[line][ptIdx].pt3D;
			}
		}
	}
	//<2F><><EFBFBD><EFBFBD>
	//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
	std::vector<std::vector< SVzNL2DPoint>> clusters; //ֻ<><D6BB>¼λ<C2BC><CEBB>
	std::vector<SWD_clustersInfo> clustersInfo;
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < linePtNum; y++)
	{
		for (int x = 0; x < lineNum; x++)
		{
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
			if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
				continue;

			SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
			SVzNL3DRangeD a_clusterRoi;
			a_clusterRoi.xRange.min = a_feature3DValue.x;
			a_clusterRoi.xRange.max = a_feature3DValue.x;
			a_clusterRoi.yRange.min = a_feature3DValue.y;
			a_clusterRoi.yRange.max = a_feature3DValue.y;
			a_clusterRoi.zRange.min = a_feature3DValue.z;
			a_clusterRoi.zRange.max = a_feature3DValue.z;

			SVzNL2DPoint a_seedPos = { x, y };
			std::vector< SVzNL2DPoint> a_cluster;
			a_cluster.push_back(a_seedPos);
			wd_gridPointClustering(
				featureInfoMask,//int<6E><74><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>clusterID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>flag
				feature3DInfo,//double,<2C><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
				clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				clusterID, //<2F><>ǰCluster<65><72>ID
				a_cluster,  //result
				a_clusterRoi
			);
			clusters.push_back(a_cluster);
			SWD_clustersInfo a_info;
			a_info.clusterIdx = clusterID;
			a_info.ptSize = (int)a_cluster.size();
			a_info.roi3D = a_clusterRoi;
			clustersInfo.push_back(a_info);
			clusterID++;
		}
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ࡣ<EFBFBD><E0A1A3><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ǰ壬<C7B0><E5A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD><C7B3><EFBFBD>
	std::sort(clustersInfo.begin(), clustersInfo.end(), compareByPtSize);
	int cluseter_wheel_id, cluster_arc_id;
	if (clustersInfo[0].roi3D.yRange.max > clustersInfo[1].roi3D.yRange.max)
	{
		cluseter_wheel_id = clustersInfo[0].clusterIdx;
		cluster_arc_id = clustersInfo[1].clusterIdx;
	}
	else
	{
		cluseter_wheel_id = clustersInfo[1].clusterIdx;
		cluster_arc_id = clustersInfo[0].clusterIdx;
	}
	std::vector< SVzNL2DPoint>& cluster_wheel = clusters[cluseter_wheel_id - 1];
	std::vector< SVzNL2DPoint>& cluster_arc = clusters[cluster_arc_id - 1];
	for (int i = 0, i_max = (int)cluster_wheel.size(); i < i_max; i++)
	{
		int lineIdx = cluster_wheel[i].x;
		int ptIdx = cluster_wheel[i].y;
		scanLines[lineIdx][ptIdx].nPointIdx = 1; //wheel
	}
	for (int i = 0, i_max = (int)cluster_arc.size(); i < i_max; i++)
	{
		int lineIdx = cluster_arc[i].x;
		int ptIdx = cluster_arc[i].y;
		scanLines[lineIdx][ptIdx].nPointIdx = 2;
	}

	std::vector<std::vector<SWDIndexingVzPoint>> upEndings;
	std::vector<std::vector<SWDIndexingVzPoint>> downEndings;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 319)
			int kkk = 1;
		std::vector<SWDIndexingVzPoint> line_upEnding;
		std::vector<SWDIndexingVzPoint> line_downEnding;
		_lineProc_getWheelEnding(scanLines[line], line, line_upEnding, line_downEnding);
		upEndings.push_back(line_upEnding);
		downEndings.push_back(line_downEnding);
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector<SSG_endingTree> upEndingTrees;
	_getWheelEndingGrowingTrees(upEndings, upEndingTrees, growParam);

	std::vector<SSG_endingTree> downEndingTrees;
	_getWheelEndingGrowingTrees(downEndings, downEndingTrees, growParam);

	//ȷ<><C8B7><EFBFBD><EFBFBD>̥<EFBFBD><CCA5><EFBFBD><EFBFBD>
	if ( (upEndingTrees.size() == 0) || (downEndingTrees.size() == 0))
	{
		*errCode = SX_ERR_INVALID_WHEEL_EDGE;
		return result;
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ӱ<EFBFBD><D3B1><EFBFBD>
	SSG_endingTree& wheedEdge_up = upEndingTrees[0];
	if (upEndingTrees.size() > 1)
	{
		double minY = _getTopY(wheedEdge_up);
		for (int i = 1; i < (int)upEndingTrees.size(); i++)
		{
			double topY = _getTopY(upEndingTrees[i]);
			if (minY > topY)
			{
				wheedEdge_up = upEndingTrees[i];
				minY = topY;
			}
		}
	}

	for (int i = 0; i < (int)downEndingTrees.size(); i++)
	{
		double btmY_0 = _getBtmY(downEndingTrees[i]);
		int left_0 = downEndingTrees[i].treeNodes[0].lineIdx;
		int right_0 = downEndingTrees[i].treeNodes.back().lineIdx;
		for (int j = i + 1; j < (int)downEndingTrees.size(); j++)
		{
			if (downEndingTrees[j].treeType < 0)
				continue;

			double btmY_1 = _getBtmY(downEndingTrees[j]);
			int left_1 = downEndingTrees[j].treeNodes[0].lineIdx;
			int right_1 = downEndingTrees[j].treeNodes.back().lineIdx;
			if ((right_1 > left_0) && (right_0 > left_1))
			{
				if (btmY_0 < btmY_1)
					downEndingTrees[i].treeType = -1;
				else
					downEndingTrees[j].treeType = -1;
			}
		}
	}
	std::vector<SSG_endingTree> validDownEndingTrees;
	for (int i = 0; i < (int)downEndingTrees.size(); i++)
	{
		if (downEndingTrees[i].treeType >= 0)
			validDownEndingTrees.push_back(downEndingTrees[i]);
	}
	//<2F><><EFBFBD><EFBFBD>
	std::sort(validDownEndingTrees.begin(), validDownEndingTrees.end(), _compareByLineIdx);
	std::vector<SVzNL2DPoint> downEdgePos;
	for (int i = 0; i < (int)validDownEndingTrees.size(); i++)
		_genEdgeContinuousPos(validDownEndingTrees[i], downEdgePos);

	//<2F><><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>ɨ<EFBFBD><C9A8><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD>ء<EFBFBD><D8A1>յĵ<D5B5><C4B5><EFBFBD><EFBFBD>в<EFBFBD>ֵ<EFBFBD>õ<EFBFBD>
	std::vector<SVzNL2DPoint> upEdgePos;
	_genEdgeContinuousPos(wheedEdge_up, upEdgePos);

	int start_up, start_down;
	if (upEdgePos[0].x < downEdgePos[0].x)
	{
		start_down = 0;
		start_up = downEdgePos[0].x - upEdgePos[0].x;
	}
	else
	{
		start_up = 0;
		start_down = upEdgePos[0].x - downEdgePos[0].x;
	}
	int commonNum = (int)upEdgePos.size() - start_up;
	int num2 = (int)downEdgePos.size() - start_down;
	if ((commonNum < 0) || (num2 < 0))
	{
		*errCode = SX_ERR_NO_WHEEL_COMMON_EDGE;
		return result;
	}

	std::vector<SSG_intPair> all_edgePairs;
	commonNum = commonNum > num2 ? num2 : commonNum;
	for (int i = 0; i < commonNum; i++)
	{
		SSG_intPair a_pair;
		a_pair.idx = upEdgePos[i + start_up].x;
		a_pair.data_0 = upEdgePos[i + start_up].y;
		a_pair.data_1 = downEdgePos[i + start_down].y;
		all_edgePairs.push_back(a_pair);
	}

	//<2F><>ȡ<EFBFBD><C8A1>̥<EFBFBD><CCA5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сֵ<D0A1><D6B5>Ȼ<EFBFBD><C8BB>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>100mm<6D><6D><EFBFBD><EFBFBD>Χ<EFBFBD><CEA7>V<EFBFBD>Ͳ<EFBFBD>
	double wheelTopY = DBL_MAX;
	SSG_intPair wheelTopPos = { -1, -1, -1 };
	int topIdx = -1;
	for (int i = 0; i < (int)all_edgePairs.size(); i++)
	{
		if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4)
			continue;

		if (wheelTopY > scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y)
		{
			wheelTopY = scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y;
			wheelTopPos = all_edgePairs[i];
			topIdx = i;
		}
	}
	int startIdx = -1;
	for (int i = topIdx; i >= 0; i--)
	{
		if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4)
			continue;

		double dist = sqrt(pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.x - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.x, 2) + 
			pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.y - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y, 2));
		if (dist > computeLen)
		{
			startIdx = i;
			break;
		}
	}
	if (startIdx < 0)
		startIdx = 0;

	int endIdx = -1;
	for (int i = topIdx; i < (int)all_edgePairs.size(); i++)
	{
		if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4)
			continue;

		double dist = sqrt(pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.x - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.x, 2) +
			pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.y - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y, 2));
		if (dist > computeLen)
		{
			endIdx = i;
			break;
		}
	}
	if (endIdx < 0)
		endIdx = (int)all_edgePairs.size() - 1;

	std::vector<SSG_intPair> edgePairs;
	for (int i = startIdx; i <= endIdx; i++)
		edgePairs.push_back(all_edgePairs[i]);

	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>V<EFBFBD>Ͳۡ<CDB2><DBA1><EFBFBD><EFBFBD><EFBFBD>6mm<6D><6D><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>V<EFBFBD>Ͳ<EFBFBD>
	std::vector< SVzNL2DPoint> upWheelPos;
	std::vector< SVzNL2DPoint> downWheelPos;
	int startLine = edgePairs[0].idx;
	int endLine = edgePairs.back().idx;
	SSG_cornerParam wheelCornerPara;
	memset(&wheelCornerPara, 0, sizeof(SSG_cornerParam));
	wheelCornerPara.scale = 5.0;
	wheelCornerPara.cornerTh = cornerPara.cornerTh;
	for (int line = startLine; line <= endLine; line++)
	{
		std::vector<SSG_basicFeature1D> cornerFeatures;
		sg_maskData_getLineCornerFeature(
			scanLines[line],
			line,
			1, //<2F><><EFBFBD>ֵ<EFBFBD>IDΪ1 
			wheelCornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
			cornerFeatures);
		
		int pairIdx = line - startLine;
		SSG_intPair& a_pair = edgePairs[pairIdx];
		int interval = (a_pair.data_1 - a_pair.data_0) / 3;
		if (cornerFeatures.size() >= 0)
		{
			for (int m = 0; m < (int)cornerFeatures.size(); m++)
			{

				if ((cornerFeatures[m].jumpPos2D.y > (a_pair.data_0 + safeGuardLen)) && 
					(cornerFeatures[m].jumpPos2D.y < (a_pair.data_0 + interval)) )

				{
					upWheelPos.push_back(cornerFeatures[m].jumpPos2D);
					break;
				}
			}
			for (int m = (int)cornerFeatures.size() - 1; m << (int)cornerFeatures.size() >= 0; m--)
			{
				if ((cornerFeatures[m].jumpPos2D.y < (a_pair.data_1 - safeGuardLen)) && 
					(cornerFeatures[m].jumpPos2D.y > (a_pair.data_1 - interval)))
				{
					downWheelPos.push_back(cornerFeatures[m].jumpPos2D);
					break;
				}
			}
			
		}
	}
	SVzNL2DPoint upPos;
	SVzNL3DPoint upWheelPt = _getWheelArcFittingPoint(scanLines, 4, upWheelPos, upPos);
	SVzNL2DPoint downPos;
	SVzNL3DPoint downWheelPt = _getWheelArcFittingPoint(scanLines, 5, downWheelPos, downPos);
	//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
	double _a, _b, _c;
	compute2ptLine(	downWheelPt, upWheelPt, &_a, &_b, &_c);
	double norm = sqrt(_a * _a + _b * _b);
	_a = _a / norm;
	_b = _b / norm;
	_c = _c / norm;

	//Ѱ<><D1B0><EFBFBD><EFBFBD>ü<EFBFBD><C3BC>
	//(1)<29><><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>¶˵㣨2<E3A3A8><32>ȡ<EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD>򣬾<EFBFBD>ȷȷ<C8B7><C8B7><EFBFBD>˵㣨3<E3A3A8><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD>4<EFBFBD><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD>㣨<EFBFBD><E3A3A8><EFBFBD>ߵ㣩
	std::vector<SVzNL2DPoint> arcEndings;
	_getArcEndings(scanLines, 2, arcEndings, upWheelPt.z); //<2F><>ü<EFBFBD><C3BC>ID<49><44>2
	if (arcEndings.size() == 0)
	{
		*errCode = SX_ERR_INVALID_ARC;
		return result;
	}
	//ȡ<><C8A1>ֱ<EFBFBD>߾<EFBFBD><DFBE><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>ĵ<EFBFBD>Ϊ<EFBFBD><CEAA>ü<EFBFBD><C3BC>
	double minDist = -1;
	SVzNL2DPoint arcPos;
	for (int i = 0; i < (int)arcEndings.size(); i++)
	{
		SVzNL3DPoint a_pt = scanLines[arcEndings[i].x][arcEndings[i].y].pt3D;
		double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
		if (minDist < -1e-4)
		{
			minDist = dist;
			arcPos = arcEndings[i];
		}
		else if (minDist > dist)
		{
			minDist = dist;
			arcPos = arcEndings[i];
		}
	}
	if (minDist < -1e-4)
	{
		*errCode = SX_ERR_NO_WHEEL_ARC;
		return result;
	}
	//<2F><>XYƽ<59><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>
	double outLineLen = 200;
	SVzNL3DPoint arcPt = scanLines[arcPos.x][arcPos.y].pt3D;
	result.wheelArchPos = arcPt;
	result.arcLine[0] = { arcPt.x - outLineLen, arcPt.y, arcPt.z };
	result.arcLine[1] = { arcPt.x + outLineLen, arcPt.y, arcPt.z };
	result.wheelUpPos = upWheelPt;
	result.upLine[0] = { upWheelPt.x - outLineLen, upWheelPt.y, upWheelPt.z };
	result.upLine[1] = { upWheelPt.x + outLineLen, upWheelPt.y, upWheelPt.z };
	result.wheelDownPos = downWheelPt;
	result.downLine[0] = { downWheelPt.x - outLineLen, downWheelPt.y, downWheelPt.z };
	result.downLine[1] = { downWheelPt.x + outLineLen, downWheelPt.y, downWheelPt.z };
	double centerY = (upWheelPt.y + downWheelPt.y) / 2;
	int searchLine = (upPos.x + downPos.x) / 2;
	minDist = DBL_MAX;
	int minPtIdx = 0;
	for (int i = 0; i < (int)scanLines[searchLine].size(); i++)
	{
		if (scanLines[searchLine][i].pt3D.z > 1e-4)
		{
			double dist = abs(scanLines[searchLine][i].pt3D.y - centerY);
			if (minDist > dist)
			{
				minDist = dist;
				minPtIdx = i;
			}
		}
	}
	result.centerLine[0] = { downWheelPt.x - outLineLen, centerY, scanLines[searchLine][minPtIdx].pt3D.z };
	result.centerLine[1] = { downWheelPt.x + outLineLen, centerY, scanLines[searchLine][minPtIdx].pt3D.z };
	result.archToCenterHeigth = sqrt(pow(centerY - arcPt.y, 2) + pow(scanLines[searchLine][minPtIdx].pt3D.z - arcPt.z, 2)); // centerY - arcPt.y;
	result.archToGroundHeigth = groundCalibPara.planeHeight - arcPt.y;
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨<DBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
	for (int i = 0; i < lineNum; i++)
		lineDataRT_vector(scanLines[i], groundCalibPara.invRMatrix, -1); 
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
	result.wheelArchPos = _ptRotate(result.wheelArchPos, groundCalibPara.invRMatrix);
	result.wheelUpPos = _ptRotate(result.wheelUpPos, groundCalibPara.invRMatrix);
	result.wheelDownPos = _ptRotate(result.wheelDownPos, groundCalibPara.invRMatrix);
	for (int i = 0; i < 2; i++)
	{
		result.arcLine[i] = _ptRotate(result.arcLine[i], groundCalibPara.invRMatrix);
		result.centerLine[i] = _ptRotate(result.centerLine[i], groundCalibPara.invRMatrix);
		result.downLine[i] = _ptRotate(result.downLine[i], groundCalibPara.invRMatrix);
		result.upLine[i] = _ptRotate(result.upLine[i], groundCalibPara.invRMatrix);
	}
	return result;
}