1284 lines
42 KiB
C++
1284 lines
42 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "beltTearingDetection_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include "SG_errCode.h"
|
||
|
||
|
||
typedef struct
|
||
{
|
||
int tearID;
|
||
int vGap_num;
|
||
double vGap_depth_max;
|
||
double vGap_width_max;
|
||
double vGap_width_sum; //用于计算均值
|
||
int hGap_num;
|
||
double hGap_depth_max;
|
||
double hGap_width_max; //最大值为撕裂最宽处
|
||
double hGap_width_sum; //用于计算均值
|
||
|
||
}SSG_tearProcessInfo; //撕裂处理过程数据
|
||
|
||
double _getGapDepth(std::vector<SVzNL3DPosition>& vldPts, int start, int end)
|
||
{
|
||
double start_z = vldPts[start].pt3D.z;
|
||
double end_z = vldPts[end].pt3D.z;
|
||
double max_z = start_z;
|
||
for (int i = start+1; i <= end; i++)
|
||
{
|
||
if (max_z < vldPts[i].pt3D.z)
|
||
max_z = vldPts[i].pt3D.z;
|
||
}
|
||
double depth = max_z - (start_z + end_z) / 2;
|
||
return depth;
|
||
}
|
||
|
||
void _combineROI(SSG_ROIRectD* roi, SSG_ROIRectD* sub_roi)
|
||
{
|
||
if (roi->left > sub_roi->left)
|
||
roi->left = sub_roi->left;
|
||
if (roi->right < sub_roi->right)
|
||
roi->right = sub_roi->right;
|
||
if (roi->top > sub_roi->top)
|
||
roi->top = sub_roi->top;
|
||
if (roi->bottom < sub_roi->bottom)
|
||
roi->bottom = sub_roi->bottom;
|
||
return;
|
||
}
|
||
|
||
bool compareByStartY(const SSG_basicFeatureGap& a, const SSG_basicFeatureGap& b) {
|
||
return a.gap_start < b.gap_start;
|
||
}
|
||
//垂直扫描线处理，进行垂直方向的特征提取
|
||
//垂直扫描线：点间距小于1mm，使用差分公式。
|
||
void sg_bearTearing_vLineProc(
|
||
SVzNL3DLaserLine* a_line, //当前扫描线
|
||
int lineIdx, //当前扫描线序号
|
||
const SSG_beltTearingParam measureParam,
|
||
std::vector<SSG_basicFeatureGap>& vLineFeatures)
|
||
{
|
||
//double minGapW = 1.0;
|
||
|
||
//从栅格数据生成非零序列
|
||
//统计Gap
|
||
std::vector<SVzNL3DPosition> vldPts;
|
||
std::vector<SSG_RUN> gaps;
|
||
int preIdx = -1;
|
||
int vldPtSize = 0;
|
||
int gapSize = 0;
|
||
for (int i = 0; i < a_line->nPositionCnt; i++)
|
||
{
|
||
SVzNL3DPosition a_pt = a_line->p3DPosition[i];
|
||
if (a_pt.pt3D.z > 1e-4)
|
||
{
|
||
a_pt.nPointIdx = -1;
|
||
if (preIdx >= 0)
|
||
{
|
||
int gapPtNum = i - preIdx - 1;
|
||
double gapW = abs(a_pt.pt3D.y - vldPts[vldPtSize - 1].pt3D.y);
|
||
if ((gapPtNum >= 2) && (gapW > 1.0))
|
||
{
|
||
SSG_RUN a_gap = { vldPtSize-1, 2, 0};//vldGap
|
||
gaps.push_back(a_gap);
|
||
vldPts[vldPtSize - 1].nPointIdx = gapSize;
|
||
a_pt.nPointIdx = gapSize;
|
||
gapSize++;
|
||
}
|
||
}
|
||
vldPts.push_back(a_pt);
|
||
vldPtSize++;
|
||
preIdx = i;
|
||
}
|
||
}
|
||
|
||
//计算一阶差分值
|
||
std::vector<double> difference;
|
||
std::vector<int> diffBinValue;
|
||
difference.resize(vldPts.size());
|
||
diffBinValue.resize(vldPts.size());
|
||
for (int i = 1, i_max = vldPts.size(); i < i_max; i++)
|
||
{
|
||
double a_difference = (vldPts[i].pt3D.z - vldPts[i-1].pt3D.z) / (vldPts[i].pt3D.y - vldPts[i - 1].pt3D.y);
|
||
difference[i] = a_difference;
|
||
int a_binValue;
|
||
a_binValue = a_difference > measureParam.differnceBinTh ? 1 : (a_difference < -measureParam.differnceBinTh ? -1 : 0);
|
||
diffBinValue[i] = a_binValue;
|
||
if (1 == i) //第一个
|
||
{
|
||
difference[0] = a_difference;
|
||
diffBinValue[0] = a_binValue;
|
||
}
|
||
}
|
||
|
||
//检测+1序列和-1序列
|
||
std::vector< SSG_RUN> runs;
|
||
SSG_RUN a_run;
|
||
memset(&a_run, 0, sizeof(SSG_RUN));
|
||
for (int i = 1, i_max = vldPts.size(); i < i_max; i++)
|
||
{
|
||
if (0 != diffBinValue[i])
|
||
{
|
||
if (0 == a_run.value)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = diffBinValue[i];
|
||
}
|
||
else
|
||
{
|
||
if (a_run.value != diffBinValue[i])
|
||
{
|
||
runs.push_back(a_run);
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = diffBinValue[i];
|
||
}
|
||
else
|
||
{
|
||
a_run.len++;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (0 != a_run.value)
|
||
{
|
||
runs.push_back(a_run);
|
||
memset(&a_run, 0, sizeof(SSG_RUN));
|
||
}
|
||
}
|
||
}
|
||
if (0 != a_run.value) //最后一个
|
||
{
|
||
runs.push_back(a_run);
|
||
}
|
||
|
||
//配对生成特征
|
||
for (int i = 0, i_max = runs.size(); i < i_max; i++)
|
||
{
|
||
SSG_RUN a_run = runs[i];
|
||
if (0 == a_run.value)
|
||
continue;
|
||
|
||
if (i < i_max-1)
|
||
{
|
||
SSG_RUN* nxt_run = &runs[i + 1];
|
||
//检查是否可以配对
|
||
if (a_run.value != nxt_run->value)
|
||
{
|
||
//检查距离
|
||
double pos_end = vldPts[a_run.start + a_run.len - 1].pt3D.y;
|
||
double nxt_pos_start = vldPts[nxt_run->start].pt3D.y;
|
||
double dist = nxt_pos_start - pos_end;
|
||
if (dist < measureParam.extractPara.sameGapTh)
|
||
{
|
||
a_run.len = nxt_run->start + nxt_run->len - a_run.start;
|
||
nxt_run->value = 0; //invalid
|
||
}
|
||
}
|
||
}
|
||
//检查前后是否有Gap，精确确定起始点
|
||
//在前后检查10个点，看是否有Gap
|
||
int start = a_run.start;
|
||
int end = a_run.start + a_run.len - 1;
|
||
bool hasGap = false;
|
||
for (int m = 0; m < measureParam.extractPara.gapChkWin; m++)
|
||
{
|
||
int ptIdx = a_run.start - m;
|
||
if (ptIdx > 0)
|
||
{
|
||
double gapLen = vldPts[ptIdx].pt3D.y - vldPts[ptIdx-1].pt3D.y;
|
||
if (gapLen > measureParam.scanYScale * 2) //找到Gap
|
||
{
|
||
start = ptIdx - 1;
|
||
hasGap = true;
|
||
if (vldPts[ptIdx].nPointIdx >= 0)
|
||
gaps[vldPts[ptIdx].nPointIdx].value = 1; //invalid
|
||
if(vldPts[ptIdx - 1].nPointIdx >= 0)
|
||
gaps[vldPts[ptIdx - 1].nPointIdx].value = 1; //invalid
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
int vldPtSize = vldPts.size();
|
||
for (int m = 0; m < measureParam.extractPara.gapChkWin; m++)
|
||
{
|
||
int ptIdx = a_run.start + a_run.len - 1 + m;
|
||
if (ptIdx < vldPtSize-1)
|
||
{
|
||
double gapLen = vldPts[ptIdx+1].pt3D.y - vldPts[ptIdx ].pt3D.y;
|
||
if (gapLen > measureParam.scanYScale * 2) //找到Gap
|
||
{
|
||
end = ptIdx + 1;
|
||
hasGap = true;
|
||
if (vldPts[ptIdx].nPointIdx >= 0)
|
||
gaps[vldPts[ptIdx].nPointIdx].value = 1; //invalid
|
||
if (vldPts[ptIdx + 1].nPointIdx >= 0)
|
||
gaps[vldPts[ptIdx + 1].nPointIdx].value = 1; //invalid
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (end > start) //gap必须大于0
|
||
{
|
||
//生成特征
|
||
SSG_basicFeatureGap a_feature;
|
||
memset(&a_feature, 0, sizeof(SSG_basicFeatureGap));
|
||
a_feature.lineIdx = lineIdx;
|
||
a_feature.gap_start = vldPts[start].pt3D.y;
|
||
a_feature.gap_width = vldPts[end].pt3D.y - vldPts[start].pt3D.y;
|
||
a_feature.gapPt_0 = vldPts[start];
|
||
a_feature.gapPt_1 = vldPts[end];
|
||
double x1, x2;
|
||
if (vldPts[end].pt3D.x > vldPts[start].pt3D.x)
|
||
{
|
||
x1 = vldPts[start].pt3D.x;
|
||
x2 = vldPts[end].pt3D.x;
|
||
}
|
||
else
|
||
{
|
||
x1 = vldPts[end].pt3D.x;
|
||
x2 = vldPts[start].pt3D.x;
|
||
}
|
||
a_feature.roi = { x1, x2, vldPts[start].pt3D.y, vldPts[end].pt3D.y };
|
||
if (true == hasGap)
|
||
a_feature.gap_depth = -1; //穿透型撕裂
|
||
else
|
||
a_feature.gap_depth = _getGapDepth(vldPts, start, end);
|
||
//检查与前一个feature是否交叉或相连
|
||
if (vLineFeatures.size() > 0)
|
||
{
|
||
SSG_basicFeatureGap* pre_feature = &vLineFeatures[vLineFeatures.size() - 1];
|
||
if ((a_feature.roi.bottom > pre_feature->roi.top) && (pre_feature->roi.bottom > a_feature.roi.top)) //合并
|
||
{
|
||
//合并
|
||
pre_feature->gapPt_1 = a_feature.gapPt_1;
|
||
_combineROI(&pre_feature->roi, &a_feature.roi);
|
||
if ((a_feature.gap_depth < 0) || (pre_feature->gap_depth < 0))
|
||
pre_feature->gap_depth = -1;
|
||
else
|
||
pre_feature->gap_depth = pre_feature->gap_depth < a_feature.gap_depth ? a_feature.gap_depth : pre_feature->gap_depth;
|
||
pre_feature->gap_width = pre_feature->roi.bottom - pre_feature->roi.top;
|
||
}
|
||
else
|
||
{
|
||
double dist_y = a_feature.roi.top - pre_feature->roi.bottom;
|
||
if (dist_y < measureParam.scanYScale * 3)
|
||
{
|
||
//合并
|
||
pre_feature->gapPt_1 = a_feature.gapPt_1;
|
||
_combineROI(&pre_feature->roi, &a_feature.roi);
|
||
if ((a_feature.gap_depth < 0) || (pre_feature->gap_depth < 0))
|
||
pre_feature->gap_depth = -1;
|
||
else
|
||
pre_feature->gap_depth = pre_feature->gap_depth < a_feature.gap_depth ? a_feature.gap_depth : pre_feature->gap_depth;
|
||
pre_feature->gap_width = pre_feature->roi.bottom - pre_feature->roi.top;
|
||
}
|
||
else
|
||
vLineFeatures.push_back(a_feature);
|
||
}
|
||
}
|
||
else //if(a_gap.gap_width > minGapW) //小于minGapW的视为噪声
|
||
vLineFeatures.push_back(a_feature);
|
||
}
|
||
}
|
||
#if KEEP_GAP_AS_FEATURE
|
||
//将Gap生成feature
|
||
int gapFeatureNum = 0;
|
||
for (int i = 0, i_max = gaps.size(); i < i_max; i++)
|
||
{
|
||
SSG_RUN* a_run = &gaps[i];
|
||
if (0 == a_run->value) //valid
|
||
{
|
||
int start = a_run->start;
|
||
int end = a_run->start + a_run->len - 1;
|
||
SSG_basicFeatureGap a_gap;
|
||
memset(&a_gap, 0, sizeof(SSG_basicFeatureGap));
|
||
a_gap.lineIdx = lineIdx;
|
||
a_gap.gap_start = vldPts[start].pt3D.y;
|
||
a_gap.gap_width = vldPts[end].pt3D.y - vldPts[start].pt3D.y;
|
||
a_gap.gapPt_0 = vldPts[start];
|
||
a_gap.gapPt_1 = vldPts[end];
|
||
double x1, x2;
|
||
if (vldPts[end].pt3D.x > vldPts[start].pt3D.x)
|
||
{
|
||
x1 = vldPts[start].pt3D.x;
|
||
x2 = vldPts[end].pt3D.x;
|
||
}
|
||
else
|
||
{
|
||
x1 = vldPts[end].pt3D.x;
|
||
x2 = vldPts[start].pt3D.x;
|
||
}
|
||
a_gap.roi = { x1, x2, vldPts[start].pt3D.y, vldPts[end].pt3D.y };
|
||
a_gap.gap_depth = -1; //穿透型撕裂
|
||
vLineFeatures.push_back(a_gap);
|
||
gapFeatureNum++;
|
||
}
|
||
}
|
||
//排序
|
||
if (gapFeatureNum > 0)
|
||
{
|
||
std::sort(vLineFeatures.begin(), vLineFeatures.end(), compareByStartY);
|
||
}
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
//水平扫描线处理，进行垂直方向的特征提取和生长
|
||
//水平扫描线：若点间距小于1mm，使用差分公式；反之直接使用前后点差值计算。
|
||
void sg_bearTearing_hLineProc(
|
||
SSG_hLineProInfo* hLineProc,
|
||
int workerIdx,
|
||
int hLineIdx, //水平扫描点序号，对应垂直扫描线的线序号
|
||
SVzNL3DLaserLine** buffLines,
|
||
const SSG_beltTearingParam measureParam,
|
||
std::vector<SSG_basicFeatureGap>& hLineFeatures )
|
||
{
|
||
//注意：进行处理时，buffer已经满，不需要考虑buffer不满的情况
|
||
//处理中间扫描线数据，这样能够检测后面的跳变。整体处理延后（SCAN_BUFF_SIZE / 2）条扫描线
|
||
int midPos = SCAN_BUFF_SIZE / 2;
|
||
SVzNL3DPosition* procData = &(buffLines[midPos]->p3DPosition[workerIdx]);
|
||
|
||
if (procData->pt3D.z < 1e-4)
|
||
return;
|
||
|
||
if (hLineProc->preVldData.pt3D.z < 1e-4)
|
||
{
|
||
hLineProc->preVldData = *procData;
|
||
return;
|
||
}
|
||
|
||
//计算差值
|
||
double x_diff = procData->pt3D.x - hLineProc->preVldData.pt3D.x;
|
||
double a_difference;
|
||
int a_binValue;
|
||
if (x_diff > measureParam.tearingMinGap)
|
||
{
|
||
hLineProc->gapPos.nPointIdx = -1;
|
||
hLineProc->hLineSM = keSG_HLineProc_Init;
|
||
memset(&hLineProc->firstHalf, 0, sizeof(SSG_RUN));
|
||
memset(&hLineProc->firstHalf_startPt, 0, sizeof(SVzNL3DPosition));
|
||
memset(&hLineProc->firstHalf_endPt, 0, sizeof(SVzNL3DPosition));
|
||
a_difference = 0.0;
|
||
a_binValue = 0;
|
||
}
|
||
else
|
||
{
|
||
//检查Gap
|
||
if (x_diff > measureParam.scanXScale * 2) //找到Gap
|
||
{
|
||
hLineProc->gapPos = hLineProc->preVldData;
|
||
}
|
||
a_difference = procData->pt3D.z - hLineProc->preVldData.pt3D.z;
|
||
if (measureParam.scanXScale < 1.0)
|
||
a_difference = a_difference / x_diff;
|
||
a_binValue = a_difference > measureParam.differnceBinTh ? 1 : (a_difference < -measureParam.differnceBinTh ? -1 : 0);
|
||
}
|
||
switch (hLineProc->hLineSM)
|
||
{
|
||
case keSG_HLineProc_Init:
|
||
if (a_binValue != 0)
|
||
{
|
||
hLineProc->gapMaxZ = procData->pt3D.z;
|
||
hLineProc->firstHalf.start = procData->nPointIdx;
|
||
hLineProc->firstHalf.len = 1;
|
||
hLineProc->firstHalf.value = a_binValue;
|
||
hLineProc->firstHalf_startPt = *procData;
|
||
hLineProc->firstHalf_endPt = *procData;
|
||
hLineProc->hLineSM = keSG_HLineProc_First_Half;
|
||
}
|
||
break;
|
||
case keSG_HLineProc_First_Half:
|
||
if(hLineProc->gapMaxZ < procData->pt3D.z)
|
||
hLineProc->gapMaxZ = procData->pt3D.z;
|
||
|
||
if (0 == a_binValue)
|
||
{
|
||
//进行wait状态
|
||
hLineProc->hLineSM = keSG_HLineProc_Wait_Second;
|
||
}
|
||
else if (hLineProc->firstHalf.value != a_binValue)
|
||
{
|
||
hLineProc->secondHalf.start = procData->nPointIdx;
|
||
hLineProc->secondHalf.len = 1;
|
||
hLineProc->secondHalf.value = a_binValue;
|
||
hLineProc->secondHalf_startPt = *procData;
|
||
hLineProc->secondHalf_endPt = *procData;
|
||
hLineProc->hLineSM = keSG_HLineProc_Second_Half;
|
||
}
|
||
else
|
||
{
|
||
hLineProc->firstHalf.len++;
|
||
hLineProc->firstHalf_endPt = *procData;
|
||
}
|
||
break;
|
||
case keSG_HLineProc_Wait_Second:
|
||
if (hLineProc->gapMaxZ < procData->pt3D.z)
|
||
hLineProc->gapMaxZ = procData->pt3D.z;
|
||
|
||
if (0 == a_binValue)
|
||
{
|
||
double x_diff = abs(procData->pt3D.x - hLineProc->firstHalf_endPt.pt3D.x);
|
||
if (x_diff > measureParam.extractPara.sameGapTh)
|
||
{
|
||
memset(&hLineProc->firstHalf, 0, sizeof(SSG_RUN));
|
||
memset(&hLineProc->firstHalf_startPt, 0, sizeof(SVzNL3DPosition));
|
||
memset(&hLineProc->firstHalf_endPt, 0, sizeof(SVzNL3DPosition));
|
||
hLineProc->hLineSM = keSG_HLineProc_Init;
|
||
}
|
||
}
|
||
else if (hLineProc->firstHalf.value != a_binValue)
|
||
{
|
||
hLineProc->secondHalf.start = procData->nPointIdx;
|
||
hLineProc->secondHalf.len = 1;
|
||
hLineProc->secondHalf.value = a_binValue;
|
||
hLineProc->secondHalf_startPt = *procData;
|
||
hLineProc->secondHalf_endPt = *procData;
|
||
hLineProc->hLineSM = keSG_HLineProc_Second_Half;
|
||
}
|
||
else
|
||
{
|
||
if (x_diff > measureParam.extractPara.sameGapTh)
|
||
{
|
||
//firstHalf重新开始
|
||
hLineProc->firstHalf.start = procData->nPointIdx;
|
||
hLineProc->firstHalf.len = 1;
|
||
hLineProc->firstHalf.value = a_binValue;
|
||
hLineProc->firstHalf_startPt = *procData;
|
||
hLineProc->firstHalf_endPt = *procData;
|
||
}
|
||
else
|
||
{
|
||
//firstHalf的延续
|
||
hLineProc->firstHalf.len = procData->nPointIdx - hLineProc->firstHalf_startPt.nPointIdx + 1;
|
||
hLineProc->firstHalf_endPt = *procData;
|
||
}
|
||
hLineProc->hLineSM = keSG_HLineProc_First_Half;
|
||
}
|
||
break;
|
||
case keSG_HLineProc_Second_Half:
|
||
if (0 == a_binValue)
|
||
{
|
||
//匹配
|
||
//生成特征
|
||
SSG_basicFeatureGap a_gap;
|
||
memset(&a_gap, 0, sizeof(SSG_basicFeatureGap));
|
||
a_gap.lineIdx = hLineIdx;
|
||
//向前检查Gap
|
||
SVzNL3DPosition start;
|
||
int x_lineDiff = hLineProc->firstHalf_startPt.nPointIdx - hLineProc->gapPos.nPointIdx;
|
||
if ((x_lineDiff > 0) && (x_lineDiff < measureParam.extractPara.gapChkWin))
|
||
{
|
||
a_gap.gap_start = hLineProc->gapPos.pt3D.x;
|
||
start = hLineProc->gapPos;
|
||
}
|
||
else
|
||
{
|
||
a_gap.gap_start = hLineProc->firstHalf_startPt.pt3D.x;
|
||
start = hLineProc->firstHalf_startPt;
|
||
}
|
||
//向后检查Gap
|
||
bool hasPostGap = false;
|
||
SVzNL3DPosition postGap;
|
||
memset(&postGap, 0, sizeof(SVzNL3DPosition));
|
||
int chkLen = measureParam.extractPara.gapChkWin;
|
||
if (chkLen > SCAN_BUFF_SIZE / 2)
|
||
chkLen = SCAN_BUFF_SIZE / 2;
|
||
|
||
SVzNL3DPosition preData = *procData;
|
||
for (int m = 1; m <= chkLen; m++)
|
||
{
|
||
int lineIdx = midPos + m;
|
||
if (buffLines[lineIdx]->p3DPosition[workerIdx].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
double x_dff_2 = buffLines[lineIdx]->p3DPosition[workerIdx].pt3D.x - preData.pt3D.x;
|
||
if (x_dff_2 > measureParam.scanXScale * 2) //找到Gap
|
||
{
|
||
hasPostGap = true;
|
||
postGap = buffLines[lineIdx]->p3DPosition[workerIdx];
|
||
break;
|
||
}
|
||
preData = buffLines[lineIdx]->p3DPosition[workerIdx];
|
||
}
|
||
|
||
SVzNL3DPosition end;
|
||
if (true == hasPostGap)
|
||
{
|
||
end = postGap;
|
||
a_gap.gap_width = postGap.pt3D.x - a_gap.gap_start;
|
||
a_gap.gap_depth = -1;//穿透型撕裂
|
||
double y1, y2;
|
||
if (start.pt3D.y < end.pt3D.y)
|
||
{
|
||
y1 = start.pt3D.y;
|
||
y2 = end.pt3D.y;
|
||
}
|
||
else
|
||
{
|
||
y1 = end.pt3D.y;
|
||
y2 = start.pt3D.y;
|
||
}
|
||
a_gap.roi = { a_gap.gap_start, postGap.pt3D.x, y1, y2};
|
||
a_gap.gapPt_0 = start;
|
||
a_gap.gapPt_1 = end;
|
||
}
|
||
else
|
||
{
|
||
end = hLineProc->secondHalf_endPt;
|
||
a_gap.gap_width = hLineProc->secondHalf_endPt.pt3D.x - a_gap.gap_start;
|
||
a_gap.gap_depth = hLineProc->gapMaxZ - (start.pt3D.z + end.pt3D.z) / 2;
|
||
double y1, y2;
|
||
if (start.pt3D.y < end.pt3D.y)
|
||
{
|
||
y1 = start.pt3D.y;
|
||
y2 = end.pt3D.y;
|
||
}
|
||
else
|
||
{
|
||
y1 = end.pt3D.y;
|
||
y2 = start.pt3D.y;
|
||
}
|
||
a_gap.roi = { a_gap.gap_start, hLineProc->secondHalf_endPt.pt3D.x, y1, y2 };
|
||
a_gap.gapPt_0 = start;
|
||
a_gap.gapPt_1 = end;
|
||
}
|
||
hLineFeatures.push_back(a_gap);
|
||
|
||
memset(&hLineProc->firstHalf, 0, sizeof(SSG_RUN));
|
||
memset(&hLineProc->firstHalf_startPt, 0, sizeof(SVzNL3DPosition));
|
||
memset(&hLineProc->firstHalf_endPt, 0, sizeof(SVzNL3DPosition));
|
||
memset(&hLineProc->secondHalf, 0, sizeof(SSG_RUN));
|
||
memset(&hLineProc->secondHalf_startPt, 0, sizeof(SVzNL3DPosition));
|
||
memset(&hLineProc->secondHalf_endPt, 0, sizeof(SVzNL3DPosition));
|
||
hLineProc->hLineSM = keSG_HLineProc_Init;
|
||
}
|
||
else if (hLineProc->firstHalf.value != a_binValue)
|
||
{
|
||
//firstHalf延续
|
||
hLineProc->firstHalf.len = procData->nPointIdx - hLineProc->firstHalf_startPt.nPointIdx + 1;
|
||
hLineProc->firstHalf_endPt = *procData;
|
||
memset(&hLineProc->secondHalf, 0, sizeof(SSG_RUN));
|
||
memset(&hLineProc->secondHalf_startPt, 0, sizeof(SVzNL3DPosition));
|
||
memset(&hLineProc->secondHalf_endPt, 0, sizeof(SVzNL3DPosition));
|
||
hLineProc->hLineSM = keSG_HLineProc_First_Half;
|
||
}
|
||
else
|
||
{
|
||
//secondHalf继续
|
||
hLineProc->secondHalf.len++;
|
||
hLineProc->secondHalf_endPt = *procData;
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
hLineProc->preVldData = *procData;
|
||
return;
|
||
}
|
||
|
||
void sg_beltTearingInit(
|
||
int linePtNum,
|
||
std::vector<SSG_hLineProInfo>& hLineWorkers )
|
||
{
|
||
hLineWorkers.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
memset(&hLineWorkers[i], 0, sizeof(SSG_hLineProInfo));
|
||
}
|
||
|
||
bool _chkTreeMatching(
|
||
SSG_basicFeatureGap* a_feature,
|
||
SSG_beltTearingInfo* a_tree,
|
||
const SSG_beltTearingParam measureParam,
|
||
double* dist_x,
|
||
double* dist_y
|
||
)
|
||
{
|
||
//此处只使用ROI来进行匹配
|
||
//计算水平方向距离
|
||
double x_dist;
|
||
if ((a_feature->roi.right >= a_tree->roi.left) && (a_tree->roi.right >= a_feature->roi.left))
|
||
x_dist = 0; //有重合区
|
||
else if (a_tree->roi.left > a_feature->roi.right)
|
||
x_dist = a_tree->roi.left - a_feature->roi.right;
|
||
else
|
||
x_dist = a_feature->roi.left - a_tree->roi.right;
|
||
|
||
//计算垂直方向距离
|
||
double y_dist;
|
||
if ((a_feature->roi.bottom >= a_tree->roi.top) && (a_tree->roi.bottom >= a_feature->roi.top))
|
||
y_dist = 0; //有重合区
|
||
else if (a_tree->roi.top > a_feature->roi.bottom)
|
||
y_dist = a_tree->roi.top - a_feature->roi.bottom;
|
||
else
|
||
y_dist = a_feature->roi.top - a_tree->roi.bottom;
|
||
|
||
*dist_x = x_dist;
|
||
*dist_y = y_dist;
|
||
if (((y_dist < measureParam.scanYScale * 3) && (x_dist < measureParam.tearingMinGap)))
|
||
//||((x_dist < measureParam.scanXScale * 3) && (y_dist < measureParam.tearingMinGap)))
|
||
return true;
|
||
|
||
return false;
|
||
}
|
||
|
||
|
||
SSG_beltTearingInfo* _findMatchTre(
|
||
SSG_basicFeatureGap* a_feature,
|
||
std::vector<SSG_beltTearingInfo>& existTrees,
|
||
const SSG_beltTearingParam measureParam)
|
||
{
|
||
for (int i = 0, i_max = existTrees.size(); i < i_max; i++)
|
||
{
|
||
SSG_beltTearingInfo* a_tree = &existTrees[i];
|
||
double x_dist, y_dist;
|
||
bool isMatching = _chkTreeMatching(
|
||
a_feature,
|
||
a_tree,
|
||
measureParam,
|
||
&x_dist,
|
||
&y_dist);
|
||
if(true == isMatching)
|
||
return a_tree;
|
||
}
|
||
return NULL;
|
||
}
|
||
|
||
void _mergeTwoTree(
|
||
SSG_beltTearingInfo* mergeTree,
|
||
SSG_beltTearingInfo* subTree
|
||
)
|
||
{
|
||
mergeTree->endLineIdx = mergeTree->endLineIdx < subTree->endLineIdx ? subTree->endLineIdx : mergeTree->endLineIdx;
|
||
#if OUTPUT_TEARING_POINTS
|
||
mergeTree->pts.insert(mergeTree->pts.end(), subTree->pts.begin(), subTree->pts.end());
|
||
#endif
|
||
_combineROI(&mergeTree->roi, &subTree->roi);
|
||
mergeTree->statLineIdx = mergeTree->statLineIdx > subTree->statLineIdx ? subTree->statLineIdx : mergeTree->statLineIdx;
|
||
mergeTree->tearDepth = mergeTree->tearDepth < subTree->tearDepth ? subTree->tearDepth : mergeTree->tearDepth;
|
||
subTree->tearStatus = keSG_tearStatus_Invalid;
|
||
return;
|
||
}
|
||
|
||
SSG_tearProcessInfo* _getProcInfo(
|
||
int tearProcInfo_IdOffset,
|
||
int tearID,
|
||
std::vector<SSG_tearProcessInfo>& tearProcInfo)
|
||
{
|
||
int index = tearID;
|
||
if (tearProcInfo_IdOffset > index)
|
||
index = INT_MAX - tearProcInfo_IdOffset + index;
|
||
else
|
||
index = index - tearProcInfo_IdOffset;
|
||
SSG_tearProcessInfo* a_procInfo = &tearProcInfo[index];
|
||
if (a_procInfo->tearID >= 0)
|
||
{
|
||
try {
|
||
if (a_procInfo->tearID != tearID)
|
||
throw std::runtime_error("撕裂的ID与撕裂处理数据的ID不匹配！");
|
||
}
|
||
catch (const std::runtime_error& e) {
|
||
std::cerr << "Caught an exception: " << e.what() << std::endl;
|
||
}
|
||
}
|
||
return a_procInfo;
|
||
}
|
||
|
||
///数据输入必须是grid格式，以进行水平方向和垂直方向的处理
|
||
//每条扫描线调用一次处理程序
|
||
//每条扫描线完成一次垂直扫描线处理，同时N个水平状态机同时处理
|
||
//生长时，通过ROI判断是否有匹配的生长树。水平Feature的生长不记录开始和结束扫描线号
|
||
void sg_detectBeltTearing(
|
||
SVzNL3DLaserLine* laser3DPoints, //一条扫描线
|
||
int inputLineIdx,
|
||
int nPointCount, //每条扫描线的点数量。当采用Grid数据格式时，每条扫描线的点的数量是相同的。必须使用Grid格式
|
||
int* errCode,
|
||
std::vector<SSG_hLineProInfo>& hLineWorkers,
|
||
std::vector<SSG_beltTearingInfo>& beltTearings_new,
|
||
std::vector<SSG_beltTearingInfo>& beltTearings_growing,
|
||
std::vector<SSG_beltTearingInfo>& beltTearings_ended,
|
||
std::vector<SSG_beltTearingInfo>& beltTearings_unknown, //未判明，应用无需处理。
|
||
const SSG_beltTearingParam measureParam)
|
||
{
|
||
//算法需要建立Buff，同时延时处理。
|
||
static SVzNL3DLaserLine** buffLines = (SVzNL3DLaserLine**)malloc(sizeof(SVzNL3DLaserLine*) * SCAN_BUFF_SIZE);
|
||
static int buffSize = 0;
|
||
static int tearID = 1; //因为皮带是循环运行,需要考虑无限增长
|
||
//撕裂处理过程数据。因为皮带是循环运行，因此容器不能无限增长，需要将结束的撕裂过程数据清除
|
||
static int tearProcInfo_IdOffset = 1; //id从1开始
|
||
static std::vector<SSG_tearProcessInfo> tearProcInfo;
|
||
|
||
if (NULL == laser3DPoints) //复位内部静态变量
|
||
{
|
||
buffSize = 0;
|
||
tearID = 1;
|
||
tearProcInfo_IdOffset = 1;
|
||
tearProcInfo.resize(0);
|
||
return;
|
||
}
|
||
|
||
*errCode = 0;
|
||
if (0 == laser3DPoints->nPositionCnt)
|
||
return;
|
||
|
||
if (hLineWorkers.size() != laser3DPoints->nPositionCnt) //空行
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
/* 数据缓冲FIFO。只有缓冲区满后才进行处理。 */
|
||
if (buffSize < SCAN_BUFF_SIZE) //需要申请buff空间
|
||
{
|
||
SVzNL3DLaserLine* a_buffLine = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine));
|
||
a_buffLine->nPositionCnt = laser3DPoints->nPositionCnt;
|
||
a_buffLine->nTimeStamp = inputLineIdx;
|
||
a_buffLine->p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints->nPositionCnt);
|
||
memcpy(a_buffLine->p3DPosition, laser3DPoints->p3DPosition, sizeof(SVzNL3DPosition) * laser3DPoints->nPositionCnt);
|
||
for (int i = 0; i < laser3DPoints->nPositionCnt; i++)
|
||
a_buffLine->p3DPosition[i].nPointIdx = inputLineIdx; //用于水平处理时得到点对应的LineIdx
|
||
buffLines[buffSize] = a_buffLine;
|
||
buffSize++;
|
||
return; //buffer未满，不处理
|
||
}
|
||
else //当Buffer已满，申请的空间复用，不需要再申请空间
|
||
{
|
||
SVzNL3DLaserLine* tmpLine = buffLines[0];
|
||
for (int i = 0; i < SCAN_BUFF_SIZE - 1; i++)
|
||
buffLines[i] = buffLines[i + 1];
|
||
tmpLine->nPositionCnt = laser3DPoints->nPositionCnt;
|
||
tmpLine->nTimeStamp = inputLineIdx;
|
||
memcpy(tmpLine->p3DPosition, laser3DPoints->p3DPosition, sizeof(SVzNL3DPosition) * laser3DPoints->nPositionCnt);
|
||
for (int i = 0; i < laser3DPoints->nPositionCnt; i++)
|
||
tmpLine->p3DPosition[i].nPointIdx = inputLineIdx; //用于水平处理时得到点对应的LineIdx
|
||
buffLines[SCAN_BUFF_SIZE - 1] = tmpLine;
|
||
}
|
||
|
||
/* 将已经生成的beltTearings_new合并到beltTearings_growing中，并清空beltTearings_new */
|
||
if (beltTearings_new.size() > 0)
|
||
{
|
||
for (int i = 0; i < beltTearings_new.size(); i++)
|
||
beltTearings_new[i].tearStatus = keSG_tearStatus_Growing;
|
||
beltTearings_growing.insert(beltTearings_growing.end(), beltTearings_new.begin(), beltTearings_new.end());
|
||
beltTearings_new.clear();
|
||
}
|
||
|
||
/* 将beltTearings_ended清空 */
|
||
if (beltTearings_ended.size() > 0)
|
||
{
|
||
for (int i = 0, i_max = beltTearings_ended.size(); i < i_max; i++)
|
||
{
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
beltTearings_ended[i].tearID,
|
||
tearProcInfo);
|
||
match_procInfo->tearID = -1;
|
||
}
|
||
beltTearings_ended.clear();
|
||
//调整tearProcInfo
|
||
int vldStart = -1;
|
||
for (int i = 0, i_max = tearProcInfo.size(); i < i_max; i++)
|
||
{
|
||
if (tearProcInfo[i].tearID > 0)
|
||
{
|
||
vldStart = i;
|
||
break;
|
||
}
|
||
}
|
||
if (vldStart > 0)
|
||
{
|
||
tearProcInfo.erase(tearProcInfo.begin(), tearProcInfo.begin() + vldStart);
|
||
tearProcInfo_IdOffset = tearProcInfo[0].tearID;
|
||
}
|
||
}
|
||
|
||
/****************************************************************/
|
||
/**** 对BuffLines的中间进行处理（延时了BuffLines的一半行数） ****/
|
||
/****************************************************************/
|
||
int buffMidPos = SCAN_BUFF_SIZE / 2;
|
||
if ( (buffLines[buffMidPos]->nTimeStamp == 422) ||(buffLines[buffMidPos]->nTimeStamp == 182))
|
||
int kkk = 1;
|
||
|
||
int procLineIdx = buffLines[buffMidPos]->nTimeStamp;
|
||
//对当前扫描线数据进行处理
|
||
std::vector<SSG_basicFeatureGap> vLineFeatures;
|
||
sg_bearTearing_vLineProc(
|
||
buffLines[buffMidPos], //当前扫描线
|
||
buffLines[buffMidPos]->nTimeStamp, //当前扫描线序号
|
||
measureParam,
|
||
vLineFeatures);
|
||
|
||
//缓冲,水平处理
|
||
std::vector<SSG_basicFeatureGap> hLineFeatures;
|
||
//hLineFeatures.resize(hLineWorkers.size());
|
||
for (int i = 0; i < laser3DPoints->nPositionCnt; i++)
|
||
{
|
||
//if ((buffLines[buffMidPos]->nTimeStamp == 295) && (i == 725))
|
||
// int kkk = 1;
|
||
|
||
//水平扫描线处理，进行垂直方向的特征提取和生长
|
||
std::vector<SSG_basicFeatureGap> a_hLineFeatures;
|
||
sg_bearTearing_hLineProc(
|
||
&hLineWorkers[i],
|
||
i,
|
||
buffLines[buffMidPos]->nTimeStamp, //水平扫描点序号，对应垂直扫描线的线序号
|
||
buffLines,
|
||
measureParam,
|
||
a_hLineFeatures);
|
||
//if (0 == a_hLineFeatures.size())
|
||
// hLineFeatures[i].lineIdx = -1;
|
||
//else
|
||
if(a_hLineFeatures.size() > 0)
|
||
{
|
||
hLineFeatures.push_back(a_hLineFeatures[0]);
|
||
//hLineFeatures[i] = a_hLineFeatures[0];
|
||
//if ((a_hLineFeatures[0].gapPt_0.pt3D.x < 10871) &&(a_hLineFeatures[0].gapPt_0.pt3D.x > 10870))
|
||
// int kkk = 1;
|
||
}
|
||
}
|
||
//垂直特征生长
|
||
//对于垂直特征生长，不能同一棵生长树对应多个特征，除非是包含关系
|
||
//当一个特征属于两个以上的生长树时，为了简化处理逻辑，不进行生长树的合并，选择最合适的生长树
|
||
if (vLineFeatures.size() > 0)
|
||
{
|
||
std::vector<SSG_intPair> feature_tree_id_pair; //注意：vector取地址时在push_back操作后单元地址可能会变化，使用序号
|
||
feature_tree_id_pair.resize(vLineFeatures.size()); //每个featurec对应一个matchTree
|
||
std::vector<SVzNL2DPointD> feature_tree_pair_dist;
|
||
feature_tree_pair_dist.resize(vLineFeatures.size());
|
||
for (int i = 0, i_max = vLineFeatures.size(); i < i_max; i++)
|
||
{
|
||
feature_tree_id_pair[i].data_0 = -1; //指示tree_id
|
||
feature_tree_id_pair[i].data_1 = -1; //指示group_id
|
||
for (int j = 0, j_max = beltTearings_growing.size(); j < j_max; j++)
|
||
{
|
||
double dist_x = -1, dist_y = -1;
|
||
bool isMatching = _chkTreeMatching(
|
||
&vLineFeatures[i],
|
||
&beltTearings_growing[j],
|
||
measureParam,
|
||
&dist_x,
|
||
&dist_y);
|
||
if (true == isMatching)
|
||
{
|
||
if (feature_tree_id_pair[i].data_0 < 0)
|
||
{
|
||
feature_tree_id_pair[i].data_0 = j;
|
||
feature_tree_id_pair[i].data_1 = 0;
|
||
feature_tree_pair_dist[i].x = dist_x;
|
||
feature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
else //取最近的tree。垂直特征生长，使用dist_y来计算
|
||
{
|
||
if (dist_y < feature_tree_pair_dist[i].y)
|
||
{
|
||
feature_tree_id_pair[i].data_0 = j;
|
||
feature_tree_id_pair[i].data_1 = 0;
|
||
feature_tree_pair_dist[i].x = dist_x;
|
||
feature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
for (int j = 0, j_max = beltTearings_unknown.size(); j < j_max; j++)
|
||
{
|
||
if (keSG_tearStatus_Invalid == beltTearings_unknown[j].tearStatus)
|
||
continue;
|
||
|
||
double dist_x = -1, dist_y = -1;
|
||
bool isMatching = _chkTreeMatching(
|
||
&vLineFeatures[i],
|
||
&beltTearings_unknown[j],
|
||
measureParam,
|
||
&dist_x,
|
||
&dist_y);
|
||
if (true == isMatching)
|
||
{
|
||
if (feature_tree_id_pair[i].data_0 < 0)
|
||
{
|
||
feature_tree_id_pair[i].data_0 = j;
|
||
feature_tree_id_pair[i].data_1 = 1;
|
||
feature_tree_pair_dist[i].x = dist_x;
|
||
feature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
else //取最近的tree。垂直特征生长，使用dist_y来计算
|
||
{
|
||
if (dist_y < feature_tree_pair_dist[i].y)
|
||
{
|
||
feature_tree_id_pair[i].data_0 = j;
|
||
feature_tree_id_pair[i].data_1 = 1;
|
||
feature_tree_pair_dist[i].x = dist_x;
|
||
feature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
//检查是否有同一个feature属于两个生长树的情况
|
||
for (int i = 0, i_max = vLineFeatures.size(); i < i_max; i++)
|
||
{
|
||
if (feature_tree_id_pair[i].data_0 < 0)
|
||
continue;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
if ( (feature_tree_id_pair[i].data_0 == feature_tree_id_pair[j].data_0) &&
|
||
(feature_tree_id_pair[i].data_1 == feature_tree_id_pair[j].data_1))
|
||
{
|
||
//比较y距离
|
||
if ( (feature_tree_pair_dist[i].y < feature_tree_pair_dist[j].y) &&
|
||
(feature_tree_pair_dist[j].y > 1e-4)) //不为0
|
||
feature_tree_id_pair[j].data_0 = -1;
|
||
else if ((feature_tree_pair_dist[j].y < feature_tree_pair_dist[i].y) &&
|
||
(feature_tree_pair_dist[i].y > 1e-4)) //不为0
|
||
feature_tree_id_pair[i].data_0 = -1;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
for (int i = 0, i_max = vLineFeatures.size(); i < i_max; i++)
|
||
{
|
||
//对于垂直特征生长，不能同一棵生长树对应多个特征，除非是包含关系
|
||
SSG_basicFeatureGap* a_vFeature = &vLineFeatures[i];
|
||
if (feature_tree_id_pair[i].data_0 < 0)
|
||
{
|
||
SSG_beltTearingInfo a_newTree;
|
||
memset(&a_newTree, 0, sizeof(SSG_beltTearingInfo));
|
||
a_newTree.tearID = tearID; //
|
||
tearID++;
|
||
if (tearID == 0) //循环一圈
|
||
tearID = 1;
|
||
a_newTree.statLineIdx = procLineIdx;
|
||
a_newTree.endLineIdx = procLineIdx;
|
||
a_newTree.roi = a_vFeature->roi;
|
||
a_newTree.tearDepth = a_vFeature->gap_depth;
|
||
a_newTree.tearType = keSG_tearType_MachineDir; //纵撕
|
||
a_newTree.tearStatus = keSG_tearStatus_Uknown; //unknown
|
||
#if OUTPUT_TEARING_POINTS
|
||
a_vFeature->gapPt_0.nPointIdx = a_newTree.tearID;
|
||
a_vFeature->gapPt_1.nPointIdx = a_newTree.tearID;
|
||
a_newTree.pts.push_back(a_vFeature->gapPt_0);
|
||
a_newTree.pts.push_back(a_vFeature->gapPt_1);
|
||
#endif
|
||
beltTearings_unknown.push_back(a_newTree);
|
||
SSG_tearProcessInfo a_procInfo;
|
||
memset(&a_procInfo, 0, sizeof(SSG_tearProcessInfo));
|
||
a_procInfo.tearID = a_newTree.tearID;
|
||
a_procInfo.vGap_depth_max = a_vFeature->gap_depth;
|
||
a_procInfo.vGap_width_max = a_vFeature->gap_width;
|
||
a_procInfo.vGap_width_sum = a_vFeature->gap_width;
|
||
a_procInfo.vGap_num = 1;
|
||
tearProcInfo.push_back(a_procInfo);
|
||
}
|
||
else
|
||
{
|
||
SSG_beltTearingInfo* a_matchTree;
|
||
int tree_id = feature_tree_id_pair[i].data_0;
|
||
if(0 == feature_tree_id_pair[i].data_1)
|
||
a_matchTree = &beltTearings_growing[tree_id];
|
||
else
|
||
a_matchTree = &beltTearings_unknown[tree_id];
|
||
//取相应的procInfo
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
a_matchTree->tearID,
|
||
tearProcInfo);
|
||
if ((a_vFeature->gap_depth < 0) || (match_procInfo->vGap_depth_max < 0))
|
||
match_procInfo->vGap_depth_max = -1; //贯穿撕裂
|
||
else
|
||
match_procInfo->vGap_depth_max = match_procInfo->vGap_depth_max < a_vFeature->gap_depth ? a_vFeature->gap_depth : match_procInfo->vGap_depth_max;
|
||
match_procInfo->vGap_width_max = match_procInfo->vGap_width_max < a_vFeature->gap_width ? a_vFeature->gap_width : match_procInfo->vGap_width_max;
|
||
match_procInfo->vGap_width_sum += a_vFeature->gap_width;
|
||
match_procInfo->vGap_num++;
|
||
#if OUTPUT_TEARING_POINTS
|
||
a_vFeature->gapPt_0.nPointIdx = a_matchTree->tearID;
|
||
a_vFeature->gapPt_1.nPointIdx = a_matchTree->tearID;
|
||
a_matchTree->pts.push_back(a_vFeature->gapPt_0);
|
||
a_matchTree->pts.push_back(a_vFeature->gapPt_1);
|
||
#endif
|
||
//
|
||
a_matchTree->endLineIdx = procLineIdx;
|
||
_combineROI(&a_matchTree->roi, &a_vFeature->roi);
|
||
if (keSG_tearType_MachineDir == a_matchTree->tearType)
|
||
{
|
||
a_matchTree->tearWidth = match_procInfo->vGap_width_max;
|
||
a_matchTree->tearDepth = match_procInfo->vGap_depth_max;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#if ENABLE_CROSS_WISE_TEAR
|
||
//水平特征生长
|
||
if (hLineFeatures.size() > 0)
|
||
{
|
||
std::vector<SSG_intPair> hFeature_tree_id_pair; //注意：vector取地址时在push_back操作后单元地址可能会变化，使用序号
|
||
hFeature_tree_id_pair.resize(hLineFeatures.size()); //每个featurec对应一个matchTree
|
||
std::vector<SVzNL2DPointD> hFeature_tree_pair_dist;
|
||
hFeature_tree_pair_dist.resize(hLineFeatures.size());
|
||
for(int i = 0, i_max = hLineFeatures.size(); i < i_max; i++)
|
||
{
|
||
hFeature_tree_id_pair[i].data_0 = -1; //指示tree_id
|
||
hFeature_tree_id_pair[i].data_1 = -1; //指示group_id
|
||
//对于beltTearings_unknown：当纵撕不允许， 横撕允许，且合并
|
||
for (int j = 0, j_max = beltTearings_unknown.size(); j < j_max; j++)
|
||
{
|
||
if (keSG_tearStatus_Invalid == beltTearings_unknown[j].tearStatus)
|
||
continue;
|
||
|
||
double dist_x = -1, dist_y = -1;
|
||
bool isMatching = _chkTreeMatching(
|
||
&hLineFeatures[i],
|
||
&beltTearings_unknown[j],
|
||
measureParam,
|
||
&dist_x,
|
||
&dist_y);
|
||
if (true == isMatching)
|
||
{
|
||
if (hFeature_tree_id_pair[i].data_0 < 0)
|
||
{
|
||
hFeature_tree_id_pair[i].data_0 = j;
|
||
hFeature_tree_id_pair[i].data_1 = 1; //1:beltTearings_unknown
|
||
hFeature_tree_pair_dist[i].x = dist_x;
|
||
hFeature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
else //检查合并。
|
||
{
|
||
int mergeTreeIdx = hFeature_tree_id_pair[i].data_0;
|
||
_mergeTwoTree(&beltTearings_unknown[mergeTreeIdx],&beltTearings_unknown[j]);
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
beltTearings_unknown[j].tearID,
|
||
tearProcInfo);
|
||
match_procInfo->tearID = -1; //被合并的处理块置无效
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int j = 0, j_max = beltTearings_growing.size(); j < j_max; j++)
|
||
{
|
||
double dist_x = -1, dist_y = -1;
|
||
bool isMatching = _chkTreeMatching(
|
||
&hLineFeatures[i],
|
||
&beltTearings_growing[j],
|
||
measureParam,
|
||
&dist_x,
|
||
&dist_y);
|
||
if (true == isMatching)
|
||
{
|
||
if (hFeature_tree_id_pair[i].data_0 < 0)
|
||
{
|
||
hFeature_tree_id_pair[i].data_0 = j;
|
||
hFeature_tree_id_pair[i].data_1 = 0; //0 : beltTearings_growing
|
||
hFeature_tree_pair_dist[i].x = dist_x;
|
||
hFeature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
else
|
||
{
|
||
#if 0
|
||
//取最近的tree。水平特征生长，也使用dist_y来计算。
|
||
if (dist_y < hFeature_tree_pair_dist[i].y)
|
||
{
|
||
hFeature_tree_id_pair[i].data_0 = j;
|
||
hFeature_tree_id_pair[i].data_1 = 0;
|
||
hFeature_tree_pair_dist[i].x = dist_x;
|
||
hFeature_tree_pair_dist[i].y = dist_y;
|
||
}
|
||
#else
|
||
//beltTearings_growing为正在生长的树，优先。
|
||
hFeature_tree_id_pair[i].data_0 = j;
|
||
hFeature_tree_id_pair[i].data_1 = 0;
|
||
hFeature_tree_pair_dist[i].x = dist_x;
|
||
hFeature_tree_pair_dist[i].y = dist_y;
|
||
#endif
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
for (int i = 0, i_max = hLineFeatures.size(); i < i_max; i++)
|
||
{
|
||
SSG_basicFeatureGap* a_hFeature = &hLineFeatures[i];
|
||
if (a_hFeature->lineIdx < 0)
|
||
continue;
|
||
if (hFeature_tree_id_pair[i].data_0 < 0)
|
||
{
|
||
SSG_beltTearingInfo a_newTree;
|
||
memset(&a_newTree, 0, sizeof(SSG_beltTearingInfo));
|
||
a_newTree.tearID = tearID; //
|
||
tearID++;
|
||
if (tearID == 0) //循环一圈
|
||
tearID = 1;
|
||
a_newTree.statLineIdx = procLineIdx;
|
||
a_newTree.endLineIdx = procLineIdx;
|
||
a_newTree.roi = a_hFeature->roi;
|
||
a_newTree.tearDepth = a_hFeature->gap_depth;
|
||
a_newTree.tearType = keSG_tearType_CrossWise; //横撕
|
||
a_newTree.tearStatus = keSG_tearStatus_Uknown; //unknown
|
||
#if OUTPUT_TEARING_POINTS
|
||
a_hFeature->gapPt_0.nPointIdx = a_newTree.tearID;
|
||
a_hFeature->gapPt_1.nPointIdx = a_newTree.tearID;
|
||
a_newTree.pts.push_back(a_hFeature->gapPt_0);
|
||
a_newTree.pts.push_back(a_hFeature->gapPt_1);
|
||
#endif
|
||
beltTearings_unknown.push_back(a_newTree);
|
||
SSG_tearProcessInfo a_procInfo;
|
||
memset(&a_procInfo, 0, sizeof(SSG_tearProcessInfo));
|
||
a_procInfo.tearID = a_newTree.tearID;
|
||
a_procInfo.hGap_depth_max = a_hFeature->gap_depth;
|
||
a_procInfo.hGap_width_max = a_hFeature->gap_width;
|
||
a_procInfo.hGap_width_sum = a_hFeature->gap_width;
|
||
a_procInfo.hGap_num = 1;
|
||
tearProcInfo.push_back(a_procInfo);
|
||
}
|
||
else
|
||
{
|
||
SSG_beltTearingInfo* a_matchTree;
|
||
int tree_id = hFeature_tree_id_pair[i].data_0;
|
||
if (0 == hFeature_tree_id_pair[i].data_1)
|
||
a_matchTree = &beltTearings_growing[tree_id];
|
||
else
|
||
a_matchTree = &beltTearings_unknown[tree_id];
|
||
//取相应的procInfo
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
a_matchTree->tearID,
|
||
tearProcInfo);
|
||
match_procInfo->hGap_depth_max = match_procInfo->hGap_depth_max < a_hFeature->gap_depth ? a_hFeature->gap_depth : match_procInfo->hGap_depth_max;
|
||
match_procInfo->hGap_width_max = match_procInfo->hGap_width_max < a_hFeature->gap_width ? a_hFeature->gap_width : match_procInfo->hGap_width_max;
|
||
match_procInfo->hGap_width_sum += a_hFeature->gap_width;
|
||
match_procInfo->hGap_num++;
|
||
#if OUTPUT_TEARING_POINTS
|
||
a_hFeature->gapPt_0.nPointIdx = a_matchTree->tearID;
|
||
a_hFeature->gapPt_1.nPointIdx = a_matchTree->tearID;
|
||
a_matchTree->pts.push_back(a_hFeature->gapPt_0);
|
||
a_matchTree->pts.push_back(a_hFeature->gapPt_1);
|
||
#endif
|
||
//
|
||
a_matchTree->endLineIdx = procLineIdx;
|
||
_combineROI(&a_matchTree->roi, &a_hFeature->roi);
|
||
if (keSG_tearType_CrossWise == a_matchTree->tearType)
|
||
{
|
||
a_matchTree->tearWidth = match_procInfo->hGap_width_max;
|
||
a_matchTree->tearDepth = match_procInfo->hGap_depth_max;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
//判断生长树是否结果，生成beltTearings_ended目标
|
||
if (beltTearings_growing.size() > 0)
|
||
{
|
||
int i_max = beltTearings_growing.size();
|
||
for (int i = i_max-1; i >= 0; i--) //从后往前处理，这样在删除时不会影响后续检查
|
||
{
|
||
if (beltTearings_growing[i].endLineIdx != procLineIdx)
|
||
{
|
||
int idx_diff;
|
||
if (procLineIdx > beltTearings_growing[i].endLineIdx)
|
||
idx_diff = procLineIdx - beltTearings_growing[i].endLineIdx;
|
||
else
|
||
idx_diff = INT_MAX - beltTearings_growing[i].endLineIdx + 1 + procLineIdx;
|
||
double dist = idx_diff * measureParam.scanXScale;
|
||
if (dist > measureParam.tearingMinGap)
|
||
{
|
||
beltTearings_growing[i].tearStatus = keSG_tearStatus_Ended; //结束
|
||
beltTearings_ended.push_back(beltTearings_growing[i]);
|
||
beltTearings_growing.erase(beltTearings_growing.begin() + i);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//处理unknown的撕裂，生成beltTearings_new
|
||
if (beltTearings_unknown.size() > 0)
|
||
{
|
||
int i_max = beltTearings_unknown.size();
|
||
for (int i = i_max - 1; i >= 0; i--)//从后往前处理，这样在删除时不会影响后续检查
|
||
{
|
||
if (keSG_tearStatus_Invalid == beltTearings_unknown[i].tearStatus)
|
||
{
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
beltTearings_unknown[i].tearID,
|
||
tearProcInfo);
|
||
match_procInfo->tearID = -1;
|
||
beltTearings_unknown.erase(beltTearings_unknown.begin() + i);
|
||
}
|
||
else
|
||
{
|
||
if (beltTearings_unknown[i].endLineIdx == procLineIdx) //检查是否需要报警
|
||
{
|
||
//计算ROI的长宽
|
||
double w = beltTearings_unknown[i].roi.right - beltTearings_unknown[i].roi.left;
|
||
double h = beltTearings_unknown[i].roi.bottom - beltTearings_unknown[i].roi.top;
|
||
if ((w >= measureParam.tearingMinLen) || (h >= measureParam.tearingMinLen))
|
||
{
|
||
int lines = beltTearings_unknown[i].endLineIdx - beltTearings_unknown[i].statLineIdx + 1;
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
beltTearings_unknown[i].tearID,
|
||
tearProcInfo);
|
||
int vldLines = match_procInfo->hGap_num > match_procInfo->vGap_num ? match_procInfo->hGap_num : match_procInfo->vGap_num;
|
||
if (vldLines < lines / 2)
|
||
{
|
||
match_procInfo->tearID = -1;
|
||
beltTearings_unknown.erase(beltTearings_unknown.begin() + i);
|
||
}
|
||
else
|
||
{
|
||
bool isVTearing = true;
|
||
if ((match_procInfo->hGap_num > 0) && (match_procInfo->vGap_num == 0))
|
||
isVTearing = false;
|
||
else if (((match_procInfo->hGap_num > 0) && (match_procInfo->vGap_num > 0)) &&
|
||
(match_procInfo->hGap_width_max < match_procInfo->vGap_width_max))
|
||
isVTearing = false;
|
||
|
||
if (false == isVTearing)
|
||
{
|
||
beltTearings_unknown[i].tearType = keSG_tearType_CrossWise;
|
||
beltTearings_unknown[i].tearStatus = keSG_tearStatus_New;
|
||
beltTearings_unknown[i].tearDepth = match_procInfo->hGap_depth_max;
|
||
beltTearings_unknown[i].tearWidth = match_procInfo->hGap_width_max;
|
||
beltTearings_new.push_back(beltTearings_unknown[i]);
|
||
beltTearings_unknown.erase(beltTearings_unknown.begin() + i);
|
||
}
|
||
else
|
||
{
|
||
beltTearings_unknown[i].tearType = keSG_tearType_MachineDir;
|
||
beltTearings_unknown[i].tearStatus = keSG_tearStatus_New;
|
||
beltTearings_unknown[i].tearDepth = match_procInfo->vGap_depth_max;
|
||
beltTearings_unknown[i].tearWidth = match_procInfo->vGap_width_max;
|
||
beltTearings_new.push_back(beltTearings_unknown[i]);
|
||
beltTearings_unknown.erase(beltTearings_unknown.begin() + i);
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
else//检查是否是噪声
|
||
{
|
||
int idx_diff;
|
||
if (procLineIdx > beltTearings_unknown[i].endLineIdx)
|
||
idx_diff = procLineIdx - beltTearings_unknown[i].endLineIdx;
|
||
else
|
||
idx_diff = INT_MAX - beltTearings_unknown[i].endLineIdx + 1 + procLineIdx;
|
||
|
||
double dist = idx_diff * measureParam.scanXScale;
|
||
if (dist > measureParam.tearingMinGap)
|
||
{
|
||
SSG_tearProcessInfo* match_procInfo = _getProcInfo(
|
||
tearProcInfo_IdOffset,
|
||
beltTearings_unknown[i].tearID,
|
||
tearProcInfo);
|
||
match_procInfo->tearID = -1;
|
||
beltTearings_unknown.erase(beltTearings_unknown.begin() + i);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|