From 1aff3306774e813f96b815d4f633046214caf04c Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 22 Jul 2025 22:52:57 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E7=BC=96=E8=AF=91?= =?UTF-8?q?=E4=B8=AD=E7=9A=84warning?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_bagPositioning.cpp | 144 ++++++------ sourceCode/SG_baseFunc.cpp | 48 ++-- sourceCode/SG_clustering.cpp | 6 +- sourceCode/SG_featureGrow.cpp | 56 ++--- sourceCode/SG_lineFeature.cpp | 371 ++++++++++++++++++++++++++++--- sourceCode/SG_regionGrow.cpp | 42 ++-- 6 files changed, 486 insertions(+), 181 deletions(-) diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index abd2023..e9c3703 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -38,7 +38,7 @@ void sg_bagPositioning_lineProc( #if BAG_ALGO_USE_CORNER_FEATURE sg_getLineCornerFeature( filterData.data(), - filterData.size(), + (int)filterData.size(), lineIdx, bagParam.cornerParam, &a_line_features); @@ -58,7 +58,7 @@ void sg_bagPositioning_lineProc( //逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) { - return (cv::Point2f(pt.x*cosTheta-pt.y*sinTheta, pt.x*sinTheta+pt.y*cosTheta)); + return (cv::Point2f((float)(pt.x*cosTheta-pt.y*sinTheta), (float)(pt.x*sinTheta+pt.y*cosTheta))); } bool compareByHeight(const SSG_2DValueI& a, const SSG_2DValueI& b) { @@ -185,7 +185,7 @@ int _computeMaxEmptyLen(std::vector& mask) int maxLen = 0; int emptyLen = 0; int preMaskValue = 0; - for (int i = 0, i_max = mask.size(); i < i_max; i++) + for (int i = 0, i_max = (int)mask.size(); i < i_max; i++) { if (0 == mask[i]) { @@ -249,7 +249,7 @@ bool _validateObj_treeROI( std::vector rightTreeRanges; std::vector innerTreeROI_v; std::vector innerTreeROI_h; - for (int i = 0, i_max = allTreesInfo.size(); i < i_max; i++) + for (int i = 0, i_max = (int)allTreesInfo.size(); i < i_max; i++) { SSG_treeInfo* a_treeInfo = &allTreesInfo[i]; if (a_treeInfo->treeType == 0) @@ -330,7 +330,7 @@ bool _validateObj_treeROI( } //将内部Tree合并 std::vector innerTreeRange_v; - int treeROI_size = innerTreeROI_v.size(); + int treeROI_size = (int)innerTreeROI_v.size(); for (int i = 0; i < treeROI_size; i++) { SSG_ROIRectD* a_roi = &innerTreeROI_v[i]; @@ -353,7 +353,7 @@ bool _validateObj_treeROI( innerTreeRange_v.push_back(a_range); } std::vector innerTreeRange_h; - treeROI_size = innerTreeROI_h.size(); + treeROI_size = (int)innerTreeROI_h.size(); for (int i = 0; i < treeROI_size; i++) { SSG_ROIRectD* a_roi = &innerTreeROI_h[i]; @@ -568,8 +568,8 @@ SSG_peakRgnInfo _getRgnObjInfo( } int line = rgnContour[m].x; - float x = laser3DPoints[line].p3DPosition[rgnContour[m].y].pt3D.x; - float y = laser3DPoints[line].p3DPosition[rgnContour[m].y].pt3D.y; + float x = (float)laser3DPoints[line].p3DPosition[rgnContour[m].y].pt3D.x; + float y = (float)laser3DPoints[line].p3DPosition[rgnContour[m].y].pt3D.y; points.push_back(cv::Point2f(x, y)); } if (points.size() == 0) @@ -708,7 +708,7 @@ if ((m == 226) && (n == 742)) best_pt = a_pt; } } - cv::Point2f r_pt = _rotate2D(cv::Point2f(a_pt->pt3D.x - vertices[0].x, a_pt->pt3D.y - vertices[0].y), sinTheta, cosTheta); + cv::Point2f r_pt = _rotate2D(cv::Point2f((float)(a_pt->pt3D.x - vertices[0].x), (float)(a_pt->pt3D.y - vertices[0].y)), sinTheta, cosTheta); if ((r_pt.x >= 0) && (r_pt.x <= rect.size.width) && (r_pt.y <= 0) && (r_pt.y >= -rect.size.height)) { int existId = (a_pt->nPointIdx >> 8) & 0xff; @@ -1183,8 +1183,8 @@ void _getMatchTable( matchTable.resize(matchSize); for (int m = 0; m < matchSize; m++) matchTable[m].roi.right = -1; //初始化ROI - int size_1 = contours_1.size(); - int size_2 = contours_2.size(); + int size_1 = (int)contours_1.size(); + int size_2 = (int)contours_2.size(); int m = 0; int n = 0; int matchNum = 0; @@ -1363,7 +1363,11 @@ SSG_peakRgnInfo _getPeakRgn( false, &lowLevelFlag_L, &lowLevelFlag_R); - int lowLevelFlag = lowLevelFlag_T + lowLevelFlag_B << 1 + lowLevelFlag_L << 2 + lowLevelFlag_R << 3; + + lowLevelFlag_B <<= 1; + lowLevelFlag_L <<= 2; + lowLevelFlag_R <<= 3; + int lowLevelFlag = lowLevelFlag_T + lowLevelFlag_B + lowLevelFlag_L + lowLevelFlag_R; if (lowLevelFlag > 0) *lowLevelChkFlag = lowLevelFlag; @@ -1399,7 +1403,7 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vectormatchValue + sigma_th; valid_match->roi.right = -1; valid_match->roi.left = 0; - for (int i = 0, i_max = all_contourPairs.size(); i < i_max; i++) + for (int i = 0, i_max = (int)all_contourPairs.size(); i < i_max; i++) { if ((all_contourPairs[i].edgeId_0 == valid_match->id1) && (all_contourPairs[i].edgeId_1 == valid_match->id2)) { @@ -1493,13 +1497,13 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vectormatchValue = -1.0; } ///去除无效匹配 - int TB_size = TB_pairs.size(); + int TB_size = (int)TB_pairs.size(); for (int i = TB_size - 1; i >= 0; i--) { if ( (TB_pairs[i].matchValue < 0) || (TB_pairs[i].id1 == TB_pairs[i].id2)) TB_pairs.erase(TB_pairs.begin() + i); } - TB_size = TB_pairs.size(); + TB_size = (int)TB_pairs.size(); //计算每个匹配的平均距离:LR for (int i = 0; i < LR_pairs.size(); i++) { @@ -1525,13 +1529,13 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vectormatchValue = -1.0; } ///去除无效匹配 - int LR_size = LR_pairs.size(); + int LR_size = (int)LR_pairs.size(); for (int i = LR_size - 1; i >= 0; i--) { if (( LR_pairs[i].matchValue < 0) || (LR_pairs[i].id1 == LR_pairs[i].id2)) LR_pairs.erase(LR_pairs.begin() + i); } - LR_size = LR_pairs.size(); + LR_size = (int)LR_pairs.size(); //四种情况：（1）TB一个， LR一个，（2）TB一个，LR多个，（3）TB多个，LR一个（4）TB多个，LR多个 int edgeId_top = -1, edgeId_btm = -1, edgeId_left = -1, edgeId_right = -1; SVzNLRangeD L_th = {-80, algoParam.bagParam.bagL * 0.25}; @@ -2253,7 +2257,7 @@ void sg_getBagPosition( px, py, interPts); - for (int m = 0, m_max = interPts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++) bwImg.at(interPts[m].y, interPts[m].x) = 1; } } @@ -2297,7 +2301,7 @@ void sg_getBagPosition( #endif int vldRgnSize = (int)((algoParam.bagParam.bagL * algoParam.bagParam.bagW / 2) / (scale * scale)); - for (int rgnid = 0, rgn_max = labelRgns.size(); rgnid < rgn_max; rgnid++) + for (int rgnid = 0, rgn_max = (int)labelRgns.size(); rgnid < rgn_max; rgnid++) { SSG_Region* a_rgn = &labelRgns[rgnid]; if (a_rgn->ptCounter < vldRgnSize) @@ -2398,7 +2402,7 @@ if (hLine == 14) #if BAG_ALGO_USE_CORNER_FEATURE sg_getLineCornerFeature( smoothData.data(), - smoothData.size(), + (int)smoothData.size(), hLine, algoParam.cornerParam, &a_hLine_featrues); @@ -2440,13 +2444,13 @@ if (hLine == 14) v_edgePts_0, v_edgePts_1); //对tree的node的连续性进行插值，保证无空缺 - for (int treeId = 0, tree_max = v_trees.size(); treeId < tree_max; treeId++) + for (int treeId = 0, tree_max = (int)v_trees.size(); treeId < tree_max; treeId++) { SSG_featureTree& a_vTree = v_trees[treeId]; if (a_vTree.treeNodes.size() > 0) { //从后往前处理，这个插值的点插入后不影响继续处理的下标 - for (int nid = a_vTree.treeNodes.size() - 2; nid >= 0; nid--) + for (int nid = (int)a_vTree.treeNodes.size() - 2; nid >= 0; nid--) { SSG_basicFeature1D curr_node = a_vTree.treeNodes[nid]; SSG_basicFeature1D nxt_node = a_vTree.treeNodes[nid+1]; @@ -2520,13 +2524,13 @@ if (hLine == 14) h_edgePts_0, h_edgePts_1); //对tree的node的连续性进行插值，保证无空缺 - for (int treeId = 0, tree_max = h_trees.size(); treeId < tree_max; treeId++) + for (int treeId = 0, tree_max = (int)h_trees.size(); treeId < tree_max; treeId++) { SSG_featureTree& a_hTree = h_trees[treeId]; if (a_hTree.treeNodes.size() > 0) { //从后往前处理，这个插值的点插入后不影响继续处理的下标 - for (int nid = a_hTree.treeNodes.size() - 2; nid >= 0; nid--) + for (int nid = (int)a_hTree.treeNodes.size() - 2; nid >= 0; nid--) { SSG_basicFeature1D curr_node = a_hTree.treeNodes[nid]; SSG_basicFeature1D nxt_node = a_hTree.treeNodes[nid + 1]; @@ -2676,7 +2680,7 @@ if (hLine == 14) px, py, interPts); - for (int m = 0, m_max = interPts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++) distTranformMask.at(interPts[m].y, interPts[m].x) = 1e+6; } } @@ -2744,7 +2748,7 @@ if (hLine == 14) } #else //根据起点的生长树进行标注 int treeID = 1; - for (int i = 0, i_max = vEdge_0_trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vEdge_0_trees.size(); i < i_max; i++) { SSG_featureTree* a_vEdgeTree = &vEdge_0_trees[i]; @@ -2758,7 +2762,7 @@ if (hLine == 14) a_treeInfo.roi = a_vEdgeTree->roi; allTreesInfo.push_back(a_treeInfo); //在原始点云上标记，同时有Mask上标记 - for (int j = 0, j_max = a_vEdgeTree->treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++) { SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j]; if (laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0 @@ -2804,7 +2808,7 @@ if (hLine == 14) } } #else //根据起点的生长树进行标注 - for (int i = 0, i_max = vEdge_1_trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vEdge_1_trees.size(); i < i_max; i++) { SSG_featureTree* a_vEdgeTree = &vEdge_1_trees[i]; @@ -2818,7 +2822,7 @@ if (hLine == 14) a_treeInfo.roi = a_vEdgeTree->roi; allTreesInfo.push_back(a_treeInfo); //在原始点云上标记，同时有Mask上标记 - for (int j = 0, j_max = a_vEdgeTree->treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++) { SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j]; if (laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0 @@ -2865,7 +2869,7 @@ if (hLine == 14) } } #else //根据起点的生长树进行标注 - for (int i = 0, i_max = hEdge_0_trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)hEdge_0_trees.size(); i < i_max; i++) { SSG_featureTree* a_hEdgeTree = &hEdge_0_trees[i]; //记录Tree的信息 @@ -2881,7 +2885,7 @@ if (hLine == 14) a_treeInfo.roi.bottom = a_hEdgeTree->roi.right; allTreesInfo.push_back(a_treeInfo); //在原始点云上标记，同时有Mask上标记 - for (int j = 0, j_max = a_hEdgeTree->treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++) { SSG_basicFeature1D* a_feature = &a_hEdgeTree->treeNodes[j]; if (laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0 @@ -2931,7 +2935,7 @@ if (hLine == 14) } } #else //根据起点的生长树进行标注 - for (int i = 0, i_max = hEdge_1_trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)hEdge_1_trees.size(); i < i_max; i++) { SSG_featureTree* a_hEdgeTree = &hEdge_1_trees[i]; //记录Tree的信息 @@ -2947,7 +2951,7 @@ if (hLine == 14) a_treeInfo.roi.bottom = a_hEdgeTree->roi.right; allTreesInfo.push_back(a_treeInfo); //在原始点云上标记，同时有Mask上标记 - for (int j = 0, j_max = a_hEdgeTree->treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++) { SSG_basicFeature1D* a_feature = &a_hEdgeTree->treeNodes[j]; if (laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0 @@ -2975,7 +2979,7 @@ if (hLine == 14) //treeIndex 5-8预留给迭代后目标的边界：5-上边界， 6-下边界，7-左边界，8-右边界 int hvTreeIdx = treeID; int vTreeStart = treeID; - for (int i = 0, i_max = v_trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) { SSG_featureTree* a_vTree = &v_trees[i]; @@ -2989,7 +2993,7 @@ if (hLine == 14) a_treeInfo.roi = a_vTree->roi; allTreesInfo.push_back(a_treeInfo); //在原始点云上标记，同时有Mask上标记 - for (int j = 0, j_max = a_vTree->treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) { SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j]; if (laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0 @@ -3014,7 +3018,7 @@ if (hLine == 14) } int hTreeStart = hvTreeIdx; ////标注:水平特征 - for (int i = 0, i_max = h_trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) { SSG_featureTree* a_hTree = &h_trees[i]; //记录Tree的信息 @@ -3030,7 +3034,7 @@ if (hLine == 14) a_treeInfo.roi.bottom = a_hTree->roi.right; allTreesInfo.push_back(a_treeInfo); //在原始点云上标记，同时有Mask上标记 - for (int j = 0, j_max = a_hTree->treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) { SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; if (laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0 @@ -3242,7 +3246,7 @@ if (col == 586) } //按照高度排序 std::sort(peaks.begin(), peaks.end(), compareByHeight); - for (int i = 0, i_max = peaks.size(); i < i_max; i++) + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) featureMask.at(peaks[i].y, peaks[i].x)[3] = 1; //peak flag #if 0 @@ -3257,7 +3261,7 @@ if (col == 586) /// 以区域最高点作为种子进行区域生长 std::vector peakRgns; int peakRgnId = 1; - for (int i = 0, i_max = peaks.size(); i < i_max; i++) + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) { if (i == 3) int kkk = 1; @@ -3393,7 +3397,7 @@ if (i == 3) std::vector iter_objs; //将没有处理的Peak点保留 std::vector residualPeaks; - for (int i = 0, i_max = peaks.size(); i < i_max; i++) + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) { SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; @@ -3544,7 +3548,7 @@ if (i == 3) std::vector smallObjPeaks; //记录0.25L * 0.25W的目标，用于碰撞检查 //将最后没有处理的Peak点保留 std::vector residualPeaks; - for (int i = 0, i_max = peaks.size(); i < i_max; i++) + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) { SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; @@ -3697,7 +3701,7 @@ if (i == 3) } //取同高度层的目标 std::vector level0_objs; - for (int i = 0, i_max = peakRgns.size(); i < i_max; i++) + for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++) { double z_diff = peakRgns[i].centerPos.z - maxHeight; if (z_diff < algoParam.bagParam.bagH / 2) //分层 @@ -3707,7 +3711,7 @@ if (i == 3) } peakRgns.clear(); peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end()); - int level0_size = peakRgns.size(); + int level0_size = (int)peakRgns.size(); if (level0_size > 1) //进一步排序，分行 { //取Y最小的目标 @@ -3790,7 +3794,7 @@ if (i == 3) invMatrix[2][0] = poseCalibPara.invRMatrix[6]; invMatrix[2][1] = poseCalibPara.invRMatrix[7]; invMatrix[2][2] = poseCalibPara.invRMatrix[8]; - for (int i = 0, i_max = objOps.size(); i < i_max; i++) + for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++) { SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw}; double pose[3][3]; @@ -3995,7 +3999,7 @@ void sg_getBagPositionAndOrientation( { *errCode = SG_ERR_3D_DATA_INVLD; //释放内存 - for (int i = 0, i_max = cloudPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)cloudPts.size(); i < i_max; i++) free(cloudPts[i].p3DPosition); return; } @@ -4004,13 +4008,13 @@ void sg_getBagPositionAndOrientation( std::vector peakRgn; sg_getBagPosition( &cloudPts[0], - cloudPts.size(), + (int)cloudPts.size(), algoParam, poseCalibPara, peakRgn); //释放内存 - for (int i = 0, i_max = cloudPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)cloudPts.size(); i < i_max; i++) { #if OUTPUT_DEBUG std::vector< SVzNL3DPosition> out_line; @@ -4030,7 +4034,7 @@ void sg_getBagPositionAndOrientation( //将颜色属性转成HSV HSV hsvPattern = RGBtoHSV(rgbColorPattern.r, rgbColorPattern.g, rgbColorPattern.b); //进行颜色统计处理：取centerPos为中心的（0.8*Width, 0.8*Height）的范围内的点进行统计 - int objNum = peakRgn.size(); + int objNum = (int)peakRgn.size(); if (objNum > 0) { //以水平放置的矩阵为标准模型位置，即水平方向为长度，垂直方向为宽度 @@ -4140,7 +4144,7 @@ void sg_getBagPositionAndOrientation( else if (m == 3) int kkk = 1; double rx_abs = abs(rx); - int hist_idx = rx_abs / x_slice[m]; + int hist_idx = (int)(rx_abs / x_slice[m]); if (hist_idx >= RGN_HIST_SIZE / 2) hist_idx = RGN_HIST_SIZE / 2 - 1; if (rx < 0) @@ -4328,7 +4332,7 @@ void sg_sideBagPosition( std::vector< SSG_basicFeature1D> localPkTop; sg_getLineLocalPeaks( filterData.data(), - filterData.size(), + (int)filterData.size(), hLine, (algoParam.bagParam.bagH / 2), localPkBtm, @@ -4376,7 +4380,7 @@ void sg_sideBagPosition( //对top进行过滤 std::vector h_trees_top; - for (int i = 0, i_max = h_trees_top_all.size(); i < i_max; i++) + for (int i = 0, i_max = (int)h_trees_top_all.size(); i < i_max; i++) { double w = h_trees_top_all[i].roi.right - h_trees_top_all[i].roi.left; if (w > algoParam.bagParam.bagW / 2) @@ -4388,7 +4392,7 @@ void sg_sideBagPosition( //确定顶层位置 double topLayer_height = std::numeric_limits::max(); //一个极大值 - for (int i = 0, i_max = h_trees_top.size(); i < i_max; i++) + for (int i = 0, i_max = (int)h_trees_top.size(); i < i_max; i++) { double layer_h = (h_trees_top[i].roi.top + h_trees_top[i].roi.bottom) / 2; if (topLayer_height > layer_h) @@ -4398,7 +4402,7 @@ void sg_sideBagPosition( ///判断层间 ///寻找第一个位置最高的Tree，这个是编织袋的突出部，为一个纺织袋的中间 std::vector hTree_top_first; - for (int i = 0, i_max = h_trees_top.size(); i < i_max; i++) + for (int i = 0, i_max = (int)h_trees_top.size(); i < i_max; i++) { double layer_h = (h_trees_top[i].roi.top + h_trees_top[i].roi.bottom) / 2; double layer_h_diff = layer_h - topLayer_height; @@ -4413,7 +4417,7 @@ void sg_sideBagPosition( std::sort(hTree_top_first.begin(), hTree_top_first.end(), compareByXPos); //寻找与topLayer_height最近的bottom std::vector hTree_btm_first; - for (int i = 0, i_max = h_trees_bottom.size(); i < i_max; i++) + for (int i = 0, i_max = (int)h_trees_bottom.size(); i < i_max; i++) { double layer_h = (h_trees_bottom[i].roi.top + h_trees_bottom[i].roi.bottom) / 2; double layer_h_diff = layer_h - topLayer_height; @@ -4423,11 +4427,11 @@ void sg_sideBagPosition( //标注 int treeId = 1000; - for (int i = 0, i_max = hTree_top_first.size(); i < i_max; i++) + for (int i = 0, i_max = (int)hTree_top_first.size(); i < i_max; i++) { treeId++; SSG_featureTree& a_tree = hTree_top_first[i]; - for (int j = 0, j_max = a_tree.treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++) { int lineIdx = a_tree.treeNodes[j].jumpPos2D.y; int ptIdx = a_tree.treeNodes[j].jumpPos2D.x; @@ -4436,11 +4440,11 @@ void sg_sideBagPosition( } #if 1 treeId = 2000; - for (int i = 0, i_max = hTree_btm_first.size(); i < i_max; i++) + for (int i = 0, i_max = (int)hTree_btm_first.size(); i < i_max; i++) { treeId++; SSG_featureTree& a_tree = hTree_btm_first[i]; - for (int j = 0, j_max = a_tree.treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++) { int lineIdx = a_tree.treeNodes[j].jumpPos2D.y; int ptIdx = a_tree.treeNodes[j].jumpPos2D.x; @@ -4450,13 +4454,13 @@ void sg_sideBagPosition( #endif //生成目标数据 int objId = 1; - for (int i = 0, i_max = hTree_top_first.size(); i < i_max; i++) + for (int i = 0, i_max = (int)hTree_top_first.size(); i < i_max; i++) { SSG_featureTree& a_tree = hTree_top_first[i]; double center_x = (a_tree.roi.left + a_tree.roi.right) / 2; std::vector< SVzNL3DPoint> pkPts; SVzNL3DPoint objPt = { 0,0,0 }; //与center_x最近的点作为目标点。 - for (int j = 0, j_max = a_tree.treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++) { SVzNL3DPoint a_pt = a_tree.treeNodes[j].jumpPos; //a_pt.y = a_tree.treeNodes[j].jumpPos.x; @@ -4484,10 +4488,10 @@ void sg_sideBagPosition( //计算抓取点 SVzNL3DPoint pre_pt = { 0,0,0 }; SVzNL3DPoint post_pt = { 0,0,0 }; - for (int m = 0, m_max = hTree_btm_first.size(); m < m_max; m++) + for (int m = 0, m_max = (int)hTree_btm_first.size(); m < m_max; m++) { SSG_featureTree& a_btm_tree = hTree_btm_first[m]; - int node_size = a_btm_tree.treeNodes.size(); + int node_size = (int)a_btm_tree.treeNodes.size(); if (node_size == 0) continue; @@ -4507,7 +4511,7 @@ void sg_sideBagPosition( } else { - for (int n = 0, n_max = a_btm_tree.treeNodes.size(); n < n_max; n++) + for (int n = 0, n_max = (int)a_btm_tree.treeNodes.size(); n < n_max; n++) { SSG_basicFeature1D& a_node = a_btm_tree.treeNodes[n]; if (a_node.jumpPos.x <= center_x) @@ -4613,7 +4617,7 @@ void sg_getSideBagStackBasePosition( std::vector< SSG_basicFeature1D> jumps; sg_getLineDownJumps( hLines[hLine].data(), - hLines[hLine].size(), + (int)hLines[hLine].size(), hLine, stackBaseParam.jumpTh, jumps); @@ -4643,10 +4647,10 @@ void sg_getSideBagStackBasePosition( //根据长度进行进行配对 std::vector basePairs; std::vector basePairYDist; - for (int i = 0, i_max = h_trees_jumps.size(); i < i_max; i++) + for (int i = 0, i_max = (int)h_trees_jumps.size(); i < i_max; i++) { SSG_featureTree& tree_1 = h_trees_jumps[i]; - int nodeSize = tree_1.treeNodes.size(); + int nodeSize = (int)tree_1.treeNodes.size(); if(nodeSize <2) continue; SSG_basicFeature1D& node1_L = tree_1.treeNodes[0]; @@ -4658,7 +4662,7 @@ void sg_getSideBagStackBasePosition( for (int j = i + 1; j < i_max; j++) { SSG_featureTree& tree_2 = h_trees_jumps[j]; - int nodeSize_2 = tree_2.treeNodes.size(); + int nodeSize_2 = (int)tree_2.treeNodes.size(); if (nodeSize_2 < 2) continue; SSG_basicFeature1D& node2_L = tree_2.treeNodes[0]; @@ -4702,7 +4706,7 @@ void sg_getSideBagStackBasePosition( double minDist = basePairYDist[0]; //挑选一个最佳的匹配 - for (int i = 1, i_max = basePairs.size(); i < i_max; i++) + for (int i = 1, i_max = (int)basePairs.size(); i < i_max; i++) { if (minDist > basePairYDist[i]) { @@ -4720,7 +4724,7 @@ void sg_getSideBagStackBasePosition( //标注 int treeId = 1; SSG_featureTree& a_tree = h_trees_jumps[bestOne.data_0]; - for (int j = 0, j_max = a_tree.treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++) { int lineIdx = a_tree.treeNodes[j].jumpPos2D.y; int ptIdx = a_tree.treeNodes[j].jumpPos2D.x; @@ -4728,7 +4732,7 @@ void sg_getSideBagStackBasePosition( } treeId = 2; a_tree = h_trees_jumps[bestOne.data_1]; - for (int j = 0, j_max = a_tree.treeNodes.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++) { int lineIdx = a_tree.treeNodes[j].jumpPos2D.y; int ptIdx = a_tree.treeNodes[j].jumpPos2D.x; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 083f0ba..66acdd8 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -71,10 +71,10 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector roi.yRange = { 0, -1 }; roi.zRange = { 0, -1 }; - int lineNum = scanLines.size(); + int lineNum = (int)scanLines.size(); for (int line = 0; line < lineNum; line++) { - int nPositionCnt = scanLines[line].size(); + int nPositionCnt = (int)scanLines[line].size(); for (int i = 0; i < nPositionCnt; i++) { SVzNL3DPosition* pt3D = &scanLines[line][i]; @@ -689,7 +689,7 @@ void vzCaculateLaserPlane(std::vector Points3ds, std::vector(2, 1) = yi; A.at(0, 2) = xi; A.at(1, 2) = yi; - A.at(2, 2) = Points3ds.size(); + A.at(2, 2) = (double)((int)Points3ds.size()); B.at(0, 0) = zixi; B.at(1, 0) = ziyi; B.at(2, 0) = zi; @@ -1014,14 +1014,14 @@ SSG_planeCalibPara sg_getPlaneCalibPara( case 2: //下降 if (z_diff > 0) // 上升 { - int pkBtmIdx = pkBtm.size(); + int pkBtmIdx = (int)pkBtm.size(); pkBtmBackIndexing[pre_i] = pkBtmIdx; pkBtm.push_back({ pre_i, pre_data }); _state = 1; } else if (i == (zHistSize - 1)) { - int pkBtmIdx = pkBtm.size(); + int pkBtmIdx = (int)pkBtm.size(); pkBtmBackIndexing[i] = pkBtmIdx; pkBtm.push_back({ i, curr_data }); } @@ -1039,7 +1039,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara( int pntSizeTh = totalPntSize / 10; SSG_intPair* vldPeak = NULL; - for (int i = 0, i_max = pkTop.size(); i < i_max; i++) + for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++) { if (pkTop[i].data_1 > pntSizeTh) { @@ -1102,9 +1102,9 @@ SSG_planeCalibPara sg_getPlaneCalibPara( if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max)) { cv::Point3f a_vldPt; - a_vldPt.x = pt3D->pt3D.x; - a_vldPt.y = pt3D->pt3D.y; - a_vldPt.z = pt3D->pt3D.z; + a_vldPt.x = (float)pt3D->pt3D.x; + a_vldPt.y = (float)pt3D->pt3D.y; + a_vldPt.z = (float)pt3D->pt3D.z; Points3ds.push_back(a_vldPt); } } @@ -1169,7 +1169,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara( //数据进行转换 SVzNLRangeD calibZRange = { 0, -1 }; topZRange = { 0, -1 }; - for (int i = 0, i_max = Points3ds.size(); i < i_max; i++) + for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) { //z if (topZRange.max < topZRange.min) @@ -1185,9 +1185,9 @@ SSG_planeCalibPara sg_getPlaneCalibPara( topZRange.max = Points3ds[i].z; } cv::Point3f a_calibPt; - a_calibPt.x = Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]; - a_calibPt.y = Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]; - a_calibPt.z = Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]; + a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]); + a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]); + a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]); //z if (calibZRange.max < calibZRange.min) { @@ -1237,7 +1237,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( continue; bool isValid = false; - for (int m = 0, m_max = ROIs.size(); m < m_max; m++) + for (int m = 0, m_max = (int)ROIs.size(); m < m_max; m++) { if ((pt3D->pt3D.x >= ROIs[m].xRange.min) && (pt3D->pt3D.x <= ROIs[m].xRange.max) && (pt3D->pt3D.y >= ROIs[m].yRange.min) && (pt3D->pt3D.y <= ROIs[m].yRange.max) && @@ -1251,9 +1251,9 @@ SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( continue; cv::Point3f a_vldPt; - a_vldPt.x = pt3D->pt3D.x; - a_vldPt.y = pt3D->pt3D.y; - a_vldPt.z = pt3D->pt3D.z; + a_vldPt.x = (float)pt3D->pt3D.x; + a_vldPt.y = (float)pt3D->pt3D.y; + a_vldPt.z = (float)pt3D->pt3D.z; Points3ds.push_back(a_vldPt); } } @@ -1317,12 +1317,12 @@ SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( #endif //数据进行转换 SVzNLRangeD calibZRange = { 0, -1 }; - for (int i = 0, i_max = Points3ds.size(); i < i_max; i++) + for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) { cv::Point3f a_calibPt; - a_calibPt.x = Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]; - a_calibPt.y = Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]; - a_calibPt.z = Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]; + a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]); + a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]); + a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]); //z if (calibZRange.max < calibZRange.min) { @@ -1489,9 +1489,9 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8]; if ((groundH > 0) && (z > groundH)) //去除地面 z = 0; - a_pt.x = x; - a_pt.y = y; - a_pt.z = z; + a_pt.x = (float)x; + a_pt.y = (float)y; + a_pt.z = (float)z; a_line->p3DPoint[i] = a_pt; } return; diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index 1dcef43..d54727d 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -16,7 +16,7 @@ void _seedClustering( break; SVzNL3DPosition a_seed = a_cluster[i]; - for (int i = 0, i_max = pts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)pts.size(); i < i_max; i++) { if (pts[i].nPointIdx < 0) continue; @@ -38,7 +38,7 @@ void sg_pointClustering( std::vector>& objClusters //result ) { - int ptSize = pts.size(); + int ptSize = (int)pts.size(); if (ptSize == 0) return; while(pts.size() > 0) @@ -55,7 +55,7 @@ void sg_pointClustering( clusterDist); objClusters.push_back(a_cluster); //保存一个聚类 //将pts中处理过的点去除 - int m_max = pts.size(); + int m_max = (int)pts.size(); for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if(pts[m].nPointIdx < 0) diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index a9724b4..d286208 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -54,7 +54,7 @@ bool _checkTypeMatch(int treeType, int featureType) //没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。 bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector& trees, SSG_treeGrowParam growParam) { - for (int i = 0, i_max = trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) { SSG_featureTree& a_tree = trees[i]; if (TREE_STATE_DEAD == a_tree.treeState) @@ -187,7 +187,7 @@ bool _invalidateVSlopeTrees(SSG_featureTree& a_tree, double minLTypeTreeLen, dou else { //为了防止两端的点的扫描点的高度异常变化， 取中间段进行坡度计算 - int size = a_tree.treeNodes.size(); + int size = (int)a_tree.treeNodes.size(); int pos_0 = size / 4; int pos_1 = size * 3 / 4; SSG_basicFeature1D* node_0 = &(a_tree.treeNodes[pos_0]); @@ -224,7 +224,7 @@ void _growingFromHead(SSG_featureTree* a_tree, std::vector& lin for (int i = head_line-1; i >= 0; i--) { SSG_lineFeature* a_line = &lineFeatures[i]; - int endingNum = a_line->endings.size(); + int endingNum = (int)a_line->endings.size(); bool growFlag = false; for (int m = 0; m < endingNum; m++) @@ -254,7 +254,7 @@ void _growingFromTail(SSG_featureTree* a_tree, std::vector& lin for (int i = tail_line + 1; i < lineFeatures.size(); i++) { SSG_lineFeature* a_line = &lineFeatures[i]; - int endingNum = a_line->endings.size(); + int endingNum = (int)a_line->endings.size(); bool growFlag = false; for (int m = 0; m < endingNum; m++) @@ -294,7 +294,7 @@ void _getTreeROI(SSG_featureTree* a_tree) a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x; a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y; a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y; - for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++) + for (int i = 1, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++) { if(a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x) a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x; @@ -312,7 +312,7 @@ void _getTreeROI(SSG_featureTree* a_tree) SSG_featureTree* _getIncludingBigTree(SSG_featureTree* a_tree, std::vector& trees, bool chk_up) { SSG_featureTree* _bigTree = NULL; - for (int i = 0, i_max = trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) { SSG_featureTree* chk_tree = &trees[i]; if (a_tree == chk_tree) @@ -361,12 +361,12 @@ void sg_getFeatureGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam) { - for (int i = 0, i_max = lineFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++) { SSG_lineFeature& a_line = lineFeatures[i]; if (a_line.features.size() > 0) { - for (int j = 0, j_max = a_line.features.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_line.features.size(); j < j_max; j++) { SSG_basicFeature1D& a_feature = a_line.features[j]; if (a_feature.jumpPos2D.x == 207) @@ -388,7 +388,7 @@ void sg_getFeatureGrowingTrees( //检查停止生长的树,加速。 //将生长节点为1的生长树移除 int lineIdx = a_line.lineIdx; - int m_max = trees.size(); + int m_max = (int)trees.size(); for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if (TREE_STATE_ALIVE == trees[m].treeState) @@ -416,7 +416,7 @@ void sg_lineFeaturesGrowing( { if (features.size() > 0) { - for (int j = 0, j_max = features.size(); j < j_max; j++) + for (int j = 0, j_max = (int)features.size(); j < j_max; j++) { SSG_basicFeature1D& a_feature = features[j]; if (a_feature.jumpPos2D.x== 207) @@ -437,7 +437,7 @@ void sg_lineFeaturesGrowing( } //检查停止生长的树,加速。 //将生长节点为1的生长树移除 - int m_max = trees.size(); + int m_max = (int)trees.size(); for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if (TREE_STATE_ALIVE == trees[m].treeState) @@ -463,7 +463,7 @@ void sg_getEndingGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam) { - for (int i = 0, i_max = lineEndings.size(); i < i_max; i++) + for (int i = 0, i_max = (int)lineEndings.size(); i < i_max; i++) { if (i == 175) int kkk = 1; @@ -496,7 +496,7 @@ void sg_getEndingGrowingTrees( //检查停止生长的树,加速。 //将生长节点为1的生长树移除 int lineIdx = an_ending.x; - int m_max = trees.size(); + int m_max = (int)trees.size(); for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if (TREE_STATE_ALIVE == trees[m].treeState) @@ -531,14 +531,14 @@ void sg_LVFeatureGrowing( growParam); //迭代一次：重新对每棵树头尾检查，看是否可以继续生长到LINE_EDGE - for (int i = 0, i_max = trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) { SSG_featureTree* a_tree = &trees[i]; //Tree类型重新统计确定。 std::vector typeStat; typeStat.resize(LINE_FEATURE_NUM); - for (int m = 0, m_max = a_tree->treeNodes.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_tree->treeNodes.size(); m < m_max; m++) { int type = a_tree->treeNodes[m].featureType; typeStat[type]++; @@ -564,7 +564,7 @@ void sg_LVFeatureGrowing( ///两种过滤方法： ///（1）针对SLOPE和V型生长树，检查生长方向和垂直方向的比例。 ///（2）宏观检查：检查短的生长树是否被长的生长树所包含 - int tree_size = trees.size(); + int tree_size = (int)trees.size(); for (int m = tree_size - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { SSG_featureTree* a_tree = &trees[m]; @@ -583,7 +583,7 @@ void sg_LVFeatureGrowing( } } } - tree_size = trees.size(); + tree_size = (int)trees.size(); for (int m = tree_size - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if (m == 24) @@ -614,10 +614,10 @@ if (m == 24) std::vector edge1_pts; std::vector edge0_y; std::vector edge1_y; - for (int i = 0, i_max = lineFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++) { SSG_lineFeature& a_line = lineFeatures[i]; - int a_line_endingNum = a_line.endings.size(); + int a_line_endingNum = (int)a_line.endings.size(); if (a_line_endingNum > 0) { for (int m = 0; m < a_line_endingNum; m++) @@ -672,7 +672,7 @@ void sg_getPeakGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam) { - for (int i = 0, i_max = peakFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)peakFeatures.size(); i < i_max; i++) { int lineIdx = i; if (i == 761) @@ -680,7 +680,7 @@ if (i == 761) std::vector< SSG_basicFeature1D>& a_linePeak = peakFeatures[i]; if (a_linePeak.size() > 0) { - for (int j = 0, j_max = a_linePeak.size(); j < j_max; j++) + for (int j = 0, j_max = (int)a_linePeak.size(); j < j_max; j++) { SSG_basicFeature1D& a_feature = a_linePeak[j]; if (a_feature.jumpPos2D.x == 207) @@ -701,7 +701,7 @@ if (i == 761) } //检查停止生长的树,加速。 //将生长节点为1的生长树移除 - int m_max = trees.size(); + int m_max = (int)trees.size(); for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if (TREE_STATE_ALIVE == trees[m].treeState) @@ -732,7 +732,7 @@ void sg_peakFeatureGrowing( trees, growParam); - for (int i = 0, i_max = trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) { SSG_featureTree* a_tree = &trees[i]; _getTreeROI(a_tree); @@ -789,7 +789,7 @@ int _featureGrowing_semiCircle( if ((FEATURE_FLAG_INVLD_START == a_feature.flag) || (FEATURE_FLAG_VALID_START == a_feature.flag)) return -1; - for (int i = 0, i_max = trees.size(); i < i_max; i++) + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) { SSG_semiCircleFeatureTree& a_tree = trees[i]; if (TREE_STATE_DEAD == a_tree.treeState) @@ -828,7 +828,7 @@ void sg_getFeatureGrowingTrees_semiCircle( std::vector& invalidTrees, SSG_treeGrowParam growParam) { - for (int i = 0, i_max = lineFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++) { SSG_featureSemiCircle& a_feature = lineFeatures[i]; if (FEATURE_FLAG_INVALID == a_feature.flag) @@ -854,7 +854,7 @@ void sg_getFeatureGrowingTrees_semiCircle( double minDist = abs(a_feature.midPt.y - last_node.midPt.y); int bestMatch = i; //取最小的匹配 - for (int m = 0, m_max = otherMatching.size(); m < m_max; m++) + for (int m = 0, m_max = (int)otherMatching.size(); m < m_max; m++) { int idx = otherMatching[m]; double y_diff = abs(lineFeatures[idx].midPt.y - last_node.midPt.y); @@ -865,7 +865,7 @@ void sg_getFeatureGrowingTrees_semiCircle( } } otherMatching.push_back(i); - for (int m = 0, m_max = otherMatching.size(); m < m_max; m++) + for (int m = 0, m_max = (int)otherMatching.size(); m < m_max; m++) { int idx = otherMatching[m]; if (bestMatch == idx) @@ -896,7 +896,7 @@ void sg_getFeatureGrowingTrees_semiCircle( } //检查停止生长的树,加速。 //将生长节点为1的生长树移除 - int m_max = trees.size(); + int m_max = (int)trees.size(); for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环 { if (TREE_STATE_ALIVE == trees[m].treeState) diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index cf028ed..fc7c9e0 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -70,7 +70,7 @@ void sg_lineDataSmoothing( output.resize(input.size()); // 预分配足够的空间 std::copy(input.begin(), input.end(), output.begin()); - int size = input.size(); + int size = (int)input.size(); for (int i = 0; i < size; i++) { if (input[i].pt3D.z > 1e-4) @@ -105,7 +105,7 @@ void sg_lineSegSmoothing( //output.resize(input.size()); // 预分配足够的空间 //std::copy(input.begin(), input.end(), output.begin()); - int dataSize = input.size(); + int dataSize = (int)input.size(); //计算分段 std::vector> segs; std::vector a_seg; @@ -144,7 +144,7 @@ void sg_lineSegSmoothing( segs.push_back(a_seg); //分段平滑 - for (int i = 0, i_max = segs.size(); i < i_max; i++) + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) { std::vector seg_output; sg_lineDataSmoothing(segs[i], smoothWin, seg_output); @@ -196,7 +196,7 @@ if (i == 370) if(a_run.len > 0) continueRuns.push_back(a_run); - for (int i = 0, i_max = continueRuns.size(); i < i_max; i++) + for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++) { if (continueRuns[i].len < filterParam.outlierTh) //噪声 { @@ -253,7 +253,7 @@ void sg_lineDataRemoveOutlier_changeOriginData( if (a_run.len > 0) continueRuns.push_back(a_run); - for (int i = 0, i_max = continueRuns.size(); i < i_max; i++) + for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++) { if (continueRuns[i].len < filterParam.outlierTh) //噪声 { @@ -329,7 +329,7 @@ void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSi if (a_run.len > 0) continueRuns.push_back(a_run); - for (int i = 0, i_max = continueRuns.size(); i < i_max; i++) + for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++) { if (continueRuns[i].len < filterParam.outlierTh) //噪声 { @@ -721,7 +721,7 @@ void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, c //首先检查L型边界：本身具有足够坡度，与下一段形成明显坡度差 //再检查是否可以形成V型特征 - for (int i = 0, i_max = slopes.size(); i < i_max; i++) + for (int i = 0, i_max = (int)slopes.size(); i < i_max; i++) { SSG_slope* a_slope = &slopes[i]; if (a_slope->flag > 0) @@ -1093,7 +1093,7 @@ void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, c } } //检查连续的JUMP是否可能合并 - int i = line_features->features.size() - 2; + int i = (int)line_features->features.size() - 2; while(i >= 0) { if (i < ((int)line_features->features.size() - 1)) @@ -1218,7 +1218,7 @@ void sg_getLineCornerFeature( //计算前向角和后向角 std::vector< SSG_pntDirAngle> corners; corners.resize(vldPts.size()); - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) { if ((lineIdx == 399) && (i == 419)) int kkk = 1; @@ -1279,7 +1279,7 @@ void sg_getLineCornerFeature( SSG_pntDirAngle* pre_data = NULL; std::vector< SSG_pntDirAngle> cornerPeakP; std::vector< SSG_pntDirAngle> cornerPeakM; - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) { if (i == 275) int kkk = 1; @@ -1346,6 +1346,309 @@ void sg_getLineCornerFeature( } //注意：最后一个不处理，为基座位置 + //极小值点（峰顶） + //极值比较，在尺度窗口下寻找局部极值点 + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。 + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //向前搜索 + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //超出尺度窗口 + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + //向后搜索 + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //超出尺度窗口 + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + SSG_basicFeature1D a_feature; + if( (cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else + a_feature.featureType = LINE_FEATURE_CORNER_V; + + + a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx }; + line_features->features.push_back(a_feature); + } + } + + //添加开始和结束边界 + //检查seg是否需要合并；向后合并 + for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++) + { + SSG_RUN* nxt_seg = &segs[i + 1]; + SSG_RUN* curr_seg = &segs[i]; + + 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); + 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; + nxt_seg->len = idx_end - curr_seg->start + 1; + curr_seg->value = 0; + } + } + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + if (0 == segs[i].value) //被合并 + continue; + + int idx_1 = segs[i].start; + int idx_2 = segs[i].start + segs[i].len - 1; + + SSG_basicFeature1D an_edge; + memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); + an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; + an_edge.jumpPos = lineData[idx_1].pt3D; + an_edge.jumpPos2D = { lineIdx, idx_1 }; + line_features->endings.push_back(an_edge); + //line_features.insert(line_features.begin(), an_edge); //头部 + //尾部 + an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; + an_edge.jumpPos = lineData[idx_2].pt3D; + an_edge.jumpPos2D = { lineIdx, idx_2 }; + line_features->endings.push_back(an_edge); + } + return; +} + +#if 0 +void sg_getLineCorners( + std::vector& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 + SSG_lineFeature* line_features) +{ + line_features->lineIdx = lineIdx; + if (lineIdx == 538) + int kkk = 1; + //去除零点 + std::vector< SVzNL3DPosition> vldPts; + std::vector segs; + //修改seg的定义。seg端点是两点间距离大于门限的点 + int segStart = -1, segEnd = -1; + int dataSize = + 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; + } + else + { + if (segStart >= 0) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + segStart = -1; + segEnd = -1; + } + } + } + //last + if (segStart >= 0) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + } + + //计算前向角和后向角 + std::vector< SSG_pntDirAngle> corners; + 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--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //后向寻找 + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //计算拐角 + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号 + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + } + + //搜索拐角极值 + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //最后一个 + { + if (1 == _state) //上升 + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //下降 + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //初态 + if (cornerDiff < 0) //下降 + { + _state = 2; + } + else if (cornerDiff > 0) //上升 + { + _state = 1; + } + break; + case 1: //上升 + if (cornerDiff < 0) //下降 + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //下降 + if (cornerDiff > 0) // 上升 + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //注意：最后一个不处理，为基座位置 + //极小值点（峰顶） //极值比较，在尺度窗口下寻找局部极值点 double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。 @@ -1391,7 +1694,7 @@ void sg_getLineCornerFeature( if (true == isPeak) { SSG_basicFeature1D a_feature; - if( (cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) + if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; @@ -1416,7 +1719,7 @@ void sg_getLineCornerFeature( int idx_2 = nxt_seg->start; double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); - if ( (y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_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; @@ -1428,7 +1731,7 @@ void sg_getLineCornerFeature( { if (0 == segs[i].value) //被合并 continue; - + int idx_1 = segs[i].start; int idx_2 = segs[i].start + segs[i].len - 1; @@ -1447,7 +1750,7 @@ void sg_getLineCornerFeature( } return; } - +#endif /// /// 提取激光线上的极值点（极大值点和极小值点） /// @@ -1539,7 +1842,7 @@ void sg_getLineLocalPeaks( //极小值点（峰顶） //极值比较，在尺度窗口下寻找局部极值点 double square_distTh = scaleWin * scaleWin; - for (int i = 0, i_max = pkTop.size(); i < i_max; i++) + for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++) { bool isPeak = true; //向前搜索 @@ -1581,7 +1884,7 @@ void sg_getLineLocalPeaks( } } //极大值点（谷底） - for (int i = 0, i_max = pkBtm.size(); i < i_max; i++) + for (int i = 0, i_max = (int)pkBtm.size(); i < i_max; i++) { bool isPeak = true; //向前搜索 @@ -1921,7 +2224,7 @@ int _findSegMaxZPt( double* meanZ, bool* foundJump) { - int dataSize = lineData.size(); + int dataSize = (int)lineData.size(); double start_y = lineData[searchStart].pt3D.y; double start_z = lineData[searchStart].pt3D.z; if (start_z < 1e-4) @@ -1978,7 +2281,7 @@ void sg_getFlatLineLocalPeaks_vector( const double holeR, std::vector< SSG_basicFeature1D>& localMax) { - int dataSize = lineData.size(); + int dataSize = (int)lineData.size(); if (dataSize < 2) return; @@ -1996,7 +2299,7 @@ void sg_getFlatLineLocalPeaks_vector( sampleData.push_back(a_sample); } - sampleNum = sampleData.size(); + sampleNum = (int)sampleData.size(); std::vector< _FeatureInfo> allSubFeatures; allSubFeatures.resize(sampleNum); for (int i = 0; i < sampleNum; i++) //初始化 @@ -2160,7 +2463,7 @@ void sg_getFlatLineLocalPeaks_vector( featureBuffer.resize(vldFeatures.size()); for (int i = 0; i < vldFeatures.size(); i++) //初始化 featureBuffer[i].featureType = LINE_FEATURE_UNDEF; - for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldFeatures.size(); i < i_max; i++) { if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) { @@ -2320,7 +2623,7 @@ void sg_getFlatLineLocalPeaks_vector( } } //对SLOPE进行配对 - for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldFeatures.size(); i < i_max; i++) { if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_L2H) { @@ -2410,7 +2713,7 @@ void sg_getFlatLineLocalPeaks_vector( } } - for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldFeatures.size(); i < i_max; i++) { if (featureBuffer[i].featureType != LINE_FEATURE_UNDEF) { @@ -2586,7 +2889,7 @@ void sg_getLineUpperSemiCircleFeature( segs.push_back(a_run); } //检查seg是否需要合并；向后合并 - for (int i = 0, i_max = segs.size(); i < i_max - 1; i++) + for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++) { SSG_RUN* nxt_seg = &segs[i + 1]; SSG_RUN* curr_seg = &segs[i]; @@ -2610,7 +2913,7 @@ void sg_getLineUpperSemiCircleFeature( endingFlag.resize(dataSize); for (int i = 0; i < dataSize; i++) endingFlag[i] = 0; - for (int i = 0,i_max=segs.size(); i < i_max; i++) + for (int i = 0,i_max= (int)segs.size(); i < i_max; i++) { if (0 == segs[i].value) //被合并 continue; @@ -2629,12 +2932,12 @@ void sg_getLineUpperSemiCircleFeature( slopePara.LSlopeZWin, segMax, segMin); - for (int m = 0, m_max = segMax.size(); m < m_max; m++) + for (int m = 0, m_max = (int)segMax.size(); m < m_max; m++) { segMax[m].jumpPos2D.y += idx_1; localMax.push_back(segMax[m]); } - for (int m = 0, m_max = segMin.size(); m < m_max; m++) + for (int m = 0, m_max = (int)segMin.size(); m < m_max; m++) { segMin[m].jumpPos2D.y += idx_1; localMin.push_back(segMin[m]); @@ -2644,7 +2947,7 @@ void sg_getLineUpperSemiCircleFeature( //计算slope的Z变化 std::vector< SSG_zWin> slopes; //记录窗口长度内的z变化 slopes.resize(vldPts.size()); - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) { //后向寻找 int post_i = -1; @@ -2683,7 +2986,7 @@ void sg_getLineUpperSemiCircleFeature( std::vector< SSG_zWin> zWinPeakM; //负的zWin SSG_zWin* maxPkPlus = NULL; SSG_zWin* maxPkMinus = NULL; - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) { if (i == 275) int kkk = 1; @@ -2802,7 +3105,7 @@ void sg_getLineUpperSemiCircleFeature( //极值比较，在尺度窗口下寻找局部极值点 std::vector validZWinPeakP; double square_distTh = 4 * slopePara.LSlopeZWin * slopePara.LSlopeZWin; //2倍的cornerScale。 - for (int i = 0, i_max = zWinPeakP.size(); i < i_max; i++) + for (int i = 0, i_max = (int)zWinPeakP.size(); i < i_max; i++) { if (zWinPeakP[i].zDiff < slopePara.validSlopeH) continue; @@ -2855,7 +3158,7 @@ void sg_getLineUpperSemiCircleFeature( //极值比较，在尺度窗口下寻找局部极值点 std::vector validZWinPeakM; - for (int i = 0, i_max = zWinPeakM.size(); i < i_max; i++) + for (int i = 0, i_max = (int)zWinPeakM.size(); i < i_max; i++) { if (zWinPeakM[i].zDiff > -slopePara.validSlopeH) continue; @@ -2908,7 +3211,7 @@ void sg_getLineUpperSemiCircleFeature( //对validCornerPeakP进行配对 std::vector< SSG_featureSemiCircle> line_features; - for (int i = 0, i_max = localMin.size(); i < i_max; i++) + for (int i = 0, i_max = (int)localMin.size(); i < i_max; i++) { int ptIdx = localMin[i].jumpPos2D.y; if (endingFlag[ptIdx] > 0) //已经被处理 @@ -2952,7 +3255,7 @@ void sg_getLineUpperSemiCircleFeature( a_feature.lineIdx = lineIdx; a_feature.midPtIdx = _getYNearestPtIdx(lineData, preIdx, postIdx, midY); a_feature.midPt = lineData[a_feature.midPtIdx].pt3D; - if (a_feature.midPtIdx >= 0); + if (a_feature.midPtIdx >= 0) { int pkIdx = _getSegPeak(lineData, preIdx, postIdx); a_feature.pkHeight = lineData[pkIdx].pt3D.z; @@ -2998,7 +3301,7 @@ void sg_getLineUpperSemiCircleFeature( } //根据孔洞目标进行合并 - for (int i = 0, i_max = line_features.size(); i < i_max; i++) + for (int i = 0, i_max = (int)line_features.size(); i < i_max; i++) { if (FEATURE_FLAG_INVALID == line_features[i].flag) continue; @@ -3031,8 +3334,6 @@ void sg_getLineUpperSemiCircleFeature( return; } - - //计算检查长度时使用街区距离以避免太多的平方和开方运算 //如果z方向为Jump返回True。Jump定义Seed前一个点z的变化大于门限 bool _getPtPreMaxDelta( diff --git a/sourceCode/SG_regionGrow.cpp b/sourceCode/SG_regionGrow.cpp index fe446db..0f615fd 100644 --- a/sourceCode/SG_regionGrow.cpp +++ b/sourceCode/SG_regionGrow.cpp @@ -127,7 +127,7 @@ void _getRgnVldContour(std::vector< SSG_2DValueI>& contourPts, std::vector< SSG_ //游程分析 std::vector< SSG_RUN> contourRuns; SSG_RUN a_run = { -1,-1,-1}; - for (int i = 0, i_max = contourPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++) { SSG_2DValueI* a_pt = &contourPts[i]; if (a_pt->x < 0) @@ -172,7 +172,7 @@ void _getRgnVldContour(std::vector< SSG_2DValueI>& contourPts, std::vector< SSG_ if (contourRuns.size() > 0) { //保留长的边界，小的短的边界可能是噪声 - for (int i = 0, i_max = contourRuns.size(); i < i_max; i++) + for (int i = 0, i_max = (int)contourRuns.size(); i < i_max; i++) { SSG_RUN* curr_run = &contourRuns[i]; if(curr_run->len > noiseRunLenTh) @@ -180,7 +180,7 @@ void _getRgnVldContour(std::vector< SSG_2DValueI>& contourPts, std::vector< SSG_ idIndexing[curr_run->value] = 1; } } - for (int i = 0, i_max = contourPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++) { SSG_2DValueI a_pt = contourPts[i]; if (a_pt.x < 0) @@ -222,10 +222,10 @@ void sg_getContourPts( //生成有效contour //统计block(遮挡)的比例 int blockPtNum = 0; - for (int m = 0, m_max = contour_all.size(); m < m_max; m++) + for (int m = 0, m_max = (int)contour_all.size(); m < m_max; m++) { std::vector& a_line_contourPts = contour_all[m].contourPts; - for (int i = 0, i_max = a_line_contourPts.size(); i < i_max; i++) + for (int i = 0, i_max = (int)a_line_contourPts.size(); i < i_max; i++) { SSG_contourPtInfo* a_contourPt = &a_line_contourPts[i]; int an_edgeIdx = a_contourPt->edgeId; @@ -260,10 +260,10 @@ void sg_getPairingContourPts( //统计block(遮挡)的比例 int blockPtNum_0 = 0; int blockPtNum_1 = 0; - for (int m = 0, m_max = contourPairs.size(); m < m_max; m++) + for (int m = 0, m_max = (int)contourPairs.size(); m < m_max; m++) { SSG_conotourPair& a_ptPair = contourPairs[m]; - for (int i = 0, i_max = idPairs.size(); i < i_max; i++) + for (int i = 0, i_max = (int)idPairs.size(); i < i_max; i++) { SSG_intPair& a_idPair = idPairs[i]; if ( (a_ptPair.edgeId_0 == a_idPair.data_0) && @@ -294,7 +294,7 @@ void sg_getPairingContourPts( } } } - int totalNum = contourFilter.size(); + int totalNum = (int)contourFilter.size(); if(blockPtNum_0 > totalNum/2) *lowLevelFlag_0 = 1; if (blockPtNum_1 > totalNum / 2) @@ -314,7 +314,7 @@ void sg_contourPostProc(std::vector< SSG_contourPtInfo>& contour, int maxEdgeIdx std::vector< SSG_contourEdgeInfo> edgeInfo; edgeInfo.resize(maxEdgeIdx + 1); std::vector egdeIndexing; - for (int i = 0, i_max = contour.size(); i < i_max; i++) + for (int i = 0, i_max = (int)contour.size(); i < i_max; i++) { SSG_contourPtInfo* a_contourPt = &contour[i]; int edgeIdx = a_contourPt->edgeId; @@ -372,7 +372,7 @@ void sg_contourPostProc(std::vector< SSG_contourPtInfo>& contour, int maxEdgeIdx double dist_diff = scanDist - minScanDist; if (dist_diff < sameConturDistTh) { - for (int m = 0, m_max = contour.size(); m < m_max; m++) + for (int m = 0, m_max = (int)contour.size(); m < m_max; m++) { SSG_contourPtInfo* a_contourPt = &contour[m]; int an_edgeIdx = a_contourPt->edgeId; @@ -945,7 +945,7 @@ if (x == 0) break; else { - for (int m = 0, m_max = a_line_upEdgePts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_upEdgePts.size(); m < m_max; m++) { if (*maxEdgeId_top < a_line_upEdgePts[m].edgeId) *maxEdgeId_top = a_line_upEdgePts[m].edgeId; @@ -956,7 +956,7 @@ if (x == 0) topContour.insert(topContour.begin(), _line_upContours); } - for (int m = 0, m_max = a_line_downEdgePts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_downEdgePts.size(); m < m_max; m++) { if (*maxEdgeId_btm < a_line_downEdgePts[m].edgeId) *maxEdgeId_btm = a_line_downEdgePts[m].edgeId; @@ -1012,7 +1012,7 @@ if (x == 34) break; else { - for (int m = 0, m_max = a_line_upEdgePts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_upEdgePts.size(); m < m_max; m++) { if (*maxEdgeId_top < a_line_upEdgePts[m].edgeId) *maxEdgeId_top = a_line_upEdgePts[m].edgeId; @@ -1023,7 +1023,7 @@ if (x == 34) topContour.push_back(_line_upContours); } - for (int m = 0, m_max = a_line_downEdgePts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_downEdgePts.size(); m < m_max; m++) { if (*maxEdgeId_btm < a_line_downEdgePts[m].edgeId) *maxEdgeId_btm = a_line_downEdgePts[m].edgeId; @@ -1083,7 +1083,7 @@ if (y == 380) break; else { - for (int m = 0, m_max = a_line_leftPts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_leftPts.size(); m < m_max; m++) { if (*maxEdgeId_left < a_line_leftPts[m].edgeId) *maxEdgeId_left = a_line_leftPts[m].edgeId; @@ -1093,7 +1093,7 @@ if (y == 380) SSG_lineConotours _line_leftContours = { y, a_line_leftPts }; leftContour.insert(leftContour.begin(), _line_leftContours); } - for (int m = 0, m_max = a_line_rightPts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_rightPts.size(); m < m_max; m++) { if (*maxEdgeId_right < a_line_rightPts[m].edgeId) *maxEdgeId_right = a_line_rightPts[m].edgeId; @@ -1149,7 +1149,7 @@ if (y == 380) break; else { - for (int m = 0, m_max = a_line_leftPts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_leftPts.size(); m < m_max; m++) { if (*maxEdgeId_left < a_line_leftPts[m].edgeId) *maxEdgeId_left = a_line_leftPts[m].edgeId; @@ -1159,7 +1159,7 @@ if (y == 380) SSG_lineConotours _line_leftContours = { y, a_line_leftPts }; leftContour.push_back( _line_leftContours); } - for (int m = 0, m_max = a_line_rightPts.size(); m < m_max; m++) + for (int m = 0, m_max = (int)a_line_rightPts.size(); m < m_max; m++) { if (*maxEdgeId_right < a_line_rightPts[m].edgeId) *maxEdgeId_right = a_line_rightPts[m].edgeId; @@ -1179,11 +1179,11 @@ float EDistance(int x1, int y1, int x2, int y2) } float MDistance(int x1, int y1, int x2, int y2) { - return abs(x1 - x2) + abs(y1 - y2); + return (float)abs(x1 - x2) + (float)abs(y1 - y2); } float CDistance(int x1, int y1, int x2, int y2) { - return std::max(abs(x1 - x2), abs(y1 - y2)); + return std::max((float)abs(x1 - x2), (float)abs(y1 - y2)); } float Distance(int x1, int y1, int x2, int y2, int type) { @@ -1195,7 +1195,7 @@ float Distance(int x1, int y1, int x2, int y2, int type) { return MDistance(x1, y1, x2, y2); } - else if (type == 2) + else //if (type == 2) { return CDistance(x1, y1, x2, y2); } From be5dbbccfbe755c0adc24053d983e88a2c2b6d15 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 28 Jul 2025 09:22:07 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=A9=A1?= =?UTF-8?q?=E8=83=B6=E6=89=8B=E5=A5=97=E5=AE=9A=E4=BD=8D=E6=89=8B=E5=A5=97?= =?UTF-8?q?=E7=8E=AF=E7=9A=84=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 26 + bagPositioning_test/bagPositioning_test.cpp | 251 +++--- baseAlgorithm/baseAlgorithm.vcxproj | 1 + glovePositioning/glovePositioning.vcxproj | 174 ++++ .../glovePositioning_test.cpp | 809 ++++++++++++++++++ .../glovePositioning_test.vcxproj | 159 ++++ sourceCode/SG_baseAlgo_Export.h | 6 + sourceCode/SG_baseDataType.h | 23 +- sourceCode/SG_errCode.h | 9 +- sourceCode/SG_featureGrow.cpp | 2 + sourceCode/SG_lineFeature.cpp | 309 ++----- sourceCode/WD_glovePositioning.cpp | 297 +++++++ sourceCode/WD_glovePositioning_Export.h | 33 + 13 files changed, 1702 insertions(+), 397 deletions(-) create mode 100644 glovePositioning/glovePositioning.vcxproj create mode 100644 glovePositioning_test/glovePositioning_test.cpp create mode 100644 glovePositioning_test/glovePositioning_test.vcxproj create mode 100644 sourceCode/WD_glovePositioning.cpp create mode 100644 sourceCode/WD_glovePositioning_Export.h diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index b41a680..740736d 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -76,6 +76,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "Q {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning", "glovePositioning\glovePositioning.vcxproj", "{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning_test", "glovePositioning_test\glovePositioning_test.vcxproj", "{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}" + ProjectSection(ProjectDependencies) = postProject + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} = {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -204,6 +214,22 @@ Global {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.Build.0 = Release|x64 {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.ActiveCfg = Release|Win32 {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.Build.0 = Release|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x64.ActiveCfg = Debug|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x64.Build.0 = Debug|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x86.ActiveCfg = Debug|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x86.Build.0 = Debug|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x64.ActiveCfg = Release|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x64.Build.0 = Release|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x86.ActiveCfg = Release|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x86.Build.0 = Release|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x64.ActiveCfg = Debug|x64 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x64.Build.0 = Debug|x64 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x86.ActiveCfg = Debug|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x86.Build.0 = Debug|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.ActiveCfg = Release|x64 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.Build.0 = Release|x64 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.ActiveCfg = Release|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 0bf6ff3..9fdb34c 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -45,7 +45,7 @@ SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9]) #define DATA_VER_FROM_CUSTOM 2 #define VZ_LASER_LINE_PT_MAX_NUM 4096 SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, - int* dataCalib, int* scanMaxStamp, int* canClockUnit) + int* dataCalib, int* scanMaxStamp, int* canClockUnit, bool removeNullLines) { std::ifstream inputFile(fileName); std::string linedata; @@ -64,175 +64,122 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int lineNum = 0; if (0 == strncmp("LineNum:", linedata.c_str(), 8)) { - dataFileVer = DATA_VER_NEW; sscanf_s(linedata.c_str(), "LineNum:%d", &lines); if (lines == 0) return NULL; lineNum = lines; _scanLines = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_OLD; - sscanf_s(linedata.c_str(), "LineNum_%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; } if (_scanLines == NULL) return NULL; - int ptNum = 0; - int lineIdx = -1; + int lineIdx = 0; int ptIdx = 0; - SVzNLPointXYZRGBA* p3DPoint = NULL; - if (dataFileVer == DATA_VER_NEW) + int ptNum = 0; + std::vector< SVzNLPointXYZRGBA> a_line; + int vldPtNum = 0; + unsigned int timeStamp = 0; + while (getline(inputFile, linedata)) { - while (getline(inputFile, linedata)) + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) { - if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); - if (scanV) - *scanV = (float)lineV; - } - else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) - { - int ptAdjusted = 0; - sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); - if (dataCalib) - *dataCalib = ptAdjusted; - } - else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) - { - unsigned int maxTimeStamp = 0; - unsigned int timePerStamp = 0; - sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); - if (scanMaxStamp) - *scanMaxStamp = maxTimeStamp; - if (canClockUnit) - *canClockUnit = timePerStamp; - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); - if (firstIndex < 0) - firstIndex = lineIndex; - - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - - //new Line - lineIdx++; - if (ptNum > 0) - { - p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); - memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum); - } - else - p3DPoint = NULL; - _scanLines[lineIdx].nPointCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPoint = p3DPoint; - - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - float leftX, leftY; - float rightX, rightY; - float r, g, b; - sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); - int id = _scanLines[lineIdx].nPointCnt; - if (id < ptNum) - { - if (lineIdx == 537) - int kkk = 1; - p3DPoint[id].x = X; - p3DPoint[id].y = Y; - p3DPoint[id].z = Z; - int nr = (int)(r * 255); - int ng = (int)(g * 255); - int nb = (int)(b * 255); - nb <<= 8; - nb += ng; - nb <<= 8; - nb += nr; - p3DPoint[id].nRGB = nb; - _scanLines[lineIdx].nPointCnt = id + 1; - } - } + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; } - - } - else if (dataFileVer == DATA_VER_OLD) - { - while (getline(inputFile, linedata)) + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) { - if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int curr_timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &curr_timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + int recvPtNum = (int)a_line.size(); + if ( (recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) { - sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); - if ((dataElements != 3) && (dataElements != 4)) - break; - } - if (0 == strncmp("LineV_", linedata.c_str(), 6)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); -#if 0 - if (scanLineListTail == NULL) - firstIndex = lineIndex; -#endif - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - //new Line - //new Line - lineIdx++; - p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM); - memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM); - _scanLines[lineIdx].nPointCnt = 0; + SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); + _scanLines[lineIdx].nPointCnt = ptNum; _scanLines[lineIdx].nTimeStamp = timeStamp; _scanLines[lineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < ptNum; m++) + p3DPoint[m] = a_line[m]; + lineIdx++; } - else if (0 == strncmp("(", linedata.c_str(), 1)) + //new Line + timeStamp = curr_timeStamp; + vldPtNum = 0; + a_line.clear(); + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + float r, g, b; + sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); { - float X, Y, Z; - int imageY = 0; - if (dataElements == 4) - sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); - else - sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); - int id = _scanLines[lineIdx].nPointCnt; - if (id < VZ_LASER_LINE_PT_MAX_NUM) - { - p3DPoint[id].x = X; - p3DPoint[id].y = Y; - p3DPoint[id].z = Z; - p3DPoint[id].nRGB = 0; - _scanLines[lineIdx].nPointCnt = id + 1; - } + if (lineIdx == 537) + int kkk = 1; + + SVzNLPointXYZRGBA a_pt; + + a_pt.x = X; + a_pt.y = Y; + a_pt.z = Z; + int nr = (int)(r * 255); + int ng = (int)(g * 255); + int nb = (int)(b * 255); + nb <<= 8; + nb += ng; + nb <<= 8; + nb += nr; + a_pt.nRGB = nb; + if (a_pt.z > 1e-4) + vldPtNum++; + a_line.push_back(a_pt); } } } + //last line + int recvPtNum = (int)a_line.size(); + if ((recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + { + SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); + _scanLines[lineIdx].nPointCnt = ptNum; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < ptNum; m++) + p3DPoint[m] = a_line[m]; + lineIdx++; + } + + if (scanLineNum) + *scanLineNum = lineIdx; inputFile.close(); return _scanLines; } @@ -2578,7 +2525,7 @@ int main() 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,1}, + {2,4}, {1,5}, {1,1}, {1,3}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -2602,7 +2549,7 @@ int main() SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 15; grp <= 15; grp++) + for (int grp = 14; grp <= 15; grp++) { if (grp < 10) { @@ -2865,17 +2812,19 @@ int main() int maxTimeStamp = 0; int clockPerSecond = 0; sprintf_s(_scan_file, "%s%d-RGB鐐逛簯.txt", dataPath[grp], fidx); + bool removeNullLines = true; SVzNLXYZRGBDLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZRGB( _scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, - &clockPerSecond); + &clockPerSecond, + removeNullLines); if (laser3DPoints == NULL) continue; - algoParam.filterParam.continuityTh = 20.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ + algoParam.filterParam.continuityTh = 20;// 10.0; // 20.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ algoParam.filterParam.outlierTh = 5; long t1 = GetTickCount64(); diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index de7d070..e68acff 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -163,6 +163,7 @@ + diff --git a/glovePositioning/glovePositioning.vcxproj b/glovePositioning/glovePositioning.vcxproj new file mode 100644 index 0000000..c18b791 --- /dev/null +++ b/glovePositioning/glovePositioning.vcxproj @@ -0,0 +1,174 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {4060ce45-6235-4ca7-b64c-6e4e5cddc34f} + glovePositioning + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + false + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + + Level3 + true + WIN32;_DEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/glovePositioning_test/glovePositioning_test.cpp b/glovePositioning_test/glovePositioning_test.cpp new file mode 100644 index 0000000..3b3a3c7 --- /dev/null +++ b/glovePositioning_test/glovePositioning_test.cpp @@ -0,0 +1,809 @@ +锘// glovePositioning_test.cpp : 姝ゆ枃浠跺寘鍚 "main" 鍑芥暟銆傜▼搴忔墽琛屽皢鍦ㄦ澶勫紑濮嬪苟缁撴潫銆 +// +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "WD_glovePositioning_Export.h" +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool exchangeXY) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + double X, Y, Z; + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &X, &Y, &Z); + SVzNL3DPoint a_pt; + if (true == exchangeXY) + { + a_pt.x = Y; //灏嗘壂鎻忕嚎璋冩暣涓篩鏂瑰悜锛涙壂鎻忔柟鍚戜负X鏂瑰悜 + a_pt.y = X; + } + else + { + a_pt.x = X; + a_pt.y = Y; + } + a_pt.z = Z; + scanData.push_back(a_pt); + } + return; +} + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int lineIdx = 0; + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + if (true == removeZeros) + { + int vldPtNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldPtNum++; + } + linePtNum = vldPtNum; + } + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + +//杈撳嚭姘村钩鎵弿鏁版嵁 +void _outputScanDataFile_vector_h(char* fileName, std::vector>& scanLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines[0].size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int linePtNum = (int)scanLines.size(); + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[i][line].pt3D; + + float x = (float)pt3D->y; + float y = (float)pt3D->x; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + +void _outputRGBDScanDataFile_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector< SSG_peakRgnInfo> objPoints) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (objPoints.size() > 0) + realLines++; + + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//娣￠粍鑹 + {210,105, 30},//宸у厠鍔涜壊 + {240,230,140},//榛勮鑹 + {135,206,235},//澶╄摑鑹 + {250,235,215},//鍙よ懀鐧 + {189,252,201},//钖勮嵎鑹 + {221,160,221},//姊呯孩鑹 + {188,143,143},//鐜懓绾㈣壊 + }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + int featureType_v = pt3D->nPointIdx & 0xffff; + int featureType_h = featureType_v >> 4; + featureType_v &= 0xff; + if (LINE_FEATURE_PEAK_TOP == featureType_v) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else if (LINE_FEATURE_PEAK_TOP == featureType_h) + { + rgb = { 97, 255, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objPoints.size() > 0) + { + int linePtNum = (int)objPoints.size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + + rgb = { 0, 0, 255 }; + size = 25; + for (int i = 0; i < linePtNum; i++) + { + float x = (float)objPoints[i].centerPos.x; + float y = (float)objPoints[i].centerPos.y; + float z = (float)objPoints[i].centerPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //鍔犱竴涓偣锛岀敤浜庤烦杩囨樉绀哄伐鍏穊ug + float x = (float)objPoints[0].centerPos.x; + float y = (float)objPoints[0].centerPos.y; + float z = (float)objPoints[0].centerPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + sw.close(); +} + + +void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum) +{ + std::ofstream sw(fileName); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + + +#define DATA_VER_OLD 0 +#define DATA_VER_NEW 1 +#define DATA_VER_FROM_CUSTOM 2 +#define VZ_LASER_LINE_PT_MAX_NUM 4096 +SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //绗竴琛 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + else if (dataFileVer == DATA_VER_OLD) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + { + sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); + if ((dataElements != 3) && (dataElements != 4)) + break; + } + if (0 == strncmp("LineV_", linedata.c_str(), 6)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); +#if 0 + if (scanLineListTail == NULL) + firstIndex = lineIndex; +#endif + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + //new Line + //new Line + lineIdx++; + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + } + else if (0 == strncmp("(", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + if (dataElements == 4) + sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); + else + sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < VZ_LASER_LINE_PT_MAX_NUM) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + } + inputFile.close(); + return _scanLines; +} + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //璋冨钩鐭╅樀 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //鍦伴潰楂樺害 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //鍙嶅悜鏃嬭浆鐭╅樀 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //璁剧疆鍒濆缁撴灉 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //璋冨钩鐭╅樀 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //鍦伴潰楂樺害 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //鍙嶅悜鏃嬭浆鐭╅樀 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + +void _getRoiClouds( + std::vector< std::vector>& scanLines, + int startLine, + int endLine, + int startPtIdx, + int endPtIdx, + std::vector< std::vector>& roiScanLines) +{ + for (int i = startLine; i < endLine; i++) + { + if (i >= scanLines.size()) + break; + + std::vector cut_line; + std::vector& a_line = scanLines[i]; + for (int j = startPtIdx; j < endPtIdx; j++) + { + SVzNL3DPosition a_pt; + if (j >= a_line.size()) + { + a_pt.nPointIdx = 0; + a_pt.pt3D = { 0,0,0 }; + } + else + a_pt = a_line[j]; + cut_line.push_back(a_pt); + } + roiScanLines.push_back(cut_line); + } + return; +} + +void _convertToGridData(std::vector< std::vector>& scanLines, + std::vector< std::vector>& gridScanLines, + double y_step) +{ + double y_min = scanLines[0][0].pt3D.y; + double y_max = y_min; + int lineNum = (int)scanLines.size(); + for(int line = 0; line < lineNum; line++) + { + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < ptNum; i++) + { + if (y_min > a_line[i].pt3D.y) + y_min = a_line[i].pt3D.y; + if (y_max < a_line[i].pt3D.y) + y_max = a_line[i].pt3D.y; + } + } + double half_step = y_step / 2; + int gridPtNum = (int)((y_max - y_min + half_step) / y_step) + 1; + for (int line = 0; line < lineNum; line++) + { + std::vector grid_line; + grid_line.resize(gridPtNum); + for (int i = 0; i < gridPtNum; i++) + grid_line[i] = { 0, {0.0, 0.0, 0.0} }; + + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < ptNum; i++) + { + int ptIdx = (int)((a_line[i].pt3D.y + half_step - y_min) / y_step); + grid_line[ptIdx] = a_line[i]; + } + gridScanLines.push_back(grid_line); + } + return; +} + +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_GLOVE_POSITION 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:\\ShangGu\\鎵嬪鎶撳彇瀹氫綅\\", //0 + + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,35} + }; + +#if CONVERT_TO_GRID + int convertGrp = 0; + for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%sglove_%d.txt", dataPath[convertGrp], fidx); + std::vector< SVzNL3DPoint> scanData; + bool exchangeXY = true; + vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, exchangeXY); + //灏嗘暟鎹仮澶嶄负鎸夋壂鎻忕嚎瀛樺偍鏍煎紡 + std::vector< std::vector> scanLines; + wd_getScanLines(scanData, scanLines); + + double y_step; + if ( (fidx == 4)|| (fidx == 5)) + y_step = 0.106; + else + y_step = 0.091; + std::vector< std::vector> grid_scanLines; + _convertToGridData(scanLines, grid_scanLines, y_step); + sprintf_s(_scan_file, "%sglove_%d_scanLine.txt", dataPath[convertGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, grid_scanLines, false, &headNullLines); + printf("%s: head null lines = %d\n", _scan_file, headNullLines); +#if 1 + sprintf_s(_scan_file, "%sglove_%d_scanLine_h.txt", dataPath[convertGrp], fidx); + _outputScanDataFile_vector_h(_scan_file, grid_scanLines); +#endif + } +#endif + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:\\ShangGu\\椤圭洰\\宸ヤ欢绔儴鍦嗙偣浜岀淮鐮乗\LaserLine3_grid.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_calib_datafile, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + if (laser3DPoints) + { + SSG_planeCalibPara calibPara = wd_getBaseCalibPara( + laser3DPoints, + lineNum); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(&laser3DPoints[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\椤圭洰\\宸ヤ欢绔儴鍦嗙偣浜岀淮鐮乗\ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:\\ShangGu\\椤圭洰\\宸ヤ欢绔儴鍦嗙偣浜岀淮鐮乗\LaserLine3_calib_data.txt"); + _outputScanDataFile_ptr(_out_file, laser3DPoints, lineNum); + printf("%s: calib done!\n", _calib_datafile); + } + +#endif + +#if TEST_COMPUTE_GLOVE_POSITION + for (int grp = 0; grp <= 0; grp++) + { + SSG_planeCalibPara poseCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; +// char calibFile[250]; +// sprintf_s(calibFile, "F:\\ShangGu\\椤圭洰\\宸ヤ欢绔儴鍦嗙偣浜岀淮鐮乗\ground_calib_para.txt"); +// poseCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =1; + char _scan_file[256]; + sprintf_s(_scan_file, "%sglove_%d_scanLine.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); + } + // + SSG_gloveArcParam arcFeatureParam; + arcFeatureParam.cornerTh = 120; //杞澶т簬150 + arcFeatureParam.scale_angle = 1.0; + arcFeatureParam.scale_corner = 5.0; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 5; + growParam.yDeviation_max = 1.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //琚嬪瓙楂樺害1/2 + growParam.minLTypeTreeLen = 30.0; //mm + growParam.minVTypeTreeLen = 30.0; //mm + + int errCode = 0; + std::vector objOps; + wd_getGloveGrabPostion( + scanLines, + arcFeatureParam, + growParam, + objOps, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDScanDataFile_RGBD(_scan_file, scanLines, objOps); + } + } +#endif +} + +// 杩愯绋嬪簭: Ctrl + F5 鎴栬皟璇 >鈥滃紑濮嬫墽琛(涓嶈皟璇)鈥濊彍鍗 +// 璋冭瘯绋嬪簭: F5 鎴栬皟璇 >鈥滃紑濮嬭皟璇曗濊彍鍗 + +// 鍏ラ棬浣跨敤鎶宸: +// 1. 浣跨敤瑙ｅ喅鏂规璧勬簮绠＄悊鍣ㄧ獥鍙ｆ坊鍔/绠＄悊鏂囦欢 +// 2. 浣跨敤鍥㈤槦璧勬簮绠＄悊鍣ㄧ獥鍙ｈ繛鎺ュ埌婧愪唬鐮佺鐞 +// 3. 浣跨敤杈撳嚭绐楀彛鏌ョ湅鐢熸垚杈撳嚭鍜屽叾浠栨秷鎭 +// 4. 浣跨敤閿欒鍒楄〃绐楀彛鏌ョ湅閿欒 +// 5. 杞埌鈥滈」鐩>鈥滄坊鍔犳柊椤光濅互鍒涘缓鏂扮殑浠ｇ爜鏂囦欢锛屾垨杞埌鈥滈」鐩>鈥滄坊鍔犵幇鏈夐」鈥濅互灏嗙幇鏈変唬鐮佹枃浠舵坊鍔犲埌椤圭洰 +// 6. 灏嗘潵锛岃嫢瑕佸啀娆℃墦寮姝ら」鐩紝璇疯浆鍒扳滄枃浠垛>鈥滄墦寮鈥>鈥滈」鐩濆苟閫夋嫨 .sln 鏂囦欢 diff --git a/glovePositioning_test/glovePositioning_test.vcxproj b/glovePositioning_test/glovePositioning_test.vcxproj new file mode 100644 index 0000000..9872dae --- /dev/null +++ b/glovePositioning_test/glovePositioning_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {7c1efab7-06cc-4b7d-81bb-64c6fa5996fd} + glovePositioningtest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + false + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;glovePositioning.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;glovePositioning.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index c07da9c..d318ccc 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -60,6 +60,12 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature( const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 SSG_lineFeature* line_features); +SG_APISHARED_EXPORT void wd_getLineGloveArcs( + std::vector& lineData, + int lineIdx, + const SSG_gloveArcParam arcPara, + std::vector& gloveArcs); + /// 提取激光线上的圆柱形特征 SG_APISHARED_EXPORT void sg_getLineUpperSemiCircleFeature( SVzNL3DPosition* lineData, diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 6721679..a3fcf08 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -92,6 +92,7 @@ typedef struct int featureType; SVzNL2DPoint jumpPos2D; SVzNL3DPoint jumpPos; + double featureValue; }SSG_basicFeature1D; typedef struct @@ -156,6 +157,13 @@ typedef struct double jumpCornerTh_2; }SSG_cornerParam; +typedef struct +{ + double scale_angle; //计算方向角的窗口比例尺 + double scale_corner; //计算方向角转角的窗口比例尺 + double cornerTh; //拐角门限，大于此门限，为有效Arc +}SSG_gloveArcParam; + typedef struct { double H_len; //直角特征水平段的长度 @@ -208,6 +216,7 @@ typedef struct int treeType; int sLineIdx; int eLineIdx; + double tree_value; SSG_ROIRectD roi; std::vector< SSG_basicFeature1D> treeNodes; }SSG_featureTree; @@ -388,4 +397,16 @@ typedef struct double planeCalib[9]; //旋转矩阵，将点云地面调平 double planeHeight;//地面调平后的高度，用于去除地面数据 double invRMatrix[9]; //旋转矩阵，回到原坐标系 -}SSG_planeCalibPara; \ No newline at end of file +}SSG_planeCalibPara; + +typedef struct +{ + int pntIdx; + double forwardAngle; //前向角 + double backwardAngle; //后向角 + double corner; //拐角 + double forwardDiffZ; + double backwardDiffZ; + double pre_stepDist; + double post_stepDist; +}SSG_pntDirAngle; diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 188b622..6dc6cfd 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -1,7 +1,8 @@ #pragma once #define SG_ERR_3D_DATA_INVLD -1000 -#define SG_ERR_FOUND_NO_TOP_PLANE -1001 -#define SG_ERR_NOT_GRID_FORMAT -1002 -#define SG_ERR_LABEL_INFO_ERROR -1003 -#define SG_ERR_INVLD_SORTING_MODE -1004 +#define SG_ERR_3D_DATA_NULL -1001 +#define SG_ERR_FOUND_NO_TOP_PLANE -1002 +#define SG_ERR_NOT_GRID_FORMAT -1003 +#define SG_ERR_LABEL_INFO_ERROR -1004 +#define SG_ERR_INVLD_SORTING_MODE -1005 diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index d286208..2cfd789 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -75,6 +75,7 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect { a_tree.eLineIdx = lineIdx; a_tree.treeNodes.push_back(a_feature); + a_tree.tree_value += a_feature.featureValue; return true; } } @@ -431,6 +432,7 @@ void sg_lineFeaturesGrowing( a_newTree.treeType = a_feature.featureType; a_newTree.sLineIdx = lineIdx; a_newTree.eLineIdx = lineIdx; + a_newTree.tree_value = a_feature.featureValue; trees.push_back(a_newTree); } } diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index fc7c9e0..a011912 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -11,16 +11,6 @@ typedef struct SVzNL3DPosition endPt; }SSG_jump; -typedef struct -{ - int pntIdx; - double forwardAngle; //前向角 - double backwardAngle; //后向角 - double corner; //拐角 - double forwardDiffZ; - double backwardDiffZ; -}SSG_pntDirAngle; - typedef struct { int flag; @@ -1448,22 +1438,18 @@ void sg_getLineCornerFeature( return; } -#if 0 -void sg_getLineCorners( +//手套的环的特征：在扫描线的Peak点的前后一定范围内，存在前向角从正到负的变化范围超过150度（门限值） +void wd_getLineGloveArcs( std::vector& lineData, int lineIdx, - const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 - SSG_lineFeature* line_features) + const SSG_gloveArcParam arcPara, + std::vector& gloveArcs) { - line_features->lineIdx = lineIdx; - if (lineIdx == 538) - int kkk = 1; //去除零点 std::vector< SVzNL3DPosition> vldPts; - std::vector segs; - //修改seg的定义。seg端点是两点间距离大于门限的点 - int segStart = -1, segEnd = -1; - int dataSize = + int dataSize = (int)lineData.size(); + if (dataSize < 10) //点太少，无法检出特征 + return; for (int i = 0; i < dataSize; i++) { if ((lineIdx == 399) && (i == 568)) @@ -1472,83 +1458,53 @@ void sg_getLineCorners( 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; - } - else - { - if (segStart >= 0) - { - SSG_RUN a_run; - a_run.start = segStart; - a_run.len = segEnd - segStart + 1; - a_run.value = 1; - segs.push_back(a_run); - segStart = -1; - segEnd = -1; - } - } } - //last - if (segStart >= 0) - { - SSG_RUN a_run; - a_run.start = segStart; - a_run.len = segEnd - segStart + 1; - a_run.value = 1; - segs.push_back(a_run); - } - - //计算前向角和后向角 - std::vector< SSG_pntDirAngle> corners; + + //在同一尺度下计算前向角和后向角，以及极大值点。 + std::vector< SSG_pntDirAngle> corners; //逐点计算前向角 corners.resize(vldPts.size()); - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + std::vector< int> localMax; //寻找极大值点 + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) { - if ((lineIdx == 399) && (i == 419)) + if ((lineIdx == 0) && (i == 2585)) int kkk = 1; + + double pre_stepDist = 0; //前向寻找 + bool isMax = true; int pre_i = -1; for (int j = i - 1; j >= 0; j--) { - double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + - pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); - if (dist >= cornerPara.scale) + if ((true == isMax) &&(vldPts[i].pt3D.z <= vldPts[j].pt3D.z)) + isMax = false; + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (j == (i - 1)) + pre_stepDist = dist; + if (dist >= arcPara.scale_angle) { pre_i = j; break; } } //后向寻找 + double post_stepDist = 0; int post_i = -1; for (int j = i + 1; j < i_max; j++) { - double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + - pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); - if (dist >= cornerPara.scale) + if (( true == isMax) && (vldPts[i].pt3D.z < vldPts[j].pt3D.z)) + isMax = false; + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (j == (i + 1)) + post_stepDist = dist; + if (dist >= arcPara.scale_angle) { post_i = j; break; } } + if (true == isMax) + localMax.push_back(i); + //计算拐角 if ((pre_i < 0) || (post_i < 0)) { @@ -1558,6 +1514,8 @@ void sg_getLineCorners( corners[i].corner = 0; corners[i].forwardDiffZ = 0; corners[i].backwardDiffZ = 0; + corners[i].pre_stepDist = pre_stepDist; + corners[i].post_stepDist = post_stepDist; } else { @@ -1568,189 +1526,58 @@ void sg_getLineCorners( corners[i].pntIdx = i; corners[i].forwardAngle = forwardAngle; corners[i].backwardAngle = backwardAngle; - corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号 + corners[i].corner = (forwardAngle - backwardAngle); corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + corners[i].pre_stepDist = pre_stepDist; + corners[i].post_stepDist = post_stepDist; } } - //搜索拐角极值 - int _state = 0; - int pre_i = -1; - int sEdgePtIdx = -1; - int eEdgePtIdx = -1; - SSG_pntDirAngle* pre_data = NULL; - std::vector< SSG_pntDirAngle> cornerPeakP; - std::vector< SSG_pntDirAngle> cornerPeakM; - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + //在极大值前后搜索 + for (int i = 0, i_max = (int)localMax.size(); i < i_max; i++) { - if (i == 275) + if (i == 31) int kkk = 1; - SSG_pntDirAngle* curr_data = &corners[i]; - if (curr_data->pntIdx < 0) + int peakIdx = localMax[i]; + + //向前搜索正的最大前向角 + double pre_sumLen = 0; + double pre_angleMax = 0; + for (int j = peakIdx - 1; j >= 0; j--) { - if (i == i_max - 1) //最后一个 - { - if (1 == _state) //上升 - { - cornerPeakP.push_back(corners[eEdgePtIdx]); - } - else if (2 == _state) //下降 - { - cornerPeakM.push_back(corners[eEdgePtIdx]); - } - } - continue; - } - - if (NULL == pre_data) - { - sEdgePtIdx = i; - eEdgePtIdx = i; - pre_data = curr_data; - pre_i = i; - continue; - } - - eEdgePtIdx = i; - double cornerDiff = curr_data->corner - pre_data->corner; - switch (_state) - { - case 0: //初态 - if (cornerDiff < 0) //下降 - { - _state = 2; - } - else if (cornerDiff > 0) //上升 - { - _state = 1; - } - break; - case 1: //上升 - if (cornerDiff < 0) //下降 - { - cornerPeakP.push_back(*pre_data); - _state = 2; - } - break; - case 2: //下降 - if (cornerDiff > 0) // 上升 - { - cornerPeakM.push_back(*pre_data); - _state = 1; - } - break; - default: - _state = 0; - break; - } - pre_data = curr_data; - pre_i = i; - } - //注意：最后一个不处理，为基座位置 - - //极小值点（峰顶） - //极值比较，在尺度窗口下寻找局部极值点 - double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。 - for (int i = 0, i_max = cornerPeakP.size(); i < i_max; i++) - { - if (cornerPeakP[i].corner < cornerPara.cornerTh) - continue; - - bool isPeak = true; - //向前搜索 - int cornerPtIdx = cornerPeakP[i].pntIdx; - for (int j = i - 1; j >= 0; j--) - { - int prePtIdx = cornerPeakP[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; - if (dist > square_distTh) //超出尺度窗口 + if (pre_angleMax < corners[j].forwardAngle) + pre_angleMax = corners[j].forwardAngle; + pre_sumLen += corners[j].post_stepDist; + if (pre_sumLen >= arcPara.scale_corner) break; - - if (cornerPeakP[i].corner < cornerPeakP[j].corner) - { - isPeak = false; - break; - } } - //向后搜索 - if (true == isPeak) + //向后搜索负的最大前向角 + double post_angleMax = corners[peakIdx].forwardAngle; + double post_sumLen = corners[peakIdx].post_stepDist; + for (int j = peakIdx + 1; j < (int)vldPts.size(); j++) { - cornerPtIdx = cornerPeakP[i].pntIdx; - for (int j = i + 1; j < i_max; j++) - { - int postPtIdx = cornerPeakP[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); - if (dist > square_distTh) //超出尺度窗口 - break; - - if (cornerPeakP[i].corner < cornerPeakP[j].corner) - { - isPeak = false; - break; - } - } + post_sumLen += corners[j].post_stepDist; + if (post_sumLen >= arcPara.scale_corner) + break; + if (post_angleMax > corners[j].forwardAngle) + post_angleMax = corners[j].forwardAngle; } - if (true == isPeak) + //计算最大转角 + double totalTurn = pre_angleMax - post_angleMax; + if (totalTurn >= arcPara.cornerTh) //有效ARC { SSG_basicFeature1D a_feature; - if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; - else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; - else - a_feature.featureType = LINE_FEATURE_CORNER_V; - - - a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; - a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx }; - line_features->features.push_back(a_feature); + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos = vldPts[peakIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, vldPts[peakIdx].nPointIdx }; + a_feature.featureValue = totalTurn; + gloveArcs.push_back(a_feature); } } - - //添加开始和结束边界 - //检查seg是否需要合并；向后合并 - for (int i = 0, i_max = segs.size(); i < i_max - 1; i++) - { - SSG_RUN* nxt_seg = &segs[i + 1]; - SSG_RUN* curr_seg = &segs[i]; - - 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); - 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; - nxt_seg->len = idx_end - curr_seg->start + 1; - curr_seg->value = 0; - } - } - for (int i = 0, i_max = segs.size(); i < i_max; i++) - { - if (0 == segs[i].value) //被合并 - continue; - - int idx_1 = segs[i].start; - int idx_2 = segs[i].start + segs[i].len - 1; - - SSG_basicFeature1D an_edge; - memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); - an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; - an_edge.jumpPos = lineData[idx_1].pt3D; - an_edge.jumpPos2D = { lineIdx, idx_1 }; - line_features->endings.push_back(an_edge); - //line_features.insert(line_features.begin(), an_edge); //头部 - //尾部 - an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; - an_edge.jumpPos = lineData[idx_2].pt3D; - an_edge.jumpPos2D = { lineIdx, idx_2 }; - line_features->endings.push_back(an_edge); - } return; } -#endif + /// /// 提取激光线上的极值点（极大值点和极小值点） /// diff --git a/sourceCode/WD_glovePositioning.cpp b/sourceCode/WD_glovePositioning.cpp new file mode 100644 index 0000000..ea8519f --- /dev/null +++ b/sourceCode/WD_glovePositioning.cpp @@ -0,0 +1,297 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "WD_glovePositioning_Export.h" +#include +#include + +//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理 +void wd_getScanLines( + std::vector& scanData, + std::vector< std::vector>& scanLines) +{ + std::vector a_line; + for (int i = 0, i_max = scanData.size(); i < i_max; i++) + { + SVzNL3DPoint a_pt = scanData[i]; + SVzNL3DPosition a_idxPt; + a_idxPt.nPointIdx = 0; + a_idxPt.pt3D = a_pt; + if (a_line.size() == 0) + a_line.push_back(a_idxPt); + else + { + SVzNL3DPosition pre_pt = a_line.back(); + double x_diff = abs(pre_pt.pt3D.x - a_idxPt.pt3D.x); + if (x_diff > 1e-4) + { + scanLines.push_back(a_line); + a_line.clear(); + a_line.push_back(a_idxPt); + } + else + a_line.push_back(a_idxPt); + } + } + //last line + if(a_line.size() > 0) + scanLines.push_back(a_line); + return; +} + +void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +void wd_getGloveGrabPostion( + std::vector< std::vector>& scanLines, + const SSG_gloveArcParam arcPara, + SSG_treeGrowParam growParam, + std::vector& objOps, + int* errCode) +{ + *errCode = 0; + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + + int linePtNum = (int)scanLines[0].size(); + bool isGridData = true; + //垂直arc特征提取 + std::vector> arcFeatures; + for(int line = 0; line < lineNum; line ++) + { + std::vector lineArcFeatures; + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + wd_getLineGloveArcs( + lineData, + line, + arcPara, + lineArcFeatures); + arcFeatures.push_back(lineArcFeatures); + } + + if (false == isGridData)//数据不是网格格式 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + //生成水平扫描 + std::vector> hLines; + hLines.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用） + hLines[j][line] = scanLines[line][j]; + hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //水平arc特征提取 + std::vector> arcFeatures_h; + int lineNum_h = (int)hLines.size(); + for (int line = 0; line < lineNum_h; line++) + { + std::vector lineArcFeatures; + std::vector& lineData = hLines[line]; + wd_getLineGloveArcs( + lineData, + line, + arcPara, + lineArcFeatures); + arcFeatures_h.push_back(lineArcFeatures); + } + + //特征生长 + //垂直方向特征生长（激光线方向） + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineArcFeature = arcFeatures[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineArcFeature, + v_trees, + growParam); + } + //水平方向特征生长（扫描运动方向） + std::vector h_trees; + for (int line = 0; line < lineNum_h; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h - 1) + isLastLine = true; + std::vector& a_lineArcFeature = arcFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineArcFeature, + h_trees, + growParam); + } + //保留最大平均转角的生长树 + double maxTurn_v = 0; + int maxTurn_v_id = -1; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + int nodeSize = (int)v_trees[i].treeNodes.size(); + v_trees[i].tree_value = v_trees[i].tree_value / (double)nodeSize; + if (maxTurn_v_id < 0) + { + maxTurn_v_id = i; + maxTurn_v = v_trees[i].tree_value; + } + else + { + if (maxTurn_v < v_trees[i].tree_value) + { + maxTurn_v_id = i; + maxTurn_v = v_trees[i].tree_value; + } + } + } + double maxTurn_h = 0; + int maxTurn_h_id = -1; + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + int nodeSize = (int)h_trees[i].treeNodes.size(); + h_trees[i].tree_value = h_trees[i].tree_value / (double)nodeSize; + if (maxTurn_h_id < 0) + { + maxTurn_h_id = i; + maxTurn_h = h_trees[i].tree_value; + } + else + { + if (maxTurn_h < h_trees[i].tree_value) + { + maxTurn_h_id = i; + maxTurn_h = h_trees[i].tree_value; + } + } + } + if ((maxTurn_v_id >= 0) && (maxTurn_h_id < 0)) + { + SSG_featureTree bestTree = v_trees[maxTurn_v_id]; + v_trees.clear(); + v_trees.push_back(bestTree); + } + else if ((maxTurn_v_id < 0) && (maxTurn_h_id >= 0)) + { + SSG_featureTree bestTree = h_trees[maxTurn_h_id]; + h_trees.clear(); + h_trees.push_back(bestTree); + } + else if ((maxTurn_v_id >= 0) && (maxTurn_h_id >= 0)) + { + if (maxTurn_h <= maxTurn_v) + { + h_trees.clear(); + SSG_featureTree bestTree = v_trees[maxTurn_v_id]; + v_trees.clear(); + v_trees.push_back(bestTree); + } + else + { + v_trees.clear(); + SSG_featureTree bestTree = h_trees[maxTurn_h_id]; + h_trees.clear(); + h_trees.push_back(bestTree); + } + } + + //tree信息 + std::vector allTreesInfo; //不包含边界 + SSG_treeInfo a_nullTree; + memset(&a_nullTree, 0, sizeof(SSG_treeInfo)); + allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引 + //标记，根据起点的生长树进行标注 + int hvTreeIdx = 1; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + + //记录Tree的信息 + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 1; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_vTree->treeType; + a_treeInfo.sLineIdx = a_vTree->sLineIdx; + a_treeInfo.eLineIdx = a_vTree->eLineIdx; + a_treeInfo.roi = a_vTree->roi; + allTreesInfo.push_back(a_treeInfo); + //在原始点云上标记，同时有Mask上标记 + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; + } + } + } + hvTreeIdx++; + } + int hTreeStart = hvTreeIdx; + ////标注:水平特征 + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + //记录Tree的信息 + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 0; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_hTree->treeType; + a_treeInfo.sLineIdx = a_hTree->sLineIdx; + a_treeInfo.eLineIdx = a_hTree->eLineIdx; + a_treeInfo.roi.left = a_hTree->roi.top; //水平扫描xy是交换的 + a_treeInfo.roi.right = a_hTree->roi.bottom; + a_treeInfo.roi.top = a_hTree->roi.left; + a_treeInfo.roi.bottom = a_hTree->roi.right; + allTreesInfo.push_back(a_treeInfo); + //在原始点云上标记，同时有Mask上标记 + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; + } + } + } + hvTreeIdx++; + } + int hvTreeSize = hvTreeIdx; + + +} \ No newline at end of file diff --git a/sourceCode/WD_glovePositioning_Export.h b/sourceCode/WD_glovePositioning_Export.h new file mode 100644 index 0000000..be10303 --- /dev/null +++ b/sourceCode/WD_glovePositioning_Export.h @@ -0,0 +1,33 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +typedef struct +{ + SSG_outlierFilterParam filterParam; + SSG_cornerParam cornerParam; + SSG_treeGrowParam growParam; +}SG_glovePositioningParam; + +SG_APISHARED_EXPORT void wd_getScanLines( + std::vector& scanData, + std::vector< std::vector>& scanLines); + +SG_APISHARED_EXPORT void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +SG_APISHARED_EXPORT void wd_getGloveGrabPostion( + std::vector< std::vector>& scanLines, + const SSG_gloveArcParam arcPara, + SSG_treeGrowParam growParam, + std::vector& objOps, + int* errCode); From f855ffd70b0f049c3fe0819fbcedb86caf8079af Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 10 Sep 2025 23:59:05 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E6=90=AD=E6=8E=A5=E7=84=8A=E7=BC=9D?= =?UTF-8?q?=E7=AE=97=E6=B3=95=E5=88=9D=E6=AC=A1=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 53 ++ bagPositioning_test/bagPositioning_test.cpp | 344 ++++++-- lapWeldDetection/lapWeldDetection.vcxproj | 172 ++++ .../lapWeldDetection_test.cpp | 829 ++++++++++++++++++ .../lapWeldDetection_test.vcxproj | 157 ++++ sourceCode/SG_baseAlgo_Export.h | 22 + sourceCode/SG_baseDataType.h | 3 + sourceCode/SG_baseFunc.cpp | 338 +++++++ sourceCode/SG_lineFeature.cpp | 371 ++++++++ sourceCode/SX_lapWeldDetection.cpp | 216 +++++ sourceCode/SX_lapWeldDetection_Export.h | 40 + 11 files changed, 2491 insertions(+), 54 deletions(-) create mode 100644 lapWeldDetection/lapWeldDetection.vcxproj create mode 100644 lapWeldDetection_test/lapWeldDetection_test.cpp create mode 100644 lapWeldDetection_test/lapWeldDetection_test.vcxproj create mode 100644 sourceCode/SX_lapWeldDetection.cpp create mode 100644 sourceCode/SX_lapWeldDetection_Export.h diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 740736d..cf60398 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -86,6 +86,27 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning_test", "gl {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} = {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bottleCapQualityCheck", "bottleCapQualityCheck\bottleCapQualityCheck.vcxproj", "{DE243631-2472-45B7-84DD-06C8B5DC1D5A}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bottleCapQualityCheck_test", "bottleCapQualityCheck_test\bottleCapQualityCheck_test.vcxproj", "{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}" + ProjectSection(ProjectDependencies) = postProject + {DE243631-2472-45B7-84DD-06C8B5DC1D5A} = {DE243631-2472-45B7-84DD-06C8B5DC1D5A} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection", "lapWeldDetection\lapWeldDetection.vcxproj", "{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection_test", "lapWeldDetection_test\lapWeldDetection_test.vcxproj", "{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} = {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -230,6 +251,38 @@ Global {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.Build.0 = Release|x64 {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.ActiveCfg = Release|Win32 {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.Build.0 = Release|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x64.ActiveCfg = Debug|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x64.Build.0 = Debug|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x86.ActiveCfg = Debug|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x86.Build.0 = Debug|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x64.ActiveCfg = Release|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x64.Build.0 = Release|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x86.ActiveCfg = Release|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x86.Build.0 = Release|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x64.ActiveCfg = Debug|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x64.Build.0 = Debug|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x86.ActiveCfg = Debug|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x86.Build.0 = Debug|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x64.ActiveCfg = Release|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x64.Build.0 = Release|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x86.ActiveCfg = Release|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x86.Build.0 = Release|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x64.ActiveCfg = Debug|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x64.Build.0 = Debug|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x86.ActiveCfg = Debug|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x86.Build.0 = Debug|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x64.ActiveCfg = Release|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x64.Build.0 = Release|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x86.ActiveCfg = Release|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x86.Build.0 = Release|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x64.ActiveCfg = Debug|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x64.Build.0 = Debug|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x86.ActiveCfg = Debug|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x86.Build.0 = Debug|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.ActiveCfg = Release|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.Build.0 = Release|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.ActiveCfg = Release|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 9fdb34c..85a2bae 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -77,7 +77,9 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int lineIdx = 0; int ptIdx = 0; int ptNum = 0; + int pre_ptNum = -1; std::vector< SVzNLPointXYZRGBA> a_line; + int vldLineIdx = 0; int vldPtNum = 0; unsigned int timeStamp = 0; while (getline(inputFile, linedata)) @@ -119,20 +121,25 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, break; int recvPtNum = (int)a_line.size(); - if ( (recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + if ( (recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) { - SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); - _scanLines[lineIdx].nPointCnt = ptNum; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPoint = p3DPoint; - for (int m = 0; m < ptNum; m++) + SVzNLPointXYZRGBA* p3DPoint; + if (pre_ptNum > 0) + p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); + else + p3DPoint = NULL; + _scanLines[vldLineIdx].nPointCnt = pre_ptNum; + _scanLines[vldLineIdx].nTimeStamp = timeStamp; + _scanLines[vldLineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < pre_ptNum; m++) p3DPoint[m] = a_line[m]; - lineIdx++; + vldLineIdx++; } //new Line timeStamp = curr_timeStamp; vldPtNum = 0; a_line.clear(); + pre_ptNum = ptNum; } else if (0 == strncmp("{", linedata.c_str(), 1)) { @@ -167,19 +174,23 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, } //last line int recvPtNum = (int)a_line.size(); - if ((recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) { - SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); - _scanLines[lineIdx].nPointCnt = ptNum; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPoint = p3DPoint; - for (int m = 0; m < ptNum; m++) + SVzNLPointXYZRGBA* p3DPoint; + if (pre_ptNum > 0) + p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); + else + p3DPoint = NULL; + _scanLines[vldLineIdx].nPointCnt = pre_ptNum; + _scanLines[vldLineIdx].nTimeStamp = timeStamp; + _scanLines[vldLineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < pre_ptNum; m++) p3DPoint[m] = a_line[m]; - lineIdx++; + vldLineIdx++; } if (scanLineNum) - *scanLineNum = lineIdx; + *scanLineNum = vldLineIdx; inputFile.close(); return _scanLines; } @@ -400,6 +411,133 @@ SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* sc return _scanLines; } +//浠嶳GBD鐐逛簯涓鍙朮YZ淇℃伅 +SVzNL3DLaserLine* vzReadXYZPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //绗竴琛 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + float leftX, leftY; + float rightX, rightY; + float r, g, b; + sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + inputFile.close(); + return _scanLines; +} + SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints, int lineNum, double _F, double* camPoseR, int* outLineNum) { int min_y = 100000000; @@ -1875,7 +2013,7 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in else y_scale = x_scale; - int angleDrawLen = dirLen / x_scale; + int angleDrawLen = dirLen;// / x_scale; _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); //鏃嬭浆瑙嗚鏄剧ず @@ -2373,7 +2511,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData, #define TEST_COMPUTE_GRASP_POINT 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_GROUP 22 +#define TEST_GROUP 27 #define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8) #define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6) int main() @@ -2407,13 +2545,13 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_dir, "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜_2\\"); - char _scan_file[256]; - double _F = 1247.95;// 1231.2; //1729.0;; //f - for (int i = 1; i <= 28; i++) + sprintf_s(_scan_dir, "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\绾哥鎷嗚泛鐐逛簯\\"); + char _scan_src_file[256]; + double _F = 1802.16; //1247.95;// 1231.2; //1729.0;; //f + for (int i = 5; i <= 22; i++) { - sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i); - SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + sprintf_s(_scan_src_file, "%sLaserLine%d.txt", _scan_dir, i); + SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_src_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond, true); if (laser3DPoints_RGBD == NULL) continue; @@ -2429,7 +2567,11 @@ int main() #endif int vldLineNum = 0; SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F, camPoseR, &vldLineNum); - + char _out_file[256]; + sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); + _outputScanDataFile_self(_out_file, gridData, vldLineNum, + lineV, maxTimeStamp, clockPerSecond); +#if 0 //鐢熸垚姘村钩鎵弿鏁版嵁 int hLineNum = gridData[0].nPositionCnt; SVzNL3DLaserLine* hScanData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * hLineNum); @@ -2448,15 +2590,11 @@ int main() hScanData[hLine].p3DPosition[m].pt3D.z = gridData[m].p3DPosition[hLine].pt3D.z; } } - char _out_file[256]; - sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); - _outputScanDataFile_self(_out_file, gridData, vldLineNum, - lineV, maxTimeStamp, clockPerSecond); sprintf_s(_out_file, "%sLaserLine%d_hScanData.txt", _scan_dir, i); _outputScanDataFile_removeZeros(_out_file, hScanData, hLineNum, lineV, maxTimeStamp, clockPerSecond); - - printf("%s: convert done!\n", _scan_file); +#endif + printf("%s: convert done!\n", _scan_src_file); } #endif @@ -2512,20 +2650,27 @@ int main() "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\鐐逛簯12_鐩愯\\", //11 "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\鐐逛簯13_鐜板満娴嬭瘯\\", //12 "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 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\鎷嗗寘妯℃嫙鐜\\", //14 + "F:\\ShangGu\\绾哥鎷嗗灈鏁版嵁\\vizum鏁版嵁\\", //15 + "F:\\ShangGu\\绾哥鎷嗗灈鏁版嵁\\380脳480mm绾哥RGB鐐逛簯鍙2D鍥惧儚\\", //16 + "F:\\ShangGu\\绾哥鎷嗗灈鏁版嵁\\380脳580mm绾哥RGB鐐逛簯鍙2d鍥惧儚\\", //17 + "F:\\ShangGu\\绾哥鎷嗗灈鏁版嵁\\480脳580mm绾哥rgb鐐逛簯鍙2d鍥惧儚\\", //18 + + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\", //19 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜_2\\", //20 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\渚ф姄鏁版嵁\\", //21 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\渚ф姄鏁版嵁_鐜板満\\20250419\\", //22 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\渚ф姄鏁版嵁_鐜板満\\20250420-1\\", //23 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\渚ф姄鏁版嵁_鐜板満\\20250420-2\\", //24 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\渚ф姄鏁版嵁_鐜板満\\20250420-3\\", //25 + "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\渚ф姄鏁版嵁_鐜板満\\20250420-4\\", //26 }; 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,3}, + {2,4}, {1,5}, {1,1}, {1,3}, {11,11}, + {5,22},{1, 15},{1,15}, {1, 15}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -2549,7 +2694,7 @@ int main() SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 14; grp <= 15; grp++) + for (int grp = 17; grp <= 18; grp++) { if (grp < 10) { @@ -2587,7 +2732,67 @@ int main() algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm } - else if ( (grp >= 14) && (grp <= 15)) + else if (grp == 14) + { + algoParam.bagParam.bagL = 750; //琚嬪瓙闀65cm + algoParam.bagParam.bagW = 420; //琚嬪瓙瀹40cm + algoParam.bagParam.bagH = 150; //琚嬪瓙楂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 == 15) //绾哥鎷嗗灈鏁版嵁\vizum鏁版嵁 + { + algoParam.bagParam.bagL = 400; //琚嬪瓙闀65cm + algoParam.bagParam.bagW = 255; //琚嬪瓙瀹40cm + algoParam.bagParam.bagH = 300; //琚嬪瓙楂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 == 16) //绾哥鎷嗗灈鏁版嵁\380脳480mm绾哥RGB鐐逛簯鍙2D鍥惧儚 + { + algoParam.bagParam.bagL = 480; //琚嬪瓙闀 + algoParam.bagParam.bagW = 380; //琚嬪瓙瀹 + algoParam.bagParam.bagH = 580; //琚嬪瓙楂 + 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 == 17) //绾哥鎷嗗灈鏁版嵁\380脳580mm绾哥RGB鐐逛簯鍙2d鍥惧儚 + { + algoParam.bagParam.bagL = 580; //琚嬪瓙闀 + algoParam.bagParam.bagW = 380; //琚嬪瓙瀹 + algoParam.bagParam.bagH = 480; //琚嬪瓙楂 + 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 == 18) //绾哥鎷嗗灈鏁版嵁\480脳580mm绾哥rgb鐐逛簯鍙2d鍥惧儚 + { + algoParam.bagParam.bagL = 580; //琚嬪瓙闀 + algoParam.bagParam.bagW = 480; //琚嬪瓙瀹 + algoParam.bagParam.bagH = 380; //琚嬪瓙楂 + 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 >= 19) && (grp <= 20)) { algoParam.bagParam.bagL = 750; //琚嬪瓙闀65cm algoParam.bagParam.bagW = 450; //琚嬪瓙瀹40cm @@ -2612,10 +2817,10 @@ int main() algoParam.growParam.minVTypeTreeLen = 50.0; //mm } #if BAG_ALGO_USE_CORNER_FEATURE - algoParam.cornerParam.cornerTh = 30; //45搴﹁ - algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + 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 = algoParam.bagParam.bagH / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_2 = 15; #else @@ -2695,13 +2900,28 @@ int main() poseCalibPara.invRMatrix[8] = 0.999699; } - else if ( grp == 14) + else if (grp == 14) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\鎷嗗寘妯℃嫙鐜\\ground_calib_para2.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if ( (grp >= 15) && (grp <= 18)) //绠卞瓙 + { + algoParam.cornerParam.cornerTh = 45; //45搴﹁ + algoParam.cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 5;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 5; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; + } + else if ( grp == 19) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } - else if (grp == 15) + else if (grp == 20) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\缂栫粐琚嬫暟鎹甛\缂栫粐琚婻GBD璇嗗埆鏂瑰悜_2\\ground_calib_para.txt"); @@ -2717,11 +2937,24 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); - SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + SVzNL3DLaserLine* laser3DPoints = NULL; + if ((grp >= 16) && (grp <= 18)) + { + sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx); + laser3DPoints = vzReadXYZPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + } + else + { + sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); + laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + } if (laser3DPoints == NULL) continue; - +#if 0 //鏁版嵁杞瓨 + sprintf_s(_scan_file, "%sLaserLine%d_grid_noRGB.txt", dataPath[grp], fidx); + _outputScanDataFile_self(_scan_file, laser3DPoints, lineNum, + lineV, maxTimeStamp, clockPerSecond); +#endif algoParam.filterParam.continuityTh = 20.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ algoParam.filterParam.outlierTh = 5; @@ -2749,7 +2982,11 @@ int main() //璋冨钩锛屽幓闄ゅ湴闈 sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } - +#if 0 //鏁版嵁杞瓨 + sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx); + _outputScanDataFile_self(_scan_file, laser3DPoints, lineNum, + lineV, maxTimeStamp, clockPerSecond); +#endif std::vector objOps; sg_getBagPosition(laser3DPoints, lineNum, algoParam, poseCalibPara, objOps); #endif @@ -2764,7 +3001,7 @@ int main() sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx); cv::String imgName(_dbg_file); double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // - double angleDrawLen = algoParam.bagParam.bagL / 3; + double angleDrawLen = algoParam.bagParam.bagL * 0.4; _genXOYProjectionImage(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen); #endif #if 0 @@ -2788,8 +3025,7 @@ int main() } else if (grp < TEST_TOP_ORIEN_GROUP) { - double FBVldPtRatioTh; - + fidx = 0; SSG_hsvCmpParam colorCmpParam; //鑹插害鍜岃壊楗卞拰搴︽瘮杈冮棬闄愶紝灏忎簬闂ㄩ檺涓哄悓涓棰滆壊 colorCmpParam.hueTh = 15.0; colorCmpParam.saturateTh = 120.0; //60 @@ -2835,7 +3071,7 @@ int main() int kkk = 1; //琛屽鐞 //璋冨钩锛屽幓闄ゅ湴闈 - sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); + //sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } #if 0 char _out_file[256]; diff --git a/lapWeldDetection/lapWeldDetection.vcxproj b/lapWeldDetection/lapWeldDetection.vcxproj new file mode 100644 index 0000000..fb8d3ac --- /dev/null +++ b/lapWeldDetection/lapWeldDetection.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {0c0b9b7a-9bf2-4157-bffd-d12012e39fa0} + lapWeldDetection + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + + + + + Level3 + true + true + true + NDEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + + + + + + \ No newline at end of file diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp new file mode 100644 index 0000000..266ef49 --- /dev/null +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -0,0 +1,829 @@ +锘// glovePositioning_test.cpp : 姝ゆ枃浠跺寘鍚 "main" 鍑芥暟銆傜▼搴忔墽琛屽皢鍦ㄦ澶勫紑濮嬪苟缁撴潫銆 +// +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "SX_lapWeldDetection_Export.h" +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool exchangeXY) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + double X, Y, Z; + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &X, &Y, &Z); + SVzNL3DPoint a_pt; + if (true == exchangeXY) + { + a_pt.x = Y; //灏嗘壂鎻忕嚎璋冩暣涓篩鏂瑰悜锛涙壂鎻忔柟鍚戜负X鏂瑰悜 + a_pt.y = X; + } + else + { + a_pt.x = X; + a_pt.y = Y; + } + a_pt.z = Z; + scanData.push_back(a_pt); + } + return; +} + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int lineIdx = 0; + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + if (true == removeZeros) + { + int vldPtNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldPtNum++; + } + linePtNum = vldPtNum; + } + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + +//杈撳嚭姘村钩鎵弿鏁版嵁 +void _outputScanDataFile_vector_h(char* fileName, std::vector>& scanLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines[0].size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int linePtNum = (int)scanLines.size(); + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[i][line].pt3D; + + float x = (float)pt3D->y; + float y = (float)pt3D->x; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + +void _outputRGBDScanLapWeld_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector< std::vector> weldOPs) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (weldOPs.size() > 0) + realLines++; + + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//娣￠粍鑹 + {210,105, 30},//宸у厠鍔涜壊 + {240,230,140},//榛勮鑹 + {135,206,235},//澶╄摑鑹 + {250,235,215},//鍙よ懀鐧 + {189,252,201},//钖勮嵎鑹 + {221,160,221},//姊呯孩鑹 + {188,143,143},//鐜懓绾㈣壊 + }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + int featureType_v = pt3D->nPointIdx & 0xffff; + int featureType_h = featureType_v >> 4; + featureType_v &= 0xff; + if (LINE_FEATURE_PEAK_TOP == featureType_v) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else if (LINE_FEATURE_PEAK_TOP == featureType_h) + { + rgb = { 97, 255, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (weldOPs.size() > 0) + { + int weldNum = (int)weldOPs.size(); + int linePtNum = 0; + for (int i = 0; i < weldNum; i++) + linePtNum += (int)weldOPs[i].size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + + rgb = { 0, 0, 255 }; + size = 25; + for (int i = 0; i < weldNum; i++) + { + rgb = objColor[i % 8]; + for (int j = 0; j < (int)weldOPs[i].size(); j++) + { + float x = (float)weldOPs[i][j].x; + float y = (float)weldOPs[i][j].y; + float z = (float)weldOPs[i][j].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + //鍔犱竴涓偣锛岀敤浜庤烦杩囨樉绀哄伐鍏穊ug + rgb = objColor[0]; + float x = (float)weldOPs[0][0].x; + float y = (float)weldOPs[0][0].y; + float z = (float)weldOPs[0][0].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + sw.close(); +} + + +void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum) +{ + std::ofstream sw(fileName); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + + +#define DATA_VER_OLD 0 +#define DATA_VER_NEW 1 +#define DATA_VER_FROM_CUSTOM 2 +#define VZ_LASER_LINE_PT_MAX_NUM 4096 +SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //绗竴琛 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + else if (dataFileVer == DATA_VER_OLD) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + { + sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); + if ((dataElements != 3) && (dataElements != 4)) + break; + } + if (0 == strncmp("LineV_", linedata.c_str(), 6)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); +#if 0 + if (scanLineListTail == NULL) + firstIndex = lineIndex; +#endif + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + //new Line + //new Line + lineIdx++; + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + } + else if (0 == strncmp("(", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + if (dataElements == 4) + sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); + else + sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < VZ_LASER_LINE_PT_MAX_NUM) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + } + inputFile.close(); + return _scanLines; +} + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //璋冨钩鐭╅樀 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //鍦伴潰楂樺害 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //鍙嶅悜鏃嬭浆鐭╅樀 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //璁剧疆鍒濆缁撴灉 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //璋冨钩鐭╅樀 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //鍦伴潰楂樺害 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //鍙嶅悜鏃嬭浆鐭╅樀 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + +void _getRoiClouds( + std::vector< std::vector>& scanLines, + int startLine, + int endLine, + int startPtIdx, + int endPtIdx, + std::vector< std::vector>& roiScanLines) +{ + for (int i = startLine; i < endLine; i++) + { + if (i >= scanLines.size()) + break; + + std::vector cut_line; + std::vector& a_line = scanLines[i]; + for (int j = startPtIdx; j < endPtIdx; j++) + { + SVzNL3DPosition a_pt; + if (j >= a_line.size()) + { + a_pt.nPointIdx = 0; + a_pt.pt3D = { 0,0,0 }; + } + else + a_pt = a_line[j]; + cut_line.push_back(a_pt); + } + roiScanLines.push_back(cut_line); + } + return; +} + +void _convertToGridData(std::vector< std::vector>& scanLines, + std::vector< std::vector>& gridScanLines, + double y_step) +{ + double y_min = scanLines[0][0].pt3D.y; + double y_max = y_min; + int lineNum = (int)scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < ptNum; i++) + { + if (y_min > a_line[i].pt3D.y) + y_min = a_line[i].pt3D.y; + if (y_max < a_line[i].pt3D.y) + y_max = a_line[i].pt3D.y; + } + } + double half_step = y_step / 2; + int gridPtNum = (int)((y_max - y_min + half_step) / y_step) + 1; + for (int line = 0; line < lineNum; line++) + { + std::vector grid_line; + grid_line.resize(gridPtNum); + for (int i = 0; i < gridPtNum; i++) + grid_line[i] = { 0, {0.0, 0.0, 0.0} }; + + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < ptNum; i++) + { + int ptIdx = (int)((a_line[i].pt3D.y + half_step - y_min) / y_step); + grid_line[ptIdx] = a_line[i]; + } + gridScanLines.push_back(grid_line); + } + return; +} + +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_GLOVE_POSITION 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:\\ShangGu\\椤圭洰\\閽㈡澘鎼帴鐒婄紳妫娴媆\鏁版嵁\\", //0 + + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {0,6} + }; + +#if CONVERT_TO_GRID + int convertGrp = 0; + for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%sglove_%d.txt", dataPath[convertGrp], fidx); + std::vector< SVzNL3DPoint> scanData; + bool exchangeXY = true; + vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, exchangeXY); + //灏嗘暟鎹仮澶嶄负鎸夋壂鎻忕嚎瀛樺偍鏍煎紡 + std::vector< std::vector> scanLines; + wd_getScanLines(scanData, scanLines); + + double y_step; + if ((fidx == 4) || (fidx == 5)) + y_step = 0.106; + else + y_step = 0.091; + std::vector< std::vector> grid_scanLines; + _convertToGridData(scanLines, grid_scanLines, y_step); + sprintf_s(_scan_file, "%sglove_%d_scanLine.txt", dataPath[convertGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, grid_scanLines, false, &headNullLines); + printf("%s: head null lines = %d\n", _scan_file, headNullLines); +#if 1 + sprintf_s(_scan_file, "%sglove_%d_scanLine_h.txt", dataPath[convertGrp], fidx); + _outputScanDataFile_vector_h(_scan_file, grid_scanLines); +#endif + } +#endif + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:\\ShangGu\\椤圭洰\\閽㈡澘鎼帴鐒婄紳妫娴媆\鏁版嵁\\scanData_ground_1.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = sx_getBaseCalibPara( scanData); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + sx_lineDataR(scanData[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\椤圭洰\\閽㈡澘鎼帴鐒婄紳妫娴媆\鏁版嵁\\ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:\\ShangGu\\椤圭洰\\閽㈡澘鎼帴鐒婄紳妫娴媆\鏁版嵁\\scanData_ground_1_calib.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_GLOVE_POSITION + for (int grp = 0; grp <= 0; grp++) + { + SSG_planeCalibPara poseCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\椤圭洰\\閽㈡澘鎼帴鐒婄紳妫娴媆\鏁版嵁\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =1; + char _scan_file[256]; + sprintf_s(_scan_file, "%sscanData_%d.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);// calibPara.planeHeight); + } +#if 0 + char _out_file[256]; + sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines); +#endif + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 5; + growParam.yDeviation_max = 1.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //琚嬪瓙楂樺害1/2 + growParam.minLTypeTreeLen = 30.0; //mm + growParam.minVTypeTreeLen = 30.0; //mm + SSG_cornerParam cornerParam; + cornerParam.cornerTh =30; //45搴﹁ + cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + cornerParam.jumpCornerTh_1 = 10; + cornerParam.jumpCornerTh_2 = 30; + SSX_lapWeldParam lapWeldParam; + lapWeldParam.lapHeight = 2.0; + lapWeldParam.weldMinLen = 2.0; + lapWeldParam.weldRefPoints = 2; + int errCode = 0; + std::vector> weldOps; + sx_getLapWeldPostion( + scanLines, + cornerParam, + growParam, + lapWeldParam, + weldOps, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps); + } + } +#endif +} + +// 杩愯绋嬪簭: Ctrl + F5 鎴栬皟璇 >鈥滃紑濮嬫墽琛(涓嶈皟璇)鈥濊彍鍗 +// 璋冭瘯绋嬪簭: F5 鎴栬皟璇 >鈥滃紑濮嬭皟璇曗濊彍鍗 + +// 鍏ラ棬浣跨敤鎶宸: +// 1. 浣跨敤瑙ｅ喅鏂规璧勬簮绠＄悊鍣ㄧ獥鍙ｆ坊鍔/绠＄悊鏂囦欢 +// 2. 浣跨敤鍥㈤槦璧勬簮绠＄悊鍣ㄧ獥鍙ｈ繛鎺ュ埌婧愪唬鐮佺鐞 +// 3. 浣跨敤杈撳嚭绐楀彛鏌ョ湅鐢熸垚杈撳嚭鍜屽叾浠栨秷鎭 +// 4. 浣跨敤閿欒鍒楄〃绐楀彛鏌ョ湅閿欒 +// 5. 杞埌鈥滈」鐩>鈥滄坊鍔犳柊椤光濅互鍒涘缓鏂扮殑浠ｇ爜鏂囦欢锛屾垨杞埌鈥滈」鐩>鈥滄坊鍔犵幇鏈夐」鈥濅互灏嗙幇鏈変唬鐮佹枃浠舵坊鍔犲埌椤圭洰 +// 6. 灏嗘潵锛岃嫢瑕佸啀娆℃墦寮姝ら」鐩紝璇疯浆鍒扳滄枃浠垛>鈥滄墦寮鈥>鈥滈」鐩濆苟閫夋嫨 .sln 鏂囦欢 diff --git a/lapWeldDetection_test/lapWeldDetection_test.vcxproj b/lapWeldDetection_test/lapWeldDetection_test.vcxproj new file mode 100644 index 0000000..38f1f0f --- /dev/null +++ b/lapWeldDetection_test/lapWeldDetection_test.vcxproj @@ -0,0 +1,157 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {bd65ac40-a2c9-4c8d-adba-aba595c5f32d} + lapWeldDetectiontest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + opencv_world480d.lib;lapWeldDetection.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + opencv_world480.lib;lapWeldDetection.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index d318ccc..d04c772 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -60,6 +60,22 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature( const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 SSG_lineFeature* line_features); +/// +/// 提取激光线上的Jumping特征 +/// nPointIdx被重新定义成Feature类型 +/// 算法流程： +/// （1）逐点计算前向角和后向角 +/// （2）逐点计算拐角，顺时针为负，逆时针为正 +/// （3）搜索正拐角的极大值。 +/// （4）判断拐角是否为跳变 +/// +SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 + std::vector< SSG_basicFeature1D> jumpFeatures); + SG_APISHARED_EXPORT void wd_getLineGloveArcs( std::vector& lineData, int lineIdx, @@ -278,6 +294,12 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara( SVzNL3DLaserLine* laser3DPoints, int lineNum); +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2( + std::vector< std::vector>& scanLines); + //计算一个平面调平参数。 //以数据输入中ROI以内的点进行平面拟合，计算调平参数 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index a3fcf08..c2e415e 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -402,6 +402,7 @@ typedef struct typedef struct { int pntIdx; + int type; double forwardAngle; //前向角 double backwardAngle; //后向角 double corner; //拐角 @@ -409,4 +410,6 @@ typedef struct double backwardDiffZ; double pre_stepDist; double post_stepDist; + double forward_z; + double backward_z; }SSG_pntDirAngle; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 66acdd8..f251c97 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -1207,6 +1207,344 @@ SSG_planeCalibPara sg_getPlaneCalibPara( return planePara; } +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SSG_planeCalibPara sg_getPlaneCalibPara2( + std::vector< std::vector>& scanLines) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + + int lineNum = (int)scanLines.size(); + //统计z范围 + SVzNLRangeD zRange = { 0, -1 }; //< Z范围 + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + //z + if (zRange.max < zRange.min) + { + zRange.min = pt3D->pt3D.z; + zRange.max = pt3D->pt3D.z; + } + else + { + if (zRange.min > pt3D->pt3D.z) + zRange.min = pt3D->pt3D.z; + if (zRange.max < pt3D->pt3D.z) + zRange.max = pt3D->pt3D.z; + } + } + } + + //在Z方向进行统计，取第一个极值 + //以mm为单位，简化量化 + int zHistSize = (int)(zRange.max - zRange.min) + 1; + if (zHistSize == 0) + return planePara; + + std::vector zHist; + zHist.resize(zHistSize); + int totalPntSize = 0; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + totalPntSize++; + int histPos = (int)(pt3D->pt3D.z - zRange.min); + zHist[histPos] ++; + } + } + std::vector zSumHist; + zSumHist.resize(zHistSize); + bool isSame = true; + //以厘米为单位进行累加 + for (int i = 0; i < zHistSize; i++) + { + int sumValue = 0; + for (int j = i - 5; j <= i + 5; j++) + { + if ((j >= 0) && (j < zHistSize)) + sumValue += zHist[j]; + } + zSumHist[i] = sumValue; + if (i > 0) + { + if (sumValue != zSumHist[i - 1]) + isSame = false; + } + } + if (true == isSame) + { + //不进行累加（如果累加，累加值相等） + for (int i = 0; i < zHistSize; i++) + zSumHist[i] = zHist[i]; + } + + //寻找极值 + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + int pre_data = -1; + std::vector< SSG_intPair> pkTop; + std::vector< SSG_intPair> pkBtm; + std::vector pkBtmBackIndexing; + pkBtmBackIndexing.resize(zHistSize); + for (int i = 0; i < zHistSize; i++) + pkBtmBackIndexing[i] = -1; + + + for (int i = 0; i < zHistSize; i++) + { + int curr_data = zSumHist[i]; + if (pre_data < 0) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double z_diff = curr_data - pre_data; + switch (_state) + { + case 0: //初态 + if (z_diff < 0) //下降 + { + _state = 2; + } + else if (z_diff > 0) //上升 + { + _state = 1; + } + break; + case 1: //上升 + if (z_diff < 0) //下降 + { + pkTop.push_back({ pre_i, pre_data }); + _state = 2; + } + else if (i == (zHistSize - 1)) + pkTop.push_back({ i, curr_data }); + break; + case 2: //下降 + if (z_diff > 0) // 上升 + { + int pkBtmIdx = (int)pkBtm.size(); + pkBtmBackIndexing[pre_i] = pkBtmIdx; + pkBtm.push_back({ pre_i, pre_data }); + _state = 1; + } + else if (i == (zHistSize - 1)) + { + int pkBtmIdx = (int)pkBtm.size(); + pkBtmBackIndexing[i] = pkBtmIdx; + pkBtm.push_back({ i, curr_data }); + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //寻找第一个超过总点数1/3的极值点 + if (pkTop.size() < 1) + return planePara; + + int pntSizeTh = totalPntSize / 10; + SSG_intPair* vldPeak = NULL; + for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++) + { + if (pkTop[i].data_1 > pntSizeTh) + { + vldPeak = &pkTop[i]; + break; + } + } + if (NULL == vldPeak) + return planePara; + + //寻找开始和结束位置 + //向前向后寻找 + int preBtmIdx = -1; + for (int j = vldPeak->data_0 - 1; j >= 0; j--) + { + if (pkBtmBackIndexing[j] >= 0) + { + int idx = pkBtmBackIndexing[j]; + if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2)) + { + preBtmIdx = j; + break; + } + } + } + int postBtmIdx = -1; + for (int j = vldPeak->data_0 + 1; j < zHistSize; j++) + { + if (pkBtmBackIndexing[j] >= 0) + { + int idx = pkBtmBackIndexing[j]; + if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2)) + { + postBtmIdx = j; + break; + } + } + } + + SVzNLRangeD topZRange; + if (preBtmIdx < 0) + topZRange.min = zRange.min; + else + topZRange.min = (float)preBtmIdx + zRange.min; + if (postBtmIdx < 0) + topZRange.max = zRange.max; + else + topZRange.max = (float)postBtmIdx + zRange.min; + + //取数据 + std::vector Points3ds; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max)) + { + cv::Point3f a_vldPt; + a_vldPt.x = (float)pt3D->pt3D.x; + a_vldPt.y = (float)pt3D->pt3D.y; + a_vldPt.z = (float)pt3D->pt3D.z; + Points3ds.push_back(a_vldPt); + } + } + } + //平面拟合 + std::vector planceFunc; + vzCaculateLaserPlane(Points3ds, planceFunc); + +#if 1 //两个向量的旋转旋转，使用四元数法， + Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]); + Vector3 b = Vector3(0, 0, -1.0); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //计算反向旋转矩阵 + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); +#else //根据平面的法向量计算欧拉角，进而计算旋转矩阵 + //参数计算 + SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]); + //反射进行校正 + eulerPra.roll = eulerPra.roll; + eulerPra.pitch = eulerPra.pitch; + eulerPra.yaw = eulerPra.yaw; + RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll); +#endif + + planePara.planeCalib[0] = rMatrix.data[0][0]; + planePara.planeCalib[1] = rMatrix.data[0][1]; + planePara.planeCalib[2] = rMatrix.data[0][2]; + planePara.planeCalib[3] = rMatrix.data[1][0]; + planePara.planeCalib[4] = rMatrix.data[1][1]; + planePara.planeCalib[5] = rMatrix.data[1][2]; + planePara.planeCalib[6] = rMatrix.data[2][0]; + planePara.planeCalib[7] = rMatrix.data[2][1]; + planePara.planeCalib[8] = rMatrix.data[2][2]; + + planePara.invRMatrix[0] = invMatrix.data[0][0]; + planePara.invRMatrix[1] = invMatrix.data[0][1]; + planePara.invRMatrix[2] = invMatrix.data[0][2]; + planePara.invRMatrix[3] = invMatrix.data[1][0]; + planePara.invRMatrix[4] = invMatrix.data[1][1]; + planePara.invRMatrix[5] = invMatrix.data[1][2]; + planePara.invRMatrix[6] = invMatrix.data[2][0]; + planePara.invRMatrix[7] = invMatrix.data[2][1]; + planePara.invRMatrix[8] = invMatrix.data[2][2]; + +#if 0 //test: 两个矩阵的乘积必须是单位阵 + double testMatrix[3][3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + testMatrix[i][j] = 0; + for (int m = 0; m < 3; m++) + testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j]; + } + } +#endif + //数据进行转换 + SVzNLRangeD calibZRange = { 0, -1 }; + topZRange = { 0, -1 }; + for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) + { + //z + if (topZRange.max < topZRange.min) + { + topZRange.min = Points3ds[i].z; + topZRange.max = Points3ds[i].z; + } + else + { + if (topZRange.min > Points3ds[i].z) + topZRange.min = Points3ds[i].z; + if (topZRange.max < Points3ds[i].z) + topZRange.max = Points3ds[i].z; + } + cv::Point3f a_calibPt; + a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]); + a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]); + a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]); + //z + if (calibZRange.max < calibZRange.min) + { + calibZRange.min = a_calibPt.z; + calibZRange.max = a_calibPt.z; + } + else + { + if (calibZRange.min > a_calibPt.z) + calibZRange.min = a_calibPt.z; + if (calibZRange.max < a_calibPt.z) + calibZRange.max = a_calibPt.z; + } + } + planePara.planeHeight = calibZRange.min; + + return planePara; +} + //计算一个平面调平参数。 //以数据输入中ROI以内的点进行平面拟合，计算调平参数 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index a011912..f39f7ac 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -1438,6 +1438,377 @@ void sg_getLineCornerFeature( return; } +bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { + return a.pntIdx < b.pntIdx; +} +/// +/// 提取激光线上的Jumping特征 +/// nPointIdx被重新定义成Feature类型 +/// 算法流程： +/// （1）逐点计算前向角和后向角 +/// （2）逐点计算拐角，顺时针为负，逆时针为正 +/// （3）搜索正拐角的极大值。 +/// （4）判断拐角是否为跳变 +/// +void sg_getLineJumpFeature_cornerMethod( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 + std::vector< SSG_basicFeature1D> jumpFeatures) +{ + //去除零点 + std::vector< SVzNL3DPosition> vldPts; + std::vector segs; + //修改seg的定义。seg端点是两点间距离大于门限的点 + int segStart = -1, segEnd = -1; + for (int i = 0; i < dataSize; i++) + { + 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; + } + else + { + if (segStart >= 0) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + segStart = -1; + segEnd = -1; + } + } + } + //last + if (segStart >= 0) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + } + + //计算前向角和后向角 + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 875) + int kkk = 1; + + //前向寻找 + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //后向寻找 + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //计算拐角 + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + corners[i].forward_z = 0; + corners[i].backward_z = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; //沿序号增长方向 + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; //沿序号减小方向 + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号 + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + corners[i].forward_z = vldPts[post_i].pt3D.z; //沿序号增长方向 + corners[i].backward_z = vldPts[pre_i].pt3D.z; //沿序号减小方向 + } + } + + //搜索拐角极值 + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //最后一个 + { + if (1 == _state) //上升 + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //下降 + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //初态 + if (cornerDiff < 0) //下降 + { + _state = 2; + } + else if (cornerDiff > 0) //上升 + { + _state = 1; + } + break; + case 1: //上升 + if (cornerDiff < 0) //下降 + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //下降 + if (cornerDiff > 0) // 上升 + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //注意：最后一个不处理，为基座位置 + + //极小值点（峰顶） + //极值比较，在尺度窗口下寻找局部极值点 + std::vector< SSG_pntDirAngle> cornerFeatures; + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。 + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //向前搜索 + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //超出尺度窗口 + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + //向后搜索 + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //超出尺度窗口 + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_HR; + cornerFeatures.push_back(cornerPeakP[i]); + } + else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_FH; + cornerFeatures.push_back(cornerPeakP[i]); + } + } + } + for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++) + { + if (abs(cornerPeakM[i].corner) < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //向前搜索 + int cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //超出尺度窗口 + break; + + if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + //向后搜索 + if (true == isPeak) + { + cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //超出尺度窗口 + break; + + if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_HF; + cornerFeatures.push_back(cornerPeakM[i]); + } + else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_RH; + cornerFeatures.push_back(cornerPeakM[i]); + } + } + } + + //排序 + std::sort(cornerFeatures.begin(), cornerFeatures.end(), compareByIdx); + + //进行组合获得Jump + for (int i = 0, i_max = (int)cornerFeatures.size(); i < i_max-1; i++) + { + if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HR) + { + if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_RH) + { + int pntIdx_1 = cornerFeatures[i].pntIdx; + int pntIdx_2 = cornerFeatures[i+1].pntIdx; + //计算距离和跳变高度 + double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); + double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; + if ((dist < cornerPara.minEndingGap) && (height < -cornerPara.minEndingGap_z)) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_feature.jumpPos = vldPts[pntIdx_1].pt3D; + a_feature.jumpPos2D = { lineIdx, pntIdx_1 }; + jumpFeatures.push_back(a_feature); + } + } + } + else if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HF) + { + if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_FH) + { + int pntIdx_1 = cornerFeatures[i].pntIdx; + int pntIdx_2 = cornerFeatures[i + 1].pntIdx; + //计算距离和跳变高度 + double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); + double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; + if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z)) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + a_feature.jumpPos = vldPts[pntIdx_2].pt3D; + a_feature.jumpPos2D = { lineIdx, pntIdx_2 }; + jumpFeatures.push_back(a_feature); + } + } + } + } + return; +} + //手套的环的特征：在扫描线的Peak点的前后一定范围内，存在前向角从正到负的变化范围超过150度（门限值） void wd_getLineGloveArcs( std::vector& lineData, diff --git a/sourceCode/SX_lapWeldDetection.cpp b/sourceCode/SX_lapWeldDetection.cpp new file mode 100644 index 0000000..03f0a63 --- /dev/null +++ b/sourceCode/SX_lapWeldDetection.cpp @@ -0,0 +1,216 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "SX_lapWeldDetection_Export.h" +#include +#include + +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SSG_planeCalibPara sx_getBaseCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//相机姿态调平，并去除地面 +void sx_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +//提取搭接焊缝 +void sx_getLapWeldPostion( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + SSG_treeGrowParam growParam, + SSX_lapWeldParam lapWeldParam, + std::vector>& objOps, + int* errCode) +{ + *errCode = 0; + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + + int linePtNum = (int)scanLines[0].size(); + bool isGridData = true; + //垂直arc特征提取 + std::vector> jumpFeatures_v; + for (int line = 0; line < lineNum; line++) + { + if (line == 400) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &scanLines[line][0], + dataSize, + line, + cornerPara, //scale通常取bagH的1/4 + a_line_features); + jumpFeatures_v.push_back(a_line_features); + } + + if (false == isGridData)//数据不是网格格式 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + //生成水平扫描 + std::vector> hLines; + hLines.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用） + hLines[j][line] = scanLines[line][j]; + hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //水平arc特征提取 + std::vector> jumpFeatures_h; + int lineNum_h = (int)hLines.size(); + for (int line = 0; line < lineNum_h; line++) + { + std::vector& lineData = hLines[line]; + + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &hLines[line][0], + dataSize, + line, + cornerPara, //scale通常取bagH的1/4 + a_line_features); + + //进行配对匹配 + jumpFeatures_h.push_back(a_line_features); + } + + //特征生长 + //垂直方向特征生长（激光线方向） + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } + //水平方向特征生长（扫描运动方向） + std::vector h_trees; + for (int line = 0; line < lineNum_h; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + //tree信息 + std::vector allTreesInfo; //不包含边界 + SSG_treeInfo a_nullTree; + memset(&a_nullTree, 0, sizeof(SSG_treeInfo)); + allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引 + //标记，根据起点的生长树进行标注 + int hvTreeIdx = 1; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + + //记录Tree的信息 + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 1; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_vTree->treeType; + a_treeInfo.sLineIdx = a_vTree->sLineIdx; + a_treeInfo.eLineIdx = a_vTree->eLineIdx; + a_treeInfo.roi = a_vTree->roi; + allTreesInfo.push_back(a_treeInfo); + //在原始点云上标记，同时有Mask上标记 + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; + } + } + } + hvTreeIdx++; + } + int hTreeStart = hvTreeIdx; + ////标注:水平特征 + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + //记录Tree的信息 + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 0; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_hTree->treeType; + a_treeInfo.sLineIdx = a_hTree->sLineIdx; + a_treeInfo.eLineIdx = a_hTree->eLineIdx; + a_treeInfo.roi.left = a_hTree->roi.top; //水平扫描xy是交换的 + a_treeInfo.roi.right = a_hTree->roi.bottom; + a_treeInfo.roi.top = a_hTree->roi.left; + a_treeInfo.roi.bottom = a_hTree->roi.right; + allTreesInfo.push_back(a_treeInfo); + //在原始点云上标记，同时有Mask上标记 + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; + } + } + } + hvTreeIdx++; + } + int hvTreeSize = hvTreeIdx; + + //输出结果 +} \ No newline at end of file diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h new file mode 100644 index 0000000..7c60495 --- /dev/null +++ b/sourceCode/SX_lapWeldDetection_Export.h @@ -0,0 +1,40 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +typedef struct +{ + double lapHeight;//搭接厚度 + double weldMinLen; //最小焊缝长度，用于过滤可能的虚假焊缝 + int weldRefPoints; //输出的直线焊缝的参考点，默认是2个（起点和终点） +}SSX_lapWeldParam; + +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SG_APISHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//相机姿态调平，并去除地面 +SG_APISHARED_EXPORT void sx_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//提取搭接焊缝 +SG_APISHARED_EXPORT void sx_getLapWeldPostion( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + SSG_treeGrowParam growParam, + SSX_lapWeldParam lapWeldParam, + std::vector>& objOps, + int* errCode); + From 94e8a1efe8f65a195ed7aeae75dbab06de45d7cd Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 11 Sep 2025 19:37:22 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E6=90=AD=E6=8E=A5=E7=84=8A=E7=BC=9D?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E7=AE=97=E6=B3=95=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../lapWeldDetection_test.cpp | 115 ++++++--- sourceCode/SG_baseAlgo_Export.h | 2 +- sourceCode/SG_baseFunc.cpp | 10 +- sourceCode/SG_lineFeature.cpp | 14 +- sourceCode/SX_lapWeldDetection.cpp | 244 ++++++++++++++---- sourceCode/SX_lapWeldDetection_Export.h | 8 + 6 files changed, 299 insertions(+), 94 deletions(-) diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp index 266ef49..7ba8ba9 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.cpp +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -149,7 +149,8 @@ void _outputScanDataFile_vector_h(char* fileName, std::vector>& scanLines, - std::vector< std::vector> weldOPs) + std::vector< std::vector> weldOPs, + bool outDebugInfo) { int lineNum = (int)scanLines.size(); std::ofstream sw(fileName); @@ -191,18 +192,38 @@ void _outputRGBDScanLapWeld_RGBD( for (int i = 0; i < linePtNum; i++) { SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->nPointIdx > 0) + int kkk = 1; int featureType_v = pt3D->nPointIdx & 0xffff; int featureType_h = featureType_v >> 4; featureType_v &= 0xff; - if (LINE_FEATURE_PEAK_TOP == featureType_v) + if (true == outDebugInfo) { - rgb = { 255, 97, 0 }; - size = 5; - } - else if (LINE_FEATURE_PEAK_TOP == featureType_h) - { - rgb = { 97, 255, 0 }; - size = 5; + if (LINE_FEATURE_L_JUMP_H2L == featureType_v) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else if (LINE_FEATURE_L_JUMP_L2H == featureType_v) + { + rgb = objColor[7]; + size = 5; + } + else if (LINE_FEATURE_L_JUMP_H2L == featureType_h) + { + rgb = objColor[6]; + size = 5; + } + else if (LINE_FEATURE_L_JUMP_L2H == featureType_h) + { + rgb = { 97, 255, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } } else { @@ -225,11 +246,10 @@ void _outputRGBDScanLapWeld_RGBD( linePtNum += (int)weldOPs[i].size(); sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; - rgb = { 0, 0, 255 }; + rgb = { 255, 0, 0 }; size = 25; for (int i = 0; i < weldNum; i++) { - rgb = objColor[i % 8]; for (int j = 0; j < (int)weldOPs[i].size(); j++) { float x = (float)weldOPs[i][j].x; @@ -241,13 +261,37 @@ void _outputRGBDScanLapWeld_RGBD( } } //鍔犱竴涓偣锛岀敤浜庤烦杩囨樉绀哄伐鍏穊ug - rgb = objColor[0]; float x = (float)weldOPs[0][0].x; float y = (float)weldOPs[0][0].y; float z = (float)weldOPs[0][0].z; sw << "{" << x << "," << y << "," << z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + double exLen = 5.0; + rgb = objColor[0]; + size = 3; + for (int i = 0; i < weldNum; i++) + { + SVzNL3DPoint pt0 = weldOPs[i][0]; + SVzNL3DPoint pt1 = weldOPs[i].back(); + double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2)); + double k = exLen / len; + sw << "Poly_" << i << "_2" << std::endl; + double x = -k * (pt1.x - pt0.x) + pt0.x; + double y = -k * (pt1.y - pt0.y) + pt0.y; + double z = -k * (pt1.z - pt0.z) + pt0.z; + sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + x = -k * (pt0.x - pt1.x) + pt1.x; + y = -k * (pt0.y - pt1.y) + pt1.y; + z = -k * (pt0.z - pt1.z) + pt1.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + } } sw.close(); } @@ -655,7 +699,7 @@ void _convertToGridData(std::vector< std::vector>& scanLines, #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_COMPUTE_GLOVE_POSITION 1 +#define TEST_COMPUTE_POSITION 1 #define TEST_GROUP 1 int main() { @@ -665,7 +709,7 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {0,6} + {1,6} }; #if CONVERT_TO_GRID @@ -720,7 +764,7 @@ int main() int kkk = 1; //琛屽鐞 //璋冨钩锛屽幓闄ゅ湴闈 - sx_lineDataR(scanData[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + sx_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight); } // char calibFile[250]; @@ -734,7 +778,7 @@ int main() } #endif -#if TEST_COMPUTE_GLOVE_POSITION +#if TEST_COMPUTE_POSITION for (int grp = 0; grp <= 0; grp++) { SSG_planeCalibPara poseCalibPara; @@ -771,7 +815,7 @@ int main() int kkk = 1; //琛屽鐞 //璋冨钩锛屽幓闄ゅ湴闈 - sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);// calibPara.planeHeight); + sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); } #if 0 char _out_file[256]; @@ -779,24 +823,26 @@ int main() int headNullLines = 0; _outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines); #endif - SSG_treeGrowParam growParam; - growParam.maxLineSkipNum = 5; - growParam.yDeviation_max = 1.0; - growParam.maxSkipDistance = 5.0; - growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //琚嬪瓙楂樺害1/2 - growParam.minLTypeTreeLen = 30.0; //mm - growParam.minVTypeTreeLen = 30.0; //mm - SSG_cornerParam cornerParam; - cornerParam.cornerTh =30; //45搴﹁ - cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; - cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; - cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; - cornerParam.jumpCornerTh_1 = 10; - cornerParam.jumpCornerTh_2 = 30; + SSX_lapWeldParam lapWeldParam; - lapWeldParam.lapHeight = 2.0; - lapWeldParam.weldMinLen = 2.0; + lapWeldParam.lapHeight = 1.5; + lapWeldParam.weldMinLen = 80.0; lapWeldParam.weldRefPoints = 2; + lapWeldParam.scanMode = keSX_ScanMode_V; + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 25; //45搴﹁ + cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 6;// algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = lapWeldParam.lapHeight; // algoParam.bagParam.bagH / 4; + cornerParam.jumpCornerTh_1 = 10; + cornerParam.jumpCornerTh_2 = 25; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 10; + growParam.yDeviation_max = 3.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 1.0;// algoParam.bagParam.bagH / 2; //琚嬪瓙楂樺害1/2 + growParam.minLTypeTreeLen = lapWeldParam.weldMinLen; //mm + growParam.minVTypeTreeLen = lapWeldParam.weldMinLen; //mm int errCode = 0; std::vector> weldOps; sx_getLapWeldPostion( @@ -804,6 +850,7 @@ int main() cornerParam, growParam, lapWeldParam, + poseCalibPara, weldOps, &errCode); @@ -811,7 +858,7 @@ int main() printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); //杈撳嚭娴嬭瘯缁撴灉 sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps); + _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps, false); } } #endif diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index d04c772..faf5070 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -74,7 +74,7 @@ SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod( int dataSize, int lineIdx, const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 - std::vector< SSG_basicFeature1D> jumpFeatures); + std::vector< SSG_basicFeature1D>& jumpFeatures); SG_APISHARED_EXPORT void wd_getLineGloveArcs( std::vector& lineData, diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index f251c97..9a7aa81 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -1507,6 +1507,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( //数据进行转换 SVzNLRangeD calibZRange = { 0, -1 }; topZRange = { 0, -1 }; + double sumMeanZ = 0; + int sumSize = 0; for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) { //z @@ -1531,6 +1533,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( { calibZRange.min = a_calibPt.z; calibZRange.max = a_calibPt.z; + sumMeanZ += a_calibPt.z; + sumSize++; } else { @@ -1538,9 +1542,13 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( calibZRange.min = a_calibPt.z; if (calibZRange.max < a_calibPt.z) calibZRange.max = a_calibPt.z; + sumMeanZ += a_calibPt.z; + sumSize++; } } - planePara.planeHeight = calibZRange.min; + if (sumSize > 0) + sumMeanZ = sumMeanZ / (double)sumSize; + planePara.planeHeight = sumMeanZ; // calibZRange.min; return planePara; } diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index f39f7ac..ba3fde9 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -1455,7 +1455,7 @@ void sg_getLineJumpFeature_cornerMethod( int dataSize, int lineIdx, const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 - std::vector< SSG_basicFeature1D> jumpFeatures) + std::vector< SSG_basicFeature1D>& jumpFeatures) { //去除零点 std::vector< SVzNL3DPosition> vldPts; @@ -1775,13 +1775,14 @@ void sg_getLineJumpFeature_cornerMethod( int pntIdx_2 = cornerFeatures[i+1].pntIdx; //计算距离和跳变高度 double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); - double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; - if ((dist < cornerPara.minEndingGap) && (height < -cornerPara.minEndingGap_z)) + double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z); + if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z)) { SSG_basicFeature1D a_feature; a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; a_feature.jumpPos = vldPts[pntIdx_1].pt3D; - a_feature.jumpPos2D = { lineIdx, pntIdx_1 }; + a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_1].nPointIdx }; + a_feature.featureValue = cornerFeatures[i].backward_z; jumpFeatures.push_back(a_feature); } } @@ -1794,13 +1795,14 @@ void sg_getLineJumpFeature_cornerMethod( int pntIdx_2 = cornerFeatures[i + 1].pntIdx; //计算距离和跳变高度 double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); - double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; + double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z); if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z)) { SSG_basicFeature1D a_feature; a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; a_feature.jumpPos = vldPts[pntIdx_2].pt3D; - a_feature.jumpPos2D = { lineIdx, pntIdx_2 }; + a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_2].nPointIdx }; + a_feature.featureValue = cornerFeatures[i + 1].forward_z; jumpFeatures.push_back(a_feature); } } diff --git a/sourceCode/SX_lapWeldDetection.cpp b/sourceCode/SX_lapWeldDetection.cpp index 03f0a63..f9f33c6 100644 --- a/sourceCode/SX_lapWeldDetection.cpp +++ b/sourceCode/SX_lapWeldDetection.cpp @@ -23,12 +23,78 @@ void sx_lineDataR( lineDataRT_vector(a_line, camPoseR, groundH); } +SVzNL2DPoint getFootPoint(double x0, double y0, double k, double b) +{ + double A = k; + double B = -1; + double C = b; + SVzNL2DPoint foot; + foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B); + foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B); + return foot; +} + +double getWinMeanZ(std::vector& points, int startIdx, int win) +{ + int size = (int)points.size(); + double sumZ = 0; + double sumSize = 0; + for (int i = 0; i < win; i++) + { + int idx = i + startIdx; + if (idx < size) + { + if (points[idx].z > 1e-4) + { + sumZ += points[idx].z; + sumSize++; + } + } + } + if (sumSize > 0) + sumZ = sumZ / (double)sumSize; + return sumZ; +} +//从焊缝的扫描点获取焊缝的起点和终点信息 +void getWeldPoint(std::vector& a_weld_contour, int midPtNum, std::vector& a_weld) +{ + if (a_weld_contour.size() == 0) + return; + + //拟合直线 + double k, b; + lineFitting(a_weld_contour, &k, &b); + //寻找起点和终点 + SVzNL3DPoint startPt = a_weld_contour[0]; + SVzNL3DPoint endPt = a_weld_contour.back(); + SVzNL2DPoint foot_s = getFootPoint(startPt.x, startPt.y, k, b); + SVzNL2DPoint foot_e = getFootPoint(endPt.x, endPt.y, k, b); + double meanZ_s = getWinMeanZ(a_weld_contour, 0, 5); + double meanZ_e = getWinMeanZ(a_weld_contour, (int)a_weld_contour.size()-1-5, 5); + + SVzNL3DPoint pt_0 = { foot_s.x, foot_s.y, meanZ_s }; + SVzNL3DPoint pt_1 = { foot_e.x, foot_e.y, meanZ_e }; + a_weld.push_back(pt_0); + double ratio = 1.0 / ((double)midPtNum + 1.0); + + for (int i = 1; i <= midPtNum; i++) + { + SVzNL3DPoint a_pt; + a_pt.x = (double)i * ratio * (pt_1.x - pt_0.x) + pt_0.x; + a_pt.y = (double)i * ratio * (pt_1.y - pt_0.y) + pt_0.y; + a_pt.z = (double)i * ratio * (pt_1.z - pt_0.z) + pt_0.z; + a_weld.push_back(a_pt); + } + a_weld.push_back(pt_1); +} + //提取搭接焊缝 void sx_getLapWeldPostion( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, SSG_treeGrowParam growParam, SSX_lapWeldParam lapWeldParam, + SSG_planeCalibPara groundCalibPara, std::vector>& objOps, int* errCode) { @@ -42,26 +108,39 @@ void sx_getLapWeldPostion( int linePtNum = (int)scanLines[0].size(); bool isGridData = true; - //垂直arc特征提取 + //垂直跳变特征提取 std::vector> jumpFeatures_v; - for (int line = 0; line < lineNum; line++) + if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - if (line == 400) - int kkk = 1; + for (int line = 0; line < lineNum; line++) + { + if (line == 400) + int kkk = 1; - std::vector& lineData = scanLines[line]; - if (linePtNum != (int)lineData.size()) - isGridData = false; + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; - std::vector a_line_features; - int dataSize = (int)lineData.size(); - sg_getLineJumpFeature_cornerMethod( - &scanLines[line][0], - dataSize, - line, - cornerPara, //scale通常取bagH的1/4 - a_line_features); - jumpFeatures_v.push_back(a_line_features); + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &scanLines[line][0], + dataSize, + line, + cornerPara, //scale通常取bagH的1/4 + a_line_features); + //滤除地面 + std::vector vld_features; + for (int i = 0, i_max = a_line_features.size(); i < i_max; i++) + { + //将与地面形成的跳变去除 + if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25)) + { + vld_features.push_back(a_line_features[i]); + } + } + jumpFeatures_v.push_back(vld_features); + } } if (false == isGridData)//数据不是网格格式 @@ -88,55 +167,77 @@ void sx_getLapWeldPostion( //水平arc特征提取 std::vector> jumpFeatures_h; int lineNum_h = (int)hLines.size(); - for (int line = 0; line < lineNum_h; line++) + if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - std::vector& lineData = hLines[line]; + for (int line = 0; line < lineNum_h; line++) + { + std::vector& lineData = hLines[line]; - std::vector a_line_features; - int dataSize = (int)lineData.size(); - sg_getLineJumpFeature_cornerMethod( - &hLines[line][0], - dataSize, - line, - cornerPara, //scale通常取bagH的1/4 - a_line_features); + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &hLines[line][0], + dataSize, + line, + cornerPara, //scale通常取bagH的1/4 + a_line_features); - //进行配对匹配 - jumpFeatures_h.push_back(a_line_features); + //滤除地面 + std::vector vld_features; + for (int i = 0, i_max = a_line_features.size(); i < i_max; i++) + { + //将与地面形成的跳变去除 + if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25)) + { + vld_features.push_back(a_line_features[i]); + } + } + jumpFeatures_h.push_back(vld_features); + } } //特征生长 //垂直方向特征生长（激光线方向） std::vector v_trees; - for (int line = 0; line < lineNum; line++) + if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - bool isLastLine = false; - if (line == lineNum - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_v[line]; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - v_trees, - growParam); + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v[line]; + if (a_lineJumpFeature.size() > 0) + int kkk = 1; + if (line == 400) + int kkk = 1; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } } //水平方向特征生长（扫描运动方向） std::vector h_trees; - for (int line = 0; line < lineNum_h; line++) + if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - if (line == 650) - int kkk = 1; - bool isLastLine = false; - if (line == lineNum_h - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_h[line]; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - h_trees, - growParam); + for (int line = 0; line < lineNum_h; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } } //tree信息 @@ -159,6 +260,8 @@ void sx_getLapWeldPostion( a_treeInfo.eLineIdx = a_vTree->eLineIdx; a_treeInfo.roi = a_vTree->roi; allTreesInfo.push_back(a_treeInfo); + + std::vector a_weld_contour; //在原始点云上标记，同时有Mask上标记 for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) { @@ -172,8 +275,13 @@ void sx_getLapWeldPostion( scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; } + a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D); } } + std::vector a_weld; + getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld); + if (a_weld.size() > 0) + objOps.push_back(a_weld); hvTreeIdx++; } int hTreeStart = hvTreeIdx; @@ -193,6 +301,8 @@ void sx_getLapWeldPostion( a_treeInfo.roi.top = a_hTree->roi.left; a_treeInfo.roi.bottom = a_hTree->roi.right; allTreesInfo.push_back(a_treeInfo); + + std::vector a_weld_contour; //在原始点云上标记，同时有Mask上标记 for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) { @@ -206,11 +316,41 @@ void sx_getLapWeldPostion( scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; } + a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D); } } + std::vector a_weld; + getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld); + if (a_weld.size() > 0) + objOps.push_back(a_weld); + hvTreeIdx++; } int hvTreeSize = hvTreeIdx; + //将数据重新投射回原来的坐标系，以保持手眼标定结果正确 + for (int i = 0; i < lineNum; i++) + sx_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1); + //将检测结果重新投射回原来的坐标系 + for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++) + { + std::vector& a_weld = objOps[i]; + for (int j = 0, j_max = a_weld.size(); j < j_max; j++) + { + double x = a_weld[j].x * groundCalibPara.invRMatrix[0] + + a_weld[j].y * groundCalibPara.invRMatrix[1] + + a_weld[j].z * groundCalibPara.invRMatrix[2]; + double y = a_weld[j].x * groundCalibPara.invRMatrix[3] + + a_weld[j].y * groundCalibPara.invRMatrix[4] + + a_weld[j].z * groundCalibPara.invRMatrix[5]; + double z = a_weld[j].x * groundCalibPara.invRMatrix[6] + + a_weld[j].y * groundCalibPara.invRMatrix[7] + + a_weld[j].z * groundCalibPara.invRMatrix[8]; + a_weld[j].x = x; + a_weld[j].y = y; + a_weld[j].z = z; + } + } + //输出结果 } \ No newline at end of file diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h index 7c60495..d6570e6 100644 --- a/sourceCode/SX_lapWeldDetection_Export.h +++ b/sourceCode/SX_lapWeldDetection_Export.h @@ -10,11 +10,18 @@ #include #include +typedef enum +{ + keSX_ScanMode_V = 0, //激光线垂直焊缝扫描 + keSX_ScanMode_H, //激光线平行焊缝扫描 + keSX_ScanMode_Both, //既有垂直扫描，也有水平扫描 +} ESX_WeldScanMode; typedef struct { double lapHeight;//搭接厚度 double weldMinLen; //最小焊缝长度，用于过滤可能的虚假焊缝 int weldRefPoints; //输出的直线焊缝的参考点，默认是2个（起点和终点） + ESX_WeldScanMode scanMode; }SSX_lapWeldParam; //计算一个平面调平参数。 @@ -35,6 +42,7 @@ SG_APISHARED_EXPORT void sx_getLapWeldPostion( const SSG_cornerParam cornerPara, SSG_treeGrowParam growParam, SSX_lapWeldParam lapWeldParam, + SSG_planeCalibPara groundCalibPara, std::vector>& objOps, int* errCode);