From bbad1650f2e2d47208198bbf45b160946e8055be Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 18 May 2026 17:23:08 +0800 Subject: [PATCH] =?UTF-8?q?wheelArchHeigthMeasure=20version=201.4.0=20:=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=A0=87=E5=AE=9A=E5=9D=97=E5=87=B9?= =?UTF-8?q?=E6=A7=BD=E9=95=BF=E5=BA=A6=E8=AE=A1=E7=AE=97API=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_featureGrow.cpp | 2 +- sourceCode/wheelArchHeigthMeasure.cpp | 239 +++++++++++++++++- sourceCode/wheelArchHeigthMeasure_Export.h | 13 + .../wheelArchHeigthMeasure_test.cpp | 212 ++++++++++++++-- 4 files changed, 449 insertions(+), 17 deletions(-) diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index ab02c36..f7e7410 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -843,7 +843,7 @@ void wd_getRodArcFeatureGrowingTrees( { for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++) { - if (i == 1147) + if (i == 1304) int kkk = 1; std::vector& a_lineFeatures = all_lineFeatures[i]; for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++) diff --git a/sourceCode/wheelArchHeigthMeasure.cpp b/sourceCode/wheelArchHeigthMeasure.cpp index 3697448..d788fd4 100644 --- a/sourceCode/wheelArchHeigthMeasure.cpp +++ b/sourceCode/wheelArchHeigthMeasure.cpp @@ -15,7 +15,8 @@ //version 1.3.4 : 轮眉到轮心高度计算方法进行了修正，由Y高度差修改为两点距离 //version 1.3.5 : 改进轮眉取点方法。 //version 1.3.6 : 轮眉到轮心高度计算方法进行了修正，重新从由两点距离修改为Y高度差。 -std::string m_strVersion = "1.3.6"; +//version 1.4.0 : 添加了标定块凹槽长度计算API。 +std::string m_strVersion = "1.4.0"; const char* wd_wheelArchHeigthMeasureVersion(void) { return m_strVersion.c_str(); @@ -592,6 +593,12 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); } + if(false == isGridData) + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return result; + } + //生成水平扫描 std::vector> hLines_raw; hLines_raw.resize(linePtNum); @@ -1044,4 +1051,234 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( result.upLine[i] = _ptRotate(result.upLine[i], groundCalibPara.invRMatrix); } return result; +} + +SVzNLRangeD _getClusterZRange(std::vector< SVzNL3DPosition>& a_cluster) +{ + SVzNLRangeD zRange = { -1, -1 }; + int nodeSize = a_cluster.size(); + if (nodeSize == 0) + return zRange; + + zRange = { a_cluster[0].pt3D.z, a_cluster[0].pt3D.z }; + for (int i = 1; i < nodeSize; i++) + { + zRange.min = zRange.min > a_cluster[i].pt3D.z ? a_cluster[i].pt3D.z : zRange.min; + zRange.max = zRange.max < a_cluster[i].pt3D.z ? a_cluster[i].pt3D.z : zRange.max; + } + return zRange; +} + +double wd_getCalibrationBlockLength( + std::vector< std::vector>& scanLines, + const double nominalTotalLen, //标定块总长度(601)，用于过滤虚假信号 + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SVzNLRangeD calibratiionBlockYRange, //标定块Y范围 +#if _OUTPUT_DEBUG_DATA + std::vector< std::vector>& debugData, +#endif + int* errCode +) +{ + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + bool isGridData = true; + for (int line = 0; line < lineNum; line++) + { + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + //滤波，滤除异常点 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + } + if (false == isGridData) + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return -1; + } + + //生成标定块ROI数据 +#if _OUTPUT_DEBUG_DATA + debugData.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + debugData[i].resize(linePtNum); + } +#endif + + std::vector calibrationData; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + SVzNL3DPosition a_pt = scanLines[line][j]; + a_pt.nPointIdx = (line << 16) | (j & 0xffff); + if ((a_pt.pt3D.y < calibratiionBlockYRange.min) || (a_pt.pt3D.y > calibratiionBlockYRange.max)) + a_pt.pt3D = { 0, 0, 0 }; + calibrationData.push_back(a_pt); +#if _OUTPUT_DEBUG_DATA + debugData[line][j] = a_pt; + debugData[line][j].nPointIdx = 0; +#endif + } + } + + //聚类 + int clusterCheckWin = 5; + int clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2)); + std::vector> objClusters; + wd_pointClustering_speedUp( + calibrationData, + lineNum, linePtNum, clusterCheckWin, //搜索窗口 + clusterDist, + 1, //0 - 2d distance; 1- 3d distance + objClusters //result + ); + + //挑选出目标 + const int minClusterPtSize = 2500; + std::vector clusterMask; + std::vector clusterZRange; + clusterMask.resize(objClusters.size()); + for (int i = 0; i < (int)objClusters.size(); i++) + { + SVzNLRangeD zRange = _getClusterZRange(objClusters[i]); + clusterZRange.push_back(zRange); + + if (objClusters[i].size() < minClusterPtSize) + clusterMask[i] = 0; + else + clusterMask[i] = 1; + } + + //将z最小的目标选为标定块 + int objIdx = -1; + for (int i = 0; i < (int)objClusters.size(); i++) + { + if ( (clusterMask[i] > 0) && (clusterZRange[i].max > 1e-4)) + { + if (objIdx < 0) + objIdx = i; + else + { + if( (clusterZRange[objIdx].min + clusterZRange[objIdx].max) > (clusterZRange[i].min + clusterZRange[i].max)) + objIdx = i; + } + } + } + if (objIdx < 0) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return -1; + } +#if _OUTPUT_DEBUG_DATA + for (int i = 0; i < objClusters[objIdx].size(); i++) + { + int nPtIdx = objClusters[objIdx][i].nPointIdx; + int lineIdx = nPtIdx >> 16; + int ptIdx = nPtIdx & 0xffff; + debugData[lineIdx][ptIdx].nPointIdx = 1; + } +#endif + + std::vector> objMask; + objMask.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + objMask[i].resize(linePtNum); + std::fill(objMask[i].begin(), objMask[i].end(), -1); + } + for (int i = 0; i < objClusters[objIdx].size(); i++) + { + int nPtIdx = objClusters[objIdx][i].nPointIdx; + int lineIdx = nPtIdx >> 16; + int ptIdx = nPtIdx & 0xffff; + objMask[lineIdx][ptIdx] = i; + } + + //计算长度 + std::vector> validLines; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition> lineData; + for (int j = 0; j < linePtNum; j++) + { + if (objMask[line][j] >= 0) + { + int clusterPtIdx = objMask[line][j]; + if(objClusters[objIdx][clusterPtIdx].pt3D.z > 1e-4) + lineData.push_back(objClusters[objIdx][clusterPtIdx]); + } + } + if(lineData.size() > 10) + validLines.push_back(lineData); + } + + double sumGaugeLen = 0; + int sumSize = 0; + int validLineNum = (int)validLines.size(); + for (int i = 0; i < validLineNum; i++) + { + int validLinePtNum = (int)validLines[i].size(); + SVzNL3DPosition first_pt = validLines[i][0]; + SVzNL3DPosition last_pt = validLines[i].back(); + double totalLen = sqrt(pow(first_pt.pt3D.x - last_pt.pt3D.x, 2) + pow(first_pt.pt3D.y - last_pt.pt3D.y, 2) + pow(first_pt.pt3D.z - last_pt.pt3D.z, 2)); + double lenDiff = abs(nominalTotalLen - totalLen); + if ( (lenDiff < 10) &&(validLinePtNum>100)) + { + //计算凹槽长度 + double _a = 0, _b = 0, _c = 0; + compute2ptLine_2(first_pt.pt3D.y, first_pt.pt3D.z, last_pt.pt3D.y, last_pt.pt3D.z, &_a, &_b, &_c); + + SVzNL3DPosition peakPt_1 = { 0, 0, 0 }; + double maxLen_1 = -1; + for (int j = 0; j < validLinePtNum / 2; j++) + { + double len = computePtDistToLine(validLines[i][j].pt3D.y, validLines[i][j].pt3D.z, _a, _b, _c); + if (maxLen_1 < len) + { + peakPt_1 = validLines[i][j]; + maxLen_1 = len; + } + } + + SVzNL3DPosition peakPt_2 = { 0, 0, 0 }; + double maxLen_2 = -1; + for (int j = validLinePtNum / 2; j < validLinePtNum; j++) + { + double len = computePtDistToLine(validLines[i][j].pt3D.y, validLines[i][j].pt3D.z, _a, _b, _c); + if (maxLen_2 < len) + { + peakPt_2 = validLines[i][j]; + maxLen_2 = len; + } + } + + if ((maxLen_1 > 2.0) && (maxLen_2 > 2.0)) + { + double vLen = sqrt(pow(peakPt_1.pt3D.x - peakPt_2.pt3D.x, 2) + pow(peakPt_1.pt3D.y - peakPt_2.pt3D.y, 2) + pow(peakPt_1.pt3D.z - peakPt_2.pt3D.z, 2)); + sumGaugeLen += vLen; + sumSize++; +#if _OUTPUT_DEBUG_DATA + int nPtIdx_1 = peakPt_1.nPointIdx; + int lineIdx_1 = nPtIdx_1 >> 16; + int ptIdx_1 = nPtIdx_1 & 0xffff; + debugData[lineIdx_1][ptIdx_1].nPointIdx = 2; + int nPtIdx_2 = peakPt_2.nPointIdx; + int lineIdx_2 = nPtIdx_2 >> 16; + int ptIdx_2 = nPtIdx_2 & 0xffff; + debugData[lineIdx_2][ptIdx_2].nPointIdx = 2; +#endif + } + } + } + if (sumSize == 0) +{ + *errCode = SX_ERR_ZERO_OBJECTS; + return -1; + } + + sumGaugeLen = sumGaugeLen / sumSize; + return sumGaugeLen; } \ No newline at end of file diff --git a/sourceCode/wheelArchHeigthMeasure_Export.h b/sourceCode/wheelArchHeigthMeasure_Export.h index 0f952de..9fb879c 100644 --- a/sourceCode/wheelArchHeigthMeasure_Export.h +++ b/sourceCode/wheelArchHeigthMeasure_Export.h @@ -47,3 +47,16 @@ SG_APISHARED_EXPORT WD_wheelArchInfo wd_wheelArchHeigthMeasure( const SSG_treeGrowParam growParam, const SSG_planeCalibPara groundCalibPara, int* errCode); + +//标定块凹槽长度计算 +SG_APISHARED_EXPORT double wd_getCalibrationBlockLength( + std::vector< std::vector>& scanLines, + const double nominalTotalLen, //标定块总长度(601)，用于过滤虚假信号 + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SVzNLRangeD calibratiionBlockYRange, +#if _OUTPUT_DEBUG_DATA + std::vector< std::vector>& debugData, +#endif + int* errCode +); \ No newline at end of file diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 6fc617a..910451c 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -1065,6 +1065,93 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector>& scanData, + float lineV, int maxTimeStamp, int clockPerSecond) +{ + int lineNum = (int)scanData.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//娣￠粍鑹 + {210,105, 30},//宸у厠鍔涜壊 + {240,230,140},//榛勮鑹 + {135,206,235},//澶╄摑鑹 + {250,235,215},//鍙よ懀鐧 + {189,252,201},//钖勮嵎鑹 + {221,160,221},//姊呯孩鑹 + {188,143,143},//鐜懓绾㈣壊 + }; + int size = 1; + int nTimeStamp = 0; + double alpha = 0.8; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition& pt3D = scanData[line][i]; + int type = pt3D.nPointIdx; + if (1 == type) + { + rgb = { 0,255,0 }; + rgb.r = (int)((double)rgb.r * alpha); + rgb.g = (int)((double)rgb.g * alpha); + rgb.b = (int)((double)rgb.b * alpha); + size = 2; + } + else if (2 == type) + { + rgb = { 255,0,0 }; + rgb.r = (int)((double)rgb.r * alpha); + rgb.g = (int)((double)rgb.g * alpha); + rgb.b = (int)((double)rgb.b * alpha); + size = 4; + } + else if (3 == type) //杞湁 + { + rgb = { 0, 0, 250 }; + size = 3; + } + else if (4 == type) // + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (5 == type) // + { + rgb = { 255, 255, 0 }; + size = 3; + } + else + { + rgb = { 100, 100, 100 }; + size = 1; + } + + float x = (float)pt3D.pt3D.x; + float y = (float)pt3D.pt3D.y; + float z = (float)pt3D.pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + sw.close(); +} + void _outputScanDataFile_obj_vector(char* fileName, std::vector>scanData, std::vector&objOps) { @@ -2643,10 +2730,11 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData } #define TEST_CONVERT_TO_GRID 0 -#define TEST_COMPUTE_WHEEL_ARCH 1 +#define TEST_COMPUTE_WHEEL_ARCH 0 +#define TEST_CAMERA_ADJUSTING 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_GROUP 6 +#define TEST_GROUP 7 int main() { #if TEST_CONVERT_TO_GRID @@ -2690,7 +2778,8 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/ground_LaserData.txt"); + + sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲2璋冨钩椤圭洰_鏉块珮3mm/鍙冲悗绂诲湴4mm.txt"); std::vector< std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); @@ -2711,10 +2800,10 @@ int main() #endif // char calibFile[250]; - sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/ground_calib_para.txt"); + sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲2璋冨钩椤圭洰_鏉块珮3mm/鍙冲悗绂诲湴4mm_ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/LaserLine_grund_calib.txt"); + sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲2璋冨钩椤圭洰_鏉块珮3mm/鍙冲悗绂诲湴4mm_ground_calib.txt"); _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0); printf("%s: calib done!\n", _calib_datafile); } @@ -2725,14 +2814,16 @@ int main() const char* dataPath[TEST_GROUP] = { "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/", //0 "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁/", //1 - "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/11/", //2 - "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/13/", //3 - "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/15/", //4 - "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/17/", //5 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/11_C573/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/13_C578/", //3 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/15_C552/", //4 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満鏁版嵁2/17_C586/", //5 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/閮ㄧ讲鐜板満2鏁版嵁/", //6 + }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,2}, {1,4}, {11,15}, {11,15}, {11,15},{16,16} + {1,2}, {1,4}, {27,41}, {27,41}, {27,41},{27,41}, {7,7} }; SSG_planeCalibPara poseCalibPara; @@ -2756,15 +2847,18 @@ int main() char _scan_file[256]; int endGroup = TEST_GROUP - 1; - for (int grp = 5; grp <= endGroup; grp++) + for (int grp = 6; 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 = 6; - sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + //fidx = 2; + if(6 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_Jl26C575.txt", dataPath[grp], fidx); + else + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); std::vector> scanData; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); if (scanData.size() == 0) @@ -2775,7 +2869,8 @@ int main() long t1 = GetTickCount64(); - SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-2000, 2000}, {500, 2500} }; + //SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-2000, 2000}, {500, 2500} }; + SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-1000, 1000}, {-1000, 1000} }; SSG_cornerParam cornerParam; cornerParam.cornerTh = 45; //45搴﹁ cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; @@ -2841,7 +2936,11 @@ int main() 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: arcY=%f, height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.wheelArchPos.y, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); + printf("%s: arcY=%f, upPtY=%f, height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.wheelArchPos.y, wheelArcHeight.wheelUpPos.y, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); + double len = sqrt(pow(wheelArcHeight.wheelUpPos.x - wheelArcHeight.wheelDownPos.x, 2) + + pow(wheelArcHeight.wheelUpPos.y - wheelArcHeight.wheelDownPos.y, 2) + + pow(wheelArcHeight.wheelUpPos.z - wheelArcHeight.wheelDownPos.z, 2)); + printf(" 杞瘋灏哄锛%g\n", len); } else { @@ -2851,6 +2950,89 @@ int main() } } #endif + +#if TEST_CAMERA_ADJUSTING + const char* dataPath[4] = { + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/宸ュ叿鏍囧畾/11鏍囧畾/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/宸ュ叿鏍囧畾/13鏍囧畾/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/宸ュ叿鏍囧畾/15鏍囧畾/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/宸ュ叿鏍囧畾/17鏍囧畾/", //3 + }; + + SVzNLRange fileIdx[4] = { + {1,20}, {1,20}, {1,20}, {1,21} + }; + + const char* ver = wd_wheelArchHeigthMeasureVersion(); + printf("ver:%s\n", ver); + + char _scan_file[256]; + + for (int grp = 0; grp <= 3; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 6; + if(0 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_JL26C573.txt", dataPath[grp], fidx); + else if (1 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_JL26C578.txt", dataPath[grp], fidx); + else if (2 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_Jl26C552.txt", dataPath[grp], fidx); + else + sprintf_s(_scan_file, "%s%d_LaserData_JL26C586.txt", dataPath[grp], fidx); + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); + if (scanData.size() == 0) + continue; + + long t1 = GetTickCount64(); + + double nominalTotalLen = 601.0; //鏍囧畾鍧楁爣绉版婚暱601mm + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 20.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ + filterParam.outlierTh = 5; + + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 10; + growParam.yDeviation_max = 20.0; + growParam.maxSkipDistance = 10.0; + growParam.zDeviation_max = 20.0;// + growParam.minLTypeTreeLen = 100; //mm + growParam.minVTypeTreeLen = 100; //mm + SVzNLRangeD calibratiionBlockYRange = { -1000, 500 }; + + int errCode = 0; +#if _OUTPUT_DEBUG_DATA + std::vector< std::vector> debugData; +#endif + double len = wd_getCalibrationBlockLength( + scanData, + nominalTotalLen, + filterParam, + growParam, + calibratiionBlockYRange, +#if _OUTPUT_DEBUG_DATA + debugData, +#endif + & errCode + ); + + long t2 = GetTickCount64(); + + char _dbg_file[256]; +#if _OUTPUT_DEBUG_DATA + + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputScanDataFile_RGBD(_dbg_file, debugData, 0, 0, 0); +#endif + printf("%s: 鍑规Ы闂撮暱搴=%f, time=%d(ms), errCode=%d\n", _scan_file, len, (int)(t2 - t1), errCode); + } + } + +#endif + printf("all done!\n"); }