algoLib/sourceCode/bagThreadPositioning.cpp

#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "bagThreadPositioning_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>˱궨Mark<72><6B><EFBFBD>⣬<EFBFBD><E2A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڱ궨<DAB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
//version 1.1.1 : <20>Ż<EFBFBD><C5BB>˱궨<CBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>궨<EFBFBD><EAB6A8>ͬʱΪɨ<CEAA><C9A8><EFBFBD>߶˵<DFB6>ʱ<EFBFBD>ܹ<EFBFBD><DCB9><EFBFBD><EFBFBD><EFBFBD>
std::string	m_strVersion = "1.1.1";
const char* wd_bagThreadPositioningVersion(void)
{
	return m_strVersion.c_str();
}

//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><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_bagThread_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

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

#if 1

SVzNL3DPosition _findClosestPoint(std::vector<SVzNL3DPosition>& lineData, double a, double  b, double c, double* distMin)
{
	SVzNL3DPosition bestPt = { -1, { 0, 0, -1 } };

	double mod = sqrt(a * a + b * b);
	a = a / mod;
	b = b / mod;
	c = c / mod;
	double minDist = DBL_MAX;
	for (int i = 0; i < (int)lineData.size(); i++)
	{
		if (lineData[i].pt3D.z > 1e-4)
		{
			double dist = abs(a * lineData[i].pt3D.x + b * lineData[i].pt3D.y + c);
			if (minDist > dist)
			{
				minDist = dist;
				bestPt = lineData[i];
				bestPt.nPointIdx = i;
			}
		}
	}
	*distMin = minDist;
	return bestPt;
}

//<2F><><EFBFBD><EFBFBD>
void cloutPointsClustering(
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	SSG_treeGrowParam growParam,
	std::vector<std::vector< SVzNL2DPoint>>& clusters, //ֻ<><D6BB>¼λ<C2BC><CEBB>
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)feature3DInfo.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)feature3DInfo[0].size();
	//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
	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><CAB1><EFBFBD><EFBFBD>תʱ <20><> > 0 <20><>˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD>תʱ <20><> < 0 
SVzNL3DPoint rotateXoY(SVzNL3DPoint& pt, double sinTheta, double cosTheta)
{
	return (SVzNL3DPoint{ (pt.x * cosTheta - pt.y * sinTheta), (pt.x * sinTheta + pt.y * cosTheta), pt.z });
}

