jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新了参数

Browse Source
This commit is contained in:
jerryzeng 2025-07-20 22:56:03 +08:00
parent e31ec0effd
commit cfb412d6cb
1 changed files with 88 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
bagPositioning_test/bagPositioning_test.cpp
View File

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