diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index c04266c..71dddbf 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -319,9 +319,8 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod( /// SG_APISHARED_EXPORT void wd_getXYVertialFeature_perSeg_dirAngleMethod( std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, int lineIdx, - const double maxDistTh, - const double minSegSize, const SSG_cornerParam cornerPara, std::vector& xyVerticalFlags ); @@ -338,6 +337,16 @@ SG_APISHARED_EXPORT void wd_getXYHorizontalFeature_dirAngleMethod( std::vector& xyHorizontalFlags ); +SG_APISHARED_EXPORT void wd_getRimConvexFeature( + std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, + int lineIdx, + const SWDRimEdgeParam rimPara_1, + const SWDRimEdgeParam rimPara_2, + const SVzNLRangeD rimWidth, + std::vector& rimFeatures +); + /// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。 /// 使用平均点距进行加速 /// nPointIdx被重新定义成Feature类型 @@ -723,6 +732,14 @@ SG_APISHARED_EXPORT double fitCircleByLeastSquare_3( const std::vector& pointArray, SVzNL2DPointD& center, double& radius); +// 加权迭代最小二乘圆拟合（Huber权重或Tukey权重） +SG_APISHARED_EXPORT double fitCircleRobust( + const std::vector& points, + SVzNL3DPoint& robustCenter, + double& robustRadius, + double outlierThreshold = 2.5, // 标准差倍数 + int maxIter = 20, + double minPointsRatio = 0.5); //抛物线最小二乘拟合 y=ax^2 + bx + c SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen( @@ -819,7 +836,7 @@ SG_APISHARED_EXPORT void SG_TwoPassLabel( //计算面参数: z = Ax + By + C //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, SG_APISHARED_EXPORT void vzCaculateLaserPlane( - std::vector Points3ds, + std::vector& Points3ds, std::vector& res); SG_APISHARED_EXPORT Plane robustFitPlane( diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 821cc80..b7d09b1 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -41,6 +41,17 @@ typedef struct double z; }SWD3DPoint; +typedef struct +{ + double cx, cy, r; +}SWD_Circle; + +typedef struct +{ + double rimEdgeAngleTh; + double rimCornerTh; +}SWDRimEdgeParam; + typedef struct { int lineIdx; diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 7ce4a71..cc61ff7 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -5946,9 +5946,8 @@ void wd_getXYVertialFeature_dirAngleMethod( /// void wd_getXYVertialFeature_perSeg_dirAngleMethod( std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, int lineIdx, - const double maxDistTh, - const double minSegSize, const SSG_cornerParam cornerPara, std::vector& xyVerticalFlags ) @@ -5960,14 +5959,7 @@ void wd_getXYVertialFeature_perSeg_dirAngleMethod( xyVerticalFlags.resize(lineData.size()); std::fill(xyVerticalFlags.begin(), xyVerticalFlags.end(), 0); - //根据z连续性分段 - std::vector segs; - wd_lineDataSegment_dist_2( - lineData, - segs, - maxDistTh, - minSegSize - ); + //计算前向角和后向角 std::vector< SSG_pntDirAngle> ptDirAngles; _computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles); @@ -6112,6 +6104,147 @@ void wd_getXYHorizontalFeature_dirAngleMethod( return; } +//提取边沿特征 +void wd_getRimConvexFeature( + std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, + int lineIdx, + const SWDRimEdgeParam rimPara_1, + const SWDRimEdgeParam rimPara_2, + const SVzNLRangeD rimWidth, + std::vector& rimFeatures +) +{ + //双尺度检测，分别用于缓变和剧变（对应step为2和1） + std::vector< SSG_pntDirAngle> ptDirAngles_1; + ptDirAngles_1.resize(lineData.size()); + std::vector< SSG_pntDirAngle> ptDirAngles_2; + ptDirAngles_2.resize(lineData.size()); + for (int i = 0; i < (int)lineData.size(); i++) + { + ptDirAngles_1[i].pntIdx = -1; + ptDirAngles_1[i].type = -1; + ptDirAngles_1[i].forwardPntIdx = 0; + ptDirAngles_1[i].backwardPntIdx = 0; + ptDirAngles_1[i].forwardAngle = 0; + ptDirAngles_1[i].backwardAngle = 0; + ptDirAngles_1[i].corner = 0; + ptDirAngles_1[i].forwardDiffZ = 0; + ptDirAngles_1[i].backwardDiffZ = 0; + + ptDirAngles_2[i].pntIdx = -1; + ptDirAngles_2[i].type = -1; + ptDirAngles_2[i].forwardPntIdx = 0; + ptDirAngles_2[i].backwardPntIdx = 0; + ptDirAngles_2[i].forwardAngle = 0; + ptDirAngles_2[i].backwardAngle = 0; + ptDirAngles_2[i].corner = 0; + ptDirAngles_2[i].forwardDiffZ = 0; + ptDirAngles_2[i].backwardDiffZ = 0; + } + //逐段进行 + int segSize = (int)segs.size(); + for (int segIdx = 0; segIdx < segSize; segIdx++) + { + int vPtIdxStart = segs[segIdx].start; + int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1; + std::vector ptIndice; + for (int i = vPtIdxStart; i < vPtIdxEnd; i++) + { + ptDirAngles_1[i].type = segIdx; + ptDirAngles_2[i].type = segIdx; + if (lineData[i].pt3D.z > 1e-4) + ptIndice.push_back(i); + } + + int vldNum = (int)ptIndice.size(); + for (int idx = 1; idx < vldNum-1; idx++) + { + int i = ptIndice[idx]; + int pre_i = ptIndice[idx-1]; + int post_i = ptIndice[idx+1]; + + //计算拐角 + double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y); + double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + ptDirAngles_1[i].pntIdx = i; + ptDirAngles_1[i].forwardPntIdx = post_i; + ptDirAngles_1[i].backwardPntIdx = pre_i; + ptDirAngles_1[i].forwardAngle = forwardAngle; + ptDirAngles_1[i].backwardAngle = backwardAngle; + ptDirAngles_1[i].corner = (forwardAngle - backwardAngle); + ptDirAngles_1[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z; + ptDirAngles_1[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z; + } + + for (int idx = 2; idx < vldNum - 2; idx++) + { + int i = ptIndice[idx]; + int pre_i = ptIndice[idx - 2]; + int post_i = ptIndice[idx + 2]; + + //计算拐角 + double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y); + double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + ptDirAngles_2[i].pntIdx = i; + ptDirAngles_2[i].forwardPntIdx = post_i; + ptDirAngles_2[i].backwardPntIdx = pre_i; + ptDirAngles_2[i].forwardAngle = forwardAngle; + ptDirAngles_2[i].backwardAngle = backwardAngle; + ptDirAngles_2[i].corner = (forwardAngle - backwardAngle); + ptDirAngles_2[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z; + ptDirAngles_2[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z; + } + + //分段搜索拐点 + int pre_start_pos = -1; + for (int i = vPtIdxStart; i <= vPtIdxEnd; i++) + { + //搜索开始 + if ((i == vPtIdxStart) || + ((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].backwardAngle < -rimPara_1.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_1.rimCornerTh)) || + ((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].backwardAngle < -rimPara_2.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_2.rimCornerTh)) || + ((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].backwardAngle < -rimPara_1.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_1.rimCornerTh)) || + ((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].backwardAngle < -rimPara_2.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_2.rimCornerTh))) + pre_start_pos = i; + + //搜索结束 + if ((i == vPtIdxEnd) || + ((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].forwardAngle > rimPara_1.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_1.rimCornerTh)) || + ((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].forwardAngle > rimPara_2.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_2.rimCornerTh)) || + ((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].forwardAngle > rimPara_1.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_1.rimCornerTh)) || + ((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].forwardAngle > rimPara_2.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_2.rimCornerTh))) + { + if (pre_start_pos >= 0) + { + if ((ptDirAngles_1[pre_start_pos].pntIdx >= 0) || (ptDirAngles_1[i].pntIdx >= 0) || + (ptDirAngles_2[pre_start_pos].pntIdx >= 0) || (ptDirAngles_2[i].pntIdx >= 0)) + { + double width = sqrt(pow(lineData[pre_start_pos].pt3D.x - lineData[i].pt3D.x, 2) + + pow(lineData[pre_start_pos].pt3D.y - lineData[i].pt3D.y, 2) + + pow(lineData[pre_start_pos].pt3D.z - lineData[i].pt3D.z, 2)); + if ((width >= rimWidth.min) && (width <= rimWidth.max)) + { + //一个rim + SSG_RUN a_rim; + a_rim.start = pre_start_pos; + a_rim.len = i - pre_start_pos + 1; + a_rim.curveLen = width; + a_rim.value = segIdx; + rimFeatures.push_back(a_rim); + pre_start_pos = -1; + } + } + } + } + } + } +} + /// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。 /// 使用平均点距进行加速 /// nPointIdx被重新定义成Feature类型 diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index f7f7ac8..ec2b97c 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -319,6 +319,122 @@ double fitCircleByLeastSquare_3( return err; } +// 计算点到圆的几何距离（带符号） +double geometricDistance(const SVzNL3DPosition& p, const SVzNL3DPoint& center, const double R) { + return std::sqrt((p.pt3D.x - center.x) * (p.pt3D.x - center.x) + (p.pt3D.y - center.y) * (p.pt3D.y - center.y)) - R; +} + +// ========== 计算几何误差（点到圆的距离） ========== +double calcGeometricError(const SVzNL3DPosition& p, const SWD_Circle& c) { + double dx = p.pt3D.x - c.cx; + double dy = p.pt3D.y - c.cy; + return fabs(sqrt(dx * dx + dy * dy) - c.r); +} + +// ========== 计算所有点的误差统计 ========== +void calcErrorStats(const std::vector& points, const SWD_Circle& c, + std::vector& errors, double& mean, double& stddev) +{ + int n = points.size(); + errors.resize(n); + mean = 0; + for (int i = 0; i < n; ++i) { + errors[i] = calcGeometricError(points[i], c); + mean += errors[i]; + } + mean /= n; + + stddev = 0; + for (int i = 0; i < n; ++i) { + double d = errors[i] - mean; + stddev += d * d; + } + stddev = sqrt(stddev / n); +} + +// ========== 主函数：迭代剔除离群点 + 最小二乘拟合 ========== +double fitCircleRobust( + const std::vector& points, + SVzNL3DPoint& robustCenter, + double& robustRadius, + double outlierThreshold, // 标准差倍数 + int maxIter, + double minPointsRatio) +{ + if (points.size() < 3) return 0; + + // 用所有点初始化 + std::vector workingPoints = points; + SVzNL3DPoint c0; + double r0; + double errVal = fitCircleByLeastSquare_2(workingPoints, c0, r0); + SWD_Circle bestCircle = { c0.x, c0.y, r0 }; + + double bestMeanError = std::numeric_limits::max(); + std::vector bestPoints = workingPoints; + + for (int iter = 0; iter < maxIter; ++iter) { + // 计算当前拟合圆的误差统计 + std::vector errors; + double mean, stddev; + calcErrorStats(workingPoints, bestCircle, errors, mean, stddev); + + // 检查收敛：如果平均误差不再显著改善 + if (mean >= bestMeanError * 0.99 && iter > 0) + break; + bestMeanError = mean; + + // 如果标准差太小，说明已经收敛 + if (stddev < 1e-8) break; + + // 剔除离群点：误差 > mean + outlierThreshold * stddev + double threshold = mean + outlierThreshold * stddev; + std::vector filteredPoints; + filteredPoints.reserve(workingPoints.size()); + + for (size_t i = 0; i < workingPoints.size(); ++i) { + if (errors[i] <= threshold) { + filteredPoints.push_back(workingPoints[i]); + } + } + + // 如果剩余点太少，停止剔除 + if (filteredPoints.size() < points.size() * minPointsRatio || + filteredPoints.size() < 3) { + break; + } + + // 用剩余点重新拟合 + SVzNL3DPoint center; + double radius; + errVal = fitCircleByLeastSquare_2(filteredPoints, center, radius); + SWD_Circle newCircle = { center.x, center.y, radius }; + + // 检查新圆是否更优（用平均误差衡量） + double newMean = 0; + for (const auto& p : filteredPoints) { + newMean += calcGeometricError(p, newCircle); + } + newMean /= filteredPoints.size(); + + if (newMean < bestMeanError) { + bestCircle = newCircle; + workingPoints = filteredPoints; + bestPoints = filteredPoints; + } + else { + // 新圆不如旧圆好，停止迭代 + break; + } + } + robustCenter.x = bestCircle.cx; + robustCenter.y = bestCircle.cy; + robustCenter.z = 0; + robustRadius = bestCircle.r; + return bestMeanError; +} + + #if 0 bool leastSquareParabolaFit(const std::vector& points, double& a, double& b, double& c, @@ -464,7 +580,7 @@ bool leastSquareParabolaFitEigen( //计算面参数: z = Ax + By + C //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, -void vzCaculateLaserPlane(std::vector Points3ds, std::vector& res) +void vzCaculateLaserPlane(std::vector& Points3ds, std::vector& res) { //最小二乘法拟合平面 //获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反 diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index da06a10..ad5d109 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -5,6 +5,8 @@ #include #include +#define _DEBUG_OUTPUT + //version 1.0.0 : base version release to customer //version 1.0.2 : 娣诲姞浜嗗伐浠跺Э鎬侊紙娆ф媺瑙掕緭鍑) //version 1.1.0 : c瀵瑰伐浠跺Э鎬佽鑼冨寲涓轰腑蹇冪偣锛堟搷浣滅偣锛夊姞涓変釜鏂瑰悜鐭㈤噺 @@ -26,12 +28,118 @@ //version 1.5.3 : 娣诲姞閮戝窞寰姏鐮傝疆鐩樺拰杞洏鏋跺畾浣岮PI銆 //version 1.5.4 : 娣诲姞灞变笢鏈簨鏈虹數杞儙瀹氫綅API銆 //version 1.5.5 : 灞变笢鏈簨鏈虹數杞儙瀹氫綅銆傦紙1锛夋坊鍔犱簡杞儙鍐呭緞澶у皬鐨勮緭鍑猴紝 锛2锛夋寜楂樺害鎺掑簭銆傛渶涓婇潰鐨勭洰鏍囨帓鍦ㄧ涓涓 -std::string m_strVersion = "HolePostion 1.5.5"; +//version 1.5.6 : 灞变笢鏈簨鏈虹數杞儙瀹氫綅:浼樺寲浜嗚疆鑳庡畾浣嶇畻娉曪紝浣跨敤鍦嗘壂鎻忔彁鍙栬疆寤撶偣銆 +std::string m_strVersion = "HolePostion 1.5.6"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); } +#ifdef _DEBUG_OUTPUT +// 浜岀淮 int vector (鐏板害鍍忕礌 0~255) 杞 Mat +cv::Mat vec2Int2VisMat(const std::vector>& data) +{ + if (data.empty()) + return cv::Mat(); + + int rows = data.size(); + int cols = data[0].size(); + cv::Mat mat(rows, cols, CV_32FC1); + + // 濉厖娴偣鏁版嵁 + for (int i = 0; i < rows; i++) + { + float* ptr = mat.ptr(i); + for (int j = 0; j < cols; j++) + { + ptr[j] = (float)data[i][j]; + } + } + + // 褰掍竴鍖栧埌 0~255 鐢ㄤ簬鏄剧ず + cv::Mat vis; + cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1); + return vis; +} +// 浜岀淮 float vector (娣卞害/娴偣鏁版嵁) 杞彲瑙嗗寲鐏板害鍥 +cv::Mat vec2Float2VisMat(const std::vector>& data) +{ + if (data.empty()) + return cv::Mat(); + + int rows = data.size(); + int cols = data[0].size(); + cv::Mat mat(rows, cols, CV_32FC1); + + // 濉厖娴偣鏁版嵁 + for (int i = 0; i < rows; i++) + { + float* ptr = mat.ptr(i); + for (int j = 0; j < cols; j++) + { + ptr[j] = (float)data[i][j]; + } + } + + // 褰掍竴鍖栧埌 0~255 鐢ㄤ簬鏄剧ず + cv::Mat vis; + cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1); + return vis; +} +// 浜岀淮 鏂瑰樊 (娣卞害/娴偣鏁版嵁) 杞彲瑙嗗寲鐏板害鍥 +cv::Mat vec2Var2VisMat(const std::vector>& data) +{ + if (data.empty()) + return cv::Mat(); + + int rows = data.size(); + int cols = data[0].size(); + cv::Mat mat(rows, cols, CV_32FC1); + + // 濉厖娴偣鏁版嵁 + for (int i = 0; i < rows; i++) + { + float* ptr = mat.ptr(i); + for (int j = 0; j < cols; j++) + { + ptr[j] = (float)data[i][j].var; + } + } + + // 褰掍竴鍖栧埌 0~255 鐢ㄤ簬鏄剧ず + cv::Mat vis; + cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1); + return vis; +} +// 浜岀淮 鏂瑰樊 (娣卞害/娴偣鏁版嵁) 杞彲瑙嗗寲鐏板害鍥 +cv::Mat vec2VarAdd2VisMat(const std::vector>& data1, const std::vector>& data2) +{ + if ((data1.empty())|| (data2.empty())) + return cv::Mat(); + + int rows = data1.size(); + int cols = data1[0].size(); + if( (data2.size() != rows) || (data2[0].size() != cols)) + return cv::Mat(); + + cv::Mat mat(rows, cols, CV_32FC1); + // 濉厖娴偣鏁版嵁 + for (int i = 0; i < rows; i++) + { + float* ptr = mat.ptr(i); + for (int j = 0; j < cols; j++) + { + ptr[j] = (float)(data1[i][j].var + data2[i][j].var); + } + } + + // 褰掍竴鍖栧埌 0~255 鐢ㄤ簬鏄剧ず + cv::Mat vis; + cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1); + return vis; +} +#endif + //鐩告満姘村钩瀹夎璁＄畻鍦伴潰璋冨钩鍙傛暟銆 //鐩告満Z杞村熀鏈钩琛屽湴闈㈡椂锛岄渶瑕佷互鍦伴潰涓哄弬鐓э紝灏嗙浉鏈鸿皟姘村钩 //鏃嬭浆鐭╅樀涓鸿皟骞冲弬鏁帮紝鍗冲皢骞抽潰娉曞悜璋冩暣涓哄瀭鐩村悜閲忕殑鍙傛暟 @@ -2699,6 +2807,39 @@ void _getFlagIntervals_h( segs.push_back(a_run); } +void _computeAnlgeScanData( + std::vector< SVzNL3DPosition>& clusterPoints, + const double angleScale, + const SVzNL3DPoint& centerPoint, + std::vector>& polarScanData) +{ + int dataSize = (int)clusterPoints.size(); + for (int i = 0; i < dataSize; i++) + { + int line = clusterPoints[i].nPointIdx >> 16; + int ptIdx = clusterPoints[i].nPointIdx & 0x0000FFFF; + SVzNL3DPoint& a_pt = clusterPoints[i].pt3D; + + double angle = atan2(a_pt.y - centerPoint.y, a_pt.x - centerPoint.x); + angle = (angle / PI) * 180 + 180.0; + double R = sqrt(pow(a_pt.y - centerPoint.y, 2) + pow(a_pt.x - centerPoint.x, 2)); + + int angleLine = (int)(angle / angleScale); + + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = line; + a_polarPt.ptIdx = ptIdx; + a_polarPt.x = a_pt.x; + a_polarPt.y = a_pt.y; + a_polarPt.z = a_pt.z; + a_polarPt.R = R; + a_polarPt.angle = angle; + polarScanData[angleLine].push_back(a_polarPt); + } +} +bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.R < b.R; +} bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b) { return a.center.z < b.center.z; @@ -2973,6 +3114,91 @@ bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b) } } //鎻愬彇杞粨鐐 + +#if 1 + //鎸夌収鍦嗘壂鎻忚幏鍙栬疆寤撶偣 + //璁＄畻璐ㄥ績 + for (int m = 0; m < (int)validObjIndice.size(); m++) + { + int objIdx = validObjIndice[m]; + SVzNL3DPoint centroid = { 0, 0, 0 }; + int cluster_size = (int)objClusters[objIdx].size(); + for (int i = 0; i < cluster_size; i++) + { + int line = objClusters[objIdx][i].nPointIdx >> 16; + int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF; + centroid.x += scanLinesInput[line][ptIdx].pt3D.x; + centroid.y += scanLinesInput[line][ptIdx].pt3D.y; + centroid.z += scanLinesInput[line][ptIdx].pt3D.z; + } + centroid.x = centroid.x / cluster_size; + centroid.y = centroid.y / cluster_size; + centroid.z = centroid.z / cluster_size; + + //鍦嗗懆鎵弿:0.25搴﹂棿闅 + double angleScale = 1.0; + std::vector> polarScanData; + int polarLines = (int)(360.0 / angleScale + 0.5); + polarScanData.resize(polarLines); + _computeAnlgeScanData( + objClusters[objIdx], + angleScale, + centroid, + polarScanData); + + //姣忎釜鎵弿瑙掑害鎸塕浠庡皬鍒板ぇ鎺掑簭 + for (int i = 0; i < polarLines; i++) + { + std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR); + } + + //鎻愬彇杞粨 + std::vector< SWD_polarPt> contourPoints; + std::vector< SVzNL3DPosition> fittingPoints; + for (int i = 0; i < polarLines; i++) + { + if (polarScanData[i].size() == 0) + continue; + + contourPoints.push_back(polarScanData[i][0]); + + SVzNL3DPosition a_pt; + a_pt.nPointIdx = 0; + a_pt.pt3D = { polarScanData[i][0].x, polarScanData[i][0].y, polarScanData[i][0].z}; + fittingPoints.push_back(a_pt); + } + + //鏍囪 + for (int m = 0; m < (int)contourPoints.size(); m++) + { + int line = contourPoints[m].lineIdx; + int ptIdx = contourPoints[m].ptIdx; + scanLinesInput[line][ptIdx].nPointIdx = 3; + } + + //鎻愬彇涓績鐐 + SVzNLRangeD holezRange; + double meanZ = _getListMeanZ(fittingPoints, holezRange); + //鎷熷悎鍦嗗績 + //鍦嗘渶灏忎簩涔樻嫙鍚 + SVzNL3DPoint center = { 0.0, 0.0, 0.0 }; + double radius = 0; +#if 0 + double errValue = fitCircleByLeastSquare_2(fittingPoints, center, radius); +#else + double errValue = fitCircleRobust(fittingPoints, center, radius); +#endif + if ((radius > tireParam.diameter * 0.15) && (radius < tireParam.diameter * 0.75)) + { + WD_HolePositionInfo a_holeInfo; + a_holeInfo.center = { center.x, center.y, meanZ }; + a_holeInfo.normDir = { 0.0, 0.0, 1.0 }; + a_holeInfo.holeR = radius; + tirePositions.push_back(a_holeInfo); + } + } + +#else int maxSkipping = 3; //闂撮殧灏忎簬3鐨勭┖鐧借涓鸿繛缁紝娑堥櫎鎵弿鎷夋潯瀛愮殑褰卞搷 std::vector> contourMasks; contourMasks.resize(lineNum); @@ -3103,6 +3329,7 @@ bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b) } } } +#endif //鎸夐珮搴︽帓搴 std::sort(tirePositions.begin(), tirePositions.end(), _compareByTireZValue); @@ -3152,6 +3379,48 @@ void _templateChecking( return; } +bool validateVarPeak(std::vector>& varPeaks, SSG_2DValueI& seedPeak, const int searchRng) +{ + int startX = seedPeak.x - searchRng; + if (startX < 0) + startX = 0; + int endX = seedPeak.x + searchRng; + if (endX >= (int)varPeaks.size()) + endX = (int)varPeaks.size() - 1; + + int startY = seedPeak.y - searchRng; + int endY = seedPeak.y + searchRng; + bool isPeak = true; + for (int x = startX; x <= endX; x++) + { + int linePkSize = (int)varPeaks[x].size(); + if (linePkSize == 0) + continue; + + for (int m = 0; m < linePkSize; m++) + { + if (varPeaks[x][m].y < startY) + continue; + if (varPeaks[x][m].y > endY) + break; + + if ((varPeaks[x][m].x == seedPeak.x) && (varPeaks[x][m].y == seedPeak.y)) + continue; + + //姣旇緝 + if (seedPeak.valueD > varPeaks[x][m].valueD) + { + isPeak = false; + break; + } + else if (seedPeak.valueD < varPeaks[x][m].valueD) + varPeaks[x][m].value = 1; + } + if (false == isPeak) + break; + } + return isPeak; +} //瀹佹尝娴风憺椹畾瀛愯姱涓績瀹氫綅 void sx_getRotorCorePose( @@ -3223,13 +3492,19 @@ void sx_getRotorCorePose( //4銆佹彁鍙栧瓟 //5銆佹嫙鍚 //6銆佽绠椾腑闂村潗鏍 + //鍐呴儴鍙傛暟 - double maxDistTh = 5.0; + double maxDistTh = 10.0; double minSegLen = 2.0; SSG_cornerParam removeVertialPara = cornerPara; removeVertialPara.scale = 5.0; removeVertialPara.cornerTh = 60; + SWDRimEdgeParam rimPara_1 = {60, 25}; + SWDRimEdgeParam rimPara_2 = { 60, 25 }; + + SVzNLRangeD rimWidth = { 1,20 }; + std::vector> flags; flags.resize(lineNum); for (int i = 0; i < lineNum; i++) @@ -3238,55 +3513,120 @@ void sx_getRotorCorePose( std::fill(flags[i].begin(), flags[i].end(), 0); } std::vector> zVertivalFlags; + std::vector> rimFeatures; for (int line = 0; line < lineNum; line++) { - if (line == 700) + if (line == 578) int kkk = 1; + + //鏍规嵁z杩炵画鎬у垎娈 + std::vector segs; + wd_lineDataSegment_dist_2( + scanLines[line], + segs, + maxDistTh, + minSegLen + ); + std::vector line_verticalFlags; wd_getXYVertialFeature_perSeg_dirAngleMethod( scanLines[line], + segs, line, - maxDistTh, - minSegLen, removeVertialPara, line_verticalFlags ); zVertivalFlags.push_back(line_verticalFlags); - + //鎻愬彇杈规部鐗瑰緛 + std::vector line_rimFeatures; + wd_getRimConvexFeature( + scanLines[line], + segs, + line, + rimPara_1, + rimPara_2, + rimWidth, + line_rimFeatures + ); + rimFeatures.push_back(line_rimFeatures); + for (int i = 0; i < (int)line_verticalFlags.size(); i++) { if (line_verticalFlags[i] > 0) flags[line][i] = 1; } + for (int i = 0; i < (int)line_rimFeatures.size(); i++) + { + SSG_RUN& a_rim = line_rimFeatures[i]; + for (int j = 0; j < a_rim.len; j++) + { + flags[line][j+a_rim.start] = 2; + } + } } std::vector> zVertivalFlags_h; + std::vector> rimFeatures_h; for (int line = 0; line < linePtNum; line++) { if (line == 1177) int kkk = 1; + + //鏍规嵁z杩炵画鎬у垎娈 + std::vector segs; + wd_lineDataSegment_dist_2( + scanLines_h[line], + segs, + maxDistTh, + minSegLen + ); + std::vector line_verticalFlags; wd_getXYVertialFeature_perSeg_dirAngleMethod( - scanLines[line], + scanLines_h[line], + segs, line, - maxDistTh, - minSegLen, removeVertialPara, line_verticalFlags ); zVertivalFlags_h.push_back(line_verticalFlags); + //鎻愬彇杈规部鐗瑰緛 + std::vector line_rimFeatures; + wd_getRimConvexFeature( + scanLines_h[line], + segs, + line, + rimPara_1, + rimPara_2, + rimWidth, + line_rimFeatures + ); + rimFeatures_h.push_back(line_rimFeatures); + for (int i = 0; i < (int)line_verticalFlags.size(); i++) { if (line_verticalFlags[i] > 0) flags[i][line] = 1; } + for (int i = 0; i < (int)line_rimFeatures.size(); i++) + { + SSG_RUN& a_rim = line_rimFeatures[i]; + for (int j = 0; j < a_rim.len; j++) + { + if (flags[j+a_rim.start][line] == 2) + flags[j + a_rim.start][line] = 4; + else + flags[j + a_rim.start][line] = 3; + } + } } + //鏍囨敞 for (int line = 0; line < lineNum; line++) { for (int j = 0; j < linePtNum; j++) { - if (flags[line][j] > 0) + if (flags[line][j] == 1) { scanLines[line][j].pt3D.z = 0; scanLines_h[j][line].pt3D.z = 0; @@ -3349,11 +3689,13 @@ void sx_getRotorCorePose( { for (int j = 0; j < linePtNum; j++) { - if (flags[line][j] > 0) + if (flags[line][j] == 1) { scanLinesInput[line][j].pt3D.z = 0; scanLines[line][j].pt3D.z = 0; } + else if (flags[line][j] > 0) + scanLinesInput[line][j].nPointIdx = flags[line][j]; if (scanLines[line][j].pt3D.z > 1e-4) { @@ -3464,9 +3806,15 @@ void sx_getRotorCorePose( } } } +#ifdef _DEBUG_OUTPUT + //杈撳嚭鍥惧儚 + cv::Mat vis = vec2Float2VisMat(quantiData); + cv::imwrite("quanti_visual.png", vis); +#endif + //鐢熸垚妯℃澘 - double template_minR = rotorParam.rotorDiameter - 2.0; - double template_maxR = rotorParam.rotorDiameter + 2.0; + double template_minR = rotorParam.rotorDiameter/2.0 - 1.0; + double template_maxR = rotorParam.rotorDiameter/2.0 + 1.0; double template_centerR = 10.0; int template_pixR = (int)(template_maxR / quantiScale + 1); int template_W = template_pixR * 2 + 1; @@ -3495,7 +3843,11 @@ void sx_getRotorCorePose( *errCode = SG_ERR_ZERO_OBJECTS; return; } - +#ifdef _DEBUG_OUTPUT + //杈撳嚭鍥惧儚 + cv::Mat vis_template = vec2Int2VisMat(templateMask); + cv::imwrite("template_visual.png", vis_template); +#endif //鎼滅储鐩爣 std::vector> statsCircle; std::vector> statsCenter; @@ -3511,10 +3863,13 @@ void sx_getRotorCorePose( int searchEnd_X = quantiW - 1 - template_pixR; int searchStart_Y = template_pixR; int searchEnd_Y = quantiH - 1 - template_pixR; +#if 0 for (int x = searchStart_X; x < searchEnd_X; x++) { for (int y = searchStart_Y; y < searchEnd_Y; y++) { + if ((x == 373) && (y == 72)) + int kkk = 1; std::vector circleZ; std::vector centerZ; //妯℃澘妫鏌 @@ -3534,18 +3889,129 @@ void sx_getRotorCorePose( statsCenter[x][y] = satasInfo_center; } } +#endif +#ifdef _DEBUG_OUTPUT + //杈撳嚭鍥惧儚 + cv::Mat vis_circle = vec2Var2VisMat(statsCircle); + cv::imwrite("var_circle_visual.png", vis_circle); + cv::Mat vis_center = vec2Var2VisMat(statsCenter); + cv::imwrite("var_center_visual.png", vis_center); + cv::Mat vis_circleCenter = vec2VarAdd2VisMat(statsCircle, statsCenter); + cv::imwrite("var_center_circle_visual.png", vis_circleCenter); +#endif //鎼滅储鏂瑰樊鏋佸皬鍊肩偣, circle鍜宑enter鐨勬柟宸疮鍔犲悗鐨勬瀬灏忓 + double rotorH_th = rotorParam.rotorH * 0.8; //涓ゆ鎼滅储娉曪細鍏堟悳绱5x5鐨勶紝鍐嶆悳绱㈡ā鍧楀ぇ灏 - std::vector< SSG_2DValueI> varPeaks_5x5; - for (int i = 0; i < template_W; i++) + std::vector> varPeaks_5x5; //鎸夎瀛樺偍 + varPeaks_5x5.resize(quantiW); + const SVzNL2DPoint searchIndice[24] = { + {-2,-2}, {-1, -2}, {0, -2}, { 1, -2 }, {2, -2 }, + {-2,-1}, {-1, -1}, {0, -1}, { 1, -1 }, {2, -1 }, + {-2, 0}, {-1, 0}, {1, 0}, {2, 0 }, + {-2, 1}, {-1, 1}, {0, 1}, { 1, 1 }, {2, 1 }, + {-2, 2}, {-1, 2}, {0, 2}, { 1, 2 }, {2, 2 } + }; + //浣跨敤涓涓緟鍔〧lag鏉ュ姞閫 + std::vector> peakFalseflags; + peakFalseflags.resize(quantiW); + for (int i = 0; i < quantiW; i++) + peakFalseflags[i].resize(quantiH); + for (int x = searchStart_X; x < searchEnd_X; x++) { - for (int j = 0; j < template_W; j++) + for (int y = searchStart_Y; y < searchEnd_Y; y++) { + if ((x == 373) && (y == 70)) + int kkk = 1; + SSG_meanVar& curr_circle = statsCircle[x][y]; + SSG_meanVar& curr_center = statsCenter[x][y]; + if (peakFalseflags[x][y] > 0) + continue; + if ((curr_center.mean - curr_circle.mean) > rotorH_th) + { + bool isPeak = true; + for (int m = 0; m < 24; m++) + { + const SVzNL2DPoint& an_idx = searchIndice[m]; + SSG_meanVar& chk_circle = statsCircle[x + an_idx.x][y+an_idx.y]; + if (curr_circle.var > chk_circle.var) + { + isPeak = false; + break; + } + } + if (true == isPeak) + { + for (int m = 0; m < 24; m++) + { + const SVzNL2DPoint& an_idx = searchIndice[m]; + peakFalseflags[x + an_idx.x][y + an_idx.y] = 1; + } + SSG_2DValueI a_peak; + a_peak.x = x; + a_peak.y = y; + a_peak.value = 0; + a_peak.valueD = curr_circle.var; + a_peak.sideID = 0; //1-T, 2-B, 3-L, 4-R + varPeaks_5x5[x].push_back(a_peak); + } + } } } + //浜屾鎼滅储 + std::vector< SSG_2DValueI> objVarPeaks; + int searchRng = (int)( rotorParam.rotorDiameter / quantiScale + 0.5 ); + for (int x = searchStart_X; x < searchEnd_X; x++) + { + int pkSize = (int)varPeaks_5x5[x].size(); + if (pkSize > 0) + { + for (int m = 0; m < pkSize; m++) + { + SSG_2DValueI& a_peak = varPeaks_5x5[x][m]; + if (a_peak.value == 1) + continue; + bool isPeak = validateVarPeak(varPeaks_5x5, a_peak, searchRng); + if (true == isPeak) + { + objVarPeaks.push_back(varPeaks_5x5[x][m]); + } + } + } + } +#ifdef _DEBUG_OUTPUT + //杈撳嚭鍥惧儚 + cv::Mat vis_gray = vec2Float2VisMat(quantiData); + cv::Mat vis_color; + cv::cvtColor(vis_gray, vis_color, cv::COLOR_GRAY2BGR); + int radius = 3; + cv::Scalar color_all(0, 255, 0); // 棰滆壊 (BGR椤哄簭锛氳摑鑹, 缁胯壊, 绾㈣壊) + for (int x = searchStart_X; x < searchEnd_X; x++) + { + int pkSize = (int)varPeaks_5x5[x].size(); + if (pkSize > 0) + { + for (int m = 0; m < pkSize; m++) + { + int px = varPeaks_5x5[x][m].x; + int py = varPeaks_5x5[x][m].y; + cv::circle(vis_color, cv::Point(py, px), radius, color_all, -1); + } + } + } + cv::Scalar color(0, 0, 255); // 棰滆壊 (BGR椤哄簭锛氳摑鑹, 缁胯壊, 绾㈣壊) + for (int i = 0; i < (int)objVarPeaks.size(); i++) + { + int px = objVarPeaks[i].x; + int py = objVarPeaks[i].y; + // thickness = -1 琛ㄧず瀹炲績 + cv::circle(vis_color, cv::Point(py, px), radius, color, -1); + } + + cv::imwrite("quanti_obj_color.png", vis_color); +#endif //鏃嬭浆鍥炲師鍧愭爣绯 @@ -3556,4 +4022,467 @@ void sx_getRotorCorePose( } return; -} \ No newline at end of file +} + +//鍖椾含鐜栫憺宸ヤ欢瀹氫綅 +void Jiurui_getWorkpiecePose( + std::vector< std::vector>& scanLinesInput, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara groundCalibPara, + const WD_JiuruiWorkpieceParam workpieceParam, + std::vector& workpiecePositions, + int* errCode) +{ + *errCode = 0; + + //鐢熸垚鏁版嵁鍓湰锛屼娇鐢ㄥ壇鏈暟鎹繘琛岃皟骞冲拰鍚庣画澶勭悊 + int lineNum = (int)scanLinesInput.size(); + std::vector< std::vector> scanLines; + scanLines.resize(lineNum); + int linePtNum = (int)scanLinesInput[0].size(); + bool isGridData = true; + for (int i = 0; i < lineNum; i++) + { + if (linePtNum != (int)scanLinesInput[i].size()) + isGridData = false; + + scanLines[i].resize(scanLinesInput[i].size()); + std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 浣跨敤std::copy绠楁硶 + + for (int j = 0; j < (int)scanLinesInput[i].size(); j++) + scanLinesInput[i][j].nPointIdx = 0; //娓呴浂锛岀敤浜巇ebug鏃惰褰曚俊鎭 + } + if (false == isGridData)//鏁版嵁涓嶆槸缃戞牸鏍煎紡 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + double removeGroundHeight = -1; + for (int i = 0; i < lineNum; i++) + { //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight); + } + + //浜х敓姘村钩鎵弿鏁版嵁 + std::vector< std::vector> scanLines_h; + scanLines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + scanLines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //灏嗗師濮嬫暟鎹殑搴忓垪娓0锛堜細杞箟浣跨敤锛 + scanLines_h[j][line] = scanLines[line][j]; + scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int line = 0; line < linePtNum; line++) + { + for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++) + scanLines_h[line][j].nPointIdx = j; + } + + //绠楁硶娴佺▼锛 + //1銆佹鏌ュ瀭鐩存柟鍚戞暟鎹苟鍘婚櫎 + //2銆佽仛绫 + //3銆佷繚鐣欐渶鍓嶉潰鐩爣 + //4銆佹彁鍙栧瓟 + //5銆佹嫙鍚 + //6銆佽绠椾腑闂村潗鏍 + //鍐呴儴鍙傛暟 + SSG_cornerParam removeVertialPara = cornerPara; + removeVertialPara.scale = 10.0; + removeVertialPara.cornerTh = 60; + + std::vector> flags; + flags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + flags[i].resize(linePtNum); + std::fill(flags[i].begin(), flags[i].end(), 0); + } + std::vector> zVertivalFlags; + for (int line = 0; line < lineNum; line++) + { + if (line == 700) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[line][i] = 1; + } + } + + std::vector> zVertivalFlags_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 1177) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines_h[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags_h.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[i][line] = 1; + } + } + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines[line][j].pt3D.z = 0; + scanLines_h[j][line].pt3D.z = 0; + } + } + } + //杩唬涓娆 + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 5.0; + lineSegPara.segGapTh_y = 5.0; + lineSegPara.segGapTh_z = 5.0; + const int minSegLen = 5; + for (int line = 0; line < lineNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[line][idx0 + j] = 1; + } + } + } + for (int line = 0; line < linePtNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_h[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[idx0 + j][line] = 1; + } + } + } + + //鏍囨敞 + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLinesInput[line][j].nPointIdx = 0; //灏嗗師濮嬫暟鎹殑搴忓垪娓0锛堜細杞箟浣跨敤锛 + scanLines[line][j].nPointIdx = 0; + } + } + //灏嗗瀭鐩寸嚎娈靛幓闄 + std::vector< SVzNL3DPosition> validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + scanLines[line][j].pt3D.z = 0; + + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines[line][j].pt3D; + a_vldPt.nPointIdx = (line << 16) | (j & 0xffff); + validPoints.push_back(a_vldPt); + } + } + } + + //鑱氱被 + int clusterCheckWin = 5; + double clusterDist = 5.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; //result + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, //鎼滅储绐楀彛 + clusterDist, + distType, + objClusters //result + ); + + //淇濈暀鏈鍓嶉潰鐨勫ぇ灏忓悎閫傜殑鐩爣 + double minObjSize = workpieceParam.width * 0.8; + double maxObjSize = workpieceParam.len * 1.5; + const double topPlateMaxZRange = 100.0; + std::vector objMeanZ; + std::vector objROIs; + std::vector objZRange; + objMeanZ.resize(objClusters.size()); + objROIs.resize(objClusters.size()); + objZRange.resize(objClusters.size()); + std::vector validObjIndice; + int topClusterId = -1; + for (int i = 0; i < (int)objClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getListROI(objClusters[i]); + objROIs[i] = a_roi; + double w = a_roi.right - a_roi.left; + double h = a_roi.bottom - a_roi.top; + if ((w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize)) + { + SVzNLRangeD zRange; + double meanZ = _getListMeanZ(objClusters[i], zRange); + objMeanZ[i] = meanZ; + objZRange[i] = zRange; + validObjIndice.push_back(i); + + if (topClusterId < 0) + topClusterId = i; + else if (objMeanZ[topClusterId] > meanZ) + topClusterId = i; + } + else + { + objMeanZ[i] = 0; + objZRange[i].max = -1.0; + objZRange[i].min = 0.0; + } + } + + if ( (validObjIndice.size() == 0) || (topClusterId < 0)) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return; + } + + //鏍囨敞 + //閲嶆柊灏唂lags璁剧疆涓虹洰鏍囩殑mask + for (int i = 0; i < lineNum; i++) + std::fill(flags[i].begin(), flags[i].end(), -1); + + for (int i = 0; i < (int)objClusters[topClusterId].size(); i++) + { + int line = objClusters[topClusterId][i].nPointIdx >> 16; + int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF; + scanLinesInput[line][ptIdx].nPointIdx = 2; + flags[line][ptIdx] = 1; //indexing + } + + //璁＄畻璐ㄥ績 + SVzNL3DPoint centroid = { 0, 0, 0 }; + int centroid_size = (int)objClusters[topClusterId].size(); + for (int i = 0; i < centroid_size; i++) + { + int line = objClusters[topClusterId][i].nPointIdx >> 16; + int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF; + centroid.x += scanLinesInput[line][ptIdx].pt3D.x; + centroid.y += scanLinesInput[line][ptIdx].pt3D.y; + centroid.z += scanLinesInput[line][ptIdx].pt3D.z; + } + centroid.x = centroid.x / centroid_size; + centroid.y = centroid.y / centroid_size; + centroid.z = centroid.z / centroid_size; + + //鍦嗗懆鎵弿:0.5搴﹂棿闅 + double angleScale = 0.5; + std::vector> polarScanData; + int polarLines = (int)(360.0 / angleScale + 0.5); + polarScanData.resize(polarLines); + _computeAnlgeScanData( + objClusters[topClusterId], + angleScale, + centroid, + polarScanData); + + //姣忎釜鎵弿瑙掑害鎸塕浠庡皬鍒板ぇ鎺掑簭 + for (int i = 0; i < polarLines; i++) + { + std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR); + } + + //鎻愬彇骞抽潰鐐-璁＄畻娉曞悜 + double distToPlane_min = 17.5; + double distToPlane_max = 18.5; + std::vector planePolarPoints; + std::vector Points3ds; + for (int i = 0; i < polarLines; i++) + { + int lineSize = (int)polarScanData[i].size(); + if (lineSize == 0) + continue; + + int minR = polarScanData[i][0].R; + for (int j = 0; j < lineSize; j++) + { + double dist = polarScanData[i][j].R - minR; + if ((dist >= distToPlane_min) && (dist <= distToPlane_max)) + { + planePolarPoints.push_back(polarScanData[i][j]); + cv::Point3d a_pt = cv::Point3d(polarScanData[i][j].x, polarScanData[i][j].y, polarScanData[i][j].z); + Points3ds.push_back(a_pt); + } + } + } + //鏍囨敞 + for (int i = 0; i < (int)planePolarPoints.size(); i++) + { + int line = planePolarPoints[i].lineIdx; + int ptIdx = planePolarPoints[i].ptIdx; + scanLinesInput[line][ptIdx].nPointIdx = 3; + } + //鎷熷悎 + //璁＄畻闈㈠弬鏁: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(Points3ds, res); + + //璁＄畻鎶曞奖鍚戦噺 + SVzNL3DPoint vec_1 = { res[0], res[1], res[2]}; + SVzNL3DPoint vec_z = { 0, 0, 1.0 }; + SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_z); + + //鎶曞奖 + std::vector projectPosition3ds; + for (int i = 0; i < (int)objClusters[topClusterId].size(); i++) + { + SVzNL3DPosition a_vldPt; + a_vldPt.nPointIdx = objClusters[topClusterId][i].nPointIdx; + a_vldPt.pt3D = wd_ptRotate(objClusters[topClusterId][i].pt3D, poseR.planeCalib); + projectPosition3ds.push_back(a_vldPt); + } + + // + //杩唬锛氬渾鍛ㄦ壂鎻-鎻愬彇杞粨-璁＄畻涓績鐐瑰拰杞村悜 + // + //杩唬璁＄畻璐ㄥ績 + centroid = { 0, 0, 0 }; + for (int i = 0; i < centroid_size; i++) + { + int line = objClusters[topClusterId][i].nPointIdx >> 16; + int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF; + centroid.x += scanLinesInput[line][ptIdx].pt3D.x; + centroid.y += scanLinesInput[line][ptIdx].pt3D.y; + centroid.z += scanLinesInput[line][ptIdx].pt3D.z; + } + centroid.x = centroid.x / centroid_size; + centroid.y = centroid.y / centroid_size; + centroid.z = centroid.z / centroid_size; + + //鍦嗗懆鎵弿:0.5搴﹂棿闅 + polarScanData.clear(); + polarScanData.resize(polarLines); + _computeAnlgeScanData( + projectPosition3ds, + angleScale, + centroid, + polarScanData); + //姣忎釜鎵弿瑙掑害鎸塕浠庡皬鍒板ぇ鎺掑簭 + for (int i = 0; i < polarLines; i++) + { + std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR); + } + + //鍙栬疆寤撶偣璁＄畻涓績 + SVzNL3DPoint objCenter = { 0, 0, 0 }; + int centerSum = 0; + for (int i = 0; i < polarLines; i++) + { + int lineSize = (int)polarScanData[i].size(); + if (lineSize == 0) + continue; + + objCenter.x += polarScanData[i][0].x; + objCenter.y += polarScanData[i][0].y; + objCenter.z += polarScanData[i][0].z; + centerSum++; + } + objCenter.x = objCenter.x / centerSum; + objCenter.y = objCenter.y / centerSum; + objCenter.z = objCenter.z / centerSum; + + //璁＄畻闀胯酱鏂瑰悜 + int degree180 = (int)(180.0 / angleScale); + int longAxisIndex = -1; + double longAxisLen = -1; + for (int i = 0; i < polarLines/2; i++) + { + int idx_1 = i; + int idx_2 = i + degree180; + if (idx_2 < polarLines) + { + if ((polarScanData[idx_1].size() == 0) || (polarScanData[idx_2].size() == 0)) + { + double sumR = polarScanData[idx_1][0].R + polarScanData[idx_2][0].R; + if (longAxisIndex < 0) + { + longAxisIndex = idx_1; + longAxisLen = sumR; + } + else + { + if (longAxisLen < sumR) + { + longAxisIndex = idx_1; + longAxisLen = sumR; + } + } + } + } + } + if(longAxisIndex < 0) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return; + } + + double longAxisAngle = longAxisIndex * angleScale; + SVzNL3DPoint vector_y = {cos(longAxisAngle/PI), sin(longAxisAngle / PI), 0 }; + //鍙変箻鍑簐ector_x + SVzNL3DPoint vector_x; + vector_x.x = vector_y.y * vec_z.z - vec_z.y * vector_y.z; + vector_x.y = vector_y.z * vec_z.x - vec_z.z * vector_y.x; + vector_x.z = vector_y.x * vec_z.y - vec_z.x * vector_y.y; + //鐢熸垚缁撴灉 + SSG_pointPose a_pose; + a_pose.point = _ptRotate(objCenter, poseR.invRMatrix); + a_pose.pose_x = _ptRotate(vector_x, poseR.invRMatrix); + a_pose.pose_y = _ptRotate(vector_y, poseR.invRMatrix); + a_pose.pose_z = _ptRotate(vec_z, poseR.invRMatrix); + + //鏃嬭浆鍥炲師鍧愭爣绯 + a_pose.point = _ptRotate(a_pose.point, groundCalibPara.invRMatrix); + a_pose.pose_x = _ptRotate(a_pose.pose_x, groundCalibPara.invRMatrix); + a_pose.pose_y = _ptRotate(a_pose.pose_y, groundCalibPara.invRMatrix); + a_pose.pose_z = _ptRotate(a_pose.pose_z, groundCalibPara.invRMatrix); + workpiecePositions.push_back(a_pose); + return; +} diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index e59a95d..e24b67d 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -33,6 +33,13 @@ typedef struct double thickness; //轮胎宽度 }WD_tireParam; +typedef struct +{ + double len; //工件长 + double width; //工件宽度 + double thickness; +}WD_JiuruiWorkpieceParam; + typedef struct { SVzNL3DPoint center; @@ -43,6 +50,7 @@ typedef struct typedef struct { double rotorDiameter; + double rotorH; double removeGroundHOffset; //去除时的高度偏置，用于设置架子高度 double basket_L; //零件筐-长 double basket_W; //零件筐-宽 @@ -111,4 +119,13 @@ SG_APISHARED_EXPORT void sx_getRotorCorePose( const SSG_planeCalibPara groundCalibPara, const WD_rotorAppParam rotorParam, std::vector& rotorPositions, + int* errCode); + +//北京玖瑞工件定位 +SG_APISHARED_EXPORT void Jiurui_getWorkpiecePose( + std::vector< std::vector>& scanLinesInput, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara groundCalibPara, + const WD_JiuruiWorkpieceParam workpieceParam, + std::vector& tirePositions, int* errCode); \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 0b5c143..ab90ae8 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -635,11 +635,22 @@ void _outputRGBDResult_HoleInfo( if (linePtNum == 0) continue; + std::vector< SVzNL3DPosition> vlPts; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vlPts.push_back(scanLines[line][i]); + } + + linePtNum = (int)vlPts.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]; + SVzNL3DPosition* pt3D = &vlPts[i]; if (pt3D->nPointIdx > 0) int kkk = 1; int flag = pt3D->nPointIdx & 0xffff; @@ -1306,7 +1317,7 @@ void Benshi_tirePosition_test(void) }; SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = { - {1,19}, + {1,34}, }; const char* ver = wd_workpieceHolePositioningVersion(); @@ -1394,7 +1405,7 @@ void Benshi_tirePosition_test(void) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =17; + //fidx =18; char _scan_file[256]; sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[grp], fidx); std::vector> scanLines; @@ -1439,18 +1450,7 @@ void Benshi_tirePosition_test(void) #endif } - -typedef enum -{ - keSG_鎷撴櫘鍙戝瓟瀹氫綅 = 0, - keSG_鍗庤埅瀛斿畾浣, - keSG_寰姏鐮傝疆鐩樺畾浣, - keSG_寰姏鐮傝疆鏋跺瓙瀹氫綅, - keSG_灞变笢鏈簨杞儙瀹氫綅, - keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅, -} ESG_testMode; - -//灞变笢鏈簨鏈虹數杞儙瀹氫綅 +//娴风憺椹 #define HAIRUIMA_COMPUTE_CALIB_PARA 0 #define HAIRUIMA_HOLE_POSITION 1 @@ -1572,7 +1572,8 @@ void HaiRuiMa_rotorCorePosition_test(void) cornerParam.jumpCornerTh_2 = 60; WD_rotorAppParam rotorParam; - rotorParam.rotorDiameter = 76; + rotorParam.rotorDiameter = 72; + rotorParam.rotorH = 40.0; rotorParam.removeGroundHOffset = 100.0; //鍘婚櫎鏃剁殑楂樺害鍋忕疆锛岀敤浜庤缃灦瀛愰珮搴 rotorParam.basket_L = 770; rotorParam.basket_W = 580; @@ -1599,7 +1600,166 @@ void HaiRuiMa_rotorCorePosition_test(void) #endif } +#define JIURUI_COMPUTE_CALIB_PARA 0 +#define JIURUI_HOLE_POSITION 1 +#define JIURUI_TIRE_TEST_GROUP 2 +void JiuRui_workpiecePosition_test(void) +{ + const char* dataPath[JIURUI_TIRE_TEST_GROUP] = { + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍖椾含鐜栫憺/宸ヤ欢璇嗗埆/宸ヤ欢鏁版嵁/1367mm/",//0 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍖椾含鐜栫憺/宸ヤ欢璇嗗埆/宸ヤ欢鏁版嵁/璺濈宸ヤ欢1183mm/", //1 + }; + + SVzNLRange fileIdx[JIURUI_TIRE_TEST_GROUP] = { + {1,8},{1,6} + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if JIURUI_COMPUTE_CALIB_PARA + int cvtGrp = 0; + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "%s璋冨钩.txt", dataPath[cvtGrp]); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvtGrp]); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[cvtGrp]); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); +#if 0 + for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[cvtGrp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + { + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1); + } + sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + } +#endif + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if JIURUI_HOLE_POSITION + for (int grp = 0; grp <= 1; grp++) + { + SSG_planeCalibPara groundCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + //groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =17; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Hi229229.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 60; //45搴﹁ + cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //姘村钩瑙掑害锛屽皬浜庢瑙掑害瑙嗕负姘村钩 + cornerParam.jumpCornerTh_2 = 60; + + WD_JiuruiWorkpieceParam workpieceParam; + workpieceParam.len = 320; + workpieceParam.width = 140; + workpieceParam.thickness = 48; + + int errCode = 0; + std::vector workpiecePositions; + Jiurui_getWorkpiecePose( + scanLines, + cornerParam, + groundCalibPara, + workpieceParam, + workpiecePositions, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //杈撳嚭娴嬭瘯缁撴灉 + double dirLen = 200; + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + SSG_pointPose a_pose; + memset(&a_pose, 0, sizeof(SSG_pointPose)); + if (workpiecePositions.size() > 0) + a_pose = workpiecePositions[0]; + _outputRGBDResult_RGBD_2(_scan_file, scanLines, a_pose); + sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputHoleInfo_3(_scan_file, a_pose); + } + } +#endif +} + + +typedef enum +{ + keSG_鎷撴櫘鍙戝瓟瀹氫綅 = 0, + keSG_鍗庤埅瀛斿畾浣, + keSG_寰姏鐮傝疆鐩樺畾浣, + keSG_寰姏鐮傝疆鏋跺瓙瀹氫綅, + keSG_灞变笢鏈簨杞儙瀹氫綅, + keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅, + keSG_鍖椾含鐜栫憺宸ヤ欢瀹氫綅, +} ESG_testMode; int main() { //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅; @@ -1608,6 +1768,7 @@ int main() //ESG_testMode testMode = keSG_寰姏鐮傝疆鏋跺瓙瀹氫綅; ESG_testMode testMode = keSG_灞变笢鏈簨杞儙瀹氫綅; //ESG_testMode testMode = keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅; + //ESG_testMode testMode = keSG_鍖椾含鐜栫憺宸ヤ欢瀹氫綅; if (keSG_鎷撴櫘鍙戝瓟瀹氫綅 == testMode) TuoPuFa_holePosition_test(); @@ -1621,4 +1782,6 @@ int main() Benshi_tirePosition_test(); else if (keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅 == testMode) HaiRuiMa_rotorCorePosition_test(); + else if (keSG_鍖椾含鐜栫憺宸ヤ欢瀹氫綅 == testMode) + JiuRui_workpiecePosition_test(); }