437 lines
15 KiB
C++
437 lines
15 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "boxCarMeasure_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
|
||
//扫描线处理，进行垂直方向的特征提取和生长
|
||
void sg_boxCarMeasure_lineProc(SVzNL3DLaserLine* a_line, int lineIdx, int* errCode, std::vector<SSG_lineFeature>& all_vLineFeatures, std::vector<std::vector<int>>& noisePts, const SG_boxCarMeasureParam measureParam)
|
||
{
|
||
SSG_lineFeature a_line_features;
|
||
a_line_features.lineIdx = lineIdx;
|
||
//滤波，滤除异常点
|
||
std::vector<SVzNL3DPosition> filterData;
|
||
std::vector<int> lineNoisePts;
|
||
sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, measureParam.filterParam, filterData, lineNoisePts);
|
||
noisePts.push_back(lineNoisePts);
|
||
|
||
sg_getLineRigthAngleFeature(
|
||
filterData.data(),
|
||
filterData.size(),
|
||
lineIdx,
|
||
measureParam.templatePara_HF,
|
||
measureParam.templatePara_FH,
|
||
measureParam.templatePara_HR,
|
||
measureParam.templatePara_RH,
|
||
&a_line_features);
|
||
//if ((a_line_features.features.size() > 0) || (a_line_features.endings.size() > 0))
|
||
all_vLineFeatures.push_back(a_line_features); //空行也加入，保证能按行号索引
|
||
return;
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
SSG_featureTree* hTree;
|
||
SSG_featureTree* head_vTree;
|
||
SSG_featureTree* tail_vTree;
|
||
int head_vTree_linkType; //1-head, 2-tail
|
||
int tail_vTree_linkType; //1-head, 2-tail
|
||
}boxCarEdge;
|
||
|
||
SSG_featureTree* _findNeighbouringTree(
|
||
SVzNL3DPoint seedPt,
|
||
std::vector<SSG_featureTree>&trees,
|
||
double neighbourDistTh,
|
||
double lenTh,
|
||
int* vTree_linkType)
|
||
{
|
||
for (int i = 0, i_max = trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_tree = &trees[i];
|
||
SVzNL3DPoint headPt = a_tree->treeNodes[0].jumpPos;
|
||
SVzNL3DPoint tailPt = a_tree->treeNodes.back().jumpPos;
|
||
double treeLen = sqrt(pow(tailPt.x - headPt.x, 2) +
|
||
pow(tailPt.y - headPt.y, 2) +
|
||
pow(tailPt.z - headPt.z, 2));
|
||
if (treeLen > lenTh)
|
||
{
|
||
double dist_head = sqrt(pow(seedPt.x - headPt.x, 2) +
|
||
pow(seedPt.y - headPt.y, 2) +
|
||
pow(seedPt.z - headPt.z, 2));
|
||
double dist_tail = sqrt(pow(seedPt.x - tailPt.x, 2) +
|
||
pow(seedPt.y - tailPt.y, 2) +
|
||
pow(seedPt.z - tailPt.z, 2));
|
||
if (dist_head < neighbourDistTh)
|
||
{
|
||
*vTree_linkType = 1; //head link
|
||
return a_tree;
|
||
}
|
||
else if (dist_tail < neighbourDistTh)
|
||
{
|
||
*vTree_linkType = 2; //tail link
|
||
return a_tree;
|
||
}
|
||
}
|
||
}
|
||
*vTree_linkType = 0;
|
||
return NULL;
|
||
}
|
||
|
||
void _getBoxCarDimensionInfo(boxCarEdge* carEdges, SSG_boxCarDimension* dimen)
|
||
{
|
||
if ((carEdges->head_vTree_linkType == 0) || (carEdges->tail_vTree_linkType == 0))
|
||
return;
|
||
|
||
//计算端点
|
||
SVzNL3DPoint hTree_firstNode = carEdges->hTree->treeNodes[0].jumpPos;
|
||
SVzNL3DPoint htree_head = { hTree_firstNode.y, hTree_firstNode.x, hTree_firstNode.z }; //htree的坐标与vtree坐标的xy是相反的
|
||
SVzNL3DPoint hTree_lastNode = carEdges->hTree->treeNodes.back().jumpPos;
|
||
SVzNL3DPoint htree_tail = { hTree_lastNode.y, hTree_lastNode.x, hTree_lastNode.z }; //htree的坐标与vtree坐标的xy是相反的
|
||
|
||
SVzNL3DPoint link_ending_0, link_ending_1;
|
||
SVzNL3DPoint endings[4];
|
||
if (carEdges->head_vTree_linkType == 1) //head link
|
||
{
|
||
link_ending_0 = carEdges->head_vTree->treeNodes[0].jumpPos;
|
||
endings[0] = carEdges->head_vTree->treeNodes.back().jumpPos;
|
||
}
|
||
else //tail link
|
||
{
|
||
link_ending_0 = carEdges->head_vTree->treeNodes.back().jumpPos;
|
||
endings[0] = carEdges->head_vTree->treeNodes[0].jumpPos;
|
||
}
|
||
|
||
if (carEdges->tail_vTree_linkType == 1) //head link
|
||
{
|
||
link_ending_1 = carEdges->tail_vTree->treeNodes[0].jumpPos;
|
||
endings[3] = carEdges->tail_vTree->treeNodes.back().jumpPos;
|
||
}
|
||
else //tail link
|
||
{
|
||
link_ending_1 = carEdges->tail_vTree->treeNodes.back().jumpPos;
|
||
endings[3] = carEdges->tail_vTree->treeNodes[0].jumpPos;
|
||
}
|
||
|
||
dimen->endings[0] = endings[0];
|
||
dimen->endings[1] = { htree_head.x, link_ending_0.y, (htree_head.z + link_ending_0.z) / 2 };
|
||
dimen->endings[2] = { htree_tail.x, link_ending_1.y, (htree_tail.z + link_ending_1.z) / 2 };
|
||
dimen->endings[3] = endings[3];
|
||
|
||
double len_1 = sqrt(pow(dimen->endings[0].x - dimen->endings[1].x, 2) + pow(dimen->endings[0].y - dimen->endings[1].y, 2));
|
||
double len_2 = sqrt(pow(dimen->endings[2].x - dimen->endings[3].x, 2) + pow(dimen->endings[2].y - dimen->endings[3].y, 2));
|
||
double w = sqrt(pow(dimen->endings[1].x - dimen->endings[2].x, 2) + pow(dimen->endings[1].y - dimen->endings[2].y, 2));
|
||
dimen->L = len_1 < len_2 ? len_2 : len_1;
|
||
dimen->W = w;
|
||
double angle = atan((dimen->endings[0].y - dimen->endings[1].y) /(dimen->endings[0].x - dimen->endings[1].x));
|
||
dimen->angle = angle * 180.0 / PI;
|
||
return;
|
||
}
|
||
|
||
///数据输入必须是grid格式，以进行水平方向和垂直方向的处理
|
||
///（1）寻找直角点
|
||
///（2）对LINE_FEATURE_RIGHT_ANGLE_HF和LINE_FEATURE_RIGHT_ANGLE_RH两种直角点进行生长
|
||
///（3) 确定尺寸
|
||
SSG_boxCarDimension sg_getBoxCarDimension(
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
int lineNum,
|
||
std::vector<SSG_lineFeature>& all_vLineFeatures,
|
||
std::vector<std::vector<int>>& noisePts,
|
||
const SG_boxCarMeasureParam measureParam)
|
||
{
|
||
//噪声去除
|
||
for (int i = 0; i < noisePts.size(); i++)
|
||
{
|
||
std::vector<int>& lineNoise = noisePts[i];
|
||
for (int j = 0; j < lineNoise.size(); j++)
|
||
{
|
||
int ptIdx = lineNoise[j];
|
||
laser3DPoints[i].p3DPosition[ptIdx].pt3D.z = 0;
|
||
}
|
||
}
|
||
|
||
int hLineNum = laser3DPoints->nPositionCnt; //Grid格式，所有扫描线的点数是一样的
|
||
//生成水平扫描数据
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
||
hLines.resize(hLineNum);
|
||
for (int i = 0; i < hLineNum; i++)
|
||
hLines[i].resize(lineNum);
|
||
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < hLineNum; j++)
|
||
{
|
||
hLines[j][line] = laser3DPoints[line].p3DPosition[j];
|
||
hLines[j][line].pt3D.x = laser3DPoints[line].p3DPosition[j].pt3D.y;
|
||
hLines[j][line].pt3D.y = laser3DPoints[line].p3DPosition[j].pt3D.x;
|
||
}
|
||
}
|
||
|
||
std::vector<SSG_lineFeature> all_hLineFeatures;
|
||
std::vector<std::vector<int>> all_hLineNoises;
|
||
//逐行提取特征
|
||
for (int hLine = 0; hLine < hLineNum; hLine++)
|
||
{
|
||
if (hLine == 520)
|
||
int kkk = 1;
|
||
//滤波，滤除异常点
|
||
std::vector<SVzNL3DPosition> filterData;
|
||
std::vector<int> lineNoise;
|
||
sg_lineDataRemoveOutlier((
|
||
SVzNL3DPosition*)hLines[hLine].data(),
|
||
(int)hLines[hLine].size(),
|
||
measureParam.filterParam,
|
||
filterData,
|
||
lineNoise);
|
||
all_hLineNoises.push_back(lineNoise);
|
||
|
||
std::vector<SVzNL3DPosition> smoothData;
|
||
sg_lineDataSmoothing(filterData, 5, smoothData);
|
||
SSG_lineFeature a_hLine_featrues;
|
||
a_hLine_featrues.lineIdx = hLine;
|
||
sg_getLineRigthAngleFeature(
|
||
smoothData.data(),
|
||
smoothData.size(),
|
||
hLine,
|
||
measureParam.templatePara_HF,
|
||
measureParam.templatePara_FH,
|
||
measureParam.templatePara_HR,
|
||
measureParam.templatePara_RH,
|
||
&a_hLine_featrues);
|
||
//if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0))
|
||
all_hLineFeatures.push_back(a_hLine_featrues);//空行也加入，保证能按行号索引
|
||
}
|
||
|
||
//噪声去除
|
||
for (int i = 0; i < all_hLineNoises.size(); i++)
|
||
{
|
||
if (i == 105)
|
||
int kkk = 1;
|
||
std::vector<int>& lineNoise = all_hLineNoises[i];
|
||
for (int j = 0; j < lineNoise.size(); j++)
|
||
{
|
||
int lineIdx = lineNoise[j];
|
||
laser3DPoints[lineIdx].p3DPosition[i].pt3D.z = 0;
|
||
}
|
||
}
|
||
|
||
//迭代处理一次，防止由于行处理时smooth将一些点无效，面垂直处理的feature点正好落在这些无效点上
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
//逐行检查
|
||
if (i == 175)
|
||
int kkk = 1;
|
||
SSG_lineFeature& a_line = all_vLineFeatures[i];
|
||
if (a_line.features.size() > 0)
|
||
{
|
||
bool reDoFlag = false;
|
||
for (int j = 0, j_max = a_line.features.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D& a_feature = a_line.features[j];
|
||
if (laser3DPoints[a_feature.jumpPos2D.x].p3DPosition[a_feature.jumpPos2D.y].pt3D.z < 1e-4)
|
||
{
|
||
reDoFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (true == reDoFlag)
|
||
{
|
||
SSG_lineFeature a_line_features;
|
||
sg_getLineRigthAngleFeature(
|
||
laser3DPoints[i].p3DPosition,
|
||
laser3DPoints[i].nPositionCnt,
|
||
i,
|
||
measureParam.templatePara_HF,
|
||
measureParam.templatePara_FH,
|
||
measureParam.templatePara_HR,
|
||
measureParam.templatePara_RH,
|
||
&a_line_features);
|
||
all_vLineFeatures[i].features.clear();
|
||
all_vLineFeatures[i].features.insert(all_vLineFeatures[i].features.end(), a_line_features.features.begin(), a_line_features.features.end());
|
||
all_vLineFeatures[i].endings.clear();
|
||
all_vLineFeatures[i].endings.insert(all_vLineFeatures[i].endings.end(), a_line_features.endings.begin(), a_line_features.endings.end());
|
||
}
|
||
}
|
||
}
|
||
|
||
//垂直方向特征生长（相机扫描方向）
|
||
std::vector<SSG_featureTree> all_v_trees;
|
||
sg_getFeatureGrowingTrees(
|
||
all_vLineFeatures,
|
||
all_v_trees,
|
||
measureParam.growParam);
|
||
|
||
//水平方向特征生长
|
||
std::vector<SSG_featureTree> all_h_trees;
|
||
sg_getFeatureGrowingTrees(
|
||
all_hLineFeatures,
|
||
all_h_trees,
|
||
measureParam.growParam);
|
||
|
||
//过滤出合适的Tree:只有LINE_FEATURE_RIGHT_ANGLE_HF和LINE_FEATURE_RIGHT_ANGLE_RH的tree是符合要求的tree
|
||
std::vector<SSG_featureTree> vld_v_trees;
|
||
for (int i = 0, i_max = all_v_trees.size(); i < i_max; i++)
|
||
{
|
||
if ((LINE_FEATURE_RIGHT_ANGLE_HF == all_v_trees[i].treeType) ||
|
||
(LINE_FEATURE_RIGHT_ANGLE_RH == all_v_trees[i].treeType))
|
||
vld_v_trees.push_back(all_v_trees[i]);
|
||
}
|
||
std::vector<SSG_featureTree> vld_h_trees;
|
||
for (int i = 0, i_max = all_h_trees.size(); i < i_max; i++)
|
||
{
|
||
if ((LINE_FEATURE_RIGHT_ANGLE_HF == all_h_trees[i].treeType) ||
|
||
(LINE_FEATURE_RIGHT_ANGLE_RH == all_h_trees[i].treeType))
|
||
vld_h_trees.push_back(all_h_trees[i]);
|
||
}
|
||
|
||
//在搜索h_trees中搜索两端均与v_trees邻接的生长树
|
||
double neighbourDistTh = 400.0;
|
||
std::vector< boxCarEdge> validBoxCarEdges;
|
||
for (int i = 0, i_max = vld_h_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_hTree = &vld_h_trees[i];
|
||
//取出头尾，htree的坐标与vtree坐标的xy是相反的
|
||
SVzNL3DPoint hTree_firstNode = a_hTree->treeNodes[0].jumpPos;
|
||
SVzNL3DPoint htree_head = { hTree_firstNode.y, hTree_firstNode.x, hTree_firstNode.z }; //htree的坐标与vtree坐标的xy是相反的
|
||
SVzNL3DPoint hTree_lastNode = a_hTree->treeNodes.back().jumpPos;
|
||
SVzNL3DPoint htree_tail = { hTree_lastNode.y, hTree_lastNode.x, hTree_lastNode.z }; //htree的坐标与vtree坐标的xy是相反的
|
||
double hTreeLen = sqrt(pow(htree_tail.x - htree_head.x, 2) +
|
||
pow(htree_tail.y - htree_head.y, 2) +
|
||
pow(htree_tail.z - htree_head.z, 2));
|
||
int vTree_linkType_0 = 0;
|
||
SSG_featureTree* head_link = _findNeighbouringTree(htree_head, vld_v_trees, neighbourDistTh, hTreeLen, &vTree_linkType_0);
|
||
int vTree_linkType_1 = 0;
|
||
SSG_featureTree* tail_link = _findNeighbouringTree(htree_tail, vld_v_trees, neighbourDistTh, hTreeLen, &vTree_linkType_1);
|
||
if ((NULL == head_link) || (NULL == tail_link))
|
||
continue;
|
||
boxCarEdge car_edge = { a_hTree , head_link , tail_link, vTree_linkType_0, vTree_linkType_1 };
|
||
validBoxCarEdges.push_back(car_edge);
|
||
}
|
||
if (validBoxCarEdges.size() == 0)
|
||
{
|
||
SSG_boxCarDimension result_null;
|
||
memset(&result_null, 0, sizeof(SSG_boxCarDimension));
|
||
return result_null;
|
||
}
|
||
|
||
//取出最长的组合
|
||
int maxLen = -1;
|
||
int max_idx = 0;
|
||
for (int i = 0, i_max = validBoxCarEdges.size(); i < i_max; i++)
|
||
{
|
||
int sum_len = abs(validBoxCarEdges[i].hTree->eLineIdx - validBoxCarEdges[i].hTree->sLineIdx) +
|
||
abs(validBoxCarEdges[i].head_vTree->eLineIdx - validBoxCarEdges[i].head_vTree->sLineIdx) +
|
||
abs(validBoxCarEdges[i].tail_vTree->eLineIdx - validBoxCarEdges[i].tail_vTree->sLineIdx);
|
||
if (maxLen < 0)
|
||
{
|
||
maxLen = sum_len;
|
||
max_idx = i;
|
||
}
|
||
else if (maxLen < sum_len)
|
||
{
|
||
maxLen = sum_len;
|
||
max_idx = i;
|
||
}
|
||
}
|
||
|
||
//用于标注
|
||
std::vector<SSG_featureTree> v_trees;
|
||
std::vector<SSG_featureTree> h_trees;
|
||
#if 1
|
||
v_trees.push_back(*validBoxCarEdges[max_idx].head_vTree);
|
||
v_trees.push_back(*validBoxCarEdges[max_idx].tail_vTree);
|
||
h_trees.push_back(*validBoxCarEdges[max_idx].hTree);
|
||
#else
|
||
v_trees.insert(v_trees.end(), all_v_trees.begin(), all_v_trees.end());
|
||
h_trees.insert(h_trees.end(), all_h_trees.begin(), all_h_trees.end());
|
||
#endif
|
||
//获取车厢信息
|
||
SSG_boxCarDimension result_dimen;
|
||
memset(&result_dimen, 0, sizeof(SSG_boxCarDimension));
|
||
_getBoxCarDimensionInfo(&validBoxCarEdges[max_idx], &result_dimen);
|
||
std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
|
||
//标注:垂直
|
||
SSG_treeInfo a_nullTree;
|
||
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
||
allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
|
||
int hvTreeIdx = 1;
|
||
for (int i = 0, i_max = v_trees.size(); i < i_max; i++)
|
||
{
|
||
if (i == 22)
|
||
int kkk = 1;
|
||
SSG_featureTree* a_vTree = &v_trees[i];
|
||
|
||
//记录Tree的信息
|
||
SSG_treeInfo a_treeInfo;
|
||
a_treeInfo.vTreeFlag = 1;
|
||
a_treeInfo.treeIdx = hvTreeIdx;
|
||
a_treeInfo.treeType = a_vTree->treeType;
|
||
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
|
||
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
|
||
a_treeInfo.roi = a_vTree->roi;
|
||
allTreesInfo.push_back(a_treeInfo);
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = a_vTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
|
||
laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
|
||
laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
|
||
laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
|
||
}
|
||
hvTreeIdx++;
|
||
}
|
||
int hTreeStart = hvTreeIdx;
|
||
////标注:水平特征
|
||
for (int i = 0, i_max = h_trees.size(); i < i_max; i++)
|
||
{
|
||
if (i == 9)
|
||
int kkk = 1;
|
||
SSG_featureTree* a_hTree = &h_trees[i];
|
||
//记录Tree的信息
|
||
SSG_treeInfo a_treeInfo;
|
||
a_treeInfo.vTreeFlag = 0;
|
||
a_treeInfo.treeIdx = hvTreeIdx;
|
||
a_treeInfo.treeType = a_hTree->treeType;
|
||
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
|
||
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
|
||
a_treeInfo.roi.left = a_hTree->roi.top; //水平扫描xy是交换的
|
||
a_treeInfo.roi.right = a_hTree->roi.bottom;
|
||
a_treeInfo.roi.top = a_hTree->roi.left;
|
||
a_treeInfo.roi.bottom = a_hTree->roi.right;
|
||
allTreesInfo.push_back(a_treeInfo);
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = a_hTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
|
||
laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
|
||
laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
|
||
laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
|
||
}
|
||
hvTreeIdx++;
|
||
}
|
||
int hvTreeSize = hvTreeIdx;
|
||
|
||
//统计VTree中LINE_FEATURE_RIGHT_ANGLE_HF和LINE_FEATURE_RIGHT_ANGLE_RH的数量
|
||
std::vector<int> vscan_HF_trees;
|
||
std::vector<int> vscan_RH_trees;
|
||
for (int i = 0; i < v_trees.size(); i++)
|
||
{
|
||
if (LINE_FEATURE_RIGHT_ANGLE_HF == v_trees[i].treeType)
|
||
vscan_HF_trees.push_back(i);
|
||
else if (LINE_FEATURE_RIGHT_ANGLE_RH == v_trees[i].treeType)
|
||
vscan_RH_trees.push_back(i);
|
||
}
|
||
//统计HTree中LINE_FEATURE_RIGHT_ANGLE_RH的数量
|
||
std::vector<int> hscan_HF_trees;
|
||
std::vector<int> hscan_RH_trees;
|
||
for (int i = 0; i < h_trees.size(); i++)
|
||
{
|
||
if (LINE_FEATURE_RIGHT_ANGLE_HF == h_trees[i].treeType)
|
||
hscan_HF_trees.push_back(i);
|
||
else if (LINE_FEATURE_RIGHT_ANGLE_RH == h_trees[i].treeType)
|
||
hscan_RH_trees.push_back(i);
|
||
}
|
||
|
||
return result_dimen;
|
||
}
|
||
|