algoLib/sourceCode/BQ_assemblyPosition.cpp

#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "BQ_assemblyPosition_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_BQAssemblyPositionVersion(void)
{
	return m_strVersion.c_str();
}

//<2F><><EFBFBD>Կװ<D4BF><D7B0><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>ȡɨ<C8A1><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>п׵<D0BF>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD><DFBD><EFBFBD><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 sx_BQ_getHoleBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getHolePlaneCalibPara(scanLines);
}

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

int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
{
	int ptNum = 0;
	for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
	{
		if ((abs(lineData[i].pt3D.z) > 1e-4) ||
			(abs(lineData[i].pt3D.x > 1e-4)) ||
			(abs(lineData[i].pt3D.y > 1e-4)))
			ptNum++;
	}
	return ptNum;
}

bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.angle < b.angle;
}

SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	SSG_treeGrowParam growParam,
	SSG_planeCalibPara groundCalibPara,
	SSX_BQAssemblyPara assemblyParam,
	int* errCode)
{
	*errCode = 0;
	SSX_BQAssemblyInfo assemblyPose;
	memset(&assemblyPose, 0, sizeof(SSX_BQAssemblyInfo));
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return assemblyPose;
	}

	std::vector<std::vector<int>> groundMask;
	for (int line = 0, line_max = (int)scanLines.size(); line < line_max; line++)
	{
		//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		std::vector<SVzNL3DPosition>& a_line = scanLines[line];
		std::vector<int> a_lineMask;
		a_lineMask.resize(a_line.size());
		for (int i = 0; i < a_line.size(); i++)
		{
			if ((line == 1000) && (i == 1439))
				int kkk = 1;
			a_line[i].nPointIdx = i;
			a_lineMask[i] = 0;
			SVzNL3DPoint& a_pt = a_line[i].pt3D;
			if (a_pt.z < 1e-4)
				continue;
			double x = a_pt.x * groundCalibPara.planeCalib[0] + a_pt.y * groundCalibPara.planeCalib[1] + a_pt.z * groundCalibPara.planeCalib[2];
			double y = a_pt.x * groundCalibPara.planeCalib[3] + a_pt.y * groundCalibPara.planeCalib[4] + a_pt.z * groundCalibPara.planeCalib[5];
			double z = a_pt.x * groundCalibPara.planeCalib[6] + a_pt.y * groundCalibPara.planeCalib[7] + a_pt.z * groundCalibPara.planeCalib[8];
			if ((groundCalibPara.planeHeight > 0) && (z > (groundCalibPara.planeHeight-5)))  //<2F><><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD>Mask
			{
				a_lineMask[i] = 1; //				a_line[i].nPointIdx = -1;  //mark
			}
			a_pt.x = x;
			a_pt.y = y;
			a_pt.z = z;
			a_line[i].pt3D = a_pt;
		}
		groundMask.push_back(a_lineMask);
	}

	//<2F><><EFBFBD><EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Ŀհ<C4BF>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
	int validStartLine = -1;
	for (int i = 0; i < lineNum; i++)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validStartLine = i;
			break;
		}
	}
	int validEndLine = -1;
	for (int i = lineNum - 1; i >= 0; i--)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validEndLine = i;
			break;
		}
	}
	if ((validStartLine < 0) || (validEndLine < 0))
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return assemblyPose;
	}

	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;

	//<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD>
	//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
	double contourWin = 40.0; //<2F><><EFBFBD>渽<EFBFBD><E6B8BD><EFBFBD>Ĺյ<C4B9><D5B5><EFBFBD>Ϊ<EFBFBD>˵<EFBFBD>
	std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 1000)
			int kkk = 1;

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

		std::vector<SSG_basicFeature1D> line_features;
		int dataSize = (int)lineData.size();
		/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵĹյ<C4B9><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵㸽<CBB5><E3B8BD><EFBFBD>Ĺյ<C4B9><D5B5><EFBFBD>Ϊ<EFBFBD>ϸ<EFBFBD><CFB8>յ<EFBFBD>
		sg_getLineContourCornerFeature_groundMask_BQ(
			lineData,
			groundMask[line],
			line,
			contourWin, //ground<6E><64>Ե<EFBFBD><D4B5>Χ
			cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
			line_features);
		jumpFeatures_v_raw.push_back(line_features);
	}

	if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return assemblyPose;
	}

	//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
	std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
	std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
	std::vector<std::vector<int>> groundMask_hLines;
	hLines_raw.resize(linePtNum);
	int lineNum_h_raw = (int)hLines_raw.size();
