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,
	bool isHorizonScan,  //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>
	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;
		}
	}
	int lineNum_h = linePtNum;
	int linePtNum_h = (int)data_lines_h[0].size();
	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;
	}

	std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
	std::vector<std::vector<SWD_segFeature>> raisedFeatures;
	if (false == isHorizonScan)
	{
		//<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;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature(
				lineData,
				line,
				cornerPara,
				line_cornerFeatures  //<2F>յ<EFBFBD>
			);
			cornerFeatures.push_back(line_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
	{
		//ˮƽɨ<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;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature(
				lineData,
				line,
				cornerPara,
				line_cornerFeatures  //<2F>յ<EFBFBD>
			);
			cornerFeatures.push_back(line_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 cornerTreeNum = (int)cornerGrowTrees.size();
	int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
	//<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
	for (int i = 0; i < cornerTreeNum; i++)
	{
		int nodeNum = (int)cornerGrowTrees[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == isHorizonScan)
			{
				lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x;
				ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y;
			}
			else
			{
				lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y;
				ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x;
			}
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
		}
	}
	for (int i = 0; i < raisedTreeNum; i++)
	{
		int nodeNum = (int)raisedFeatureGrowTrees[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == isHorizonScan)
			{
				lineIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx;
				for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++)
				{
					ptIdx = m;
					scanLines[lineIdx][ptIdx].nPointIdx = 2;
				}
			}
			else
			{
				ptIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx;
				for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++)
				{
					lineIdx = m;
					scanLines[lineIdx][ptIdx].nPointIdx = 2;
				}
			}
		}
	}
	return;
}
#endif