349 lines
11 KiB
C++
349 lines
11 KiB
C++
#include "SG_baseDataType.h"
|
|
#include "SG_baseAlgo_Export.h"
|
|
#include <vector>
|
|
#ifdef _WIN32
|
|
#include <corecrt_math_defines.h>
|
|
#endif
|
|
#include <cmath>
|
|
|
|
void _seedClustering(
|
|
std::vector< SVzNL3DPosition>& a_cluster,
|
|
std::vector< SVzNL3DPosition>& pts,
|
|
double clusterDist)
|
|
{
|
|
int i = 0;
|
|
while (1)
|
|
{
|
|
if (i >= a_cluster.size())
|
|
break;
|
|
|
|
SVzNL3DPosition a_seed = a_cluster[i];
|
|
for (int i = 0, i_max = (int)pts.size(); i < i_max; i++)
|
|
{
|
|
if (pts[i].nPointIdx < 0)
|
|
continue;
|
|
double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
|
|
if (dist < clusterDist)
|
|
{
|
|
a_cluster.push_back(pts[i]);
|
|
pts[i].nPointIdx = -1;
|
|
}
|
|
}
|
|
i++;
|
|
}
|
|
return;
|
|
}
|
|
|
|
void sg_pointClustering(
|
|
std::vector< SVzNL3DPosition>& pts,
|
|
double clusterDist,
|
|
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
|
)
|
|
{
|
|
int ptSize = (int)pts.size();
|
|
if (ptSize == 0)
|
|
return;
|
|
while(pts.size() > 0)
|
|
{
|
|
SVzNL3DPosition a_pt = pts[0];
|
|
|
|
//新建一个cluster
|
|
std::vector< SVzNL3DPosition> a_cluster;
|
|
a_cluster.push_back(a_pt);
|
|
pts[0].nPointIdx = -1; //防止重复被计算
|
|
_seedClustering(
|
|
a_cluster,
|
|
pts,
|
|
clusterDist);
|
|
objClusters.push_back(a_cluster); //保存一个聚类
|
|
//将pts中处理过的点去除
|
|
int m_max = (int)pts.size();
|
|
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
|
{
|
|
if(pts[m].nPointIdx < 0)
|
|
pts.erase(pts.begin() + m);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
//对特征点的聚类
|
|
void wd_gridPointClustering(
|
|
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
|
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
|
int clusterCheckWin, //搜索窗口
|
|
SSG_treeGrowParam growParam,//聚类条件
|
|
int clusterID, //当前Cluster的ID
|
|
std::vector< SVzNL2DPoint>& a_cluster, //result
|
|
SVzNL3DRangeD& clusterRoi //roi3D
|
|
)
|
|
{
|
|
int i = 0;
|
|
int lineNum = (int)featureMask.size();
|
|
int linePtNum = (int)featureMask[0].size();
|
|
featureMask[a_cluster[0].x][a_cluster[0].y].flag = 1; //防止第一个被重复添加
|
|
while (1)
|
|
{
|
|
if (i >= a_cluster.size())
|
|
break;
|
|
|
|
SVzNL2DPoint a_seedPos = a_cluster[i];
|
|
if ((a_seedPos.x == 752) && (a_seedPos.y == 2399))
|
|
int kkk = 1;
|
|
|
|
SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
|
|
SVzNL3DPoint& seedValue = feature3DInfo[a_seedPos.x][a_seedPos.y];
|
|
if (0 == a_seed.clusterID) //clusterID == 0, 未被处理，搜索邻域
|
|
{
|
|
for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
|
|
{
|
|
for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
|
|
{
|
|
int y = j + a_seedPos.y;
|
|
int x = i + a_seedPos.x;
|
|
if ((x == 645) && (y == 2240))
|
|
int kkk = 1;
|
|
|
|
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
|
|
{
|
|
SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
|
|
if ((chk_seed.featurType ==0) || (chk_seed.clusterID > 0)) //只检查未聚类的特征点
|
|
continue;
|
|
|
|
SVzNL3DPoint& chkValue = feature3DInfo[x][y];
|
|
double y_diff = abs(seedValue.y - chkValue.y);
|
|
double z_diff = abs(seedValue.z - chkValue.z);
|
|
double x_diff = abs(seedValue.x - chkValue.x);
|
|
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
|
(x_diff < growParam.maxSkipDistance))
|
|
{
|
|
if (0 == chk_seed.flag)//防止被重复添加
|
|
{
|
|
chk_seed.flag = 1;
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
a_cluster.push_back(new_seed);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
a_seed.clusterID = clusterID;
|
|
//更新ROI
|
|
clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.x ? seedValue.x : clusterRoi.xRange.min;
|
|
clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.x ? seedValue.x : clusterRoi.xRange.max;
|
|
clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.y ? seedValue.y : clusterRoi.yRange.min;
|
|
clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.y ? seedValue.y : clusterRoi.yRange.max;
|
|
clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.z ? seedValue.z : clusterRoi.zRange.min;
|
|
clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.z ? seedValue.z : clusterRoi.zRange.max;
|
|
i++;
|
|
}
|
|
return;
|
|
}
|
|
|
|
//对扫描数据的聚类
|
|
//SSG_intPair成员变量定义:
|
|
// data_0: flag
|
|
// data_1: valid point flag
|
|
// idx: cluster ID
|
|
void wd_gridPointClustering_2(
|
|
std::vector<std::vector<SVzNL3DPosition>>& gridData,
|
|
std::vector<std::vector<SSG_intPair>>& pointMaskInfo, //防止聚类时重复选中
|
|
int clusterCheckWin, //搜索窗口
|
|
SSG_treeGrowParam growParam,//聚类条件
|
|
int clusterID, //当前Cluster的ID
|
|
std::vector< SVzNL2DPoint>& a_cluster, //result
|
|
SVzNL3DRangeD& clusterRoi //roi3D
|
|
)
|
|
{
|
|
int i = 0;
|
|
int lineNum = (int)gridData.size();
|
|
int linePtNum = (int)gridData[0].size();
|
|
pointMaskInfo[a_cluster[0].x][a_cluster[0].y].data_0 = 1; //防止第一个被重复添加
|
|
while (1)
|
|
{
|
|
if (i >= a_cluster.size())
|
|
break;
|
|
|
|
SVzNL2DPoint a_seedPos = a_cluster[i];
|
|
if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
|
|
int kkk = 1;
|
|
|
|
SSG_intPair& a_seed = pointMaskInfo[a_seedPos.x][a_seedPos.y];
|
|
SVzNL3DPosition& seedValue = gridData[a_seedPos.x][a_seedPos.y];
|
|
if (0 == a_seed.idx) //clusterID == 0, 未被处理，搜索邻域
|
|
{
|
|
for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
|
|
{
|
|
for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
|
|
{
|
|
int y = j + a_seedPos.y;
|
|
int x = i + a_seedPos.x;
|
|
if ((x == 390) && (y == 949))
|
|
int kkk = 1;
|
|
|
|
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
|
|
{
|
|
SSG_intPair& chk_seed = pointMaskInfo[x][y];
|
|
if ((chk_seed.data_1 == 0) || (chk_seed.idx > 0)) //idx:clusterID, data_1:valid point
|
|
continue;
|
|
|
|
SVzNL3DPosition& chkValue = gridData[x][y];
|
|
double y_diff = abs(seedValue.pt3D.y - chkValue.pt3D.y);
|
|
double z_diff = abs(seedValue.pt3D.z - chkValue.pt3D.z);
|
|
double x_diff = abs(seedValue.pt3D.x - chkValue.pt3D.x);
|
|
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
|
(x_diff < growParam.maxSkipDistance))
|
|
{
|
|
if (0 == chk_seed.data_0)//防止被重复添加
|
|
{
|
|
chk_seed.data_0 = 1;
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
a_cluster.push_back(new_seed);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
a_seed.idx = clusterID;
|
|
//更新ROI
|
|
clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.min;
|
|
clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.max;
|
|
clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.min;
|
|
clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.max;
|
|
clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.min;
|
|
clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.max;
|
|
i++;
|
|
}
|
|
return;
|
|
}
|
|
|
|
//使用聚类方法完成8连通连通域分析
|
|
void wd_gridPointClustering_labelling(
|
|
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
|
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
|
int clusterID, //当前Cluster的ID
|
|
std::vector< SVzNL2DPoint>& a_cluster, //result
|
|
SVzNLRect& clusterRoi //roi2D
|
|
)
|
|
{
|
|
int i = 0;
|
|
int lineNum = (int)featureMask.size();
|
|
int linePtNum = (int)featureMask[0].size();
|
|
while (1)
|
|
{
|
|
if (i >= a_cluster.size())
|
|
break;
|
|
|
|
SVzNL2DPoint a_seedPos = a_cluster[i];
|
|
if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
|
|
int kkk = 1;
|
|
|
|
SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
|
|
if (0 == a_seed.clusterID) //clusterID == 0, 未被处理，搜索邻域
|
|
{
|
|
//8连通
|
|
for (int i = -1; i <= 1; i++)
|
|
{
|
|
for (int j = -1; j <= 1; j++)
|
|
{
|
|
int y = j + a_seedPos.y;
|
|
int x = i + a_seedPos.x;
|
|
if ((x == 390) && (y == 949))
|
|
int kkk = 1;
|
|
|
|
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
|
|
{
|
|
SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
|
|
if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0)) //只检查未聚类的特征点
|
|
continue;
|
|
|
|
if (0 == chk_seed.flag)//防止被重复添加
|
|
{
|
|
chk_seed.flag = 1;
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
a_cluster.push_back(new_seed);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
a_seed.clusterID = clusterID;
|
|
//更新ROI
|
|
clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left;
|
|
clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right;
|
|
clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top;
|
|
clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom;
|
|
i++;
|
|
}
|
|
return;
|
|
}
|
|
|
|
//使用聚类方法完成8连通连通域分析
|
|
void wd_gridPointClustering_labelling_2(
|
|
std::vector<std::vector<SVzNL3DPoint>>& srcData,
|
|
std::vector<std::vector<SSG_clusterLabel>>& labelMask,
|
|
int clusterID, //当前Cluster的ID
|
|
std::vector< SVzNL2DPoint>& a_cluster, //result
|
|
SVzNL3DRangeD& clusterRoi //roi3D
|
|
)
|
|
{
|
|
int i = 0;
|
|
int lineNum = (int)srcData.size();
|
|
int linePtNum = (int)srcData[0].size();
|
|
while (1)
|
|
{
|
|
if (i >= a_cluster.size())
|
|
break;
|
|
|
|
SVzNL2DPoint a_seedPos = a_cluster[i];
|
|
if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
|
|
int kkk = 1;
|
|
|
|
SSG_clusterLabel& a_seed = labelMask[a_seedPos.x][a_seedPos.y];
|
|
SVzNL3DPoint& a_feature3DValue = srcData[a_seedPos.x][a_seedPos.y];
|
|
if (0 == a_seed.clusterID) //clusterID == 0, 未被处理，搜索邻域
|
|
{
|
|
//8连通
|
|
for (int i = -1; i <= 1; i++)
|
|
{
|
|
for (int j = -1; j <= 1; j++)
|
|
{
|
|
int y = j + a_seedPos.y;
|
|
int x = i + a_seedPos.x;
|
|
if ((x == 390) && (y == 949))
|
|
int kkk = 1;
|
|
|
|
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
|
|
{
|
|
SSG_clusterLabel& chk_seed = labelMask[x][y];
|
|
if ((chk_seed.validFlag == 0) || (chk_seed.clusterID > 0)) //只检查未聚类的特征点
|
|
continue;
|
|
|
|
if (0 == chk_seed.flag)//防止被重复添加
|
|
{
|
|
chk_seed.flag = 1;
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
a_cluster.push_back(new_seed);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
a_seed.clusterID = clusterID;
|
|
//更新ROI
|
|
clusterRoi.xRange.min = clusterRoi.xRange.min > a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.min;
|
|
clusterRoi.xRange.max = clusterRoi.xRange.max < a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.max;
|
|
clusterRoi.yRange.min = clusterRoi.yRange.min > a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.min;
|
|
clusterRoi.yRange.max = clusterRoi.yRange.max < a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.max;
|
|
clusterRoi.zRange.min = clusterRoi.zRange.min > a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.min;
|
|
clusterRoi.zRange.max = clusterRoi.zRange.max < a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.max;
|
|
i++;
|
|
}
|
|
return;
|
|
} |