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
std::string	m_strVersion = "1.0.0";
const char* wd_bagThreadPositioningVersion(void)
{
	return m_strVersion.c_str();
}
#if 1
//<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_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,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		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><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;

	double vCornerScale = cornerPara.scale * 4;
	std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
	std::vector<std::vector<SWD_segFeature>> raisedFeatures;
	if (false == scanInfo.isHorizonScan)
	{
		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 == 577) ||  (line == 932))
				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);
		}
#if 0
		//ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӱ<EFBFBD>
		std::vector<std::vector<SSG_basicFeature1D>> jumpdFeatures;
#endif
	}
	else
	{
		int validVCornerSCale = (int)(vCornerScale / lineInterval);
		//ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>V<EFBFBD>Ͳۺ<CDB2><DBBA><EFBFBD>β
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 329)
				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><>ȡ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,
				lineInterval,
				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><>ֱɨ<D6B1><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӱ<EFBFBD>
	}

	//<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;
		}
	}
	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;
				}
			}
		}
	}
	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>(<28><>ȡ4<C8A1><34><EFBFBD><EFBFBD>
	std::vector<SVzNLRect> stitchROIs;
	int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
	//<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)
				{
					SVzNLRect a_stitch;
					memset(&a_stitch, 0, sizeof(SVzNLRect));
					if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x)
					{
						a_stitch.left = a_node.jumpPos2D.x;
						a_stitch.right = pre_node.jumpPos2D.x;
					}
					else
					{
						a_stitch.left = pre_node.jumpPos2D.x;
						a_stitch.right = a_node.jumpPos2D.x;
					}
					if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y)
					{
						a_stitch.top = a_node.jumpPos2D.y;
						a_stitch.bottom = pre_node.jumpPos2D.y;
					}
					else
					{
						a_stitch.top = pre_node.jumpPos2D.y;
						a_stitch.bottom = a_node.jumpPos2D.y;
					}
					stitchROIs.push_back(a_stitch);
					if (stitchROIs.size() >= 4)
						break;
				}
			}
		}
		pre_idx = nodeIdx;
	}
	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
	if (stitchROIs.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	//<2F><><EFBFBD><EFBFBD>node<64>±<EFBFBD><C2B1><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŵ<EFBFBD><C5B5><EFBFBD><EFBFBD><EFBFBD>
	std::vector<int> backIndexing;
	int indexingSize = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
	backIndexing.resize(indexingSize);
	for (int i = 0; i < indexingSize; i++)
		backIndexing[i] = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx;
		backIndexing[indexingIdx] = i;
	}

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

	for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++)
	{
		//<2F><><EFBFBD><EFBFBD>Zֵ<5A><D6B5><EFBFBD>ߵ<EFBFBD>
		SVzNLRect& a_stitch = stitchROIs[i];
		SVzNL3DPoint stitchPos = { 0, 0, -1 };
		for (int j = a_stitch.left + 1; j < a_stitch.right; j++)
		{
			SVzNL3DPoint linePeak = { 0, 0, -1 };
			for (int m = a_stitch.top; m <= a_stitch.bottom; m++)
			{
				SVzNL3DPoint a_pt;
				if (false == scanInfo.isHorizonScan)  //<2F><>ֱ<EFBFBD><D6B1><EFBFBD>߷<EFBFBD>ɨ<EFBFBD><C9A8>
					a_pt = scanLines[j][m].pt3D;
				else
					a_pt = scanLines[m][j].pt3D;
				if (a_pt.z > 1e-4)
				{
					if (linePeak.z < 0)
						linePeak = a_pt;
					else
					{
						if (linePeak.z > a_pt.z)
							linePeak = a_pt;
					}
				}
			}
			if (linePeak.z > 1e-4)
			{
				if (stitchPos.z < 0)
					stitchPos = linePeak;
				else
				{
					if (stitchPos.z > linePeak.z)
						stitchPos = linePeak;
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>λ<EFBFBD><CEBB>
		if (stitchPos.z > 1e-4)
		{
			int op_centerLine;
			int searchStart, searchEnd;
			if (true == scanInfo.scanFromThreadHead)
			{
				op_centerLine = a_stitch.right + opDist_lines;
				searchStart = a_stitch.right; 
				searchEnd = a_stitch.right + opDist_lines * 2;
				if (searchEnd > cornerGrowTrees[objTreeIdx].eLineIdx)
					searchEnd = cornerGrowTrees[objTreeIdx].eLineIdx;
			}
			else
			{
				op_centerLine = a_stitch.left - opDist_lines;
				searchEnd = a_stitch.left;
				searchStart = a_stitch.left - opDist_lines * 2;
				if (searchStart < cornerGrowTrees[objTreeIdx].sLineIdx)
					searchStart = cornerGrowTrees[objTreeIdx].sLineIdx;
			}
			//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>
			int best_idx = -1;
			int minLineDist = INT_MAX;
			for (int j = searchStart; j <= searchEnd; j++)
			{
				int indexingIdx = j - cornerGrowTrees[objTreeIdx].sLineIdx;
				if (backIndexing[indexingIdx] >= 0)
				{
					int lineDist = j - op_centerLine;
					if (lineDist < 0)
						lineDist = -lineDist;
					if (minLineDist > lineDist)
					{
						minLineDist = lineDist;
						best_idx = backIndexing[indexingIdx];
					}
				}
			}
			if (best_idx >= 0)
			{
				int op_lineIdx, op_ptIdx;
				if (false == scanInfo.isHorizonScan)  //<2F><>ֱ<EFBFBD><D6B1><EFBFBD>߷<EFBFBD>ɨ<EFBFBD><C9A8>
				{
					op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
					op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
				}
				else
				{
					op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
					op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
				}
				SSX_bagThreadInfo a_stitchInfo;
				memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
				a_stitchInfo.threadPos = stitchPos;
				a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D;
				a_stitchInfo.rotateAngle = 0;
				bagThreadInfo.push_back(a_stitchInfo);
			}
		}
	}


	return;
}
#endif