代码备份
This commit is contained in:
parent
8d34a53ace
commit
1026adaa5c
@ -957,7 +957,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
|
|||||||
if (pkTop.size() < 1)
|
if (pkTop.size() < 1)
|
||||||
return planePara;
|
return planePara;
|
||||||
|
|
||||||
int pntSizeTh = totalPntSize * 3 / 10;
|
int pntSizeTh = totalPntSize / 10;
|
||||||
SSG_intPair* vldPeak = NULL;
|
SSG_intPair* vldPeak = NULL;
|
||||||
for (int i = 0, i_max = pkTop.size(); i < i_max; i++)
|
for (int i = 0, i_max = pkTop.size(); i < i_max; i++)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -30,7 +30,7 @@ SG_APISHARED_EXPORT void sg_getSieveNodes(
|
|||||||
SVzNL3DLaserLine* laser3DPoints,
|
SVzNL3DLaserLine* laser3DPoints,
|
||||||
int lineNum,
|
int lineNum,
|
||||||
const SSG_sieveNodeDetectionParam sieveDetectParam,
|
const SSG_sieveNodeDetectionParam sieveDetectParam,
|
||||||
std::vector<SVzNL3DPoint>& nodePos);
|
std::vector<std::vector<SVzNL3DPoint>>& nodePos);
|
||||||
|
|
||||||
//计算一个平面调平参数。
|
//计算一个平面调平参数。
|
||||||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||||||
|
|||||||
@ -5,6 +5,8 @@
|
|||||||
#include <opencv2/opencv.hpp>
|
#include <opencv2/opencv.hpp>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
|
||||||
|
#define ALGO_USE_PATTERN_MATCH 0
|
||||||
|
|
||||||
void sg_lineDataR(SVzNL3DLaserLine* a_line,
|
void sg_lineDataR(SVzNL3DLaserLine* a_line,
|
||||||
const double* camPoseR,
|
const double* camPoseR,
|
||||||
double groundH)
|
double groundH)
|
||||||
@ -141,12 +143,162 @@ SVzNL2DPoint _getNearestScanPt(
|
|||||||
return bestPos;
|
return bestPos;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//搜索目标在排序目标中的位置。返回的是排序目标前一位的序号，-1为最后一个
|
||||||
|
int getObjPostPos(std::vector<int>& sortedTrees, int treeID, std::vector<SSG_semiCircleFeatureTree>& trees, bool HPos)
|
||||||
|
{
|
||||||
|
SSG_semiCircleFeatureTree& obj_tree = trees[treeID];
|
||||||
|
|
||||||
|
for (int i = 0, i_max = sortedTrees.size(); i < i_max; i++)
|
||||||
|
{
|
||||||
|
int a_tree_id = sortedTrees[i];
|
||||||
|
SSG_semiCircleFeatureTree& a_tree = trees[a_tree_id];
|
||||||
|
if (((obj_tree.centerPt.x < a_tree.centerPt.x) && (true == HPos)) ||
|
||||||
|
((obj_tree.centerPt.y < a_tree.centerPt.y) && (false == HPos)))
|
||||||
|
{
|
||||||
|
return i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return -1; //最后一个
|
||||||
|
}
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
SVzNL2DPoint nodePos;
|
||||||
|
int LT_link;
|
||||||
|
int RT_link;
|
||||||
|
int LB_link;
|
||||||
|
int RB_link;
|
||||||
|
double LT_dist;
|
||||||
|
double RT_dist;
|
||||||
|
double RB_dist;
|
||||||
|
double LB_dist;
|
||||||
|
}NodeQuadLink;
|
||||||
|
|
||||||
|
int getObjPostPos(std::vector<int>& sortedLine, int nodeID, std::vector<NodeQuadLink>& nodeLink, bool HPos)
|
||||||
|
{
|
||||||
|
SVzNL2DPoint& nodePos = nodeLink[nodeID].nodePos;
|
||||||
|
for (int i = 0, i_max = sortedLine.size(); i < i_max; i++)
|
||||||
|
{
|
||||||
|
int a_node_id = sortedLine[i];
|
||||||
|
SVzNL2DPoint& sorting_node_pos = nodeLink[a_node_id].nodePos;
|
||||||
|
if (((nodePos.x < sorting_node_pos.x) && (true == HPos)) ||
|
||||||
|
((nodePos.y < sorting_node_pos.y) && (false == HPos)))
|
||||||
|
{
|
||||||
|
return i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return -1; //最后一个
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void _recursiveSorting(
|
||||||
|
int nodeId,
|
||||||
|
std::vector<std::vector<int>>& hSorting,
|
||||||
|
std::vector< int>& nodeProcFlag,
|
||||||
|
std::vector< int>& nodeSortingLineID,
|
||||||
|
std::vector< NodeQuadLink>& nodeLinks,
|
||||||
|
bool HPos,
|
||||||
|
int* maxLineId)
|
||||||
|
{
|
||||||
|
std::vector<int> processBuff;
|
||||||
|
processBuff.push_back(nodeId);
|
||||||
|
while (processBuff.size() > 0)
|
||||||
|
{
|
||||||
|
int nodeId = processBuff.front();
|
||||||
|
if (nodeProcFlag[nodeId] == 0)
|
||||||
|
{
|
||||||
|
int currLineId = nodeSortingLineID[nodeId];
|
||||||
|
if (*maxLineId < currLineId)
|
||||||
|
*maxLineId = currLineId;
|
||||||
|
std::vector<int>& currLine = hSorting[currLineId];
|
||||||
|
//将当前Node插入currLine
|
||||||
|
int nodePos = getObjPostPos(currLine, nodeId, nodeLinks, HPos);
|
||||||
|
if (nodePos < 0)
|
||||||
|
currLine.push_back(nodeId);
|
||||||
|
else
|
||||||
|
currLine.insert(currLine.begin() + nodePos, nodeId);
|
||||||
|
nodeProcFlag[nodeId] = 1;
|
||||||
|
//检查Link
|
||||||
|
NodeQuadLink& currLink = nodeLinks[nodeId];
|
||||||
|
if ((currLink.LT_dist > 0) && (currLineId > 0))
|
||||||
|
{
|
||||||
|
int linkNode = currLink.LT_link;
|
||||||
|
int lineId = currLineId - 1;
|
||||||
|
nodeSortingLineID[linkNode] = lineId;
|
||||||
|
processBuff.push_back(linkNode);
|
||||||
|
currLink.LT_dist = -1.0;
|
||||||
|
nodeLinks[linkNode].RB_dist = -1.0; //取消
|
||||||
|
}
|
||||||
|
if ((currLink.RT_dist > 0) && (currLineId > 0))
|
||||||
|
{
|
||||||
|
int linkNode = currLink.RT_link;
|
||||||
|
int lineId = currLineId - 1;
|
||||||
|
nodeSortingLineID[linkNode] = lineId;
|
||||||
|
processBuff.push_back(linkNode);
|
||||||
|
currLink.RT_dist = -1.0;
|
||||||
|
nodeLinks[linkNode].LB_dist = -1.0; //取消
|
||||||
|
}
|
||||||
|
if ((currLink.LB_dist > 0) && (currLineId < hSorting.size() - 1))
|
||||||
|
{
|
||||||
|
int linkNode = currLink.LB_link;
|
||||||
|
int lineId = currLineId + 1;
|
||||||
|
nodeSortingLineID[linkNode] = lineId;
|
||||||
|
processBuff.push_back(linkNode);
|
||||||
|
currLink.LB_dist = -1.0;
|
||||||
|
nodeLinks[linkNode].RT_dist = -1.0; //取消
|
||||||
|
}
|
||||||
|
if ((currLink.RB_dist > 0) && (currLineId < hSorting.size() - 1))
|
||||||
|
{
|
||||||
|
int linkNode = currLink.RB_link;
|
||||||
|
int lineId = currLineId + 1;
|
||||||
|
nodeSortingLineID[linkNode] = lineId;
|
||||||
|
processBuff.push_back(linkNode);
|
||||||
|
currLink.RB_dist = -1.0;
|
||||||
|
nodeLinks[linkNode].LT_dist = -1.0; //取消
|
||||||
|
}
|
||||||
|
}
|
||||||
|
processBuff.erase(processBuff.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void _lineFillNullNode(
|
||||||
|
std::vector<SVzNL3DPosition>& sortingNodes_line,
|
||||||
|
std::vector< NodeQuadLink>& nodeLinks,
|
||||||
|
const double lineNodeDistMean
|
||||||
|
)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < sortingNodes_line.size() -1; i++)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition node_1 = sortingNodes_line[i];
|
||||||
|
if (node_1.nPointIdx < 0)
|
||||||
|
continue;
|
||||||
|
SVzNL3DPosition node_2 = sortingNodes_line[i + 1];
|
||||||
|
double dist = sqrt(pow(node_1.pt3D.x - node_2.pt3D.x, 2) + pow(node_1.pt3D.y - node_2.pt3D.y, 2));
|
||||||
|
int n = (int)(dist / lineNodeDistMean + 0.5) - 1;
|
||||||
|
if (n > 0)
|
||||||
|
{
|
||||||
|
//判断遗漏几个目标
|
||||||
|
double x_step = (node_2.pt3D.x - node_1.pt3D.x) / (n + 1);
|
||||||
|
double y_step = (node_2.pt3D.y - node_1.pt3D.y) / (n + 1);
|
||||||
|
double z_step = (node_2.pt3D.z - node_1.pt3D.z) / (n + 1);
|
||||||
|
for (int m = 0; m < n; m++)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition a_newNode;
|
||||||
|
a_newNode.nPointIdx = -1;
|
||||||
|
a_newNode.pt3D.x = x_step * (m + 1) + node_1.pt3D.x;
|
||||||
|
a_newNode.pt3D.y = y_step * (m + 1) + node_1.pt3D.y;
|
||||||
|
a_newNode.pt3D.z = z_step * (m + 1) + node_1.pt3D.z;
|
||||||
|
sortingNodes_line.insert(sortingNodes_line.begin() + i + 1 + m, a_newNode);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void sg_getSieveNodes(
|
void sg_getSieveNodes(
|
||||||
SVzNL3DLaserLine* laser3DPoints,
|
SVzNL3DLaserLine* laser3DPoints,
|
||||||
int lineNum,
|
int lineNum,
|
||||||
const SSG_sieveNodeDetectionParam sieveDetectParam,
|
const SSG_sieveNodeDetectionParam sieveDetectParam,
|
||||||
std::vector<SVzNL3DPoint>& nodePos)
|
std::vector<std::vector<SVzNL3DPoint>>& nodePos)
|
||||||
{
|
{
|
||||||
const int hole_max_ptSize = 20;
|
const int hole_max_ptSize = 20;
|
||||||
int errCode = 0;
|
int errCode = 0;
|
||||||
@ -287,19 +439,332 @@ void sg_getSieveNodes(
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
//按行排序
|
//按行排序
|
||||||
// (1)建立2D索引实现有序搜索
|
#if 1
|
||||||
//（2）从
|
// (1)量化到2D,建立2D索引实现有序搜索
|
||||||
std::vector<std::vector<SSG_semiCircleFeatureTree>> sortedTrees;
|
SVzNL3DRangeD ROI = sg_getScanDataROI(laser3DPoints, lineNum);
|
||||||
|
double scale_x = 1.0;
|
||||||
|
double scale_y = 1.0;
|
||||||
|
int rows = (int)((ROI.yRange.max - ROI.yRange.min) / scale_y) + 1;
|
||||||
|
int cols = (int)((ROI.xRange.max - ROI.xRange.min) / scale_x) + 1;
|
||||||
|
if ((rows == 0) || (cols == 0))
|
||||||
|
return;
|
||||||
|
cv::Mat objMask = cv::Mat(rows, cols, CV_32SC1, -1);//rows, cols
|
||||||
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
||||||
{
|
{
|
||||||
//if(stopTrees[i].)
|
SVzNL3DPoint treeCenter = laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].pt3D;
|
||||||
|
int px = (int)((treeCenter.x - ROI.xRange.min) / scale_x);
|
||||||
|
int py = (int)((treeCenter.y - ROI.yRange.min) / scale_y);
|
||||||
|
objMask.at<int>(py, px) = i;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
std::vector< NodeQuadLink> nodeLinks(stopTrees.size(), { {0,0},-1,-1,-1,-1, -1.0, -1.0, -1.0, -1.0});
|
||||||
|
double neighbourDistTh = sieveDetectParam.sieveHoleSize * 1.2; //以正方形为模型，边长为1，则对角线为1.414，取1.2为长度门限
|
||||||
|
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
||||||
|
{
|
||||||
|
SSG_semiCircleFeatureTree& tree_1 = stopTrees[i];
|
||||||
|
NodeQuadLink& link_1 = nodeLinks[i];
|
||||||
|
link_1.nodePos = tree_1.centerPos;
|
||||||
|
for (int j = i + 1; j < i_max; j++)
|
||||||
|
{
|
||||||
|
SSG_semiCircleFeatureTree& tree_2 = stopTrees[j];
|
||||||
|
NodeQuadLink& link_2 = nodeLinks[j];
|
||||||
|
double dist = sqrt(pow(tree_1.centerPt.x - tree_2.centerPt.x, 2) + pow(tree_1.centerPt.y - tree_2.centerPt.y, 2));
|
||||||
|
if (dist < neighbourDistTh)
|
||||||
|
{
|
||||||
|
if (tree_1.centerPt.x < tree_2.centerPt.x) //left
|
||||||
|
{
|
||||||
|
if (tree_1.centerPt.y < tree_2.centerPt.y) //top
|
||||||
|
{
|
||||||
|
//tree1:RB link is tree_2
|
||||||
|
//tree2:LT link is tree_1
|
||||||
|
if ( (link_1.RB_link >= 0) || (link_2.LT_link >= 0))
|
||||||
|
{
|
||||||
|
bool currIsBest = true;
|
||||||
|
//判断最佳关系
|
||||||
|
if ((link_1.RB_link >= 0) && (dist > link_1.RB_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if ((link_2.LT_link >= 0) && (dist > link_2.LT_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if (true == currIsBest)
|
||||||
|
{
|
||||||
|
if (link_1.RB_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_1.RB_link].LT_link = -1;
|
||||||
|
nodeLinks[link_1.RB_link].LT_dist = 0;
|
||||||
|
}
|
||||||
|
link_1.RB_link = j;
|
||||||
|
link_1.RB_dist = dist;
|
||||||
|
if (link_2.LT_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_2.LT_link].RB_link = -1;
|
||||||
|
nodeLinks[link_2.LT_link].RB_dist = 0;
|
||||||
|
}
|
||||||
|
link_2.LT_link = i;
|
||||||
|
link_2.LT_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
link_1.RB_link = j;
|
||||||
|
link_1.RB_dist = dist;
|
||||||
|
link_2.LT_link = i;
|
||||||
|
link_2.LT_dist = dist;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else//bottom
|
||||||
|
{
|
||||||
|
//tree1:RT link is tree_2
|
||||||
|
//tree2:LB link is tree_1
|
||||||
|
if ((link_1.RT_link >= 0) || (link_2.LB_link >= 0))
|
||||||
|
{
|
||||||
|
bool currIsBest = true;
|
||||||
|
//判断最佳关系
|
||||||
|
if ((link_1.RT_link >= 0) && (dist > link_1.RT_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if ((link_2.LB_link >= 0) && (dist > link_2.LB_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if (true == currIsBest)
|
||||||
|
{
|
||||||
|
if (link_1.RT_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_1.RT_link].LB_link = -1;
|
||||||
|
nodeLinks[link_1.RT_link].LB_dist = 0;
|
||||||
|
}
|
||||||
|
link_1.RT_link = j;
|
||||||
|
link_1.RT_dist = dist;
|
||||||
|
if (link_2.LB_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_2.LB_link].RT_link = -1;
|
||||||
|
nodeLinks[link_2.LB_link].RT_dist = 0;
|
||||||
|
}
|
||||||
|
link_2.LB_link = i;
|
||||||
|
link_2.LB_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
link_1.RT_link = j;
|
||||||
|
link_1.RT_dist = dist;
|
||||||
|
link_2.LB_link = i;
|
||||||
|
link_2.LB_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else //right
|
||||||
|
{
|
||||||
|
if (tree_1.centerPt.y < tree_2.centerPt.y) //top
|
||||||
|
{
|
||||||
|
//tree1:LB link is tree_2
|
||||||
|
//tree2:RT link is tree_1
|
||||||
|
if ((link_1.LB_link >= 0) || (link_2.RT_link >= 0))
|
||||||
|
{
|
||||||
|
bool currIsBest = true;
|
||||||
|
//判断最佳关系
|
||||||
|
if ((link_1.LB_link >= 0) && (dist > link_1.LB_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if ((link_2.RT_link >= 0) && (dist > link_2.RT_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if (true == currIsBest)
|
||||||
|
{
|
||||||
|
if (link_1.LB_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_1.LB_link].RT_link = -1;
|
||||||
|
nodeLinks[link_1.LB_link].RT_dist = 0;
|
||||||
|
}
|
||||||
|
link_1.LB_link = j;
|
||||||
|
link_1.LB_dist = dist;
|
||||||
|
if (link_2.RT_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_2.RT_link].LB_link = -1;
|
||||||
|
nodeLinks[link_2.RT_link].LB_dist = 0;
|
||||||
|
}
|
||||||
|
link_2.RT_link = i;
|
||||||
|
link_2.RT_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
link_1.LB_link = j;
|
||||||
|
link_1.LB_dist = dist;
|
||||||
|
link_2.RT_link = i;
|
||||||
|
link_2.RT_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else //bottom
|
||||||
|
{
|
||||||
|
//tree1:LT link is tree_2
|
||||||
|
//tree2:RB link is tree_1
|
||||||
|
if ((link_1.LT_link >= 0) || (link_2.RB_link >= 0))
|
||||||
|
{
|
||||||
|
bool currIsBest = true;
|
||||||
|
//判断最佳关系
|
||||||
|
if ((link_1.LT_link >= 0) && (dist > link_1.LT_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if ((link_2.RB_link >= 0) && (dist > link_2.RB_dist))
|
||||||
|
currIsBest = false;
|
||||||
|
if (true == currIsBest)
|
||||||
|
{
|
||||||
|
if (link_1.LT_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_1.LT_link].RB_link = -1;
|
||||||
|
nodeLinks[link_1.LT_link].RB_dist = 0;
|
||||||
|
}
|
||||||
|
link_1.LT_link = j;
|
||||||
|
link_1.LT_dist = dist;
|
||||||
|
if (link_2.RB_link >= 0)
|
||||||
|
{
|
||||||
|
nodeLinks[link_2.RB_link].LT_link = -1;
|
||||||
|
nodeLinks[link_2.RB_link].LT_dist = 0;
|
||||||
|
}
|
||||||
|
link_2.RB_link = i;
|
||||||
|
link_2.RB_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
link_1.LT_link = j;
|
||||||
|
link_1.LT_dist = dist;
|
||||||
|
link_2.RB_link = i;
|
||||||
|
link_2.RB_dist = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//（2）建立节点间的相互关系
|
||||||
|
std::vector< int> nodeSortingLineID(stopTrees.size(), -1 ); //排序后的行位置
|
||||||
|
std::vector< int> nodeProcFlag(stopTrees.size(), 0); //已经处理标志，”1“已经处理
|
||||||
|
std::vector<std::vector<int>> hSorting;
|
||||||
|
hSorting.resize(stopTrees.size());
|
||||||
|
int startSortingLine = -1; //第一个检查的Node使用最中间的排序行，这样可以向上和向下添加行
|
||||||
|
int gMaxLineId = 0;
|
||||||
|
for (int y = 0; y < rows; y++)
|
||||||
|
{
|
||||||
|
for (int x = 0; x < cols; x++)
|
||||||
|
{
|
||||||
|
int nodeId = objMask.at<int>(y, x);
|
||||||
|
if (nodeId >= 0)
|
||||||
|
{
|
||||||
|
if (nodeProcFlag[nodeId] > 0)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
std::vector<int> processBuff;
|
||||||
|
processBuff.push_back(nodeId);
|
||||||
|
if (startSortingLine < 0)
|
||||||
|
startSortingLine = 0;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
//确定行序号
|
||||||
|
startSortingLine = 0; //TBD
|
||||||
|
|
||||||
|
}
|
||||||
|
nodeSortingLineID[nodeId] = startSortingLine;
|
||||||
|
int maxLineId = startSortingLine;
|
||||||
|
_recursiveSorting(
|
||||||
|
nodeId,
|
||||||
|
hSorting,
|
||||||
|
nodeProcFlag,
|
||||||
|
nodeSortingLineID,
|
||||||
|
nodeLinks,
|
||||||
|
true,
|
||||||
|
&maxLineId);
|
||||||
|
if (gMaxLineId < maxLineId)
|
||||||
|
gMaxLineId = maxLineId;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//计算行Node的距离均值
|
||||||
|
double lineNodeDist = 0;
|
||||||
|
int lineNodeDistNum = 0;
|
||||||
|
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
||||||
|
{
|
||||||
|
NodeQuadLink& link_1 = nodeLinks[i];
|
||||||
|
if (link_1.LB_link >= 0)
|
||||||
|
{
|
||||||
|
if (nodeLinks[link_1.LB_link].LT_link >= 0)
|
||||||
|
{
|
||||||
|
int id2 = nodeLinks[link_1.LB_link].LT_link;
|
||||||
|
double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
|
||||||
|
lineNodeDist += dist;
|
||||||
|
lineNodeDistNum++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (link_1.LT_link >= 0)
|
||||||
|
{
|
||||||
|
if (nodeLinks[link_1.LT_link].LB_link >= 0)
|
||||||
|
{
|
||||||
|
int id2 = nodeLinks[link_1.LT_link].LB_link;
|
||||||
|
double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
|
||||||
|
lineNodeDist += dist;
|
||||||
|
lineNodeDistNum++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (link_1.RB_link >= 0)
|
||||||
|
{
|
||||||
|
if (nodeLinks[link_1.RB_link].RT_link >= 0)
|
||||||
|
{
|
||||||
|
int id2 = nodeLinks[link_1.RB_link].RT_link;
|
||||||
|
double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
|
||||||
|
lineNodeDist += dist;
|
||||||
|
lineNodeDistNum++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (link_1.RT_link >= 0)
|
||||||
|
{
|
||||||
|
if (nodeLinks[link_1.RT_link].RB_link >= 0)
|
||||||
|
{
|
||||||
|
int id2 = nodeLinks[link_1.RT_link].RB_link;
|
||||||
|
double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
|
||||||
|
lineNodeDist += dist;
|
||||||
|
lineNodeDistNum++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (lineNodeDistNum > 0)
|
||||||
|
lineNodeDist = lineNodeDist / lineNodeDistNum;
|
||||||
|
|
||||||
|
//生成目标坐标点,遗漏点检查
|
||||||
|
std::vector<std::vector<SVzNL3DPosition>> sortingNodes;
|
||||||
|
for (int i = 0; i <= gMaxLineId; i++)
|
||||||
|
{
|
||||||
|
std::vector<int>& a_sortingLine = hSorting[i];
|
||||||
|
std::vector<SVzNL3DPosition> sortingNodes_line;
|
||||||
|
for (int j = 0, j_max = a_sortingLine.size(); j < j_max; j++)
|
||||||
|
{
|
||||||
|
int nodeId = a_sortingLine[j];
|
||||||
|
SVzNL3DPosition a_pt;
|
||||||
|
a_pt.nPointIdx = nodeId;
|
||||||
|
a_pt.pt3D = stopTrees[nodeId].centerPt;
|
||||||
|
sortingNodes_line.push_back(a_pt);
|
||||||
|
}
|
||||||
//遗漏点检查
|
//遗漏点检查
|
||||||
|
_lineFillNullNode( sortingNodes_line, nodeLinks, lineNodeDist );
|
||||||
|
sortingNodes.push_back(sortingNodes_line);
|
||||||
|
}
|
||||||
|
|
||||||
//迭代生长
|
//结果数据第一行和最后一行如果不完整，不使用
|
||||||
|
if (sortingNodes.size() > 3)
|
||||||
|
{
|
||||||
|
if (sortingNodes[0].size() < sortingNodes[2].size())
|
||||||
|
sortingNodes.erase(sortingNodes.begin());
|
||||||
|
}
|
||||||
|
|
||||||
|
//输出
|
||||||
|
for (int i = 0, i_max = sortingNodes.size(); i < i_max; i++)
|
||||||
|
{
|
||||||
|
std::vector<SVzNL3DPosition>& sortingNodes_line = sortingNodes[i];
|
||||||
|
if (sortingNodes_line.size() == 0)
|
||||||
|
break;
|
||||||
|
std::vector<SVzNL3DPoint> resultLine;
|
||||||
|
for (int j = 0, j_max = sortingNodes_line.size(); j < j_max; j++)
|
||||||
|
resultLine.push_back(sortingNodes_line[j].pt3D);
|
||||||
|
nodePos.push_back(resultLine);
|
||||||
|
}
|
||||||
#if _OUTPUT_LINE_PROC_RESULT
|
#if _OUTPUT_LINE_PROC_RESULT
|
||||||
//输出扫描线处理结果
|
//输出扫描线处理结果
|
||||||
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
|
||||||
@ -309,13 +774,14 @@ void sg_getSieveNodes(
|
|||||||
{
|
{
|
||||||
SSG_featureSemiCircle& a_feature = a_tree_features[j];
|
SSG_featureSemiCircle& a_feature = a_tree_features[j];
|
||||||
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
|
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
|
||||||
laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 1; //此处nPointIdx转义
|
laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 0; //1; //此处nPointIdx转义
|
||||||
|
#if 0
|
||||||
laser3DPoints[a_feature.lineIdx].p3DPosition[a_feature.midPtIdx].nPointIdx = 2;
|
laser3DPoints[a_feature.lineIdx].p3DPosition[a_feature.midPtIdx].nPointIdx = 2;
|
||||||
if(stopTrees[i].treeType == 0)
|
if(stopTrees[i].treeType == 0)
|
||||||
laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 3;
|
laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 3;
|
||||||
else
|
else
|
||||||
laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 4;
|
laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 4;
|
||||||
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user