//<2F><>ͷλ<CDB7>ü<EFBFBD><C3BC>ⶨλ
void wd_bagThreadPositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSX_ScanInfo scanInfo,  //true:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>в۵<D0B2><DBB5><EFBFBD>false:<3A><><EFBFBD><EFBFBD><EFBFBD>ߴ<EFBFBD>ֱ<EFBFBD>۵<EFBFBD>
	const SSG_planeCalibPara groundCalibPara,
	const SSG_outlierFilterParam filterParam, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
	const SSG_cornerParam cornerPara, //V<><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	const SSG_raisedFeatureParam raisedFeaturePara,//<2F><>β͹<CEB2><CDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	const SSG_treeGrowParam growParam, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector<SSX_bagThreadInfo>& bagThreadInfo,
	std::vector<SSX_bagThreadInfo>& bagThreadInfo_relative, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Mark<72><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector<SVzNL3DPoint>& output_markCenter,  //<2F><><EFBFBD><EFBFBD>Markλ<6B><CEBB><EFBFBD><EFBFBD>Ϣ
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	if (true == scanInfo.isHorizonScan)
	{
		*errCode = SG_ERR_LASER_DIR_NOT_SUPPORTED;
		return;
	}
	if (false == scanInfo.scanFromThreadHead)
	{
		*errCode = SG_ERR_SCAN_DIR_NOT_SUPPORTED;
		return;
	}

	int linePtNum = (int)scanLines[0].size();
	//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//<2F><>ƽ<EFBFBD><C6BD>
	for (int i = 0; i < lineNum; i++)
	{																					 //<2F>д<EFBFBD><D0B4><EFBFBD>
		//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		wd_bagThread_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}

	//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//ͳ<><CDB3>ƽ<EFBFBD><C6BD><EFBFBD>߼<EFBFBD><DFBC><EFBFBD><EFBFBD>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>ڼ<EFBFBD><DABC><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>Ǻͺ<C7BA><CDBA><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>
	double ptInterval = 0;
	int ptIntevalNum = 0;
	std::vector< std::vector<SVzNL3DPosition>> data_lines_h;  //ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	data_lines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		data_lines_h[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>
			data_lines_h[j][line] = scanLines[line][j];
			data_lines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			data_lines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			if (j > 0)
			{
				if ((scanLines[line][j - 1].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z > 1e-4))
				{
					ptInterval += abs(scanLines[line][j].pt3D.y - scanLines[line][j - 1].pt3D.y);
					ptIntevalNum++;
				}
			}
		}
	}
	if(ptIntevalNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	ptInterval = ptInterval / ptIntevalNum;
	int lineNum_h = linePtNum;
	int linePtNum_h = (int)data_lines_h[0].size();
	double lineInterval = 0;
	int lineIntervalNum = 0;
	for (int line = 0; line< lineNum_h; line++)
	{
		for (int j = 0, j_max = (int)data_lines_h[line].size(); j < j_max; j++)
		{
			data_lines_h[line][j].nPointIdx = j;
			if (j > 0)
			{
				if ((data_lines_h[line][j - 1].pt3D.z > 1e-4) && (data_lines_h[line][j].pt3D.z > 1e-4))
				{
					lineInterval += abs(data_lines_h[line][j].pt3D.y - data_lines_h[line][j - 1].pt3D.y);
					lineIntervalNum++;
				}
			}
		}
	}
	if (lineIntervalNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	lineInterval = lineInterval / lineIntervalNum;

	SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0};
	double markRotateAngle = 0;
	SVzNL3DPoint markCenter = { 0,0,0 };
	std::vector< SVzNL2DPoint> debug_markOrigin;
	std::vector< SVzNL2DPoint> debug_markXDir;

	double vCornerScale = cornerPara.scale * 4;
	std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
	std::vector<std::vector<SWD_segFeature>> raisedFeatures;
	//<2F>ֳ<EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>ֱɨ<D6B1><C9A8>
	//if (false == scanInfo.isHorizonScan) //
	{
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;

		int validVCornerSCale = (int)(vCornerScale / ptInterval);
		//<2F><>ֱɨ<D6B1><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>V<EFBFBD>Ͳۺ<CDB2><DBBA><EFBFBD>β
		for (int line = 0; line < lineNum; line++)
		{
			if ((line == 755) ||  (line == 905))
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
			//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
			//<2F><>ȡV<C8A1>Ͳ<EFBFBD>
			std::vector<SSG_basicFeature1D> line_cornerFeatures;
			std::vector<SSG_RUN_EX> segs;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature_accelerate(
				lineData,
				line,
				cornerPara,
				ptInterval,
				segs,
				line_cornerFeatures  //<2F>յ<EFBFBD>
			);
			//<2F><><EFBFBD><EFBFBD>V<EFBFBD><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD><E0A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			std::vector<SSG_basicFeature1D> valid_cornerFeatures;
			int vCornerSize = (int)line_cornerFeatures.size();
			int segSize = (int)segs.size();
			for (int m = 0; m < vCornerSize; m++)
			{
				int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
				for (int n = 0; n < segSize; n++)
				{
					int segEnd = segs[n].start + segs[n].len - 1;
					if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
					{
						int skip_1 = cornerPos - segs[n].start;
						int skip_2 = segEnd - cornerPos;
						if((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
							valid_cornerFeatures.push_back(line_cornerFeatures[m]);
						break;
					}
				}
			}
			cornerFeatures.push_back(valid_cornerFeatures);

			//<2F><>ȡ͹<C8A1><CDB9><EFBFBD><EFBFBD>
			std::vector<SWD_segFeature> line_raisedFeatures;
			wd_getLineRaisedFeature(
				lineData,
				line,
				raisedFeaturePara, //͹<><CDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				line_raisedFeatures  //͹<><CDB9>
			);
			raisedFeatures.push_back(line_raisedFeatures);

			//<2F><>ȡ<EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			std::vector<SSG_basicFeature1D> line_jumpFearures;
			wd_getSpecifiedHeightJumping(
				lineData,
				line,
				jumpHeight, //<2F><><EFBFBD><EFBFBD><EFBFBD>߶Ȳ<DFB6><C8B2><EFBFBD>
				line_jumpFearures  //<2F><><EFBFBD><EFBFBD>
			);
			jumpFeatures_v.push_back(line_jumpFearures);
		}
#if 1
		//ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>궨<EFBFBD><EAB6A8>
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
		for (int line = 0; line < lineNum_h; line++)
		{
			if ((line == 2240) || (line == 905))
				int kkk = 1;

			std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
			//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
			//<2F><>ȡ<EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			std::vector<SSG_basicFeature1D> line_jumpFearures;
			wd_getSpecifiedHeightJumping(
				lineData,
				line,
				jumpHeight, //<2F><><EFBFBD><EFBFBD><EFBFBD>߶Ȳ<DFB6><C8B2><EFBFBD>
				line_jumpFearures  //<2F><><EFBFBD><EFBFBD>
			);
			jumpFeatures_h.push_back(line_jumpFearures);
		}

		//<2F>Ա궨<D4B1><EAB6A8><EFBFBD><EFBFBD><EFBFBD>о<EFBFBD><D0BE><EFBFBD>
		std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
		std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
		featureInfoMask.resize(lineNum);
		feature3DInfo.resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			featureInfoMask[line].resize(linePtNum);
			std::fill(featureInfoMask[line].begin(), featureInfoMask[line].end(), SSG_featureClusteringInfo{ 0,0,0,0,0,0,0 });
			feature3DInfo[line].resize(linePtNum);
			std::fill(feature3DInfo[line].begin(), feature3DInfo[line].end(), SVzNL3DPoint{ 0,0,0 });
		}
		//<2F><>ֱ
		for (int line = 0; line < lineNum; line++)
		{
			int lineJumpNum = (int)jumpFeatures_v[line].size();
			for(int j = 0; j <lineJumpNum; j++)
			{
				SSG_basicFeature1D& a_jump = jumpFeatures_v[line][j];
				SSG_featureClusteringInfo a_feature;
				a_feature.featurType = a_jump.featureType;
				a_feature.featureIdx_v = j;
				a_feature.featureIdx_h = -1;
				a_feature.clusterID = 0;
				a_feature.flag = 0;
				a_feature.lineIdx = line;
				a_feature.ptIdx = a_jump.jumpPos2D.y;

				int ptIdx = a_jump.jumpPos2D.y;
				featureInfoMask[line][ptIdx] = a_feature;
				feature3DInfo[line][ptIdx] = a_jump.jumpPos;
			}
		}
		//ˮƽ
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 1067) 
				int kkk = 1;
			int lineJumpNum = (int)jumpFeatures_h[line].size();
			for (int j = 0; j < lineJumpNum; j++)
			{
				SSG_basicFeature1D& a_jump = jumpFeatures_h[line][j];
				SSG_featureClusteringInfo a_feature;
				a_feature.featurType = a_jump.featureType;
				a_feature.featureIdx_v = -1;
				a_feature.featureIdx_h = j;
				a_feature.clusterID = 0;
				a_feature.flag = 0;
				a_feature.lineIdx = a_jump.jumpPos2D.y;
				a_feature.ptIdx = line;

				int ptIdx = a_jump.jumpPos2D.y;
				if (featureInfoMask[ptIdx][line].featurType == 0)  //<2F><><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
				{
					featureInfoMask[ptIdx][line] = a_feature;
					feature3DInfo[ptIdx][line] = { a_jump.jumpPos.y, a_jump.jumpPos.x, a_jump.jumpPos.z };
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD>
		std::vector<std::vector< SVzNL2DPoint>> markClusters; //ֻ<><D6BB>¼λ<C2BC><CEBB>
		std::vector<SWD_clustersInfo> markClustersInfo;
		SSG_treeGrowParam markGrowParam;
		markGrowParam.yDeviation_max = 1.0;//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Yƫ<59><C6AB>
		markGrowParam.zDeviation_max = 1.0; //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Zƫ<5A><C6AB>
		markGrowParam.maxLineSkipNum = 5;  //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD><DFBC><EFBFBD><EFBFBD><EFBFBD> -1ʱʹ<CAB1><CAB9>maxDkipDistance
		markGrowParam.maxSkipDistance = 1.0; //<2F><>maxLineSkipNumΪ-1<><31> ʹ<>ô˲<C3B4><CBB2><EFBFBD>.<2E><>Ϊ-1ʱ<31><CAB1><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>Ч
		markGrowParam.minLTypeTreeLen = 5.0;  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵĽڵ<C4BD><DAB5><EFBFBD>Ŀ<EFBFBD><C4BF>С<EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD>
		markGrowParam.minVTypeTreeLen = 5.0;  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵĽڵ<C4BD><DAB5><EFBFBD>Ŀ<EFBFBD><C4BF>С<EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD>
		cloutPointsClustering(
			featureInfoMask,
			feature3DInfo,
			markGrowParam,
			markClusters, //ֻ<><D6BB>¼λ<C2BC><CEBB>
			markClustersInfo);
		
		//<2F>жϱ궨<CFB1><EAB6A8><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD> 
		int clusterNum = (int)markClustersInfo.size();
		if (clusterNum < 2)
		{
			*errCode = SX_ERR_NO_MARK;
			return;
		}
		else
		{
			std::vector<int> validMarks;
			for (int i = 0; i < clusterNum; i++)
			{
				double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min;
				double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min;
				if ((w >= scanInfo.mark_diameter - 1.0) && (w <= scanInfo.mark_diameter + 1.0) &&
					(h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0))
				{
					validMarks.push_back(i);
				}
			}

			int mark_O_idx = -1, mark_x_idx = -1;
			if (validMarks.size() < 2)
			{
				*errCode = SX_ERR_NO_MARK;
				return;
			}
			else if (validMarks.size() == 2)
			{
				int mark0Idx = validMarks[0];
				int mark1Idx = validMarks[1];
				double cy_1 = (markClustersInfo[mark0Idx].roi3D.yRange.max + markClustersInfo[mark0Idx].roi3D.yRange.min) / 2;
				double cy_2 = (markClustersInfo[mark1Idx].roi3D.yRange.max + markClustersInfo[mark1Idx].roi3D.yRange.min) / 2;
				if (cy_1 < cy_2)
				{
					mark_O_idx = mark0Idx;
					mark_x_idx = mark1Idx;
				}
				else
				{
					mark_O_idx = mark1Idx;
					mark_x_idx = mark0Idx;
				}
			}
			else// if (validMarks.size() > 2)
			{
				//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
				SSG_intPair obj_pair = {-1,-1,-1};
				double best_dist = -1;
				for (int i = 0; i < (int)validMarks.size(); i++)
				{
					int vldIdx0 = validMarks[i];
					double cx_1 = (markClustersInfo[vldIdx0].roi3D.xRange.max + markClustersInfo[vldIdx0].roi3D.xRange.min) / 2;
					double cy_1 = (markClustersInfo[vldIdx0].roi3D.yRange.max + markClustersInfo[vldIdx0].roi3D.yRange.min) / 2;
					for (int j = i + 1; j < (int)validMarks.size(); j++)
					{
						int vldIdx1 = validMarks[j];
						double cx_2 = (markClustersInfo[vldIdx1].roi3D.xRange.max + markClustersInfo[vldIdx1].roi3D.xRange.min) / 2;
						double cy_2 = (markClustersInfo[vldIdx1].roi3D.yRange.max + markClustersInfo[vldIdx1].roi3D.yRange.min) / 2;
						double dist = sqrt(pow(cx_1 - cx_2, 2) + pow(cy_1 - cy_2, 2));
						if (best_dist < 0)
						{
							best_dist = dist;
							obj_pair.data_0 = vldIdx0;
							obj_pair.data_1 = vldIdx1;
						}
						else
						{
							double dist_diff1 = abs(best_dist - scanInfo.mark_distance);
							double dist_diff2 = abs(dist - scanInfo.mark_distance);
							if (dist_diff1 > dist_diff2)
							{
								best_dist = dist;
								obj_pair.data_0 = vldIdx0;
								obj_pair.data_1 = vldIdx1;
							}
						}
					}
				}
				if (obj_pair.data_0 < 0)
				{
					*errCode = SX_ERR_NO_MARK;
					return;
				}
				else
				{
					double cy_1 = (markClustersInfo[obj_pair.data_0].roi3D.yRange.max + markClustersInfo[obj_pair.data_0].roi3D.yRange.min) / 2;
					double cy_2 = (markClustersInfo[obj_pair.data_1].roi3D.yRange.max + markClustersInfo[obj_pair.data_1].roi3D.yRange.min) / 2;
					if (cy_1 < cy_2)
					{
						mark_O_idx = obj_pair.data_0;
						mark_x_idx = obj_pair.data_1;
					}
					else
					{
						mark_O_idx = obj_pair.data_1;
						mark_x_idx = obj_pair.data_0;
					}
				}
			}
			//Բ<><D4B2><EFBFBD>ϼ<EFBFBD><CFBC><EFBFBD>Բ<EFBFBD>ģ<EFBFBD><C4A3>Ե<EFBFBD>Zֵ<5A><D6B5>ֵ<EFBFBD><D6B5>ΪԲ<CEAA>ĵ<EFBFBD>Zֵ
			std::vector<SVzNL3DPoint> fittingData_0;
			double meanZ_0 = 0;
			for (int i = 0; i < markClusters[mark_O_idx].size(); i++)
			{
				SVzNL2DPoint& a_pt2D = markClusters[mark_O_idx][i];
				SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y];
				meanZ_0 += a_pt3D.z;
				fittingData_0.push_back(a_pt3D);
			}
			meanZ_0 = meanZ_0 / (double)markClusters[mark_O_idx].size();
			SVzNL3DPoint mark0_center;
			double mark0_radius;
			double fitErr1 = fitCircleByLeastSquare(
				fittingData_0,
				mark0_center,
				mark0_radius);
			mark0_center.z = meanZ_0;

			std::vector<SVzNL3DPoint> fittingData_1;
			double meanZ_1 = 0;
			for (int i = 1; i < markClusters[mark_x_idx].size(); i++)
			{
				SVzNL2DPoint& a_pt2D = markClusters[mark_x_idx][i];
				SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y];
				meanZ_1 += a_pt3D.z;
				fittingData_1.push_back(a_pt3D);
			}
			meanZ_1 = meanZ_1 / (double)markClusters[mark_x_idx].size();
			SVzNL3DPoint mark1_center;
			double mark1_radius;
			double fitErr2 = fitCircleByLeastSquare(
				fittingData_1,
				mark1_center,
				mark1_radius);
			mark1_center.z = meanZ_1;

			output_markCenter.push_back(mark0_center);
			output_markCenter.push_back(mark1_center);
			debug_markOrigin.insert(debug_markOrigin.end(), markClusters[mark_O_idx].begin(), markClusters[mark_O_idx].end());
			debug_markXDir.insert(debug_markXDir.end(), markClusters[mark_x_idx].begin(), markClusters[mark_x_idx].end());

			markCenter = mark0_center;
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
			SVzNL2DPointD a = { mark1_center.x - mark0_center.x, mark1_center.y - mark0_center.y };
			SVzNL2DPointD b = { 1, 0 };
			double rotAngle = 0;
			bool validRotate = calcRotateAngle(a, b, rotAngle);
			markRotateAngle = rotAngle;
		}
#endif
	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector< SSG_featureTree> cornerGrowTrees;
	sg_cornerFeaturesGrowing(
		cornerFeatures,
		cornerGrowTrees,
		growParam);
	std::vector<SWD_segFeatureTree> raisedFeatureGrowTrees;
	wd_getSegFeatureGrowingTrees(
		raisedFeatures,
		raisedFeatureGrowTrees,
		growParam);

	if (cornerGrowTrees.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
	int threadTailTreeIdx = -1; //<2F><>β<EFBFBD><CEB2><EFBFBD>ڵ<EFBFBD>tree
	if (raisedTreeNum == 1)
		threadTailTreeIdx = 0;
	else if (raisedTreeNum > 1)
	{
		//ȡ<><EFBFBD><EEB3A4>tree<65><65>Ϊ<EFBFBD><CEAA>β<EFBFBD><CEB2><EFBFBD>ڵ<EFBFBD>tree
		int maxLines = 0;
		threadTailTreeIdx = 0;
		for (int i = 0; i < raisedTreeNum; i++)
		{
			int treeLines = raisedFeatureGrowTrees[i].eLineIdx - raisedFeatureGrowTrees[i].sLineIdx;
			if (maxLines < treeLines)
			{
				maxLines = treeLines;
				threadTailTreeIdx = i;
			}
		}
	}
	int cornerTreeNum = (int)cornerGrowTrees.size();
	int objTreeIdx = -1;
	if (threadTailTreeIdx < 0)
	{
		int maxLines = 0;
		//ȡ<><EFBFBD><EEB3A4>tree<65><65>Ϊ<EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		for (int i = 0; i < cornerTreeNum; i++)
		{
			int treeLines = cornerGrowTrees[i].eLineIdx - cornerGrowTrees[i].sLineIdx;
			if (maxLines < treeLines)
			{
				maxLines = treeLines;
				objTreeIdx = i;
			}
		}
	}
	else
	{
		//ȡ<><C8A1><EFBFBD><EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD>tree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>tree<65><65>Ϊ<EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>2D<32><44><EFBFBD>룬<EFBFBD><EBA3AC><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱɨ<D6B1>跽ʽ

		SVzNL3DPoint pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].startPt;
		SVzNL3DPoint pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].endPt;
		SVzNL3DPoint tail_end_1 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
		pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().startPt;
		pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().endPt;
		SVzNL3DPoint tail_end_2 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
		double minDist = DBL_MAX;
		for (int i = 0; i < cornerTreeNum; i++)
		{
			//<2F>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>tree<65><65><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD> 
			SVzNL3DPoint end_1 = cornerGrowTrees[i].treeNodes[0].jumpPos;
			SVzNL3DPoint end_2 = cornerGrowTrees[i].treeNodes.back().jumpPos;
			int lineIdx_2 = cornerGrowTrees[i].eLineIdx;
			double dist_11 = sqrt(pow(tail_end_1.x - end_1.x, 2) + pow(tail_end_1.y - end_1.y, 2));
			double dist_12 = sqrt(pow(tail_end_1.x - end_2.x, 2) + pow(tail_end_1.y - end_2.y, 2));
			double dist_21 = sqrt(pow(tail_end_2.x - end_1.x, 2) + pow(tail_end_2.y - end_1.y, 2));
			double dist_22 = sqrt(pow(tail_end_2.x - end_2.x, 2) + pow(tail_end_2.y - end_2.y, 2));
			double dist = dist_11 < dist_12 ? dist_11 : dist_12;
			dist = dist < dist_21 ? dist : dist_21;
			dist = dist < dist_22 ? dist : dist_22;
			if (minDist > dist)
			{
				minDist = dist;
				objTreeIdx = i;
			}
		}
	}
	if(objTreeIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	
	//<2F><>ʾ 
	//<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30>ת<EFBFBD><D7AA>ʹ<EFBFBD><CAB9>
	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>
	//<2F>ñ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>debug
	{
		int nodeNum = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == scanInfo.isHorizonScan)
			{
				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
			}
			else
			{
				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
			}
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
		}
		//<2F><>ʾ<EFBFBD><CABE>ͷ
		if (threadTailTreeIdx >= 0)
		{
			int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
			for (int j = 0; j < nodeNum; j++)
			{
				int lineIdx, ptIdx;
				if (false == scanInfo.isHorizonScan)
				{
					lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
					for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
					{
						ptIdx = m;
						scanLines[lineIdx][ptIdx].nPointIdx = 2;
					}
				}
				else
				{
					ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
					for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
					{
						lineIdx = m;
						scanLines[lineIdx][ptIdx].nPointIdx = 2;
					}
				}
			}
		}
		for (int i = 0; i < (int)debug_markOrigin.size(); i++)
		{
			int lineIdx = debug_markOrigin[i].x;
			int ptIdx = debug_markOrigin[i].y;
			scanLines[lineIdx][ptIdx].nPointIdx = 3;
		}
		for (int i = 0; i < (int)debug_markXDir.size(); i++)
		{
			int lineIdx = debug_markXDir[i].x;
			int ptIdx = debug_markXDir[i].y;
			scanLines[lineIdx][ptIdx].nPointIdx = 4;
		}
	}
	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>(<28><>ȡ4<C8A1><34><EFBFBD><EFBFBD>
	std::vector<SVzNLRect> stitchROIs;
	int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();

	//<2F><><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ÿ<EFBFBD><C3BF>ɨ<EFBFBD><C9A8><EFBFBD>߹յ<DFB9>λ<EFBFBD>á<EFBFBD>ʹ<EFBFBD>÷ֶ<C3B7><D6B6><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD>
	double lenFittingScale = 10.0;//
	int lenFittingScale_lines = (int)(lenFittingScale / lineInterval);

	int totalLines = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
	int num_intervals = totalLines / lenFittingScale_lines;  //<2F>˴<EFBFBD>ʣ<EFBFBD>ಿ<EFBFBD>ֲ<EFBFBD><D6B2>ٴ<EFBFBD><D9B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊֻ<CEAA><D6BB>Ҫȡǰ3<C7B0><33><EFBFBD><EFBFBD>ͷ
	std::vector<std::vector<SVzNL3DPoint>> segPoints;
	segPoints.resize(num_intervals);
	std::vector<int> cornerNodeLineIndice;
	cornerNodeLineIndice.resize(totalLines);
	std::fill(cornerNodeLineIndice.begin(), cornerNodeLineIndice.end(), -1);
	int startLineIdx = cornerGrowTrees[objTreeIdx].sLineIdx;
	for (int i = 0; i < nodeSize; i++)
	{
		SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[i];
		int lineDiff = a_node.jumpPos2D.x - startLineIdx;
		cornerNodeLineIndice[lineDiff] = i;
		int interval_id = lineDiff / lenFittingScale_lines;
		if (interval_id < num_intervals)
			segPoints[interval_id].push_back(a_node.jumpPos);
	}
	//<2F>ֶ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector<SVzNL3DPoint> baseLinePoints; //<2F>߷<EFBFBD><DFB7><EFBFBD>ÿ<EFBFBD><C3BF>ɨ<EFBFBD><C9A8><EFBFBD>ߵĵ<DFB5>
	std::vector<SVzNL2DPoint> baseLinePtPos;
	for (int i = 0; i < num_intervals; i++)
	{
		std::vector<SVzNL3DPoint> fittingPoints;
		if ((i == 0) || (i == (num_intervals - 1)))
		{
			fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end());
		}
		else
		{
			//ʹ<><CAB9>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			fittingPoints.insert(fittingPoints.end(), segPoints[i-1].begin(), segPoints[i-1].end());
			fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end());
			fittingPoints.insert(fittingPoints.end(), segPoints[i+1].begin(), segPoints[i+1].end());
		}
		//<2F>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD>
		double a, b, c;  //<2F><><EFBFBD>̣<EFBFBD>ax+by+c=0;
		lineFitting_abc(fittingPoints, &a, &b, &c);

		int segStartLineIdx = i * lenFittingScale_lines + startLineIdx;
		for (int j = 0; j < lenFittingScale_lines; j++)
		{
			int idx = i * lenFittingScale_lines + j;
			if (cornerNodeLineIndice[idx] >= 0)
			{
				int nodeIdx = cornerNodeLineIndice[idx];
				SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];
				baseLinePtPos.push_back(a_node.jumpPos2D);
				baseLinePoints.push_back(a_node.jumpPos);
			}
			else
			{
				double dist = DBL_MAX;
				SVzNL3DPosition bestPoint = _findClosestPoint(scanLines[segStartLineIdx + j], a, b, c, &dist);
				SVzNL2DPoint pos = { segStartLineIdx + j , bestPoint.nPointIdx };
				if (dist > ptInterval * 2)
				{
					bestPoint.pt3D = { 0, 0, -1 };
					pos.y = -1;
				}
				baseLinePtPos.push_back(pos);
				baseLinePoints.push_back(bestPoint.pt3D);
			}
		}

	}

	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>ŷ<EFBFBD>Χ
	int pre_idx = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		int nodeIdx = (true == scanInfo.scanFromThreadHead) ? i : (nodeSize - 1 - i);
		SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];	
		if(pre_idx >= 0)
		{
			SSG_basicFeature1D& pre_node = cornerGrowTrees[objTreeIdx].treeNodes[pre_idx];
			int line_diff = a_node.jumpPos2D.x < pre_node.jumpPos2D.x ? (pre_node.jumpPos2D.x - a_node.jumpPos2D.x) : (a_node.jumpPos2D.x - pre_node.jumpPos2D.x);
			if (line_diff > 2)
			{
				double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
				if (width > scanInfo.stitchWidth)
				{
					//<2F><><EFBFBD><EFBFBD>Zֵ<5A><D6B5><EFBFBD>ߵ<EFBFBD>
					int searchStart = pre_node.jumpPos2D.x - startLineIdx;
					int searchEnd = a_node.jumpPos2D.x - startLineIdx;
					
					int mean_Lines = (int)(0.5 / lineInterval) + 1;  //ȡ1mm<6D>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD>ϵ<EFBFBD>Zƽ<5A><C6BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ڸ<EFBFBD><DAB8><EFBFBD>
					int validMeanLinesTh = mean_Lines - 5;
					std::vector<double> validStitch;
					std::vector<int> validStitchIndice;

					//<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>߶Ⱥ͸߶ȼ<DFB6>ֵ
					double sticthPeak = DBL_MAX;
					int debug_pkPos = -1;
					for (int m = searchStart + 1; m < searchEnd; m++)
					{
						if (baseLinePoints[m].z > 1e-4)
						{
							double sumZ = 0;
							int sumZ_counter = 0;
							for (int n = m - mean_Lines; n <= m+ mean_Lines; n++)
							{
								if ((n > searchStart) && (n < searchEnd))
								{
									if (baseLinePoints[n].z > 1e-4)
									{
										sumZ += baseLinePoints[n].z;
										sumZ_counter++;
									}
								}
							}
							validStitchIndice.push_back(m);
							if (sumZ_counter > validMeanLinesTh)
							{
								double meanZ = sumZ / (double)sumZ_counter;
								validStitch.push_back(meanZ);
								
								if (sticthPeak > meanZ)
								{
									sticthPeak = meanZ;
									debug_pkPos = m;
								}
							}
							else
								validStitch.push_back(-1);
						}
						else
							validStitch.push_back(-1);
					}

					//<2F><> <20><>sticthPeak+0.5)Ϊ<><CEAA><EFBFBD>ޣ<EFBFBD>ȡ<EFBFBD>м<EFBFBD><D0BC><EFBFBD>
					double stitchPeakTh = sticthPeak + 0.5;
					int validStart = -1, validEnd = 0;
					for (int n = 0; n < (int)validStitch.size(); n++)
					{
						if ((validStitch[n] > 1e-4) &&(validStitch[n] < stitchPeakTh))
						{
							if (validStart < 0)
								validStart = n;
							validEnd = n;
						}
					}
					int refCenterLine = (validStart + validEnd)/2 + searchStart + 1;
					SVzNL3DPoint stitchPos = { 0, 0, DBL_MAX };
					int stitchLineIdx = -1;
					int searchRng = (validEnd - validStart) / 2;
					for (int m = 0; m < searchRng; m++)  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΧҲ<CEA7><D2B2>ΪopDist_lines
					{
						int index_1 = refCenterLine + m;
						if (baseLinePoints[index_1].z > 1e-4)
						{
							stitchLineIdx = index_1;
							stitchPos = baseLinePoints[index_1];
							break;
						}
						int index_2 = refCenterLine - m;
						if (baseLinePoints[index_2].z > 1e-4)
						{
							stitchLineIdx = index_2;
							stitchPos = baseLinePoints[index_2];
							break;
						}
					}

					//<2F><><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>λ<EFBFBD><CEBB>
					SVzNL3DPoint operatePos = {0,0, -1};
					if (stitchLineIdx >= 0)
					{
						int opDist_lines;
						if (false == scanInfo.isHorizonScan)  //<2F><>ֱ<EFBFBD><D6B1><EFBFBD>߷<EFBFBD>ɨ<EFBFBD><C9A8>
							opDist_lines = (int)(scanInfo.operateDist / lineInterval);
						else
							opDist_lines = (int)(scanInfo.operateDist / ptInterval);

						int op_centerLine = stitchLineIdx + opDist_lines;
						for (int m = 0; m < opDist_lines; m++)  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΧҲ<CEA7><D2B2>ΪopDist_lines
						{
							int index_1 = op_centerLine + m;
							if (index_1 < totalLines)
							{
								if (cornerNodeLineIndice[index_1] > 0)
								{
									int nodeIndex = cornerNodeLineIndice[index_1];
									operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
									break;
								}
							}
							int index_2 = op_centerLine - m;
							if (index_2 >= 0)
							{
								if (cornerNodeLineIndice[index_2] > 0)
								{
									int nodeIndex = cornerNodeLineIndice[index_2];
									operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
									break;
								}
							}
						}
						if ( (stitchLineIdx >= 0) && (operatePos.z > 1e-4))
						{
							SSX_bagThreadInfo a_stitchInfo;
							memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
							a_stitchInfo.threadPos = stitchPos;
							a_stitchInfo.operatePos = operatePos;
							a_stitchInfo.rotateAngle = 0;
							bagThreadInfo.push_back(a_stitchInfo);
							if (bagThreadInfo.size() >= 4)
								break;
						}
					}
				}
			}
		}
		pre_idx = nodeIdx;
	}
	//<2F><><EFBFBD><EFBFBD>
	if (bagThreadInfo.size() == 0)
		*errCode = SG_ERR_ZERO_OBJECTS;

	//<2F><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double cosTheta = cos(markRotateAngle);
	double sinTheta = sin(markRotateAngle);
	bagThreadInfo_relative.resize(bagThreadInfo.size());
	for (int i = 0; i < (int)bagThreadInfo.size(); i++)
	{
		SSX_bagThreadInfo& a_stitch = bagThreadInfo[i];
		SVzNL3DPoint a_pt = { a_stitch.threadPos.x - markCenter.x, a_stitch.threadPos.y - markCenter.y, a_stitch.threadPos.z - markCenter.z };
		SVzNL3DPoint rot_pt = rotateXoY(a_pt, sinTheta, cosTheta);
		bagThreadInfo_relative[i].threadPos = rot_pt;
		a_pt = { a_stitch.operatePos.x - markCenter.x, a_stitch.operatePos.y - markCenter.y, a_stitch.operatePos.z - markCenter.z };
		rot_pt = rotateXoY(a_pt, sinTheta, cosTheta);
		bagThreadInfo_relative[i].operatePos = rot_pt;
		bagThreadInfo_relative[i].rotateAngle = 0;
	}
	return;
}
#endif