1264 lines
40 KiB
C++
1264 lines
40 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
|
||
bool _checkTypeMatch(int treeType, int featureType)
|
||
{
|
||
bool isMatch = false;
|
||
switch (treeType)
|
||
{
|
||
case LINE_FEATURE_L_JUMP_H2L:
|
||
if ((LINE_FEATURE_L_JUMP_H2L == featureType) || (LINE_FEATURE_L_SLOPE_H2L == featureType))
|
||
isMatch = true;
|
||
break;
|
||
case LINE_FEATURE_L_JUMP_L2H:
|
||
if ((LINE_FEATURE_L_JUMP_L2H == featureType) || (LINE_FEATURE_L_SLOPE_L2H == featureType))
|
||
isMatch = true;
|
||
break;
|
||
case LINE_FEATURE_L_SLOPE_H2L:
|
||
if ((LINE_FEATURE_L_SLOPE_H2L == featureType) || (LINE_FEATURE_L_JUMP_H2L == featureType))
|
||
isMatch = true;
|
||
break;
|
||
case LINE_FEATURE_L_SLOPE_L2H:
|
||
if ((LINE_FEATURE_L_SLOPE_L2H == featureType) || (LINE_FEATURE_L_JUMP_L2H == featureType))
|
||
isMatch = true;
|
||
break;
|
||
case LINE_FEATURE_V_SLOPE:
|
||
if ((LINE_FEATURE_V_SLOPE == featureType) ||
|
||
(LINE_FEATURE_L_SLOPE_L2H == featureType) ||
|
||
(LINE_FEATURE_L_SLOPE_H2L == featureType))
|
||
isMatch = true;
|
||
break;
|
||
case LINE_FEATURE_RIGHT_ANGLE_HR:
|
||
case LINE_FEATURE_RIGHT_ANGLE_HF:
|
||
case LINE_FEATURE_RIGHT_ANGLE_RH:
|
||
case LINE_FEATURE_RIGHT_ANGLE_FH:
|
||
case LINE_FEATURE_PEAK_TOP:
|
||
case LINE_FEATURE_PEAK_BOTTOM:
|
||
case LINE_FEATURE_CORNER_V:
|
||
case LINE_FEATURE_LINE_ENDING_0:
|
||
case LINE_FEATURE_LINE_ENDING_1:
|
||
if (featureType == treeType)
|
||
isMatch = true;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
return isMatch;
|
||
}
|
||
|
||
#define TREE_STATE_UNDEF 0
|
||
#define TREE_STATE_ALIVE 1
|
||
#define TREE_STATE_DEAD 2
|
||
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
|
||
bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector<SSG_featureTree>& trees, SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree& a_tree = trees[i];
|
||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||
continue;
|
||
//检查生长点
|
||
SSG_basicFeature1D last_node = a_tree.treeNodes.back();
|
||
if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //x为lineIdx，同一条扫描线上的不进行生长
|
||
continue;
|
||
|
||
//判断生长点
|
||
double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
|
||
double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
|
||
int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
|
||
double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
|
||
bool typeMatch = _checkTypeMatch(a_tree.treeType, a_feature.featureType);
|
||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||
( (line_diff < growParam.maxLineSkipNum)||(x_diff < growParam.maxSkipDistance)) && (true == typeMatch))
|
||
{
|
||
a_tree.eLineIdx = lineIdx;
|
||
a_tree.treeNodes.push_back(a_feature);
|
||
a_tree.tree_value += a_feature.featureValue;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
|
||
bool _featureGrowing_noTypeMatch(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector<SSG_featureTree>& trees, SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree& a_tree = trees[i];
|
||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||
continue;
|
||
//检查生长点
|
||
SSG_basicFeature1D last_node = a_tree.treeNodes.back();
|
||
if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //x为lineIdx，同一条扫描线上的不进行生长
|
||
continue;
|
||
|
||
//判断生长点
|
||
double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
|
||
double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
|
||
int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
|
||
double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
|
||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
|
||
{
|
||
a_tree.eLineIdx = lineIdx;
|
||
a_tree.treeNodes.push_back(a_feature);
|
||
a_tree.tree_value += a_feature.featureValue;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
//将segment feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||
bool _segFeatureGrowing(SWD_segFeature& a_feature, const int lineIdx, std::vector<SWD_segFeatureTree>& trees, SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = trees[i];
|
||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||
continue;
|
||
//检查生长点
|
||
SWD_segFeature last_node = a_tree.treeNodes.back();
|
||
if (last_node.lineIdx == a_feature.lineIdx) //x为lineIdx，同一条扫描线上的不进行生长
|
||
continue;
|
||
|
||
//判断生长点
|
||
double y_diff_1 = abs(a_feature.startPt.y - last_node.startPt.y);
|
||
double y_diff_2 = abs(a_feature.endPt.y - last_node.endPt.y);
|
||
double z_diff_1 = abs(a_feature.startPt.z - last_node.startPt.z);
|
||
double z_diff_2 = abs(a_feature.endPt.z - last_node.endPt.z);
|
||
int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
|
||
double x_diff = (abs(a_feature.startPt.x - last_node.startPt.x) + abs(a_feature.endPt.x - last_node.endPt.x))/2;
|
||
if ((y_diff_1 < growParam.yDeviation_max) && (y_diff_2 < growParam.yDeviation_max) &&
|
||
(z_diff_1 < growParam.zDeviation_max) && (z_diff_2 < growParam.zDeviation_max) &&
|
||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
|
||
{
|
||
a_tree.eLineIdx = lineIdx;
|
||
a_tree.treeNodes.push_back(a_feature);
|
||
a_tree.tree_value += (a_feature.startPt.z + a_feature.endPt.z)/2;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
int _getMatchedTree_angleCheck(
|
||
SSG_basicFeature1D& a_feature,
|
||
const int lineIdx,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam,
|
||
double angleCheckScale)
|
||
{
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree& a_tree = trees[i];
|
||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||
continue;
|
||
//检查生长点
|
||
SSG_basicFeature1D last_node = a_tree.treeNodes.back();
|
||
if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //x为lineIdx，同一条扫描线上的不进行生长
|
||
continue;
|
||
|
||
//判断生长点
|
||
double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
|
||
double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
|
||
int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
|
||
double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
|
||
bool typeMatch = _checkTypeMatch(a_tree.treeType, a_feature.featureType);
|
||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)) && (true == typeMatch))
|
||
{
|
||
int nodeSize = (int)a_tree.treeNodes.size();
|
||
bool isValid = true;
|
||
|
||
double dist = 0;
|
||
if (a_tree.angleChkScalePos < 0)
|
||
{
|
||
SSG_basicFeature1D& scale_node = a_tree.treeNodes[0];
|
||
double dist = sqrt(pow(a_feature.jumpPos.x - scale_node.jumpPos.x, 2) +
|
||
pow(a_feature.jumpPos.y - scale_node.jumpPos.y, 2));
|
||
if (dist >= angleCheckScale)
|
||
{
|
||
a_tree.angleChkScalePos = 0;
|
||
//计算角度
|
||
double tanValue = abs(a_feature.jumpPos.y - scale_node.jumpPos.y) /
|
||
abs(a_feature.jumpPos.x - scale_node.jumpPos.x);
|
||
if (tanValue > 1.0) //对应45度
|
||
isValid = false;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
for (int m = a_tree.angleChkScalePos+1; m < nodeSize; m++)
|
||
{
|
||
SSG_basicFeature1D& a_node = a_tree.treeNodes[m];
|
||
double dist = sqrt(pow(a_feature.jumpPos.x - a_node.jumpPos.x, 2) +
|
||
pow(a_feature.jumpPos.y - a_node.jumpPos.y, 2));
|
||
if (dist < angleCheckScale)
|
||
{
|
||
a_tree.angleChkScalePos = m - 1;
|
||
break;
|
||
}
|
||
}
|
||
SSG_basicFeature1D& scale_node = a_tree.treeNodes[a_tree.angleChkScalePos];
|
||
//计算角度
|
||
double tanValue = abs(a_feature.jumpPos.y - scale_node.jumpPos.y) /
|
||
abs(a_feature.jumpPos.x - scale_node.jumpPos.x);
|
||
if (tanValue > 1.0) //对应45度
|
||
isValid = false;
|
||
}
|
||
if(true == isValid)
|
||
return i;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
#if 0
|
||
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
|
||
bool _hFeatureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector<SSG_featureTree>& trees, SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree& a_tree = trees[i];
|
||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||
continue;
|
||
//检查生长点
|
||
SSG_basicFeature1D last_node = a_tree.treeNodes.back();
|
||
if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //x为lineIdx，同一条扫描线上的不进行生长
|
||
continue;
|
||
|
||
//判断生长点
|
||
double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
|
||
double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
|
||
double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
|
||
int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
|
||
bool typeMatch = _checkTypeMatch(a_tree.treeType, a_feature.featureType);
|
||
if ((x_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||
((line_diff < growParam.maxLineSkipNum) || (y_diff < growParam.maxSkipDistance)) && (true == typeMatch))
|
||
{
|
||
a_tree.eLineIdx = lineIdx;
|
||
a_tree.treeNodes.push_back(a_feature);
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
#endif
|
||
#if 0
|
||
bool compareByLen(const SSG_RUN& a, const SSG_RUN& b) {
|
||
return a.len < b.len;
|
||
}
|
||
|
||
bool _checkCombine(SSG_featureTree* a_tree, SSG_featureTree* chk_tree)
|
||
{
|
||
|
||
double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
|
||
double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
|
||
int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
|
||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||
(line_diff < growParam.maxLineSkipNum) && (a_feature.featureType == a_tree.treeType))
|
||
{
|
||
a_tree.treeNodes.push_back(a_feature);
|
||
return true;
|
||
}
|
||
}
|
||
|
||
void _growTreeCombine(std::vector<SSG_featureTree>& trees)
|
||
{
|
||
std::vector< SSG_RUN> treeInfo;
|
||
treeInfo.resize(trees.size());
|
||
for (int i = 0, i_max = trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_tree = &trees[i];
|
||
SSG_lineFeature* first_node = &(a_tree->treeNodes[0]);
|
||
SSG_lineFeature* last_node = &(a_tree->treeNodes[a_tree->treeNodes.size() - 1]);
|
||
int len = last_node->jumpPos2D.x - first_node->jumpPos2D.x;
|
||
SSG_RUN an_info = { i, len, a_tree->treeType };
|
||
treeInfo.push_back(an_info);
|
||
}
|
||
//从长到短排序
|
||
std::sort(treeInfo.begin(), treeInfo.end(), compareByLen);
|
||
//以长的准
|
||
int treeSize = treeInfo.size();
|
||
for (int i = 0; i < treeSize; i++)
|
||
{
|
||
SSG_RUN* an_info = &treeInfo[i];
|
||
if (an_info->value == 0)
|
||
continue;
|
||
|
||
SSG_featureTree* a_tree = &trees[an_info->start];
|
||
bool toChk = true;
|
||
while (toChk)
|
||
{
|
||
for (int j = i + 1; j < treeSize; j++)
|
||
{
|
||
SSG_featureTree* chk_tree = &trees[j];
|
||
if (chk_tree->treeType == 0)
|
||
continue;
|
||
bool vldToCombine = _checkCombine(a_tree, chk_tree);
|
||
}
|
||
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
//对于V型生长树，如果是单调上升或单调下降，且最高点没有继续生长的去除（褶皱)
|
||
bool _invalidateVSlopeTrees(SSG_featureTree& a_tree, double minLTypeTreeLen, double minVTypeTreeLen)
|
||
{
|
||
SSG_basicFeature1D* first_node = &(a_tree.treeNodes[0]);
|
||
SSG_basicFeature1D* last_node = &(a_tree.treeNodes[a_tree.treeNodes.size() - 1]);
|
||
double len = sqrt(pow(first_node->jumpPos.x - last_node->jumpPos.x, 2) + pow(first_node->jumpPos.y - last_node->jumpPos.y, 2));
|
||
if ( ((LINE_FEATURE_V_SLOPE!=a_tree.treeType)&&(len <= minLTypeTreeLen)) ||
|
||
((LINE_FEATURE_V_SLOPE == a_tree.treeType) && (len <= minVTypeTreeLen)))
|
||
return false;
|
||
else
|
||
{
|
||
if (LINE_FEATURE_V_SLOPE != a_tree.treeType)
|
||
return true;
|
||
else
|
||
{
|
||
//为了防止两端的点的扫描点的高度异常变化， 取中间段进行坡度计算
|
||
int size = (int)a_tree.treeNodes.size();
|
||
int pos_0 = size / 4;
|
||
int pos_1 = size * 3 / 4;
|
||
SSG_basicFeature1D* node_0 = &(a_tree.treeNodes[pos_0]);
|
||
SSG_basicFeature1D* node_1 = &(a_tree.treeNodes[pos_1]);
|
||
double node_len = sqrt(pow(node_0->jumpPos.x - node_1->jumpPos.x, 2) + pow(node_0->jumpPos.y - node_1->jumpPos.y, 2));
|
||
double height = abs(node_0->jumpPos.z - node_1->jumpPos.z);
|
||
double k = height / len;
|
||
if (k < 0.3) //大约为16度的坡度
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
bool _checkTwoFeatureVldToGrow(SSG_basicFeature1D& a_node, SSG_basicFeature1D* a_feature, SSG_treeGrowParam growParam)
|
||
{
|
||
//判断是否可以生长
|
||
double y_diff = abs(a_feature->jumpPos.y - a_node.jumpPos.y);
|
||
double z_diff = abs(a_feature->jumpPos.z - a_node.jumpPos.z);
|
||
int line_diff = abs(a_feature->jumpPos2D.x - a_node.jumpPos2D.x);
|
||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||
(line_diff < growParam.maxLineSkipNum))
|
||
{
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
void _growingFromHead(SSG_featureTree* a_tree, std::vector<SSG_lineFeature>& lineFeatures, SSG_treeGrowParam growParam)
|
||
{
|
||
SSG_basicFeature1D& head_node = a_tree->treeNodes[0];
|
||
int head_line = head_node.jumpPos2D.x;
|
||
for (int i = head_line-1; i >= 0; i--)
|
||
{
|
||
SSG_lineFeature* a_line = &lineFeatures[i];
|
||
int endingNum = (int)a_line->endings.size();
|
||
|
||
bool growFlag = false;
|
||
for (int m = 0; m < endingNum; m++)
|
||
{
|
||
bool vld_0 = _checkTwoFeatureVldToGrow(head_node, &a_line->endings[m], growParam);
|
||
if (true == vld_0)
|
||
{
|
||
a_line->endings[m].featureType = a_tree->treeType;
|
||
a_tree->treeNodes.insert(a_tree->treeNodes.begin(), a_line->endings[m]);
|
||
growFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (false == growFlag)
|
||
{
|
||
int line_diff = abs(a_line->lineIdx - a_tree->treeNodes[0].jumpPos2D.x);
|
||
if (line_diff > growParam.maxLineSkipNum)
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
void _growingFromTail(SSG_featureTree* a_tree, std::vector<SSG_lineFeature>& lineFeatures, SSG_treeGrowParam growParam)
|
||
{
|
||
SSG_basicFeature1D& tail_node = a_tree->treeNodes[a_tree->treeNodes.size()-1];
|
||
int tail_line = tail_node.jumpPos2D.x;
|
||
for (int i = tail_line + 1; i < lineFeatures.size(); i++)
|
||
{
|
||
SSG_lineFeature* a_line = &lineFeatures[i];
|
||
int endingNum = (int)a_line->endings.size();
|
||
|
||
bool growFlag = false;
|
||
for (int m = 0; m < endingNum; m++)
|
||
{
|
||
bool vld_0 = _checkTwoFeatureVldToGrow(tail_node, &a_line->endings[m], growParam);
|
||
if (true == vld_0)
|
||
{
|
||
a_line->endings[m].featureType = a_tree->treeType;
|
||
a_tree->treeNodes.push_back(a_line->endings[m]);
|
||
growFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (false == growFlag)
|
||
{
|
||
int line_diff = abs(a_line->lineIdx - a_tree->treeNodes.back().jumpPos2D.x);
|
||
if (line_diff > growParam.maxLineSkipNum)
|
||
break;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
//获取生长树的ROI
|
||
void _getTreeROI(SSG_featureTree* a_tree)
|
||
{
|
||
if (a_tree->treeNodes.size() == 0)
|
||
{
|
||
a_tree->roi.left = 0;
|
||
a_tree->roi.right = 0;
|
||
a_tree->roi.top = 0;
|
||
a_tree->roi.bottom = 0;
|
||
}
|
||
else
|
||
{
|
||
a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
|
||
a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
|
||
a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
|
||
a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
|
||
for (int i = 1, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++)
|
||
{
|
||
if(a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
|
||
a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
|
||
if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
|
||
a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
|
||
if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
|
||
a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
|
||
if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
|
||
a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
SSG_featureTree* _getIncludingBigTree(SSG_featureTree* a_tree, std::vector<SSG_featureTree>& trees, bool chk_up)
|
||
{
|
||
SSG_featureTree* _bigTree = NULL;
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* chk_tree = &trees[i];
|
||
if (a_tree == chk_tree)
|
||
continue;
|
||
|
||
if (true == chk_up)
|
||
{
|
||
if (chk_tree->roi.bottom < a_tree->roi.top)
|
||
{
|
||
if ((chk_tree->roi.left < a_tree->roi.left) &&
|
||
(chk_tree->roi.right > a_tree->roi.right)) //包含关系成立
|
||
{
|
||
if (NULL == _bigTree)
|
||
_bigTree = chk_tree;
|
||
else
|
||
{
|
||
if(chk_tree->roi.bottom > _bigTree->roi.bottom)
|
||
_bigTree = chk_tree;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (chk_tree->roi.top > a_tree->roi.bottom)
|
||
{
|
||
if ((chk_tree->roi.left < a_tree->roi.left) &&
|
||
(chk_tree->roi.right > a_tree->roi.right)) //包含关系成立
|
||
{
|
||
if (NULL == _bigTree)
|
||
_bigTree = chk_tree;
|
||
else
|
||
{
|
||
if (chk_tree->roi.bottom < _bigTree->roi.bottom)
|
||
_bigTree = chk_tree;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return _bigTree;
|
||
}
|
||
|
||
void sg_getFeatureGrowingTrees(
|
||
std::vector<SSG_lineFeature>& lineFeatures,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++)
|
||
{
|
||
SSG_lineFeature& a_line = lineFeatures[i];
|
||
if (a_line.features.size() > 0)
|
||
{
|
||
for (int j = 0, j_max = (int)a_line.features.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D& a_feature = a_line.features[j];
|
||
if (a_feature.jumpPos2D.x == 207)
|
||
int kkk = 1;
|
||
bool isMatched = _featureGrowing(a_feature, a_line.lineIdx, trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = a_feature.featureType;
|
||
a_newTree.sLineIdx = a_line.lineIdx;
|
||
a_newTree.eLineIdx = a_line.lineIdx;
|
||
trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
}
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int lineIdx = a_line.lineIdx;
|
||
int m_max = (int)trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
|
||
if ( ((growParam.maxLineSkipNum > 0) &&(line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void wd_getFeatureGrowingTrees_noTypeMatch(
|
||
std::vector<SSG_lineFeature>& lineFeatures,
|
||
std::vector<SSG_featureTree>& feature_trees,
|
||
std::vector<SSG_featureTree>& ending_trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++)
|
||
{
|
||
SSG_lineFeature& a_line = lineFeatures[i];
|
||
if (a_line.features.size() > 0)
|
||
{
|
||
for (int j = 0, j_max = (int)a_line.features.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D& a_feature = a_line.features[j];
|
||
if (a_feature.jumpPos2D.x == 207)
|
||
int kkk = 1;
|
||
bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, feature_trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = a_feature.featureType;
|
||
a_newTree.sLineIdx = a_line.lineIdx;
|
||
a_newTree.eLineIdx = a_line.lineIdx;
|
||
feature_trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
}
|
||
if (a_line.endings.size() > 0)
|
||
{
|
||
for (int j = 0, j_max = (int)a_line.endings.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D& a_feature = a_line.endings[j];
|
||
if (a_feature.jumpPos2D.x == 207)
|
||
int kkk = 1;
|
||
bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, ending_trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = a_feature.featureType;
|
||
a_newTree.sLineIdx = a_line.lineIdx;
|
||
a_newTree.eLineIdx = a_line.lineIdx;
|
||
ending_trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
}
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int lineIdx = a_line.lineIdx;
|
||
int m_max = (int)feature_trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == feature_trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().jumpPos2D.x);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
feature_trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(feature_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
feature_trees.erase(feature_trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
m_max = (int)ending_trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == ending_trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - ending_trees[m].treeNodes.back().jumpPos2D.x);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
ending_trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(ending_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
ending_trees.erase(ending_trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//segment特征生长
|
||
void wd_getSegFeatureGrowingTrees(
|
||
std::vector<std::vector<SWD_segFeature>>& all_lineFeatures,
|
||
std::vector<SWD_segFeatureTree>& feature_trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
|
||
{
|
||
std::vector<SWD_segFeature>& a_lineFeatures = all_lineFeatures[i];
|
||
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
|
||
{
|
||
SWD_segFeature& a_feature = a_lineFeatures[j];
|
||
bool isMatched = _segFeatureGrowing(a_feature, i, feature_trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SWD_segFeatureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.sLineIdx = a_feature.lineIdx;
|
||
a_newTree.eLineIdx = a_feature.lineIdx;
|
||
a_newTree.tree_value = (a_feature.startPt.z + a_feature.endPt.z) / 2;
|
||
feature_trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int lineIdx = i;
|
||
int m_max = (int)feature_trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == feature_trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().lineIdx);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
feature_trees[m].treeState = TREE_STATE_DEAD;
|
||
SWD_segFeature* first_node = &(feature_trees[m].treeNodes[0]);
|
||
SWD_segFeature* last_node = &(feature_trees[m].treeNodes[feature_trees[m].treeNodes.size() - 1]);
|
||
double len = (abs(first_node->startPt.x - last_node->startPt.x) + abs(first_node->endPt.x - last_node->endPt.x))/2;
|
||
if (len <= growParam.minLTypeTreeLen)
|
||
feature_trees.erase(feature_trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void sg_lineFeaturesGrowing(
|
||
int lineIdx,
|
||
bool isLastLine,
|
||
std::vector<SSG_basicFeature1D>& features,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
if (features.size() > 0)
|
||
{
|
||
for (int j = 0, j_max = (int)features.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D& a_feature = features[j];
|
||
if (a_feature.jumpPos2D.x== 207)
|
||
int kkk = 1;
|
||
bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = a_feature.featureType;
|
||
a_newTree.sLineIdx = lineIdx;
|
||
a_newTree.eLineIdx = lineIdx;
|
||
a_newTree.tree_value = a_feature.featureValue;
|
||
trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
}
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int m_max = (int)trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(true == isLastLine))
|
||
{
|
||
trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void sg_getEndingGrowingTrees(
|
||
std::vector<SSG_2DValueI>& lineEndings,
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
bool isVScan,
|
||
int featureType,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)lineEndings.size(); i < i_max; i++)
|
||
{
|
||
if (i == 175)
|
||
int kkk = 1;
|
||
SSG_2DValueI& an_ending = lineEndings[i];
|
||
SSG_basicFeature1D endingFeature;
|
||
endingFeature.featureType = featureType;
|
||
endingFeature.jumpPos2D.x = an_ending.x;
|
||
endingFeature.jumpPos2D.y = an_ending.y;
|
||
if(true == isVScan)
|
||
endingFeature.jumpPos = laser3DPoints[an_ending.x].p3DPosition[an_ending.y].pt3D;
|
||
else
|
||
{
|
||
endingFeature.jumpPos.x = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.y;
|
||
endingFeature.jumpPos.y = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.x;
|
||
endingFeature.jumpPos.z = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.z;
|
||
}
|
||
bool isMatched = _featureGrowing(endingFeature, an_ending.x, trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
memset(&a_newTree, 0, sizeof(SSG_featureTree));
|
||
a_newTree.treeNodes.push_back(endingFeature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = endingFeature.featureType;
|
||
a_newTree.sLineIdx = an_ending.x;
|
||
a_newTree.eLineIdx = an_ending.x;
|
||
a_newTree.angleChkScalePos = -1;
|
||
trees.push_back(a_newTree);
|
||
}
|
||
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int lineIdx = an_ending.x;
|
||
int m_max = (int)trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void sg_getEndingGrowingTrees_angleCheck(
|
||
std::vector<SSG_2DValueI>& lineEndings,
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
bool isVScan,
|
||
int featureType,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam,
|
||
double angleCheckScale)
|
||
{
|
||
for (int i = 0, i_max = (int)lineEndings.size(); i < i_max; i++)
|
||
{
|
||
if (i == 175)
|
||
int kkk = 1;
|
||
SSG_2DValueI& an_ending = lineEndings[i];
|
||
SSG_basicFeature1D endingFeature;
|
||
endingFeature.featureType = featureType;
|
||
endingFeature.jumpPos2D.x = an_ending.x;
|
||
endingFeature.jumpPos2D.y = an_ending.y;
|
||
if (true == isVScan)
|
||
endingFeature.jumpPos = laser3DPoints[an_ending.x].p3DPosition[an_ending.y].pt3D;
|
||
else
|
||
{
|
||
endingFeature.jumpPos.x = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.y;
|
||
endingFeature.jumpPos.y = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.x;
|
||
endingFeature.jumpPos.z = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.z;
|
||
}
|
||
int matchedTree = _getMatchedTree_angleCheck(endingFeature, an_ending.x, trees, growParam, angleCheckScale);
|
||
if (matchedTree >= 0)
|
||
{
|
||
trees[matchedTree].eLineIdx = an_ending.x;
|
||
trees[matchedTree].treeNodes.push_back(endingFeature);
|
||
trees[matchedTree].tree_value += endingFeature.featureValue;
|
||
}
|
||
else
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
memset(&a_newTree, 0, sizeof(SSG_featureTree));
|
||
a_newTree.treeNodes.push_back(endingFeature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = endingFeature.featureType;
|
||
a_newTree.sLineIdx = an_ending.x;
|
||
a_newTree.eLineIdx = an_ending.x;
|
||
a_newTree.angleChkScalePos = -1;
|
||
trees.push_back(a_newTree);
|
||
}
|
||
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int lineIdx = an_ending.x;
|
||
int m_max = (int)trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
//对扫描线的V型特征进行生长
|
||
void sg_LVFeatureGrowing(
|
||
std::vector<SSG_lineFeature>& lineFeatures,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_bagParam bagParam,
|
||
SSG_treeGrowParam growParam,
|
||
std::vector<SSG_2DValueI>& edgePts_0,
|
||
std::vector<SSG_2DValueI>& edgePts_1)
|
||
{
|
||
//特征生长
|
||
sg_getFeatureGrowingTrees(
|
||
lineFeatures,
|
||
trees,
|
||
growParam);
|
||
|
||
//迭代一次：重新对每棵树头尾检查，看是否可以继续生长到LINE_EDGE
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_tree = &trees[i];
|
||
|
||
//Tree类型重新统计确定。
|
||
std::vector<int> typeStat;
|
||
typeStat.resize(LINE_FEATURE_NUM);
|
||
for (int m = 0, m_max = (int)a_tree->treeNodes.size(); m < m_max; m++)
|
||
{
|
||
int type = a_tree->treeNodes[m].featureType;
|
||
typeStat[type]++;
|
||
}
|
||
int maxId = -1;
|
||
int maxSize = 0;
|
||
for (int m = 0; m < typeStat.size(); m++)
|
||
{
|
||
if (maxSize < typeStat[m])
|
||
{
|
||
maxId = m;
|
||
maxSize = typeStat[m];
|
||
}
|
||
}
|
||
if (maxId > 0)
|
||
a_tree->treeType = maxId;
|
||
|
||
_growingFromHead(a_tree, lineFeatures, growParam);
|
||
_growingFromTail(a_tree, lineFeatures, growParam);
|
||
}
|
||
|
||
///过滤
|
||
///两种过滤方法：
|
||
///（1）针对SLOPE和V型生长树，检查生长方向和垂直方向的比例。
|
||
///（2）宏观检查：检查短的生长树是否被长的生长树所包含
|
||
int tree_size = (int)trees.size();
|
||
for (int m = tree_size - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
SSG_featureTree* a_tree = &trees[m];
|
||
_getTreeROI(a_tree);
|
||
if ((LINE_FEATURE_V_SLOPE == a_tree->treeType) ||
|
||
(LINE_FEATURE_L_SLOPE_H2L == a_tree->treeType) ||
|
||
(LINE_FEATURE_L_SLOPE_L2H == a_tree->treeType) ||
|
||
(LINE_FEATURE_CORNER_V == a_tree->treeType))
|
||
{
|
||
//检查生长方向（x方向）的长度
|
||
double grow_len = a_tree->roi.right - a_tree->roi.left;
|
||
double grow_width = a_tree->roi.bottom - a_tree->roi.top;
|
||
if ((grow_len < bagParam.bagW / 4) && (grow_width > grow_len / 2))
|
||
{
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
tree_size = (int)trees.size();
|
||
for (int m = tree_size - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (m == 24)
|
||
int kkk = 1;
|
||
SSG_featureTree* a_tree = &trees[m];
|
||
double grow_len = a_tree->roi.right - a_tree->roi.left;
|
||
//检查前后是否被包含
|
||
//取前面最近的包含a_tree的生长树
|
||
if ((grow_len < bagParam.bagW / 2) && ((LINE_FEATURE_V_SLOPE == a_tree->treeType) ||
|
||
(LINE_FEATURE_L_SLOPE_H2L == a_tree->treeType) ||
|
||
(LINE_FEATURE_L_SLOPE_L2H == a_tree->treeType) ||
|
||
(LINE_FEATURE_CORNER_V == a_tree->treeType)))
|
||
{
|
||
SSG_featureTree* up_bigTree = _getIncludingBigTree(a_tree, trees, true);
|
||
SSG_featureTree* down_bigTree = _getIncludingBigTree(a_tree, trees, false);
|
||
if ((NULL != up_bigTree) && (NULL != down_bigTree))
|
||
{
|
||
double dist = (down_bigTree->roi.top + down_bigTree->roi.bottom - up_bigTree->roi.top - up_bigTree->roi.bottom) / 2;
|
||
if (dist < bagParam.bagL * 1.3)
|
||
{
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
std::vector<SSG_2DValueI> edge0_pts;
|
||
std::vector<SSG_2DValueI> edge1_pts;
|
||
std::vector<double> edge0_y;
|
||
std::vector<double> edge1_y;
|
||
for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++)
|
||
{
|
||
SSG_lineFeature& a_line = lineFeatures[i];
|
||
int a_line_endingNum = (int)a_line.endings.size();
|
||
if (a_line_endingNum > 0)
|
||
{
|
||
for (int m = 0; m < a_line_endingNum; m++)
|
||
{
|
||
SSG_2DValueI an_edge = { a_line.endings[m].jumpPos2D.x, a_line.endings[m].jumpPos2D.y, a_line.endings[m].featureType, a_line.endings[m].jumpPos.z };
|
||
if (LINE_FEATURE_LINE_ENDING_0 == a_line.endings[m].featureType)
|
||
{
|
||
edge0_pts.push_back(an_edge);
|
||
edge0_y.push_back(a_line.endings[m].jumpPos.y);
|
||
}
|
||
else if(LINE_FEATURE_LINE_ENDING_1 == a_line.endings[m].featureType)
|
||
{
|
||
edge1_pts.push_back(an_edge);
|
||
edge1_y.push_back(a_line.endings[m].jumpPos.y);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#if 1
|
||
//区分左右
|
||
edgePts_0.insert(edgePts_0.end(), edge0_pts.begin(), edge0_pts.end());
|
||
edgePts_1.insert(edgePts_1.end(), edge1_pts.begin(), edge1_pts.end());
|
||
#else
|
||
//Edge点需要进行滤除离群点（由于扫描原因，可能会出现拉条子现象）
|
||
const double outlier_sigma_k = 5.0;//5倍sigma
|
||
SSG_meanVar meanVar_edge0 = _computeMeanVar(edge0_y.data(), edge0_y.size());
|
||
double edge0_dyTh = meanVar_edge0.var * outlier_sigma_k; //5倍sigma
|
||
for (int i = 0; i < edge0_pts.size(); i++)
|
||
{
|
||
if (edge0_pts[i].x == 622)
|
||
int kkk = 1;
|
||
double yDiff = abs(edge0_y[i] - meanVar_edge0.mean);
|
||
if (yDiff < edge0_dyTh)
|
||
edgePts.push_back(edge0_pts[i]);
|
||
}
|
||
SSG_meanVar meanVar_edge1 = _computeMeanVar(edge1_y.data(), edge1_y.size());
|
||
double edge1_dyTh = meanVar_edge1.var * outlier_sigma_k; //5倍sigma
|
||
for (int i = 1; i < edge1_pts.size(); i++)
|
||
{
|
||
if (edge1_pts[i].x == 622)
|
||
int kkk = 1;
|
||
double yDiff = abs(edge1_y[i] - meanVar_edge1.mean);
|
||
if (yDiff < edge1_dyTh)
|
||
edgePts.push_back(edge1_pts[i]);
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void sg_getPeakGrowingTrees(
|
||
std::vector<std::vector< SSG_basicFeature1D>>& peakFeatures,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)peakFeatures.size(); i < i_max; i++)
|
||
{
|
||
int lineIdx = i;
|
||
if (i == 761)
|
||
int kkk = 1;
|
||
std::vector< SSG_basicFeature1D>& a_linePeak = peakFeatures[i];
|
||
if (a_linePeak.size() > 0)
|
||
{
|
||
for (int j = 0, j_max = (int)a_linePeak.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D& a_feature = a_linePeak[j];
|
||
if (a_feature.jumpPos2D.x == 207)
|
||
int kkk = 1;
|
||
bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam);
|
||
if (false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_featureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = a_feature.featureType;
|
||
a_newTree.sLineIdx = lineIdx;
|
||
a_newTree.eLineIdx = lineIdx;
|
||
trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
}
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int m_max = (int)trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(i == i_max - 1))
|
||
{
|
||
trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
if (false == isValid)
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//对扫描线的peak特征进行生长
|
||
void sg_peakFeatureGrowing(
|
||
std::vector<std::vector< SSG_basicFeature1D>>& lineFeatures,
|
||
std::vector<SSG_featureTree>& trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
//特征生长
|
||
sg_getPeakGrowingTrees(
|
||
lineFeatures,
|
||
trees,
|
||
growParam);
|
||
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_tree = &trees[i];
|
||
_getTreeROI(a_tree);
|
||
}
|
||
///过滤
|
||
|
||
}
|
||
#if 0
|
||
void sg_fillingNullNodes(std::vector<SSG_featureTree>& trees)
|
||
{
|
||
int treeSize = trees.size();
|
||
for (int i = 0; i < treeSize; i++)
|
||
{
|
||
SSG_featureTree* a_tree = &trees[i];
|
||
int nodeSize = a_tree->treeNodes.size();
|
||
for (int j = nodeSize-2; j >= 0; j--)
|
||
{
|
||
SSG_lineFeature* curr_node = &(a_tree->treeNodes[j]);
|
||
SSG_lineFeature* nxt_node = &(a_tree->treeNodes[j+1]);
|
||
int line_diff = nxt_node->jumpPos2D.x - curr_node->jumpPos2D.x;
|
||
if (line_diff > 1)
|
||
{
|
||
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
//判断生长点
|
||
bool validGrowing(
|
||
SSG_featureSemiCircle& a_feature,
|
||
SSG_semiCircleFeatureTree& a_tree,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
SSG_featureSemiCircle last_node = a_tree.treeNodes.back();
|
||
double y_diff = abs(a_feature.midPt.y - last_node.midPt.y);
|
||
int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
|
||
double x_diff = abs(a_feature.midPt.x - last_node.midPt.x);
|
||
if ((y_diff < growParam.yDeviation_max) &&
|
||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
// 将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
|
||
int _featureGrowing_semiCircle(
|
||
SSG_featureSemiCircle& a_feature,
|
||
std::vector<SSG_semiCircleFeatureTree>&trees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
if ((FEATURE_FLAG_INVLD_START == a_feature.flag) || (FEATURE_FLAG_VALID_START == a_feature.flag))
|
||
return -1;
|
||
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_semiCircleFeatureTree& a_tree = trees[i];
|
||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||
continue;
|
||
//检查生长点
|
||
SSG_featureSemiCircle last_node = a_tree.treeNodes.back();
|
||
if ( (last_node.lineIdx == a_feature.lineIdx)|| //x为lineIdx，同一条扫描线上的不进行生长
|
||
(FEATURE_FLAG_INVLD_END == last_node.flag) || (FEATURE_FLAG_VALID_END == last_node.flag))
|
||
continue;
|
||
|
||
//判断生长点
|
||
bool vldGrowing = validGrowing(a_feature, a_tree, growParam);
|
||
if (true == vldGrowing)
|
||
return i;
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
bool _invalidateSemiCircleTrees(SSG_semiCircleFeatureTree& a_tree, double minTreeLen)
|
||
{
|
||
SSG_featureSemiCircle* first_node = &(a_tree.treeNodes[0]);
|
||
SSG_featureSemiCircle* last_node = &(a_tree.treeNodes[a_tree.treeNodes.size() - 1]);
|
||
double len = sqrt(pow(first_node->midPt.x - last_node->midPt.x, 2) + pow(first_node->midPt.y - last_node->midPt.y, 2));
|
||
if (len <= minTreeLen)
|
||
return false;
|
||
else
|
||
return true;
|
||
}
|
||
|
||
void sg_getFeatureGrowingTrees_semiCircle(
|
||
std::vector< SSG_featureSemiCircle>& lineFeatures,
|
||
const int lineIdx,
|
||
const int lineSize,
|
||
std::vector<SSG_semiCircleFeatureTree>& trees,
|
||
std::vector<SSG_semiCircleFeatureTree>& stopTrees,
|
||
std::vector<SSG_semiCircleFeatureTree>& invalidTrees,
|
||
SSG_treeGrowParam growParam)
|
||
{
|
||
for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureSemiCircle& a_feature = lineFeatures[i];
|
||
if (FEATURE_FLAG_INVALID == a_feature.flag)
|
||
continue;
|
||
|
||
if ( (a_feature.lineIdx >= 205) && (i == 5))
|
||
int kkk = 1;
|
||
int matchedTreeIdx = _featureGrowing_semiCircle(a_feature, trees, growParam);
|
||
if (matchedTreeIdx >= 0)
|
||
{
|
||
//检查最佳匹配
|
||
std::vector<int> otherMatching;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
bool vldGrowing = validGrowing(lineFeatures[j], trees[matchedTreeIdx], growParam);
|
||
if (false == vldGrowing)
|
||
break;
|
||
otherMatching.push_back(j);
|
||
}
|
||
if (otherMatching.size() > 0)
|
||
{
|
||
SSG_featureSemiCircle last_node = trees[matchedTreeIdx].treeNodes.back();
|
||
double minDist = abs(a_feature.midPt.y - last_node.midPt.y);
|
||
int bestMatch = i;
|
||
//取最小的匹配
|
||
for (int m = 0, m_max = (int)otherMatching.size(); m < m_max; m++)
|
||
{
|
||
int idx = otherMatching[m];
|
||
double y_diff = abs(lineFeatures[idx].midPt.y - last_node.midPt.y);
|
||
if (y_diff < minDist)
|
||
{
|
||
minDist = y_diff;
|
||
bestMatch = idx;
|
||
}
|
||
}
|
||
otherMatching.push_back(i);
|
||
for (int m = 0, m_max = (int)otherMatching.size(); m < m_max; m++)
|
||
{
|
||
int idx = otherMatching[m];
|
||
if (bestMatch == idx)
|
||
{
|
||
trees[matchedTreeIdx].eLineIdx = lineFeatures[idx].lineIdx;
|
||
trees[matchedTreeIdx].treeNodes.push_back(lineFeatures[idx]);
|
||
}
|
||
lineFeatures[idx].flag = FEATURE_FLAG_INVALID;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
trees[matchedTreeIdx].eLineIdx = a_feature.lineIdx;
|
||
trees[matchedTreeIdx].treeNodes.push_back(a_feature);
|
||
}
|
||
}
|
||
else //(false == isMatched)
|
||
{
|
||
//新的生长树
|
||
SSG_semiCircleFeatureTree a_newTree;
|
||
a_newTree.treeNodes.push_back(a_feature);
|
||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||
a_newTree.treeType = 0;
|
||
a_newTree.sLineIdx = a_feature.lineIdx;
|
||
a_newTree.eLineIdx = a_feature.lineIdx;
|
||
trees.push_back(a_newTree);
|
||
}
|
||
}
|
||
//检查停止生长的树,加速。
|
||
//将生长节点为1的生长树移除
|
||
int m_max = (int)trees.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||
{
|
||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||
{
|
||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx);
|
||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||
(lineIdx == lineSize-1))
|
||
{
|
||
trees[m].treeState = TREE_STATE_DEAD;
|
||
bool isValid = _invalidateSemiCircleTrees(trees[m], growParam.minVTypeTreeLen);
|
||
if (true == isValid)
|
||
stopTrees.push_back(trees[m]);
|
||
else
|
||
invalidTrees.push_back(trees[m]);
|
||
trees.erase(trees.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
} |