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

bagPositioning version 1.1.2

Browse Source 
修正了碰撞检测的一个程序Bug
This commit is contained in:
jerryzeng 2026-02-05 15:58:38 +08:00
parent 11b93b92df
commit 94aaf725f9
2 changed files with 52 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
bagPositioning_test/bagPositioning_test.cpp
View File

@ -2530,7 +2530,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 28 #define TEST_GROUP 29
#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()
@ -2675,15 +2675,16 @@ int main()
"F:\\ShangGu\\纸箱拆垛数据\\380×580mm纸箱RGB点云及2d图像\\", //17 "F:\\ShangGu\\纸箱拆垛数据\\380×580mm纸箱RGB点云及2d图像\\", //17
"F:\\ShangGu\\纸箱拆垛数据\\480×580mm纸箱rgb点云及2d图像\\", //18 "F:\\ShangGu\\纸箱拆垛数据\\480×580mm纸箱rgb点云及2d图像\\", //18
"F:\\ShangGu\\编织袋数据\\科一现场\\", //19 "F:\\ShangGu\\编织袋数据\\科一现场\\", //19
"F:/ShangGu/编织袋数据/理想拆包数据/", //20
"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //20 "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //21
"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //21 "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //22
"F:\\ShangGu\\编织袋数据\\侧抓数据\\", //22 "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //23
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //23 "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //24
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //24 "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //25
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //25 "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //26
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //26 "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //27
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //27 "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //28
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
@ -2691,6 +2692,8 @@ int main()
{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,3}, {1,11}, {2,4}, {1,5}, {1,1}, {1,3}, {1,11},
{5,22},{1, 15},{1,15}, {1, 15},{1,7}, {5,22},{1, 15},{1,15}, {1, 15},{1,7},
{1,16},
{1,21},{1,28}, {1,21},{1,28},
{3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84}
}; };
@ -2718,7 +2721,7 @@ int main()
SG_bagPositionParam algoParam; SG_bagPositionParam algoParam;
algoParam.outputMode = 0; //0-按序输出（左上），1-按大小输出（最大） algoParam.outputMode = 0; //0-按序输出（左上），1-按大小输出（最大）
int endGroup = TEST_GROUP - 1; int endGroup = TEST_GROUP - 1;
for (int grp = 0; grp <= 19; grp++) for (int grp = 20; grp <= 20; grp++)
{ {
if (grp < 10) if (grp < 10)
{ {
@ -2891,7 +2894,31 @@ int main()
algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_1 = 60;
algoParam.cornerParam.jumpCornerTh_2 = 15; 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.bagL = 750; //袋子长65cm
algoParam.bagParam.bagW = 450; //袋子宽40cm algoParam.bagParam.bagW = 450; //袋子宽40cm
@ -3026,21 +3053,22 @@ int main()
algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_1 = 60;
algoParam.cornerParam.jumpCornerTh_2 = 15; algoParam.cornerParam.jumpCornerTh_2 = 15;
} }
else if ( grp == 20) else if ( grp == 21)
{ {
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 == 21) else if (grp == 22)
{ {
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");
poseCalibPara = _readCalibPara(calibFile); poseCalibPara = _readCalibPara(calibFile);
} }
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{ {
//fidx = 193; //fidx = 2;
if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 if (grp < TEST_TOP_VIEW_GROUP) //正面抓取
{ {
algoParam.supportRotate = 0; //支持相机与对象之间有一个旋转角度（小于30度） algoParam.supportRotate = 0; //支持相机与对象之间有一个旋转角度（小于30度）
@ -3056,6 +3084,11 @@ int main()
sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx);
laser3DPoints = vzReadXYZPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); 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 else
{ {
sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx);

9
sourceCode/SG_bagPositioning.cpp
View File

@ -7,7 +7,8 @@
//version 1.1.0 : 基准版本基础上添加了（1）版本号（2）输出模式，支持按序输出和按大输出 //version 1.1.0 : 基准版本基础上添加了（1）版本号（2）输出模式，支持按序输出和按大输出
//version 1.1.1 : 增加了错误代码。当托盘为空时，错误代码为 SX_BAG_TRAY_EMPTY，指示托盘为空 //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) const char* sg_bagPositioningVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();
@ -717,7 +718,7 @@ SSG_peakRgnInfo _getRgnObjInfo(
{ {
for (int n = roi.top; n <= roi.bottom; n++) for (int n = roi.top; n <= roi.bottom; n++)
{ {
if ((m == 226) && (n == 742)) if ((m == 663) && (n == 796))
int kkk = 1; int kkk = 1;
SVzNL3DPosition* a_pt = &(laser3DPoints[m].p3DPosition[n]); SVzNL3DPosition* a_pt = &(laser3DPoints[m].p3DPosition[n]);
@ -3913,9 +3914,9 @@ if (i == 3)
for (int i = 0; i < smallObjPeaks.size(); i++) for (int i = 0; i < smallObjPeaks.size(); i++)
{ {
SSG_2DValueI* a_samllPk = &smallObjPeaks[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]; SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y];
if (highest_obj->centerPos.z > smallPkPt->pt3D.z + algoParam.bagParam.bagH / 2)
{
double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2)); 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)); double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2));
//同层比较 //同层比较