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
//线头位置检测定位
void wd_bagThreadPositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_outlierFilterParam filterParam, //噪点过滤参数
	const SSG_cornerParam cornerPara, //V型特征参数
	const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
	const SSG_treeGrowParam growParam, //特征生长参数
	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();
	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//生成水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> data_lines_h;  //水平扫描数据
	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; //将原始数据的序列清0（会转义使用）
			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)
	{
		//垂直扫描检测V型槽和线尾
		for (int line = 0; line < lineNum; line++)
		{
			if ((line == 577) ||  (line == 932))
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
			//提取V型槽
			std::vector<SSG_basicFeature1D> line_cornerFeatures;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature(
				lineData,
				line,
				cornerPara,
				line_cornerFeatures  //拐点
			);
			cornerFeatures.push_back(line_cornerFeatures);
			//提取凸起段
			std::vector<SWD_segFeature> line_raisedFeatures;
			wd_getLineRaisedFeature(
				lineData,
				line,
				raisedFeaturePara, //凸起参数
				line_raisedFeatures  //凸起
			);
			raisedFeatures.push_back(line_raisedFeatures);
		}
#if 0
		//水平扫描检测袋子边
		std::vector<std::vector<SSG_basicFeature1D>> jumpdFeatures;
#endif
	}
	else
	{
		//水平扫描检测V型槽和线尾
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 329)
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
			//提取V型槽
			std::vector<SSG_basicFeature1D> line_cornerFeatures;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature(
				lineData,
				line,
				cornerPara,
				line_cornerFeatures  //拐点
			);
			cornerFeatures.push_back(line_cornerFeatures);
			//提取凸起段
			std::vector<SWD_segFeature> line_raisedFeatures;
			wd_getLineRaisedFeature(
				lineData,
				line,
				raisedFeaturePara, //凸起参数
				line_raisedFeatures  //凸起
			);
			raisedFeatures.push_back(line_raisedFeatures);
		}
		//垂直扫描检测袋子边
	}

	//特征生长
	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();
	//显示 
	//将原始数据的序列清0，转义使用
	for (int line = 0; line < lineNum; line++)
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	//置标志，用于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