#if 0
	hLines_raw.resize(linePtNum);
	groundMask_hLines.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
	{
		hLines_raw[i].resize(lineNum);
		groundMask_hLines[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;
			groundMask_hLines[j][line] = groundMask[line][j];
		}
	}
	//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		if (line == 1906)
			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);

		std::vector<SSG_basicFeature1D> line_features;
		int dataSize = (int)lineData.size();

		sg_getLineContourCornerFeature_groundMask_BQ(
			hLines_raw[line],
			groundMask_hLines[line],
			line,
			contourWin, //ground<6E><64>Ե<EFBFBD><D4B5>Χ
			cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
			line_features);
		jumpFeatures_h_raw.push_back(line_features);
	}
#endif
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
	//ʹ<>þ<EFBFBD><C3BE>෨<EFBFBD><E0B7A8><EFBFBD><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><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
	double meanX = 0, meanY = 0, meanZ = 0;
	int objPtSize = 0;
	for (int line = 0; line < lineNum; line++)
	{
		int featureNum = jumpFeatures_v_raw[line].size();
		for (int j = 0; j < featureNum; j++)
		{
			SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D;
			SSG_featureClusteringInfo a_mask;
			memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
			a_mask.featurType = 1;
			a_mask.lineIdx = a_pos.x;
			a_mask.ptIdx = a_pos.y;
			featureInfoMask[a_pos.x][a_pos.y] = a_mask;
			feature3DInfo[a_pos.x][a_pos.y] = scanLines[a_pos.x][a_pos.y].pt3D;
			meanX += scanLines[a_pos.x][a_pos.y].pt3D.x;
			meanY += scanLines[a_pos.x][a_pos.y].pt3D.y;
			meanZ += scanLines[a_pos.x][a_pos.y].pt3D.z;
			objPtSize++;
		}
	}
#if 0
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		int featureNum = jumpFeatures_h_raw[line].size();
		for (int j = 0; j < featureNum; j++)
		{
			SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D;
			if (featureInfoMask[a_pos.y][a_pos.x].featurType == 0)
			{
				SSG_featureClusteringInfo a_mask;
				memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
				a_mask.featurType = 1;
				a_mask.lineIdx = a_pos.y;
				a_mask.ptIdx = a_pos.x;
				featureInfoMask[a_pos.y][a_pos.x] = a_mask;
				feature3DInfo[a_pos.y][a_pos.x] = scanLines[a_pos.y][a_pos.x].pt3D;
			}
		}
	}
#endif

#if 0
	//<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 = 30;
	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++;
		}
	}
	//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>Ϊ<EFBFBD>߽<EFBFBD>
	int clusterSize = clusters.size();
	if(clusterSize == 0)
	{
		*errCode = SX_ERR_ZERO_OBJ;
		return assemblyPose;
	}

	int objPtSize = clusters[0].size();
	int objClusterID = 0;
	for (int i = 1; i < clusterSize; i++)
	{
		if (clusters[i].size() > objPtSize)
		{
			objPtSize = (int)clusters[i].size();
			objClusterID = i;
		}
	}
	if (objPtSize == 0)
	{
		*errCode = SX_ERR_ZERO_OBJ;
		return assemblyPose;
	}

	double meanX = 0, meanY = 0, meanZ = 0;
	for (int i = 0; i < objPtSize; i++)
	{
		int objLineIdx = clusters[objClusterID][i].x;
		int objPtIdx = clusters[objClusterID][i].y;
		scanLines[objLineIdx][objPtIdx].nPointIdx = 1;
		meanX += scanLines[objLineIdx][objPtIdx].pt3D.x;
		meanY += scanLines[objLineIdx][objPtIdx].pt3D.y;
		meanZ += scanLines[objLineIdx][objPtIdx].pt3D.z;
	}
#endif
	meanX = meanX / objPtSize;
	meanY = meanY / objPtSize;
	meanZ = meanZ / objPtSize;
	assemblyPose.O.x = meanX;
	assemblyPose.O.y = meanY;
	assemblyPose.O.z = meanZ;

