diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 53a67aa..456aaba 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -39,6 +39,13 @@ typedef struct SWD3DPoint point; }SWDIndexing3DPoint; +typedef struct +{ + int lineIdx; + int ptIdx; + SVzNL3DPoint point; +}SWDIndexingVzPoint; + typedef struct { int start; @@ -604,6 +611,7 @@ typedef struct double backwardDiffZ; double pre_stepDist; double post_stepDist; + double curr_z; double forward_z; double backward_z; }SSG_pntDirAngle; diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 2b493b5..f3052b7 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -32,6 +32,10 @@ //汽车轮眉高度测量 #define SX_ERR_INVALID_ARC -2301 +#define SX_ERR_INVALID_WHEEL_EDGE -2302 +#define SX_ERR_NO_WHEEL_COMMON_EDGE -2303 +#define SX_ERR_NO_WHEEL_ARC -2304 + //糖袋子拆线 #define SX_ERR_NO_MARK -2401 diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index bfc1df7..267013e 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3979,6 +3979,9 @@ void wd_computeDirAngle_wholeLine( int dataSize = (int)line_data.size(); for (int i = 0; i < dataSize; i++) { + if (i == 930) + int kkk = 1; + if (line_data[i].pt3D.z < 1e-4) { ptDirAngles[i].pntIdx = i; @@ -3988,6 +3991,7 @@ void wd_computeDirAngle_wholeLine( ptDirAngles[i].forwardAngle = 0; ptDirAngles[i].backwardAngle = 0; ptDirAngles[i].corner = 0; + ptDirAngles[i].curr_z = 0; ptDirAngles[i].forwardDiffZ = 0; ptDirAngles[i].backwardDiffZ = 0; continue; @@ -3995,14 +3999,15 @@ void wd_computeDirAngle_wholeLine( } //前向寻找 int pre_i = -1; + double pre_dist = 0; for (int j = i - 1; j >= 0; j--) { if (line_data[j].pt3D.z < 1e-4) continue; - double dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) + + pre_dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) + pow(line_data[i].pt3D.z - line_data[j].pt3D.z, 2)); - if (dist >= cornerPara.scale) + if (pre_dist >= cornerPara.scale) { pre_i = j; break; @@ -4010,11 +4015,14 @@ void wd_computeDirAngle_wholeLine( } //后向寻找 int post_i = -1; + double post_dist = 0; for (int j = i + 1; j < dataSize; j++) { - double dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) + + if (line_data[j].pt3D.z < 1e-4) + continue; + post_dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) + pow(line_data[i].pt3D.z - line_data[j].pt3D.z, 2)); - if (dist >= cornerPara.scale) + if (post_dist >= cornerPara.scale) { post_i = j; break; @@ -4030,6 +4038,7 @@ void wd_computeDirAngle_wholeLine( ptDirAngles[i].forwardPntIdx = 0; ptDirAngles[i].backwardPntIdx = 0; ptDirAngles[i].corner = 0; + ptDirAngles[i].curr_z = 0; ptDirAngles[i].forwardDiffZ = 0; ptDirAngles[i].backwardDiffZ = 0; } @@ -4048,6 +4057,7 @@ void wd_computeDirAngle_wholeLine( ptDirAngles[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号 ptDirAngles[i].forwardDiffZ = line_data[post_i].pt3D.z - line_data[i].pt3D.z; ptDirAngles[i].backwardDiffZ = line_data[i].pt3D.z - line_data[pre_i].pt3D.z; + ptDirAngles[i].curr_z = line_data[i].pt3D.z; } } } diff --git a/sourceCode/wheelArchHeigthMeasure.cpp b/sourceCode/wheelArchHeigthMeasure.cpp index d06fbe8..7f36d4b 100644 --- a/sourceCode/wheelArchHeigthMeasure.cpp +++ b/sourceCode/wheelArchHeigthMeasure.cpp @@ -10,7 +10,8 @@ //version 1.2.0 : 修正了地面调平，防止法向量旋转180度 //version 1.3.0 : 修正了寻找上下点使用固定门限的问题 //version 1.3.1 : 添加了一个检测轮毂是否存在的函数 -std::string m_strVersion = "1.3.1"; +//version 1.3.2 : 针对不理想点云改进了算法，增强鲁棒性 +std::string m_strVersion = "1.3.2"; const char* wd_wheelArchHeigthMeasureVersion(void) { return m_strVersion.c_str(); @@ -273,6 +274,249 @@ bool wd_wheelPresenseDetection( } +void _lineProc_getWheelEnding( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + std::vector& line_upEnding, + std::vector& line_downEnding, + double scale_segGap = 10.0, + double scale_endingAngleLen = 25.0, //端点方向角计算尺度 + double wheel_endingAngleTh = 30) +{ + int dataSize = (int)lineData.size(); + std::vector segs; + SSG_RUN a_run = { 0, -1, 0, }; //startIdx, len, lastIdx + SVzNL3DPosition preData = { -1, {0, 0, 0} }; + for (int i = 0; i < dataSize; i++) + { + if ( (lineData[i].pt3D.z > 1e-4) && (lineData[i].nPointIdx == 1)) //wheel + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double dist = sqrt(pow(lineData[i].pt3D.y - preData.pt3D.y, 2) + pow(lineData[i].pt3D.z - preData.pt3D.z, 2)); + if (dist < scale_segGap) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + preData = lineData[i]; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //逐段计算端点方向角 + for (int i = 0; i < (int)segs.size(); i++) + { + SSG_RUN& a_seg = segs[i]; + int sPtIdx = a_seg.start; + int ePtIdx = a_seg.value; + //计算起点方向角 + for (int j = sPtIdx; j < ePtIdx; j++) + { + if (lineData[j].pt3D.z > 1e-4) + { + double dist = sqrt(pow(lineData[j].pt3D.y - lineData[sPtIdx].pt3D.y, 2) + + pow(lineData[j].pt3D.z - lineData[sPtIdx].pt3D.z, 2)); + if (dist > scale_endingAngleLen) + { + double tanValue_post = (lineData[j].pt3D.z - lineData[sPtIdx].pt3D.z) / abs(lineData[j].pt3D.y - lineData[sPtIdx].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double corner = -(forwardAngle - 0); + if (corner > wheel_endingAngleTh) + { + SWDIndexingVzPoint a_upEndig; + a_upEndig.lineIdx = lineIdx; + a_upEndig.ptIdx = sPtIdx; + a_upEndig.point = lineData[sPtIdx].pt3D; + line_upEnding.push_back(a_upEndig); + } + break; + } + } + } + //计算终点方向角 + for (int j = ePtIdx; j > sPtIdx; j--) + { + if (lineData[j].pt3D.z > 1e-4) + { + double dist = sqrt(pow(lineData[ePtIdx].pt3D.y - lineData[j].pt3D.y, 2) + + pow(lineData[ePtIdx].pt3D.z - lineData[j].pt3D.z, 2)); + if (dist > scale_endingAngleLen) + { + double tanValue_pre = (lineData[ePtIdx].pt3D.z - lineData[j].pt3D.z) / abs(lineData[ePtIdx].pt3D.y - lineData[j].pt3D.y); + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + double corner = -(0 - backwardAngle); + if (corner > wheel_endingAngleTh) + { + SWDIndexingVzPoint a_downEndig; + a_downEndig.lineIdx = lineIdx; + a_downEndig.ptIdx = ePtIdx; + a_downEndig.point = lineData[ePtIdx].pt3D; + line_downEnding.push_back(a_downEndig); + } + break; + } + } + } + } +} + +typedef struct +{ + int treeState; + int treeType; + int sLineIdx; + int eLineIdx; + SSG_ROIRectD roi; + std::vector< SWDIndexingVzPoint> treeNodes; +}SSG_endingTree; + +#define _TREE_STATE_UNDEF 0 +#define _TREE_STATE_ALIVE 1 +#define _TREE_STATE_DEAD 2 +//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false +//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。 +bool _endingFeatureGrowing(SWDIndexingVzPoint& a_feature, std::vector& trees, SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) + { + SSG_endingTree& a_tree = trees[i]; + if (_TREE_STATE_DEAD == a_tree.treeState) + continue; + //检查生长点 + SWDIndexingVzPoint last_node = a_tree.treeNodes.back(); + if (last_node.lineIdx == a_feature.lineIdx) //x为lineIdx，同一条扫描线上的不进行生长 + continue; + + //判断生长点 + double y_diff = abs(a_feature.point.y - last_node.point.y); + double z_diff = abs(a_feature.point.z - last_node.point.z); + int line_diff = abs(a_feature.lineIdx - last_node.lineIdx); + double x_diff = abs(a_feature.point.x - last_node.point.x); + if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) && + ((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance))) + { + a_tree.eLineIdx = a_feature.lineIdx; + a_tree.treeNodes.push_back(a_feature); + return true; + } + } + return false; +} +void _getWheelEndingGrowingTrees( + std::vector>& endings, + std::vector& trees, + SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)endings.size(); i < i_max; i++) + { + std::vector& line_endings = endings[i]; + if (line_endings.size() > 0) + { + for (int j = 0, j_max = (int)line_endings.size(); j < j_max; j++) + { + SWDIndexingVzPoint& an_ending = line_endings[j]; + bool isMatched = _endingFeatureGrowing(an_ending, trees, growParam); + if (false == isMatched) + { + //新的生长树 + SSG_endingTree a_newTree; + a_newTree.treeNodes.push_back(an_ending); + a_newTree.treeState = _TREE_STATE_ALIVE; + a_newTree.treeType = 0; + a_newTree.sLineIdx = an_ending.lineIdx; + a_newTree.eLineIdx = an_ending.lineIdx; + trees.push_back(a_newTree); + } + } + } + //检查停止生长的树,加速。 + //将生长节点为1的生长树移除 + int lineIdx = i; + int m_max = (int)trees.size(); + for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 + { + if (_TREE_STATE_ALIVE == trees[m].treeState) + { + int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + trees[m].treeState = _TREE_STATE_DEAD; + SWDIndexingVzPoint first_node = trees[m].treeNodes[0]; + SWDIndexingVzPoint last_node = trees[m].treeNodes.back(); + double len = sqrt(pow(first_node.point.x - last_node.point.x, 2) + pow(first_node.point.y - last_node.point.y, 2)); + if (len <= growParam.minVTypeTreeLen) + trees.erase(trees.begin() + m); + } + } + } + } +} + +double _getTopY(SSG_endingTree& wheedEdge) +{ + double minY = DBL_MAX; + for (int i = 0; i < (int)wheedEdge.treeNodes.size(); i++) + minY = minY > wheedEdge.treeNodes[i].point.y ? wheedEdge.treeNodes[i].point.y : minY; + return minY; +} + +double _getBtmY(SSG_endingTree& wheedEdge) +{ + double maxY = DBL_MIN; + for (int i = 0; i < (int)wheedEdge.treeNodes.size(); i++) + maxY = maxY < wheedEdge.treeNodes[i].point.y ? wheedEdge.treeNodes[i].point.y : maxY; + return maxY; +} + +bool _compareByLineIdx(SSG_endingTree& a, SSG_endingTree& b) +{ + return a.sLineIdx < b.sLineIdx; +} + +void _genEdgeContinuousPos(SSG_endingTree&edgeTree, std::vector& edgePos) +{ + for (int i = 0; i < (int)edgeTree.treeNodes.size(); i++) + { + SVzNL2DPoint a_pos = { edgeTree.treeNodes[i].lineIdx, edgeTree.treeNodes[i].ptIdx }; + if (edgePos.size() == 0) + edgePos.push_back(a_pos); + else + { + SVzNL2DPoint last_pos = edgePos.back(); + if ((a_pos.x - last_pos.x) == 1) + edgePos.push_back(a_pos); + else //需要插值 + { + for (int j = last_pos.x + 1; j < a_pos.x; j++) + { + int meany = (last_pos.y + a_pos.y) / 2; + SVzNL2DPoint new_pos = {j, meany}; + edgePos.push_back(new_pos); + } + edgePos.push_back(a_pos); + } + } + } +} + //轮眉高度测量 WD_wheelArchInfo wd_wheelArchHeigthMeasure( std::vector< std::vector>& scanLines, @@ -284,6 +528,17 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( int* errCode) { *errCode = 0; + + //内部参数 + //使用尺度1确定轮胎上沿和下沿点 + double scale_segGap = 10.0; + double scale_endingAngleLen = 25.0; //端点方向角计算尺度 + double wheel_endingAngleTh = 25; + double arcScale_1 = 10.0; + double arcScale_2 = 5.0; + double computeLen = 150.0; + int safeGuardLen = 20; + WD_wheelArchInfo result; memset(&result, 0, sizeof(WD_wheelArchInfo)); @@ -417,7 +672,7 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( { int lineIdx = cluster_wheel[i].x; int ptIdx = cluster_wheel[i].y; - scanLines[lineIdx][ptIdx].nPointIdx = 1; + scanLines[lineIdx][ptIdx].nPointIdx = 1; //wheel } for (int i = 0, i_max = (int)cluster_arc.size(); i < i_max; i++) { @@ -426,41 +681,177 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( scanLines[lineIdx][ptIdx].nPointIdx = 2; } - //寻找轮眉点 - //(1)快速寻找下端点（2）取中间区域，精确确定端点（3）抛物线拟合（4）计算轮眉点（最高点） - std::vector arcEndings; - _getArcEndings(scanLines, 2, arcEndings); //轮眉的ID是2 - if (arcEndings.size() == 0) + std::vector> upEndings; + std::vector> downEndings; + for (int line = 0; line < lineNum; line++) { - *errCode = SX_ERR_INVALID_ARC; + if (line == 319) + int kkk = 1; + std::vector line_upEnding; + std::vector line_downEnding; + _lineProc_getWheelEnding(scanLines[line], line, line_upEnding, line_downEnding); + upEndings.push_back(line_upEnding); + downEndings.push_back(line_downEnding); + } + //生长聚类 + std::vector upEndingTrees; + _getWheelEndingGrowingTrees(upEndings, upEndingTrees, growParam); + + std::vector downEndingTrees; + _getWheelEndingGrowingTrees(downEndings, downEndingTrees, growParam); + + //确定轮胎上沿 + if ( (upEndingTrees.size() == 0) || (downEndingTrees.size() == 0)) + { + *errCode = SX_ERR_INVALID_WHEEL_EDGE; return result; } - int arcMidPos = _getArcEndingsCenterPos(scanLines, arcEndings); - //取两侧固定长度（100mm内的点，进行精确边界提取，拟合） - double chkWin = 100.0; - std::vector< SVzNL2DPoint> arcFittingPos; - _extractArcFittingPts( - scanLines, - arcEndings, - chkWin, - arcMidPos, - arcFittingPos); - //在XY平面生成拟合点 - double outLineLen = 200; - SVzNL2DPoint arcPtPos; - SVzNL3DPoint arcPt = _getWheelArcFittingPoint(scanLines, 3, arcFittingPos, arcPtPos); - result.wheelArchPos = arcPt; - result.arcLine[0] = { arcPt.x - outLineLen, arcPt.y, arcPt.z }; - result.arcLine[1] = { arcPt.x + outLineLen, arcPt.y, arcPt.z }; - + //以最上面的为轮子边沿 + SSG_endingTree& wheedEdge_up = upEndingTrees[0]; + if (upEndingTrees.size() > 1) + { + double minY = _getTopY(wheedEdge_up); + for (int i = 1; i < (int)upEndingTrees.size(); i++) + { + double topY = _getTopY(upEndingTrees[i]); + if (minY > topY) + { + wheedEdge_up = upEndingTrees[i]; + minY = topY; + } + } + } + + for (int i = 0; i < (int)downEndingTrees.size(); i++) + { + double btmY_0 = _getBtmY(downEndingTrees[i]); + int left_0 = downEndingTrees[i].treeNodes[0].lineIdx; + int right_0 = downEndingTrees[i].treeNodes.back().lineIdx; + for (int j = i + 1; j < (int)downEndingTrees.size(); j++) + { + if (downEndingTrees[j].treeType < 0) + continue; + + double btmY_1 = _getBtmY(downEndingTrees[j]); + int left_1 = downEndingTrees[j].treeNodes[0].lineIdx; + int right_1 = downEndingTrees[j].treeNodes.back().lineIdx; + if ((right_1 > left_0) && (right_0 > left_1)) + { + if (btmY_0 < btmY_1) + downEndingTrees[i].treeType = -1; + else + downEndingTrees[j].treeType = -1; + } + } + } + std::vector validDownEndingTrees; + for (int i = 0; i < (int)downEndingTrees.size(); i++) + { + if (downEndingTrees[i].treeType >= 0) + validDownEndingTrees.push_back(downEndingTrees[i]); + } + //排序 + std::sort(validDownEndingTrees.begin(), validDownEndingTrees.end(), _compareByLineIdx); + std::vector downEdgePos; + for (int i = 0; i < (int)validDownEndingTrees.size(); i++) + _genEdgeContinuousPos(validDownEndingTrees[i], downEdgePos); + + //生成每条扫描线的上沿。空的点进行插值得到 + std::vector upEdgePos; + _genEdgeContinuousPos(wheedEdge_up, upEdgePos); + + int start_up, start_down; + if (upEdgePos[0].x < downEdgePos[0].x) + { + start_down = 0; + start_up = downEdgePos[0].x - upEdgePos[0].x; + } + else + { + start_up = 0; + start_down = upEdgePos[0].x - downEdgePos[0].x; + } + int commonNum = (int)upEdgePos.size() - start_up; + int num2 = (int)downEdgePos.size() - start_down; + if ((commonNum < 0) || (num2 < 0)) + { + *errCode = SX_ERR_NO_WHEEL_COMMON_EDGE; + return result; + } + + std::vector all_edgePairs; + commonNum = commonNum > num2 ? num2 : commonNum; + for (int i = 0; i < commonNum; i++) + { + SSG_intPair a_pair; + a_pair.idx = upEdgePos[i + start_up].x; + a_pair.data_0 = upEdgePos[i + start_up].y; + a_pair.data_1 = downEdgePos[i + start_down].y; + all_edgePairs.push_back(a_pair); + } + + //提取轮胎上沿最小值，然后取左右100mm长范围内V型槽 + double wheelTopY = DBL_MAX; + SSG_intPair wheelTopPos = { -1, -1, -1 }; + int topIdx = -1; + for (int i = 0; i < (int)all_edgePairs.size(); i++) + { + if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4) + continue; + + if (wheelTopY > scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y) + { + wheelTopY = scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y; + wheelTopPos = all_edgePairs[i]; + topIdx = i; + } + } + int startIdx = -1; + for (int i = topIdx; i >= 0; i--) + { + if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4) + continue; + + double dist = sqrt(pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.x - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.x, 2) + + pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.y - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y, 2)); + if (dist > computeLen) + { + startIdx = i; + break; + } + } + if (startIdx < 0) + startIdx = 0; + + int endIdx = -1; + for (int i = topIdx; i < (int)all_edgePairs.size(); i++) + { + if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4) + continue; + + double dist = sqrt(pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.x - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.x, 2) + + pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.y - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y, 2)); + if (dist > computeLen) + { + endIdx = i; + break; + } + } + if (endIdx < 0) + endIdx = (int)all_edgePairs.size() - 1; + + std::vector edgePairs; + for (int i = startIdx; i <= endIdx; i++) + edgePairs.push_back(all_edgePairs[i]); + //提取轮毂特征：V型槽。连长6mm，角度最大的V型槽 std::vector< SVzNL2DPoint> upWheelPos; std::vector< SVzNL2DPoint> downWheelPos; - int startLine = arcFittingPos[0].x; - int endLine = arcFittingPos.back().x; + int startLine = edgePairs[0].idx; + int endLine = edgePairs.back().idx; SSG_cornerParam wheelCornerPara; memset(&wheelCornerPara, 0, sizeof(SSG_cornerParam)); - wheelCornerPara.scale = 6.0; + wheelCornerPara.scale = 5.0; wheelCornerPara.cornerTh = cornerPara.cornerTh; for (int line = startLine; line <= endLine; line++) { @@ -471,26 +862,94 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( 1, //车轮的ID为1 wheelCornerPara, //scale通常取bagH的1/4 cornerFeatures); - //取第一个为up - if (cornerFeatures.size() >= 2) + + int pairIdx = line - startLine; + SSG_intPair& a_pair = edgePairs[pairIdx]; + int interval = (a_pair.data_1 - a_pair.data_0) / 3; + if (cornerFeatures.size() >= 0) { - upWheelPos.push_back(cornerFeatures[0].jumpPos2D); - downWheelPos.push_back(cornerFeatures.back().jumpPos2D); + for (int m = 0; m < (int)cornerFeatures.size(); m++) + { + + if ((cornerFeatures[m].jumpPos2D.y > (a_pair.data_0 + safeGuardLen)) && + (cornerFeatures[m].jumpPos2D.y < (a_pair.data_0 + interval)) ) + + { + upWheelPos.push_back(cornerFeatures[m].jumpPos2D); + break; + } + } + for (int m = (int)cornerFeatures.size() - 1; m << (int)cornerFeatures.size() >= 0; m--) + { + if ((cornerFeatures[m].jumpPos2D.y < (a_pair.data_1 - safeGuardLen)) && + (cornerFeatures[m].jumpPos2D.y > (a_pair.data_1 - interval))) + { + downWheelPos.push_back(cornerFeatures[m].jumpPos2D); + break; + } + } + } } SVzNL2DPoint upPos; SVzNL3DPoint upWheelPt = _getWheelArcFittingPoint(scanLines, 4, upWheelPos, upPos); + SVzNL2DPoint downPos; + SVzNL3DPoint downWheelPt = _getWheelArcFittingPoint(scanLines, 5, downWheelPos, downPos); + //计算直线 + double _a, _b, _c; + compute2ptLine( downWheelPt, upWheelPt, &_a, &_b, &_c); + double norm = sqrt(_a * _a + _b * _b); + _a = _a / norm; + _b = _b / norm; + _c = _c / norm; + + //寻找轮眉点 + //(1)快速寻找下端点（2）取中间区域，精确确定端点（3）抛物线拟合（4）计算轮眉点（最高点） + std::vector arcEndings; + _getArcEndings(scanLines, 2, arcEndings); //轮眉的ID是2 + if (arcEndings.size() == 0) + { + *errCode = SX_ERR_INVALID_ARC; + return result; + } + //取与直线距离最小的点为轮眉点 + double minDist = -1; + SVzNL2DPoint arcPos; + for (int i = 0; i < (int)arcEndings.size(); i++) + { + SVzNL3DPoint a_pt = scanLines[arcEndings[i].x][arcEndings[i].y].pt3D; + double dist = abs(a_pt.x * _a + a_pt.y * _b + _c); + if (minDist < -1e-4) + { + minDist = dist; + arcPos = arcEndings[i]; + } + else if (minDist > dist) + { + minDist = dist; + arcPos = arcEndings[i]; + } + } + if (minDist < -1e-4) + { + *errCode = SX_ERR_NO_WHEEL_ARC; + return result; + } + //在XY平面生成拟合点 + double outLineLen = 200; + SVzNL3DPoint arcPt = scanLines[arcPos.x][arcPos.y].pt3D; + result.wheelArchPos = arcPt; + result.arcLine[0] = { arcPt.x - outLineLen, arcPt.y, arcPt.z }; + result.arcLine[1] = { arcPt.x + outLineLen, arcPt.y, arcPt.z }; result.wheelUpPos = upWheelPt; result.upLine[0] = { upWheelPt.x - outLineLen, upWheelPt.y, upWheelPt.z }; result.upLine[1] = { upWheelPt.x + outLineLen, upWheelPt.y, upWheelPt.z }; - SVzNL2DPoint downPos; - SVzNL3DPoint downWheelPt = _getWheelArcFittingPoint(scanLines, 5, downWheelPos, downPos); result.wheelDownPos = downWheelPt; result.downLine[0] = { downWheelPt.x - outLineLen, downWheelPt.y, downWheelPt.z }; result.downLine[1] = { downWheelPt.x + outLineLen, downWheelPt.y, downWheelPt.z }; double centerY = (upWheelPt.y + downWheelPt.y) / 2; int searchLine = (upPos.x + downPos.x) / 2; - double minDist = DBL_MAX; + minDist = DBL_MAX; int minPtIdx = 0; for (int i = 0; i < (int)scanLines[searchLine].size(); i++) { diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index c767174..7aeac8b 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -960,17 +960,21 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos, std::vector>& pntDirAngles_v, std::vector>& pntDirAngles_h, double pntCornenrMax, + double minZHeight, SVzNLRect& roi ) { int lineNum = (int)pntDirAngles_v.size(); int ptNum = (int)pntDirAngles_h.size(); + double seed_z = pntDirAngles_v[seedPos.x][seedPos.y].curr_z; //鍚戝乏鎼滅储 int left = -1; for (int line = seedPos.x; line >= 0; line--) { - if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax)) + double z_diff = seed_z - pntDirAngles_v[line][seedPos.y].curr_z; + if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax) + && (z_diff > minZHeight)) { left = line; break; @@ -980,7 +984,9 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos, int right = -1; for (int line = seedPos.x; line < lineNum; line++) { - if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax)) + double z_diff = seed_z - pntDirAngles_v[line][seedPos.y].curr_z; + if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax) + && (z_diff > minZHeight)) { right = line; break; @@ -990,7 +996,9 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos, int top = -1; for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--) { - if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax)) + double z_diff = seed_z - pntDirAngles_h[ptIdx][seedPos.x].curr_z; + if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax) + && (z_diff > minZHeight)) { top = ptIdx; break; @@ -1000,7 +1008,9 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos, int bottom = -1; for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++) { - if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax)) + double z_diff = seed_z - pntDirAngles_h[ptIdx][seedPos.x].curr_z; + if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax) + && (z_diff > minZHeight)) { bottom = ptIdx; break; @@ -1069,13 +1079,12 @@ void wd_HolePositioning( double flagCornerMax = 3.0; //鐩寸嚎涓婄偣鐨勬嫄瑙掓渶澶у double maxHoleDiameter = 30.0;//鏈澶х殑瀛旂洿寰 double valieCommonNumRatio = 0.125; //1/8 - //璁＄畻dirAngle std::vector> pntDirAngles_v; std::vector> zMaxPeaks_v; for (int line = 0; line < lineNum; line++) { - if (line == 152) + if (line == 1062) int kkk = 1; std::vector& lineData = scanLines[line]; @@ -1159,19 +1168,28 @@ void wd_HolePositioning( { //鍒ゆ柇ROI SVzNLRect a_roi; - bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, a_roi); + bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, minZPeakHeight, a_roi); if (true == validPk) { double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z; + double minZ = scanLines[a_roi.left][idx_pt].pt3D.z; + double maxZ = scanLines[a_roi.left][idx_pt].pt3D.z; meanZ += scanLines[a_roi.right][idx_pt].pt3D.z; + minZ = minZ < scanLines[a_roi.right][idx_pt].pt3D.z ? minZ : scanLines[a_roi.right][idx_pt].pt3D.z; + maxZ = maxZ > scanLines[a_roi.right][idx_pt].pt3D.z ? maxZ : scanLines[a_roi.right][idx_pt].pt3D.z; meanZ += scanLines[idx_line][a_roi.top].pt3D.z; + minZ = minZ < scanLines[idx_line][a_roi.top].pt3D.z ? minZ : scanLines[idx_line][a_roi.top].pt3D.z; + maxZ = maxZ > scanLines[idx_line][a_roi.top].pt3D.z ? maxZ : scanLines[idx_line][a_roi.top].pt3D.z; meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z; + minZ = minZ < scanLines[idx_line][a_roi.bottom].pt3D.z ? minZ : scanLines[idx_line][a_roi.bottom].pt3D.z; + maxZ = maxZ > scanLines[idx_line][a_roi.bottom].pt3D.z ? maxZ : scanLines[idx_line][a_roi.bottom].pt3D.z; + double zMaxMin = maxZ - minZ; meanZ = meanZ / 4.0; double z_diff = a_lineZMax[j].jumpPos.z - meanZ; double xLen = abs(scanLines[a_roi.right][idx_pt].pt3D.x - scanLines[a_roi.left][idx_pt].pt3D.x); double yLen = abs(scanLines[idx_line][a_roi.bottom].pt3D.y - scanLines[idx_line][a_roi.top].pt3D.y); - if ( (z_diff > minZPeakHeight) && (xLen < maxHoleDiameter) && (yLen < maxHoleDiameter)) + if ( (z_diff > minZPeakHeight) && (xLen < maxHoleDiameter) && (yLen < maxHoleDiameter) && (zMaxMin < planeInlierDistTh*2)) { _zMaxInfo a_info; a_info.flag = 0; diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 20081c8..6ed16b4 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -2637,9 +2637,9 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData #define TEST_CONVERT_TO_GRID 0 #define TEST_COMPUTE_WHEEL_ARCH 1 -#define TEST_COMPUTE_CALIB_PARA 1 +#define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_GROUP 1 +#define TEST_GROUP 2 int main() { #if TEST_CONVERT_TO_GRID @@ -2716,11 +2716,12 @@ int main() #if TEST_COMPUTE_WHEEL_ARCH const char* dataPath[TEST_GROUP] = { - "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁/", //1 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,2}, + {1,2}, {1,4} }; SSG_planeCalibPara poseCalibPara; @@ -2744,14 +2745,14 @@ int main() char _scan_file[256]; int endGroup = TEST_GROUP - 1; - for (int grp = 0; grp <= endGroup; grp++) + for (int grp = 1; grp <= endGroup; grp++) { char calibFile[250]; sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); poseCalibPara = _readCalibPara(calibFile); for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 193; + //fidx = 4; sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); std::vector> scanData; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); @@ -2783,17 +2784,17 @@ int main() SSG_treeGrowParam growParam; growParam.maxLineSkipNum = 10; - growParam.yDeviation_max = 5.0; - growParam.maxSkipDistance = 5.0; - growParam.zDeviation_max = 2.0;// + growParam.yDeviation_max = 10.0; + growParam.maxSkipDistance = 10.0; + growParam.zDeviation_max = 10.0;// growParam.minLTypeTreeLen = 100; //mm growParam.minVTypeTreeLen = 100; //mm WD_wheelArchInfo wheelArcHeight; memset(&wheelArcHeight, 0, sizeof(WD_wheelArchInfo)); - bool wheePresense = wd_wheelPresenseDetection( scanData, wheelRoi3d); - if (true == wheePresense) + bool wheelPresense = wd_wheelPresenseDetection( scanData, wheelRoi3d); + if (true == wheelPresense) { int lineNum = (int)scanData.size(); for (int i = 0; i < lineNum; i++) @@ -2802,7 +2803,7 @@ int main() int kkk = 1; //琛屽鐞 //璋冨钩锛屽幓闄ゅ湴闈 - wd_horizonCamera_lineDataR(scanData[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight - 5); + wd_horizonCamera_lineDataR(scanData[i], poseCalibPara.planeCalib, -1); // poseCalibPara.planeHeight - 5); } #if 0 //鏁版嵁杞瓨 sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx); @@ -2819,17 +2820,23 @@ int main() growParam, poseCalibPara, &errCode); - } + long t2 = GetTickCount64(); char _dbg_file[256]; #if 1 - sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight); + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight); #endif - printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); + printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); + } + else + { + printf("%s: no Wheel!", _scan_file); + } + } } #endif diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 825c903..2c2a882 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -811,26 +811,29 @@ void TuoPuFa_holePosition_test(void) } //鍗庤埅瀛斿畾浣 -#define HH_TEST_GROUP 4 +#define HH_TEST_GROUP 6 void HuaHang_holePosition_test(void) { const char* dataPath[HH_TEST_GROUP] = { "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁1/", //0 "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/1/", //1 - "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/2/", //1 - "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/3/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/2/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/3/", //3 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁4/濂界殑/", //4 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁4/鍧忕殑/", //5 }; SVzNLRange fileIdx[HH_TEST_GROUP] = { - {1,15}, {1, 21},{1,19}, {1,13} + {1,15}, {1, 21},{1,19}, {1,13}, + {1, 4}, {4,13}, }; const char* ver = wd_workpieceHolePositioningVersion(); printf("ver:%s\n", ver); #if TEST_COMPUTE_HOLE - for (int grp = 0; grp <= 3; grp++) + for (int grp = 5; grp <= 5; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -851,7 +854,7 @@ void HuaHang_holePosition_test(void) SSG_cornerParam cornerParam; cornerParam.cornerTh = 60; //45搴﹁ - cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4; cornerParam.minEndingGap_z = 5.0; cornerParam.jumpCornerTh_1 = 15; //姘村钩瑙掑害锛屽皬浜庢瑙掑害瑙嗕负姘村钩