diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 1155a66..268131a 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -410,6 +410,18 @@ SG_APISHARED_EXPORT void lineFitting_abc( double* _b, double* _c); +/** + * @brief 空间直线最小二乘拟合 + * @param points 输入的三维点集（至少2个点，否则无意义） + * @param center 输出：拟合直线的质心（基准点P0） + * @param direction 输出：拟合直线的方向向量v（单位化） + * @return 拟合是否成功（点集有效返回true） + */ +SG_APISHARED_EXPORT bool fitLine3DLeastSquares( + const std::vector& points, + SVzNL3DPoint& center, + SVzNL3DPoint& direction); + //圆最小二乘拟合 SG_APISHARED_EXPORT double fitCircleByLeastSquare( const std::vector& pointArray, @@ -537,10 +549,15 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( //计算一个平面调平参数。 //以数据输入中ROI以内的点进行平面拟合，计算调平参数 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 -SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( std::vector< std::vector>& scanLines, SVzNL3DRangeD roi); +//根据两个向量计算旋转矩阵 +SG_APISHARED_EXPORT SSG_planeCalibPara wd_conputeRTMatrix( + SVzNL3DPoint& vector1, + SVzNL3DPoint& vector2); + // 从旋转矩阵计算欧拉角（Z-Y-X顺序） SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]); // 从欧拉角计算旋转矩阵（Z-Y-X顺序） diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 6a3c5a4..e1d9638 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -2417,6 +2417,44 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( return planePara; } +SSG_planeCalibPara wd_conputeRTMatrix(SVzNL3DPoint& vector1, SVzNL3DPoint& vector2) +{ + Vector3 a = Vector3(vector1.x, vector1.y, vector1.z); + Vector3 b = Vector3(vector2.x, vector2.y, vector2.z); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //计算反向旋转矩阵 + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); + + SSG_planeCalibPara calibPara; + calibPara.planeCalib[0] = rMatrix.data[0][0]; + calibPara.planeCalib[1] = rMatrix.data[0][1]; + calibPara.planeCalib[2] = rMatrix.data[0][2]; + calibPara.planeCalib[3] = rMatrix.data[1][0]; + calibPara.planeCalib[4] = rMatrix.data[1][1]; + calibPara.planeCalib[5] = rMatrix.data[1][2]; + calibPara.planeCalib[6] = rMatrix.data[2][0]; + calibPara.planeCalib[7] = rMatrix.data[2][1]; + calibPara.planeCalib[8] = rMatrix.data[2][2]; + + calibPara.invRMatrix[0] = invMatrix.data[0][0]; + calibPara.invRMatrix[1] = invMatrix.data[0][1]; + calibPara.invRMatrix[2] = invMatrix.data[0][2]; + calibPara.invRMatrix[3] = invMatrix.data[1][0]; + calibPara.invRMatrix[4] = invMatrix.data[1][1]; + calibPara.invRMatrix[5] = invMatrix.data[1][2]; + calibPara.invRMatrix[6] = invMatrix.data[2][0]; + calibPara.invRMatrix[7] = invMatrix.data[2][1]; + calibPara.invRMatrix[8] = invMatrix.data[2][2]; + + calibPara.planeHeight = 0; + return calibPara; +} + SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( std::vector< std::vector>& scanLines, SVzNL3DRangeD roi) @@ -2796,7 +2834,7 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH } void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH) { - for (int i = 0; i < a_line.size(); i++) + for (int i = 0; i < (int)a_line.size(); i++) { SVzNL3DPoint a_pt = a_line[i].pt3D; if (a_pt.z < 1e-4) @@ -3034,3 +3072,56 @@ void pointRT(const cv::Mat& R, const cv::Mat& T, rtPt.z = result(2); return; } + + +/** + * @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） + Eigen::MatrixXd centered = A.rowwise() - Eigen::Vector3d(cx, cy, cz); + 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; +} diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 3eec456..f61d4f6 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3443,7 +3443,7 @@ void wd_getRingArcFeature( std::vector< SVzNL3DPosition>& lineData, int lineIdx, const SSG_cornerParam cornerPara, - double ringArcWidth, //定子的环宽度 + double ringArcWidth, //环宽度 std::vector& line_ringArcs //环 ) { diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index e619693..c60a96f 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -12,6 +12,107 @@ const char* wd_rodAndBarDetectionVersion(void) return m_strVersion.c_str(); } +SVzNL3DPoint getArcPeak( + std::vector< std::vector>& scanLines, + SWD_segFeature & a_arcFeature) +{ + SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D; + for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++) + { + if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点 + { + if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z) + arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D; + } + } + return arcPeak; +} + +//投影，提取ROI内的数据 +void xoyROIProjection( + std::vector< std::vector>& scanLines, + const double* rtMatrix, + SSG_ROIRectD& roi_xoy, + std::vector& projectPoints + ) +{ + int lineNum = (int)scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < (int)a_line.size(); i++) + { + SVzNL3DPoint a_pt = a_line[i].pt3D; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2]; + double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5]; + double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8]; + if ((x >= roi_xoy.left) && (x <= roi_xoy.right) && + (y >= roi_xoy.top) && (y <= roi_xoy.bottom)) + { + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + projectPoints.push_back(a_pt); + } + } + } +} + +SVzNLRangeD getZRange(std::vector& projectPoints) +{ + int ptNum = (int)projectPoints.size(); + SVzNLRangeD zRange; + zRange.min = DBL_MAX; + zRange.max = DBL_MIN; + for (int i = 0; i < ptNum; i++) + { + zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min; + zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max; + } + return zRange; +} + +void zCutPointClouds( + std::vector& projectPoints, + SVzNLRangeD& zRange, + std::vector& cutLayerPoints) +{ + int ptNum = (int)projectPoints.size(); + for (int i = 0; i < ptNum; i++) + { + if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max)) + cutLayerPoints.push_back(projectPoints[i]); + } +} + +SVzNL3DPoint getXoYCentroid(std::vector& points) +{ + int ptNum = (int)points.size(); + SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 }; + if (ptNum == 0) + return centroid; + for (int i = 0; i < ptNum; i++) + { + centroid.x += points[i].x; + centroid.y += points[i].y; + } + centroid.x = centroid.x / ptNum; + centroid.y = centroid.y / ptNum; + return centroid; +} + +SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) +{ + SVzNL3DPoint _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; +} + void sx_hexHeadScrewMeasure( std::vector< std::vector>& scanLines, bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 @@ -128,160 +229,70 @@ const char* wd_rodAndBarDetectionVersion(void) for (int i = 0; i < objNum; i++) { //空间直线拟合 - + std::vector fitPoints; + int nodeSize = (int)growTrees[i].treeNodes.size(); + for (int j = 0; j < nodeSize; j++) + { + SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]); + fitPoints.push_back(a_pt); + } + if (fitPoints.size() < 12) + continue; + //去除头尾各5个点，防止在端部和根部扫描时的数据有干扰 + fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5); + fitPoints.erase(fitPoints.end() - 5, fitPoints.end()); + //拟合 + SVzNL3DPoint P0_center, P1_dir; + bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir); + if (false == result) + continue; //投影 - + //计算旋转向量 + SVzNL3DPoint vector1 = P1_dir; + SVzNL3DPoint vector2 = { 0, 0, -1.0 }; + SSG_planeCalibPara rotatePara = wd_conputeRTMatrix( vector1, vector2); // + SSG_ROIRectD roi_xoy; + roi_xoy.left = P0_center.x - rodRidius * 4; //2D范围 + roi_xoy.right = P0_center.x + rodRidius * 4; //2D范围 + roi_xoy.top = P0_center.y - rodRidius * 4; //2D范围 + roi_xoy.bottom = P0_center.y + rodRidius * 4; //2D范围 + std::vector< SVzNL3DPoint> roiProjectionData; + xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData); + //取端面 + SVzNLRangeD zRange = getZRange(roiProjectionData); + SVzNLRangeD cutZRange; + cutZRange.min = zRange.min; + cutZRange.max = zRange.min + 10.0; //取10mm的端面 + std::vector surfacePoints; + zCutPointClouds(roiProjectionData, cutZRange, surfacePoints); + //计算中心点 + SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints); + projectionCenter.z = zRange.min; + //旋转回原坐标系 + SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix); + //生成Rod信息 + SSX_hexHeadScrewInfo a_rod; + a_rod.center = surfaceCenter; + a_rod.axialDir = P1_dir; + a_rod.rotateAngle = 0; + screwInfo.push_back(a_rod); } - - - std::vector& nodes_1 = growTrees[0].treeNodes; - std::vector& nodes_2 = growTrees[1].treeNodes; -#if _OUTPUT_DEBUG_DATA - for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++) + if (true == isHorizonScan) { - int lineIdx, ptIdx; - if (false == isHorizonScan) + int objNum = (int)screwInfo.size(); + for (int i = 0; i < objNum; i++) { - lineIdx = nodes_1[i].lineIdx; - ptIdx = nodes_1[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 1; - ptIdx = nodes_1[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 2; - } - else - { - ptIdx = nodes_1[i].lineIdx; - lineIdx = nodes_1[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 1; - lineIdx = nodes_1[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 2; + double tmp = screwInfo[i].center.x; + screwInfo[i].center.x = screwInfo[i].center.y; + screwInfo[i].center.y = tmp; + tmp = screwInfo[i].axialDir.x; + screwInfo[i].axialDir.x = screwInfo[i].axialDir.y; + screwInfo[i].axialDir.y = tmp; + //screwInfo[i].rotateAngle += 90; } } - for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++) - { - int lineIdx, ptIdx; - if (false == isHorizonScan) - { - lineIdx = nodes_2[i].lineIdx; - ptIdx = nodes_2[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 1; - ptIdx = nodes_2[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 2; - } - else - { - ptIdx = nodes_2[i].lineIdx; - lineIdx = nodes_2[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 1; - lineIdx = nodes_2[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 2; - } - } -#endif - - //按扫描线配对 - std::vector line_1; //line_1和line_2为按扫描线对应的两个槽道上的Gap特征 - std::vector line_2; - int i_idx = 0; - int j_idx = 0; - while (1) - { - int line_idx1 = nodes_1[i_idx].lineIdx; - int line_idx2 = nodes_2[j_idx].lineIdx; - if (line_idx1 == line_idx2) - { - line_1.push_back(nodes_1[i_idx]); - line_2.push_back(nodes_2[j_idx]); - i_idx++; - j_idx++; - } - else - { - if (line_idx1 < line_idx2) - i_idx++; - else - j_idx++; - } - if ((i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size())) - break; - } - -#if _OUTPUT_DEBUG_DATA - for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++) - { - int lineIdx, ptIdx; - if (false == isHorizonScan) - { - lineIdx = line_1[i].lineIdx; - ptIdx = line_1[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 3; - ptIdx = line_1[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 4; - } - else - { - ptIdx = line_1[i].lineIdx; - lineIdx = line_1[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 3; - lineIdx = line_1[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 4; - } - } - for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++) - { - int lineIdx, ptIdx; - if (false == isHorizonScan) - { - lineIdx = line_2[i].lineIdx; - ptIdx = line_2[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 3; - ptIdx = line_2[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 4; - } - else - { - ptIdx = line_2[i].lineIdx; - lineIdx = line_2[i].gapPt_0.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 3; - lineIdx = line_2[i].gapPt_1.nPointIdx; - scanLines[lineIdx][ptIdx].nPointIdx = 4; - } - } -#endif - - //旋转，保证扫描线与Gap垂直 - double angleSearchWin = 30; - double angleStepping = 0.1; - - int loop = (int)(angleSearchWin / angleStepping); - double bestSpace = 0; - double rotateAngle = 0; - for (int i = -loop; i <= loop; i++) - { - double angle = i * angleStepping; - std::vector rotate_line_1; - _XY_rotateLine(angle, line_1, rotate_line_1); - std::vector rotate_line_2; - _XY_rotateLine(angle, line_2, rotate_line_2); - double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2); - if (bestSpace < meanSpace) - { - bestSpace = meanSpace; - rotateAngle = angle; - } - } - std::vector calib_line_1; - _XY_rotateLine(rotateAngle, line_1, calib_line_1); - std::vector calib_line_2; - _XY_rotateLine(rotateAngle, line_2, calib_line_2); - - channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2); - channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1); - channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2); - channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines); - channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines); - return channelInfo; + return; } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index e1c15bc..dc833f0 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -8,7 +8,7 @@ typedef struct { SVzNL3DPoint center; //螺杆端部中心点 - SVzNL3DPoint axialPoint; //轴向点，与center构成轴向向量 + SVzNL3DPoint axialDir; //轴向向量 double rotateAngle; //-30 - 30度 }SSX_hexHeadScrewInfo; //六角头螺杆 @@ -16,10 +16,12 @@ typedef struct SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void); //六角头螺杆端部中心点和轴向测量 -SG_APISHARED_EXPORT SSX_hexHeadScrewInfo sx_hexHeadScrewMeasure( +SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure( std::vector< std::vector>& scanLines, bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, + double rodRidius, + std::vector& screwInfo, int* errCode);