1363 lines
43 KiB
C++
1363 lines
43 KiB
C++
|
#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};
|
|||
|
|
for (int i = 0, i_max = contourPts.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
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>
|
|||
|
|
for (int i = 0, i_max = contourRuns.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
SSG_RUN* curr_run = &contourRuns[i];
|
|||
|
|
if(curr_run->len > noiseRunLenTh)
|
|||
|
|
{
|
|||
|
|
idIndexing[curr_run->value] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int i = 0, i_max = contourPts.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
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;
|
|||
|
|
for (int m = 0, m_max = contour_all.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_contourPtInfo>& a_line_contourPts = contour_all[m].contourPts;
|
|||
|
|
for (int i = 0, i_max = a_line_contourPts.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
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;
|
|||
|
|
for (int m = 0, m_max = contourPairs.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
SSG_conotourPair& a_ptPair = contourPairs[m];
|
|||
|
|
for (int i = 0, i_max = idPairs.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
int totalNum = contourFilter.size();
|
|||
|
|
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;
|
|||
|
|
for (int i = 0, i_max = contour.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
for (int m = 0, m_max = contour.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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
|
|||
|
|
{
|
|||
|
|
for (int m = 0, m_max = a_line_upEdgePts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
for (int m = 0, m_max = a_line_downEdgePts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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
|
|||
|
|
{
|
|||
|
|
for (int m = 0, m_max = a_line_upEdgePts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
for (int m = 0, m_max = a_line_downEdgePts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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
|
|||
|
|
{
|
|||
|
|
for (int m = 0, m_max = a_line_leftPts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
for (int m = 0, m_max = a_line_rightPts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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
|
|||
|
|
{
|
|||
|
|
for (int m = 0, m_max = a_line_leftPts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
for (int m = 0, m_max = a_line_rightPts.size(); m < m_max; m++)
|
|||
|
|
{
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
return abs(x1 - x2) + abs(y1 - y2);
|
|||
|
|
}
|
|||
|
|
float CDistance(int x1, int y1, int x2, int y2)
|
|||
|
|
{
|
|||
|
|
return std::max(abs(x1 - x2), abs(y1 - y2));
|
|||
|
|
}
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
else if (type == 2)
|
|||
|
|
{
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|