From 09de9f850dc9f37a14abde33417feb3402d7882a Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 8 Feb 2026 22:32:36 +0800 Subject: [PATCH] =?UTF-8?q?bagThreadPositioning=20version=201.1.0=20:=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=A0=87=E5=AE=9AMark=E6=A3=80?= =?UTF-8?q?=E6=B5=8B=EF=BC=8C=E6=B7=BB=E5=8A=A0=E4=BA=86=E7=9B=B8=E5=AF=B9?= =?UTF-8?q?=E4=BA=8E=E6=A0=87=E5=AE=9A=E6=9F=B1=E7=9A=84=E5=9D=90=E6=A0=87?= =?UTF-8?q?=E8=BE=93=E5=87=BA=EF=BC=8C=E6=B7=BB=E5=8A=A0=E4=BA=86=E7=9B=B8?= =?UTF-8?q?=E6=9C=BA=E8=B0=83=E5=B9=B3=E5=8A=9F=E8=83=BD=20=E5=90=8C?= =?UTF-8?q?=E6=97=B6=E5=8C=85=E5=90=AB=E7=94=B5=E6=9C=BA=E5=AE=9A=E5=AD=90?= =?UTF-8?q?=E6=8A=93=E5=8F=96=E7=9A=84=E7=AE=97=E6=B3=95=E5=BC=80=E5=8F=91?= =?UTF-8?q?=E7=9A=84=E8=BF=87=E7=A8=8B=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../bagThreadPositioning_test.cpp | 133 ++- baseAlgorithm/baseAlgorithm.vcxproj | 1 + sourceCode/SG_baseAlgo_Export.h | 33 +- sourceCode/SG_baseDataType.h | 19 + sourceCode/SG_baseFunc.cpp | 462 ++-------- sourceCode/SG_clustering.cpp | 28 +- sourceCode/SG_errCode.h | 8 +- sourceCode/SG_lineFeature.cpp | 47 + sourceCode/bagThreadPositioning.cpp | 828 +++++++++++++----- sourceCode/bagThreadPositioning_Export.h | 19 + 10 files changed, 952 insertions(+), 626 deletions(-) diff --git a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp index f3854c0..8cfafba 100644 --- a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp +++ b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp @@ -99,6 +99,54 @@ bool checkGridFormat(std::vector>& scanData) return true; } +void downSampleGridData(std::vector>& scanData, int dwnSampleRatio, std::vector>& sampleData) +{ + int lines = (int)scanData.size(); + for (int i = 0; i < lines; i++) + { + if ((i % dwnSampleRatio) == 0) + sampleData.push_back(scanData[i]); + } + return; +} + +int counterValidPts(std::vector< SVzNL3DPosition>& a_line) +{ + int ptNum = (int)a_line.size(); + int validNum = 0; + for (int i = 0; i < ptNum; i++) + { + if (a_line[i].pt3D.z > 1e-4) + validNum++; + } + return validNum; +} +void removeNullLines(std::vector>& scanData) +{ + if (scanData.size() == 0) + return; + + int lineNum = (int)scanData.size(); + int startLineIdx = -1; + int endLineIdx = 0; + for (int i = 0; i < lineNum; i++) + { + int validPtNum = counterValidPts(scanData[i]); + if (validPtNum > 0) + { + if (startLineIdx < 0) + startLineIdx = i; + endLineIdx = i; + } + } + if( (endLineIdx < lineNum-1) && (endLineIdx > 0)) + scanData.erase(scanData.begin() + endLineIdx+1, scanData.end()); + if (startLineIdx > 0) + scanData.erase(scanData.begin(), scanData.begin() + startLineIdx); + return; +} + + void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) { std::ifstream inputFile(fileName); @@ -252,7 +300,8 @@ void _outputChanneltInfo(char* fileName, std::vector& threadI void _outputRGBDScan_RGBD( char* fileName, std::vector>& scanLines, - std::vector& threadInfo + std::vector& threadInfo, + std::vector< SVzNL3DPoint>& markPoints ) { int lineNum = (int)scanLines.size(); @@ -306,6 +355,16 @@ void _outputRGBDScan_RGBD( rgb = { 250, 0, 0 }; size = 3; } + else if (pt3D->nPointIdx == 3) + { + rgb = { 250, 250, 0 }; + size = 3; + } + else if (pt3D->nPointIdx == 4) + { + rgb = { 0, 250, 250 }; + size = 3; + } else //if (pt3D->nPointIdx == 0) { rgb = { 200, 200, 200 }; @@ -322,8 +381,9 @@ void _outputRGBDScan_RGBD( if (objNum > 0) { - sw << "Line_" << lineIdx << "_0_" << (objNum*2) << std::endl; - size = 8; + int markNum = (int)markPoints.size(); + sw << "Line_" << lineIdx << "_0_" << (objNum*2 + markNum) << std::endl; + size = 10; for (int i = 0; i < objNum; i++) { rgb = { 250, 0, 0 }; @@ -342,6 +402,16 @@ void _outputRGBDScan_RGBD( sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } + for (int i = 0; i < markNum; i++) + { + rgb = { 250, 0, 0 }; + float x = (float)markPoints[i].x; + float y = (float)markPoints[i].y; + float z = (float)markPoints[i].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } //多输出一个，修正显示工具bug rgb = { 250, 0, 0 }; float x = (float)threadInfo[0].threadPos.x; @@ -387,34 +457,41 @@ void _outputRGBDScan_RGBD( #define CONVERT_TO_GRID 0 #define BAG_THREAD_POSITIONING 1 -#define TEST_GROUP 1 +#define TEST_GROUP 3 int main() { const char* dataPath[TEST_GROUP] = { "F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/", //0 + "F:/ShangGu/项目/吕宁项目/袋子拆线/现场数据/", //1 + "F:/ShangGu/项目/吕宁项目/袋子拆线/BJDGS/", //2 }; SVzNLRange fileIdx[TEST_GROUP] = { {1,13}, + {2,5}, + {8,9} }; const char* ver = wd_bagThreadPositioningVersion(); printf("ver:%s\n", ver); #if CONVERT_TO_GRID - int convertGrp = 0; - for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) + int convertGrp = 2; + for (int fidx = 11; fidx <= 11; fidx++) { char _scan_file[256]; - sprintf_s(_scan_file, "%s袋子_%d.txt", dataPath[convertGrp], fidx); + sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx); std::vector> scanData; - double lineStep = 0.096; + double lineStep = 0.032; double baseZ = 350.0; vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData); bool isGridData = checkGridFormat(scanData); int gridFormat = (true == isGridData) ? 1 : 0; printf("%s: 栅格格式=%d\n", _scan_file, gridFormat); + removeNullLines(scanData); + //std::vector> sampleData; + //downSampleGridData(scanData, 4, sampleData); //将数据恢复为按扫描线存储格式 sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx); _outputScanDataFile(_scan_file, scanData,0, 0, 0); @@ -423,7 +500,7 @@ int main() #endif #if BAG_THREAD_POSITIONING - for (int grp = 0; grp < TEST_GROUP; grp++) + for (int grp = 1; grp <= 1; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -436,10 +513,10 @@ int main() long t1 = (long)GetTickCount64();//统计时间 SSG_cornerParam cornerParam; - cornerParam.cornerTh = 70; - cornerParam.scale = 1.5; //袋子缝的尺度1.5mm比较合适 + cornerParam.cornerTh = 90; + cornerParam.scale = 4; //袋子缝的尺度1.5mm比较合适 cornerParam.minEndingGap = 1.0; // - cornerParam.minEndingGap_z = 1.0; + cornerParam.minEndingGap_z = 3.5; cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 cornerParam.jumpCornerTh_2 = 60; @@ -456,33 +533,57 @@ int main() SSG_treeGrowParam growParam; growParam.maxLineSkipNum = -1; //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance growParam.maxSkipDistance = 5.0; - growParam.yDeviation_max = 5.0; - growParam.zDeviation_max = 5.0;// + growParam.yDeviation_max = 1.0; + growParam.zDeviation_max = 1.0;// growParam.minLTypeTreeLen = 10; //mm, growParam.minVTypeTreeLen = 10; //mm SSX_ScanInfo scanInfo; scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 - scanInfo.scanFromThreadHead = false; //true:线袋子线缝头部开始扫描。 + scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。 scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度 scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置 + scanInfo.mark_diameter = 6.0; //mark外径 + scanInfo.mark_height = 3.5;//mark高 + scanInfo.mark_distance = 53.0; //两个Mark的距离 + + SSG_planeCalibPara poseCalibPara; + //初始化成单位阵 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = 2600.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + int errCode = 0; std::vector bagThreadInfo; + std::vector bagThreadInfo_relative; //相对于Mark的坐标 + std::vector output_markCenter; wd_bagThreadPositioning( scanLines, scanInfo, //true:激光线平行线缝；false:激光线垂直线缝 + poseCalibPara, filterParam, //噪点过滤参数 cornerParam, //V型特征参数 raisedFeatureParam,//线尾凸起参数 growParam, //特征生长参数 bagThreadInfo, + bagThreadInfo_relative, + output_markCenter, &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, bagThreadInfo); + _outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo, output_markCenter); sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); _outputChanneltInfo(_scan_file, bagThreadInfo); } diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 157bced..2ecea6d 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -169,6 +169,7 @@ + diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index b39b7f1..d1a06e5 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -192,6 +192,14 @@ SG_APISHARED_EXPORT void wd_getLineRaisedFeature( std::vector& line_raisedFeatures //͹ ); +//ȡָ߶ȵ +SG_APISHARED_EXPORT void wd_getSpecifiedHeightJumping( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SVzNLRangeD jumpHeight, //߶Ȳ + std::vector& line_jumpFeatures // +); + //ֱȡsplit-and-merge˼򻯣յֱߴֱ SG_APISHARED_EXPORT void wd_surfaceLineSegment( std::vector< SVzNL3DPosition>& lineData, @@ -502,6 +510,15 @@ SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts); //ǶȲֵ0-180ȷΧ SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2); +/** + * @brief ab**зת**Χ- ~ У + * @param a Դ + * @param b Ŀ + * @param rotAngle תǣȣʱΪ˳ʱΪ + * @return trueɹfalseʧܣ + */ +SG_APISHARED_EXPORT bool calcRotateAngle(const SVzNL2DPointD& a, const SVzNL2DPointD& b, double& rotAngle); + //ֱ߽ SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs( double a1, double b1, double c1, @@ -686,6 +703,15 @@ SG_APISHARED_EXPORT void wd_gridPointClustering_labelling( SVzNLRect& clusterRoi //roi2D ); +//ʹþ෽8ͨͨ:Ӧͬݽṹ +SG_APISHARED_EXPORT void wd_gridPointClustering_labelling_2( + std::vector>& srcData, + std::vector>& labelMask, + int clusterID, //ǰClusterID + std::vector< SVzNL2DPoint>& a_cluster, //result + SVzNL3DRangeD& clusterRoi //roi3D +); + //դݽXYƽϵͶӰĿհ׵вֵ SG_APISHARED_EXPORT void pointClout2DProjection( std::vector< std::vector>& gridScanData, @@ -700,16 +726,15 @@ SG_APISHARED_EXPORT void pointClout2DProjection( ); //դݽXYƽϵͶӰZֵĿհ׵вֵ -SG_APISHARED_EXPORT void pointClout2DQuantization( +SG_APISHARED_EXPORT void pointCloud2DQuantization( std::vector< std::vector>& gridScanData, SVzNLRangeD x_range, SVzNLRangeD y_range, double scale, - double cuttingGrndZ, int edgeSkip, double inerPolateDistTh, //ֵֵڴֵIJֵ - std::vector> quantiData, //ݣʼΪһֵ1e+6 - std::vector> backIndexing //ڻ3D + std::vector>& quantiData, //ݣʼΪһֵ1e+6 + std::vector>& backIndexing //ڻ3D ); //ˮ㷨 diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 9f15391..dc79e67 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -39,6 +39,12 @@ typedef struct SWD3DPoint point; }SWDIndexing3DPoint; +typedef struct +{ + int start; + int end; +}SWD_Interval; // + typedef struct { bool validFlag; //ָʾǷЧ @@ -55,6 +61,12 @@ typedef enum keSG_PoseSorting_T2B_L2R, ///ϵ£ } ESG_poseSortingMode; +typedef enum +{ + KeWD_Mask_ValidPt = 0, + KeWD_Mask_NullPt, +}EWD_maskMode; + typedef struct { int data_0; @@ -62,6 +74,13 @@ typedef struct int idx; }SSG_intPair; +typedef struct +{ + int flag; + int validFlag; + int clusterID; +}SSG_clusterLabel; + typedef struct { int featurType; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 53a0b64..6a04742 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -9,6 +9,8 @@ #include #include +const double EPS = 1e-10; + SVzNL3DRangeD sg_getScanDataROI( //ɨROI SVzNL3DLaserLine* laser3DPoints, @@ -276,277 +278,6 @@ SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector& inliers, double* _k, double* _b) -{ - //Сֱ߲ - double xx_sum = 0; - double x_sum = 0; - double y_sum = 0; - double xy_sum = 0; - int num = 0; - for (int i = 0; i < inliers.size(); i++) - { - x_sum += inliers[i].x; //xۼӺ - y_sum += inliers[i].y; //yۼӺ - xx_sum += inliers[i].x * inliers[i].x; //xƽۼӺ - xy_sum += inliers[i].x * inliers[i].y; //xyۼӺ - num++; - } - *_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //ݹʽk - *_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//ݹʽb -} - -//ϳֱͨ߷ֱ̣ -void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c) -{ - //жǷΪֱ - int dataSize = (int)inliers.size(); - if (dataSize <2) - return; - - double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x); - double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y); - std::vector< SVzNL3DPoint> fittingData; - if (deltaX < deltaY) - { - //x=ky+b - for (int i = 0; i < dataSize; i++) - { - SVzNL3DPoint a_fitPt; - a_fitPt.x = inliers[i].y; - a_fitPt.y = inliers[i].x; - a_fitPt.z = inliers[i].z; - fittingData.push_back(a_fitPt); - } - double k = 0, b = 0; - lineFitting(fittingData, &k, &b); - //ax+by+c - *_a = 1.0; - *_b = -k; - *_c = -b; - } - else - { - //y = kx+b - double k = 0, b = 0; - lineFitting(inliers, &k, &b); - //ax+by+c - *_a = k; - *_b = -1; - *_c = b; - } - return; -} - -//ԲС -double fitCircleByLeastSquare( - const std::vector& pointArray, - SVzNL3DPoint& center, - double& radius) -{ - int N = pointArray.size(); - if (N < 3) { - return std::numeric_limits::max(); - } - - double sumX = 0.0; - double sumY = 0.0; - double sumX2 = 0.0; - double sumY2 = 0.0; - double sumX3 = 0.0; - double sumY3 = 0.0; - double sumXY = 0.0; - double sumXY2 = 0.0; - double sumX2Y = 0.0; - - for (int pId = 0; pId < N; ++pId) { - sumX += pointArray[pId].x; - sumY += pointArray[pId].y; - - double x2 = pointArray[pId].x * pointArray[pId].x; - double y2 = pointArray[pId].y * pointArray[pId].y; - sumX2 += x2; - sumY2 += y2; - - sumX3 += x2 * pointArray[pId].x; - sumY3 += y2 * pointArray[pId].y; - sumXY += pointArray[pId].x * pointArray[pId].y; - sumXY2 += pointArray[pId].x * y2; - sumX2Y += x2 * pointArray[pId].y; - } - - double C, D, E, G, H; - double a, b, c; - - C = N * sumX2 - sumX * sumX; - D = N * sumXY - sumX * sumY; - E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX; - G = N * sumY2 - sumY * sumY; - H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY; - - a = (H * D - E * G) / (C * G - D * D); - b = (H * C - E * D) / (D * D - G * C); - c = -(a * sumX + b * sumY + sumX2 + sumY2) / N; - - center.x = -a / 2.0; - center.y = -b / 2.0; - radius = sqrt(a * a + b * b - 4 * c) / 2.0; - - double err = 0.0; - double e; - double r2 = radius * radius; - for (int pId = 0; pId < N; ++pId) { - e = pow(pointArray[pId].x - center.x,2) + pow(pointArray[pId].y - center.y, 2) - r2; - if (e > err) { - err = e; - } - } - return err; -} - -#if 0 -bool leastSquareParabolaFit(const std::vector& points, - double& a, double& b, double& c, - double& mse, double& max_err) -{ - // У㼯3ߣ - int n = points.size(); - if (n < 3) { - return false; - } - - // ʼͲ - double sum_x = 0.0, sum_x2 = 0.0, sum_x3 = 0.0, sum_x4 = 0.0; - double sum_y = 0.0, sum_xy = 0.0, sum_x2y = 0.0; - - // - for (const auto& p : points) { - double x = p.x; - double y = p.y; - double x2 = x * x; - double x3 = x2 * x; - double x4 = x3 * x; - - sum_x += x; - sum_x2 += x2; - sum_x3 += x3; - sum_x4 += x4; - sum_y += y; - sum_xy += x * y; - sum_x2y += x2 * y; - } - - // Է M * [a,b,c]^T = N - // M3x3 - double M[3][3] = { - {sum_x4, sum_x3, sum_x2}, - {sum_x3, sum_x2, sum_x}, - {sum_x2, sum_x, (double)n} - }; - - // N3x1 - double N[3] = { sum_x2y, sum_xy, sum_y }; - - // ˹ԪԷ飨3Ԫһη飩 - // 1MתΪǾ - for (int i = 0; i < 3; i++) { - // ѡԪΪ0 - int pivot = i; - for (int j = i; j < 3; j++) { - if (fabs(M[j][i]) > fabs(M[pivot][i])) { - pivot = j; - } - } - // - std::swap(M[i], M[pivot]); - std::swap(N[i], N[pivot]); - - // һԪ - double div = M[i][i]; - if (fabs(div) < 1e-10) { - return false; - } - for (int j = i; j < 3; j++) { - M[i][j] /= div; - } - N[i] /= div; - - // ȥ· - for (int j = i + 1; j < 3; j++) { - double factor = M[j][i]; - for (int k = i; k < 3; k++) { - M[j][k] -= factor * M[i][k]; - } - N[j] -= factor * N[i]; - } - } - - // 2ش - c = N[2]; - b = N[1] - M[1][2] * c; - a = N[0] - M[0][1] * b - M[0][2] * c; - - // - mse = 0.0; - max_err = 0.0; - for (const auto& p : points) { - double y_fit = a * p.x * p.x + b * p.x + c; - double err = y_fit - p.y; - double err_abs = fabs(err); - mse += err * err; - if (err_abs > max_err) { - max_err = err_abs; - } - } - mse /= n; // - - return true; -} -#endif -//С y=ax^2 + bx + c -bool leastSquareParabolaFitEigen( - const std::vector& points, - double& a, double& b, double& c, - double& mse, double& max_err) -{ - int n = points.size(); - if (n < 3) { - return false; - } - - // ϵAĿB - Eigen::MatrixXd A(n, 3); - Eigen::VectorXd B(n); - for (int i = 0; i < n; i++) { - double x = points[i].x; - A(i, 0) = x * x; - A(i, 1) = x; - A(i, 2) = 1.0; - B(i) = points[i].y; - } - - // С⣺Ax = BֱӵEigen - Eigen::Vector3d coeffs = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B); - a = coeffs(0); - b = coeffs(1); - c = coeffs(2); - - // - mse = 0.0; - max_err = 0.0; - for (const auto& p : points) { - double y_fit = a * p.x * p.x + b * p.x + c; - double err = y_fit - p.y; - double err_abs = fabs(err); - mse += err * err; - if (err_abs > max_err) { - max_err = err_abs; - } - } - mse /= n; - return true; -} - //Zֵ double computeMeanZ(std::vector< SVzNL3DPoint>& pts) { @@ -653,6 +384,67 @@ double computeXOYVertexAngle(SVzNL3DPoint p0, SVzNL3DPoint p1, SVzNL3DPoint p2) return angle; } + +// ģ +double vecNorm(const SVzNL2DPointD& v) { + return sqrt(v.x * v.x + v.y * v.y); +} + +// һλǷɹfalse +bool vecNormalize(SVzNL2DPointD& v) { + double norm = vecNorm(v); + if (norm < EPS) { // ޷һ + return false; + } + v.x /= norm; + v.y /= norm; + return true; +} + +// ĵ +double vecDot(const SVzNL2DPointD& a, const SVzNL2DPointD& b) { + return a.x * b.x + a.y * b.y; +} + +// 2Dֵ +double vecCross(const SVzNL2DPointD& a, const SVzNL2DPointD& b) { + return a.x * b.y - a.y * b.x; +} +/** + * @brief ab**зת**Χ- ~ У + * @param a Դ + * @param b Ŀ + * @param rotAngle תǣȣʱΪ˳ʱΪ + * @return trueɹfalseʧܣ + */ +bool calcRotateAngle(const SVzNL2DPointD& a, const SVzNL2DPointD& b, double& rotAngle) { + SVzNL2DPointD aNorm = a; + SVzNL2DPointD bNorm = b; + // һֱӷʧ + if (!vecNormalize(aNorm) || !vecNormalize(bNorm)) { + std::cerr << "Error: Ϊ޷תǣ" << std::endl; + return false; + } + // ǯλ⸡㾫ȵ³[-1,1] + double dot = vecDot(aNorm, bNorm); + if (dot < -1.0 + EPS) + dot = -1.0 + EPS; + if (dot > 1.0 - EPS) + dot = 1.0 - EPS; + + // ޷нǣ0 ~ У + double angle = acos(dot); + // жת + double cross = vecCross(aNorm, bNorm); + if (cross < -EPS) { // ˳ʱ룬Ƕȡ + rotAngle = -angle; + } + else { // ʱ/ߣǶȡ + rotAngle = angle; + } + return true; +} + double computePtDistToLine(double x0, double y0, double a, double b, double c) { double tmp = sqrt(pow(a, 2) + pow(b, 2)); @@ -1193,54 +985,6 @@ void SG_TwoPassLabel( } #endif -//: z = Ax + By + C -//res: [0]=A, [1]= B, [2]=-1.0, [3]=C, -void vzCaculateLaserPlane(std::vector Points3ds, std::vector& res) -{ - //С˷ƽ - //ȡcv::MatϵΪxᣬΪyᣬcvPoint෴ - //ϵ - cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1); - // - cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1); - // - cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1); - double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0; - for (int i = 0; i < Points3ds.size(); i++) - { - x2 += (double)Points3ds[i].x * (double)Points3ds[i].x; - y2 += (double)Points3ds[i].y * (double)Points3ds[i].y; - xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y; - xi += (double)Points3ds[i].x; - yi += (double)Points3ds[i].y; - zixi += (double)Points3ds[i].z * (double)Points3ds[i].x; - ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y; - zi += (double)Points3ds[i].z; - } - A.at(0, 0) = x2; - A.at(1, 0) = xiyi; - A.at(2, 0) = xi; - A.at(0, 1) = xiyi; - A.at(1, 1) = y2; - A.at(2, 1) = yi; - A.at(0, 2) = xi; - A.at(1, 2) = yi; - A.at(2, 2) = (double)((int)Points3ds.size()); - B.at(0, 0) = zixi; - B.at(1, 0) = ziyi; - B.at(2, 0) = zi; - //ƽϵ - X = A.inv() * B; - //A - res.push_back(X.at(0, 0)); - //B - res.push_back(X.at(1, 0)); - //ZϵΪ-1 - res.push_back(-1.0); - //C - res.push_back(X.at(2, 0)); - return; -} // ƽ淨ŷǣZYX˳ SSG_EulerAngles planeNormalToEuler(double A, double B, double C) { @@ -3063,18 +2807,16 @@ void pointClout2DProjection( } } -#if 0 //դݽXYƽϵͶӰZֵĿհ׵вֵ -void pointClout2DQuantization( +void pointCloud2DQuantization( std::vector< std::vector>& gridScanData, SVzNLRangeD x_range, SVzNLRangeD y_range, double scale, - double cuttingGrndZ, int edgeSkip, double inerPolateDistTh, //ֵֵڴֵIJֵ - std::vector> quantiData, //ݣʼΪһֵ1e+6 - std::vector> backIndexing //ڻ3D + std::vector>& quantiData, //ݣʼΪһֵ1e+6 + std::vector>& backIndexing //ڻ3D ) { int lineNum = (int)gridScanData.size(); @@ -3086,12 +2828,15 @@ void pointClout2DQuantization( int y_rows = (int)((y_range.max - y_range.min) / scale) + 1 + edgeSkip * 2; quantiData.resize(x_cols); backIndexing.resize(x_cols); + double quantiXStart = x_range.min - edgeSkip * scale; + double quantiYStart = y_range.min - edgeSkip * scale; for (int i = 0; i < x_cols; i++) { quantiData[i].resize(y_rows); - std::fill(quantiData[i].begin(), quantiData[i].end(), 0); + for (int j = 0; j < y_rows; j++) + quantiData[i][j] = {i * scale + quantiXStart + scale/2, j * scale + quantiYStart + scale / 2 , 0}; backIndexing[i].resize(y_rows); - std::fill(backIndexing[i].begin(), backIndexing[i].end(), 0); + std::fill(backIndexing[i].begin(), backIndexing[i].end(), SVzNL2DPoint{0,0}); } int nPointCnt = (int)gridScanData[0].size(); @@ -3104,8 +2849,6 @@ void pointClout2DQuantization( SVzNL3DPosition* pt3D = &gridScanData[line][i]; if (pt3D->pt3D.z < 1e-4) continue; - if ((cuttingGrndZ > 0) && (pt3D->pt3D.z > cuttingGrndZ)) - continue; double x = pt3D->pt3D.x; double y = pt3D->pt3D.y; int px = (int)(x - x_range.min) / scale + edgeSkip; @@ -3122,7 +2865,7 @@ void pointClout2DQuantization( { SVzNL2DPoint v2i = { line, i }; backIndexing[px][px] = v2i; - quantiData[px][py] = pt3D->pt3D.z; + quantiData[px][py].z = pt3D->pt3D.z; //ֱֵ if (prePt) { @@ -3148,7 +2891,7 @@ void pointClout2DQuantization( k2 = 1.0 - k1; } double inter_z = k1 * pt3D->pt3D.z + k2 * prePt->pt3D.z; - quantiData[interPts[m].x][interPts[m].y] = inter_z; + quantiData[interPts[m].x][interPts[m].y].z = inter_z; } } } @@ -3168,7 +2911,7 @@ void pointClout2DQuantization( double pre_value = -1; for (int x = 0; x < cols; x++) { - double value = quantiData[x][y]; + double value = quantiData[x][y].z; if (value > 1e-4) { if (pre_x >= 0) @@ -3182,7 +2925,7 @@ void pointClout2DQuantization( double k1 = ((double)(m - pre_x)) / ((double)x_diff); double k2 = 1.0 - k1; double inter_z = k1 * value + k2 * pre_value; - quantiData[x][y] = inter_z; + quantiData[x][y].z = inter_z; } } } @@ -3192,7 +2935,6 @@ void pointClout2DQuantization( } } } -#endif //ԿռӦתƽƾ // Eigenʵ @@ -3373,60 +3115,6 @@ void WD_EulerRpyToDirVectors(double rpy[3],std::vector& dirVectors) } -/** - * @brief ռֱС - * @param points ά㼯2㣬壩 - * @param center ֱߵģ׼P0 - * @param direction ֱߵķvλ - * @return Ƿɹ㼯Чtrue - */ -bool fitLine3DLeastSquares(const std::vector& points, SVzNL3DPoint& center, SVzNL3DPoint& direction) -{ - // 㼯Ч - if (points.size() < 2) { - std::cerr << "Error: 㼯ڵ2" << std::endl; - return false; - } - - int n = points.size(); - Eigen::MatrixXd A(n, 3); // 㼯ÿһ(x,y,z) - - // 1. ģcenter - double cx = 0.0, cy = 0.0, cz = 0.0; - for (const auto& p : points) { - cx += p.x; - cy += p.y; - cz += p.z; - A.row(points.size() - n) << p.x, p.y, p.z; // 㼯 - n--; - } - cx /= points.size(); - cy /= points.size(); - cz /= points.size(); - center = { cx, cy, cz }; - - // 2. ȥĻЭ3x3 - // ؼ޸ʹRowVector3drowwiseƥά - Eigen::RowVector3d centroid_row(cx, cy, cz); - Eigen::MatrixXd centered = A.rowwise() - centroid_row; // άƥ䣬ޱ - - // Э㣨n-1ΪƫƣҲֱn - Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1); - // 3. ֵֽ⣺Эֵ - Eigen::SelfAdjointEigenSolver eigensolver(cov); - if (eigensolver.info() != Eigen::Success) { - std::cerr << "Error: ֵֽʧܣ" << std::endl; - return false; - } - - // ֵӦΪEigenĬϰֵУȡһ - Eigen::Vector3d dir = eigensolver.eigenvectors().col(2); - // λѡͨ׼ - dir.normalize(); - - direction = { dir(0), dir(1), dir(2) }; - return true; -} #if 0 #include diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index 536783a..a5cd241 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -282,14 +282,14 @@ void wd_gridPointClustering_labelling( } return; } -#if 0 + //ʹþ෽8ͨͨ void wd_gridPointClustering_labelling_2( - std::vector>& srcData, - std::vector>& labelMask, + std::vector>& srcData, + std::vector>& labelMask, int clusterID, //ǰClusterID std::vector< SVzNL2DPoint>& a_cluster, //result - SVzNLRect& clusterRoi //roi2D + SVzNL3DRangeD& clusterRoi //roi3D ) { int i = 0; @@ -304,7 +304,8 @@ void wd_gridPointClustering_labelling_2( if ((a_seedPos.x == 390) && (a_seedPos.y == 949)) int kkk = 1; - SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y]; + SSG_clusterLabel& a_seed = labelMask[a_seedPos.x][a_seedPos.y]; + SVzNL3DPoint& a_feature3DValue = srcData[a_seedPos.x][a_seedPos.y]; if (0 == a_seed.clusterID) //clusterID == 0, δ { //8ͨ @@ -319,8 +320,8 @@ void wd_gridPointClustering_labelling_2( if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum)) { - SSG_featureClusteringInfo& chk_seed = featureMask[x][y]; - if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0)) //ֻδ + SSG_clusterLabel& chk_seed = labelMask[x][y]; + if ((chk_seed.validFlag == 0) || (chk_seed.clusterID > 0)) //ֻδ continue; if (0 == chk_seed.flag)//ֹظ @@ -336,12 +337,13 @@ void wd_gridPointClustering_labelling_2( } a_seed.clusterID = clusterID; //ROI - clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left; - clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right; - clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top; - clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom; + clusterRoi.xRange.min = clusterRoi.xRange.min > a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.min; + clusterRoi.xRange.max = clusterRoi.xRange.max < a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.max; + clusterRoi.yRange.min = clusterRoi.yRange.min > a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.min; + clusterRoi.yRange.max = clusterRoi.yRange.max < a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.max; + clusterRoi.zRange.min = clusterRoi.zRange.min > a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.min; + clusterRoi.zRange.max = clusterRoi.zRange.max < a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.max; i++; } return; -} -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 9615e7b..51ca935 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -8,6 +8,8 @@ #define SG_ERR_INVLD_SORTING_MODE -1005 #define SG_ERR_INVLD_Q_SCALE -1006 #define SG_ERR_ZERO_OBJECTS -1007 +#define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008 +#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009 //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 @@ -21,7 +23,8 @@ //ץȡ #define SX_ERR_INVLID_CUTTING_Z -2101 -#define SX_ERR_ZERO_OBJ -2102 +#define SX_ERR_ZERO_OBJ_TOPLAYER -2102 +#define SX_ERR_ZERO_OBJ_BTMLAYER -2103 // #define SX_BAG_TRAY_EMPTY -2201 @@ -29,3 +32,6 @@ //ü߶Ȳ #define SX_ERR_INVALID_ARC -2301 +//ǴӲ +#define SX_ERR_NO_MARK -2401 + diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 12e4182..2dbb827 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -4574,6 +4574,53 @@ void wd_getLineRaisedFeature( } } +//ȡָ߶ȵ +void wd_getSpecifiedHeightJumping( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SVzNLRangeD jumpHeight, //߶Ȳ + std::vector& line_jumpFeatures // +) +{ + int dataSize = (int)lineData.size(); + double pre_z = 0; + int preIdx = -1; + for (int i = 0; i < dataSize; i++) + { + if (i == 1850) + int kkk = 1; + lineData[i].nPointIdx = i; //± + if (lineData[i].pt3D.z > 1e-4) + { + if(preIdx >= 0) + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + + if ( (z_diff >= jumpHeight.min) && (z_diff <= jumpHeight.max)) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + + if (lineData[i].pt3D.z < pre_z) + { + a_feature.jumpPos = lineData[i].pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + } + else + { + a_feature.jumpPos = lineData[preIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, preIdx }; + } + + line_jumpFeatures.push_back(a_feature); + } + } + preIdx = i; + pre_z = lineData[i].pt3D.z; + } + } +} + //ʹö˵ֱߣ㵽ֱߵľ룬޵ķָ void split( SSG_RUN a_run, diff --git a/sourceCode/bagThreadPositioning.cpp b/sourceCode/bagThreadPositioning.cpp index a7dc060..c4453dc 100644 --- a/sourceCode/bagThreadPositioning.cpp +++ b/sourceCode/bagThreadPositioning.cpp @@ -6,21 +6,132 @@ #include //version 1.0.0 : base version release to customer -std::string m_strVersion = "1.0.0"; +//version 1.1.0 : ˱궨Mark⣬ڱ궨ƽ +std::string m_strVersion = "1.1.0"; const char* wd_bagThreadPositioningVersion(void) { return m_strVersion.c_str(); } + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara wd_bagThread_getBaseCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//̬ƽȥ +void wd_bagThread_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + #if 1 + +SVzNL3DPosition _findClosestPoint(std::vector& lineData, double a, double b, double c, double* distMin) +{ + SVzNL3DPosition bestPt = { -1, { 0, 0, -1 } }; + + double mod = sqrt(a * a + b * b); + a = a / mod; + b = b / mod; + c = c / mod; + double minDist = DBL_MAX; + for (int i = 0; i < (int)lineData.size(); i++) + { + if (lineData[i].pt3D.z > 1e-4) + { + double dist = abs(a * lineData[i].pt3D.x + b * lineData[i].pt3D.y + c); + if (minDist > dist) + { + minDist = dist; + bestPt = lineData[i]; + bestPt.nPointIdx = i; + } + } + } + *distMin = minDist; + return bestPt; +} + +// +void cloutPointsClustering( + std::vector>& featureInfoMask, + std::vector>& feature3DInfo, + SSG_treeGrowParam growParam, + std::vector>& clusters, //ֻ¼λ + std::vector& clustersInfo) +{ + int lineNum = (int)feature3DInfo.size(); + if (lineNum == 0) + return; + int linePtNum = (int)feature3DInfo[0].size(); + //õ˼룬ع˼·иЧ + int clusterID = 1; + int clusterCheckWin = 5; + for (int y = 0; y < linePtNum; y++) + { + for (int x = 0; x < lineNum; x++) + { + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; + if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //Ѿ + continue; + + SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; + SVzNL3DRangeD a_clusterRoi; + a_clusterRoi.xRange.min = a_feature3DValue.x; + a_clusterRoi.xRange.max = a_feature3DValue.x; + a_clusterRoi.yRange.min = a_feature3DValue.y; + a_clusterRoi.yRange.max = a_feature3DValue.y; + a_clusterRoi.zRange.min = a_feature3DValue.z; + a_clusterRoi.zRange.max = a_feature3DValue.z; + + SVzNL2DPoint a_seedPos = { x, y }; + std::vector< SVzNL2DPoint> a_cluster; + a_cluster.push_back(a_seedPos); + wd_gridPointClustering( + featureInfoMask,//int¼ǺclusterIDһflag + feature3DInfo,//double,¼Ϣ + clusterCheckWin, // + growParam,// + clusterID, //ǰClusterID + a_cluster, //result + a_clusterRoi + ); + clusters.push_back(a_cluster); + SWD_clustersInfo a_info; + a_info.clusterIdx = clusterID; + a_info.ptSize = (int)a_cluster.size(); + a_info.roi3D = a_clusterRoi; + clustersInfo.push_back(a_info); + clusterID++; + } + } +} + +//ʱתʱ > 0 ˳ʱתʱ < 0 +SVzNL3DPoint rotateXoY(SVzNL3DPoint& pt, double sinTheta, double cosTheta) +{ + return (SVzNL3DPoint{ (pt.x * cosTheta - pt.y * sinTheta), (pt.x * sinTheta + pt.y * cosTheta), pt.z }); +} + //ͷλüⶨλ void wd_bagThreadPositioning( std::vector< std::vector>& scanLines, const SSX_ScanInfo scanInfo, //true:ƽв۵false:ߴֱ۵ + const SSG_planeCalibPara groundCalibPara, const SSG_outlierFilterParam filterParam, //˲ const SSG_cornerParam cornerPara, //V const SSG_raisedFeatureParam raisedFeaturePara,//β͹ const SSG_treeGrowParam growParam, // std::vector& bagThreadInfo, + std::vector& bagThreadInfo_relative, //Mark + std::vector& output_markCenter, //MarkλϢ int* errCode) { *errCode = 0; @@ -30,6 +141,16 @@ void wd_bagThreadPositioning( *errCode = SG_ERR_3D_DATA_NULL; return; } + if (true == scanInfo.isHorizonScan) + { + *errCode = SG_ERR_LASER_DIR_NOT_SUPPORTED; + return; + } + if (false == scanInfo.scanFromThreadHead) + { + *errCode = SG_ERR_SCAN_DIR_NOT_SUPPORTED; + return; + } int linePtNum = (int)scanLines[0].size(); //жݸʽǷΪgrid㷨ֻܴgridݸʽ @@ -48,6 +169,13 @@ void wd_bagThreadPositioning( return; } + //ƽ + for (int i = 0; i < lineNum; i++) + { //д + //ƽȥ + wd_bagThread_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); + } + //ˮƽɨ //ͳƽ߼͵㷨ڼǰǺͺʱ double ptInterval = 0; @@ -106,16 +234,25 @@ void wd_bagThreadPositioning( } lineInterval = lineInterval / lineIntervalNum; + SVzNLRangeD jumpHeight = { scanInfo.mark_height-1.0, scanInfo.mark_height+1.0}; + double markRotateAngle = 0; + SVzNL3DPoint markCenter = { 0,0,0 }; + std::vector< SVzNL2DPoint> debug_markOrigin; + std::vector< SVzNL2DPoint> debug_markXDir; + double vCornerScale = cornerPara.scale * 4; std::vector> cornerFeatures; std::vector> raisedFeatures; - if (false == scanInfo.isHorizonScan) + //ֳҪֱɨ + //if (false == scanInfo.isHorizonScan) // { + std::vector> jumpFeatures_v; + int validVCornerSCale = (int)(vCornerScale / ptInterval); //ֱɨVͲۺβ for (int line = 0; line < lineNum; line++) { - if ((line == 577) || (line == 932)) + if ((line == 750) || (line == 905)) int kkk = 1; std::vector& lineData = scanLines[line]; //˲˳쳣 @@ -163,67 +300,266 @@ void wd_bagThreadPositioning( line_raisedFeatures //͹ ); raisedFeatures.push_back(line_raisedFeatures); + + //ȡ궨 + std::vector line_jumpFearures; + wd_getSpecifiedHeightJumping( + lineData, + line, + jumpHeight, //߶Ȳ + line_jumpFearures // + ); + jumpFeatures_v.push_back(line_jumpFearures); } -#if 0 - //ˮƽɨӱ - std::vector> jumpdFeatures; -#endif - } - else - { - int validVCornerSCale = (int)(vCornerScale / lineInterval); - //ˮƽɨVͲۺβ +#if 1 + //ˮƽɨ궨 + std::vector> jumpFeatures_h; for (int line = 0; line < lineNum_h; line++) { - if (line == 329) + if ((line == 750) || (line == 905)) int kkk = 1; + std::vector& lineData = data_lines_h[line]; //˲˳쳣 sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam); - //ȡVͲ - std::vector line_cornerFeatures; - std::vector segs; - int dataSize = (int)lineData.size(); - wd_getLineCorerFeature_accelerate( + //ȡ궨 + std::vector line_jumpFearures; + wd_getSpecifiedHeightJumping( lineData, line, - cornerPara, - lineInterval, - segs, - line_cornerFeatures //յ + jumpHeight, //߶Ȳ + line_jumpFearures // ); - //V࣬ - std::vector valid_cornerFeatures; - int vCornerSize = (int)line_cornerFeatures.size(); - int segSize = (int)segs.size(); - for (int m = 0; m < vCornerSize; m++) + jumpFeatures_h.push_back(line_jumpFearures); + } + + //Ա궨о + std::vector> featureInfoMask; + std::vector> feature3DInfo; + featureInfoMask.resize(lineNum); + feature3DInfo.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + featureInfoMask[line].resize(linePtNum); + std::fill(featureInfoMask[line].begin(), featureInfoMask[line].end(), SSG_featureClusteringInfo{ 0,0,0,0,0,0,0 }); + feature3DInfo[line].resize(linePtNum); + std::fill(feature3DInfo[line].begin(), feature3DInfo[line].end(), SVzNL3DPoint{ 0,0,0 }); + } + //ֱ + for (int line = 0; line < lineNum; line++) + { + int lineJumpNum = (int)jumpFeatures_v[line].size(); + for(int j = 0; j = segs[n].start) && (cornerPos <= segEnd)) + featureInfoMask[ptIdx][line] = a_feature; + feature3DInfo[ptIdx][line] = { a_jump.jumpPos.y, a_jump.jumpPos.x, a_jump.jumpPos.z }; + } + } + } + // + std::vector> markClusters; //ֻ¼λ + std::vector markClustersInfo; + SSG_treeGrowParam markGrowParam; + markGrowParam.yDeviation_max = 1.0;//ʱYƫ + markGrowParam.zDeviation_max = 1.0; //ʱZƫ + markGrowParam.maxLineSkipNum = 5; //ʱ߼ -1ʱʹmaxDkipDistance + markGrowParam.maxSkipDistance = 1.0; //maxLineSkipNumΪ-1 ʹô˲.Ϊ-1ʱ˲Ч + markGrowParam.minLTypeTreeLen = 5.0; //ٵĽڵĿСڴĿƳ + markGrowParam.minVTypeTreeLen = 5.0; //ٵĽڵĿСڴĿƳ + cloutPointsClustering( + featureInfoMask, + feature3DInfo, + markGrowParam, + markClusters, //ֻ¼λ + markClustersInfo); + + //жϱ궨Ƿ + int clusterNum = (int)markClustersInfo.size(); + if (clusterNum < 2) + { + *errCode = SX_ERR_NO_MARK; + return; + } + else + { + std::vector validMarks; + for (int i = 0; i < clusterNum; i++) + { + double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min; + double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min; + if ((w >= scanInfo.mark_diameter - 0.5) && (w <= scanInfo.mark_diameter + 1.0) && + (h >= scanInfo.mark_diameter - 0.5) && (h <= scanInfo.mark_diameter + 1.0)) + { + validMarks.push_back(i); + } + } + + int mark_O_idx = -1, mark_x_idx = -1; + if (validMarks.size() < 2) + { + *errCode = SX_ERR_NO_MARK; + return; + } + else if (validMarks.size() == 2) + { + int mark0Idx = validMarks[0]; + int mark1Idx = validMarks[1]; + double cy_1 = (markClustersInfo[mark0Idx].roi3D.yRange.max + markClustersInfo[mark0Idx].roi3D.yRange.min) / 2; + double cy_2 = (markClustersInfo[mark1Idx].roi3D.yRange.max + markClustersInfo[mark1Idx].roi3D.yRange.min) / 2; + if (cy_1 < cy_2) + { + mark_O_idx = mark0Idx; + mark_x_idx = mark1Idx; + } + else + { + mark_O_idx = mark1Idx; + mark_x_idx = mark0Idx; + } + } + else// if (validMarks.size() > 2) + { + //һ + SSG_intPair obj_pair = {-1,-1,-1}; + double best_dist = -1; + for (int i = 0; i < (int)validMarks.size(); i++) + { + int vldIdx0 = validMarks[i]; + double cx_1 = (markClustersInfo[vldIdx0].roi3D.xRange.max + markClustersInfo[vldIdx0].roi3D.xRange.min) / 2; + double cy_1 = (markClustersInfo[vldIdx0].roi3D.yRange.max + markClustersInfo[vldIdx0].roi3D.yRange.min) / 2; + for (int j = i + 1; j < (int)validMarks.size(); j++) { - int skip_1 = cornerPos - segs[n].start; - int skip_2 = segEnd - cornerPos; - if ((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale)) - valid_cornerFeatures.push_back(line_cornerFeatures[m]); - break; + int vldIdx1 = validMarks[j]; + double cx_2 = (markClustersInfo[vldIdx1].roi3D.xRange.max + markClustersInfo[vldIdx1].roi3D.xRange.min) / 2; + double cy_2 = (markClustersInfo[vldIdx1].roi3D.yRange.max + markClustersInfo[vldIdx1].roi3D.yRange.min) / 2; + double dist = sqrt(pow(cx_1 - cx_2, 2) + pow(cy_1 - cy_2, 2)); + if (best_dist < 0) + { + best_dist = dist; + obj_pair.data_0 = vldIdx0; + obj_pair.data_1 = vldIdx1; + } + else + { + double dist_diff1 = abs(best_dist - scanInfo.mark_distance); + double dist_diff2 = abs(dist - scanInfo.mark_distance); + if (dist_diff1 > dist_diff2) + { + best_dist = dist; + obj_pair.data_0 = vldIdx0; + obj_pair.data_1 = vldIdx1; + } + } + } + } + if (obj_pair.data_0 < 0) + { + *errCode = SX_ERR_NO_MARK; + return; + } + else + { + double cy_1 = (markClustersInfo[obj_pair.data_0].roi3D.yRange.max + markClustersInfo[obj_pair.data_0].roi3D.yRange.min) / 2; + double cy_2 = (markClustersInfo[obj_pair.data_1].roi3D.yRange.max + markClustersInfo[obj_pair.data_1].roi3D.yRange.min) / 2; + if (cy_1 < cy_2) + { + mark_O_idx = obj_pair.data_0; + mark_x_idx = obj_pair.data_1; + } + else + { + mark_O_idx = obj_pair.data_1; + mark_x_idx = obj_pair.data_0; } } } - cornerFeatures.push_back(valid_cornerFeatures); - //ȡ͹ - std::vector line_raisedFeatures; - wd_getLineRaisedFeature( - lineData, - line, - raisedFeaturePara, //͹ - line_raisedFeatures //͹ - ); - raisedFeatures.push_back(line_raisedFeatures); + //ԲϼԲģԵZֵֵΪԲĵZֵ + std::vector fittingData_0; + double meanZ_0 = 0; + for (int i = 0; i < markClusters[mark_O_idx].size(); i++) + { + SVzNL2DPoint& a_pt2D = markClusters[mark_O_idx][i]; + SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y]; + meanZ_0 += a_pt3D.z; + fittingData_0.push_back(a_pt3D); + } + meanZ_0 = meanZ_0 / (double)markClusters[mark_O_idx].size(); + SVzNL3DPoint mark0_center; + double mark0_radius; + double fitErr1 = fitCircleByLeastSquare( + fittingData_0, + mark0_center, + mark0_radius); + mark0_center.z = meanZ_0; + + std::vector fittingData_1; + double meanZ_1 = 0; + for (int i = 1; i < markClusters[mark_x_idx].size(); i++) + { + SVzNL2DPoint& a_pt2D = markClusters[mark_x_idx][i]; + SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y]; + meanZ_1 += a_pt3D.z; + fittingData_1.push_back(a_pt3D); + } + meanZ_1 = meanZ_1 / (double)markClusters[mark_x_idx].size(); + SVzNL3DPoint mark1_center; + double mark1_radius; + double fitErr2 = fitCircleByLeastSquare( + fittingData_1, + mark1_center, + mark1_radius); + mark1_center.z = meanZ_1; + + output_markCenter.push_back(mark0_center); + output_markCenter.push_back(mark1_center); + debug_markOrigin.insert(debug_markOrigin.end(), markClusters[mark_O_idx].begin(), markClusters[mark_O_idx].end()); + debug_markXDir.insert(debug_markXDir.end(), markClusters[mark_x_idx].begin(), markClusters[mark_x_idx].end()); + + markCenter = mark0_center; + //ת + SVzNL2DPointD a = { mark1_center.x - mark0_center.x, mark1_center.y - mark0_center.y }; + SVzNL2DPointD b = { 1, 0 }; + double rotAngle = 0; + bool validRotate = calcRotateAngle(a, b, rotAngle); + markRotateAngle = rotAngle; } - //ֱɨӱ +#endif } // @@ -338,36 +674,120 @@ void wd_bagThreadPositioning( } scanLines[lineIdx][ptIdx].nPointIdx = 1; } - } - if (threadTailTreeIdx >= 0) - { - int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size(); - for (int j = 0; j < nodeNum; j++) + //ʾͷ + if (threadTailTreeIdx >= 0) { - int lineIdx, ptIdx; - if (false == scanInfo.isHorizonScan) + int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) { - lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx; - for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++) + int lineIdx, ptIdx; + if (false == scanInfo.isHorizonScan) { - ptIdx = m; - scanLines[lineIdx][ptIdx].nPointIdx = 2; - } - } - else - { - ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx; - for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++) - { - lineIdx = m; - scanLines[lineIdx][ptIdx].nPointIdx = 2; + lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx; + for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++) + { + ptIdx = m; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } + else + { + ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx; + for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++) + { + lineIdx = m; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } } } } + for (int i = 0; i < (int)debug_markOrigin.size(); i++) + { + int lineIdx = debug_markOrigin[i].x; + int ptIdx = debug_markOrigin[i].y; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + } + for (int i = 0; i < (int)debug_markXDir.size(); i++) + { + int lineIdx = debug_markXDir[i].x; + int ptIdx = debug_markXDir[i].y; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } } //ȡ(ȡ4 std::vector stitchROIs; int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size(); + + //ȷ߷ÿɨ߹յλáʹ÷ֶСŻ + double lenFittingScale = 10.0;// + int lenFittingScale_lines = (int)(lenFittingScale / lineInterval); + + int totalLines = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1; + int num_intervals = totalLines / lenFittingScale_lines; //˴ʣಿֲٴΪֻҪȡǰ3ͷ + std::vector> segPoints; + segPoints.resize(num_intervals); + std::vector cornerNodeLineIndice; + cornerNodeLineIndice.resize(totalLines); + std::fill(cornerNodeLineIndice.begin(), cornerNodeLineIndice.end(), -1); + int startLineIdx = cornerGrowTrees[objTreeIdx].sLineIdx; + for (int i = 0; i < nodeSize; i++) + { + SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[i]; + int lineDiff = a_node.jumpPos2D.x - startLineIdx; + cornerNodeLineIndice[lineDiff] = i; + int interval_id = lineDiff / lenFittingScale_lines; + if (interval_id < num_intervals) + segPoints[interval_id].push_back(a_node.jumpPos); + } + //ֱֶ + std::vector baseLinePoints; //߷ÿɨߵĵ + std::vector baseLinePtPos; + for (int i = 0; i < num_intervals; i++) + { + std::vector fittingPoints; + if ((i == 0) || (i == (num_intervals - 1))) + { + fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end()); + } + else + { + //ʹ3 + fittingPoints.insert(fittingPoints.end(), segPoints[i-1].begin(), segPoints[i-1].end()); + fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end()); + fittingPoints.insert(fittingPoints.end(), segPoints[i+1].begin(), segPoints[i+1].end()); + } + //ֲ + double a, b, c; //̣ax+by+c=0; + lineFitting_abc(fittingPoints, &a, &b, &c); + + int segStartLineIdx = i * lenFittingScale_lines + startLineIdx; + for (int j = 0; j < lenFittingScale_lines; j++) + { + int idx = i * lenFittingScale_lines + j; + if (cornerNodeLineIndice[idx] >= 0) + { + int nodeIdx = cornerNodeLineIndice[idx]; + SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx]; + baseLinePtPos.push_back(a_node.jumpPos2D); + baseLinePoints.push_back(a_node.jumpPos); + } + else + { + double dist = DBL_MAX; + SVzNL3DPosition bestPoint = _findClosestPoint(scanLines[segStartLineIdx + j], a, b, c, &dist); + SVzNL2DPoint pos = { segStartLineIdx + j , bestPoint.nPointIdx }; + if (dist > ptInterval * 2) + { + bestPoint.pt3D = { 0, 0, -1 }; + pos.y = -1; + } + baseLinePtPos.push_back(pos); + baseLinePoints.push_back(bestPoint.pt3D); + } + } + + } + //ȡŷΧ int pre_idx = -1; for (int i = 0; i < nodeSize; i++) @@ -383,160 +803,158 @@ void wd_bagThreadPositioning( double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval); if (width > scanInfo.stitchWidth) { - SVzNLRect a_stitch; - memset(&a_stitch, 0, sizeof(SVzNLRect)); - if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x) + //Zֵߵ + int searchStart = pre_node.jumpPos2D.x - startLineIdx; + int searchEnd = a_node.jumpPos2D.x - startLineIdx; + + int mean_Lines = (int)(0.5 / lineInterval) + 1; //ȡ1mmľϵZƽֵڸ + int validMeanLinesTh = mean_Lines - 5; + std::vector validStitch; + std::vector validStitchIndice; + + //ƽ߶Ⱥ͸߶ȼֵ + double sticthPeak = DBL_MAX; + int debug_pkPos = -1; + for (int m = searchStart + 1; m < searchEnd; m++) { - a_stitch.left = a_node.jumpPos2D.x; - a_stitch.right = pre_node.jumpPos2D.x; + if (baseLinePoints[m].z > 1e-4) + { + double sumZ = 0; + int sumZ_counter = 0; + for (int n = m - mean_Lines; n <= m+ mean_Lines; n++) + { + if ((n > searchStart) && (n < searchEnd)) + { + if (baseLinePoints[n].z > 1e-4) + { + sumZ += baseLinePoints[n].z; + sumZ_counter++; + } + } + } + validStitchIndice.push_back(m); + if (sumZ_counter > validMeanLinesTh) + { + double meanZ = sumZ / (double)sumZ_counter; + validStitch.push_back(meanZ); + + if (sticthPeak > meanZ) + { + sticthPeak = meanZ; + debug_pkPos = m; + } + } + else + validStitch.push_back(-1); + } + else + validStitch.push_back(-1); } - else + + // sticthPeak+0.5)Ϊޣȡм + double stitchPeakTh = sticthPeak + 0.5; + int validStart = -1, validEnd = 0; + for (int n = 0; n < (int)validStitch.size(); n++) { - a_stitch.left = pre_node.jumpPos2D.x; - a_stitch.right = a_node.jumpPos2D.x; + if ((validStitch[n] > 1e-4) &&(validStitch[n] < stitchPeakTh)) + { + if (validStart < 0) + validStart = n; + validEnd = n; + } } - if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y) + int refCenterLine = (validStart + validEnd)/2 + searchStart + 1; + SVzNL3DPoint stitchPos = { 0, 0, DBL_MAX }; + int stitchLineIdx = -1; + int searchRng = (validEnd - validStart) / 2; + for (int m = 0; m < searchRng; m++) //ΧҲΪopDist_lines { - a_stitch.top = a_node.jumpPos2D.y; - a_stitch.bottom = pre_node.jumpPos2D.y; + int index_1 = refCenterLine + m; + if (baseLinePoints[index_1].z > 1e-4) + { + stitchLineIdx = index_1; + stitchPos = baseLinePoints[index_1]; + break; + } + int index_2 = refCenterLine - m; + if (baseLinePoints[index_2].z > 1e-4) + { + stitchLineIdx = index_2; + stitchPos = baseLinePoints[index_2]; + break; + } } - else + + //µλ + SVzNL3DPoint operatePos = {0,0, -1}; + if (stitchLineIdx >= 0) { - a_stitch.top = pre_node.jumpPos2D.y; - a_stitch.bottom = a_node.jumpPos2D.y; + int opDist_lines; + if (false == scanInfo.isHorizonScan) //ֱ߷ɨ + opDist_lines = (int)(scanInfo.operateDist / lineInterval); + else + opDist_lines = (int)(scanInfo.operateDist / ptInterval); + + int op_centerLine = stitchLineIdx + opDist_lines; + for (int m = 0; m < opDist_lines; m++) //ΧҲΪopDist_lines + { + int index_1 = op_centerLine + m; + if (index_1 < totalLines) + { + if (cornerNodeLineIndice[index_1] > 0) + { + int nodeIndex = cornerNodeLineIndice[index_1]; + operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos; + break; + } + } + int index_2 = op_centerLine - m; + if (index_2 >= 0) + { + if (cornerNodeLineIndice[index_2] > 0) + { + int nodeIndex = cornerNodeLineIndice[index_2]; + operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos; + break; + } + } + } + if ( (stitchLineIdx >= 0) && (operatePos.z > 1e-4)) + { + SSX_bagThreadInfo a_stitchInfo; + memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo)); + a_stitchInfo.threadPos = stitchPos; + a_stitchInfo.operatePos = operatePos; + a_stitchInfo.rotateAngle = 0; + bagThreadInfo.push_back(a_stitchInfo); + if (bagThreadInfo.size() >= 4) + break; + } } - stitchROIs.push_back(a_stitch); - if (stitchROIs.size() >= 4) - break; } } } pre_idx = nodeIdx; } - //ȡλ - if (stitchROIs.size() == 0) - { + // + if (bagThreadInfo.size() == 0) *errCode = SG_ERR_ZERO_OBJECTS; - return; - } - //node±ɨŵ - std::vector backIndexing; - int indexingSize = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1; - backIndexing.resize(indexingSize); - for (int i = 0; i < indexingSize; i++) - backIndexing[i] = -1; - for (int i = 0; i < nodeSize; i++) + + //Ϊ + double cosTheta = cos(markRotateAngle); + double sinTheta = sin(markRotateAngle); + bagThreadInfo_relative.resize(bagThreadInfo.size()); + for (int i = 0; i < (int)bagThreadInfo.size(); i++) { - int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx; - backIndexing[indexingIdx] = i; + SSX_bagThreadInfo& a_stitch = bagThreadInfo[i]; + SVzNL3DPoint a_pt = { a_stitch.threadPos.x - markCenter.x, a_stitch.threadPos.y - markCenter.y, a_stitch.threadPos.z - markCenter.z }; + SVzNL3DPoint rot_pt = rotateXoY(a_pt, sinTheta, cosTheta); + bagThreadInfo_relative[i].threadPos = rot_pt; + a_pt = { a_stitch.operatePos.x - markCenter.x, a_stitch.operatePos.y - markCenter.y, a_stitch.operatePos.z - markCenter.z }; + rot_pt = rotateXoY(a_pt, sinTheta, cosTheta); + bagThreadInfo_relative[i].operatePos = rot_pt; + bagThreadInfo_relative[i].rotateAngle = 0; } - - int opDist_lines; - if (false == scanInfo.isHorizonScan) //ֱ߷ɨ - opDist_lines = (int)(scanInfo.operateDist / lineInterval); - else - opDist_lines = (int)(scanInfo.operateDist / ptInterval); - - for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++) - { - //Zֵߵ - SVzNLRect& a_stitch = stitchROIs[i]; - SVzNL3DPoint stitchPos = { 0, 0, -1 }; - for (int j = a_stitch.left + 1; j < a_stitch.right; j++) - { - SVzNL3DPoint linePeak = { 0, 0, -1 }; - for (int m = a_stitch.top; m <= a_stitch.bottom; m++) - { - SVzNL3DPoint a_pt; - if (false == scanInfo.isHorizonScan) //ֱ߷ɨ - a_pt = scanLines[j][m].pt3D; - else - a_pt = scanLines[m][j].pt3D; - if (a_pt.z > 1e-4) - { - if (linePeak.z < 0) - linePeak = a_pt; - else - { - if (linePeak.z > a_pt.z) - linePeak = a_pt; - } - } - } - if (linePeak.z > 1e-4) - { - if (stitchPos.z < 0) - stitchPos = linePeak; - else - { - if (stitchPos.z > linePeak.z) - stitchPos = linePeak; - } - } - } - //µλ - if (stitchPos.z > 1e-4) - { - int op_centerLine; - int searchStart, searchEnd; - if (true == scanInfo.scanFromThreadHead) - { - op_centerLine = a_stitch.right + opDist_lines; - searchStart = a_stitch.right; - searchEnd = a_stitch.right + opDist_lines * 2; - if (searchEnd > cornerGrowTrees[objTreeIdx].eLineIdx) - searchEnd = cornerGrowTrees[objTreeIdx].eLineIdx; - } - else - { - op_centerLine = a_stitch.left - opDist_lines; - searchEnd = a_stitch.left; - searchStart = a_stitch.left - opDist_lines * 2; - if (searchStart < cornerGrowTrees[objTreeIdx].sLineIdx) - searchStart = cornerGrowTrees[objTreeIdx].sLineIdx; - } - //Ѱʵ - int best_idx = -1; - int minLineDist = INT_MAX; - for (int j = searchStart; j <= searchEnd; j++) - { - int indexingIdx = j - cornerGrowTrees[objTreeIdx].sLineIdx; - if (backIndexing[indexingIdx] >= 0) - { - int lineDist = j - op_centerLine; - if (lineDist < 0) - lineDist = -lineDist; - if (minLineDist > lineDist) - { - minLineDist = lineDist; - best_idx = backIndexing[indexingIdx]; - } - } - } - if (best_idx >= 0) - { - int op_lineIdx, op_ptIdx; - if (false == scanInfo.isHorizonScan) //ֱ߷ɨ - { - op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x; - op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y; - } - else - { - op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x; - op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y; - } - SSX_bagThreadInfo a_stitchInfo; - memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo)); - a_stitchInfo.threadPos = stitchPos; - a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D; - a_stitchInfo.rotateAngle = 0; - bagThreadInfo.push_back(a_stitchInfo); - } - } - } - - return; } #endif diff --git a/sourceCode/bagThreadPositioning_Export.h b/sourceCode/bagThreadPositioning_Export.h index 0754ccc..9ba4425 100644 --- a/sourceCode/bagThreadPositioning_Export.h +++ b/sourceCode/bagThreadPositioning_Export.h @@ -16,20 +16,39 @@ typedef struct { bool isHorizonScan; bool scanFromThreadHead; + double camInstallAngle; //װʱ̨н double stitchWidth; //Сȣڹٵ double operateDist; //µλþž + double mark_height; //궨߶ + double mark_diameter; //궨⾶ + double mark_distance; //궨ľ }SSX_ScanInfo; //汾 SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void); +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara wd_bagThread_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//̬ƽȥ +SG_APISHARED_EXPORT void wd_bagThread_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + //ͷλüⶨλ SG_APISHARED_EXPORT void wd_bagThreadPositioning( std::vector< std::vector>& scanLines, const SSX_ScanInfo scanInfo, //true:ƽв۵false:ߴֱ۵ + const SSG_planeCalibPara groundCalibPara, const SSG_outlierFilterParam filterParam, //˲ const SSG_cornerParam cornerPara, //V const SSG_raisedFeatureParam raisedFeaturePara,//β͹ const SSG_treeGrowParam growParam, // std::vector& bagThreadInfo, + std::vector& bagThreadInfo_relative, //Mark + std::vector& output_markCenter, //MarkλϢ int* errCode);