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

651 lines
17 KiB
C++
Raw Normal View History

source code
2025-06-08 10:46:41 +08:00
#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);
}
//ʹ<><CAB9>RANSAC<41><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>棬<EFBFBD>õ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ķ<EFBFBD><C4B7>򣬽<EFBFBD><F2A3ACBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תУ<D7AA><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3*3<><33>
//poseR: <20><><EFBFBD>ڽ<EFBFBD><DABD>õ<EFBFBD><C3B5>ĵ<EFBFBD><C4B5><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>빤<EFBFBD><EBB9A4><EFBFBD>洹ֱʱ<D6B1>ĵ<EFBFBD><C4B5><EFBFBD>
//poseInvR<76><52><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD>õ<EFBFBD><C3B5><EFBFBD>ץȡ<D7A5><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ꡣ<EFBFBD><EAA1A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨ʱ<EAB6A8><CAB1>ʹ<EFBFBD><CAB9>δУ<CEB4><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>еģ<D0B5><C4A3><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ñ궨<C3B1><EAB6A8><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>
void sgGetCamPose_3DApproach(SVzNL3DLaserLine* scanData, int nLines, double* poseR, double* poseInvR)
{
}
void sgGetCamPose_2DApproach(double* poseR, double* poseInvR)
{
}
/// @brief
/// <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>
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;
}
//ɨ<><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿһ<C3BF><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD><DFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ͬʱ<CDAC><CAB1><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
//ֱ<>߶<EFBFBD><DFB6><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD>ֱ<EFBFBD>߶ε<DFB6><CEB5><EFBFBD><EFBFBD><EFBFBD>
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)У<><D0A3>
//(2)Z<><5A><EFBFBD><EFBFBD>ͳ<EFBFBD><CDB3>
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;
//ͳ<><CDB3>Z<EFBFBD><5A>Χ
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-<2D><>̬<EFBFBD><CCAC>1-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2-<2D>½<EFBFBD>
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) //<2F><><EFBFBD><EFBFBD>
{
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) //<2F>½<EFBFBD>
{
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 //<2F>쳣
{
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡһ<D1A1><D2BB>Ŀ<EFBFBD><C4BF>
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];
//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE>ص<EFBFBD><D8B5><EFBFBD><EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD>ص<EFBFBD><D8B5><EFBFBD>
bool isVOverlap = _checkVOverlap(objRgn, chkRgn);
bool isHOverlap = _checkHOverlap(objRgn, chkRgn);
//Ĭ<>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ң<EFBFBD><D2A3><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>
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
{
//<2F><>֧<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
*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} };
//<2F>߽<EFBFBD><DFBD><EFBFBD><EFBFBD><EFBFBD>
while (1)
{
//ȡ<><C8A1><EFBFBD>ӵ<EFBFBD>,ѡ<><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)) //<2F>߽<EFBFBD><DFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)) //<2F>ⲿ<EFBFBD><E2B2BF><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>
outerNum++;
else if (RGN_INNER_PT == rgnImg.at<uchar>(i + scanSeq[m].y, j + scanSeq[m].x)) //<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>
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; //<2F>Ѿ<EFBFBD><D1BE><EFBFBD>Ѱ<EFBFBD><D1B0>
while (1)
{
//<2F><><EFBFBD><EFBFBD> RGN_CONTOUR_PT-RGN_INNER_PT <20><><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>򡣻<EFBFBD><F2A1A3BB>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD>RGN_CONTOUR_PTΪһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>
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); //<2F><><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD>֤8<D6A4><38><EFBFBD>Ӵ<EFBFBD><D3B4><EFBFBD>ʱ<EFBFBD>ܿ<EFBFBD><DCBF>߽紦<DFBD><E7B4A6>
cv::Mat objInvImg = cv::Mat::ones(rgnH + 2, rgnW + 2, CV_8UC1); //<2F><><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD>֤8<D6A4><38><EFBFBD>Ӵ<EFBFBD><D3B4><EFBFBD>ʱ<EFBFBD>ܿ<EFBFBD><DCBF>߽紦<DFBD><E7B4A6>
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;
}
}
}
//<2F><>Ѱ<EFBFBD>ұ߽<D2B1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>еĿ׶<C4BF>
//<2F><>objInvImg<6D><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>ע
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;
//<2F><><EFBFBD><EFBFBD>
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
//<2F><>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
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; //<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
}
}
}
std::vector<SSG_RgnContour2D> contours;
_rgnContourTracking(contourImg, contours);
//<2F><><EFBFBD>ɱ߽<C9B1><DFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>3D<33>㣩
for (int i = 0; i < contours.size(); i++)
{
//<2F><>դ<EFBFBD><D5A4><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{
///ͨ<><CDA8><EFBFBD><EFBFBD>ֵ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϿ<D0B6><CFBF>ܵİ<DCB5><C4B0><EFBFBD>
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);
//<2F><>ֵ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EEB4A6><EFBFBD><EFBFBD>Ѱ<EFBFBD>Ұ<EFBFBD><D2B0><EFBFBD>
}
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>
}
/// @brief
/// <20><>ȡ<EFBFBD>ͻ<EFBFBD>שץȡ<D7A5><C8A1>̬
/// <20><><EFBFBD><EFBFBD>ʽ<EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>Ϊɨ<CEAA><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݺ<EFBFBD><DDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void CSGFireBrick::sgGetBrickPose(SVzNL3DLaserLine* scanData, int nLines, std::vector<SSG_6AxisAttitude>& brickPoses, int* errCode, SVzNL3DLaserLine* resultData)
{
*errCode = 0;
if (m_nFrameHeight < 10) //ɨ<><C9A8><EFBFBD>߹<EFBFBD><DFB9><EFBFBD>
{
*errCode = SG_ERR_3D_DATA_INVLD;
return;
}
//<2F>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>: (1)У<><D0A3><EFBFBD><EFBFBD>2<EFBFBD><32>Z<EFBFBD><5A><EFBFBD><EFBFBD>ͳ<EFBFBD><CDB3> <20><>3<EFBFBD><33>ȡZ<C8A1><5A>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD><CEAA><EFBFBD>棨4<E6A3A8><34><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>зָ<D0B7>
//(1)<29><>(2)<29>Ѿ<EFBFBD><D1BE><EFBFBD>sgScanLineProc<6F><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<int> sumHist;
const int sumWin = 5;
for (int i = m_zIdxRange.nMin; i <= m_zIdxRange.nMax; i++)
{
//<2F><><EFBFBD><EFBFBD>
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);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>Ϊ<EFBFBD>ͻ<EFBFBD>ש<EFBFBD><D7A9><EFBFBD><EFBFBD><EEB6A5>
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;
}
//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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) //<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>봦<EFBFBD><EBB4A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵĺ<DCB5><C4BA><EFBFBD>֮һ<D6AE>ǵ<EFBFBD><C7B5><EFBFBD>
{
//<2F><>ע<EFBFBD><D7A2><EFBFBD><EFBFBD>
cv::Mat labImg = cv::Mat::zeros(m_nFrameHeight, m_nFrameWidth, CV_32SC1);//rows, cols,
std::vector<SSG_Region>labelRgns;
SG_TwoPassLabel(bwImg, labImg, labelRgns);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>ȡһ<C8A1><D2BB>Ŀ<EFBFBD><C4BF>
SSG_Region* objRgn = _getSortedObj(labelRgns, m_sortingMode, errCode);
if (*errCode != 0)
return;
//<2F><><EFBFBD>о<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>߶α߽<CEB1><DFBD>㣬<EFBFBD><E3A3AC>һ<EFBFBD><D2BB><EFBFBD>ָ<EFBFBD>
_rgnLineSegmentation();
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD>ǵ<EFBFBD>
std::vector<SSG_RgnContour3D> contours3D;
_getRgnContourData(scanData, labImg, objRgn, contours3D);
#if ENABLE_OUTPUT_DEBUG_IMAGE
//<2F><><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>3D<33><44>
#endif
_trackingRgnContour();
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǵ㣬ȷ<E3A3AC><C8B7><EFBFBD>Ƿ<EFBFBD>Ҫϸ<D2AA><CFB8>
}
}
void CSGFireBrick::sgSortBrickPoses(std::vector<SSG_6AxisAttitude>& brickPoses, ESG_poseSortingMode sortingMode)
{
}
/// @brief
/// <20><>ʼ<EFBFBD><CABC>
bool CSGFireBrick::_Init(double dHistScale)
{
m_histScale = dHistScale;
return true;
}
bool SGCreateFireBrick(double dHistScale, ISGFireBrick** ppFireBrick)
{
return CSGFireBrick::CreateInstance(dHistScale, ppFireBrick);
}
Reference in New Issue Copy Permalink