From 7af730c44f03156a554e152a5d1eb67fc7c5a5a5 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 18 Jul 2025 16:27:34 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E4=B8=B4=E6=97=B6=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_bagPositioning.cpp | 12 ++++- sourceCode/SG_baseAlgo_Export.h | 7 +++ sourceCode/SG_baseDataType.h | 3 +- sourceCode/SG_lineFeature.cpp | 84 ++++++++++++++++++++++++++++++-- 4 files changed, 99 insertions(+), 7 deletions(-) diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index c52dd4b..5fd70c6 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -2386,6 +2386,12 @@ void sg_getBagPosition( if (hLine == 116) int kkk = 1; std::vector 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; @@ -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 32c2087..c07da9c 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -35,6 +35,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..23d9ca2 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,57 @@ 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; + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPosition a_pt = input[i]; + //seg判断 + if (a_pt.pt3D.z > 1e-4) + { + if (a_seg.size() == 0) + a_seg.push_back(a_pt); + 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); + } + } + } + } + //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 +1148,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 +1212,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 +1407,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; From e31ec0effd5d8120003913b89aacdf2acd81b237 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 20 Jul 2025 22:54:30 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=88=86=E6=AE=B5?= =?UTF-8?q?=E5=B9=B3=E6=BB=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_bagPositioning.cpp | 16 ++++++++-------- sourceCode/SG_lineFeature.cpp | 16 ++++++++++++---- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index 5fd70c6..abd2023 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -2383,16 +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_lineSegSmoothing( - // hLines[hLine], - // algoParam.cornerParam.minEndingGap, //分段的Y间隔。大于此间隔，为新的分段 - // algoParam.cornerParam.minEndingGap_z,//分段的Z间隔。大于此间隔，为新的分段 - // 5, - // 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 diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 23d9ca2..cf028ed 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -94,7 +94,7 @@ void sg_lineDataSmoothing( return; } -//对扫描线按分段进行平滑 +//对扫描线按分段进行平滑。 void sg_lineSegSmoothing( std::vector& input, double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段 @@ -102,21 +102,25 @@ void sg_lineSegSmoothing( int smoothWin, std::vector& output) { - output.resize(input.size()); // 预分配足够的空间 - std::copy(input.begin(), input.end(), output.begin()); + //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 (a_seg.size() == 0) + if (segStart < 0) + { a_seg.push_back(a_pt); + segStart = 1; + } else //检查两点距离 { SVzNL3DPosition pre_pt = a_seg.back(); @@ -128,8 +132,12 @@ void sg_lineSegSmoothing( 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) From cfb412d6cb4a7b9929e7e52fb70385a90d2acd7a Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 20 Jul 2025 22:56:03 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=8F=82?= =?UTF-8?q?=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bagPositioning_test/bagPositioning_test.cpp | 110 ++++++++++++++++---- 1 file changed, 88 insertions(+), 22 deletions(-) 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\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\搴曢潰鐐逛簯\\搴曢潰鐐逛簯.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\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\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\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\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\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\", //13 - "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜_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\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\", //14 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜_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\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } - else if (grp == 14) + else if (grp == 15) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜_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];