diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 94404a2..749d978 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -2530,7 +2530,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData, #define TEST_COMPUTE_GRASP_POINT 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_GROUP 28 +#define TEST_GROUP 29 #define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8) #define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6) int main() @@ -2675,15 +2675,16 @@ int main() "F:\\ShangGu\\纸箱拆垛数据\\380×580mm纸箱RGB点云及2d图像\\", //17 "F:\\ShangGu\\纸箱拆垛数据\\480×580mm纸箱rgb点云及2d图像\\", //18 "F:\\ShangGu\\编织袋数据\\科一现场\\", //19 + "F:/ShangGu/编织袋数据/理想拆包数据/", //20 - "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //20 - "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //21 - "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //22 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //23 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //24 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //25 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //26 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //27 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //21 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //22 + "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //23 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //24 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //25 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //26 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //27 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //28 }; SVzNLRange fileIdx[TEST_GROUP] = { @@ -2691,6 +2692,8 @@ int main() {1,29},{108,135},{0,200}, {1,200}, {1,12}, {2,4}, {1,5}, {1,1}, {1,3}, {1,11}, {5,22},{1, 15},{1,15}, {1, 15},{1,7}, + {1,16}, + {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -2718,7 +2721,7 @@ int main() SG_bagPositionParam algoParam; algoParam.outputMode = 0; //0-按序输出(左上),1-按大小输出(最大) int endGroup = TEST_GROUP - 1; - for (int grp = 0; grp <= 19; grp++) + for (int grp = 20; grp <= 20; grp++) { if (grp < 10) { @@ -2891,7 +2894,31 @@ int main() algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_2 = 15; } - else if ( (grp >= 20) && (grp <= 21)) + else if (grp == 20) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\理想拆包数据\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + + algoParam.bagParam.bagL = 650; + algoParam.bagParam.bagW = 450; // 420; // 320; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 40; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; + } + else if ( (grp >= 21) && (grp <= 22)) { algoParam.bagParam.bagL = 750; //袋子长65cm algoParam.bagParam.bagW = 450; //袋子宽40cm @@ -3026,21 +3053,22 @@ int main() algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_2 = 15; } - else if ( grp == 20) + else if ( grp == 21) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } - else if (grp == 21) + else if (grp == 22) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 193; + //fidx = 2; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { algoParam.supportRotate = 0; //支持相机与对象之间有一个旋转角度(小于30度) @@ -3056,6 +3084,11 @@ int main() sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx); laser3DPoints = vzReadXYZPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); } + else if (grp == 20) + { + sprintf_s(_scan_file, "%s%d_LaserData_Je08A354.txt", dataPath[grp], fidx); + laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + } else { sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index 9f128fa..9458894 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -7,7 +7,8 @@ //version 1.1.0 : ׼汾ˣ1汾ţ2ģʽְ֧Ͱ //version 1.1.1 : ˴롣ΪʱΪ SX_BAG_TRAY_EMPTYָʾΪ -std::string m_strVersion = "1.1.1"; +//version 1.1.2 : ײһBug +std::string m_strVersion = "1.1.2"; const char* sg_bagPositioningVersion(void) { return m_strVersion.c_str(); @@ -717,7 +718,7 @@ SSG_peakRgnInfo _getRgnObjInfo( { for (int n = roi.top; n <= roi.bottom; n++) { -if ((m == 226) && (n == 742)) +if ((m == 663) && (n == 796)) int kkk = 1; SVzNL3DPosition* a_pt = &(laser3DPoints[m].p3DPosition[n]); @@ -3913,9 +3914,9 @@ if (i == 3) for (int i = 0; i < smallObjPeaks.size(); i++) { SSG_2DValueI* a_samllPk = &smallObjPeaks[i]; - if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2) + SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y]; + if (highest_obj->centerPos.z > smallPkPt->pt3D.z + algoParam.bagParam.bagH / 2) { - SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y]; double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2)); double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2)); //ͬȽ