651 lines
17 KiB
C++
651 lines
17 KiB
C++
#include <opencv2/opencv.hpp>
|
||
#include "SG_fireBrickAlgo.h"
|
||
#include "SG_fireBrick_Export.h"
|
||
#include "SG_errCode.h"
|
||
#include "SG_labelling.h"
|
||
|
||
#define M_PI 3.14159265358979323846 // pi
|
||
#define M_VALID_TOP_PLANE_PTNUM 1000
|
||
|
||
std::string CSGFireBrick::m_strVersion = "1.0.0";
|
||
|
||
CSGFireBrick::CSGFireBrick()
|
||
{
|
||
m_zHist.resize(Z_HIST_MAX_SIZE);
|
||
}
|
||
|
||
//使用RANSAC方法拟合平面，得到平面的法向，进而得到相机的旋转校正矩阵（3*3）
|
||
//poseR: 用于将得到的点云校正到相机与工作面垂直时的点云
|
||
//poseInvR：用于将得到的抓取点坐标调整到原始点云坐标。这个是因为进行手眼标定时是使用未校正的相机姿态进行的（相机使用标定板进行）
|
||
void sgGetCamPose_3DApproach(SVzNL3DLaserLine* scanData, int nLines, double* poseR, double* poseInvR)
|
||
{
|
||
|
||
}
|
||
|
||
void sgGetCamPose_2DApproach(double* poseR, double* poseInvR)
|
||
{
|
||
|
||
}
|
||
|
||
/// @brief
|
||
/// 创建实例
|
||
bool CSGFireBrick::CreateInstance(double dHistScale, ISGFireBrick** ppFireBrick)
|
||
{
|
||
CSGFireBrick* p = new CSGFireBrick;
|
||
if (false == p->_Init(dHistScale))
|
||
{
|
||
delete p;
|
||
p = nullptr;
|
||
}
|
||
*ppFireBrick = p;
|
||
return nullptr != (*ppFireBrick);
|
||
}
|
||
|
||
const char* CSGFireBrick::GetVersion()
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
void CSGFireBrick::sgCalibCamPose()
|
||
{
|
||
|
||
}
|
||
|
||
void CSGFireBrick::sgSegHistScale(double data)
|
||
{
|
||
m_histScale = data;
|
||
return;
|
||
}
|
||
|
||
void CSGFireBrick::sgSetPoseSortingMode(ESG_poseSortingMode mode)
|
||
{
|
||
m_sortingMode = mode;
|
||
return;
|
||
}
|
||
|
||
ESG_poseSortingMode CSGFireBrick::sgGetPoseSortingMode()
|
||
{
|
||
return m_sortingMode;
|
||
}
|
||
|
||
//扫描线处理：针对每一条扫描线进行处理，在扫描的同时进行处理
|
||
//直线段提取，和基于直线段的生长
|
||
void CSGFireBrick::sgScanLineProc(SVzNL3DLaserLine* a_line, double* camPoseR, int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
if (m_nFrameWidth < 0)
|
||
{
|
||
m_nFrameWidth = a_line->nPositionCnt;
|
||
m_nFrameHeight = 1;
|
||
}
|
||
else
|
||
{
|
||
if (m_nFrameWidth != a_line->nPositionCnt)
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
m_nFrameHeight++;
|
||
}
|
||
//(1)校正
|
||
//(2)Z方向统计
|
||
for (int i = 0; i < a_line->nPositionCnt; i++)
|
||
{
|
||
SVzNL3DPosition* a_pt = &a_line->p3DPosition[i];
|
||
if (a_pt->pt3D.z > 1e-4)
|
||
{
|
||
double x = camPoseR[0] * a_pt->pt3D.x + camPoseR[1] * a_pt->pt3D.y + camPoseR[2] * a_pt->pt3D.z;
|
||
double y = camPoseR[3] * a_pt->pt3D.x + camPoseR[4] * a_pt->pt3D.y + camPoseR[5] * a_pt->pt3D.z;
|
||
double z = camPoseR[6] * a_pt->pt3D.x + camPoseR[7] * a_pt->pt3D.y + camPoseR[8] * a_pt->pt3D.z;
|
||
a_pt->pt3D.x = x;
|
||
a_pt->pt3D.y = y;
|
||
a_pt->pt3D.z = z;
|
||
//Hist
|
||
int idx = (int)(z / m_histScale); //0.5mm
|
||
m_zHist[idx] = m_zHist[idx]+1;
|
||
//统计Z范围
|
||
if (m_zIdxRange.nMin < 0)
|
||
{
|
||
m_zIdxRange.nMin = idx;
|
||
m_zIdxRange.nMax = idx;
|
||
}
|
||
else
|
||
{
|
||
if (m_zIdxRange.nMin > idx)
|
||
m_zIdxRange.nMin = idx;
|
||
if (m_zIdxRange.nMax < idx)
|
||
m_zIdxRange.nMax = idx;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void _sgSearchPeaks(std::vector<int> sumHist, std::vector<SSG_RunData>& peaks)
|
||
{
|
||
int state = 0; //0-初态；1-上升；2-下降
|
||
int preSum = -1;
|
||
SSG_RunData a_run = {0, 0, 0};
|
||
for (int i = 0, i_max = sumHist.size(); i < i_max; i++)
|
||
{
|
||
int curSum = sumHist[i];
|
||
if (0 == state)
|
||
{
|
||
if (preSum >= 0)
|
||
{
|
||
if (curSum > preSum)
|
||
{
|
||
state = 1;
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = curSum;
|
||
}
|
||
else if (curSum < preSum)
|
||
{
|
||
state = 2;
|
||
}
|
||
}
|
||
preSum = curSum;
|
||
}
|
||
else if (1 == state) //上升
|
||
{
|
||
if (curSum > preSum)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = curSum;
|
||
if(i == i_max-1)
|
||
peaks.push_back(a_run);
|
||
}
|
||
else if (curSum == preSum)
|
||
{
|
||
a_run.len++;
|
||
if (i == i_max - 1)
|
||
peaks.push_back(a_run);
|
||
}
|
||
else
|
||
{
|
||
peaks.push_back(a_run);
|
||
a_run = { 0, 0, 0 };
|
||
state = 2;
|
||
}
|
||
preSum = curSum;
|
||
}
|
||
else if (2 == state) //下降
|
||
{
|
||
if (curSum > preSum)
|
||
{
|
||
state = 1;
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = curSum;
|
||
if (i == i_max - 1)
|
||
peaks.push_back(a_run);
|
||
}
|
||
preSum = curSum;
|
||
}
|
||
else //异常
|
||
{
|
||
state = 0;
|
||
continue;
|
||
}
|
||
}
|
||
}
|
||
|
||
bool _checkHOverlap(const SSG_Region* rgn_1, const SSG_Region* rgn_2)
|
||
{
|
||
if ((rgn_1->roi.right > rgn_2->roi.left) && (rgn_2->roi.right > rgn_1->roi.left))
|
||
{
|
||
int overlap_left = rgn_1->roi.left > rgn_2->roi.left ? rgn_1->roi.left : rgn_2->roi.left;
|
||
int overlap_right = rgn_1->roi.right < rgn_2->roi.right ? rgn_1->roi.right : rgn_2->roi.right;
|
||
int overlap_width = overlap_right - overlap_left;
|
||
int w_1 = rgn_1->roi.right - rgn_1->roi.left;
|
||
int w_2 = rgn_2->roi.right - rgn_2->roi.left;
|
||
if ((overlap_width > w_1 / 2) && (overlap_width > w_2 / 2))
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
bool _checkVOverlap(const SSG_Region* rgn_1, const SSG_Region* rgn_2)
|
||
{
|
||
if ((rgn_1->roi.bottom > rgn_2->roi.top) && (rgn_2->roi.bottom > rgn_1->roi.top))
|
||
{
|
||
int overlap_top = rgn_1->roi.top > rgn_2->roi.top ? rgn_1->roi.top : rgn_2->roi.top;
|
||
int overlap_bottom = rgn_1->roi.bottom < rgn_2->roi.bottom ? rgn_1->roi.bottom : rgn_2->roi.bottom;
|
||
int overlap_height = overlap_bottom - overlap_top;
|
||
int h_1 = rgn_1->roi.bottom - rgn_1->roi.top;
|
||
int h_2 = rgn_2->roi.bottom - rgn_2->roi.top;
|
||
if ((overlap_height > h_1 / 2) && (overlap_height > h_2 / 2))
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
SSG_Region* _getSortedObj(std::vector<SSG_Region>& labelRgns, ESG_poseSortingMode sortingMode, int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSG_Region* objRgn = nullptr;
|
||
//按照排序挑选一个目标
|
||
for (int i = 0, i_max = labelRgns.size(); i < i_max; i++)
|
||
{
|
||
if (labelRgns[i].ptCounter > M_VALID_TOP_PLANE_PTNUM)
|
||
{
|
||
if (nullptr == objRgn)
|
||
objRgn = &labelRgns[i];
|
||
else
|
||
{
|
||
SSG_Region* chkRgn = &labelRgns[i];
|
||
//计算水平重叠区和垂直重叠区
|
||
bool isVOverlap = _checkVOverlap(objRgn, chkRgn);
|
||
bool isHOverlap = _checkHOverlap(objRgn, chkRgn);
|
||
//默认从左到右，从上到下
|
||
if ((keSG_PoseSorting_L2R_T2B == sortingMode) || (keSG_PoseSorting_Uknown == sortingMode))
|
||
{
|
||
if (true == isHOverlap)
|
||
{
|
||
//T2B
|
||
if (objRgn->roi.top > chkRgn->roi.top)
|
||
objRgn = chkRgn;
|
||
}
|
||
else
|
||
if (objRgn->roi.left > chkRgn->roi.left)
|
||
objRgn = chkRgn;
|
||
}
|
||
else if (keSG_PoseSorting_T2B_L2R == sortingMode)
|
||
{
|
||
if(true == isVOverlap)
|
||
{
|
||
//L2R
|
||
if (objRgn->roi.left > chkRgn->roi.left)
|
||
objRgn = chkRgn;
|
||
}
|
||
else
|
||
if (objRgn->roi.top > chkRgn->roi.top)
|
||
objRgn = chkRgn;
|
||
}
|
||
else
|
||
{
|
||
//不支持的排序方式
|
||
*errCode = SG_ERR_INVLD_SORTING_MODE;
|
||
return nullptr;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return objRgn;
|
||
}
|
||
|
||
#define RGN_OUTER_PT 0
|
||
#define RGN_CONTOUR_PT 1
|
||
#define RGN_INNER_PT 2
|
||
#define RGN_CONTOUR_TRACKED 3
|
||
|
||
void _rgnContourTracking(cv::Mat& rgnImg, std::vector<SSG_RgnContour2D>& contours)
|
||
{
|
||
SVzNL2DPoint scanSeq[8] = {
|
||
{-1,-1}, {0,-1}, {1,-1}, {1,0}, {1, 1}, {0, 1}, {-1, 1}, {-1, 0} };
|
||
//边界跟踪
|
||
while (1)
|
||
{
|
||
//取种子点,选定任意一个方向
|
||
bool foundSeed = false;
|
||
SVzNL2DPoint seed;
|
||
for (int i = 1; i < rgnImg.rows; i++)
|
||
{
|
||
for (int j = 1; j < rgnImg.cols; j++)
|
||
{
|
||
if (1 == rgnImg.at<uchar>(i, j)) //边界像素
|
||
{
|
||
int outerNum = 0;
|
||
int innerNum = 0;
|
||
for (int m = 0; m < 8; m++)
|
||
{
|
||
if (RGN_OUTER_PT == rgnImg.at<uchar>(i + scanSeq[m].y, j + scanSeq[m].x)) //外部像素计数
|
||
outerNum++;
|
||
else if (RGN_INNER_PT == rgnImg.at<uchar>(i + scanSeq[m].y, j + scanSeq[m].x)) //内部像素计数
|
||
innerNum++;
|
||
}
|
||
if ((outerNum > 0) && (innerNum > 0))
|
||
{
|
||
foundSeed = true;
|
||
seed = { j, i };
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (true == foundSeed)
|
||
break;
|
||
}
|
||
if (false == foundSeed)
|
||
break;
|
||
|
||
SSG_RgnContour2D a_contour;
|
||
a_contour.isClosed = 0;
|
||
a_contour.contourPtList.push_back(seed);
|
||
rgnImg.at<uchar>(seed.y, seed.x) = RGN_CONTOUR_TRACKED; //已经被寻迹
|
||
while (1)
|
||
{
|
||
//搜索 RGN_CONTOUR_PT-RGN_INNER_PT 序列。这个是搜索方向。或找到序列，由RGN_CONTOUR_PT为一个种子点
|
||
int preState = rgnImg.at<uchar>(seed.y + scanSeq[0].y, seed.x + scanSeq[0].x);
|
||
bool found_next = false;
|
||
for (int m = 1; m <= 8; m++)
|
||
{
|
||
int idx = m % 8;
|
||
int currState = rgnImg.at<uchar>(seed.y + scanSeq[idx].y, seed.x + scanSeq[idx].x);
|
||
if ((RGN_CONTOUR_PT == preState) && (RGN_INNER_PT == currState))
|
||
{
|
||
seed = { seed.x + scanSeq[m-1].x , seed.y + scanSeq[m-1].y };
|
||
a_contour.contourPtList.push_back(seed);
|
||
rgnImg.at<uchar>(seed.y, seed.x) = RGN_CONTOUR_TRACKED;
|
||
found_next = true;
|
||
break;
|
||
}
|
||
else if ((RGN_CONTOUR_TRACKED == preState) && (RGN_INNER_PT == currState))
|
||
{
|
||
if (a_contour.contourPtList.size() > 2)
|
||
{
|
||
SVzNL2DPoint seed_0 = a_contour.contourPtList[0];
|
||
SVzNL2DPoint seed_1 = a_contour.contourPtList[1];
|
||
if ( ((seed_0.x == seed.x + scanSeq[m - 1].x) && (seed_0.y == seed.y + scanSeq[m - 1].y)) ||
|
||
((seed_1.x == seed.x + scanSeq[m - 1].x) && (seed_1.y == seed.y + scanSeq[m - 1].y)))
|
||
{
|
||
a_contour.isClosed = 1;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
preState = currState;
|
||
}
|
||
if (false == found_next)
|
||
break;
|
||
}
|
||
contours.push_back(a_contour);
|
||
if (1 == a_contour.isClosed)
|
||
break;
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _getRgnContourData(SVzNL3DLaserLine* scanData, cv::Mat& labImg, const SSG_Region* objRgn, std::vector<SSG_RgnContour3D>& contours3D)
|
||
{
|
||
int rgnH = objRgn->roi.bottom - objRgn->roi.top + 1;
|
||
int rgnW = objRgn->roi.right - objRgn->roi.left + 1;
|
||
cv::Mat objImg = cv::Mat::zeros(rgnH+2, rgnW+2, CV_8UC1); //扩大一圈，保证8连接处理时避开边界处理
|
||
cv::Mat objInvImg = cv::Mat::ones(rgnH + 2, rgnW + 2, CV_8UC1); //扩大一圈，保证8连接处理时避开边界处理
|
||
for (int i = 0; i < rgnH; i++)
|
||
{
|
||
for (int j = 0; j < rgnW; j++)
|
||
{
|
||
int gi = i + objRgn->roi.top;
|
||
int gj = j + objRgn->roi.left;
|
||
if (objRgn->labelID == labImg.at<int>(gi, gj))
|
||
{
|
||
objImg.at<uchar>(i + 1, j + 1) = RGN_CONTOUR_PT;
|
||
objInvImg.at<uchar>(i + 1, j + 1) = 0;
|
||
}
|
||
}
|
||
}
|
||
//在寻找边界前填充所有的孔洞
|
||
//对objInvImg进行连通域标注
|
||
cv::Mat invLabImg = cv::Mat::zeros(rgnH + 2, rgnW + 2, CV_32SC1);//rows, cols,
|
||
std::vector<SSG_Region> holeRgns;
|
||
SG_TwoPassLabel(objInvImg, invLabImg, holeRgns);
|
||
cv::Mat holeFillingImg = objImg.clone();
|
||
for (int rgnId = 0, rgnId_max = holeRgns.size(); rgnId < rgnId_max; rgnId++)
|
||
{
|
||
SSG_Region* a_hole = &holeRgns[rgnId];
|
||
if ((1 >= a_hole->roi.left) || (1 >= a_hole->roi.top) ||
|
||
(invLabImg.rows-2 <= a_hole->roi.bottom) || (invLabImg.cols-2 <= a_hole->roi.right))
|
||
continue;
|
||
//填充
|
||
for (int m = a_hole->roi.top; m <= a_hole->roi.bottom; m++)
|
||
{
|
||
for (int n = a_hole->roi.left; n <= a_hole->roi.right; n++)
|
||
{
|
||
int kkk = invLabImg.at<int>(m, n);
|
||
if (a_hole->labelID == invLabImg.at<int>(m, n))
|
||
holeFillingImg.at<uchar>(m, n) = RGN_CONTOUR_PT;
|
||
}
|
||
}
|
||
}
|
||
#if ENABLE_OUTPUT_DEBUG_IMAGE
|
||
cv::Mat dbg_holeFillingImg; // = bwImg.clone();
|
||
holeFillingImg.convertTo(dbg_holeFillingImg, CV_8UC3, 255);
|
||
cv::imwrite("top_plane_holeFilling.png", dbg_holeFillingImg);
|
||
#endif
|
||
//边缘像素提取
|
||
cv::Mat contourImg = holeFillingImg.clone();
|
||
for (int i = 1; i <= rgnH; i++)
|
||
{
|
||
for (int j = 1; j <= rgnW; j++)
|
||
{
|
||
if (holeFillingImg.at<uchar>(i, j) > 0)
|
||
{
|
||
uchar sum = holeFillingImg.at<uchar>(i - 1, j - 1) + holeFillingImg.at<uchar>(i, j - 1) + holeFillingImg.at<uchar>(i + 1, j - 1) +
|
||
holeFillingImg.at<uchar>(i - 1, j) + holeFillingImg.at<uchar>(i + 1, j) +
|
||
holeFillingImg.at<uchar>(i - 1, j + 1) + holeFillingImg.at<uchar>(i, j + 1) + holeFillingImg.at<uchar>(i + 1, j + 1);
|
||
if (sum == 8)
|
||
contourImg.at<uchar>(i, j) = RGN_INNER_PT; //内部像素
|
||
}
|
||
}
|
||
}
|
||
std::vector<SSG_RgnContour2D> contours;
|
||
_rgnContourTracking(contourImg, contours);
|
||
//生成边界点序列（3D点）
|
||
for (int i = 0; i < contours.size(); i++)
|
||
{
|
||
//从栅格格式生成三维点序列
|
||
SSG_RgnContour3D a_contour3D;
|
||
a_contour3D.isClosed = contours[i].isClosed;
|
||
for (int n = 0; n < contours[i].contourPtList.size(); n++)
|
||
{
|
||
SSG_gridPt3D a_pt;
|
||
a_pt.gridPos = { contours[i].contourPtList[n].x - 3, contours[i].contourPtList[n].y - 3 };
|
||
a_pt.pt3D = scanData[a_pt.gridPos.y].p3DPosition[a_pt.gridPos.x].pt3D;
|
||
a_contour3D.contourPtList.push_back(a_pt);
|
||
}
|
||
contours3D.push_back(a_contour3D);
|
||
}
|
||
#if ENABLE_OUTPUT_DEBUG_IMAGE
|
||
cv::Mat dbg_contourImg; // = bwImg.clone();
|
||
contourImg.convertTo(dbg_contourImg, CV_8UC3, 128);
|
||
cv::imwrite("top_plane_contour.png", dbg_contourImg);
|
||
#endif
|
||
}
|
||
|
||
void _trackingRgnContour()
|
||
{
|
||
|
||
}
|
||
|
||
void _getTrackingCorers()
|
||
{
|
||
|
||
}
|
||
|
||
SSG_meanVar _computeMeanVar(double* data, int size, bool skipZeroData)
|
||
{
|
||
double sum_x = 0;
|
||
double sum_square = 0;
|
||
int num = 0;
|
||
for (int i = 0; i < size; i++)
|
||
{
|
||
if ((skipZeroData == true) && (data[i] < 1e-4))
|
||
continue;
|
||
sum_x += data[i];
|
||
sum_square += data[i] * data[i];
|
||
num++;
|
||
}
|
||
SSG_meanVar a_meanVar = { 0, 0 };
|
||
if (num > 0)
|
||
{
|
||
a_meanVar.mean = sum_x / num;
|
||
a_meanVar.var = sum_square / num - a_meanVar.mean * a_meanVar.mean;
|
||
if (a_meanVar.var < 0)
|
||
a_meanVar.var = 0;
|
||
else
|
||
a_meanVar.var = sqrt(a_meanVar.var);
|
||
}
|
||
return a_meanVar;
|
||
}
|
||
|
||
void _getLineDataMeanVar(std::vector<double>& lineData, const int meanVarWinSize, std::vector<SSG_meanVar>& meanVarData)
|
||
{
|
||
int halfWin = meanVarWinSize / 2;
|
||
int size = lineData.size();
|
||
for (int i = 0; i < size; i++)
|
||
{
|
||
SSG_meanVar a_meanVar = { 0, 0 };
|
||
if ((i >= halfWin) && (i < size - halfWin))
|
||
SSG_meanVar a_meanVar = _computeMeanVar(&lineData[i - halfWin], meanVarWinSize, true);
|
||
meanVarData.push_back(a_meanVar);
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _getMeanVarAbnormalPt(std::vector<SSG_meanVar>& meanVarData, std::vector< SSG_meanVarAbnormalPt> abnormaPts)
|
||
{
|
||
|
||
|
||
}
|
||
|
||
void _rgnLineSegmentation(SVzNL3DLaserLine* scanData, cv::Mat& labImg, SSG_Region* objRgn)
|
||
{
|
||
///通过均值和方差分析，判断可能的凹点
|
||
for (int y = objRgn->roi.top; y <= objRgn->roi.bottom; y++)
|
||
{
|
||
std::vector<double> lineData;
|
||
for (int x = objRgn->roi.left; x <= objRgn->roi.right; x++)
|
||
lineData.push_back(scanData[y - 2].p3DPosition[x - 2].pt3D.z);
|
||
//均值和方差处理，寻找凹点
|
||
|
||
}
|
||
//水平处理
|
||
//垂直处理
|
||
}
|
||
|
||
/// @brief
|
||
/// 获取耐火砖抓取姿态
|
||
/// 两段式算法：第一段为扫描线处理，在接收扫描线数据后便立即处理；第二段为本处理
|
||
void CSGFireBrick::sgGetBrickPose(SVzNL3DLaserLine* scanData, int nLines, std::vector<SSG_6AxisAttitude>& brickPoses, int* errCode, SVzNL3DLaserLine* resultData)
|
||
{
|
||
*errCode = 0;
|
||
if (m_nFrameHeight < 10) //扫描线过少
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||
return;
|
||
}
|
||
|
||
//算法流程: (1)校正（2）Z方向统计 （3）取Z最小的一个峰值作为顶面（4）针对顶面进行分割
|
||
//(1)和(2)已经在sgScanLineProc（）中完成
|
||
std::vector<int> sumHist;
|
||
const int sumWin = 5;
|
||
for (int i = m_zIdxRange.nMin; i <= m_zIdxRange.nMax; i++)
|
||
{
|
||
//窗口
|
||
int sumValue = 0;
|
||
for (int j = -sumWin; j <= sumWin; j++)
|
||
{
|
||
int chkIdx = i + j;
|
||
if ((chkIdx >= 0) && (chkIdx < Z_HIST_MAX_SIZE))
|
||
sumValue += m_zHist[chkIdx];
|
||
}
|
||
sumHist.push_back(sumValue);
|
||
}
|
||
//搜索第一个峰值，作为耐火砖的最顶面
|
||
std::vector<SSG_RunData> peaks;
|
||
_sgSearchPeaks(sumHist, peaks);
|
||
if (0 == peaks.size())
|
||
{
|
||
*errCode = SG_ERR_FOUND_NO_TOP_PLANE;
|
||
return;
|
||
}
|
||
SSG_RunData* top_plane = nullptr;
|
||
for (int i = 0, i_max = peaks.size(); i < i_max; i++)
|
||
{
|
||
if (peaks[i].value < M_VALID_TOP_PLANE_PTNUM)
|
||
continue;
|
||
|
||
top_plane = &peaks[i];
|
||
break;
|
||
}
|
||
if (nullptr == top_plane)
|
||
{
|
||
*errCode = SG_ERR_FOUND_NO_TOP_PLANE;
|
||
return;
|
||
}
|
||
|
||
//取顶面数据
|
||
SVzNLRangeD zTopRng;
|
||
zTopRng.min = (top_plane->start + m_zIdxRange.nMin) * m_histScale - m_planeThick /2;
|
||
zTopRng.max = (top_plane->start + top_plane->len -1 + m_zIdxRange.nMin) * m_histScale + m_planeThick / 2;
|
||
cv::Mat bwImg = cv::Mat::zeros(m_nFrameHeight+4, m_nFrameWidth+4, CV_8UC1);//rows, cols,
|
||
for (int i = 0; i < m_nFrameHeight; i++)
|
||
{
|
||
for (int j = 0; j < m_nFrameWidth; j++)
|
||
{
|
||
double z = scanData[i].p3DPosition[j].pt3D.z;
|
||
if ((z >= zTopRng.min) && (z <= zTopRng.max))
|
||
bwImg.at<uchar>(i+2,j+2) = 1;
|
||
}
|
||
|
||
}
|
||
|
||
#if ENABLE_OUTPUT_DEBUG_IMAGE
|
||
cv::Mat grayImg; // = bwImg.clone();
|
||
bwImg.convertTo(grayImg, CV_8UC3, 255);
|
||
cv::imwrite("top_plane.png", grayImg);
|
||
#endif
|
||
//
|
||
while (1) //采用迭代思想处理。智能的核心之一是迭代
|
||
{
|
||
//标注分类
|
||
cv::Mat labImg = cv::Mat::zeros(m_nFrameHeight, m_nFrameWidth, CV_32SC1);//rows, cols,
|
||
std::vector<SSG_Region>labelRgns;
|
||
SG_TwoPassLabel(bwImg, labImg, labelRgns);
|
||
//按照排序要求取一个目标
|
||
SSG_Region* objRgn = _getSortedObj(labelRgns, m_sortingMode, errCode);
|
||
if (*errCode != 0)
|
||
return;
|
||
|
||
//进行均值方差分析，提取线段边界点，进一步分割
|
||
_rgnLineSegmentation();
|
||
|
||
|
||
|
||
//获取轮廓点序列，根据序列计算角点
|
||
std::vector<SSG_RgnContour3D> contours3D;
|
||
_getRgnContourData(scanData, labImg, objRgn, contours3D);
|
||
#if ENABLE_OUTPUT_DEBUG_IMAGE
|
||
//输出边界3D点
|
||
|
||
#endif
|
||
_trackingRgnContour();
|
||
//检查角点，确定是否要细分
|
||
|
||
}
|
||
|
||
|
||
}
|
||
|
||
void CSGFireBrick::sgSortBrickPoses(std::vector<SSG_6AxisAttitude>& brickPoses, ESG_poseSortingMode sortingMode)
|
||
{
|
||
|
||
}
|
||
|
||
/// @brief
|
||
/// 初始化
|
||
bool CSGFireBrick::_Init(double dHistScale)
|
||
{
|
||
m_histScale = dHistScale;
|
||
return true;
|
||
}
|
||
|
||
bool SGCreateFireBrick(double dHistScale, ISGFireBrick** ppFireBrick)
|
||
{
|
||
return CSGFireBrick::CreateInstance(dHistScale, ppFireBrick);
|
||
} |