diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index ad5d109..d5b0722 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -29,7 +29,8 @@ //version 1.5.4 : 添加山东本事机电轮胎定位API。 //version 1.5.5 : 山东本事机电轮胎定位。(1)添加了轮胎内径大小的输出, (2)按高度排序。最上面的目标排在第一个。 //version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法,使用圆扫描提取轮廓点。 -std::string m_strVersion = "HolePostion 1.5.6"; +//version 1.6.0 : 添加了玖瑞工件(单件)的姿态计算。 +std::string m_strVersion = "HolePostion 1.6.0"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -2824,7 +2825,8 @@ void _computeAnlgeScanData( 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); + int angleLine = (int)(angle / angleScale + 0.5); + angleLine = angleLine % (int)polarScanData.size(); SWD_polarPt a_polarPt; a_polarPt.lineIdx = line; @@ -4313,7 +4315,7 @@ void Jiurui_getWorkpiecePose( centroid.z = centroid.z / centroid_size; //圆周扫描:0.5度间隔 - double angleScale = 0.5; + double angleScale = 1.0; std::vector> polarScanData; int polarLines = (int)(360.0 / angleScale + 0.5); polarScanData.resize(polarLines); @@ -4330,12 +4332,17 @@ void Jiurui_getWorkpiecePose( } //提取平面点-计算法向 - double distToPlane_min = 17.5; - double distToPlane_max = 18.5; + double distToPlane_min = 19.5; + double distToPlane_max = 20.5; std::vector planePolarPoints; std::vector Points3ds; for (int i = 0; i < polarLines; i++) { + double angle = angleScale * i; + if ((angle < 30) || ((angle > 75) && (angle < 120)) || ((angle > 150) && (angle < 210)) || + ((angle > 240) && (angle < 300)) || (angle > 330)) //避开水平和垂直区域 + continue; + int lineSize = (int)polarScanData[i].size(); if (lineSize == 0) continue; @@ -4367,6 +4374,8 @@ void Jiurui_getWorkpiecePose( //计算投影向量 SVzNL3DPoint vec_1 = { res[0], res[1], res[2]}; + if (res[2] < 0) + vec_1 = { -res[0], -res[1], -res[2] }; SVzNL3DPoint vec_z = { 0, 0, 1.0 }; SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_z); @@ -4385,19 +4394,22 @@ void Jiurui_getWorkpiecePose( // //迭代计算质心 centroid = { 0, 0, 0 }; - for (int i = 0; i < centroid_size; i++) + std::vector< SVzNL2DPointD> points_2d; + points_2d.resize(projectPosition3ds.size()); + for (int i = 0; i < (int)projectPosition3ds.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 += projectPosition3ds[i].pt3D.x; + centroid.y += projectPosition3ds[i].pt3D.y; + centroid.z += projectPosition3ds[i].pt3D.z; + points_2d[i] = { projectPosition3ds[i].pt3D.x , projectPosition3ds[i].pt3D.y }; } centroid.x = centroid.x / centroid_size; centroid.y = centroid.y / centroid_size; centroid.z = centroid.z / centroid_size; //圆周扫描:0.5度间隔 + angleScale = 1.0; + polarLines = (int)(360.0 / angleScale + 0.5); polarScanData.clear(); polarScanData.resize(polarLines); _computeAnlgeScanData( @@ -4429,30 +4441,43 @@ void Jiurui_getWorkpiecePose( objCenter.y = objCenter.y / centerSum; objCenter.z = objCenter.z / centerSum; - //计算长轴方向 +#if 0 + //计算长轴方向: 长轴短轴比最大 + int degree90 = (int)(90.0 / angleScale); int degree180 = (int)(180.0 / angleScale); int longAxisIndex = -1; - double longAxisLen = -1; + double axisLenRatio = 0; + std::vector ratioBuff; + ratioBuff.resize(polarLines / 2); + std::vector sumRBuff; + sumRBuff.resize(polarLines / 2); for (int i = 0; i < polarLines/2; i++) { int idx_1 = i; - int idx_2 = i + degree180; + int idx_2 = (i + degree180) % polarLines; + int idx_3 = (i + degree90) % polarLines; + int idx_4 = (idx_3 + degree180) % polarLines; if (idx_2 < polarLines) { - if ((polarScanData[idx_1].size() == 0) || (polarScanData[idx_2].size() == 0)) + if ((polarScanData[idx_1].size() > 0) && (polarScanData[idx_2].size() > 0) && + (polarScanData[idx_3].size() > 0) && (polarScanData[idx_4].size() > 0)) { - double sumR = polarScanData[idx_1][0].R + polarScanData[idx_2][0].R; + double sumR_long = polarScanData[idx_1].back().R + polarScanData[idx_2].back().R; + double sumR_short = polarScanData[idx_3].back().R + polarScanData[idx_4].back().R; + double ratio = sumR_long / sumR_short; + sumRBuff[i] = sumR_long; + ratioBuff[i] = ratio; if (longAxisIndex < 0) { longAxisIndex = idx_1; - longAxisLen = sumR; + axisLenRatio = ratio; } else { - if (longAxisLen < sumR) + if (axisLenRatio < ratio) { longAxisIndex = idx_1; - longAxisLen = sumR; + axisLenRatio = ratio; } } } @@ -4464,8 +4489,14 @@ void Jiurui_getWorkpiecePose( return; } - double longAxisAngle = longAxisIndex * angleScale; - SVzNL3DPoint vector_y = {cos(longAxisAngle/PI), sin(longAxisAngle / PI), 0 }; + double longAxisAngle = longAxisIndex * angleScale * PI / 180; + SVzNL3DPoint vector_y = { cos(longAxisAngle), sin(longAxisAngle), 0 }; +#else + //PCA方法 + SVzNL2DPointD axis, roi_centroid; + pca2D(points_2d, axis, roi_centroid); + SVzNL3DPoint vector_y = { axis.x, axis.y, 0 }; +#endif //叉乘出vector_x SVzNL3DPoint vector_x; vector_x.x = vector_y.y * vec_z.z - vec_z.y * vector_y.z; diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index ab90ae8..ab7848c 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -1701,11 +1701,34 @@ void JiuRui_workpiecePosition_test(void) { //fidx =17; char _scan_file[256]; - sprintf_s(_scan_file, "%s%d_LaserData_Hi229229.txt", dataPath[grp], fidx); + sprintf_s(_scan_file, "%s%d_LaserData_Hi229229_sampleDown.txt", dataPath[grp], fidx); std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); if (scanLines.size() == 0) continue; +#if 0 + //抽样 + std::vector> sampleData; + int sampleLines = (int)scanLines.size() / 4; + sampleData.resize(sampleLines); + int sLine = 0; + for (int i = 0; i < (int)scanLines.size(); i++) + { + if (i % 4 == 0) + { + sampleData[sLine].resize(scanLines[i].size()); + std::copy(scanLines[i].begin(), scanLines[i].end(), sampleData[sLine].begin()); // 使用std::copy算法 + sLine++; + if (sLine >= sampleLines) + break; + } + } + //写入 + sprintf_s(_scan_file, "%s%d_LaserData_Hi229229_sampleDown.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, sampleData, false, &headNullLines); + continue; +#endif long t1 = (long)GetTickCount64();//统计时间 @@ -1766,9 +1789,9 @@ int main() //ESG_testMode testMode = keSG_华航孔定位; //ESG_testMode testMode = keSG_微力砂轮盘定位; //ESG_testMode testMode = keSG_微力砂轮架子定位; - ESG_testMode testMode = keSG_山东本事轮胎定位; + //ESG_testMode testMode = keSG_山东本事轮胎定位; //ESG_testMode testMode = keSG_宁波海瑞马转子芯定位; - //ESG_testMode testMode = keSG_北京玖瑞工件定位; + ESG_testMode testMode = keSG_北京玖瑞工件定位; if (keSG_拓普发孔定位 == testMode) TuoPuFa_holePosition_test();