141 lines
3.9 KiB
C++
141 lines
3.9 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_pointClustering2D(
|
|
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();
|
|
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];
|
|
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 == 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;
|
|
|
|
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;
|
|
} |