diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 5011e8b..68bdaba 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -2429,7 +2429,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() @@ -2518,7 +2518,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; @@ -2541,10 +2541,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); @@ -2567,22 +2567,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; @@ -2604,7 +2605,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) { @@ -2630,7 +2631,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 @@ -2656,8 +2669,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 @@ -2693,13 +2707,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"); @@ -2707,7 +2765,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; @@ -2747,6 +2805,7 @@ int main() //调平，去除地面 sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } + std::vector objOps; sg_getBagPosition(laser3DPoints, lineNum, algoParam, poseCalibPara, objOps); #endif @@ -2832,6 +2891,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]; diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index c52dd4b..abd2023 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -2383,10 +2383,16 @@ void sg_getBagPosition( //ȡ for (int hLine = 0; hLine < hLineNum; hLine++) { -if (hLine == 116) +if (hLine == 14) int kkk = 1; std::vector smoothData; - sg_lineDataSmoothing(hLines[hLine], 5, smoothData); + sg_lineSegSmoothing( + hLines[hLine], + algoParam.cornerParam.minEndingGap, //ֶεYڴ˼Ϊµķֶ + algoParam.cornerParam.minEndingGap_z,//ֶεZڴ˼Ϊµķֶ + 5, + smoothData); + //sg_lineDataSmoothing(hLines[hLine], 5, smoothData); SSG_lineFeature a_hLine_featrues; a_hLine_featrues.lineIdx = hLine; #if BAG_ALGO_USE_CORNER_FEATURE @@ -3214,6 +3220,7 @@ if (col == 586) sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); //ȡPeaks int invlidDistToEdge = 50; //ԵPeakǷǷ㡣 + double minPeakValue = algoParam.bagParam.bagW / 8; std::vector peaks; for (int i = 0; i < dt_peaks.size(); i++) { @@ -3223,7 +3230,8 @@ if (col == 586) int y_diff_0 = dt_peaks[i].y;//ϱ߾ int y_diff_1 = distTransform.rows - dt_peaks[i].y;//±߾ if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) || - (y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge)) + (y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) || + (dt_peaks[i].valueD < minPeakValue)) continue; //distTranformIndexingл @@ -3251,7 +3259,7 @@ if (col == 586) int peakRgnId = 1; for (int i = 0, i_max = peaks.size(); i < i_max; i++) { -if (i == 2) +if (i == 3) int kkk = 1; SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index f68f0c5..e64bd97 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -44,6 +44,13 @@ SG_APISHARED_EXPORT void sg_lineDataSmoothing( std::vector& input, int smoothWin, std::vector& output); +//ֶƽӰֶζ˵ +SG_APISHARED_EXPORT void sg_lineSegSmoothing( + std::vector& input, + double seg_y_deltaTh, //ֶεYڴ˼Ϊµķֶ + double seg_z_deltaTh,//ֶεZڴ˼Ϊµķֶ + int smoothWin, + std::vector& output); //VZ_APISHARED_EXPORT void sg_getLineMeanVar(); SG_APISHARED_EXPORT void sg_getLineLVFeature( diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 5cc63de..6721679 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -148,7 +148,8 @@ typedef struct typedef struct { - double minEndingGap; //ޡڴޣΪ + double minEndingGap; //yޡڴޣΪ + double minEndingGap_z; //zޡڴޣΪ double scale; //㷽ǵĴڱ double cornerTh; //սޣڴޣΪЧյ double jumpCornerTh_1; //жϹսǷΪޡһ޴jumpCornerTh_1һСjumpCornerTh_2ʱ diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 72933bd..cf028ed 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -62,7 +62,10 @@ SSG_meanVar _computeMeanVar(double* data, int size) return a_meanVar; } -void sg_lineDataSmoothing(std::vector& input, int smoothWin, std::vector& output) +void sg_lineDataSmoothing( + std::vector& input, + int smoothWin, + std::vector& output) { output.resize(input.size()); // Ԥ㹻Ŀռ std::copy(input.begin(), input.end(), output.begin()); @@ -91,6 +94,65 @@ void sg_lineDataSmoothing(std::vector& input, int smoothWin, st return; } +//ɨֶ߰νƽ +void sg_lineSegSmoothing( + std::vector& input, + double seg_y_deltaTh, //ֶεYڴ˼Ϊµķֶ + double seg_z_deltaTh,//ֶεZڴ˼Ϊµķֶ + int smoothWin, + std::vector& output) +{ + //output.resize(input.size()); // Ԥ㹻Ŀռ + //std::copy(input.begin(), input.end(), output.begin()); + + int dataSize = input.size(); + //ֶ + std::vector> segs; + std::vector a_seg; + int segStart = -1; + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPosition a_pt = input[i]; + //segж + if (a_pt.pt3D.z > 1e-4) + { + if (segStart < 0) + { + a_seg.push_back(a_pt); + segStart = 1; + } + else // + { + SVzNL3DPosition pre_pt = a_seg.back(); + double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z); + double diff_y = abs(a_pt.pt3D.y - pre_pt.pt3D.y); + if ((diff_y > seg_y_deltaTh) || (diff_z > seg_z_deltaTh)) + { + segs.push_back(a_seg); + a_seg.clear(); + a_seg.push_back(a_pt); + } + else + a_seg.push_back(a_pt); + } + } + else + a_seg.push_back(a_pt); + } + //last + if (a_seg.size() > 0) + segs.push_back(a_seg); + + //ֶƽ + for (int i = 0, i_max = segs.size(); i < i_max; i++) + { + std::vector seg_output; + sg_lineDataSmoothing(segs[i], smoothWin, seg_output); + output.insert(output.end(), seg_output.begin(), seg_output.end()); + } + return; +} + //˳Ⱥ㣺z޷ void sg_lineDataRemoveOutlier(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector& filerData, std::vector& noisePts) { @@ -1094,21 +1156,40 @@ void sg_getLineCornerFeature( line_features->lineIdx = lineIdx; if (lineIdx == 538) int kkk = 1; - //ȥ std::vector< SVzNL3DPosition> vldPts; std::vector segs; + //޸segĶ塣seg˵޵ĵ int segStart = -1, segEnd = -1; for (int i = 0; i < dataSize; i++) { + if ( (lineIdx == 399)&&(i==568)) + int kkk = 1; SVzNL3DPosition a_pt = lineData[i]; a_pt.nPointIdx = i; + if (lineData[i].pt3D.z > 1e-4) + vldPts.push_back(a_pt); + + //segж if (lineData[i].pt3D.z > 1e-4) { if (segStart < 0) segStart = i; + else // + { + SVzNL3DPosition pre_pt = lineData[i-1]; + double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z); + if (diff_z > cornerPara.minEndingGap_z) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + segStart = i; + } + } segEnd = i; - vldPts.push_back(a_pt); } else { @@ -1139,6 +1220,8 @@ void sg_getLineCornerFeature( corners.resize(vldPts.size()); for (int i = 0, i_max = vldPts.size(); i < i_max; i++) { + if ((lineIdx == 399) && (i == 419)) + int kkk = 1; //ǰѰ int pre_i = -1; for (int j = i - 1; j >= 0; j--) @@ -1332,7 +1415,8 @@ void sg_getLineCornerFeature( int idx_1 = curr_seg->start + curr_seg->len - 1; int idx_2 = nxt_seg->start; double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); - if (y_diff < cornerPara.minEndingGap) //ϲ + double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); + if ( (y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //ϲ { int idx_end = nxt_seg->start + nxt_seg->len - 1; nxt_seg->start = curr_seg->start;