#if 0
	std::vector<SWD_polarPt> polarPoints;
	for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vTree = &v_trees[i];
		//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
		for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
		{
			int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
			int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
			if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
			{
				SWD_polarPt a_polarPt;
				a_polarPt.lineIdx = lineIdx;
				a_polarPt.ptIdx = ptIdx;
				a_polarPt.R = 0;
				a_polarPt.angle = 0;
				a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
				a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
				a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
				polarPoints.push_back(a_polarPt);
				scanLines[lineIdx][ptIdx].nPointIdx = -1;
			}
		}
	}
	for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &h_trees[i];
		//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
		for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
		{
			int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
			int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
			if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
			{
				SWD_polarPt a_polarPt;
				a_polarPt.lineIdx = lineIdx;
				a_polarPt.ptIdx = ptIdx;
				a_polarPt.cptIndex = -1;
				a_polarPt.R = 0;
				a_polarPt.angle = 0;
				a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
				a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
				a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
				polarPoints.push_back(a_polarPt);
				scanLines[lineIdx][ptIdx].nPointIdx = -1;
			}
		}
	}

	//<2F><><EFBFBD>㼸<EFBFBD><E3BCB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	int contourPtSize = (int)polarPoints.size();
	if (contourPtSize == 0)
	{
		*errCode = SX_ERR_ZERO_CONTOUR_PT;
		return assemblyPose;
	}
	double center_x = 0;
	double center_y = 0;
	for (int pi = 0; pi < contourPtSize; pi++)
	{
		center_x += polarPoints[pi].x;
		center_y += polarPoints[pi].y;
	}
	center_x = center_x / (double)contourPtSize;
	center_y = center_y / (double)contourPtSize;
	//<2F><><EFBFBD>㼫<EFBFBD><E3BCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52>Theta
	for (int pi = 0; pi < contourPtSize; pi++)
	{
		double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
		polarPoints[pi].R = R;
		polarPoints[pi].angle = angle;
	}
	//<2F><><EFBFBD>Ƕȴ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
	std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
	for (int pi = 0; pi < contourPtSize; pi++)
		polarPoints[pi].cptIndex = pi;  // index

	//<2F><>ȡR<C8A1><52>ֵ<EFBFBD><D6B5>
	double minR = -1, maxR = -1; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>з<EFBFBD>֧<EFBFBD><D6A7>minR<6E><52>maxR<78><52><EFBFBD><EFBFBD>Сʱ<D0A1><CAB1>ΪԲ<CEAA>λ<EFBFBD>8<EFBFBD><38><EFBFBD>Σ<EFBFBD>û<EFBFBD>з<EFBFBD>֧
	int minRPos = -1;
	std::vector<SWD_polarPt> polarRPeakPts;
	int winSize = contourPtSize / 36;  //+-10<31>ȷ<EFBFBD>Χ
	if (winSize < 5)
		winSize = 5;
	for (int pi = 0; pi < contourPtSize; pi++)
	{
		double currR = polarPoints[pi].R;
		if (minR < 0)
		{
			minR = currR;
			maxR = currR;
			minRPos = pi;
		}
		else
		{
			minRPos = minR > currR ? pi : minRPos;
			minR = minR > currR ? currR : minR;
			maxR = maxR < currR ? currR : maxR;
		}
		bool isPeak = true;
		for (int k = -winSize; k <= winSize; k++)
		{
			int idx = (pi + k + contourPtSize) % contourPtSize; //Ͳ<>νṹ
			if (polarPoints[idx].R > currR)
			{
				isPeak = false;
				break;
			}
		}
		if (true == isPeak)
			polarRPeakPts.push_back(polarPoints[pi]);
	}

	double ratio_MaxMin = maxR / minR;
	bool hasBranch = ratio_MaxMin < 1.25 ? false : true;

	std::vector<SWD_polarPt> validPolarRPeakPts;
	std::vector<SWD_polarPeakInfo> polarPeakInfo;
	int pkId = 0;
	//<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>εļ<CEB5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>ƣ<EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>γɼ<CEB3>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>L=ֱ<>߶γ<DFB6><CEB3>ȹ<EFBFBD><C8B9>ɵ<EFBFBD><C9B5>Ž<EFBFBD><C5BD>ж<EFBFBD>
	double arcAngleChkLen = 100; //<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>Žǵij߶<C4B3>
	for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
	{
		int ptidx = polarRPeakPts[i].cptIndex;
		double px, py, pz;
		px = polarRPeakPts[i].x;
		py = polarRPeakPts[i].y;

		int LL1 = -1;
		int halfLL1 = -1;
		for (int j = ptidx - 1; j > -contourPtSize; j--)
		{
			int idx = (j + contourPtSize) % contourPtSize; //Ͳ<>νṹ
			double cx = polarPoints[idx].x;
			double cy = polarPoints[idx].y;
			double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
			if (len < arcAngleChkLen)
				halfLL1 = idx;
			if (len > (assemblyParam.lineLen))
			{
				LL1 = idx;
				break;
			}
		}

		int LL2 = -1;
		int halfLL2 = -1;
		for (int j = ptidx + 1; j < contourPtSize * 2; j++)
		{
			int idx = j % contourPtSize; //Ͳ<>νṹ
			double cx = polarPoints[idx].x;
			double cy = polarPoints[idx].y;
			double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
			if (len < arcAngleChkLen)
				halfLL2 = idx;
			if (len > (assemblyParam.lineLen))
			{
				LL2 = idx;
				break;
			}
		}

		if ((LL1 >= 0) && (LL2 >= 0))
		{
			double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
			double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
			double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
				pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
			double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
			double theta = acos(cosTheta) * 180.0 / PI;
			if (theta < 150)
			{
				SWD_polarPeakInfo a_pkInfo;
				a_pkInfo.cptIndex = ptidx;
				a_pkInfo.L1_ptIndex = LL1;
				a_pkInfo.L2_ptIndex = LL2;
				a_pkInfo.cornerAngle = theta;
				a_pkInfo.cornerDir = 0;
				polarRPeakPts[i].cptIndex = ptidx;
				polarRPeakPts[i].pkId = pkId;
				pkId++;
				validPolarRPeakPts.push_back(polarRPeakPts[i]);
				polarPeakInfo.push_back(a_pkInfo);
			}
		}
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ι<EFBFBD><CEB9><EFBFBD>
	if (validPolarRPeakPts.size() != 4)
	{
		*errCode = SX_ERR_INVLID_RPEAK_NUM;
		return assemblyPose;
	}

	//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD>ᣬ<EFBFBD>̱<EFBFBD><CCB1><EFBFBD>Y<EFBFBD>ᣬ<EFBFBD><E1A3AC><EFBFBD><EFBFBD>ΪZ<CEAA>ᣬ<EFBFBD><E1A3AC><EFBFBD>ĵ<EFBFBD>ΪO<CEAA><4F>
	std::vector<SVzNL3DPoint> sidePts_1;
	std::vector<SVzNL3DPoint> sidePts_2;
	std::vector<SVzNL3DPoint> sidePts_3;
	std::vector<SVzNL3DPoint> sidePts_4;
#endif
	return assemblyPose;
}

