290 lines
7.3 KiB
C++
290 lines
7.3 KiB
C++
#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:激光线平行槽道；false:激光线垂直槽道
|
||
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();
|
||
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*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; //转义复用
|
||
}
|
||
}
|
||
}
|
||
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; //将原始数据的序列清0（会转义使用）
|
||
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];
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
std::vector<SWD_segFeature> line_ringArcs;
|
||
int dataSize = (int)lineData.size();
|
||
//提取Arc特征
|
||
wd_getRingArcFeature(
|
||
lineData,
|
||
line, //当前扫描线序号
|
||
cornerPara,
|
||
rodRidius / 2, //环宽度
|
||
line_ringArcs //环
|
||
);
|
||
arcFeatures.push_back(line_ringArcs);
|
||
}
|
||
//特征生长
|
||
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++)
|
||
{
|
||
//空间直线拟合
|
||
|
||
//投影
|
||
|
||
//
|
||
}
|
||
|
||
|
||
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
|
||
|
||
//按扫描线配对
|
||
std::vector<SSG_basicFeatureGap> line_1; //line_1和line_2为按扫描线对应的两个槽道上的Gap特征
|
||
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
|
||
|
||
//旋转，保证扫描线与Gap垂直
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
|