bagThreadPositioning v1.0.0
初版算法
This commit is contained in:
parent
360460320e
commit
2d7bb92410
@ -461,12 +461,16 @@ int main()
|
||||
growParam.minLTypeTreeLen = 10; //mm,
|
||||
growParam.minVTypeTreeLen = 10; //mm
|
||||
|
||||
bool isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝
|
||||
SSX_ScanInfo scanInfo;
|
||||
scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝
|
||||
scanInfo.scanFromThreadHead = false; //true:线袋子线缝头部开始扫描。
|
||||
scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
|
||||
scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
|
||||
int errCode = 0;
|
||||
std::vector<SSX_bagThreadInfo> bagThreadInfo;
|
||||
wd_bagThreadPositioning(
|
||||
scanLines,
|
||||
isHorizonScan, //true:激光线平行线缝；false:激光线垂直线缝
|
||||
scanInfo, //true:激光线平行线缝；false:激光线垂直线缝
|
||||
filterParam, //噪点过滤参数
|
||||
cornerParam, //V型特征参数
|
||||
raisedFeatureParam,//线尾凸起参数
|
||||
|
||||
@ -166,6 +166,24 @@ SG_APISHARED_EXPORT void wd_getLineCorerFeature(
|
||||
std::vector<SSG_basicFeature1D>& line_cornerFeatures //拐点
|
||||
);
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getLineCorerFeature_accelerate(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const double pointInterval,
|
||||
std::vector<SSG_RUN_EX>& segs,
|
||||
std::vector<SSG_basicFeature1D>& line_cornerFeatures //拐点特征
|
||||
);
|
||||
|
||||
//提取凸起段
|
||||
SG_APISHARED_EXPORT void wd_getLineRaisedFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
|
||||
@ -3724,6 +3724,9 @@ void _searchCornerPeaks(
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_pntDirAngle* curr_data = &corners[i];
|
||||
if (curr_data->pntIdx < 0)
|
||||
continue;
|
||||
|
||||
if (curr_data->pntIdx < 0)
|
||||
{
|
||||
if (i == i_max - 1) //最后一个
|
||||
@ -4071,6 +4074,202 @@ void wd_getLineCorerFeature(
|
||||
return;
|
||||
}
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
void wd_getLineCorerFeature_accelerate(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const double pointInterval,
|
||||
std::vector<SSG_RUN_EX>& segs,
|
||||
std::vector<SSG_basicFeature1D>& line_cornerFeatures //拐点特征
|
||||
)
|
||||
{
|
||||
int dataSize = (int)lineData.size();
|
||||
//去除零点
|
||||
int runIdx = 1;
|
||||
SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx
|
||||
double pre_z = 0;
|
||||
double pre_y = 0;
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if (i == 1100)
|
||||
int kkk = 1;
|
||||
lineData[i].nPointIdx = i; //重新编序号
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if (a_run.len < 0)
|
||||
{
|
||||
a_run.start_zRising = true;
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.end_zRising = true;
|
||||
a_run.value = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
double z_diff = abs(lineData[i].pt3D.z - pre_z);
|
||||
if (z_diff < cornerPara.minEndingGap_z)
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
bool next_zRising;
|
||||
if (pre_z > lineData[i].pt3D.z)
|
||||
{
|
||||
a_run.end_zRising = true;
|
||||
next_zRising = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_run.end_zRising = false;
|
||||
next_zRising = true;
|
||||
}
|
||||
a_run.value = runIdx;
|
||||
runIdx++;
|
||||
segs.push_back(a_run);
|
||||
|
||||
a_run.start = i;
|
||||
a_run.start_zRising = next_zRising;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
a_run.end_zRising = true;
|
||||
}
|
||||
}
|
||||
//vldPtSegIdx.push_back(runIdx);
|
||||
|
||||
pre_z = lineData[i].pt3D.z;
|
||||
}
|
||||
}
|
||||
if (a_run.len > 0)
|
||||
segs.push_back(a_run);
|
||||
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> corners;
|
||||
corners.resize(lineData.size());
|
||||
//逐段进行
|
||||
int ptSkipping = cornerPara.scale / pointInterval;
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
if (i == 1100)
|
||||
int kkk = 1;
|
||||
|
||||
if (lineData[i].pt3D.z < 1e-4)
|
||||
{
|
||||
corners[i].pntIdx = -1;
|
||||
corners[i].forwardAngle = 0;
|
||||
corners[i].backwardAngle = 0;
|
||||
corners[i].corner = 0;
|
||||
corners[i].forwardDiffZ = 0;
|
||||
corners[i].backwardDiffZ = 0;
|
||||
continue;
|
||||
}
|
||||
//前向寻找
|
||||
int pre_i = -1;
|
||||
int searchStart = i - ptSkipping;
|
||||
if (searchStart >= vPtIdxStart)
|
||||
{
|
||||
for (int j = searchStart; j < i; j++)
|
||||
{
|
||||
if (lineData[j].pt3D.z > 1e-4)
|
||||
{
|
||||
double dist = sqrt(pow(lineData[i].pt3D.y - lineData[j].pt3D.y, 2) +
|
||||
pow(lineData[i].pt3D.z - lineData[j].pt3D.z, 2));
|
||||
if (dist < cornerPara.scale)
|
||||
break;
|
||||
else
|
||||
pre_i = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
//后向寻找
|
||||
int post_i = -1;
|
||||
int searchEnd = i + ptSkipping;
|
||||
if (searchEnd <= vPtIdxEnd)
|
||||
{
|
||||
for (int j = searchEnd; j > i; j--)
|
||||
{
|
||||
if (lineData[j].pt3D.z > 1e-4)
|
||||
{
|
||||
double dist = sqrt(pow(lineData[i].pt3D.y - lineData[j].pt3D.y, 2) +
|
||||
pow(lineData[i].pt3D.z - lineData[j].pt3D.z, 2));
|
||||
if (dist < cornerPara.scale)
|
||||
break;
|
||||
else
|
||||
post_i = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
if ((pre_i < 0) || (post_i < 0))
|
||||
{
|
||||
corners[i].pntIdx = -1;
|
||||
corners[i].forwardAngle = 0;
|
||||
corners[i].backwardAngle = 0;
|
||||
corners[i].corner = 0;
|
||||
corners[i].forwardDiffZ = 0;
|
||||
corners[i].backwardDiffZ = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y);
|
||||
double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||||
corners[i].pntIdx = i;
|
||||
corners[i].forwardAngle = forwardAngle;
|
||||
corners[i].backwardAngle = backwardAngle;
|
||||
corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||||
corners[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z;
|
||||
corners[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector< SSG_pntDirAngle> cornerPeakP;
|
||||
std::vector< SSG_pntDirAngle> cornerPeakM;
|
||||
double cornerMergeScale = cornerPara.scale * 2;
|
||||
//提取corner极值
|
||||
_searchCornerPeaks(
|
||||
corners,
|
||||
lineData,
|
||||
cornerPara,
|
||||
cornerMergeScale,
|
||||
cornerPeakP,
|
||||
cornerPeakM
|
||||
);
|
||||
for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||||
else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||||
else
|
||||
a_feature.featureType = LINE_FEATURE_CORNER_V;
|
||||
|
||||
int cornerPtIdx = cornerPeakP[i].pntIdx;
|
||||
a_feature.jumpPos = lineData[cornerPtIdx].pt3D;
|
||||
a_feature.jumpPos2D = { lineIdx, lineData[cornerPtIdx].nPointIdx };
|
||||
line_cornerFeatures.push_back(a_feature);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//提取凸起段
|
||||
void wd_getLineRaisedFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
|
||||
@ -15,7 +15,7 @@ const char* wd_bagThreadPositioningVersion(void)
|
||||
//线头位置检测定位
|
||||
void wd_bagThreadPositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const SSG_outlierFilterParam filterParam, //噪点过滤参数
|
||||
const SSG_cornerParam cornerPara, //V型特征参数
|
||||
const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
|
||||
@ -49,6 +49,9 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
|
||||
//生成水平扫描数据
|
||||
//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
|
||||
double ptInterval = 0;
|
||||
int ptIntevalNum = 0;
|
||||
std::vector< std::vector<SVzNL3DPosition>> data_lines_h; //水平扫描数据
|
||||
data_lines_h.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
@ -61,20 +64,54 @@ void wd_bagThreadPositioning(
|
||||
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;
|
||||
if (j > 0)
|
||||
{
|
||||
if ((scanLines[line][j - 1].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z > 1e-4))
|
||||
{
|
||||
ptInterval += abs(scanLines[line][j].pt3D.y - scanLines[line][j - 1].pt3D.y);
|
||||
ptIntevalNum++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if(ptIntevalNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_NULL;
|
||||
return;
|
||||
}
|
||||
ptInterval = ptInterval / ptIntevalNum;
|
||||
int lineNum_h = linePtNum;
|
||||
int linePtNum_h = (int)data_lines_h[0].size();
|
||||
double lineInterval = 0;
|
||||
int lineIntervalNum = 0;
|
||||
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;
|
||||
if (j > 0)
|
||||
{
|
||||
if ((data_lines_h[line][j - 1].pt3D.z > 1e-4) && (data_lines_h[line][j].pt3D.z > 1e-4))
|
||||
{
|
||||
lineInterval += abs(data_lines_h[line][j].pt3D.y - data_lines_h[line][j - 1].pt3D.y);
|
||||
lineIntervalNum++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (lineIntervalNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_NULL;
|
||||
return;
|
||||
}
|
||||
lineInterval = lineInterval / lineIntervalNum;
|
||||
|
||||
double vCornerScale = cornerPara.scale * 4;
|
||||
std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
|
||||
std::vector<std::vector<SWD_segFeature>> raisedFeatures;
|
||||
if (false == isHorizonScan)
|
||||
if (false == scanInfo.isHorizonScan)
|
||||
{
|
||||
int validVCornerSCale = (int)(vCornerScale / ptInterval);
|
||||
//垂直扫描检测V型槽和线尾
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
@ -85,14 +122,38 @@ void wd_bagThreadPositioning(
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
//提取V型槽
|
||||
std::vector<SSG_basicFeature1D> line_cornerFeatures;
|
||||
std::vector<SSG_RUN_EX> segs;
|
||||
int dataSize = (int)lineData.size();
|
||||
wd_getLineCorerFeature(
|
||||
wd_getLineCorerFeature_accelerate(
|
||||
lineData,
|
||||
line,
|
||||
cornerPara,
|
||||
ptInterval,
|
||||
segs,
|
||||
line_cornerFeatures //拐点
|
||||
);
|
||||
cornerFeatures.push_back(line_cornerFeatures);
|
||||
//检查V型特征两侧，不能有跳变
|
||||
std::vector<SSG_basicFeature1D> valid_cornerFeatures;
|
||||
int vCornerSize = (int)line_cornerFeatures.size();
|
||||
int segSize = (int)segs.size();
|
||||
for (int m = 0; m < vCornerSize; m++)
|
||||
{
|
||||
int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
|
||||
for (int n = 0; n < segSize; n++)
|
||||
{
|
||||
int segEnd = segs[n].start + segs[n].len - 1;
|
||||
if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
|
||||
{
|
||||
int skip_1 = cornerPos - segs[n].start;
|
||||
int skip_2 = segEnd - cornerPos;
|
||||
if((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
|
||||
valid_cornerFeatures.push_back(line_cornerFeatures[m]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
cornerFeatures.push_back(valid_cornerFeatures);
|
||||
|
||||
//提取凸起段
|
||||
std::vector<SWD_segFeature> line_raisedFeatures;
|
||||
wd_getLineRaisedFeature(
|
||||
@ -110,6 +171,7 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
else
|
||||
{
|
||||
int validVCornerSCale = (int)(vCornerScale / lineInterval);
|
||||
//水平扫描检测V型槽和线尾
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
@ -120,14 +182,37 @@ void wd_bagThreadPositioning(
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
|
||||
//提取V型槽
|
||||
std::vector<SSG_basicFeature1D> line_cornerFeatures;
|
||||
std::vector<SSG_RUN_EX> segs;
|
||||
int dataSize = (int)lineData.size();
|
||||
wd_getLineCorerFeature(
|
||||
wd_getLineCorerFeature_accelerate(
|
||||
lineData,
|
||||
line,
|
||||
cornerPara,
|
||||
lineInterval,
|
||||
segs,
|
||||
line_cornerFeatures //拐点
|
||||
);
|
||||
cornerFeatures.push_back(line_cornerFeatures);
|
||||
//检查V型特征两侧，不能有跳变
|
||||
std::vector<SSG_basicFeature1D> valid_cornerFeatures;
|
||||
int vCornerSize = (int)line_cornerFeatures.size();
|
||||
int segSize = (int)segs.size();
|
||||
for (int m = 0; m < vCornerSize; m++)
|
||||
{
|
||||
int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
|
||||
for (int n = 0; n < segSize; n++)
|
||||
{
|
||||
int segEnd = segs[n].start + segs[n].len - 1;
|
||||
if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
|
||||
{
|
||||
int skip_1 = cornerPos - segs[n].start;
|
||||
int skip_2 = segEnd - cornerPos;
|
||||
if ((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
|
||||
valid_cornerFeatures.push_back(line_cornerFeatures[m]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
cornerFeatures.push_back(valid_cornerFeatures);
|
||||
//提取凸起段
|
||||
std::vector<SWD_segFeature> line_raisedFeatures;
|
||||
wd_getLineRaisedFeature(
|
||||
@ -158,44 +243,112 @@ void wd_bagThreadPositioning(
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
int cornerTreeNum = (int)cornerGrowTrees.size();
|
||||
int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
|
||||
int threadTailTreeIdx = -1; //线尾所在的tree
|
||||
if (raisedTreeNum == 1)
|
||||
threadTailTreeIdx = 0;
|
||||
else if (raisedTreeNum > 1)
|
||||
{
|
||||
//取最长的tree作为线尾所在的tree
|
||||
int maxLines = 0;
|
||||
threadTailTreeIdx = 0;
|
||||
for (int i = 0; i < raisedTreeNum; i++)
|
||||
{
|
||||
int treeLines = raisedFeatureGrowTrees[i].eLineIdx - raisedFeatureGrowTrees[i].sLineIdx;
|
||||
if (maxLines < treeLines)
|
||||
{
|
||||
maxLines = treeLines;
|
||||
threadTailTreeIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
int cornerTreeNum = (int)cornerGrowTrees.size();
|
||||
int objTreeIdx = -1;
|
||||
if (threadTailTreeIdx < 0)
|
||||
{
|
||||
int maxLines = 0;
|
||||
//取最长的tree作为线缝所有的生长树
|
||||
for (int i = 0; i < cornerTreeNum; i++)
|
||||
{
|
||||
int treeLines = cornerGrowTrees[i].eLineIdx - cornerGrowTrees[i].sLineIdx;
|
||||
if (maxLines < treeLines)
|
||||
{
|
||||
maxLines = treeLines;
|
||||
objTreeIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//取与线尾所在tree最近的tree作为线缝所在的生长树,计算2D距离，所以不需要区分水平和垂直扫描方式
|
||||
|
||||
SVzNL3DPoint pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].startPt;
|
||||
SVzNL3DPoint pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].endPt;
|
||||
SVzNL3DPoint tail_end_1 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
|
||||
pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().startPt;
|
||||
pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().endPt;
|
||||
SVzNL3DPoint tail_end_2 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
|
||||
double minDist = DBL_MAX;
|
||||
for (int i = 0; i < cornerTreeNum; i++)
|
||||
{
|
||||
//端点最近的距离为两个tree间的距离
|
||||
SVzNL3DPoint end_1 = cornerGrowTrees[i].treeNodes[0].jumpPos;
|
||||
SVzNL3DPoint end_2 = cornerGrowTrees[i].treeNodes.back().jumpPos;
|
||||
int lineIdx_2 = cornerGrowTrees[i].eLineIdx;
|
||||
double dist_11 = sqrt(pow(tail_end_1.x - end_1.x, 2) + pow(tail_end_1.y - end_1.y, 2));
|
||||
double dist_12 = sqrt(pow(tail_end_1.x - end_2.x, 2) + pow(tail_end_1.y - end_2.y, 2));
|
||||
double dist_21 = sqrt(pow(tail_end_2.x - end_1.x, 2) + pow(tail_end_2.y - end_1.y, 2));
|
||||
double dist_22 = sqrt(pow(tail_end_2.x - end_2.x, 2) + pow(tail_end_2.y - end_2.y, 2));
|
||||
double dist = dist_11 < dist_12 ? dist_11 : dist_12;
|
||||
dist = dist < dist_21 ? dist : dist_21;
|
||||
dist = dist < dist_22 ? dist : dist_22;
|
||||
if (minDist > dist)
|
||||
{
|
||||
minDist = dist;
|
||||
objTreeIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(objTreeIdx < 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
//显示
|
||||
//将原始数据的序列清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();
|
||||
int nodeNum = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
|
||||
for (int j = 0; j < nodeNum; j++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
if (false == scanInfo.isHorizonScan)
|
||||
{
|
||||
lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x;
|
||||
ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y;
|
||||
lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
|
||||
ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y;
|
||||
ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x;
|
||||
lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
|
||||
ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
|
||||
}
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < raisedTreeNum; i++)
|
||||
if (threadTailTreeIdx >= 0)
|
||||
{
|
||||
int nodeNum = (int)raisedFeatureGrowTrees[i].treeNodes.size();
|
||||
int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
|
||||
for (int j = 0; j < nodeNum; j++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
if (false == scanInfo.isHorizonScan)
|
||||
{
|
||||
lineIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx;
|
||||
for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++)
|
||||
lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
|
||||
for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
|
||||
{
|
||||
ptIdx = m;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
@ -203,8 +356,8 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
else
|
||||
{
|
||||
ptIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx;
|
||||
for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++)
|
||||
ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
|
||||
for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
|
||||
{
|
||||
lineIdx = m;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
@ -212,6 +365,178 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
}
|
||||
}
|
||||
//提取针脚(提取4个）
|
||||
std::vector<SVzNLRect> stitchROIs;
|
||||
int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
|
||||
//提取针脚范围
|
||||
int pre_idx = -1;
|
||||
for (int i = 0; i < nodeSize; i++)
|
||||
{
|
||||
int nodeIdx = (true == scanInfo.scanFromThreadHead) ? i : (nodeSize - 1 - i);
|
||||
SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];
|
||||
if(pre_idx >= 0)
|
||||
{
|
||||
SSG_basicFeature1D& pre_node = cornerGrowTrees[objTreeIdx].treeNodes[pre_idx];
|
||||
int line_diff = a_node.jumpPos2D.x < pre_node.jumpPos2D.x ? (pre_node.jumpPos2D.x - a_node.jumpPos2D.x) : (a_node.jumpPos2D.x - pre_node.jumpPos2D.x);
|
||||
if (line_diff > 2)
|
||||
{
|
||||
double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
|
||||
if (width > scanInfo.stitchWidth)
|
||||
{
|
||||
SVzNLRect a_stitch;
|
||||
memset(&a_stitch, 0, sizeof(SVzNLRect));
|
||||
if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x)
|
||||
{
|
||||
a_stitch.left = a_node.jumpPos2D.x;
|
||||
a_stitch.right = pre_node.jumpPos2D.x;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_stitch.left = pre_node.jumpPos2D.x;
|
||||
a_stitch.right = a_node.jumpPos2D.x;
|
||||
}
|
||||
if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y)
|
||||
{
|
||||
a_stitch.top = a_node.jumpPos2D.y;
|
||||
a_stitch.bottom = pre_node.jumpPos2D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_stitch.top = pre_node.jumpPos2D.y;
|
||||
a_stitch.bottom = a_node.jumpPos2D.y;
|
||||
}
|
||||
stitchROIs.push_back(a_stitch);
|
||||
if (stitchROIs.size() >= 4)
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
pre_idx = nodeIdx;
|
||||
}
|
||||
//提取针脚位置
|
||||
if (stitchROIs.size() == 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
//建立node下标与扫描线序号的索引
|
||||
std::vector<int> backIndexing;
|
||||
int indexingSize = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
|
||||
backIndexing.resize(indexingSize);
|
||||
for (int i = 0; i < indexingSize; i++)
|
||||
backIndexing[i] = -1;
|
||||
for (int i = 0; i < nodeSize; i++)
|
||||
{
|
||||
int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx;
|
||||
backIndexing[indexingIdx] = i;
|
||||
}
|
||||
|
||||
int opDist_lines;
|
||||
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
|
||||
opDist_lines = (int)(scanInfo.operateDist / lineInterval);
|
||||
else
|
||||
opDist_lines = (int)(scanInfo.operateDist / ptInterval);
|
||||
|
||||
for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++)
|
||||
{
|
||||
//搜索Z值最高点
|
||||
SVzNLRect& a_stitch = stitchROIs[i];
|
||||
SVzNL3DPoint stitchPos = { 0, 0, -1 };
|
||||
for (int j = a_stitch.left + 1; j < a_stitch.right; j++)
|
||||
{
|
||||
SVzNL3DPoint linePeak = { 0, 0, -1 };
|
||||
for (int m = a_stitch.top; m <= a_stitch.bottom; m++)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
|
||||
a_pt = scanLines[j][m].pt3D;
|
||||
else
|
||||
a_pt = scanLines[m][j].pt3D;
|
||||
if (a_pt.z > 1e-4)
|
||||
{
|
||||
if (linePeak.z < 0)
|
||||
linePeak = a_pt;
|
||||
else
|
||||
{
|
||||
if (linePeak.z > a_pt.z)
|
||||
linePeak = a_pt;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (linePeak.z > 1e-4)
|
||||
{
|
||||
if (stitchPos.z < 0)
|
||||
stitchPos = linePeak;
|
||||
else
|
||||
{
|
||||
if (stitchPos.z > linePeak.z)
|
||||
stitchPos = linePeak;
|
||||
}
|
||||
}
|
||||
}
|
||||
//搜索下刀位置
|
||||
if (stitchPos.z > 1e-4)
|
||||
{
|
||||
int op_centerLine;
|
||||
int searchStart, searchEnd;
|
||||
if (true == scanInfo.scanFromThreadHead)
|
||||
{
|
||||
op_centerLine = a_stitch.right + opDist_lines;
|
||||
searchStart = a_stitch.right;
|
||||
searchEnd = a_stitch.right + opDist_lines * 2;
|
||||
if (searchEnd > cornerGrowTrees[objTreeIdx].eLineIdx)
|
||||
searchEnd = cornerGrowTrees[objTreeIdx].eLineIdx;
|
||||
}
|
||||
else
|
||||
{
|
||||
op_centerLine = a_stitch.left - opDist_lines;
|
||||
searchEnd = a_stitch.left;
|
||||
searchStart = a_stitch.left - opDist_lines * 2;
|
||||
if (searchStart < cornerGrowTrees[objTreeIdx].sLineIdx)
|
||||
searchStart = cornerGrowTrees[objTreeIdx].sLineIdx;
|
||||
}
|
||||
//寻找最合适点
|
||||
int best_idx = -1;
|
||||
int minLineDist = INT_MAX;
|
||||
for (int j = searchStart; j <= searchEnd; j++)
|
||||
{
|
||||
int indexingIdx = j - cornerGrowTrees[objTreeIdx].sLineIdx;
|
||||
if (backIndexing[indexingIdx] >= 0)
|
||||
{
|
||||
int lineDist = j - op_centerLine;
|
||||
if (lineDist < 0)
|
||||
lineDist = -lineDist;
|
||||
if (minLineDist > lineDist)
|
||||
{
|
||||
minLineDist = lineDist;
|
||||
best_idx = backIndexing[indexingIdx];
|
||||
}
|
||||
}
|
||||
}
|
||||
if (best_idx >= 0)
|
||||
{
|
||||
int op_lineIdx, op_ptIdx;
|
||||
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
|
||||
{
|
||||
op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
|
||||
op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
|
||||
op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
|
||||
}
|
||||
SSX_bagThreadInfo a_stitchInfo;
|
||||
memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
|
||||
a_stitchInfo.threadPos = stitchPos;
|
||||
a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D;
|
||||
a_stitchInfo.rotateAngle = 0;
|
||||
bagThreadInfo.push_back(a_stitchInfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -12,6 +12,13 @@ typedef struct
|
||||
double rotateAngle; //水平旋转角
|
||||
}SSX_bagThreadInfo; //线头信息
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool isHorizonScan;
|
||||
bool scanFromThreadHead;
|
||||
double stitchWidth; //针脚最小宽度，用于过滤虚假的针脚
|
||||
double operateDist; //下刀位置距针脚距离
|
||||
}SSX_ScanInfo;
|
||||
|
||||
//读版本号
|
||||
SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void);
|
||||
@ -19,7 +26,7 @@ SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void);
|
||||
//线头位置检测定位
|
||||
SG_APISHARED_EXPORT void wd_bagThreadPositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const SSG_outlierFilterParam filterParam, //噪点过滤参数
|
||||
const SSG_cornerParam cornerPara, //V型特征参数
|
||||
const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user