algoLib/sourceCode/sieveNodeDetection.cpp

#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "SG_sieveNodeDetection_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>

void sg_lineDataR(SVzNL3DLaserLine* a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT(a_line, camPoseR, groundH);
}

#if 0
//ɨ<><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void sg_sieveNodeDetection_lineProc(
	SVzNL3DLaserLine* a_line,
	int lineIdx,
	int* errCode,
	std::vector<std::vector< SSG_featureSemiCircle>>& all_vLineFeatures,
	std::vector<std::vector<int>>& noisePts,
	const SSG_sieveNodeDetectionParam sieveDetectParam)
{
	std::vector< SSG_featureSemiCircle> a_line_features;
	//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
	std::vector<SVzNL3DPosition> filterData;
	std::vector<int> lineNoisePts;
	sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, sieveDetectParam.filterParam, filterData, lineNoisePts);
	noisePts.push_back(lineNoisePts);

	sg_getLineUpperSemiCircleFeature(
		filterData.data(),
		filterData.size(),
		lineIdx,
		sieveDetectParam.sieveDiameter,
		sieveDetectParam.slopeParam,
		a_line_features);

	all_vLineFeatures.push_back(a_line_features);  //<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
	return;
}
#endif

int _checkFeatureSplit(
	SSG_featureSemiCircle& a_feaurue, 
	std::vector< SSG_featureSemiCircle>& chk_line_feature,
	double splitMinDist,
	double splitMaxDist)  //<2F>ڴ˾<DAB4><CBBE><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>Ч<EFBFBD>ֲ<EFBFBD>
{
	int split = -1;
	
	for (int i = 0, i_max = chk_line_feature.size(); i < i_max; i++)
	{
		if (i < i_max - 1)
		{
			if ((chk_line_feature[i].midPt.y < a_feaurue.midPt.y) && (chk_line_feature[i + 1].midPt.y > a_feaurue.midPt.y))
			{
				double dist_1 = abs(chk_line_feature[i].midPt.y - a_feaurue.midPt.y);
				double dist_2 = abs(chk_line_feature[i+1].midPt.y - a_feaurue.midPt.y);
				if ((dist_1 > splitMinDist) && (dist_1 < splitMaxDist) && (dist_2 > splitMinDist)&& (dist_2 < splitMaxDist))
				{
					split = i;
					break;
				}
			}
		}
	}
	return split;
}

bool compareByWidth(const SSG_featureSemiCircle& a, const SSG_featureSemiCircle& b) {
	return a.width < b.width;
}

SVzNL3DPoint _getMeanPt(
	std::vector<SSG_featureSemiCircle>& nodes, 
	SVzNL3DLaserLine* laser3DPoints)
{
	int nodeSize = nodes.size();
	int num = 0;
	SVzNL3DPoint meanPt = { 0.0,0.0,0.0 };
	for (int i = 0; i < nodeSize; i++)
	{
		SSG_featureSemiCircle& a_node = nodes[i];
		int lineIdx = a_node.lineIdx;
		for (int j = a_node.startPtIdx; j <= a_node.endPtIdx; j++)
		{
			if (laser3DPoints[lineIdx].p3DPosition[j].pt3D.z > 1e-4)
			{
				num++;
				meanPt.x += laser3DPoints[lineIdx].p3DPosition[j].pt3D.x;
				meanPt.y += laser3DPoints[lineIdx].p3DPosition[j].pt3D.y;
				meanPt.z += laser3DPoints[lineIdx].p3DPosition[j].pt3D.z;
			}
		}
	}
	if (num > 0)
	{
		meanPt.x = meanPt.x / num;
		meanPt.y = meanPt.y / num;
		meanPt.z = meanPt.z / num;
	}
	return meanPt;
}

SVzNL2DPoint _getNearestScanPt(
	std::vector<SSG_featureSemiCircle>& nodes,
	SVzNL3DLaserLine* laser3DPoints,
	SVzNL3DPoint objPt)
{
	int nodeSize = nodes.size();
	SVzNL2DPoint bestPos = { -1, -1 };
	double minDist = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		SSG_featureSemiCircle& a_node = nodes[i];
		int lineIdx = a_node.lineIdx;
		for (int j = a_node.startPtIdx; j <= a_node.endPtIdx; j++)
		{
			if (laser3DPoints[lineIdx].p3DPosition[j].pt3D.z > 1e-4)
			{
				SVzNL3DPoint& a_pt = laser3DPoints[lineIdx].p3DPosition[j].pt3D;
				double dist = sqrt(pow(a_pt.x - objPt.x, 2) + pow(a_pt.y - objPt.y, 2));
				if (minDist < 0)
				{
					minDist = dist;
					bestPos = { a_node.lineIdx, j };
				}
				else
				{
					if (minDist > dist)
					{
						minDist = dist;
						bestPos = { a_node.lineIdx, j };
					}
				}
			}
		}
	}
	return bestPos;
}


