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

1363 lines
43 KiB
C++
Raw Normal View History

source code
2025-06-08 10:46:41 +08:00
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#include <opencv2/opencv.hpp>
//<2F><>searchWin<69><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>(z<><7A>С)
void _findPeak(SVzNL3DLaserLine* scanLines, int lineNum, SVzNL2DPoint LTpos, SSG_localPkParam searchWin, SSG_2DValueI* pkPos)
{
int line_end = LTpos.x + searchWin.seachW_lines;
if (line_end >= lineNum)
line_end = lineNum;
int ptNum = scanLines->nPositionCnt;
int pt_end = LTpos.y + searchWin.searchW_pts;
if (pt_end >= ptNum)
pt_end = ptNum;
SSG_2DValueI peak = { -1, -1, 0, 0};
double minZ = -1;
for (int i = LTpos.x; i < line_end; i++)
{
for (int j = LTpos.y; j < pt_end; j++)
{
SVzNL3DPosition* a_pt = &scanLines[i].p3DPosition[j];
if (a_pt->pt3D.z < 1e-4)
continue;
if (minZ < 0)
{
peak = { i, j , 0, a_pt->pt3D.z };
minZ = a_pt->pt3D.z;
}
else
{
if (minZ > a_pt->pt3D.z)
{
peak = { i, j, 0, a_pt->pt3D.z };
minZ = a_pt->pt3D.z;
}
}
}
}
*pkPos = peak;
return;
}
///<2F><><EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD>ߵ㣨z<E3A3A8><7A>С<EFBFBD>㣩<EFBFBD><E3A3A9>
///<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD><DAB3>ȵ<EFBFBD>һ<EFBFBD>롣<EFBFBD>Ծֲ<D4BE><D6B2><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD>б<EFBFBD><D0B1>ǣ<EFBFBD><C7A3><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD>¼<EFBFBD><C2BC>
void sg_getLocalPeaks(SVzNL3DLaserLine* scanLines, int lineNum, std::vector<SSG_2DValueI>& peaks, SSG_localPkParam searchWin)
{
int ptNum = scanLines->nPositionCnt;
cv::Mat mask = cv::Mat::zeros(ptNum, lineNum, CV_32SC1);
int winNum_cols = lineNum / (searchWin.seachW_lines/2);
if ((lineNum % (searchWin.seachW_lines/2)) > 0)
winNum_cols = winNum_cols + 1;
int winNum_rows = ptNum / (searchWin.searchW_pts/2);
if ((ptNum % (searchWin.searchW_pts/2)) > 0)
winNum_rows = winNum_rows + 1;
for (int i = 0; i < winNum_rows; i++)
{
for (int j = 0; j < winNum_cols; j++)
{
SVzNL2DPoint LTpos = {i* searchWin.seachW_lines / 2, j* searchWin.searchW_pts / 2 };
SSG_2DValueI pkPos = { -1, -1, 0, 0 };
_findPeak(scanLines, lineNum, LTpos, searchWin, &pkPos);
if ((pkPos.x >= 0) && (pkPos.y >= 0))
{
//<2F>߽紦<DFBD>ļ<EFBFBD>ֵ<EFBFBD><D6B5>ȥ<EFBFBD><C8A5>
if ((pkPos.x != LTpos.x) && (pkPos.x != (LTpos.x + searchWin.seachW_lines - 1)) &&
(pkPos.y != LTpos.y) && (pkPos.y != (LTpos.y + searchWin.searchW_pts - 1)))
{
int maskValue = mask.at<int>(pkPos.y, pkPos.x);
if (maskValue == 0)
{
peaks.push_back(pkPos);
mask.at<int>(pkPos.y, pkPos.x) = 1;
}
}
}
}
}
return;
}
void _polarScan(cv::Mat& edgeMask, SVzNL2DPoint scanSeed, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& contours, int* maxEdgeId)
{
int scanTime = (int)(360.0 / polarScanParam.angleStep);
int edgeId = 0;
for (int i = 0; i < scanTime; i++)
{
double angle = i * polarScanParam.angleStep;
double sinTheta = sin(angle * PI / 180);
double cosTheta = cos(angle * PI / 180);
double r = 0;
while (1)
{
r += polarScanParam.radiusStep;
int x = (int)(scanSeed.x + r * cosTheta);
int y = (int)(scanSeed.y - r * sinTheta);
if ((x >= edgeMask.cols) || (x < 0) ||
(y >= edgeMask.rows) || (y < 0))
{
//SSG_2DValueI edge_pt = { -1,-1, -1, -1 };
//contours.push_back(edge_pt);
break;
}
else
{
int maskValue = edgeMask.at<int>(y, x);
if (maskValue > 0)
{
edgeId = edgeId < maskValue ? maskValue : edgeId;
SSG_2DValueI edge_pt = { x, y, maskValue, r };
contours.push_back(edge_pt);
break;
}
}
}
}
*maxEdgeId = edgeId;
return;
}
void _getRgnVldContour(std::vector< SSG_2DValueI>& contourPts, std::vector< SSG_2DValueI>& vldContour, int maxEdgeId, const int noiseRunLenTh)
{
std::vector<int> idIndexing(maxEdgeId + 1);
//<2F>γ̷<CEB3><CCB7><EFBFBD>
std::vector< SSG_RUN> contourRuns;
SSG_RUN a_run = { -1,-1,-1};
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_2DValueI* a_pt = &contourPts[i];
if (a_pt->x < 0)
continue;
if (a_run.start < 0)
{
a_run.start = i;
a_run.len = 1;
a_run.value = a_pt->value;
}
else
{
if (a_run.value == a_pt->value)
a_run.len++;
else
{
contourRuns.push_back(a_run);
//<2F>µ<EFBFBD><C2B5>γ<EFBFBD>
a_run.start = i;
a_run.len = 1;
a_run.value = a_pt->value;
}
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
if (contourRuns.size() > 0)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>͵<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ϲ<EFBFBD>
SSG_RUN* first_run = &contourRuns[0];
if (first_run->value == a_run.value)
{
first_run->start = a_run.start;
first_run->len += a_run.len;
}
else
contourRuns.push_back(a_run);
}
else
contourRuns.push_back(a_run);
if (contourRuns.size() > 0)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı߽磬С<E7A3AC>Ķ̵ı߽<C4B1><DFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)contourRuns.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_RUN* curr_run = &contourRuns[i];
if(curr_run->len > noiseRunLenTh)
{
idIndexing[curr_run->value] = 1;
}
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_2DValueI a_pt = contourPts[i];
if (a_pt.x < 0)
continue;
if (idIndexing[a_pt.value] > 0)
vldContour.push_back(a_pt);
}
}
return;
}
/// <summary>
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ծֲ<D4BE><D6B2><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>ΪԲ<CEAA><D4B2><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2>ɨ<EFBFBD>裬<EFBFBD><E8A3AC>¼ɨ<C2BC>赽<EFBFBD>ı߽<C4B1>
/// </summary>
/// <param name="scanLines"></param>
/// <param name="lineNum"></param>
/// <param name="edgeMask"></param>
/// <param name="peaks"></param>
void sg_peakPolarScan(cv::Mat& edgeMask, SVzNL2DPoint a_peak, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& rgnContour)
{
std::vector< SSG_2DValueI> contours;
int maxEdgeId = 0;
_polarScan(edgeMask, a_peak, polarScanParam, contours, &maxEdgeId);
int noiseRunLenTh = (int)(11.25 / polarScanParam.angleStep); //С<><D0A1>22.5<EFBFBD>ȵı߽<EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
_getRgnVldContour(contours, rgnContour, maxEdgeId, noiseRunLenTh);
return;
}
//ȡ<><C8A1>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>contour<75><72>
void sg_getContourPts(
std::vector< SSG_lineConotours>& contour_all,
int vldEdgeId,
std::vector< SSG_2DValueI>& contourFilter,
int* lowLevelFlag)
{
*lowLevelFlag = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чcontour
//ͳ<><CDB3>block(<28>ڵ<EFBFBD>)<29>ı<EFBFBD><C4B1><EFBFBD>
int blockPtNum = 0;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)contour_all.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
std::vector<SSG_contourPtInfo>& a_line_contourPts = contour_all[m].contourPts;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)a_line_contourPts.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_contourPtInfo* a_contourPt = &a_line_contourPts[i];
int an_edgeIdx = a_contourPt->edgeId;
if (an_edgeIdx == vldEdgeId)
{
if (a_contourPt->blockFlag > 0)
blockPtNum++;
SSG_2DValueI a_filterPt = { a_contourPt->x, a_contourPt->y, a_contourPt->type, a_contourPt->edgePt.z };
contourFilter.push_back(a_filterPt);
}
}
}
blockPtNum = blockPtNum * 2;
if (blockPtNum > contourFilter.size())
*lowLevelFlag = 1;
return;
}
//ȡ<><C8A1>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>contour<75><72>
void sg_getPairingContourPts(
std::vector<SSG_conotourPair>& contourPairs,
std::vector<SSG_intPair>& idPairs,
std::vector< SSG_conotourPair>& contourFilter,
SVzNLRangeD range,
bool isTBDir,
int* lowLevelFlag_0,
int* lowLevelFlag_1)
{
*lowLevelFlag_0 = 0;
*lowLevelFlag_1 = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чcontour
//ͳ<><CDB3>block(<28>ڵ<EFBFBD>)<29>ı<EFBFBD><C4B1><EFBFBD>
int blockPtNum_0 = 0;
int blockPtNum_1 = 0;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)contourPairs.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_conotourPair& a_ptPair = contourPairs[m];
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)idPairs.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_intPair& a_idPair = idPairs[i];
if ( (a_ptPair.edgeId_0 == a_idPair.data_0) &&
(a_ptPair.edgeId_1 == a_idPair.data_1))
{
bool isValid = false;
if (true == isTBDir)
{
if ((a_ptPair.contourPt_0.edgePt.x > range.min) && (a_ptPair.contourPt_0.edgePt.x < range.max) &&
(a_ptPair.contourPt_1.edgePt.x > range.min) && (a_ptPair.contourPt_1.edgePt.x < range.max))
isValid = true;
}
else
{
if ((a_ptPair.contourPt_0.edgePt.y > range.min) && (a_ptPair.contourPt_0.edgePt.y < range.max) &&
(a_ptPair.contourPt_1.edgePt.y > range.min) && (a_ptPair.contourPt_1.edgePt.y < range.max))
isValid = true;
}
if (true == isValid)
{
if (a_ptPair.contourPt_0.blockFlag > 0)
blockPtNum_0++;
if (a_ptPair.contourPt_1.blockFlag > 0)
blockPtNum_1++;
contourFilter.push_back(contourPairs[m]);
break;
}
}
}
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
int totalNum = (int)contourFilter.size();
source code
2025-06-08 10:46:41 +08:00
if(blockPtNum_0 > totalNum/2)
*lowLevelFlag_0 = 1;
if (blockPtNum_1 > totalNum / 2)
*lowLevelFlag_1 = 1;
return;
}
//ȡ<><C8A1>contour<75><72>
void sg_contourPostProc(std::vector< SSG_contourPtInfo>& contour, int maxEdgeIdx, double sameConturDistTh,
std::vector< SSG_2DValueI>& contourFilter, int sideID, int* blockFlag)
{
*blockFlag = 0;
if (contour.size() == 0)
return;
std::vector< SSG_contourEdgeInfo> edgeInfo;
edgeInfo.resize(maxEdgeIdx + 1);
std::vector<int> egdeIndexing;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)contour.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_contourPtInfo* a_contourPt = &contour[i];
int edgeIdx = a_contourPt->edgeId;
if (edgeInfo[edgeIdx].ptNum == 0) //<2F>µ<EFBFBD>Edge
{
edgeInfo[edgeIdx].sPt = a_contourPt->edgePt;
egdeIndexing.push_back(edgeIdx);
}
edgeInfo[edgeIdx].ePt = a_contourPt->edgePt;
edgeInfo[edgeIdx].scanDist += a_contourPt->scanDist;
edgeInfo[edgeIdx].ptNum += 1;
}
//<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>scanDist
//<2F><><EFBFBD><EFBFBD>scanDist<73><74>С<EFBFBD>ģ<EFBFBD>ֻ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD><EFBFBD><EFBFBD>10%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чedge
int lenTh = (int)(contour.size() * 3) / 10;
int nearestIdx = -1;
double minScanDist = -1;
for (int i = 0; i < egdeIndexing.size(); i++)
{
int edgeId = egdeIndexing[i];
edgeInfo[edgeId].scanDist = edgeInfo[edgeId].scanDist / edgeInfo[edgeId].ptNum;
if (edgeInfo[edgeId].ptNum > lenTh)
{
if (minScanDist < 0)
{
nearestIdx = i;
minScanDist = edgeInfo[edgeId].scanDist;
}
else
{
if (minScanDist > edgeInfo[edgeId].scanDist)
{
nearestIdx = i;
minScanDist = edgeInfo[edgeId].scanDist;
}
}
}
else
edgeInfo[edgeId].ptNum = 0; //invalid
}
if (nearestIdx < 0)
return;
//<2F><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶εľ<CEB5><C4BE>롣С<EBA1A3><D0A1>sameConturDistThΪͬһ<CDAC>߶<EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чcontour
//ͳ<><CDB3>block(<28>ڵ<EFBFBD>)<29>ı<EFBFBD><C4B1><EFBFBD>
int blockPtNum = 0;
for (int i = 0; i < egdeIndexing.size(); i++)
{
int edgeId = egdeIndexing[i];
if (0 == edgeInfo[edgeId].ptNum)
continue;
double scanDist = edgeInfo[edgeId].scanDist;
double dist_diff = scanDist - minScanDist;
if (dist_diff < sameConturDistTh)
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)contour.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
SSG_contourPtInfo* a_contourPt = &contour[m];
int an_edgeIdx = a_contourPt->edgeId;
if (an_edgeIdx == edgeId)
{
if (a_contourPt->blockFlag > 0)
blockPtNum++;
SSG_2DValueI a_filterPt = { a_contourPt->x, a_contourPt->y, a_contourPt->type, a_contourPt->edgePt.z, sideID };
contourFilter.push_back(a_filterPt);
}
}
}
}
blockPtNum = blockPtNum * 2;
if (blockPtNum > contourFilter.size())
*blockFlag = 1;
return;
}
void _checkVEdgePt(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
int px,
int py,
int linePtNum,
SSG_2DValueI a_peak,
SVzNL3DPosition* a_pt,
int* a_pt_edgeId,
double pk_y,
bool rgnPtAsEdge,
double scanDistTh,
bool dirUp,
SSG_contourPtInfo* edgePt,
bool* stopFlag,
bool* foundHScanEdge)
{
int ptNum = laser3DPoints[0].nPositionCnt;
int objId = (a_pt->nPointIdx >> 8) & 0xff;
int vType = (a_pt->nPointIdx) & 0x00ff;//<2F><><EFBFBD><EFBFBD>rgnID
int hType = vType >> 4;
vType &= 0x0f;
*stopFlag = false;
double scanDist = abs(pk_y - a_pt->pt3D.y);
if ((vType > 0) || (py == 0) ||(py == linePtNum-1)|| ((true == rgnPtAsEdge) && (objId > 0)))
{
//<2F><>ײ<EFBFBD><D7B2><EFBFBD>⣨<EFBFBD><E2A3A8><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD>²<EFBFBD>Ŀ<EFBFBD><C4BF>)
if ((vType == 0) && ((true == rgnPtAsEdge) && (objId > 0)))
{
vType = LINE_FEATURE_RGN_EDGE;
if (true == dirUp)
*a_pt_edgeId = 5;
else
*a_pt_edgeId = 6;
}
int blockFlag = 0;
if (true == dirUp)
{
if ((LINE_FEATURE_L_JUMP_H2L == vType) || (LINE_FEATURE_L_SLOPE_H2L == vType))
blockFlag = 1;
}
else
{
if ((LINE_FEATURE_L_JUMP_L2H == vType) || (LINE_FEATURE_L_SLOPE_L2H == vType))
blockFlag = 1;
}
if (scanDist > scanDistTh)
*stopFlag = true;
if (false == *stopFlag)
{
*edgePt = { px, py, vType, *a_pt_edgeId, blockFlag, scanDist, a_pt->pt3D };
if ((LINE_FEATURE_RGN_EDGE == vType) || (LINE_FEATURE_L_JUMP_H2L == vType) ||
(LINE_FEATURE_L_JUMP_L2H == vType) ||
(LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType))// ||
//(LINE_FEATURE_CORNER_V == vType))
*stopFlag = true;
else if((*a_pt_edgeId == 5) || (*a_pt_edgeId == 6))
*stopFlag = true;
}
}
#if 1
else if ((vType == 0) && (hType > 0))
{
//
if ((LINE_FEATURE_L_JUMP_H2L == hType) || (LINE_FEATURE_L_JUMP_L2H == hType))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>5<EFBFBD><35>
bool tmp_flag = true;
for (int m = 0; m < 5; m++)
{
int chk_y;
if (true == dirUp)
{
chk_y = py - m;
if (chk_y < 0)
break;
}
else
{
chk_y = py + m;
if (chk_y >= ptNum)
break;
}
int chk_vType = (laser3DPoints[px].p3DPosition[chk_y].nPointIdx) & 0x000f;//<2F><><EFBFBD><EFBFBD>rgnID
if (chk_vType > 0)
{
tmp_flag = false;
break;
}
}
if (true == tmp_flag)
{
*foundHScanEdge = true;
*stopFlag = true;
}
}
}
#endif
return;
}
void _checkHEdgePt(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
int px,
int py,
SSG_2DValueI a_peak,
SVzNL3DPosition* a_pt,
int* a_pt_edgeId,
double pk_x,
bool rgnPtAsEdge,
double scanDistTh,
bool dirLeft,
SSG_contourPtInfo* edgePt,
bool* stopFlag,
bool* foundVScanEdge)
{
int objId = (a_pt->nPointIdx >> 8) & 0xff;
int vType = (a_pt->nPointIdx) & 0x00ff;//<2F><><EFBFBD><EFBFBD>rgnID
int hType = vType >> 4;
vType &= 0x0f;
*stopFlag = false;
double scanDist = abs(pk_x - a_pt->pt3D.x);
if ((hType > 0) || (px == 0) ||(px == lineNum-1)|| ((true == rgnPtAsEdge) && (objId > 0)))
{
//<2F><>ײ<EFBFBD><D7B2><EFBFBD>⣨<EFBFBD><E2A3A8><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD>²<EFBFBD>Ŀ<EFBFBD><C4BF>)
if ((hType == 0) && ((true == rgnPtAsEdge) && (objId > 0)))
{
hType = LINE_FEATURE_RGN_EDGE;
if (true == dirLeft)
*a_pt_edgeId = 7;
else
*a_pt_edgeId = 8;
}
int blockFlag = 0;
if (true == dirLeft)
{
if ((LINE_FEATURE_L_JUMP_H2L == hType) || (LINE_FEATURE_L_SLOPE_H2L == hType))
blockFlag = 1;
}
else
{
if ((LINE_FEATURE_L_JUMP_L2H == hType) || (LINE_FEATURE_L_SLOPE_L2H == hType))
blockFlag = 1;
}
if (scanDist > scanDistTh)
*stopFlag = true;
if (false == *stopFlag)
{
*edgePt = { px, py, hType, *a_pt_edgeId, blockFlag, scanDist, a_pt->pt3D };
if ((LINE_FEATURE_RGN_EDGE == hType) || (LINE_FEATURE_L_JUMP_H2L == hType) ||
(LINE_FEATURE_L_JUMP_L2H == hType) ||
(LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType))//||
//(LINE_FEATURE_CORNER_V == hType))
*stopFlag = true;
else if ((*a_pt_edgeId == 7) || (*a_pt_edgeId == 8))
*stopFlag = true;
}
}
#if 1
else if ((hType == 0) && ( vType > 0))
{
if ((LINE_FEATURE_L_JUMP_H2L == vType) || (LINE_FEATURE_L_JUMP_L2H == vType))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>5<EFBFBD><35>
bool tmp_flag = true;
for (int m = 0; m < 5; m++)
{
int chk_x;
if (true == dirLeft)
{
chk_x = px - m;
if (chk_x < 0)
break;
}
else
{
chk_x = px + m;
if (chk_x >= lineNum)
break;
}
int chk_hType = (laser3DPoints[chk_x].p3DPosition[py].nPointIdx) & 0x00f0;//<2F><><EFBFBD><EFBFBD>rgnID
if (chk_hType > 0)
{
tmp_flag = false;
break;
}
}
if (true == tmp_flag)
{
*foundVScanEdge = true;
*stopFlag = true;
}
}
}
#endif
return;
}
void _searchUpDownEdgePts(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
cv::Mat& featureEdgeMask,
SSG_2DValueI a_peak,
int scan_x,
bool rgnPtAsEdge,
double scanDistTh,
bool searchDir_up,
std::vector< SSG_contourPtInfo>& edgePts,
bool* foundHScanEdge
)
{
*foundHScanEdge = false;
double pk_y = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.y;
int ptNum = laser3DPoints[0].nPositionCnt;
if (true == searchDir_up)
{
SSG_contourPtInfo pre_pt;
memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
for (int y = a_peak.y; y >= 0; y--)
{
if (y == 84)
int kkk = 1;
SVzNL3DPosition* a_pt = &laser3DPoints[scan_x].p3DPosition[y];
int edgeId = featureEdgeMask.at<cv::Vec4i>(y, scan_x)[0];
SSG_contourPtInfo _top;
memset(&_top, 0, sizeof(SSG_contourPtInfo));
_top.x = -1; //<2F><>Ч
bool stopFlag = false;
_checkVEdgePt(
laser3DPoints,
lineNum,
scan_x,
y,
ptNum,
a_peak,
a_pt,
&edgeId,
pk_y,
rgnPtAsEdge,
scanDistTh,
true, //Dir_UP
&_top,
&stopFlag,
foundHScanEdge);
if ( (_top.x >= 0) && (_top.type > 0) && (edgeId > 0))
{
if ((_top.type > 0) && (_top.y >= a_peak.y - 1)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
stopFlag = true;
else
{
//<2F><><EFBFBD><EFBFBD>Edge<67><65><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD>Edge<67><65>û<EFBFBD><C3BB>ͻȻ<CDBB>仯
if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_L2H == pre_pt.type)) &&
((LINE_FEATURE_L_JUMP_H2L == _top.type) || (LINE_FEATURE_L_JUMP_L2H == _top.type) ))
//(LINE_FEATURE_LINE_ENDING_0 == _top.type) || (LINE_FEATURE_LINE_ENDING_1 == _top.type)))
{
double dist_diff = abs(pre_pt.scanDist - _top.scanDist);
if (dist_diff > 100)
{
stopFlag = true;
*foundHScanEdge = true;
}
else
edgePts.push_back(_top);
}
else
edgePts.push_back(_top);
}
}
if (true == stopFlag)
break;
pre_pt = _top;
}
}
else
{
SSG_contourPtInfo pre_pt;
memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
for (int y = a_peak.y; y < ptNum; y++)
{
if (y == 336)
int kkk = 1;
SVzNL3DPosition* a_pt = &laser3DPoints[scan_x].p3DPosition[y];
int edgeId = featureEdgeMask.at<cv::Vec4i>(y, scan_x)[0];
SSG_contourPtInfo _btm;
memset(&_btm, 0, sizeof(SSG_contourPtInfo));
_btm.x = -1; //<2F><>Ч
bool stopFlag = false;
_checkVEdgePt(
laser3DPoints,
lineNum,
scan_x,
y,
ptNum,
a_peak,
a_pt,
&edgeId,
pk_y,
rgnPtAsEdge,
scanDistTh,
false, //dir_down
&_btm,
&stopFlag,
foundHScanEdge);
if ( (_btm.x >= 0) && (_btm.type > 0) && (edgeId > 0))
{
if ((_btm.type > 0) && (_btm.y <= a_peak.y + 1))//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
stopFlag = true;
else
{
//<2F><><EFBFBD><EFBFBD>Edge<67><65><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD>Edge<67><65>û<EFBFBD><C3BB>ͻȻ<CDBB>仯
if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_L2H == pre_pt.type)) &&
((LINE_FEATURE_L_JUMP_H2L == _btm.type) || (LINE_FEATURE_L_JUMP_L2H == _btm.type)))// ||
//(LINE_FEATURE_LINE_ENDING_0 == _btm.type) || (LINE_FEATURE_LINE_ENDING_1 == _btm.type)))
{
double dist_diff = abs(pre_pt.scanDist - _btm.scanDist);
if (dist_diff > 100)
{
stopFlag = true;
*foundHScanEdge = true;
}
else
edgePts.push_back(_btm);
}
else
edgePts.push_back(_btm);
}
}
if (true == stopFlag)
break;
pre_pt = _btm;
}
}
}
void _searchLeftRightEdgePts(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
cv::Mat& featureEdgeMask,
SSG_2DValueI a_peak,
int scan_y,
bool rgnPtAsEdge,
double scanDistTh,
bool searchDir_left,
std::vector< SSG_contourPtInfo>& edgePts,
bool* foundVScanEdge)
{
*foundVScanEdge = false;
double pk_x = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.x;
if (true == searchDir_left)
{
SSG_contourPtInfo pre_pt;
memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
for (int x = a_peak.x; x >= 0; x--)
{
if (x == 4)
int kkk = 1;
SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[scan_y];
int edgeId = featureEdgeMask.at<cv::Vec4i>(scan_y, x)[0];
SSG_contourPtInfo _left;
memset(&_left, 0, sizeof(SSG_contourPtInfo));
_left.x = -1; //<2F><>Ч
bool stopFlag = false;
_checkHEdgePt(
laser3DPoints,
lineNum,
x,
scan_y,
a_peak,
a_pt,
&edgeId,
pk_x,
rgnPtAsEdge,
scanDistTh,
true, //DIR_left
&_left,
&stopFlag,
foundVScanEdge);
if ((_left.x >= 0) && (_left.type > 0) && (edgeId > 0))
{
if ((_left.type > 0) && (_left.x >= a_peak.x - 1)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
stopFlag = true;
else
{
//<2F><><EFBFBD><EFBFBD>Edge<67><65><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD>Edge<67><65>û<EFBFBD><C3BB>ͻȻ<CDBB>仯
if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_H2L == pre_pt.type)) &&
((LINE_FEATURE_L_JUMP_H2L == _left.type) || (LINE_FEATURE_L_JUMP_H2L == _left.type)))// ||
//(LINE_FEATURE_LINE_ENDING_0 == _left.type) || (LINE_FEATURE_LINE_ENDING_1 == _left.type)))
{
double dist_diff = abs(pre_pt.scanDist - _left.scanDist);
if (dist_diff > 100)
{
stopFlag = true;
*foundVScanEdge = true;
}
else
edgePts.push_back(_left);
}
else
edgePts.push_back(_left);
}
}
if (true == stopFlag)
break;
pre_pt = _left;
}
}
else
{
SSG_contourPtInfo pre_pt;
memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
for (int x = a_peak.x; x < lineNum; x++)
{
if (x == 374)
int kkk = 1;
SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[scan_y];
int edgeId = featureEdgeMask.at<cv::Vec4i>(scan_y, x)[0];
SSG_contourPtInfo _right;
memset(&_right, 0, sizeof(SSG_contourPtInfo));
_right.x = -1; //<2F><>Ч
bool stopFlag = false;
_checkHEdgePt(
laser3DPoints,
lineNum,
x,
scan_y,
a_peak,
a_pt,
&edgeId,
pk_x,
rgnPtAsEdge,
scanDistTh,
false, //DIR_right
&_right,
&stopFlag,
foundVScanEdge);
if ( (_right.x >= 0) && (_right.type > 0) && (edgeId > 0))
{
if ((_right.type > 0) && (_right.x <= a_peak.x + 1))//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
stopFlag = true;
else
{
//<2F><><EFBFBD><EFBFBD>Edge<67><65><EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD>Edge<67><65>û<EFBFBD><C3BB>ͻȻ<CDBB>仯
if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_H2L == pre_pt.type)) &&
((LINE_FEATURE_L_JUMP_H2L == _right.type) || (LINE_FEATURE_L_JUMP_H2L == _right.type))) //||
// (LINE_FEATURE_LINE_ENDING_0 == _right.type) || (LINE_FEATURE_LINE_ENDING_1 == _right.type)))
{
double dist_diff = abs(pre_pt.scanDist - _right.scanDist);
if (dist_diff > 100)
{
stopFlag = true;
*foundVScanEdge = true;
}
else
edgePts.push_back(_right);
}
else
edgePts.push_back(_right);
}
}
if (true == stopFlag)
break;
pre_pt = _right;
}
}
}
//<2F><>Peak<61><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD>ֱɨ<D6B1><C9A8><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>
void sg_peakXYScan(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
cv::Mat& featureEdgeMask,
SSG_2DValueI a_peak,
SSG_treeGrowParam growParam,
SSG_bagParam bagParam,
bool rgnPtAsEdge,
//int stopEdgeId_T, int stopEdgeId_B, int stopEdgeId_L, int stopEdgeId_R,
std::vector< SSG_lineConotours>& topContour,
std::vector< SSG_lineConotours>& bottomContour,
std::vector< SSG_lineConotours>& leftContour,
std::vector< SSG_lineConotours>& rightContour,
int* maxEdgeId_top,
int* maxEdgeId_btm,
int* maxEdgeId_left,
int* maxEdgeId_right)
{
if (featureEdgeMask.at<cv::Vec4i>(a_peak.y, a_peak.x)[3] == 0)
return;
int ptNum = laser3DPoints[0].nPositionCnt;
double pk_x = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.x;
double pk_y = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.y;
*maxEdgeId_top = 0;
*maxEdgeId_btm = 0;
double scanDistTh = bagParam.bagL -a_peak.valueD /2;
//<2F><>Peak<61><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>
for (int x = a_peak.x; x >= 0; x--)
{
if (x == 0)
int kkk = 1;
//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>bagL<67><4C><EFBFBD><EFBFBD>
double curr_x = laser3DPoints[x].p3DPosition[a_peak.y].pt3D.x;
double x_diff = abs(curr_x - pk_x);
if (x_diff > scanDistTh)
break;
bool foundHScanEdge = false;
std::vector< SSG_contourPtInfo> a_line_upEdgePts;
_searchUpDownEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
x,
rgnPtAsEdge,
scanDistTh,
true, //searchDir_up,
a_line_upEdgePts,
&foundHScanEdge);
std::vector< SSG_contourPtInfo> a_line_downEdgePts;
_searchUpDownEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
x,
rgnPtAsEdge,
scanDistTh,
false, //searchDir_down,
a_line_downEdgePts,
&foundHScanEdge);
if ( true == foundHScanEdge)
break;
else
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_upEdgePts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_top < a_line_upEdgePts[m].edgeId)
*maxEdgeId_top = a_line_upEdgePts[m].edgeId;
}
if (a_line_upEdgePts.size() > 0)
{
SSG_lineConotours _line_upContours = { x,a_line_upEdgePts };
topContour.insert(topContour.begin(), _line_upContours);
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_downEdgePts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_btm < a_line_downEdgePts[m].edgeId)
*maxEdgeId_btm = a_line_downEdgePts[m].edgeId;
}
if (a_line_downEdgePts.size() > 0)
{
SSG_lineConotours _line_downContours = { x,a_line_downEdgePts };
bottomContour.insert(bottomContour.begin(), _line_downContours);
}
}
}
//<2F><>Peak<61><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>
for (int x = a_peak.x+1; x < lineNum; x++)
{
if (x == 34)
int kkk = 1;
//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>bagL<67><4C><EFBFBD><EFBFBD>
double curr_x = laser3DPoints[x].p3DPosition[a_peak.y].pt3D.x;
double x_diff = abs(curr_x - pk_x);
if (x_diff > scanDistTh)
break;
bool foundHScanEdge = false;
std::vector< SSG_contourPtInfo> a_line_upEdgePts;
_searchUpDownEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
x,
rgnPtAsEdge,
scanDistTh,
true, //searchDir_up,
a_line_upEdgePts,
&foundHScanEdge);
std::vector< SSG_contourPtInfo> a_line_downEdgePts;
_searchUpDownEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
x,
rgnPtAsEdge,
scanDistTh,
false, //searchDir_down,
a_line_downEdgePts,
&foundHScanEdge);
if (true == foundHScanEdge)
break;
else
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_upEdgePts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_top < a_line_upEdgePts[m].edgeId)
*maxEdgeId_top = a_line_upEdgePts[m].edgeId;
}
if (a_line_upEdgePts.size() > 0)
{
SSG_lineConotours _line_upContours = { x,a_line_upEdgePts };
topContour.push_back(_line_upContours);
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_downEdgePts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_btm < a_line_downEdgePts[m].edgeId)
*maxEdgeId_btm = a_line_downEdgePts[m].edgeId;
}
if (a_line_downEdgePts.size() > 0)
{
SSG_lineConotours _line_downContours = { x,a_line_downEdgePts };
bottomContour.push_back(_line_downContours);
}
}
}
*maxEdgeId_left = 0;
*maxEdgeId_right = 0;
//<2F><>Peak<61><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>
for (int y= a_peak.y; y >= 0; y--)
{
if (y == 380)
int kkk = 1;
//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>bagL<67><4C><EFBFBD><EFBFBD>
double curr_y = laser3DPoints[a_peak.x].p3DPosition[y].pt3D.y;
double y_diff = abs(curr_y - pk_y);
if (y_diff > scanDistTh)
break;
bool foundVScanEdge = false;
std::vector< SSG_contourPtInfo> a_line_leftPts;
_searchLeftRightEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
y,
rgnPtAsEdge,
scanDistTh,
true,// searchDir_left,
a_line_leftPts,
&foundVScanEdge);
std::vector< SSG_contourPtInfo> a_line_rightPts;
_searchLeftRightEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
y,
rgnPtAsEdge,
scanDistTh,
false,// searchDir_right,
a_line_rightPts,
&foundVScanEdge);
if ( true == foundVScanEdge)
break;
else
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_leftPts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_left < a_line_leftPts[m].edgeId)
*maxEdgeId_left = a_line_leftPts[m].edgeId;
}
if (a_line_leftPts.size() > 0)
{
SSG_lineConotours _line_leftContours = { y, a_line_leftPts };
leftContour.insert(leftContour.begin(), _line_leftContours);
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_rightPts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_right < a_line_rightPts[m].edgeId)
*maxEdgeId_right = a_line_rightPts[m].edgeId;
}
if (a_line_rightPts.size() > 0)
{
SSG_lineConotours _line_rightContours = { y, a_line_rightPts };
rightContour.insert(rightContour.begin(), _line_rightContours);
}
}
}
//<2F><>Peak<61><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>
for (int y = a_peak.y+1; y < ptNum; y++)
{
if (y == 285)
int kkk = 1;
//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8>bagL<67><4C><EFBFBD><EFBFBD>
double curr_y = laser3DPoints[a_peak.x].p3DPosition[y].pt3D.y;
double y_diff = abs(curr_y - pk_y);
if (y_diff > scanDistTh)
break;
bool foundVScanEdge = false;
std::vector< SSG_contourPtInfo> a_line_leftPts;
_searchLeftRightEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
y,
rgnPtAsEdge,
scanDistTh,
true,// searchDir_left,
a_line_leftPts,
&foundVScanEdge);
std::vector< SSG_contourPtInfo> a_line_rightPts;
_searchLeftRightEdgePts(
laser3DPoints,
lineNum,
featureEdgeMask,
a_peak,
y,
rgnPtAsEdge,
scanDistTh,
false,// searchDir_right,
a_line_rightPts,
&foundVScanEdge);
if (true == foundVScanEdge)
break;
else
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_leftPts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_left < a_line_leftPts[m].edgeId)
*maxEdgeId_left = a_line_leftPts[m].edgeId;
}
if (a_line_leftPts.size() > 0)
{
SSG_lineConotours _line_leftContours = { y, a_line_leftPts };
leftContour.push_back( _line_leftContours);
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)a_line_rightPts.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if (*maxEdgeId_right < a_line_rightPts[m].edgeId)
*maxEdgeId_right = a_line_rightPts[m].edgeId;
}
if (a_line_rightPts.size() > 0)
{
SSG_lineConotours _line_rightContours = { y, a_line_rightPts };
rightContour.push_back(_line_rightContours);
}
}
}
return;
}
float EDistance(int x1, int y1, int x2, int y2)
{
return sqrt(float((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)));
}
float MDistance(int x1, int y1, int x2, int y2)
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
return (float)abs(x1 - x2) + (float)abs(y1 - y2);
source code
2025-06-08 10:46:41 +08:00
}
float CDistance(int x1, int y1, int x2, int y2)
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
return std::max((float)abs(x1 - x2), (float)abs(y1 - y2));
source code
2025-06-08 10:46:41 +08:00
}
float Distance(int x1, int y1, int x2, int y2, int type)
{
if (type == 0)
{
return EDistance(x1, y1, x2, y2);
}
else if (type == 1)
{
return MDistance(x1, y1, x2, y2);
}
解决编译中的warning
2025-07-22 22:52:57 +08:00
else //if (type == 2)
source code
2025-06-08 10:46:41 +08:00
{
return CDistance(x1, y1, x2, y2);
}
}
/// <summary>
/// <20><><EFBFBD><EFBFBD><EFBFBD>任
/// </summary>
/// <param name="input"> float<61><74> </param>
/// <param name="output"></param>
/// <param name="distType"></param>
/// mask1
/// q1 q2
/// q3 P
/// q4
///mask2
/// q4
/// P q3
/// q1 q2
void sg_distanceTrans(const cv::Mat input, cv::Mat& output, int distType)
{
cv::Mat BinaryImage = input.clone();
float* pRowOne;
float* pRowNext;
float distance;
float Mindis;
for (int i = 1; i < BinaryImage.rows - 1; i++)
{
pRowOne = BinaryImage.ptr<float>(i);
for (int j = 1; j < BinaryImage.cols; j++)
{
pRowNext = BinaryImage.ptr<float>(i - 1);
distance = Distance(i, j, i - 1, j - 1, distType);//q1
Mindis = std::min((float)pRowOne[j], distance + pRowNext[j - 1]);
distance = Distance(i, j, i - 1, j, distType);//q2
Mindis = std::min(Mindis, distance + pRowNext[j]);
pRowNext = BinaryImage.ptr<float>(i);
distance = Distance(i, j, i, j - 1, distType);//q3
Mindis = std::min(Mindis, distance + pRowNext[j-1]);
pRowNext = BinaryImage.ptr<float>(i + 1);//q4
distance = Distance(i, j, i + 1, j - 1, distType);
Mindis = std::min(Mindis, distance + pRowNext[j - 1]);
pRowOne[j] = Mindis;
}
}
for (int i = BinaryImage.rows - 2; i > 0; i--)
{
pRowOne = BinaryImage.ptr<float>(i);
for (int j = BinaryImage.cols - 2; j >= 0; j--)
{
pRowNext = BinaryImage.ptr<float>(i + 1);
distance = Distance(i, j, i + 1, j, distType);//q1
Mindis = std::min((float)pRowOne[j], distance + pRowNext[j]);
distance = Distance(i, j, i + 1, j + 1, distType);//q2
Mindis = std::min(Mindis, distance + pRowNext[j + 1]);
pRowNext = BinaryImage.ptr<float>(i);//q3
distance = Distance(i, j, i, j + 1, distType);
Mindis = std::min(Mindis, distance + pRowNext[j + 1]);
pRowNext = BinaryImage.ptr<float>(i - 1);//q4
distance = Distance(i, j, i - 1, j + 1, distType);
Mindis = std::min(Mindis, distance + pRowNext[j + 1]);
pRowOne[j] = Mindis;
}
}
//<2F><><EFBFBD>ܱ<EFBFBD>һȦ<D2BB><C8A6>0
float* row_0 = BinaryImage.ptr<float>(0);
float* row_last = BinaryImage.ptr<float>(input.rows - 1);
for (int i = 0; i < BinaryImage.cols; i++)
{
row_0[i] = 0;
row_last[i] = 0;
}
for (int i = 0; i < input.rows; i++)
{
BinaryImage.ptr<float>(i)[0] = 0;
BinaryImage.ptr<float>(i)[input.cols - 1] = 0;
}
output = BinaryImage;
}
//<2F><>searchWin<69><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB><EFBFBD>ߵ<EFBFBD>
void _findDistTransformPeak(cv::Mat& distTransform, SVzNL2DPoint LTpos, SSG_localPkParam searchWin, SSG_2DValueI* pkPos)
{
int line_end = LTpos.x + searchWin.seachW_lines;
if (line_end >= distTransform.cols)
line_end = distTransform.cols;
int pt_end = LTpos.y + searchWin.searchW_pts;
if (pt_end >= distTransform.rows)
pt_end = distTransform.rows;
SSG_2DValueI peak = { -1, -1, 0, 0, 0 };
double maxValue = -1;
for (int i = LTpos.x; i < line_end; i++)
{
for (int j = LTpos.y; j < pt_end; j++)
{
float value = distTransform.at<float>(j, i);
if (maxValue < 0)
{
peak = { i, j , 0, value };
maxValue = value;
}
else
{
if (maxValue <value)
{
peak = { i, j, 0, value , 0};
maxValue = value;
}
}
}
}
*pkPos = peak;
return;
}
/// <summary>
/// <20><>5x5<78><35>ʽѰ<CABD><D1B0>localPeaks
/// </summary>
/// <param name="input"></param>
/// <param name="peaks"></param>
void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector<SSG_2DValueI>& peaks, SSG_localPkParam searchWin)
{
cv::Mat mask = cv::Mat::zeros(input.rows,input.cols, CV_32SC1);
int winNum_cols = input.cols / (searchWin.seachW_lines / 2);
if ((input.cols % (searchWin.seachW_lines / 2)) > 0)
winNum_cols = winNum_cols + 1;
int winNum_rows = input.rows / (searchWin.searchW_pts / 2);
if ((input.rows % (searchWin.searchW_pts / 2)) > 0)
winNum_rows = winNum_rows + 1;
for (int i = 0; i < winNum_rows; i++)
{
for (int j = 0; j < winNum_cols; j++)
{
SVzNL2DPoint LTpos = { j * searchWin.searchW_pts / 2 , i * searchWin.seachW_lines / 2 };
SSG_2DValueI pkPos = { -1, -1, 0, 0 };
_findDistTransformPeak(input, LTpos, searchWin, &pkPos);
if ((pkPos.x >= 0) && (pkPos.y >= 0))
{
//<2F>߽紦<DFBD>ļ<EFBFBD>ֵ<EFBFBD><D6B5>ȥ<EFBFBD><C8A5>
if ((pkPos.x != LTpos.x) && (pkPos.x != (LTpos.x + searchWin.seachW_lines - 1)) &&
(pkPos.y != LTpos.y) && (pkPos.y != (LTpos.y + searchWin.searchW_pts - 1)))
{
int maskValue = mask.at<int>(pkPos.y, pkPos.x);
if (maskValue == 0)
{
peaks.push_back(pkPos);
mask.at<int>(pkPos.y, pkPos.x) = 1;
}
}
}
}
}
return;
}
Reference in New Issue Copy Permalink