diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index d785b19..0bf6ff3 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -2426,7 +2426,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData, #define TEST_COMPUTE_GRASP_POINT 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_GROUP 21 +#define TEST_GROUP 22 #define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8) #define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6) int main() @@ -2515,7 +2515,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\底面点云\\底面点云.txt"); + sprintf_s(_calib_datafile, "F:\\ShangGu\\编织袋数据\\山东现场\\调平数据.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -2538,10 +2538,10 @@ int main() } // char calibFile[250]; - sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\山东现场\\ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\LaserLine1_grid_calib.txt"); + sprintf_s(_out_file, "F:\\ShangGu\\编织袋数据\\山东现场\\ground_grid_calib.txt"); _outputScanDataFile_self(_out_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond); printf("%s: calib done!\n", _calib_datafile); @@ -2564,22 +2564,23 @@ int main() "F:\\ShangGu\\编织袋数据\\点云11_盐袋\\", //10 "F:\\ShangGu\\编织袋数据\\点云12_盐袋\\", //11 "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\", //12 - "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //13 - "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //14 - "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //15 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //16 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //17 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //18 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //19 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //20 + "F:\\ShangGu\\编织袋数据\\山东现场\\", //13 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //14 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //15 + "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //16 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //17 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //18 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //19 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //20 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //21 }; SVzNLRange fileIdx[TEST_GROUP] = { {0,176},{1,200},{1,166},{122,141},{1,65}, {1,29},{108,135},{0,200}, {1,200}, {1,12}, - {2,4}, {1,5}, {1,1}, {1,21},{1,28}, - {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, - {1,84} + {2,4}, {1,5}, {1,1}, {1,1}, + {1,21},{1,28}, + {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; SSG_planeCalibPara poseCalibPara; @@ -2601,7 +2602,7 @@ int main() SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 14; grp <= 14; grp++) + for (int grp = 15; grp <= 15; grp++) { if (grp < 10) { @@ -2627,7 +2628,19 @@ int main() algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm } - else if ( (grp >= 13) && (grp <= 14)) + else if (grp ==13) + { + algoParam.bagParam.bagL = 550; //袋子长65cm + algoParam.bagParam.bagW = 400; //袋子宽40cm + algoParam.bagParam.bagH = 100; //袋子高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 + } + else if ( (grp >= 14) && (grp <= 15)) { algoParam.bagParam.bagL = 750; //袋子长65cm algoParam.bagParam.bagW = 450; //袋子宽40cm @@ -2653,8 +2666,9 @@ int main() } #if BAG_ALGO_USE_CORNER_FEATURE algoParam.cornerParam.cornerTh = 30; //45度角 - algoParam.cornerParam.scale = algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; - algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4; + algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4; algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_2 = 15; #else @@ -2690,13 +2704,57 @@ int main() sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } - else if ( grp == 13) + else if (grp == 13) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\山东现场\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + + algoParam.bagParam.bagL = 550; //袋子长65cm + algoParam.bagParam.bagW = 400; //袋子宽40cm + algoParam.bagParam.bagH = 100; //袋子高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 = 30; //45度角 + algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; + + poseCalibPara.planeCalib[0] = 0.999975; + poseCalibPara.planeCalib[1] = -8.27654e-05; + poseCalibPara.planeCalib[2] = 0.00703687; + poseCalibPara.planeCalib[3] = -8.27654e-05; + poseCalibPara.planeCalib[4] = 0.999723; + poseCalibPara.planeCalib[5] = 0.0235198; + poseCalibPara.planeCalib[6] = -0.00703687; + poseCalibPara.planeCalib[7] = -0.0235198; + poseCalibPara.planeCalib[8] = 0.999699; + poseCalibPara.planeHeight = 3010.61; + poseCalibPara.invRMatrix[0] = 0.999975; + poseCalibPara.invRMatrix[1] = -8.27654e-05; + poseCalibPara.invRMatrix[2] = -0.00703687; + poseCalibPara.invRMatrix[3] = -8.27654e-05; + poseCalibPara.invRMatrix[4] = 0.999723; + poseCalibPara.invRMatrix[5] = -0.0235198; + poseCalibPara.invRMatrix[6] = 0.00703687; + poseCalibPara.invRMatrix[7] = 0.0235198; + poseCalibPara.invRMatrix[8] = 0.999699; + + } + else if ( grp == 14) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } - else if (grp == 14) + else if (grp == 15) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt"); @@ -2704,7 +2762,7 @@ int main() } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 10; + //fidx = 22; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { int lineNum = 0; @@ -2744,6 +2802,7 @@ int main() //调平,去除地面 sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } + std::vector objOps; sg_getBagPosition(laser3DPoints, lineNum, algoParam, poseCalibPara, objOps); #endif @@ -2829,6 +2888,13 @@ int main() //调平,去除地面 sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } +#if 0 + char _out_file[256]; + sprintf_s(_out_file, "%sresult\\LaserLine%d_ground_calib.txt", dataPath[grp], fidx); + std::vector nullObjs; + _outputRGBDScanDataFile_RGBD_obj(_out_file, laser3DPoints, lineNum, + lineV, maxTimeStamp, clockPerSecond, nullObjs); +#endif #if 0 //产生一个调平后的2D图像,核对颜色 char _tst_file[256];