//<2F><><EFBFBD><EFBFBD>Mark<72><6B><EFBFBD>㹤<EFBFBD><E3B9A4>λ<EFBFBD>ú<EFBFBD><C3BA><EFBFBD>̬
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
	SSX_BQAssemblyInfo& originPos,
	std::vector<SVzNL3DPoint>& originMark,
	std::vector<SVzNL3DPoint>& currMark,
	int* errCode)
{
	*errCode = 0;
	SSX_BQAssemblyInfo resultPos;
	memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo));
	if ((originMark.size() < 3) || (currMark.size() < 3) || (originMark.size() != currMark.size()))
	{
		*errCode = SX_ERR_INVLID_MARK_NUM;
		return resultPos;
	}

	cv::Mat R, T; //<2F><>תƽ<D7AA><C6BD>
	cv::Point3d C_origin, C_curr;  //<2F><><EFBFBD><EFBFBD>

	std::vector<cv::Point3d> originMarkPos;
	originMarkPos.resize(originMark.size());
	std::vector<cv::Point3d> currMarkPos;
	currMarkPos.resize(originMark.size());
	for (int i = 0; i < (int)originMark.size(); i++)
	{
		originMarkPos[i] = { originMark[i].x, originMark[i].y, originMark[i].z };
		currMarkPos[i] = { currMark[i].x, currMark[i].y, currMark[i].z};
	}
	caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr);
	std::vector<cv::Point3d> originMarkPos_RT;
	originMarkPos_RT.resize(originMarkPos.size());
	for (int i = 0; i < (int)originMark.size(); i++)
	{
		cv::Point3d RT_pt;
		pointRT(R, T, C_origin, C_curr, originMarkPos[i], RT_pt);
		originMarkPos_RT[i] = RT_pt;
	}

	// 5. <20><><EFBFBD><EFBFBD>Pn<50><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	cv::Point3d O = { originPos.O.x, originPos.O.y, originPos.O.z};
	cv::Point3d X = { originPos.X.x, originPos.X.y, originPos.X.z };
	cv::Point3d Y = { originPos.Y.x, originPos.Y.y, originPos.Y.z };
	cv::Point3d Z = { originPos.Z.x, originPos.Z.y, originPos.Z.z };

	cv::Point3d RO, RX, RY, RZ;
	pointRT(R, T, C_origin, C_curr, O, RO);
	pointRT(R, T, C_origin, C_curr, X, RX);
	pointRT(R, T, C_origin, C_curr, Y, RY);
	pointRT(R, T, C_origin, C_curr, Z, RZ);
	resultPos.O = { RO.x, RO.y, RO.z };
	resultPos.X = { RX.x, RX.y, RX.z };
	resultPos.Y = { RY.x, RY.y, RY.z };
	resultPos.Z = { RZ.x, RZ.y, RZ.z };

	return resultPos;
}