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>
|
|||
|
|
|
|||
|
|
//ɨ<><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
|
|||
|
|
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); //<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD>
|
|||
|
|
SVzNL3DPoint hTree_firstNode = carEdges->hTree->treeNodes[0].jumpPos;
|
|||
|
|
SVzNL3DPoint htree_head = { hTree_firstNode.y, hTree_firstNode.x, hTree_firstNode.z }; //htree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>vtree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>xy<78><79><EFBFBD>෴<EFBFBD><E0B7B4>
|
|||
|
|
SVzNL3DPoint hTree_lastNode = carEdges->hTree->treeNodes.back().jumpPos;
|
|||
|
|
SVzNL3DPoint htree_tail = { hTree_lastNode.y, hTree_lastNode.x, hTree_lastNode.z }; //htree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>vtree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>xy<78><79><EFBFBD>෴<EFBFBD><E0B7B4>
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>grid<69><64>ʽ<EFBFBD><CABD><EFBFBD>Խ<EFBFBD><D4BD><EFBFBD>ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD>
|
|||
|
|
///<2F><>1<EFBFBD><31>Ѱ<EFBFBD><D1B0>ֱ<EFBFBD>ǵ<EFBFBD>
|
|||
|
|
///<2F><>2<EFBFBD><32><EFBFBD><EFBFBD>LINE_FEATURE_RIGHT_ANGLE_HF<48><46>LINE_FEATURE_RIGHT_ANGLE_RH<52><48><EFBFBD><EFBFBD>ֱ<EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
///<2F><>3) ȷ<><C8B7><EFBFBD>ߴ<EFBFBD>
|
|||
|
|
SSG_boxCarDimension sg_getBoxCarDimension(
|
|||
|
|
SVzNL3DLaserLine* laser3DPoints,
|
|||
|
|
int lineNum,
|
|||
|
|
std::vector<SSG_lineFeature>& all_vLineFeatures,
|
|||
|
|
std::vector<std::vector<int>>& noisePts,
|
|||
|
|
const SG_boxCarMeasureParam measureParam)
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5>
|
|||
|
|
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<69><64>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>ߵĵ<DFB5><C4B5><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
|
|||
|
|
for (int hLine = 0; hLine < hLineNum; hLine++)
|
|||
|
|
{
|
|||
|
|
if (hLine == 520)
|
|||
|
|
int kkk = 1;
|
|||
|
|
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
|
|||
|
|
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);//<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5>
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Σ<EFBFBD><CEA3><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>ʱsmooth<74><68>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>洹ֱ<E6B4B9><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>feature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Щ<EFBFBD><D0A9>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
|
|||
|
|
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());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD>跽<EFBFBD><E8B7BD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SSG_featureTree> all_v_trees;
|
|||
|
|
sg_getFeatureGrowingTrees(
|
|||
|
|
all_vLineFeatures,
|
|||
|
|
all_v_trees,
|
|||
|
|
measureParam.growParam);
|
|||
|
|
|
|||
|
|
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SSG_featureTree> all_h_trees;
|
|||
|
|
sg_getFeatureGrowingTrees(
|
|||
|
|
all_hLineFeatures,
|
|||
|
|
all_h_trees,
|
|||
|
|
measureParam.growParam);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ʵ<EFBFBD>Tree:ֻ<><D6BB>LINE_FEATURE_RIGHT_ANGLE_HF<48><46>LINE_FEATURE_RIGHT_ANGLE_RH<52><48>tree<65>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>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]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>h_trees<65><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˾<EFBFBD><CBBE><EFBFBD>v_trees<65>ڽӵ<DABD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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];
|
|||
|
|
//ȡ<><C8A1>ͷβ<CDB7><CEB2>htree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>vtree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>xy<78><79><EFBFBD>෴<EFBFBD><E0B7B4>
|
|||
|
|
SVzNL3DPoint hTree_firstNode = a_hTree->treeNodes[0].jumpPos;
|
|||
|
|
SVzNL3DPoint htree_head = { hTree_firstNode.y, hTree_firstNode.x, hTree_firstNode.z }; //htree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>vtree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>xy<78><79><EFBFBD>෴<EFBFBD><E0B7B4>
|
|||
|
|
SVzNL3DPoint hTree_lastNode = a_hTree->treeNodes.back().jumpPos;
|
|||
|
|
SVzNL3DPoint htree_tail = { hTree_lastNode.y, hTree_lastNode.x, hTree_lastNode.z }; //htree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>vtree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>xy<78><79><EFBFBD>෴<EFBFBD><E0B7B4>
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ȡ<><C8A1><EFBFBD><EFBFBD><EEB3A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڱ<EFBFBD>ע
|
|||
|
|
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
|
|||
|
|
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
SSG_boxCarDimension result_dimen;
|
|||
|
|
memset(&result_dimen, 0, sizeof(SSG_boxCarDimension));
|
|||
|
|
_getBoxCarDimensionInfo(&validBoxCarEdges[max_idx], &result_dimen);
|
|||
|
|
std::vector<SSG_treeInfo> allTreesInfo; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>
|
|||
|
|
//<2F><>ע:<3A><>ֱ
|
|||
|
|
SSG_treeInfo a_nullTree;
|
|||
|
|
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
|||
|
|
allTreesInfo.push_back(a_nullTree); //<2F><><EFBFBD>ִ洢λ<E6B4A2><CEBB><EFBFBD><EFBFBD>treeIdx<64><78>ͬλ<CDAC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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];
|
|||
|
|
|
|||
|
|
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
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);
|
|||
|
|
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
////<2F><>ע:ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
|
|||
|
|
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];
|
|||
|
|
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
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; //ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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);
|
|||
|
|
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
//ͳ<><CDB3>VTree<65><65>LINE_FEATURE_RIGHT_ANGLE_HF<48><46>LINE_FEATURE_RIGHT_ANGLE_RH<52><48><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
//ͳ<><CDB3>HTree<65><65>LINE_FEATURE_RIGHT_ANGLE_RH<52><48><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|