diff --git a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp index 5915c23..f3854c0 100644 --- a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp +++ b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp @@ -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 bagThreadInfo; wd_bagThreadPositioning( scanLines, - isHorizonScan, //true:激光线平行线缝；false:激光线垂直线缝 + scanInfo, //true:激光线平行线缝；false:激光线垂直线缝 filterParam, //噪点过滤参数 cornerParam, //V型特征参数 raisedFeatureParam,//线尾凸起参数 diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 15c4971..885db6b 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -166,6 +166,24 @@ SG_APISHARED_EXPORT void wd_getLineCorerFeature( std::vector& line_cornerFeatures //յ ); +/// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 +/// ʹƽм +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCorerFeature_accelerate( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + const double pointInterval, + std::vector& segs, + std::vector& line_cornerFeatures //յ +); + //ȡ͹ SG_APISHARED_EXPORT void wd_getLineRaisedFeature( std::vector< SVzNL3DPosition>& lineData, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 74c0552..8acbabe 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -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жϹսǷΪ +/// +void wd_getLineCorerFeature_accelerate( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + const double pointInterval, + std::vector& segs, + std::vector& 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, diff --git a/sourceCode/bagThreadPositioning.cpp b/sourceCode/bagThreadPositioning.cpp index 1339b7c..a7dc060 100644 --- a/sourceCode/bagThreadPositioning.cpp +++ b/sourceCode/bagThreadPositioning.cpp @@ -15,7 +15,7 @@ const char* wd_bagThreadPositioningVersion(void) //ͷλüⶨλ void wd_bagThreadPositioning( std::vector< std::vector>& 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> 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> cornerFeatures; std::vector> 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 line_cornerFeatures; + std::vector 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 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 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 line_cornerFeatures; + std::vector 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 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 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 + { + //ȡβtreetreeΪ߷ڵ,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 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 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 diff --git a/sourceCode/bagThreadPositioning_Export.h b/sourceCode/bagThreadPositioning_Export.h index 43430b3..0754ccc 100644 --- a/sourceCode/bagThreadPositioning_Export.h +++ b/sourceCode/bagThreadPositioning_Export.h @@ -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>& scanLines, - bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSX_ScanInfo scanInfo, //true:ƽв۵false:ߴֱ۵ const SSG_outlierFilterParam filterParam, //˲ const SSG_cornerParam cornerPara, //V const SSG_raisedFeatureParam raisedFeaturePara,//β͹