void sg_getSieveNodes(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SSG_sieveNodeDetectionParam sieveDetectParam,
	std::vector<SVzNL3DPoint>& nodePos)
{
	const int hole_max_ptSize = 20;
	int errCode = 0;
	for (int i = 0; i < lineNum; i++)
	{
		if (i == 19)
			int kkk = 1;

		sg_lineDataRemoveOutlier_changeOriginData(
			laser3DPoints[i].p3DPosition, 
			laser3DPoints[i].nPositionCnt, 
			sieveDetectParam.filterParam);

		//<2F><>nPointIdxת<78><D7AA>ʹ<EFBFBD><CAB9>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
			laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
	}
	//<2F>׶<EFBFBD><D7B6><EFBFBD><EFBFBD>⣬<EFBFBD><E2A3AC>С<EFBFBD>Ŀ׶<C4BF><D7B6><EFBFBD>Ҫ<EFBFBD>ϲ<EFBFBD>
	int maskY = laser3DPoints[0].nPositionCnt;
	int maskX = lineNum;
	//<2F><><EFBFBD>ɿ׶<C9BF><D7B6><EFBFBD>עMask<73><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ע
	cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols
	for (int i = 0; i < lineNum; i++)
	{
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
		{
			if(laser3DPoints[i].p3DPosition[j].pt3D.z < 1e-4)
				bwImg.at<uchar>(j, i) = 1;
		}
	}
	//<2F>׶<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ע
	cv::Mat labImg;
	std::vector<SSG_Region> labelRgns;
	SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
	//<2F><><EFBFBD>׶<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>ʶ
	cv::Mat holeMask = cv::Mat::zeros(maskY, maskX, CV_32SC1);//rows, cols
	for (int i = 0, i_max = labelRgns.size(); i < i_max; i++)
	{
		int rgnID = labelRgns[i].labelID;
		if (labelRgns[i].ptCounter < hole_max_ptSize)  //<2F>׶<EFBFBD>
		{
			for (int m = labelRgns[i].roi.left; m <= labelRgns[i].roi.right; m++)
			{
				for (int n = labelRgns[i].roi.top; n <= labelRgns[i].roi.bottom; n++)
				{
					if (rgnID == labImg.at<int>(n, m))
						holeMask.at<int>(n, m) = rgnID;
				}
			}
		}
	}
#if _OUTPUT_LINE_PROC_RESULT
	cv::Mat holeMaskImage;
	cv::normalize(holeMask, holeMaskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("holeMask.png", holeMaskImage);
#endif

	std::vector<std::vector< SSG_featureSemiCircle>> all_vLineFeatures;
	for (int i = 0; i < lineNum; i++)
	{
		std::vector< SSG_featureSemiCircle> a_line_features;
		sg_getLineUpperSemiCircleFeature(
			laser3DPoints[i].p3DPosition,
			laser3DPoints[i].nPositionCnt,
			i,
			sieveDetectParam.sieveDiameter,
			sieveDetectParam.slopeParam,
			a_line_features,
			holeMask);
		//<2F><>nPointIdxת<78><D7AA>ʹ<EFBFBD><CAB9>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
			laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
		all_vLineFeatures.push_back(a_line_features);  //<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
	}

	//<2F><><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>feature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>feature<72><65><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱<EFBFBD><DFB1>ϲ<EFBFBD><CFB2><EFBFBD>һ<EFBFBD><D2BB>featureʱ<65><CAB1>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>feature<72><65><EFBFBD><EFBFBD>Чfeature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>feature<72><65>Ϊ<EFBFBD><CEAA>Чfeature
	for (int i = 0; i < lineNum; i++)
	{
		//<2F><>ǰһ<C7B0><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱Ƚ<DFB1>,Ѱ<>ҿ<EFBFBD>ʼ
		std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
		if (i > 0)
		{
			std::vector< SSG_featureSemiCircle>& pre_line_features = all_vLineFeatures[i-1];
			for (int j = 0, j_max = line_features.size(); j < j_max; j++)
			{
				int split = _checkFeatureSplit(line_features[j], pre_line_features, sieveDetectParam.sieveDiameter/2, sieveDetectParam.sieveHoleSize);
				if (split >= 0)
				{
					pre_line_features[split].flag = FEATURE_FLAG_INVLD_END;
					pre_line_features[split + 1].flag = FEATURE_FLAG_INVLD_END;
					line_features[j].flag = FEATURE_FLAG_VALID_START;
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱Ƚ<DFB1>,Ѱ<>ҽ<EFBFBD><D2BD><EFBFBD>
		if (i < lineNum - 1)
		{
			std::vector< SSG_featureSemiCircle>& post_line_features = all_vLineFeatures[i + 1];
			for (int j = 0, j_max = line_features.size(); j < j_max; j++)
			{
				int split = _checkFeatureSplit(line_features[j], post_line_features, sieveDetectParam.sieveDiameter/2, sieveDetectParam.sieveHoleSize);
				if (split >= 0)
				{
					post_line_features[split].flag = FEATURE_FLAG_INVLD_START;
					post_line_features[split + 1].flag = FEATURE_FLAG_INVLD_START;
					line_features[j].flag = FEATURE_FLAG_VALID_END;
				}
			}
		}
	}

	//feature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чfeature<72><65><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㡣<EFBFBD><E3A1A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>feature<72><65>Ϊ<EFBFBD><CEAA>Чfeature
	std::vector<SSG_semiCircleFeatureTree> trees;
	std::vector<SSG_semiCircleFeatureTree> stopTrees;  //ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector<SSG_semiCircleFeatureTree> invalidTrees;  //<2F><><EFBFBD>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Щ<EFBFBD><D0A9><EFBFBD><EFBFBD><EFBFBD>ܽ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ֳɶ<D6B3><C9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӷ<EFBFBD><D3B6><EFBFBD><EFBFBD>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	for (int i = 0; i < lineNum; i++)
	{
		//<2F><>ǰһ<C7B0><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱Ƚ<DFB1>,Ѱ<>ҿ<EFBFBD>ʼ
		std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
		sg_getFeatureGrowingTrees_semiCircle(
			line_features,
			i,
			lineNum,
			trees,
			stopTrees,
			invalidTrees,
			sieveDetectParam.growParam);
	}
	//<2F><>ȷȷ<C8B7><C8B7><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		//<2F><><EFBFBD>Ӷ<EFBFBD>ΪΪ<CEAA><CEAA><EFBFBD>е<EFBFBD><D0B5>ģ<EFBFBD>x,y,z)<29><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD>
		SSG_semiCircleFeatureTree& a_tree = stopTrees[i];
		a_tree.centerPt = _getMeanPt(a_tree.treeNodes, laser3DPoints);
		a_tree.centerPos = _getNearestScanPt(a_tree.treeNodes, laser3DPoints, a_tree.centerPt);
		//<2F>жϺ<D0B6><CFBA><EFBFBD><EFBFBD>Ƿ񱻺<C7B7><F1B1BBBA>ӹ<EFBFBD><D3B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD>ĸ߶<C4B8><DFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5>м<EFBFBD><D0BC>߶ȱȽ<C8B1>


	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 
	// (1)<29><><EFBFBD><EFBFBD>2D<32><44><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//<2F><>2<EFBFBD><32><EFBFBD><EFBFBD>
	std::vector<std::vector<SSG_semiCircleFeatureTree>> sortedTrees;
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		//if(stopTrees[i].)
	}

	//<2F><>©<EFBFBD><C2A9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

#if _OUTPUT_LINE_PROC_RESULT
	//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		std::vector< SSG_featureSemiCircle>& a_tree_features = stopTrees[i].treeNodes;
		for (int j = 0, j_max = a_tree_features.size(); j < j_max; j++)
		{
			SSG_featureSemiCircle& a_feature = a_tree_features[j];
			for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
				laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 1; //<2F>˴<EFBFBD>nPointIdxת<78><D7AA>

			laser3DPoints[a_feature.lineIdx].p3DPosition[a_feature.midPtIdx].nPointIdx = 2;
			if(stopTrees[i].treeType == 0)
				laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 3;
			else
				laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 4;
				
		}
	}
#endif


}

//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROI<4F><49><EFBFBD>ڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><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 sg_getSieveBaseCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	std::vector<SVzNL3DRangeD>& ROIs)
{
	return sg_getPlaneCalibPara_ROIs(laser3DPoints, lineNum, ROIs);
}