diff --git a/sourceCode/wheelArchHeigthMeasure.cpp b/sourceCode/wheelArchHeigthMeasure.cpp index 296c0fc..b651dee 100644 --- a/sourceCode/wheelArchHeigthMeasure.cpp +++ b/sourceCode/wheelArchHeigthMeasure.cpp @@ -13,7 +13,8 @@ //version 1.3.2 : 针对不理想点云改进了算法,增强鲁棒性 //version 1.3.3 : 轮眉点的提取进行了改进,修正了可能的取点错误 //version 1.3.4 : 轮眉到轮心高度计算方法进行了修正,由Y高度差修改为两点距离 -std::string m_strVersion = "1.3.4"; +//version 1.3.5 : 改进轮眉取点方法。 +std::string m_strVersion = "1.3.5"; const char* wd_wheelArchHeigthMeasureVersion(void) { return m_strVersion.c_str(); @@ -519,6 +520,40 @@ void _genEdgeContinuousPos(SSG_endingTree&edgeTree, std::vector& e } } +SVzNL2DPoint _getRealArcPos(SVzNL2DPoint seedPos, double a, double b, double c, std::vector& lineData, double chkRng_Y) +{ + SVzNL3DPosition seedPt = lineData[seedPos.y]; + + double minDist = -1; + int idx = -1; + for (int i = seedPos.y; i >= 0; i--) + { + if ( lineData[i].pt3D.z < 1e-4) + continue; + if (lineData[i].nPointIdx != 2) + break; + + double yLen = abs(lineData[i].pt3D.y - seedPt.pt3D.y); + if (yLen > chkRng_Y) + break; + + double dist = abs(lineData[i].pt3D.y * a + lineData[i].pt3D.z * b + c); + if (minDist < 0) + { + minDist = dist; + idx = i; + } + else if(minDist > dist) + { + minDist = dist; + idx = i; + } + } + if (idx >= 0) + seedPos.y = idx; + return seedPos; +} + //轮眉高度测量 WD_wheelArchInfo wd_wheelArchHeigthMeasure( std::vector< std::vector>& scanLines, @@ -937,6 +972,29 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( *errCode = SX_ERR_NO_WHEEL_ARC; return result; } + + //检查轮眉点:使用45方向的线作切线,取切点作为轮眉点 + double chkRng_Y = 50.0; +#if 0 + double vLine_a = downWheelPt.z - upWheelPt.z; + double vLine_b = upWheelPt.y - downWheelPt.y; + double vLine_c = downWheelPt.y * upWheelPt.z - upWheelPt.y * downWheelPt.z; + //旋转45度 + // 旋转后直线的系数(基于数学推导) + double r_a = vLine_a + vLine_b; + double r_b = vLine_b - vLine_a; + double r_c = -r_a * upWheelPt.y - r_b * upWheelPt.z; +#else + //旋转30度 + double vAngle = atan2(downWheelPt.z - upWheelPt.z, downWheelPt.y - upWheelPt.y); + vAngle += (60.0 / 180.0) * PI; + double r_a = tan(vAngle); + double r_b = -1.0; + double r_c = upWheelPt.z - r_a * upWheelPt.y; +#endif + arcPos = _getRealArcPos(arcPos, r_a, r_b, r_c, scanLines[arcPos.x], chkRng_Y); + + //在XY平面生成拟合点 double outLineLen = 200; SVzNL3DPoint arcPt = scanLines[arcPos.x][arcPos.y].pt3D; diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 90b627e..acf38fc 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -973,7 +973,7 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector 0) { - int ptNum = 3; + int ptNum = 4; sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; rgb = { 255, 0, 0 }; @@ -1000,6 +1000,13 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector