1362 lines
43 KiB
C++
1362 lines
43 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
#include <opencv2/opencv.hpp>
|
||
|
||
//在searchWin中搜索最高点(z最小)
|
||
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;
|
||
}
|
||
|
||
///搜索局部最高点（z最小点）。
|
||
///搜索方法：每次步进搜索窗口长度的一半。对局部最高点进行标记，防止被重复记录。
|
||
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))
|
||
{
|
||
//边界处的极值点去除
|
||
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);
|
||
//游程分析
|
||
std::vector< SSG_RUN> contourRuns;
|
||
SSG_RUN a_run = { -1,-1,-1};
|
||
for (int i = 0, i_max = (int)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);
|
||
//新的游程
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = a_pt->value;
|
||
}
|
||
}
|
||
}
|
||
//处理最后一个
|
||
if (contourRuns.size() > 0)
|
||
{
|
||
//检查是否和第一个合并
|
||
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)
|
||
{
|
||
//保留长的边界，小的短的边界可能是噪声
|
||
for (int i = 0, i_max = (int)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 = (int)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>
|
||
/// 区域生长法：以局部最高点作为生长种子进行生长
|
||
/// 生长方法与一般的区域生长不同：以种子点为圆心作圆周扫描，记录扫描到的边界
|
||
/// </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); //小于22.5度的边界为噪声
|
||
_getRgnVldContour(contours, rgnContour, maxEdgeId, noiseRunLenTh);
|
||
return;
|
||
}
|
||
|
||
//取出指定的contour点
|
||
void sg_getContourPts(
|
||
std::vector< SSG_lineConotours>& contour_all,
|
||
int vldEdgeId,
|
||
std::vector< SSG_2DValueI>& contourFilter,
|
||
int* lowLevelFlag)
|
||
{
|
||
*lowLevelFlag = 0;
|
||
//生成有效contour
|
||
//统计block(遮挡)的比例
|
||
int blockPtNum = 0;
|
||
for (int m = 0, m_max = (int)contour_all.size(); m < m_max; m++)
|
||
{
|
||
std::vector<SSG_contourPtInfo>& a_line_contourPts = contour_all[m].contourPts;
|
||
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;
|
||
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;
|
||
}
|
||
|
||
//取出指定的contour点
|
||
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;
|
||
//生成有效contour
|
||
//统计block(遮挡)的比例
|
||
int blockPtNum_0 = 0;
|
||
int blockPtNum_1 = 0;
|
||
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 = (int)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 = (int)contourFilter.size();
|
||
if(blockPtNum_0 > totalNum/2)
|
||
*lowLevelFlag_0 = 1;
|
||
if (blockPtNum_1 > totalNum / 2)
|
||
*lowLevelFlag_1 = 1;
|
||
|
||
return;
|
||
}
|
||
|
||
//取出contour点
|
||
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 = (int)contour.size(); i < i_max; i++)
|
||
{
|
||
SSG_contourPtInfo* a_contourPt = &contour[i];
|
||
int edgeIdx = a_contourPt->edgeId;
|
||
if (edgeInfo[edgeIdx].ptNum == 0) //新的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;
|
||
}
|
||
//计算平均scanDist
|
||
//保留scanDist最小的，只有点数大于总点数的10%才是有效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;
|
||
|
||
//以最小距离为基础，检查其余线段与此线段的距离。小于sameConturDistTh为同一线段
|
||
//生成有效contour
|
||
//统计block(遮挡)的比例
|
||
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 = (int)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;//屏蔽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)))
|
||
{
|
||
//碰撞检测（检测是否为下层目标)
|
||
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))
|
||
{
|
||
//继续向前检测5步
|
||
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;//屏蔽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;//屏蔽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)))
|
||
{
|
||
//碰撞检测（检测是否为下层目标)
|
||
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))
|
||
{
|
||
//继续向前检测5步
|
||
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;//屏蔽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; //无效
|
||
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)) //在区域外面
|
||
stopFlag = true;
|
||
else
|
||
{
|
||
//进行Edge检查，检查Edge有没有突然变化
|
||
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; //无效
|
||
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))//在区域外面
|
||
stopFlag = true;
|
||
else
|
||
{
|
||
//进行Edge检查，检查Edge有没有突然变化
|
||
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; //无效
|
||
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)) //在区域外面
|
||
stopFlag = true;
|
||
else
|
||
{
|
||
//进行Edge检查，检查Edge有没有突然变化
|
||
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; //无效
|
||
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))//在区域外面
|
||
stopFlag = true;
|
||
else
|
||
{
|
||
//进行Edge检查，检查Edge有没有突然变化
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//从Peak点进行水平垂直扫描得到区域边界
|
||
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;
|
||
//从Peak点向左扫描
|
||
for (int x = a_peak.x; x >= 0; x--)
|
||
{
|
||
if (x == 0)
|
||
int kkk = 1;
|
||
|
||
//最多扫描bagL宽度
|
||
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 = (int)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 = (int)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);
|
||
}
|
||
}
|
||
}
|
||
//从Peak点向右扫描
|
||
for (int x = a_peak.x+1; x < lineNum; x++)
|
||
{
|
||
if (x == 34)
|
||
int kkk = 1;
|
||
|
||
//最多扫描bagL宽度
|
||
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 = (int)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 = (int)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;
|
||
//从Peak点向上扫描
|
||
for (int y= a_peak.y; y >= 0; y--)
|
||
{
|
||
if (y == 380)
|
||
int kkk = 1;
|
||
|
||
//最多扫描bagL宽度
|
||
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 = (int)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 = (int)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);
|
||
}
|
||
}
|
||
}
|
||
//从Peak点向下扫描
|
||
for (int y = a_peak.y+1; y < ptNum; y++)
|
||
{
|
||
if (y == 285)
|
||
int kkk = 1;
|
||
//最多扫描bagL宽度
|
||
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 = (int)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 = (int)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 (float)abs(x1 - x2) + (float)abs(y1 - y2);
|
||
}
|
||
float CDistance(int x1, int y1, int x2, int y2)
|
||
{
|
||
return std::max((float)abs(x1 - x2), (float)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>
|
||
/// 距离变换
|
||
/// </summary>
|
||
/// <param name="input"> float型 </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;
|
||
}
|
||
}
|
||
//将周边一圈置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;
|
||
}
|
||
|
||
//在searchWin中搜索距离变换最高点
|
||
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>
|
||
/// 以5x5方式寻找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, 0};
|
||
_findDistTransformPeak(input, LTpos, searchWin, &pkPos);
|
||
if ((pkPos.x >= 0) && (pkPos.y >= 0))
|
||
{
|
||
//边界处的极值点去除
|
||
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;
|
||
}
|