336 lines
9.7 KiB
C++
336 lines
9.7 KiB
C++
#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
|
||
//扫描线处理，进行垂直方向的特征提取和生长
|
||
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;
|
||
//滤波，滤除异常点
|
||
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); //空行也加入，保证能按行号索引
|
||
return;
|
||
}
|
||
#endif
|
||
|
||
int _checkFeatureSplit(
|
||
SSG_featureSemiCircle& a_feaurue,
|
||
std::vector< SSG_featureSemiCircle>& chk_line_feature,
|
||
double splitMinDist,
|
||
double splitMaxDist) //在此距离内为有效分叉
|
||
{
|
||
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);
|
||
|
||
//将nPointIdx转义使用前清零
|
||
for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
|
||
laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
|
||
}
|
||
//孔洞检测，对小的孔洞需要合并
|
||
int maskY = laser3DPoints[0].nPositionCnt;
|
||
int maskX = lineNum;
|
||
//生成孔洞标注Mask，进行目标标注
|
||
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;
|
||
}
|
||
}
|
||
//孔洞目标标注
|
||
cv::Mat labImg;
|
||
std::vector<SSG_Region> labelRgns;
|
||
SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
|
||
//将孔洞目标进行标识
|
||
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) //孔洞
|
||
{
|
||
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);
|
||
//将nPointIdx转义使用前清零
|
||
for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
|
||
laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
|
||
all_vLineFeatures.push_back(a_line_features); //空行也加入，保证能按行号索引
|
||
}
|
||
|
||
//根据筛网的特点去除无效的feature。当上下两个feature在下一条扫描线被合并成一个feature时，说明上一条扫描线的两个feature是无效feature。其相邻的feature均为无效feature
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
//与前一条扫描线比较,寻找开始
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
//与后一条扫描线比较,寻找结束
|
||
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生长。碰到无效feature生长停止。无效生长可以作为生长起点。其生长树上的所有feature均为无效feature
|
||
std::vector<SSG_semiCircleFeatureTree> trees;
|
||
std::vector<SSG_semiCircleFeatureTree> stopTrees; //停止生长的树
|
||
std::vector<SSG_semiCircleFeatureTree> invalidTrees; //被移除的树，这些树可能将目标分成多个树，从而被移除。需要保存下来迭代分析
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
//与前一条扫描线比较,寻找开始
|
||
std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
|
||
sg_getFeatureGrowingTrees_semiCircle(
|
||
line_features,
|
||
i,
|
||
lineNum,
|
||
trees,
|
||
stopTrees,
|
||
invalidTrees,
|
||
sieveDetectParam.growParam);
|
||
}
|
||
//精确确定焊接点
|
||
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
||
{
|
||
//焊接定为为所有点的（x,y,z)的平均处（质心）。
|
||
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);
|
||
//判断焊点是否被焊接过：以中间点的高度与左右两边的中间高度比较
|
||
|
||
|
||
}
|
||
|
||
//按行排序
|
||
// (1)建立2D索引实现有序搜索
|
||
//（2）从
|
||
std::vector<std::vector<SSG_semiCircleFeatureTree>> sortedTrees;
|
||
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
||
{
|
||
//if(stopTrees[i].)
|
||
}
|
||
|
||
//遗漏点检查
|
||
|
||
//迭代生长
|
||
|
||
#if _OUTPUT_LINE_PROC_RESULT
|
||
//输出扫描线处理结果
|
||
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; //此处nPointIdx转义
|
||
|
||
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
|
||
|
||
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara sg_getSieveBaseCalibPara(
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
int lineNum,
|
||
std::vector<SVzNL3DRangeD>& ROIs)
|
||
{
|
||
return sg_getPlaneCalibPara_ROIs(laser3DPoints, lineNum, ROIs);
|
||
} |