372 lines
9.8 KiB
C++
372 lines
9.8 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "channelSpaceMeasure_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_ChannelSPaceMeasureVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
|
||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||
SVzNL3DPoint _rotate2D(SVzNL3DPoint pt, double sinTheta, double cosTheta)
|
||
{
|
||
SVzNL3DPoint rotatePt;
|
||
rotatePt.x = pt.x * cosTheta - pt.y * sinTheta;
|
||
rotatePt.y = pt.x * sinTheta + pt.y * cosTheta;
|
||
rotatePt.z = pt.z;
|
||
return rotatePt;
|
||
}
|
||
|
||
void _XY_rotateLine(double angle, std::vector<SSG_basicFeatureGap>& line_src, std::vector<SSG_basicFeatureGap>& rotate_dst)
|
||
{
|
||
rotate_dst.resize(line_src.size());
|
||
double sinTheta = sin(PI * angle / 180);
|
||
double cosTheta = cos(PI * angle / 180);
|
||
for (int i = 0, i_max = (int)line_src.size(); i < i_max; i++)
|
||
{
|
||
rotate_dst[i] = line_src[i];
|
||
rotate_dst[i].gapPt_0.pt3D = _rotate2D(line_src[i].gapPt_0.pt3D, sinTheta, cosTheta);
|
||
rotate_dst[i].gapPt_1.pt3D = _rotate2D(line_src[i].gapPt_1.pt3D, sinTheta, cosTheta);
|
||
}
|
||
return;
|
||
}
|
||
|
||
double _computeChannelSpace(
|
||
std::vector<SSG_basicFeatureGap>& line_1,
|
||
std::vector<SSG_basicFeatureGap>& line_2)
|
||
{
|
||
double meanSpace = 0;
|
||
int dataSize = (int)line_1.size();
|
||
if (dataSize == 0)
|
||
return 0;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
double cy_1 = (line_1[i].gapPt_0.pt3D.y + line_1[i].gapPt_1.pt3D.y) / 2;
|
||
double cy_2 = (line_2[i].gapPt_0.pt3D.y + line_2[i].gapPt_1.pt3D.y) / 2;
|
||
meanSpace += abs(cy_2 - cy_1);
|
||
}
|
||
meanSpace = meanSpace / dataSize;
|
||
return meanSpace;
|
||
}
|
||
|
||
double _computeGapMeanWidth(std::vector<SSG_basicFeatureGap>& gap_data)
|
||
{
|
||
double meanWidth = 0;
|
||
int dataSize = (int)gap_data.size();
|
||
if (dataSize == 0)
|
||
return 0;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
double width = abs(gap_data[i].gapPt_0.pt3D.y - gap_data[i].gapPt_1.pt3D.y);
|
||
meanWidth += width;
|
||
}
|
||
meanWidth = meanWidth / dataSize;
|
||
return meanWidth;
|
||
}
|
||
|
||
double _computeGapMeanDepth(std::vector<SSG_basicFeatureGap>& gap_data, std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
double meanDepth = 0;
|
||
int dataSize = (int)gap_data.size();
|
||
if (dataSize == 0)
|
||
return 0;
|
||
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
int ptIdx_1 = gap_data[i].gapPt_0.nPointIdx;
|
||
int ptIdx_2 = gap_data[i].gapPt_1.nPointIdx;
|
||
int line = gap_data[i].lineIdx;
|
||
double max_z = 0;
|
||
for (int j = ptIdx_1; j <= ptIdx_2; j++)
|
||
{
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
if (max_z < scanLines[line][j].pt3D.z)
|
||
max_z = scanLines[line][j].pt3D.z;
|
||
}
|
||
}
|
||
double depth = max_z - (gap_data[i].gapPt_0.pt3D.z + gap_data[i].gapPt_1.pt3D.z) / 2;
|
||
meanDepth += depth;
|
||
}
|
||
meanDepth = meanDepth / dataSize;
|
||
return meanDepth;
|
||
}
|
||
|
||
SSX_channelInfo sx_channelSpaceMeasure(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const SSX_channelParam channelParam,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSX_channelInfo channelInfo;
|
||
memset(&channelInfo, 0, sizeof(SSX_channelInfo));
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return channelInfo;
|
||
}
|
||
|
||
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 channelInfo;
|
||
}
|
||
|
||
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<SSG_basicFeatureGap>> gapFeatures;
|
||
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<SSG_basicFeatureGap> line_gaps;
|
||
int dataSize = (int)lineData.size();
|
||
//提取Gap特征
|
||
sg_getLineGapFeature(
|
||
lineData, //扫描线
|
||
line, //当前扫描线序号
|
||
cornerPara,
|
||
channelParam.channelWidthRng,
|
||
line_gaps);
|
||
gapFeatures.push_back(line_gaps);
|
||
}
|
||
//特征生长
|
||
std::vector<SSG_gapFeatureTree> growTrees;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
bool isLastLine = false;
|
||
if (line == lineNum - 1)
|
||
isLastLine = true;
|
||
std::vector<SSG_basicFeatureGap>& line_gapFeatures = gapFeatures[line];
|
||
|
||
sg_lineGapsGrowing(
|
||
line,
|
||
isLastLine,
|
||
line_gapFeatures,
|
||
growTrees,
|
||
growParam);
|
||
}
|
||
if (growTrees.size() != 2)
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return channelInfo;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
|