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();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>תʱ <20><> > 0 <20><>˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD>תʱ <20><> < 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:<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,
|
|||
|
|
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();
|
|||
|
|
|
|||
|
|
//<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 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; //ת<>帴<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<SSG_basicFeatureGap>> gapFeatures;
|
|||
|
|
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<SSG_basicFeatureGap> line_gaps;
|
|||
|
|
int dataSize = (int)lineData.size();
|
|||
|
|
//<2F><>ȡGap<61><70><EFBFBD><EFBFBD>
|
|||
|
|
sg_getLineGapFeature(
|
|||
|
|
lineData, //ɨ<><C9A8><EFBFBD><EFBFBD>
|
|||
|
|
line, //<2F><>ǰɨ<C7B0><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
cornerPara,
|
|||
|
|
channelParam.channelWidthRng,
|
|||
|
|
line_gaps);
|
|||
|
|
gapFeatures.push_back(line_gaps);
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
//<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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|