diff --git a/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp b/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp index 0fd5b67..6d03c07 100644 --- a/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp +++ b/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp @@ -245,15 +245,16 @@ void _outputRGBDScan_RGBD( #define TEST_GET_ROI_DATA 0 #define TEST_COMPUTE_SPACE 1 -#define TEST_GROUP 1 +#define TEST_GROUP 2 int main() { const char* dataPath[TEST_GROUP] = { "F:/ShangGu/项目/水木宏创/槽道间距测量/模拟数据/", //0 + "F:/ShangGu/项目/水木宏创/槽道间距测量/现场数据/", //0 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,9}, + {1,9},{1,4} }; const char* ver = wd_ChannelSPaceMeasureVersion(); @@ -285,7 +286,7 @@ int main() #endif #if TEST_COMPUTE_SPACE - for (int grp = 0; grp < TEST_GROUP; grp++) + for (int grp = 1; grp < TEST_GROUP; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -316,15 +317,15 @@ int main() SSG_treeGrowParam growParam; growParam.maxLineSkipNum = 10; - growParam.yDeviation_max = 20.0; + growParam.yDeviation_max = 40.0; growParam.maxSkipDistance = 20.0; - growParam.zDeviation_max = 50.0;// - growParam.minLTypeTreeLen = 100; //mm - growParam.minVTypeTreeLen = 100; //mm + growParam.zDeviation_max = 100.0;// + growParam.minLTypeTreeLen = 500; //mm + growParam.minVTypeTreeLen = 500; //mm SSX_channelParam channelParam; channelParam.channleSpaceRng = { 300, 800 }; - channelParam.channelWidthRng = { 5,30 }; + channelParam.channelWidthRng = { 5,50 }; bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 int errCode = 0; diff --git a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp index 7c6e38a..82687d0 100644 --- a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp +++ b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp @@ -563,7 +563,7 @@ int main() SVzNLRange fileIdx[TEST_GROUP] = { {1,13}, {2,5}, - {1,10} + {18,18} }; const char* ver = wd_bagThreadPositioningVersion(); @@ -571,33 +571,33 @@ int main() #if CONVERT_TO_GRID int convertGrp = 2; - for (int fidx = 1; fidx <= 10; fidx++) + for (int fidx = 17; fidx <= 17; fidx++) { char _scan_file[256]; sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx); std::vector> scanData; - double lineStep = 0.032; - double baseZ = 350.0; + double lineStep = 0.033; + double baseZ = 300.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, 3, sampleData); + downSampleGridData(scanData, 2, sampleData); //将数据恢复为按扫描线存储格式 sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx); _outputScanDataFile(_scan_file, sampleData,0, 0, 0); printf("%s: done.\n", _scan_file); } - +#if 0 char _scan_file[256]; sprintf_s(_scan_file, "%sgroundData.txt", dataPath[convertGrp]); std::vector> scanData; double lineStep = 0.032; - double baseZ = 350.0; + double baseZ = 300.0; vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData); bool isGridData = checkGridFormat(scanData); int gridFormat = (true == isGridData) ? 1 : 0; @@ -609,6 +609,7 @@ int main() sprintf_s(_scan_file, "%sgroundData_grid.txt", dataPath[convertGrp]); _outputScanDataFile(_scan_file, sampleData, 0, 0, 0); printf("%s: done.\n", _scan_file); +#endif #endif @@ -678,8 +679,8 @@ int main() } sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx); - std::vector> scanLines; - wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + std::vector> scanLines_src; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines_src); long t1 = (long)GetTickCount64();//统计时间 @@ -741,6 +742,12 @@ int main() scanInfo.mark_distance = 39.0; //两个Mark的距离 } + std::vector> scanLines;; + if ((grp == 2) && (fidx == 18)) + downSampleGridData(scanLines_src, 3, scanLines); + else + scanLines = scanLines_src; + int errCode = 0; std::vector bagThreadInfo; std::vector bagThreadInfo_relative; //相对于Mark的坐标 @@ -761,10 +768,19 @@ int main() long t2 = (long)GetTickCount64(); printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); //输出测试结果 + if (output_markCenter.size() == 2) + { + SVzNL3DPoint markDir = { output_markCenter[1].x - output_markCenter[0].x, + output_markCenter[1].y - output_markCenter[0].y, + output_markCenter[1].z - output_markCenter[0].z }; + double distance = sqrt(pow(markDir.x, 2) + pow(markDir.y, 2) + pow(markDir.z, 2)); + printf(" mark1: ( %g, %g, %g ), distance = %g\n", markDir.x, markDir.y, markDir.z, distance); + } + sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); _outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo, output_markCenter); sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); - _outputChanneltInfo(_scan_file, bagThreadInfo); + _outputChanneltInfo(_scan_file, bagThreadInfo_relative); } } #endif diff --git a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp index 1d2b5e5..4f49686 100644 --- a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp +++ b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp @@ -303,8 +303,8 @@ void _outputRGBDScan_fillingPort_RGBD( sw.close(); } -#define TEST_CONVERT_TO_GRID 1 -#define TEST_COMPUTE_POSE 0 +#define TEST_CONVERT_TO_GRID 0 +#define TEST_COMPUTE_POSE 1 #define TEST_GROUP 2 int main() { @@ -314,7 +314,7 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {2,30}, + {31,31}, }; const char* ver = wd_gasFillingPortPositionVersion(); @@ -333,7 +333,7 @@ int main() #endif #if TEST_COMPUTE_POSE - for (int grp = 0; grp < TEST_GROUP; grp++) + for (int grp = 0; grp <=0; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -355,7 +355,7 @@ int main() gasFillingPortPara.outerD = 52.0; SSG_lineSegParam lineSegPara; - lineSegPara.maxDist = 1.0; + lineSegPara.distScale = 1.0; lineSegPara.segGapTh_y = 5.0; //y方向间隔大于5mm认为是分段 lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段 diff --git a/motorStatorPosition_test/motorStatorPosition_test.cpp b/motorStatorPosition_test/motorStatorPosition_test.cpp index f2df5bc..e6bd7f0 100644 --- a/motorStatorPosition_test/motorStatorPosition_test.cpp +++ b/motorStatorPosition_test/motorStatorPosition_test.cpp @@ -273,6 +273,59 @@ SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints return gridData; } +void vzReadLaserScanPointFromFile_feihu( + const char* fileName, + std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> linePoints; + linePoints.clear(); + int startIdx = -1; + int ptIdx = 0; + double pre_y = DBL_MAX; + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + int r = -1, g = -1, b = -1; + sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); + + SVzNL3DPosition a_pt; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + + if ((Y+5.0) < pre_y) + { + //新行 + if (linePoints.size() > 0) + scanData.push_back(linePoints); + linePoints.clear(); + ptIdx = 0; + } + linePoints.push_back(a_pt); + pre_y = Y; + } + } + if (linePoints.size() > 0) + scanData.push_back(linePoints); + return; +} + void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int lineNum, float lineV, int maxTimeStamp, int clockPerSecond) { @@ -359,7 +412,7 @@ void _outputScanDataFile_RGBD_obj( char* fileName, std::vector>& scanData, float lineV, int maxTimeStamp, int clockPerSecond, - std::vector& resultObjPositions) + std::vector& resultObjPositions) { int lineNum = (int)scanData.size(); std::ofstream sw(fileName); @@ -396,43 +449,8 @@ void _outputScanDataFile_RGBD_obj( for (int i = 0; i < nPositionCnt; i++) { SVzNL3DPosition& pt3D = scanData[line][i]; - int type = pt3D.nPointIdx; - if (1 == type) - { - rgb = { 0,255,0 }; - rgb.r = (int)((double)rgb.r * alpha); - rgb.g = (int)((double)rgb.g * alpha); - rgb.b = (int)((double)rgb.b * alpha); - size = 2; - } - else if (2 == type) - { - rgb = { 0,0,255 }; - rgb.r = (int)((double)rgb.r * alpha); - rgb.g = (int)((double)rgb.g * alpha); - rgb.b = (int)((double)rgb.b * alpha); - size = 2; - } - else if (3 == type) //轮眉 - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (4 == type) // - { - rgb = { 255, 255, 0 }; - size = 3; - } - else if (5 == type) // - { - rgb = { 255, 255, 0 }; - size = 3; - } - else - { - rgb = { 100, 100, 100 }; - size = 1; - } + rgb = { 250, 250, 250 }; + size = 1; float x = (float)pt3D.pt3D.x; float y = (float)pt3D.pt3D.y; @@ -442,90 +460,61 @@ void _outputScanDataFile_RGBD_obj( sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } } -#if 0 - //if (objOps.size() > 0) +#if 1 + std::vector centerPoints; + std::vector dirPoints; + int ptNum = (int)resultObjPositions.size(); + if (ptNum > 0) { - int ptNum = 3; - sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - - rgb = { 255, 0, 0 }; + for (int i = 0; i < ptNum; i++) + { + SVzNL3DPoint a_center = { resultObjPositions[i].opCenter.x, resultObjPositions[i].opCenter.y, resultObjPositions[i].opCenter.z }; + SVzNL3DPoint a_dir = { resultObjPositions[i].opCenter.x, resultObjPositions[i].opCenter.y, resultObjPositions[i].opCenter.z + 50 }; + centerPoints.push_back(a_center); + dirPoints.push_back(a_dir); + } + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << (ptNum+1) << std::endl; size = 10; - float x = (float)wheelArcHeight.wheelArchPos.x; - float y = (float)wheelArcHeight.wheelArchPos.y; - float z = (float)wheelArcHeight.wheelArchPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; + for (int i = 0; i < ptNum; i++) + { + if(i == 0) rgb = { 255, 0, 0 }; + else rgb = { 255, 255, 0 }; + sw << "{" << centerPoints[i].x << "," << centerPoints[i].y << "," << centerPoints[i].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + rgb = { 255, 0, 0 }; + sw << "{" << centerPoints[0].x << "," << centerPoints[0].y << "," << centerPoints[0].z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - x = (float)wheelArcHeight.wheelUpPos.x; - y = (float)wheelArcHeight.wheelUpPos.y; - z = (float)wheelArcHeight.wheelUpPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; + //画出方向线 + size = 3; + int lineIdx = 0; + for (int i = 0; i < ptNum; i++) + { + if (i == 0) rgb = { 255, 0, 0 }; + else rgb = { 255, 255, 0 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << centerPoints[i].x << "," << centerPoints[i].y << "," << centerPoints[i].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPoints[i].x << "," << dirPoints[i].y << "," << dirPoints[i].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + + rgb = { 255, 0, 0 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << centerPoints[0].x << "," << centerPoints[0].y << "," << centerPoints[0].z << "}-"; sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - - x = (float)wheelArcHeight.wheelDownPos.x; - y = (float)wheelArcHeight.wheelDownPos.y; - z = (float)wheelArcHeight.wheelDownPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPoints[0].x << "," << dirPoints[0].y << "," << dirPoints[0].z << "}-"; sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - - x = (float)wheelArcHeight.wheelArchPos.x; - y = (float)wheelArcHeight.wheelArchPos.y; - z = (float)wheelArcHeight.wheelArchPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; } - //画出方向线 - rgb = { 255, 0, 0 }; - size = 3; - int lineIdx = 0; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.upLine[0].x << "," << (float)wheelArcHeight.upLine[0].y << "," << (float)wheelArcHeight.upLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.upLine[1].x << "," << (float)wheelArcHeight.upLine[1].y << "," << (float)wheelArcHeight.upLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.downLine[0].x << "," << (float)wheelArcHeight.downLine[0].y << "," << (float)wheelArcHeight.downLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.downLine[1].x << "," << (float)wheelArcHeight.downLine[1].y << "," << (float)wheelArcHeight.downLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.centerLine[0].x << "," << (float)wheelArcHeight.centerLine[0].y << "," << (float)wheelArcHeight.centerLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.centerLine[1].x << "," << (float)wheelArcHeight.centerLine[1].y << "," << (float)wheelArcHeight.centerLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; #endif sw.close(); } @@ -767,6 +756,13 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in #define TEST_GROUP 1 int main() { + const char* dataPath[TEST_GROUP] = { +"F:/ShangGu/电机定子/数据1/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {4,18} }; + #if CONVERT_TO_GRID //将数据转换成栅格格式格式 char _scan_dir[256]; @@ -775,12 +771,12 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_dir, "E:\\上古\\电机定子\\"); + int cvt_grp = 0; char _scan_file[256]; - double _F = 1300; //f - for (int i = 1; i <= 1; i++) + double _F = 1299.956; //f + for (int i = 4; i <= 18; i++) { - sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i); + sprintf_s(_scan_file, "%sLaserData%d.txt", dataPath[cvt_grp], i); SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); if (laser3DPoints_RGBD == NULL) continue; @@ -794,7 +790,7 @@ int main() SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F, camPoseR, &vldLineNum); char _out_file[256]; - sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); + sprintf_s(_out_file, "%sLaserLine%d_grid.txt", dataPath[cvt_grp], i); _outputScanDataFile_self(_out_file, gridData, vldLineNum, lineV, maxTimeStamp, clockPerSecond); printf("%s: convert done!\n", _scan_file); @@ -802,12 +798,6 @@ int main() #endif #if TEST_COMPUTE_GRASP_POS - const char* dataPath[TEST_GROUP] = { - "F:/ShangGu/电机定子/数据1/", //0 - }; - - SVzNLRange fileIdx[TEST_GROUP] = { - {1,1} }; int endGroup = TEST_GROUP - 1; @@ -824,14 +814,14 @@ int main() poseCalibPara.planeCalib[6] = 0.0; poseCalibPara.planeCalib[7] = 0.0; poseCalibPara.planeCalib[8] = 1.0; - poseCalibPara.planeHeight = 2600.0; + poseCalibPara.planeHeight = 645.0; for (int i = 0; i < 9; i++) poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; char calibFile[250]; #if 0 if (grp == 0) { - sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt"); + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); poseCalibPara = _readCalibPara(calibFile); } #endif @@ -839,6 +829,10 @@ int main() SWD_statorParam statorParam; statorParam.statorOuterD = 85.0; statorParam.statorInnerD = 50.0; + statorParam.statorHeight = 100.0; //定子高度 + statorParam.plateThickness = 8.0; //隔板厚度 + statorParam.plateW = 500.0;//隔板长 + statorParam.plateH = 300.0;//隔板宽 SWD_statorPositonParam algoParam; memset(&algoParam, 0, sizeof(SWD_statorPositonParam)); @@ -859,11 +853,10 @@ int main() algoParam.growParam.minLTypeTreeLen = 100; //mm algoParam.growParam.minVTypeTreeLen = 100; //mm - SWD_nextOpParam refPos; - memset(&refPos, 0, sizeof(SWD_nextOpParam)); - refPos.cuttingZ = -1; //初始值，设为-1 for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { + SWD_statorGriperState stateMachine; + memset(&stateMachine, 0, sizeof(SWD_statorGriperState)); //fidx = 4; int lineNum = 0; float lineV = 0.0f; @@ -880,23 +873,21 @@ int main() long t1 = GetTickCount64(); int errCode = 0; - std::vector resultObjPositions; - SWD_statorOuterGrasper resultGraspPos; + std::vector resultObjPositions; wd_motorStatorPosition( scanLines, statorParam, poseCalibPara, algoParam, - &refPos, //上一次给出的参考位置，同时输出下一次的参考位置 + &stateMachine, //操作状态机。指示当前状态 &errCode, - resultObjPositions, - resultGraspPos); + resultObjPositions); long t2 = GetTickCount64(); char _dbg_file[256]; #if 1 sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputScanDataFile_RGBD_obj(_dbg_file,scanLines, 0,0,0, resultObjPositions); + _outputScanDataFile_RGBD_obj(_dbg_file,scanLines, 0,0,0, resultObjPositions); sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx); cv::String imgName(_dbg_file); double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index dc79e67..d818c94 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -441,9 +441,9 @@ typedef struct typedef struct { - SVzNL3DPoint opCenter; //λ - double objR; -}SWD_motorStatorPosition; + int objID; + SSG_6DOF opCenter; //λ +}SWD_statorInnerGrasper; typedef struct { diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 6a04742..5b402de 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -2864,7 +2864,7 @@ void pointCloud2DQuantization( #endif { SVzNL2DPoint v2i = { line, i }; - backIndexing[px][px] = v2i; + backIndexing[px][py] = v2i; quantiData[px][py].z = pt3D->pt3D.z; //ֱֵ if (prePt) @@ -2905,11 +2905,11 @@ void pointCloud2DQuantization( int pixWin = (int)(inerPolateDistTh / scale); int cols = (int)quantiData[0].size(); int rows = (int)quantiData.size(); - for (int y = 0; y < rows; y++) //ͼɨ跽һ + for (int y = 0; y < cols; y++) //ͼɨ跽һ { int pre_x = -1; double pre_value = -1; - for (int x = 0; x < cols; x++) + for (int x = 0; x < rows; x++) { double value = quantiData[x][y].z; if (value > 1e-4) diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index a9c5297..ba0a133 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -295,6 +295,9 @@ void wd_gridPointClustering_labelling_2( int i = 0; int lineNum = (int)srcData.size(); int linePtNum = (int)srcData[0].size(); + + SVzNL2DPoint& inputSeed = a_cluster[0]; + labelMask[inputSeed.x][inputSeed.y].flag = 1; //һ while (1) { if (i >= a_cluster.size()) diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 51ca935..2b493b5 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -25,6 +25,7 @@ #define SX_ERR_INVLID_CUTTING_Z -2101 #define SX_ERR_ZERO_OBJ_TOPLAYER -2102 #define SX_ERR_ZERO_OBJ_BTMLAYER -2103 +#define SX_ERR_GET_INVALID_PALTE -2104 //ȡ // #define SX_BAG_TRAY_EMPTY -2201 diff --git a/sourceCode/bagThreadPositioning.cpp b/sourceCode/bagThreadPositioning.cpp index 052253c..45ca471 100644 --- a/sourceCode/bagThreadPositioning.cpp +++ b/sourceCode/bagThreadPositioning.cpp @@ -235,7 +235,7 @@ void wd_bagThreadPositioning( } lineInterval = lineInterval / lineIntervalNum; - SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0}; + SVzNLRangeD jumpHeight = { scanInfo.mark_height-3.0, scanInfo.mark_height+3.0}; double markRotateAngle = 0; SVzNL3DPoint markCenter = { 0,0,0 }; std::vector< SVzNL2DPoint> debug_markOrigin; @@ -424,8 +424,8 @@ void wd_bagThreadPositioning( { 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 - 1.0) && (w <= scanInfo.mark_diameter + 1.0) && - (h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0)) + if ((w >= scanInfo.mark_diameter - 1.5) && (w <= scanInfo.mark_diameter + 1.5) && + (h >= scanInfo.mark_diameter - 1.5) && (h <= scanInfo.mark_diameter + 1.5)) { validMarks.push_back(i); } diff --git a/sourceCode/motorStatorPosition.cpp b/sourceCode/motorStatorPosition.cpp index 0d94317..69d0289 100644 --- a/sourceCode/motorStatorPosition.cpp +++ b/sourceCode/motorStatorPosition.cpp @@ -775,271 +775,251 @@ void cloutPointsClustering( } } -//Ӷλ -//1Ŀ꣨2ץȡ -//㷨߼ҵӵĸ߶->ýȡZȥ-> -// ͶӰעʱ߿ҲͬʱͶӰ->任->ȡĿ-> -// ںͱ߿ѰץȡĿץȡ -void wd_motorStatorPosition( - std::vector< std::vector>& scanLines, - const SWD_statorParam statorParam, - const SSG_planeCalibPara groundCalibPara, - const SWD_statorPositonParam algoParam, - SWD_nextOpParam* refPos, //һθIJολãͬʱһεIJολ - int* errCode, - std::vector& resultObjPositions, - SWD_statorOuterGrasper& resultGraspPos) +//ɵMask ʹSSG_clusterLabelṹ +void genValidPtLabelMask( + std::vector< std::vector>& srcData, + std::vector>& labelMask, + EWD_maskMode maskMode) { - int lineNum = (int)scanLines.size(); - if (lineNum == 0) + int lineNum = (int)srcData.size(); + labelMask.resize(lineNum); + for(int i = 0; i < lineNum; i ++) { - *errCode = SG_ERR_3D_DATA_NULL; - return; - } - // - wd_noiseFilter(scanLines, algoParam.filterParam, errCode); - if (*errCode != 0) - return; - - ///ʼݴ - //ʹcutZ_levelи - - - - double statorRingWidth = (statorParam.statorOuterD - statorParam.statorInnerD) / 2; - // - - - - if (refPos->cuttingZ < 0) //ȡϰ벿ĻȡĻж϶Ӹ߶ - { - //ֱݴ - std::vector> all_vLineArcs; - for (int i = 0; i < lineNum; i++) + int ptNum = (int)srcData[i].size(); + labelMask[i].resize(ptNum); + std::fill(labelMask[i].begin(), labelMask[i].end(), SSG_clusterLabel{ 0,0,0 }); + for (int j = 0; j < ptNum; j++) { - if (i == 576) - int k = 1; - //ȡӻ - std::vector line_ringArcs; - wd_getRingArcFeature( - scanLines[i], - i, - algoParam.cornerParam, - statorRingWidth, //ӵĻ - line_ringArcs // - ); - all_vLineArcs.push_back(line_ringArcs); //Ҳ룬֤ܰк - } - //˳ٵ - //ֱɨ跽 - std::vector v_seg_trees; - // - wd_getSegFeatureGrowingTrees( - all_vLineArcs, - v_seg_trees, - algoParam.growParam); - //ȡ߶ - if (v_seg_trees.size() > 0) - { - int treeSize = (int)v_seg_trees.size(); - double z_sum = 0; - for (int m = 0; m < treeSize; m++) + if (KeWD_Mask_ValidPt == maskMode) { - z_sum += v_seg_trees[m].tree_value; + if (srcData[i][j].z > 1e-4) + labelMask[i][j].validFlag = 1; + } + else if(KeWD_Mask_NullPt == maskMode) + { + if (srcData[i][j].z < 1e-4) + labelMask[i][j].validFlag = 1; } - refPos->cuttingZ = z_sum / (double)treeSize + statorParam.statorHeight / 2; } } - if (refPos->cuttingZ < 0) - { - *errCode = SX_ERR_INVLID_CUTTING_Z; - return; - } - //ʹ refPos->cuttingZ ָ - //ͶӰ2D - SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);// ͳҰС - double scale; - if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0)) - { - *errCode = SG_ERR_INVLD_Q_SCALE; - return; - } - if (pntCloudPara.scale_x < pntCloudPara.scale_y) - scale = pntCloudPara.scale_x; - else - scale = pntCloudPara.scale_y; + return; +} - double inerPolateDistTh = scale * 10; //ֵޣ ڴֵֵ - int edgeSkip = 2; - int maskX = (int)(pntCloudPara.xRange.max - pntCloudPara.xRange.min) / scale + 1; - int maskY = (int)(pntCloudPara.yRange.max - pntCloudPara.yRange.min) / scale + 1; - if ((maskX < 16) || (maskY < 16)) - return; - maskY = maskY + edgeSkip * 2; - maskX = maskX + edgeSkip * 2; - cv::Mat projectionImg(maskY, maskX, CV_32FC1, 0.0f); //任MaskʼΪһֵ1e+6 - cv::Mat projectionIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); // - pointClout2DProjection( - scanLines, - pntCloudPara.xRange, - pntCloudPara.yRange, - scale, - refPos->cuttingZ, - edgeSkip, - inerPolateDistTh, //ֵֵڴֵIJֵ - projectionImg,//ͶӰݣʼΪһֵ1e+6 - projectionIndexing //ڻ3D - ); - //任 - cv::Mat distTransform; - sg_distanceTrans(projectionImg, distTransform, 0); -#if OUTPUT_DEBUG //debug - cv::Mat maskImage; - cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U); - cv::imwrite("distTransformMask.png", maskImage); - cv::Mat dtImage; - cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); - cv::Mat dtImage_color; - cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR); - cv::imwrite("distTransform.png", dtImage_color); -#endif - //ȡĿ꣬ĵ - //Ծ任ֵнȡʹĿ겻ߡ - double distanceTh = (statorParam.statorOuterD - statorParam.statorInnerD) / 8; - cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols - for (int y = 0; y < maskY; y++) +//ɵMask ʹSSG_clusterLabelṹ +int countQuantiSize( + std::vector< std::vector>& srcData, + EWD_maskMode maskMode) +{ + int totalNum = 0; + int lineNum = (int)srcData.size(); + for (int i = 0; i < lineNum; i++) { - for (int x = 0; x < maskX; x++) + int ptNum = (int)srcData[i].size(); + for (int j = 0; j < ptNum; j++) { - if(distTransform.at(y,x) > distanceTh) - bwImg.at(y, x) = 1; + if (KeWD_Mask_ValidPt == maskMode) + { + if (srcData[i][j].z > 1e-4) + totalNum ++; + } + else if (KeWD_Mask_NullPt == maskMode) + { + if (srcData[i][j].z < 1e-4) + totalNum ++; + } } } - //ͨע - cv::Mat labImg; - std::vector labelRgns; - SG_TwoPassLabel(bwImg, labImg, labelRgns, 8); - //ݴСȡĿ - double objPixSize = statorParam.statorOuterD / scale; - int objSizeTh1 = (int)(objPixSize * 0.75); - int objSizeTh2 = (int)(objPixSize * 1.1); - std::vector objectRgns; - for (int i = 0, i_max = (int)labelRgns.size(); i < i_max; i++) - { - int width = labelRgns[i].roi.right - labelRgns[i].roi.left; - int height = labelRgns[i].roi.bottom - labelRgns[i].roi.top; - if ((width > objSizeTh1) && (width < objSizeTh2) && - (height > objSizeTh1) && (height < objSizeTh2)) - objectRgns.push_back(labelRgns[i]); - } - // ĵԲɨ裬ȷȷĵ - double scanR1 = (statorParam.statorInnerD / 2) * 0.75; - double scanR2 = (statorParam.statorOuterD / 2) * 1.25; - std::vector< SSG_peakRgnInfo> objects; - SSG_peakRgnInfo a_nullObj; - memset(&a_nullObj, 0, sizeof(SSG_peakRgnInfo)); - objects.push_back(a_nullObj); //ID1ʼID±ͬ - int objNumber = 1; - for (int i = 0, i_max = (int)objectRgns.size(); i < i_max; i++) - { - SSG_Region& a_objRgn = objectRgns[i]; - //y - int x = (a_objRgn.roi.left + a_objRgn.roi.right) / 2; - int y = (a_objRgn.roi.top + a_objRgn.roi.bottom) / 2; - //Բɨ趨ӻ - std::vector peakPts; - circileScan( - distTransform, //ɨ - x, y, //ɨ - scanR1, scanR2, //ɨ - peakPts //ɨ - ); - //ģϣʹüģ - double cx = 0, cy = 0, cz = 0; - int ptSize = (int)peakPts.size(); - if (ptSize == 0) - continue; + return totalNum; +} - for (int m = 0; m < ptSize; m++) - { - cx += (double)peakPts[m].x; - cy += (double)peakPts[m].y; - cz += projectionImg.at(peakPts[m].y, peakPts[m].x); - } - cx = cx / (double)ptSize; - cy = cy / (double)ptSize; - cz = cz / (double)ptSize; - // - SSG_peakRgnInfo a_obj; - a_obj.pos2D = {(int)cy,(int)cx}; - a_obj.centerPos.x = cx * scale + pntCloudPara.xRange.min; - a_obj.centerPos.y = cy * scale + pntCloudPara.yRange.min; - a_obj.centerPos.z = cz; - a_obj.pkRgnIdx = objNumber; - objNumber++; - objects.push_back(a_obj); - } - if (objNumber < 2) - { - *errCode = SX_ERR_ZERO_OBJ; +//ƾ +//SSG_intPairԱ: +// data_0: flag +// data_1: valid point flag +// idx: cluster ID +void cloutPointsClustering_2( + std::vector< std::vector>& clusterSrcData, + std::vector>& labelMask, + std::vector>& clusters, //ֻ¼λ + std::vector& clustersInfo) +{ + int lineNum = (int)clusterSrcData.size(); + if (lineNum == 0) return; - } - //⵽Ŀλ - for (int i = 1; i < objNumber; i++) + int linePtNum = (int)clusterSrcData[0].size(); + + //õ˼룬ع˼·иЧ + int clusterID = 1; + int clusterCheckWin = 5; + for (int y = 0; y < linePtNum; y++) { - SWD_motorStatorPosition a_obj; - a_obj.objR = statorParam.statorOuterD / 2; - a_obj.opCenter.x = objects[i].centerPos.x; - a_obj.opCenter.y = objects[i].centerPos.y; - a_obj.opCenter.z = objects[i].centerPos.z; - resultObjPositions.push_back(a_obj); + for (int x = 0; x < lineNum; x++) + { + SSG_clusterLabel& a_seedInfo = labelMask[x][y]; + if ((0 == a_seedInfo.validFlag) || (a_seedInfo.clusterID > 0)) //Ѿ + continue; + + SVzNL3DPoint a_feature3DValue = clusterSrcData[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_labelling_2( + clusterSrcData, + labelMask, + clusterID, //ǰClusterID + a_cluster, //result + a_clusterRoi //roi2D + ); + 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++; + } + } +} + +//ݿ׶С˺ϸĿ +void clusterSizeMatching( + std::vector>& clusters, + std::vector& clustersInfo, + const SWD_statorParam statorParam, + std::vector& validClusterIndice //ϸĿš˴ʹţݿ +) +{ + double objSizeTh_min = statorParam.statorInnerD * 0.75; + double objSizeTh_max = statorParam.statorInnerD * 1.25; + int clusterNum = (int)clusters.size(); + for (int i = 0; i < clusterNum; i++) + { + double xw = clustersInfo[i].roi3D.xRange.max - clustersInfo[i].roi3D.xRange.min; + double yw = clustersInfo[i].roi3D.yRange.max - clustersInfo[i].roi3D.yRange.min; + if ((xw >= objSizeTh_min) && (xw <= objSizeTh_max) && (yw >= objSizeTh_min) && (yw <= objSizeTh_max)) + validClusterIndice.push_back(i); + } + return; +} + +SSG_peakRgnInfo _computeObjInfo(int cluterIndex, + std::vector>& clusters, + std::vector& clustersInfo, + std::vector>& srcData, + const double quatiStatorR +) +{ + SSG_peakRgnInfo a_obj; + memset(&a_obj, 0, sizeof(SSG_peakRgnInfo)); + a_obj.pkRgnIdx = -1; + + int counterNum = (int)clusters[cluterIndex].size(); + if (counterNum == 0) + return a_obj; + + SVzNL3DPoint centerPos = { 0, 0, 0 }; + SVzNL2DPoint center2D = { 0, 0 }; + + for (int i = 0; i < counterNum; i++) + { + SVzNL2DPoint& a_pt = clusters[cluterIndex][i]; + center2D.x += a_pt.x; + center2D.y += a_pt.y; + centerPos.x += srcData[a_pt.x][a_pt.y].x; + centerPos.y += srcData[a_pt.x][a_pt.y].y; } + a_obj.pkRgnIdx = cluterIndex; + a_obj.pos2D.x = center2D.x / counterNum; + a_obj.pos2D.y = center2D.y / counterNum; + + //ZȡΧZСֵ + int winR = (int)quatiStatorR + 1; + int xMax = srcData.size(); + int yMax = srcData[0].size(); + double cx = a_obj.pos2D.x; + double cy = a_obj.pos2D.y; + double minZ = DBL_MAX; + for (int x = a_obj.pos2D.x - winR; x <= a_obj.pos2D.x + winR; x++) + { + for (int y = a_obj.pos2D.y - winR; y <= a_obj.pos2D.y + winR; y++) + { + if ((x >= 0) && (x < xMax) && (y >= 0) && (y < yMax)) + { + double dx = x; + double dy = y; + double dist = sqrt(pow(dx - cx, 2) + pow(dy - cy, 2)); + if (dist < quatiStatorR) + { + if ((srcData[x][y].z > 1e-4) && (minZ > srcData[x][y].z)) + { + minZ = srcData[x][y].z; + } + } + } + } + } + + a_obj.centerPos.x = centerPos.x / counterNum; + a_obj.centerPos.y = centerPos.y / counterNum; + a_obj.centerPos.z = minZ; + + a_obj.objSize.dWidth = clustersInfo[cluterIndex].roi3D.xRange.max - clustersInfo[cluterIndex].roi3D.xRange.min; + a_obj.objSize.dHeight = clustersInfo[cluterIndex].roi3D.yRange.max - clustersInfo[cluterIndex].roi3D.yRange.min; + return a_obj; +} + +void computeBestObject( + std::vector& objectRgns, + const SWD_statorParam statorParam, + SWD_statorGriperState* opState, + std::vector& resultObjPositions) +{ + int objNumber = (int)objectRgns.size(); //ץȡ //ڽӹϵ,ÿĿ6ڽĿ - std::vector< SSG_hexagonNeighbour> objLinkings; + std::vector< int> objLinkings; objLinkings.resize(objNumber);//һʹáĿID1ʼID±һ for (int i = 0; i < objNumber; i++) { - objLinkings[i].ID = i; - objLinkings[i].linkNum = 0; - for (int j = 0; j < 6; j++) - objLinkings[i].neighborID[j] = -1; + objLinkings[i] = 0; } - objLinkings[0].linkNum = INT_MAX; - double linkPixDistTh = (statorParam.statorOuterD * 1.5) / scale; - for (int i = 1; i < objNumber; i++) + double linkPixDistTh = statorParam.statorOuterD * 1.5; + for (int i = 0; i < objNumber; i++) { - SSG_peakRgnInfo& obj_0 = objects[i]; + SSG_peakRgnInfo& obj_0 = objectRgns[i]; for (int j = i + 1; j < objNumber; j++) { - SSG_peakRgnInfo& obj_1 = objects[j]; + SSG_peakRgnInfo& obj_1 = objectRgns[j]; double dy = obj_1.pos2D.y - obj_0.pos2D.y; double dx = obj_1.pos2D.x - obj_0.pos2D.x; double dist = sqrt(pow(dx, 2) + pow(dy, 2)); if (dist < linkPixDistTh) { - double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360 - //60Ϊһ0-59Ϊ0 60-119Ϊ1 - int angleIndx = (int)(angle+ 30.0)/ 60; //˴30ȣֹĿ괦0ٽ硣ݶˮƽʹֱ - int angleIndx_1 = (angleIndx + 3) % 6; - objLinkings[i].neighborID[angleIndx] = j; - objLinkings[j].neighborID[angleIndx_1] = i; + objLinkings[i]++; + objLinkings[j]++; } } } //ȷץȡ - int objID = -1; - bool validGrasper = false; - if (refPos->refPos.z > 1e-4) //һθλýץȡ + if (opState->refPos.z > 1e-4) //һθλýץȡ { + int objID = -1; double minDist = -1; - int refx = (refPos->refPos.x - pntCloudPara.xRange.min) / scale; - int refy = (refPos->refPos.y - pntCloudPara.yRange.min) / scale; - for (int i = 1; i < objNumber; i++) //ĿΪץȡĿ + double refx = opState->refPos.x; + double refy = opState->refPos.y; + for (int i = 0; i < objNumber; i++) //ĿΪץȡĿ { - double dist = sqrt(pow(objects[i].pos2D.x-refx, 2) + pow(objects[i].pos2D.y - refy, 2)); + double dist = sqrt(pow(objectRgns[i].centerPos.x - refx, 2) + pow(objectRgns[i].centerPos.y - refy, 2)); if (minDist < 0) { minDist = dist; @@ -1054,353 +1034,473 @@ void wd_motorStatorPosition( } } } - //ץȡ - SWD_statorOuterGrasper outerGrasper; - double angleDistance; - bool validGrasper = computeOuterGraspPoint( - objID, - statorParam.statorOuterD / 2, - objects, - objLinkings, - outerGrasper, - &angleDistance - ); - resultGraspPos = outerGrasper; - + SWD_statorInnerGrasper innerGrasper; + innerGrasper.objID = objID; + innerGrasper.opCenter = objectRgns[objID].centerPos; + resultObjPositions.push_back(innerGrasper); + //ĿгDebugĿǷ׼ȷ + for (int i = 0; i < objNumber; i++) + { + if (i != objID) + { + innerGrasper.objID = i; + innerGrasper.opCenter = objectRgns[i].centerPos; + resultObjPositions.push_back(innerGrasper); + } + } } else { //ͳĿ int minLinkNum = -1; - for (int i = 1; i < objNumber; i++) + int minLindID = -1; + for (int i = 0; i < objNumber; i++) { - objLinkings[i].linkNum = 0; - for (int j = 0; j < 6; j++) - { - if (objLinkings[i].neighborID[j] >= 0) - objLinkings[i].linkNum++; - } + if (minLinkNum < 0) - minLinkNum = objLinkings[i].linkNum; + { + minLinkNum = objLinkings[i]; + minLindID = i; + } else { - if (minLinkNum > objLinkings[i].linkNum) - minLinkNum = objLinkings[i].linkNum; + if (minLinkNum > objLinkings[i]) + { + minLinkNum = objLinkings[i]; + minLindID = i; + } } } - - std::vector objCandidates; - for (int i = 1; i < objNumber; i++) + //ĿٵĿ궨ΪץȡĿ + SWD_statorInnerGrasper innerGrasper; + innerGrasper.objID = minLindID; + innerGrasper.opCenter = objectRgns[minLindID].centerPos; + resultObjPositions.push_back(innerGrasper); + //ĿгDebugĿǷ׼ȷ + for (int i = 0; i < objNumber; i++) { - if (minLinkNum = objLinkings[i].linkNum) - objCandidates.push_back(i); + if (i != minLindID) + { + innerGrasper.objID = i; + innerGrasper.opCenter = objectRgns[i].centerPos; + resultObjPositions.push_back(innerGrasper); + } + } + } +} + +void getDistanceTransformMaskData( + std::vector>& quantiData, + cv::Mat& projectionData//ͶӰݣʼΪһֵ1e+6 +) +{ + for (int row = 0; row < (int)quantiData.size(); row++) + { + for (int col = 0; col < (int)quantiData[row].size(); col++) + { + if(quantiData[row][col].z > 1e-4) + projectionData.at(row, col) = 1e+6; + } + } + return; +} + +//Ӷλ +//1Ŀ꣨2ץȡ +//㷨߼ҵӵĸ߶->ýȡZȥ-> +// ͶӰעʱ߿ҲͬʱͶӰ->任->ȡĿ-> +// ںͱ߿ѰץȡĿץȡ +//ʹ״̬ÿһ״̬Ͽж㹤İץȡ +void wd_motorStatorPosition( + std::vector< std::vector>& scanLinesInput, + const SWD_statorParam statorParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_statorPositonParam algoParam, + SWD_statorGriperState* opState, //״ָ̬ʾǰ״̬ + int* errCode, + std::vector& resultObjPositions) +{ + int lineNum = (int)scanLinesInput.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + + 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㷨 + } + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + for (int i = 0; i < lineNum; i++) + { //д + //ƽȥ + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); + } + + // + wd_noiseFilter(scanLines, algoParam.filterParam, errCode); + if (*errCode != 0) + return; + + ///ʼݴ + double cutZ_topLevel = groundCalibPara.planeHeight - statorParam.plateThickness - statorParam.statorHeight * 1.5; + double cutZ_btmLevel = groundCalibPara.planeHeight - statorParam.statorHeight * 0.5; + // + std::vector< std::vector> cutData_topLevel; + cutData_topLevel.resize(lineNum); + //ײ + std::vector< std::vector> cutData_btmLevel; + cutData_btmLevel.resize(lineNum); + + int ptNum_total = 0; + int ptNum_topLevel = 0; + int ptNum_btmLevel = 0; + + //ʹcutZ_levelи + for (int line = 0; line < lineNum; line++) + { + //top level + cutData_topLevel[line].resize(linePtNum); + std::fill(cutData_topLevel[line].begin(), cutData_topLevel[line].end(), SVzNL3DPosition{ 0,{0,0,0} }); + //bottom level + cutData_btmLevel[line].resize(linePtNum); + std::fill(cutData_btmLevel[line].begin(), cutData_btmLevel[line].end(), SVzNL3DPosition{ 0,{0,0,0} }); + for (int j = 0; j < linePtNum; j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + ptNum_total++; + if (scanLines[line][j].pt3D.z < cutZ_topLevel) + { + cutData_topLevel[line][j] = scanLines[line][j]; + ptNum_topLevel++; + } + if (scanLines[line][j].pt3D.z < cutZ_btmLevel) + { + cutData_btmLevel[line][j] = scanLines[line][j]; + ptNum_btmLevel++; + } + } + } + } + + double quatiScale = 1.0; + double quatiStatorR = (statorParam.statorOuterD / 2.0) / quatiScale; + int quatiEdgeSkip = 8; + double inerPolateDistTh = quatiScale * 3; + SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines); +#if 0 + //ȫͼΪжϻ + std::vector> quantiData_all; + std::vector> backIndexing_all; + pointCloud2DQuantization( + scanLines, + roi3D.xRange, + roi3D.yRange, + quatiScale, + quatiEdgeSkip, + inerPolateDistTh, //ֵֵڴֵIJֵ + quantiData_all, //ݣʼΪһֵ1e+6 + backIndexing_all //ڻ3D + ); + int totalQuantiSize = countQuantiSize(quantiData_all, KeWD_Mask_ValidPt); +#endif + + bool topLevelIsEmpty = false; + if ((keSG_OPERATE_BTM_LAYER == opState->opState) || + (keSG_OPERATE_PLATE == opState->opState)) + topLevelIsEmpty = true; + + if ((keWD_OPERATE_Uknown == opState->opState) || + (keSG_OPERATE_TOP_LAYER == opState->opState)) //״̬Ϊδ֪״̬ϲץȡʱԶ + { + //Զ + //ͼϴռ䴦û꣬޷ȷռ޷ȷ׵ĴС + std::vector> quantiData_topLevel; + std::vector> backIndexing_topLevel; + pointCloud2DQuantization( + cutData_topLevel, + roi3D.xRange, + roi3D.yRange, + quatiScale, + quatiEdgeSkip, + inerPolateDistTh, //ֵֵڴֵIJֵ + quantiData_topLevel, //ݣʼΪһֵ1e+6 + backIndexing_topLevel //ڻ3D + ); + int topLevelQuantiSize = countQuantiSize(quantiData_topLevel, KeWD_Mask_ValidPt); + double ringArea = (pow(statorParam.statorOuterD, 2) - pow(statorParam.statorInnerD, 2)) * PI * 0.25; + if (topLevelQuantiSize < (ringArea * 0.5)) + { + topLevelIsEmpty = true; } - //std::sort(objLinkings.begin(), objLinkings.end(), compareByNeighbourNumber); - double maxAngleInterval = -1; - SWD_statorOuterGrasper bestGrasper; - for (int i = 0, i_max=(int)objCandidates.size(); i < i_max; i++) + //е + if (false == topLevelIsEmpty) { - int id = objCandidates[i]; - //ץȡ - SWD_statorOuterGrasper outerGrasper; - double angleDistance; - bool validGrasper = computeOuterGraspPoint( - id, - statorParam.statorOuterD / 2, - objects, - objLinkings, - outerGrasper, - &angleDistance - ); - if (true == validGrasper) + //ɵMask ʹSSG_clusterLabelṹ + std::vector> labelMask_topLevel; + genValidPtLabelMask(quantiData_topLevel, labelMask_topLevel, KeWD_Mask_NullPt); //Ŀģȡ׵ + // + //ʹþ෽ͨ + std::vector> clusters; + std::vector clustersInfo; + cloutPointsClustering_2( + quantiData_topLevel, + labelMask_topLevel, + clusters, //ֻ¼λ + clustersInfo); + + if (clusters.size() == 1) //ֻһĿ꣬˵topLevelǿյ㣬ϲΪ { - if (maxAngleInterval < 0) + topLevelIsEmpty = true; + } + else + { + //Ŀ + std::vector validClusterIndice;//ϸĿš˴ʹţݿ + //ݴСȡĿ + clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //ϸĿš˴ʹţݿ + if (validClusterIndice.size() == 0) //ûҵĿ(ʱеݣ˱ { - maxAngleInterval = angleDistance; - bestGrasper = outerGrasper; + *errCode = SX_ERR_ZERO_OBJ_TOPLAYER; + return; } else { - if (maxAngleInterval < angleDistance) + std::vector objectRgns; + int objNum = (int)validClusterIndice.size(); + for (int i = 0; i < objNum; i++) { - maxAngleInterval = angleDistance; - bestGrasper = outerGrasper; + //Ŀλ + SSG_peakRgnInfo a_obj = _computeObjInfo( + validClusterIndice[i], + clusters, + clustersInfo, + quantiData_topLevel, + quatiStatorR + ); + if (a_obj.pkRgnIdx < 0) + continue; + + objectRgns.push_back(a_obj); } + if (objectRgns.size() == 0) //ûҵĿ(ʱеݣ˱ + { + *errCode = SX_ERR_ZERO_OBJ_TOPLAYER; + return; + } + + //ɽ + computeBestObject( + objectRgns, + statorParam, + opState, + resultObjPositions + ); + + //״̬ + opState->refPos.x = resultObjPositions[0].opCenter.x; + opState->refPos.y = resultObjPositions[0].opCenter.y; + opState->refPos.z = resultObjPositions[0].opCenter.z; + opState->opState = keSG_OPERATE_TOP_LAYER; } } } - resultGraspPos = bestGrasper; } - //һοץȡ - int candiID = -1; - int candiLinkNum = -1; - for (int i = 0; i < 6; i++) + + //Ϊգײ + if (true == topLevelIsEmpty) { - int neighborID = objLinkings[objID].neighborID[i]; - if (neighborID >= 0) + //Եײд + //ͼϴռ䴦û꣬޷ȷռ޷ȷ׵ĴС + std::vector> quantiData_btmLevel; + std::vector> backIndexing_btmLevel; + pointCloud2DQuantization( + cutData_btmLevel, + roi3D.xRange, + roi3D.yRange, + quatiScale, + quatiEdgeSkip, + inerPolateDistTh, //ֵֵڴֵIJֵ + quantiData_btmLevel, //ݣʼΪһֵ1e+6 + backIndexing_btmLevel //ڻ3D + ); + int bmtLevelQuantiSize = countQuantiSize(quantiData_btmLevel, KeWD_Mask_ValidPt); + double ringArea = (pow(statorParam.statorOuterD, 2) - pow(statorParam.statorInnerD, 2)) * PI * 0.25; + if (bmtLevelQuantiSize < (ringArea * 0.5)) { - int linkNum = objLinkings[neighborID].linkNum; - if (candiLinkNum < 0) + //տ + *errCode = SX_ERR_ZERO_OBJ_BTMLAYER; + return; + } + + //任жϸ + if (opState->opState != keSG_OPERATE_BTM_LAYER) + { + int maskRow = quantiData_btmLevel.size(); + int maskCol = quantiData_btmLevel[0].size(); + cv::Mat projectionImg(maskRow, maskCol, CV_32FC1, 0.0f); //任MaskʼΪһֵ1e+6 + getDistanceTransformMaskData(quantiData_btmLevel, projectionImg ); // ͶӰݣʼΪһֵ1e + 6 + cv::Mat distTransform; + sg_distanceTrans(projectionImg, distTransform, 0); +#if DEBUG_OUT_IMAGE //debug + cv::Mat maskImage; + cv::normalize(projectionImg, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::imwrite("distTransformMask.png", maskImage); + cv::Mat dtImage; + cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::Mat dtImage_color; + cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR); + cv::imwrite("distTransform.png", dtImage_color); +#endif + //ȡľ任ֵ + double distancePeakTh = statorParam.plateH < statorParam.plateW ? statorParam.plateH : statorParam.plateW; + distancePeakTh = distancePeakTh / quatiScale; + distancePeakTh = (distancePeakTh / 2) * 0.75; + double maxDistTransformValue = 0; + for (int y = 0; y < maskRow; y++) { - candiID = neighborID; - candiLinkNum = linkNum; + for (int x = 0; x < maskCol; x++) + { + if (distTransform.at(y, x) > maxDistTransformValue) + maxDistTransformValue = distTransform.at(y, x); + } + } + if (maxDistTransformValue > distancePeakTh) + { + cv::Mat bwImg = cv::Mat::zeros(maskRow, maskCol, CV_8UC1);//rows, cols + for (int row = 0; row < maskRow; row++) + { + for (int col = 0; col < maskCol; col++) + { + if (quantiData_btmLevel[row][col].z > 1e-4) + bwImg.at(row, col) = 1; + } + } + //ͨע + cv::Mat labImg; + std::vector labelRgns; + SG_TwoPassLabel(bwImg, labImg, labelRgns, 8); + //ĿΪץȡĵ + SSG_Region maxRgn; + memset(&maxRgn, 0, sizeof(SSG_Region)); + int maxId = -1; + for (int i = 0; i < (int)labelRgns.size(); i++) + { + if (maxRgn.ptCounter < labelRgns[i].ptCounter) + { + maxRgn = labelRgns[i]; + maxId = i; + } + } + if (maxId < 0) + { + //ȡ + *errCode = SX_ERR_GET_INVALID_PALTE; + return; + } + + double plate_cx = (double)(labelRgns[maxId].roi.right + labelRgns[maxId].roi.left) / 2; + double plate_cy = (double)(labelRgns[maxId].roi.bottom + labelRgns[maxId].roi.top) / 2; + int cx_i = (int)plate_cx; + int cy_i = (int)plate_cy; + double op_z = quantiData_btmLevel[cx_i][cy_i].z; + plate_cx = (plate_cx - quatiEdgeSkip) * quatiScale + roi3D.xRange.min; + plate_cy = (plate_cy - quatiEdgeSkip) * quatiScale + roi3D.yRange.min; + + SWD_statorInnerGrasper plateGrasper; + plateGrasper.objID = 0; + plateGrasper.opCenter = { plate_cx , plate_cy, op_z , 0,0,0 }; + resultObjPositions.push_back(plateGrasper); + opState->opState = keSG_OPERATE_PLATE; + opState->refPos = { 0,0,0 }; + return; + } + } + + //²Ŀ + //ɵMask ʹSSG_clusterLabelṹ + std::vector> labelMask_btmLevel; + genValidPtLabelMask(quantiData_btmLevel, labelMask_btmLevel, KeWD_Mask_NullPt); //Ŀģȡ׵ + //ʹþ෽ͨ + std::vector> clusters; + std::vector clustersInfo; + cloutPointsClustering_2( + quantiData_btmLevel, + labelMask_btmLevel, + clusters, //ֻ¼λ + clustersInfo); + + if (clusters.size() == 1) //ֻһĿ꣬˵topLevelǿյ㣬ϲΪ + { + //տ + *errCode = SX_ERR_ZERO_OBJ_BTMLAYER; + return; + } + else + { + //Ŀ + std::vector validClusterIndice;//ϸĿš˴ʹţݿ + //ݴСȡĿ + clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //ϸĿš˴ʹţݿ + if (validClusterIndice.size() == 0) //ûҵĿ(ʱеݣ˱ + { + *errCode = SX_ERR_ZERO_OBJ_TOPLAYER; + return; } else { - if (candiLinkNum > linkNum) + std::vector objectRgns; + int objNum = (int)validClusterIndice.size(); + for (int i = 0; i < objNum; i++) { - candiID = neighborID; - candiLinkNum = linkNum; + //Ŀλ + SSG_peakRgnInfo a_obj = _computeObjInfo( + validClusterIndice[i], + clusters, + clustersInfo, + quantiData_btmLevel, + quatiStatorR + ); + if (a_obj.pkRgnIdx < 0) + continue; + + objectRgns.push_back(a_obj); } + if (objectRgns.size() == 0) //ûҵĿ(ʱеݣ˱ + { + *errCode = SX_ERR_ZERO_OBJ_BTMLAYER; + return; + } + + //ɽ + computeBestObject( + objectRgns, + statorParam, + opState, + resultObjPositions); + + //״̬ + opState->refPos.x = resultObjPositions[0].opCenter.x; + opState->refPos.y = resultObjPositions[0].opCenter.y; + opState->refPos.z = resultObjPositions[0].opCenter.z; + opState->opState = keSG_OPERATE_BTM_LAYER; } } } - if (candiID >= 0) - { - refPos->refPos.x = objects[candiID].centerPos.x; - refPos->refPos.y = objects[candiID].centerPos.y; - refPos->refPos.z = objects[candiID].centerPos.z; - } - else - { - refPos->refPos.x = 0; - refPos->refPos.y = 0; - refPos->refPos.z = -1; - } return; - -#if 0 - //ZͳơΪ1mm - int zHistSize = (int)(z_range.max - z_range.min) + 1; - std::vector zHist; - zHist.resize(zHistSize); - int totalPtNum = 0; - for (int line = 0; line < lineNum; line++) - { - for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - totalPtNum++; - int zPos = (int)(pt3D->pt3D.z - z_range.min); - zHist[zPos] ++; - } - } - //ȡӶ棺ȡ5mmͳ䣬zHist10% - std::vector sumHist; - sumHist.resize(zHistSize); - for (int i = 0; i < zHistSize; i++) - { - int data = 0; - for (int j = i - 2; j < i + 2; j++) - { - if ((j >= 0) && (j < zHistSize)) - data += zHist[j]; - } - sumHist[i] = data; - } - //ȡһڴ10%ļֵ - int dataTh = totalPtNum / 10; - int maxPos = -1; - for (int i = 1; i < zHistSize-1; i++) - { - int preData = sumHist[i-1]; - int data = sumHist[i]; - int nxtData = sumHist[i+1]; - if ((data > preData) && (data > nxtData) && (data > dataTh)) - { - maxPos = i; - break; - } - } - if (maxPos < 0) - return; - - //ȡӶ - double zSliceTop = (double)maxPos - 10 + z_range.min; - double zSliceBtm = (double)maxPos + 8 + z_range.min; - - //XOYͶӰ - //任Mask1mmΪ߶ - int maskX = (int)(roi3D.xRange.max - roi3D.xRange.min) + 1; - int maskY = (int)(roi3D.yRange.max - roi3D.yRange.min) + 1; - cv::Mat zSliceData = cv::Mat::zeros(maskY, maskX, CV_8U); - cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 1e+6); //任MaskʼΪһֵ1e+6 - //ھ任 - cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); // - for (int line = 0; line < lineNum; line++) - { - for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i]; - if ( (pt3D->pt3D.z < zSliceTop) || (pt3D->pt3D.z > zSliceBtm)) - continue; - double x = pt3D->pt3D.x; - double y = pt3D->pt3D.y; - - int px = (int)(x - roi3D.xRange.min); - int py = (int)(y - roi3D.yRange.min); - cv::Vec2i v2i = { line, i }; - zSliceData.at(py, px) = (uchar)255; - distTranformIndexing.at(py, px) = v2i; - distTranformMask.at(py, px) = 0; - } - } - cv::Mat zSliceData_origin = zSliceData.clone(); - - //任 - cv::Mat distTransform; - sg_distanceTrans(distTranformMask, distTransform, 0); -#if DEBUG_OUT_IMAGE //debug - cv::Mat dtImage; - cv::normalize(distTranformMask, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); - cv::imwrite("distTransform.png", dtImage); - //cv::normalize(zSliceData, dtImage, 64, 255, cv::NORM_MINMAX, CV_8U); - cv::imwrite("zSliceImage.png", zSliceData); -#endif - - //zSliceDataд1䣨2ʴ3ͨõĿ꣨4Բϵõĵ - cv::Mat invertSliceData; - cv::bitwise_not(zSliceData, invertSliceData); -#if DEBUG_OUT_IMAGE - cv::imwrite("zSliceImage_invert.png", invertSliceData); -#endif - //ͨ - cv::Mat labels, centroids, stats; - int nccomps = connectedComponentsWithStats(invertSliceData, labels, stats, centroids, 4); - for (int i = 1; i < nccomps; i++) - { - int size = stats.at(i, cv::CC_STAT_AREA); - if (size < 20) - { - int roi_x = stats.at(i, cv::CC_STAT_LEFT); - int roi_y = stats.at(i, cv::CC_STAT_TOP); - int roi_w = stats.at(i, cv::CC_STAT_WIDTH); - int roi_h = stats.at(i, cv::CC_STAT_HEIGHT); - //С20 - for (int row = roi_y; row < roi_y+roi_h; row++) - { - for (int col = roi_x; col < roi_x+roi_w; col++) - { - if (labels.at(row, col) == i) - zSliceData.at(row, col) = (uchar)255; - } - } - } - } -#if DEBUG_OUT_IMAGE - cv::imwrite("zSliceImage_filled.png", zSliceData); -#endif - - //ʴ - cv::Mat test = cv::Mat::zeros(64, 64, CV_8UC1); - cv::rectangle(test, cv::Rect(30, 30, 5, 5), 255, -1); - - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); - cv::Mat zSliceData_erode; - cv::erode(zSliceData, zSliceData_erode, element, cv::Point(-1,-1),2); -#if DEBUG_OUT_IMAGE - cv::imwrite("zSliceImage_erode.png", zSliceData_erode); -#endif - //ȡĿ - nccomps = connectedComponentsWithStats(zSliceData_erode, labels, stats, centroids, 4); - //ĿС - int obj_size_min = (int)(positionParam.statorInnerD * 0.75); - int obj_size_max = (int)(positionParam.statorOuterD * 1.25); - std::vector fittingObs; - for (int i = 1; i < nccomps; i++) - { - int roi_x = stats.at(i, cv::CC_STAT_LEFT); - int roi_y = stats.at(i, cv::CC_STAT_TOP); - int roi_w = stats.at(i, cv::CC_STAT_WIDTH); - int roi_h = stats.at(i, cv::CC_STAT_HEIGHT); - if( (roi_w > obj_size_min) && (roi_w < obj_size_max) && (roi_h > obj_size_min) && (roi_h < obj_size_max)) - { - //ϸĿ - //ȡ - std::vector contourPts; - for (int row = roi_y; row < roi_y + roi_h; row++) - { - for (int col = roi_x; col < roi_x + roi_w; col++) - { - if (labels.at(row, col) == i) - contourPts.push_back(cv::Point(col, row)); - } - } - cv::RotatedRect ellipse = cv::fitEllipse(contourPts); - SG_fittingInfo a_fitting; - a_fitting.fittingPara = ellipse; - //߶Z - double z_sum = 0; - double z_counter = 0; - for (int m = 0; m < contourPts.size(); m++) - { - cv::Vec2i ptIndexing = distTranformIndexing.at(contourPts[m].y, contourPts[m].x); - int line = ptIndexing[0]; - int ptIdx = ptIndexing[1]; - SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[ptIdx]; - if (pt3D->pt3D.z < 1e-4) - continue; - z_sum += pt3D->pt3D.z; - z_counter++; - } - if (z_counter > 0) - { - a_fitting.objCenter.x = ellipse.center.x + roi3D.xRange.min; - a_fitting.objCenter.y = ellipse.center.y + roi3D.yRange.min; - z_sum = z_sum / z_counter; - a_fitting.objCenter.z = z_sum; - a_fitting.fittingPara = ellipse; - fittingObs.push_back(a_fitting); - } - - } - } -#if DEBUG_OUT_IMAGE - cv::Mat fittingImage; - cv::cvtColor(zSliceData_origin, fittingImage, cv::COLOR_GRAY2BGR); - for(int i = 0; i < fittingObs.size(); i ++) - cv::ellipse(fittingImage, fittingObs[i].fittingPara, cv::Scalar(0, 255, 0), 2); // ԭͼϻԲ - cv::imwrite("zSliceImage_fitting.png", fittingImage); -#endif - //ڽӹϵȷץȡ - std::vector< SSG_statorNeighbourInfo> neighbouringInfo; - _getNeighbouringInfo( - positionParam, - zSliceData_origin, //Ѱұ߽磨߿ - fittingObs, //Ŀλ - neighbouringInfo //ڽӹϵ - ); - - //ڽӹϵȷץȡ - std::vector< SSG_motorStatorPosition> grabPoses; //Сڵ3Ŀץȡ㣨 - std::vector< SSG_motorStatorPosition> grabPoses_2; //3Ŀץȡ㣨 - //grabPoses.resize(neighbouringInfo.size()); - for (int i = 0; i < neighbouringInfo.size(); i++) - { - double opAngle = -1; - double obstacleDist = -1; - _computeGripperPose(neighbouringInfo[i], positionParam.gripperR, &opAngle, &obstacleDist); - SSG_motorStatorPosition opPos; - memset(&opPos, 0, sizeof(SSG_motorStatorPosition)); - opPos.neighbourNum = neighbouringInfo[i].neighbours.size(); - opPos.obstacleDist = obstacleDist; - opPos.opAngle = opAngle; - opPos.opCenter = neighbouringInfo[i].objPos3D; - if (opPos.neighbourNum <= 3) - grabPoses.push_back(opPos); - else - grabPoses_2.push_back(opPos); - } - - //ѡȡץȡ㣺1ܱھ٣2ߵľ - std::sort(grabPoses.begin(), grabPoses.end(),compareByNeighbourDist); - std::sort(grabPoses_2.begin(), grabPoses_2.end(), compareByNeighbourNum); - //ϲ - grabPoses.insert(grabPoses.end(), grabPoses_2.begin(), grabPoses_2.end()); - return; -#endif } \ No newline at end of file diff --git a/sourceCode/motorStatorPosition_Export.h b/sourceCode/motorStatorPosition_Export.h index c17c3e0..f0e4df6 100644 --- a/sourceCode/motorStatorPosition_Export.h +++ b/sourceCode/motorStatorPosition_Export.h @@ -13,10 +13,10 @@ typedef struct { double statorOuterD; //ֱ double statorInnerD; //ڿֱ - double statorHeight; - double cutZ_level0; - double cutZ_level1; - double gripperR;//צ뾶 + double statorHeight; //Ӹ߶ + double plateThickness; // + double plateW;//峤 + double plateH;// }SWD_statorParam; typedef struct @@ -26,11 +26,19 @@ typedef struct SSG_treeGrowParam growParam; }SWD_statorPositonParam; +typedef enum +{ + keWD_OPERATE_Uknown = 0, + keSG_OPERATE_TOP_LAYER, //ץȡϲ + keSG_OPERATE_BTM_LAYER, //ץȡ² + keSG_OPERATE_PLATE, //ץȡ +} ESG_OpeationState; + typedef struct { SVzNL3DPoint refPos; - double cuttingZ; //zضֵڷָϰ벿͵ -}SWD_nextOpParam; + ESG_OpeationState opState; //ָʾǰһĿολǷЧ +}SWD_statorGriperState; //汾 SG_APISHARED_EXPORT const char* wd_particleSegVersion(void); @@ -53,11 +61,10 @@ SG_APISHARED_EXPORT void wd_lineDataR( // ͶӰעʱ߿ҲͬʱͶӰ->任->ȡĿ-> // ںͱ߿ѰץȡĿץȡ SG_APISHARED_EXPORT void wd_motorStatorPosition( - std::vector< std::vector>& scanLines, + std::vector< std::vector>& scanLinesInput, const SWD_statorParam statorParam, const SSG_planeCalibPara groundCalibPara, const SWD_statorPositonParam algoParam, - SWD_nextOpParam* refPos, //һθIJολãͬʱһεIJολ + SWD_statorGriperState* opState, //״ָ̬ʾǰ״̬ int* errCode, - std::vector& resultObjPositions, - SWD_statorOuterGrasper& resultGraspPos); \ No newline at end of file + std::vector& resultObjPositions); \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index a1b1c45..9199636 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -461,25 +461,27 @@ int main() pts_eye.resize(6); std::vector pts_robot; pts_robot.resize(6); - pts_eye[0] = { 187.22, -99.43, 1797.70 }; //, R = 0.6413, P = 0.0302, Y = -91.1494 - pts_eye[1] = { 186.50, -140.52, 1797.69 }; // R = 0.6413, P = 0.0302, Y = -88.8130 - pts_eye[2] = { 256.73, -93.55, 1797.83 }; //, R = 0.6413, P = 0.0302, Y = -89.3432 - pts_eye[3] = { 236.69, -48.82, 1797.77 }; // R = 0.6413, P = 0.0302, Y = -89.5285 - pts_eye[4] = { 173.62, 13.92, 1797.30 };// R = 0.6413, P = 0.0302, Y = -89.6450 - pts_eye[5] = { 232.11, -153.38, 1797.88 };// R = 0.6413, P = 0.0302, Y = -82.0278 + pts_eye[0] = { 463.48, 347.63, 1301.43 }; //, R = 0.6413, P = 0.0302, Y = -91.1494 + pts_eye[1] = { 254.15, 259.55, 1303.12 }; // R = 0.6413, P = 0.0302, Y = -88.8130 + pts_eye[2] = { 141.16, 284.02, 1302.27 }; //, R = 0.6413, P = 0.0302, Y = -89.3432 + pts_eye[3] = { 90.66, 5.80, 1303.27 }; // R = 0.6413, P = 0.0302, Y = -89.5285 + pts_eye[4] = { 252.47, -10.45, 1304.33 };// R = 0.6413, P = 0.0302, Y = -89.6450 + pts_eye[5] = { 403.53, -13.99, 1304.24 };// R = 0.6413, P = 0.0302, Y = -82.0278 + //pts_eye[6] = { 242.70, -74.51, 1796.83 }; //A:绕z, B：绕y， C:绕x - pts_robot[0] = { 55.830, -984.472, 97.687 }; // A:0.018 B : -0.638 C : -0.292 - pts_robot[1] = { 95.469, -985.345, 97.782 }; // A:-1.032 B : -0.354 C : 1.001 - pts_robot[2] = { 47.908, -1053.709, 97.802 }; // A:-1.032 B : -0.356 C : 0.998 - pts_robot[3] = { 3.646, -1033.152, 96.494 }; // A:-1.028 B : -0.367 C : 0.442 - pts_robot[4] = { -58.843, -969.793, 95.297 }; // A:-1.029 B : -0.367 C : 0.442 - pts_robot[5] = { 108.031, -1029.803, 98.333 }; // A:-9.208 B : -0.367 C : 0.442 + pts_robot[0] = { -406.649, -1246.187, 588.611 }; // A:0.018 B : -0.638 C : -0.292 + pts_robot[1] = { -316.649, -1036.290, 587.619 }; // A:-1.032 B : -0.354 C : 1.001 + pts_robot[2] = { -340.590, -922.510, 586.731 }; // A:-1.032 B : -0.356 C : 0.998 + pts_robot[3] = { -61.892,-874.564,589.841 }; // A:-1.028 B : -0.367 C : 0.442 + pts_robot[4] = { -46.658,-1036.959,589.820 }; // A:-1.029 B : -0.367 C : 0.442 + pts_robot[5] = { -43.92, -1188.370,591.006 }; // A:-9.208 B : -0.367 C : 0.442 + //pts_robot[6] = { 29.298, -1033.69, 97.909 }; //使用前6组数据 std::vector test_pts_eye; std::vector test_pts_robot; - for (int i = 0; i < 5; i++) + for (int i = 0; i < 6; i++) { test_pts_eye.push_back(pts_eye[i]); test_pts_robot.push_back(pts_robot[i]); @@ -501,21 +503,30 @@ int main() //验算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); + verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.begin()+6); + //cv::Point3d a_center = { 232.997, -173.533, 1795.9 }; + //verify_pts_eye.push_back(a_center); + //a_center = { 225.57, -68.33, 1796.77 }; + //verify_pts_eye.push_back(a_center); + //a_center = { 120.45, -45.97, 1796.25 }; + //verify_pts_eye.push_back(a_center); + //a_center = { 242.70, -74.51, 1796.83 }; + //verify_pts_eye.push_back(a_center); std::vector> pose_eye; - pose_eye.resize(7); - for (int i = 0; i < 7; i++) + pose_eye.resize(6); + for (int i = 0; i < 6; 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 }; - pose_eye[2][0] = { 0.011,-1.000,-0.011 }; pose_eye[2][1] = { 1.000,0.011,-0.000 }; pose_eye[2][2] = { 0.001,-0.011,1.000 }; - 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 }; - 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++) + pose_eye[0][0] = { -0.046,-0.999,-0.009 }; pose_eye[0][1] = { 0.999,-0.046,0.004 }; pose_eye[0][2] = { -0.005,-0.009,1.000 }; + pose_eye[1][0] = { -0.049,-0.999,-0.009 }; pose_eye[1][1] = { 0.999,-0.050,0.004 }; pose_eye[1][2] = { -0.005,-0.009,1.000 }; + pose_eye[2][0] = { -0.049,-0.999,-0.009 }; pose_eye[2][1] = { 0.999,-0.049,0.004 }; pose_eye[2][2] = { -0.005,-0.009,1.000 }; + pose_eye[3][0] = { -0.059,-0.998,-0.009 }; pose_eye[3][1] = { 0.998,-0.059,0.004 }; pose_eye[3][2] = { -0.005,-0.009,1.000 }; + pose_eye[4][0] = { -0.070,-0.997,-0.009 }; pose_eye[4][1] = { 0.998,-0.070,0.004 }; pose_eye[4][2] = { -0.005,-0.009,1.000 }; + pose_eye[5][0] = { -0.068,-0.998,-0.009 }; pose_eye[5][1] = { 0.998,-0.068,0.004 }; pose_eye[5][2] = { -0.005,-0.009,1.000 }; + //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 }; + //pose_eye[7][0] = { 0.058, 0.998, 0.011 }; pose_eye[7][1] = { 0.999, -0.046, 0.003 }; pose_eye[7][2] = { 0.003, 0.011, -1.000 }; + //pose_eye[8][0] = { 0.017,1.000,0.011 }; pose_eye[8][1] = { 1.000,-0.008,0.003 }; pose_eye[8][2] = { 0.003,0.011,-1.000 }; + //pose_eye[9][0] = { -0.316, 0.949, 0.009 }; pose_eye[9][1] = { 0.951, 0.310, 0.007 }; pose_eye[9][2] = { 0.003, 0.011, -1.000 }; + for (int i = 0; i < 6; i++) { cv::Point3d rtPt; pointRT_2(R, T, verify_pts_eye[i], rtPt); //RT前后的点