diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index 65f5674..cb0cf9f 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -1,4 +1,4 @@ -#include +#include #include "SG_baseDataType.h" #include "SG_baseAlgo_Export.h" #include "workpieceHolePositioning_Export.h" @@ -6,24 +6,25 @@ #include //version 1.0.0 : base version release to customer -//version 1.0.2 : ˹̬ŷ) -//version 1.1.0 : cԹ̬淶Ϊĵ㣨㣩ʸ -std::string m_strVersion = "1.1.1"; +//version 1.0.2 : 添加了工件姿态（欧拉角输出) +//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量 +//version 1.2.0 : 算法完成了6轴验证 +std::string m_strVersion = "1.2.0"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); } -//ˮƽװƽ -//ZƽеʱҪԵΪգˮƽ -//תΪƽƽ淨ΪֱIJ +//相机水平安装计算地面调平参数。 +//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 SSG_planeCalibPara wd_getGroundCalibPara( std::vector< std::vector>& scanLines) { return sg_getPlaneCalibPara2(scanLines); } -//ˮƽʱ̬ƽȥ +//相机水平时姿态调平，并去除地面 void wd_lineDataR( std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, @@ -41,7 +42,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9]) return _r_pt; } -//ӽdistanceĿ +//搜索最接近distance的目标 int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, double distance, double distDeviation) { int result = -1; @@ -66,7 +67,7 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, do return result; } -//ӽdistanceҽǶΪangleĿ, ԽǶΪ +//搜索最接近distance且角度为angle的目标, 以角度为优先 int angleConditionDistanceSearch( SVzNL3DPoint seed, SVzNL3DPoint angleSide, std::vector& holes, @@ -151,7 +152,7 @@ double _getMeanZ(std::vector>& quantiValue, SVzNL3DPoint see return (zSum / hist); } -//׶λ +//工件孔定位 void wd_workpieceHolePositioning( std::vector< std::vector>& scanLinesInput, const WD_workpieceHoleParam workpiecePara, @@ -175,20 +176,20 @@ void wd_workpieceHolePositioning( isGridData = false; scanLines[i].resize(scanLinesInput[i].size()); - std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // ʹstd::copy㷨 + std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法 } - if (false == isGridData)//ݲʽ + if (false == isGridData)//数据不是网格格式 { *errCode = SG_ERR_NOT_GRID_FORMAT; return; } for (int i = 0; i < lineNum; i++) - { //д - //ƽȥ + { //行处理 + //调平，去除地面 wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); } - //ݣ1mmΪ߶ȣȷ߶ + //生成量化数据，以1mm为量化尺度，用于确定工件表面高度 SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines); SVzNLRect roi2D; roi2D.left = (int)roi3D.xRange.min; @@ -204,11 +205,11 @@ void wd_workpieceHolePositioning( for (int i = 0; i < quanti_X; i++) { quantiValue[i].resize(quanti_Y); - std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//ʼΪ0 + std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0 quantiHist[i].resize(quanti_Y); - std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//ʼΪ0 + std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0 } - //1mm߶ + //以1mm尺度量化 for (int line = 0; line < lineNum; line++) { for (int j = 0; j < linePtNum; j++) @@ -236,7 +237,7 @@ void wd_workpieceHolePositioning( pointMask.resize(lineNum); std::vector endingPoints; - //ȡ߶ζ˵ + //提取线段端点特征 for (int line = 0; line < lineNum; line++) { if (line == 1677) @@ -244,8 +245,8 @@ void wd_workpieceHolePositioning( std::vector& lineData = scanLines[line]; pointMask[line].resize(lineData.size()); - std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//ʼΪ0 - //˲˳쳣 + std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0 + //滤波，滤除异常点 sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); std::vector segs; @@ -253,19 +254,19 @@ void wd_workpieceHolePositioning( lineData, lineSegPara, segs); - //seg˵ΪԵ㡣˵ƽ󣬴ֱ׵ڲXYƽϾΪԵ㡣 + //将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。 for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) { int ptIdx = segs[i].start; endingPoints.push_back(lineData[ptIdx].pt3D); - pointMask[line][ptIdx] = 1; //ֹظ + pointMask[line][ptIdx] = 1; //防止重复 ptIdx = segs[i].start + segs[i].len - 1; endingPoints.push_back(lineData[ptIdx].pt3D); pointMask[line][ptIdx] = 1; } } - //ˮƽɨ + //生成水平扫描 std::vector> hLines_raw; hLines_raw.resize(linePtNum); for (int i = 0; i < linePtNum; i++) @@ -274,20 +275,20 @@ void wd_workpieceHolePositioning( { for (int j = 0; j < linePtNum; j++) { - scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用） hLines_raw[j][line] = scanLines[line][j]; hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; } } - //ˮƽarcȡ + //水平arc特征提取 int lineNum_h_raw = (int)hLines_raw.size(); for (int line = 0; line < lineNum_h_raw; line++) { if (line == 974) int kkk = 1; std::vector& lineData = hLines_raw[line]; - //˲˳쳣 + //滤波，滤除异常点 int ptNum = (int)lineData.size(); sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); @@ -296,11 +297,11 @@ void wd_workpieceHolePositioning( lineData, lineSegPara, segs); - //seg˵ΪԵ㡣˵ƽ󣬴ֱ׵ڲXYƽϾΪԵ㡣 + //将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。 for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) { int ptIdx = segs[i].start; - if (pointMask[ptIdx][line] == 0) //ֹظ + if (pointMask[ptIdx][line] == 0) //防止点重复 { SVzNL3DPoint an_ending; an_ending.x = lineData[ptIdx].pt3D.y; @@ -310,7 +311,7 @@ void wd_workpieceHolePositioning( pointMask[ptIdx][line] = 1; } ptIdx = segs[i].start + segs[i].len - 1; - if (pointMask[ptIdx][line] == 0) //ֹظ + if (pointMask[ptIdx][line] == 0) //防止点重复 { SVzNL3DPoint an_ending; an_ending.x = lineData[ptIdx].pt3D.y; @@ -322,7 +323,7 @@ void wd_workpieceHolePositioning( } } - //ע + //标注 std::vector> featureInfoMask; std::vector> feature3DInfo; featureInfoMask.resize(lineNum); @@ -333,7 +334,7 @@ void wd_workpieceHolePositioning( feature3DInfo[i].resize(lineNum_h_raw); } - //ע + //标注 for (int line = 0; line < lineNum; line++) { std::vector& a_lineMask = pointMask[line]; @@ -353,9 +354,9 @@ void wd_workpieceHolePositioning( } } } - // - //õ˼룬ع˼·иЧ - std::vector> clusters; //ֻ¼λ + //聚类 + //采用迭代思想，回归思路进行高效聚类 + std::vector> clusters; //只记录位置 std::vector clustersRoi3D; int clusterID = 1; int clusterCheckWin = 5; @@ -364,7 +365,7 @@ void wd_workpieceHolePositioning( for (int x = 0; x < lineNum; x++) { SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; - if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //Ѿ + if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理 continue; SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; @@ -380,11 +381,11 @@ void wd_workpieceHolePositioning( std::vector< SVzNL2DPoint> a_cluster; a_cluster.push_back(a_seedPos); wd_gridPointClustering( - featureInfoMask,//int¼ǺclusterIDһflag - feature3DInfo,//double,¼Ϣ - clusterCheckWin, // - growParam,// - clusterID, //ǰClusterID + featureInfoMask,//int，记录特征标记和clusterID，附加一个flag + feature3DInfo,//double,记录坐标信息 + clusterCheckWin, //搜索窗口 + growParam,//聚类条件 + clusterID, //当前Cluster的ID a_cluster, //result a_clusterRoi ); @@ -393,7 +394,7 @@ void wd_workpieceHolePositioning( clusterID++; } } - // + //聚类结果分析 std::vector validCluserIndexing; int clusterSize = (int)clusters.size(); for (int i = 0; i < clusterSize; i++) @@ -405,14 +406,14 @@ void wd_workpieceHolePositioning( (W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2)) validCluserIndexing.push_back(i); } - //ɽ + //生成结果 std::vector< SWD_HoleInfo> holes; int objectSize = (int)validCluserIndexing.size(); for (int objIdx = 0; objIdx < objectSize; objIdx++) { std::vector pointArray; int clusterIdx = validCluserIndexing[objIdx]; - //ȡclusterϵĵ + //取cluster上的点 int clusterPtSize = (int)clusters[clusterIdx].size(); double minZ = DBL_MAX; for (int i = 0; i < clusterPtSize; i++) @@ -426,7 +427,7 @@ void wd_workpieceHolePositioning( minZ = a_pt3d.z; pointArray.push_back(a_pt3d); } - //Բ + //圆拟合 SVzNL3DPoint center; double radius; double err = fitCircleByLeastSquare(pointArray, center, radius); @@ -436,9 +437,9 @@ void wd_workpieceHolePositioning( a_hole.radius = radius; holes.push_back(a_hole); } - //ָ - //Wӽĵ㣬ȻֱLӽĵ - double distDeviation = 5.0; //ĺϸޡСڴ˾룬ΪĿΪЧ + //分割 + //方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点 + double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效 for (int objIdx = 0; objIdx < objectSize; objIdx++) { if (holes[objIdx].radius < 0) @@ -450,9 +451,9 @@ void wd_workpieceHolePositioning( if (idx1 < 0) continue; - SVzNLRangeD angleRange = { 85, 95 }; //ֱ5ȷΧ + SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围 SWD_HoleInfo& p1 = holes[idx1]; - //ӽdistanceҽǶΪangleĿ, ԽǶΪ + //搜索最接近distance且角度为angle的目标, 以角度为优先 int idx2 = angleConditionDistanceSearch( p0.center, p1.center, holes, @@ -462,7 +463,7 @@ void wd_workpieceHolePositioning( continue; SWD_HoleInfo& p2 = holes[idx2]; - //ӽdistanceҽǶΪangleĿ, ԽǶΪ + //搜索最接近distance且角度为angle的目标, 以角度为优先 int idx3 = angleConditionDistanceSearch( p1.center, p0.center, holes, @@ -475,7 +476,7 @@ void wd_workpieceHolePositioning( p2.radius = -1; p3.radius = -1; - //¼ZֵΪ׵ԭZֵ᲻׼ȷȡߵе㴦ZֵľֵΪZֵ + //重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值 SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 }; SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 }; SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 }; @@ -500,7 +501,7 @@ void wd_workpieceHolePositioning( for (int m = 0; m < 4; m++) { SVzNL3DPoint a_pt = a_workpiece.holes[m]; - a_pt.z = a_pt.z + 20; //ΪƽԷֻz + a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向 a_workpiece.holesDir.push_back(a_pt); } a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4, @@ -508,10 +509,10 @@ void wd_workpieceHolePositioning( (z1 + z2 + z3 + z4) / 4 }; SVzNL3DPoint y_dir; - if (p0.center.y < p2.center.y) - y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, 0 }; + if (p0.center.x < p1.center.x) + y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 }; else - y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, 0 }; + y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 }; double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2)); y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 }; a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z }; @@ -521,7 +522,7 @@ void wd_workpieceHolePositioning( } int workpieceNum = (int)workpiecePositioning.size(); - //תȥ + //旋转回去 for (int i = 0; i < workpieceNum; i++) { SVzNL3DPoint rpt; @@ -546,10 +547,10 @@ void wd_workpieceHolePositioning( workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y, workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z }; double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2)); - vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //һ + vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化 double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2)); - vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //һ - //˳vector_x + vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化 + //叉乘出vector_x SVzNL3DPoint vector_x; vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z; vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x; @@ -558,7 +559,7 @@ void wd_workpieceHolePositioning( workpiecePositioning[i].y_dir = vector_y; workpiecePositioning[i].z_dir = vector_z; #if 0 - //õת + //得到旋转矩阵 double R[3][3]; R[0][0] = vector_x.x; R[1][0] = vector_x.y; diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 07418fd..a1b1c45 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -154,6 +154,12 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi } sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositioning[i].center.x, workpiecePositioning[i].center.y, workpiecePositioning[i].center.z); sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " x_dir: (%g, %g, %g)", workpiecePositioning[i].x_dir.x, workpiecePositioning[i].x_dir.y, workpiecePositioning[i].x_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " y_dir: (%g, %g, %g)", workpiecePositioning[i].y_dir.x, workpiecePositioning[i].y_dir.y, workpiecePositioning[i].y_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " z_dir: (%g, %g, %g)", workpiecePositioning[i].z_dir.x, workpiecePositioning[i].z_dir.y, workpiecePositioning[i].z_dir.z); + sw << dataStr << std::endl; } sw.close(); } @@ -444,7 +450,7 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {5,5}, + {6,6}, }; const char* ver = wd_workpieceHolePositioningVersion(); @@ -494,9 +500,13 @@ int main() std::cout << T << std::endl; //验算6轴姿态 + std::vector verify_pts_eye; + verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.end()); + cv::Point3d a_center = { 232.997, -173.533, 1795.9 }; + verify_pts_eye.push_back(a_center); std::vector> pose_eye; - pose_eye.resize(6); - for (int i = 0; i < 6; i++) + pose_eye.resize(7); + for (int i = 0; i < 7; i++) pose_eye[i].resize(3); pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 }; pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 }; @@ -504,13 +514,14 @@ int main() pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 }; pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 }; pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 }; - - for (int i = 0; i < 6; i++) + pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 }; + for (int i = 0; i < 7; i++) { cv::Point3d rtPt; - pointRT_2(R, T, pts_eye[i], rtPt); //RT前后的点 + pointRT_2(R, T, verify_pts_eye[i], rtPt); //RT前后的点 std::vector dirVectors_eye = pose_eye[i]; + //dirVectors_eye[0] = { -dirVectors_eye[0].x, -dirVectors_eye[0].y, -dirVectors_eye[0].z }; dirVectors_eye[1] = { -dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z }; dirVectors_eye[2] = { -dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z }; std::vector dirVectors_robot; @@ -613,7 +624,7 @@ int main() groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; char calibFile[250]; sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); - //groundCalibPara = _readCalibPara(calibFile); + groundCalibPara = _readCalibPara(calibFile); for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) {