jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wheelArchHeigthMeasure version 1.3.5：

Browse Source 
改进轮眉取点方法
This commit is contained in:
jerryzeng 2026-04-15 14:10:45 +08:00
parent 680ff878fb
commit 4e8f6943d8
2 changed files with 69 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
sourceCode/wheelArchHeigthMeasure.cpp
View File

@ -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<SVzNL2DPoint>& e
}
}
SVzNL2DPoint _getRealArcPos(SVzNL2DPoint seedPos, double a, double b, double c, std::vector<SVzNL3DPosition>& 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<SVzNL3DPosition>>& 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;

13
wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp
View File

@ -973,7 +973,7 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector<std::vector< SVzNL
}
//if (objOps.size() > 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<std::vector< SVzNL
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
x = (float)(wheelArcHeight.centerLine[0].x + wheelArcHeight.centerLine[1].x) / 2.0f;
y = (float)(wheelArcHeight.centerLine[0].y + wheelArcHeight.centerLine[1].y) / 2.0f;
z = (float)(wheelArcHeight.centerLine[0].z + wheelArcHeight.centerLine[1].z) / 2.0f;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
x = (float)wheelArcHeight.wheelArchPos.x;
y = (float)wheelArcHeight.wheelArchPos.y;
z = (float)wheelArcHeight.wheelArchPos.z;
@ -2725,7 +2732,7 @@ int main()
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,2}, {1,4}, {8,9}, {8,9}, {8,9},{9,9}
{1,2}, {1,4}, {1,9}, {5,9}, {1,10},{1,10}
};
SSG_planeCalibPara poseCalibPara;
@ -2749,7 +2756,7 @@ int main()
char _scan_file[256];
int endGroup = TEST_GROUP - 1;
for (int grp = 5; grp <= endGroup; grp++)
for (int grp = 2; grp <= endGroup; grp++)
{
char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);