From 0111e2a56a3e4f8ce1fa79782abc208d2496f7a9 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 12 Jan 2026 17:27:50 +0800 Subject: [PATCH] =?UTF-8?q?rodAndBarDetection=20version=201.0.0=20?= =?UTF-8?q?=E5=88=9D=E5=A7=8B=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../rodAndBarDetection_test.cpp | 391 +++++++++++++++++- sourceCode/SG_baseAlgo_Export.h | 6 +- sourceCode/SG_baseDataType.h | 1 + sourceCode/SG_baseFunc.cpp | 145 ++++++- sourceCode/SG_lineFeature.cpp | 340 +++++---------- sourceCode/rodAndBarDetection.cpp | 157 ++++++- sourceCode/rodAndBarDetection_Export.h | 2 +- 7 files changed, 776 insertions(+), 266 deletions(-) diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 5678d2a..06523df 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -1,11 +1,398 @@ -// rodAndBarDetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 // #include +#include +#include +#include +#include +#include "direct.h" +#include +#include "rodAndBarDetection_Export.h" +#include +#include +#include +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + +void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void wd_gridScan_GetROIData(std::vector>& scanData, SVzNLRangeD roi_y, std::vector>& roiData) +{ + int lineNum = (int)scanData.size(); + int linePtNum = (int)scanData[0].size(); + int globalPtStart = INT_MAX; + int globalPtEnd = 0; + int lineStart = INT_MAX; + int lineEnd = 0; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition >& lineData = scanData[line]; + int ptSize = (int)lineData.size(); + int vldNum = 0; + int ptStart = INT_MAX; + int ptEnd = 0; + for (int i = 0; i < ptSize; i++) + { + if (lineData[i].pt3D.z > 1e-4) + { + if ((lineData[i].pt3D.y < roi_y.min) || (lineData[i].pt3D.y > roi_y.max)) + lineData[i].pt3D = { 0.0, 0.0, 0.0 }; + } + + if (lineData[i].pt3D.z > 1e-4) + { + if (ptStart > i) + ptStart = i; + ptEnd = i; + vldNum++; + } + } + if (vldNum > 0) + { + if (globalPtStart > ptStart) + globalPtStart = ptStart; + if (globalPtEnd < ptEnd) + globalPtEnd = ptEnd; + + if (lineStart > line) + lineStart = line; + lineEnd = line; + } + } + int vldLineNum = lineEnd - lineStart + 1; + int vldPtNum = globalPtEnd - globalPtStart + 1; + + roiData.resize(vldLineNum); + for (int line = 0; line < vldLineNum; line++) + { + roiData[line].resize(vldPtNum); + for (int i = 0; i < vldPtNum; i++) + roiData[line][i] = scanData[line + lineStart][i + globalPtStart]; + } +return; +} + +void _outputScanDataFile(char* fileName, std::vector>& scanData, + float lineV, int maxTimeStamp, int clockPerSecond) +{ + std::ofstream sw(fileName); + + int lineNum = (int)scanData.size(); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + 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]; + 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}" << std::endl; + } + } + sw.close(); +} + +void _outputChanneltInfo(char* fileName, std::vector& screwInfo) +{ + std::ofstream sw(fileName); + + char dataStr[250]; + int objNum = (int)screwInfo.size(); + for (int i = 0; i < objNum; i++) + { + sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_( %g, %g, %g ), anlge_%g", + i + 1, screwInfo[i].center.x, screwInfo[i].center.y, screwInfo[i].center.z, + screwInfo[i].axialDir.x, screwInfo[i].axialDir.y, screwInfo[i].axialDir.z, screwInfo[i].rotateAngle); + sw << dataStr << std::endl; + } + sw.close(); +} + +void _outputRGBDScan_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector& screwInfo +) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + int objNum = (int)screwInfo.size(); + if (objNum > 0) + realLines += 1; + + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << 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 lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->nPointIdx == 1) + { + rgb = objColor[pt3D->nPointIdx]; + size = 3; + } + else if (pt3D->nPointIdx == 2) + { + rgb = { 250, 0, 0 }; + size = 5; + } + + else //if (pt3D->nPointIdx == 0) + { + rgb = { 200, 200, 200 }; + 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; + } + } + + if (objNum > 0) + { + sw << "Line_" << lineIdx << "_0_" << objNum << std::endl; + rgb = { 250, 0, 0 }; + size = 8; + for (int i = 0; i < objNum; i++) + { + float x = (float)screwInfo[i].center.x; + float y = (float)screwInfo[i].center.y; + float z = (float)screwInfo[i].center.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //多输出一个，修正显示工具bug + float x = (float)screwInfo[0].center.x; + float y = (float)screwInfo[0].center.y; + float z = (float)screwInfo[0].center.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + //输出法向 + size = 1; + double len = 60; + lineIdx = 0; + for (int i = 0; i < objNum; i++) + { + SVzNL3DPoint pt0 = { screwInfo[i].center.x - len * screwInfo[i].axialDir.x, + screwInfo[i].center.y - len * screwInfo[i].axialDir.y, + screwInfo[i].center.z - len * screwInfo[i].axialDir.z }; + SVzNL3DPoint pt1 = { screwInfo[i].center.x + len * screwInfo[i].axialDir.x, + screwInfo[i].center.y + len * screwInfo[i].axialDir.y, + screwInfo[i].center.z + len * screwInfo[i].axialDir.z }; + //显示法向量 + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + //多输出一个，修正显示工具bug + SVzNL3DPoint pt0 = { screwInfo[0].center.x - len * screwInfo[0].axialDir.x, + screwInfo[0].center.y - len * screwInfo[0].axialDir.y, + screwInfo[0].center.z - len * screwInfo[0].axialDir.z }; + SVzNL3DPoint pt1 = { screwInfo[0].center.x + len * screwInfo[0].axialDir.x, + screwInfo[0].center.y + len * screwInfo[0].axialDir.y, + screwInfo[0].center.z + len * screwInfo[0].axialDir.z }; + //显示法向量 + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + sw.close(); +} + +#define TEST_GROUP 1 int main() { - std::cout << "Hello World!\n"; + const char* dataPath[TEST_GROUP] = { + "F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,4}, + }; + + const char* ver = wd_rodAndBarDetectionVersion(); + printf("ver:%s\n", ver); + + for (int grp = 0; grp < TEST_GROUP; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =7; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + double rodDiameter = 10.0; + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 60; //45度角 + cornerParam.scale = rodDiameter/4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + + 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 = 20.0; + growParam.zDeviation_max = 50.0;// + growParam.minLTypeTreeLen = 10; //mm, 螺杆长度 + growParam.minVTypeTreeLen = 10; //mm + + bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 + int errCode = 0; + std::vector screwInfo; + sx_hexHeadScrewMeasure( + scanLines, + isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 + cornerParam, + filterParam, + growParam, + rodDiameter, + screwInfo, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); + _outputRGBDScan_RGBD(_scan_file, scanLines, screwInfo); + sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); + _outputChanneltInfo(_scan_file, screwInfo); + } + } + } // 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index eb7de69..3ffd37b 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -146,7 +146,7 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature( std::vector< SVzNL3DPosition>& lineData, int lineIdx, const SSG_cornerParam cornerPara, - double ringArcWidth, //ӵĻ + SVzNLRangeD ringArcWidth, //ӵĻ std::vector& line_ringArcs // ); @@ -393,6 +393,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector( std::vector< std::vector>& scanLines ); +//ROI: vecotrʽ +SG_APISHARED_EXPORT SVzNL3DRangeD wd_getPointCloudROI( + std::vector& scanData); + //ƵROIscale: vecotrʽ SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara( std::vector< std::vector>& scanLines); diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index a54cdf2..dd3a2d6 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -202,6 +202,7 @@ typedef struct int endPtIdx; SVzNL3DPoint startPt; SVzNL3DPoint endPt; + double featureValue; }SWD_segFeature; typedef struct diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 1b61b45..312af44 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -131,6 +131,64 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector return roi; } +//ROI: vecotrʽ +SVzNL3DRangeD wd_getPointCloudROI(std::vector& scanData) +{ + SVzNL3DRangeD roi; + roi.xRange = { 0, -1 }; + roi.yRange = { 0, -1 }; + roi.zRange = { 0, -1 }; + + int nPositionCnt = (int)scanData.size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPoint& pt3D = scanData[i]; + if (pt3D.z < 1e-4) + continue; + + if (roi.xRange.max < roi.xRange.min) + { + roi.xRange.min = pt3D.x; + roi.xRange.max = pt3D.x; + } + else + { + if (roi.xRange.min > pt3D.x) + roi.xRange.min = pt3D.x; + if (roi.xRange.max < pt3D.x) + roi.xRange.max = pt3D.x; + } + //y + if (roi.yRange.max < roi.yRange.min) + { + roi.yRange.min = pt3D.y; + roi.yRange.max = pt3D.y; + } + else + { + if (roi.yRange.min > pt3D.y) + roi.yRange.min = pt3D.y; + if (roi.yRange.max < pt3D.y) + roi.yRange.max = pt3D.y; + } + //z + if (roi.zRange.max < roi.zRange.min) + { + roi.zRange.min = pt3D.z; + roi.zRange.max = pt3D.z; + } + else + { + if (roi.zRange.min > pt3D.z) + roi.zRange.min = pt3D.z; + if (roi.zRange.max < pt3D.z) + roi.zRange.max = pt3D.z; + } + } + + return roi; +} + //ƵROIscale: vecotrʽ SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector>& scanLines) { @@ -445,6 +503,7 @@ bool leastSquareParabolaFit(const std::vector& points, return true; } #endif +//С y=ax^2 + bx + c bool leastSquareParabolaFitEigen( const std::vector& points, double& a, double& b, double& c, @@ -3112,7 +3171,7 @@ bool fitLine3DLeastSquares(const std::vector& points, SVzNL3DPoint Eigen::MatrixXd centered = A.rowwise() - centroid_row; // άƥ䣬ޱ // Э㣨n-1ΪƫƣҲֱn - Eigen::Matrix3d cov = centered.transpose() * centered / (points.size() - 1); + Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1); // 3. ֵֽ⣺Эֵ Eigen::SelfAdjointEigenSolver eigensolver(cov); if (eigensolver.info() != Eigen::Success) { @@ -3128,3 +3187,87 @@ bool fitLine3DLeastSquares(const std::vector& points, SVzNL3DPoint direction = { dir(0), dir(1), dir(2) }; return true; } + +#if 0 +#include +#include +#include +#include + +// Define a struct for 3D points +struct Point3D { + double x, y, z; +}; + +// Function to perform 3D line fitting using SVD +void fitLine3D(const std::vector& points, Eigen::Vector3d& centroid, Eigen::Vector3d& direction) { + int n = points.size(); + if (n < 2) { + std::cerr << "Need at least 2 points to fit a line." << std::endl; + return; + } + + // 1. Calculate Centroid + centroid.setZero(); + for (const auto& p : points) { + centroid += Eigen::Vector3d(p.x, p.y, p.z); + } + centroid /= n; + + // 2. Center the data and build the data matrix + Eigen::MatrixXd data_matrix(n, 3); + for (int i = 0; i < n; ++i) { + data_matrix.row(i) << points[i].x - centroid(0), + points[i].y - centroid(1), + points[i].z - centroid(2); + } + + // 3. Apply SVD + // We compute the SVD of the centered data matrix + Eigen::JacobiSVD svd(data_matrix, Eigen::ComputeThinV); + + // 4. Extract the direction vector + // The right singular vector corresponding to the largest singular value (first column of V) + // gives the direction of the best-fit line. + direction = svd.matrixV().col(0); +} + +int main() { + // Sample data points + std::vector points = { + {1.0, 2.0, 3.0}, + {2.0, 3.0, 4.0}, + {3.0, 4.0, 5.0}, + {4.0, 5.0, 6.0}, + {5.0, 6.0, 7.0} + }; + + Eigen::Vector3d centroid; + Eigen::Vector3d direction; + + fitLine3D(points, centroid, direction); + + std::cout << "Centroid (point on the line): " << centroid.transpose() << std::endl; + std::cout << "Direction vector of the line: " << direction.transpose() << std::endl; + std::cout << "Equation of the line: P(t) = Centroid + t * Direction" << std::endl; + + return 0; +} + +template +std::pair < Vector3, Vector3 > best_line_from_points(const std::vector& c) +{ + // copy coordinates to matrix in Eigen format + size_t num_atoms = c.size(); + Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3); + for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i]; + + Vector3 origin = centers.colwise().mean(); + Eigen::MatrixXd centered = centers.rowwise() - origin.transpose(); + Eigen::MatrixXd cov = centered.adjoint() * centered; + Eigen::SelfAdjointEigenSolver eig(cov); + Vector3 axis = eig.eigenvectors().col(2).normalized(); + + return std::make_pair(origin, axis); +} +#endif \ No newline at end of file diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index f61d4f6..b67b056 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3443,7 +3443,7 @@ void wd_getRingArcFeature( std::vector< SVzNL3DPosition>& lineData, int lineIdx, const SSG_cornerParam cornerPara, - double ringArcWidth, // + SVzNLRangeD ringArcWidth, // std::vector& line_ringArcs // ) { @@ -3588,262 +3588,112 @@ void wd_getRingArcFeature( } } - //սǼֵ - int _state = 0; - int pre_i = -1; - int sEdgePtIdx = -1; - int eEdgePtIdx = -1; - SSG_pntDirAngle* pre_data = NULL; - std::vector< SSG_pntDirAngle> cornerPeakP; - std::vector< SSG_pntDirAngle> cornerPeakM; - for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + //ѰĸcornerΡArcϵÿһ㶼Ǹcorner + int vldPtSize = (int)vldPts.size(); + int startIdx = -1; + int endIdx = -1; + double startAngle = 0; + double endAngle = 0; + for (int segIdx = 0; segIdx < segSize; segIdx++) { - if (i == 275) - int kkk = 1; - SSG_pntDirAngle* curr_data = &corners[i]; - if (curr_data->pntIdx < 0) + int vPtIdxStart = segs[segIdx].start; + int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1; + for (int i = vPtIdxStart; i <= vPtIdxEnd; i++) { - if (i == i_max - 1) //һ + if (corners[i].corner < 0) { - if (1 == _state) // + if (startIdx < 0) { - cornerPeakP.push_back(corners[eEdgePtIdx]); + startIdx = i; + endIdx = i; + startAngle = corners[i].backwardAngle; + endAngle = corners[i].forwardAngle; } - else if (2 == _state) //½ + else { - cornerPeakM.push_back(corners[eEdgePtIdx]); + endIdx = i; + endAngle = corners[i].forwardAngle; + if (i == dataSize - 1) + { + double totalCorner = abs(endAngle - startAngle); + double len = sqrt(pow(vldPts[startIdx].pt3D.y - vldPts[endIdx].pt3D.y, 2) + + pow(vldPts[startIdx].pt3D.z - vldPts[endIdx].pt3D.z, 2)); + if ((totalCorner > cornerPara.cornerTh) && (len >= ringArcWidth.min) && + (len <= ringArcWidth.max)) + { + int preIdx = startIdx - 1; + if (preIdx < vPtIdxStart) + preIdx = vPtIdxStart; + int postIdx = endIdx + 1; + if (postIdx > vPtIdxEnd) + postIdx = vPtIdxEnd; + //жsegDzArc + SWD_segFeature a_ringFeautre; + memset(&a_ringFeautre, 0, sizeof(SWD_segFeature)); + a_ringFeautre.lineIdx = lineIdx; + if ((corners[preIdx].pntIdx < 0) && (corners[postIdx].pntIdx < 0)) + { + a_ringFeautre.startPt = vldPts[vPtIdxStart].pt3D; + a_ringFeautre.endPt = vldPts[vPtIdxEnd].pt3D; + a_ringFeautre.startPtIdx = vldPts[vPtIdxStart].nPointIdx; + a_ringFeautre.endPtIdx = vldPts[vPtIdxEnd].nPointIdx; + } + else + { + a_ringFeautre.startPt = vldPts[startIdx].pt3D; + a_ringFeautre.endPt = vldPts[endIdx].pt3D; + a_ringFeautre.startPtIdx = vldPts[startIdx].nPointIdx; + a_ringFeautre.endPtIdx = vldPts[endIdx].nPointIdx; + } + a_ringFeautre.featureValue = totalCorner; + line_ringArcs.push_back(a_ringFeautre); + } + } } } - continue; - } - - if (NULL == pre_data) - { - sEdgePtIdx = i; - eEdgePtIdx = i; - pre_data = curr_data; - pre_i = i; - continue; - } - - eEdgePtIdx = i; - double cornerDiff = curr_data->corner - pre_data->corner; - switch (_state) - { - case 0: //̬ - if (cornerDiff < 0) //½ - { - _state = 2; - } - else if (cornerDiff > 0) // - { - _state = 1; - } - break; - case 1: // - if (cornerDiff < 0) //½ - { - if(pre_data->corner > 0) - cornerPeakP.push_back(*pre_data); - _state = 2; - } - break; - case 2: //½ - if (cornerDiff > 0) // - { - if(pre_data->corner < 0) - cornerPeakM.push_back(*pre_data); - _state = 1; - } - break; - default: - _state = 0; - break; - } - pre_data = curr_data; - pre_i = i; - } - - std::vector lineFeatures; - //Сֵ㣨嶥 - //ֵȽϣڳ߶ȴѰҾֲֵ - double square_distTh = 16 * cornerPara.scale * cornerPara.scale; //2cornerScale - for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) - { - if (cornerPeakP[i].corner < cornerPara.cornerTh) - continue; - - bool isPeak = true; - //ǰ - int cornerPtIdx = cornerPeakP[i].pntIdx; - for (int j = i - 1; j >= 0; j--) - { - int prePtIdx = cornerPeakP[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; - if (dist > square_distTh) //߶ȴ - break; - - if (cornerPeakP[i].corner < cornerPeakP[j].corner) - { - isPeak = false; - break; - } - } - // - if (true == isPeak) - { - cornerPtIdx = cornerPeakP[i].pntIdx; - for (int j = i + 1; j < i_max; j++) - { - int postPtIdx = cornerPeakP[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); - if (dist > square_distTh) //߶ȴ - break; - - if (cornerPeakP[i].corner < cornerPeakP[j].corner) - { - isPeak = false; - break; - } - } - } - if (true == isPeak) - { - - - 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; - - a_feature.featureValue = cornerPeakP[i].corner; - a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; - a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx }; - lineFeatures.push_back(a_feature); - } - } - - for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++) - { - if (cornerPeakM[i].corner > -cornerPara.cornerTh) - continue; - - bool isPeak = true; - //ǰ - int cornerPtIdx = cornerPeakM[i].pntIdx; - for (int j = i - 1; j >= 0; j--) - { - int prePtIdx = cornerPeakM[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; - if (dist > square_distTh) //߶ȴ - break; - - if (cornerPeakM[i].corner > cornerPeakM[j].corner) { - isPeak = false; - break; - } - } - // - if (true == isPeak) - { - cornerPtIdx = cornerPeakM[i].pntIdx; - for (int j = i + 1; j < i_max; j++) - { - int postPtIdx = cornerPeakM[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); - if (dist > square_distTh) //߶ȴ - break; - - if (cornerPeakM[i].corner > cornerPeakM[j].corner) + if (startIdx >= 0) { - isPeak = false; - break; + double totalCorner = abs(endAngle - startAngle); + double len = sqrt(pow(vldPts[startIdx].pt3D.y - vldPts[endIdx].pt3D.y, 2) + + pow(vldPts[startIdx].pt3D.z - vldPts[endIdx].pt3D.z, 2)); + if ((totalCorner > cornerPara.cornerTh) && (len >= ringArcWidth.min) && + (len <= ringArcWidth.max)) + { + int preIdx = startIdx - 1; + if (preIdx < vPtIdxStart) + preIdx = vPtIdxStart; + int postIdx = endIdx + 1; + if (postIdx > vPtIdxEnd) + postIdx = vPtIdxEnd; + //жsegDzArc + SWD_segFeature a_ringFeautre; + memset(&a_ringFeautre, 0, sizeof(SWD_segFeature)); + a_ringFeautre.lineIdx = lineIdx; + if ((corners[preIdx].pntIdx < 0) && (corners[postIdx].pntIdx < 0)) + { + a_ringFeautre.startPt = vldPts[vPtIdxStart].pt3D; + a_ringFeautre.endPt = vldPts[vPtIdxEnd].pt3D; + a_ringFeautre.startPtIdx = vldPts[vPtIdxStart].nPointIdx; + a_ringFeautre.endPtIdx = vldPts[vPtIdxEnd].nPointIdx; + } + else + { + a_ringFeautre.startPt = vldPts[startIdx].pt3D; + a_ringFeautre.endPt = vldPts[endIdx].pt3D; + a_ringFeautre.startPtIdx = vldPts[startIdx].nPointIdx; + a_ringFeautre.endPtIdx = vldPts[endIdx].nPointIdx; + } + a_ringFeautre.featureValue = totalCorner; + line_ringArcs.push_back(a_ringFeautre); + } + } - } - } - if (true == isPeak) - { - - - SSG_basicFeature1D a_feature; - if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; - else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; - else - a_feature.featureType = LINE_FEATURE_CORNER_V; - - a_feature.featureValue = cornerPeakM[i].corner; - a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; - a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx }; - lineFeatures.push_back(a_feature); - } - } - - //ӿʼͽ߽ - for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) - { - int idx_1 = segs[i].start; - int idx_2 = segs[i].start + segs[i].len - 1; - - SSG_basicFeature1D an_edge; - memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); - //ͷ - an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; - int ptIdx = vldPts[idx_1].nPointIdx; - an_edge.jumpPos2D = { 0, ptIdx }; - an_edge.jumpPos = lineData[ptIdx].pt3D; - lineFeatures.push_back(an_edge); - //β - an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; - ptIdx = vldPts[idx_2].nPointIdx; - an_edge.jumpPos2D = { 0, ptIdx }; - an_edge.jumpPos = lineData[ptIdx].pt3D; - lineFeatures.push_back(an_edge); - } - - //򣺰 - std::sort(lineFeatures.begin(), lineFeatures.end(), compareByPtIdx); - //ҷϿȵĶӻ - for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max-1; i++) - { - if (lineFeatures[i].featureType < 0) - continue; - - bool pairing = false; - if (lineFeatures[i].featureType == LINE_FEATURE_LINE_ENDING_0) - { - if ((lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) || - ((lineFeatures[i + 1].featureType == LINE_FEATURE_L_JUMP_L2H)&&(lineFeatures[i + 1].featureValue < 0)) ) - pairing = true; - } - else if ( (lineFeatures[i].featureType == LINE_FEATURE_L_JUMP_H2L) && (lineFeatures[i].featureValue <0)) - { - if ((lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) || - ((lineFeatures[i + 1].featureType == LINE_FEATURE_L_JUMP_L2H) && (lineFeatures[i + 1].featureValue < 0))) - pairing = true; - } - if (true == pairing) - { - double deltaY = abs(lineFeatures[i].jumpPos.y - lineFeatures[i + 1].jumpPos.y); - double deltaZ = abs(lineFeatures[i].jumpPos.z - lineFeatures[i + 1].jumpPos.z); - double th1 = ringArcWidth * 0.5; - double th2 = ringArcWidth * 1.25; - if ((deltaY >= th1) && (deltaY < th2) && (deltaZ < ringArcWidth * 0.5)) - { - SWD_segFeature a_ringFeautre; - a_ringFeautre.lineIdx = lineIdx; - a_ringFeautre.startPt = lineFeatures[i].jumpPos; - a_ringFeautre.endPt = lineFeatures[i + 1].jumpPos; - a_ringFeautre.startPtIdx = lineFeatures[i].jumpPos2D.y; - a_ringFeautre.endPtIdx = lineFeatures[i+1].jumpPos2D.y; - line_ringArcs.push_back(a_ringFeautre); - lineFeatures[i].featureType = -1; - lineFeatures[i + 1].featureType = -1; + startIdx = -1; + endIdx = -1; + startAngle = 0; + endAngle = 0; } } } diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 1eabc5f..1d466aa 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -14,7 +14,8 @@ const char* wd_rodAndBarDetectionVersion(void) SVzNL3DPoint getArcPeak( std::vector< std::vector>& scanLines, - SWD_segFeature & a_arcFeature) + SWD_segFeature & a_arcFeature, + SVzNL2DPoint& arcPos) { SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D; for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++) @@ -22,7 +23,64 @@ SVzNL3DPoint getArcPeak( if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //յ { if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z) + { arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D; + arcPos = { a_arcFeature.lineIdx , i }; + } + } + } + return arcPeak; +} + +SVzNL3DPoint getArcPeak_parabolaFitting( + std::vector< std::vector>& scanLines, + SWD_segFeature& a_arcFeature, + SVzNL2DPoint& arcPos) +{ + std::vector points; + for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++) + { + if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //յ + { + cv::Point2d a_pt2D; + if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) + { + a_pt2D.x = scanLines[a_arcFeature.lineIdx][i].pt3D.y; + a_pt2D.y = scanLines[a_arcFeature.lineIdx][i].pt3D.z; + points.push_back(a_pt2D); + } + } + } + double a, b, c, mse, max_err; + //С y = ax ^ 2 + bx + c + bool result = leastSquareParabolaFitEigen( + points, + a, b, c, + mse, max_err); + double yP = -b / (2 * a); + //ѰyPĵΪPeak + SVzNL3DPoint arcPeak; + double minDist = -1; + for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++) + { + if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //յ + { + double dist = abs(scanLines[a_arcFeature.lineIdx][i].pt3D.y - yP); + if (minDist < 0) + { + minDist = dist; + arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D; + arcPos = { a_arcFeature.lineIdx , i }; + } + else + { + if(minDist > dist) + { + minDist = dist; + arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D; + arcPos = { a_arcFeature.lineIdx , i }; + } + } } } return arcPeak; @@ -119,12 +177,11 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, - double rodRidius, + double rodDiameter, std::vector& screwInfo, int* errCode) { *errCode = 0; - memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo)); int lineNum = (int)scanLines.size(); if (lineNum == 0) { @@ -193,7 +250,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) std::vector> arcFeatures; for (int line = 0; line < lineNum; line++) { - if (line == 44) + if (line == 329) int kkk = 1; std::vector& lineData = data_lines[line]; @@ -201,12 +258,15 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); std::vector line_ringArcs; int dataSize = (int)lineData.size(); + SVzNLRangeD arcWidth; + arcWidth.min = rodDiameter / 2; + arcWidth.max = rodDiameter * 1.5; //ȡArc wd_getRingArcFeature( lineData, line, //ǰɨ cornerPara, - rodRidius / 2, // + arcWidth, //ȣ԰뾶Ϊ׼Ӧ60Ƚ line_ringArcs // ); arcFeatures.push_back(line_ringArcs); @@ -223,23 +283,75 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) *errCode = SG_ERR_NOT_GRID_FORMAT; return; } - int objNum = (int)growTrees.size(); + + //ñ־debug + for (int i = 0; i < objNum; i++) + { + int nodeNum = (int)growTrees[i].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = growTrees[i].treeNodes[j].lineIdx; + for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++) + { + ptIdx = m; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + } + } + else + { + ptIdx = growTrees[i].treeNodes[j].lineIdx; + for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++) + { + lineIdx = m; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + } + } + } + } + + //Ŀ괦 for (int i = 0; i < objNum; i++) { //ռֱ std::vector fitPoints; + std::vector fit2DPos; int nodeSize = (int)growTrees[i].treeNodes.size(); for (int j = 0; j < nodeSize; j++) { - SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]); + SVzNL2DPoint arcPos; + SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos); + //SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos); fitPoints.push_back(a_pt); + fit2DPos.push_back(arcPos); } - if (fitPoints.size() < 12) + if (fitPoints.size() < 27) continue; //ȥͷβ5㣬ֹڶ˲͸ɨʱи - fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5); + fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10); + fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10); fitPoints.erase(fitPoints.end() - 5, fitPoints.end()); + fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end()); + //ñ־ + for (int j = 0; j < (int)fit2DPos.size(); j++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = fit2DPos[j].x; + ptIdx = fit2DPos[j].y; + } + else + { + lineIdx = fit2DPos[j].y; + ptIdx = fit2DPos[j].x; + } + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + // SVzNL3DPoint P0_center, P1_dir; bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir); @@ -251,22 +363,35 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) SVzNL3DPoint vector2 = { 0, 0, -1.0 }; SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2); // + SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib); SSG_ROIRectD roi_xoy; - roi_xoy.left = P0_center.x - rodRidius * 4; //2DΧ - roi_xoy.right = P0_center.x + rodRidius * 4; //2DΧ - roi_xoy.top = P0_center.y - rodRidius * 4; //2DΧ - roi_xoy.bottom = P0_center.y + rodRidius * 4; //2DΧ + roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2DΧ + roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2DΧ + roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2DΧ + roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2DΧ + +#if 1 + std::vector< SVzNL3DPoint> verifyData; + for (int m = 0; m < (int)fitPoints.size(); m++) + { + SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib); + verifyData.push_back(rPt); + } +#endif std::vector< SVzNL3DPoint> roiProjectionData; - xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData); + xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData); //ȡ SVzNLRangeD zRange = getZRange(roiProjectionData); SVzNLRangeD cutZRange; cutZRange.min = zRange.min; - cutZRange.max = zRange.min + 10.0; //ȡ10mmĶ + cutZRange.max = zRange.min + 5.0; //5mmĶ std::vector surfacePoints; zCutPointClouds(roiProjectionData, cutZRange, surfacePoints); //ĵ - SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints); + SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints); + SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints); + projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2; + projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2; projectionCenter.z = zRange.min; //תԭϵ SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix); diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index dc833f0..8b1a0fa 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -22,6 +22,6 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure( const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, - double rodRidius, + double rodDiameter, std::vector& screwInfo, int* errCode);