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

290 lines
7.3 KiB
C++
Raw Normal View History

rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "rodAndBarDetection_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//version 1.0.0 : base version release to customer
std::string m_strVersion = "1.0.0";
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
}
void sx_hexHeadScrewMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
bool isHorizonScan, //true:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>в۵<D0B2><DBB5><EFBFBD>false:<3A><><EFBFBD><EFBFBD><EFBFBD>ߴ<EFBFBD>ֱ<EFBFBD>۵<EFBFBD>
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
double rodRidius,
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
int* errCode)
{
*errCode = 0;
SSX_hexHeadScrewInfo screwInfo;
memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo));
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
int linePtNum = (int)scanLines[0].size();
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
std::vector< std::vector<SVzNL3DPosition>> data_lines;
if (false == isHorizonScan)
{
data_lines.resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
{
data_lines[line][j].nPointIdx = j;
scanLines[line][j].nPointIdx = 0; //ת<>帴<EFBFBD><E5B8B4>
}
}
}
else
{
data_lines.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
data_lines[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
data_lines[j][line] = scanLines[line][j];
data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
lineNum = linePtNum;
linePtNum = (int)data_lines[0].size();
for (int line = 0; line < lineNum; line++)
{
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
data_lines[line][j].nPointIdx = j;
}
}
std::vector<std::vector<SWD_segFeature>> arcFeatures;
for (int line = 0; line < lineNum; line++)
{
if (line == 44)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SWD_segFeature> line_ringArcs;
int dataSize = (int)lineData.size();
//<2F><>ȡArc<72><63><EFBFBD><EFBFBD>
wd_getRingArcFeature(
lineData,
line, //<2F><>ǰɨ<C7B0><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cornerPara,
rodRidius / 2, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
line_ringArcs //<2F><>
);
arcFeatures.push_back(line_ringArcs);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SWD_segFeatureTree> growTrees;
wd_getSegFeatureGrowingTrees(
arcFeatures,
growTrees,
growParam);
if (growTrees.size() == 0)
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
int objNum = (int)growTrees.size();
for (int i = 0; i < objNum; i++)
{
//<2F>ռ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//ͶӰ
//
}
std::vector<SSG_basicFeatureGap>& nodes_1 = growTrees[0].treeNodes;
std::vector<SSG_basicFeatureGap>& nodes_2 = growTrees[1].treeNodes;
#if _OUTPUT_DEBUG_DATA
for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = nodes_1[i].lineIdx;
ptIdx = nodes_1[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 1;
ptIdx = nodes_1[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 2;
}
else
{
ptIdx = nodes_1[i].lineIdx;
lineIdx = nodes_1[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 1;
lineIdx = nodes_1[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 2;
}
}
for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = nodes_2[i].lineIdx;
ptIdx = nodes_2[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 1;
ptIdx = nodes_2[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 2;
}
else
{
ptIdx = nodes_2[i].lineIdx;
lineIdx = nodes_2[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 1;
lineIdx = nodes_2[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 2;
}
}
#endif
//<2F><>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_basicFeatureGap> line_1; //line_1<5F><31>line_2Ϊ<32><CEAA>ɨ<EFBFBD><C9A8><EFBFBD>߶<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۵<EFBFBD><DBB5>ϵ<EFBFBD>Gap<61><70><EFBFBD><EFBFBD>
std::vector<SSG_basicFeatureGap> line_2;
int i_idx = 0;
int j_idx = 0;
while (1)
{
int line_idx1 = nodes_1[i_idx].lineIdx;
int line_idx2 = nodes_2[j_idx].lineIdx;
if (line_idx1 == line_idx2)
{
line_1.push_back(nodes_1[i_idx]);
line_2.push_back(nodes_2[j_idx]);
i_idx++;
j_idx++;
}
else
{
if (line_idx1 < line_idx2)
i_idx++;
else
j_idx++;
}
if ((i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size()))
break;
}
#if _OUTPUT_DEBUG_DATA
for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = line_1[i].lineIdx;
ptIdx = line_1[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 3;
ptIdx = line_1[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 4;
}
else
{
ptIdx = line_1[i].lineIdx;
lineIdx = line_1[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 3;
lineIdx = line_1[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 4;
}
}
for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = line_2[i].lineIdx;
ptIdx = line_2[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 3;
ptIdx = line_2[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 4;
}
else
{
ptIdx = line_2[i].lineIdx;
lineIdx = line_2[i].gapPt_0.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 3;
lineIdx = line_2[i].gapPt_1.nPointIdx;
scanLines[lineIdx][ptIdx].nPointIdx = 4;
}
}
#endif
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>֤ɨ<D6A4><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Gap<61><70>ֱ
double angleSearchWin = 30;
double angleStepping = 0.1;
int loop = (int)(angleSearchWin / angleStepping);
double bestSpace = 0;
double rotateAngle = 0;
for (int i = -loop; i <= loop; i++)
{
double angle = i * angleStepping;
std::vector<SSG_basicFeatureGap> rotate_line_1;
_XY_rotateLine(angle, line_1, rotate_line_1);
std::vector<SSG_basicFeatureGap> rotate_line_2;
_XY_rotateLine(angle, line_2, rotate_line_2);
double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2);
if (bestSpace < meanSpace)
{
bestSpace = meanSpace;
rotateAngle = angle;
}
}
std::vector<SSG_basicFeatureGap> calib_line_1;
_XY_rotateLine(rotateAngle, line_1, calib_line_1);
std::vector<SSG_basicFeatureGap> calib_line_2;
_XY_rotateLine(rotateAngle, line_2, calib_line_2);
channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2);
channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1);
channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2);
channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines);
channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines);
return channelInfo;
}
Reference in New Issue Copy Permalink