2025-07-15 21:37:09 +08:00
|
|
|
|
#include "SG_baseDataType.h"
|
|
|
|
|
|
#include "SG_baseAlgo_Export.h"
|
|
|
|
|
|
#include <vector>
|
2025-11-10 22:44:31 +08:00
|
|
|
|
#ifdef _WIN32
|
2025-07-15 21:37:09 +08:00
|
|
|
|
#include <corecrt_math_defines.h>
|
2025-11-10 22:44:31 +08:00
|
|
|
|
#endif
|
2025-07-15 21:37:09 +08:00
|
|
|
|
#include <cmath>
|
|
|
|
|
|
|
|
|
|
|
|
void _seedClustering(
|
|
|
|
|
|
std::vector< SVzNL3DPosition>& a_cluster,
|
|
|
|
|
|
std::vector< SVzNL3DPosition>& pts,
|
2026-03-19 00:43:39 +08:00
|
|
|
|
std::vector<int> flags,
|
2025-07-15 21:37:09 +08:00
|
|
|
|
double clusterDist)
|
|
|
|
|
|
{
|
|
|
|
|
|
int i = 0;
|
|
|
|
|
|
while (1)
|
|
|
|
|
|
{
|
|
|
|
|
|
if (i >= a_cluster.size())
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
|
|
SVzNL3DPosition a_seed = a_cluster[i];
|
2026-03-19 00:43:39 +08:00
|
|
|
|
for (int j = 0, j_max = (int)pts.size(); j < j_max; j++)
|
2025-07-15 21:37:09 +08:00
|
|
|
|
{
|
2026-03-19 00:43:39 +08:00
|
|
|
|
if (flags[j] < 0)
|
2025-07-15 21:37:09 +08:00
|
|
|
|
continue;
|
2026-03-19 00:43:39 +08:00
|
|
|
|
double dist_x = abs(a_seed.pt3D.x - pts[j].pt3D.x);
|
|
|
|
|
|
double dist_y = abs(a_seed.pt3D.y - pts[j].pt3D.y);
|
|
|
|
|
|
if ( (dist_x < clusterDist) && (dist_y < clusterDist))
|
2025-07-15 21:37:09 +08:00
|
|
|
|
{
|
2026-03-19 00:43:39 +08:00
|
|
|
|
a_cluster.push_back(pts[j]);
|
|
|
|
|
|
flags[j] = -1;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
i++;
|
|
|
|
|
|
}
|
|
|
|
|
|
return;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void _seedClustering_speedUp(
|
|
|
|
|
|
std::vector< SVzNL3DPosition>& a_cluster,
|
|
|
|
|
|
std::vector< SVzNL3DPosition>& pts,
|
|
|
|
|
|
std::vector<int>& flags,
|
|
|
|
|
|
double clusterDist,
|
2026-04-14 15:04:46 +08:00
|
|
|
|
int distType, //0 - 2d distance; 1- 3d distance
|
2026-03-19 00:43:39 +08:00
|
|
|
|
std::vector<std::vector<int>>& indexing2D,
|
|
|
|
|
|
int lineNum, int ptSize, int clusterCheckWin) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
int i = 0;
|
|
|
|
|
|
while (1)
|
|
|
|
|
|
{
|
|
|
|
|
|
if (i >= a_cluster.size())
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
|
|
SVzNL3DPosition a_seed = a_cluster[i];
|
|
|
|
|
|
int seed_line = a_seed.nPointIdx >> 16;
|
|
|
|
|
|
int seed_ptIdx =a_seed.nPointIdx & 0x0000FFFF;
|
|
|
|
|
|
for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
|
|
|
|
|
|
{
|
|
|
|
|
|
if ((line >= 0) && (line < lineNum))
|
|
|
|
|
|
{
|
|
|
|
|
|
for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
|
|
|
|
|
|
{
|
|
|
|
|
|
if ((ptIdx >= 0) && (ptIdx < ptSize))
|
|
|
|
|
|
{
|
|
|
|
|
|
int backIdx = indexing2D[line][ptIdx];
|
|
|
|
|
|
if (backIdx < 0)
|
|
|
|
|
|
continue;
|
|
|
|
|
|
if (flags[backIdx] < 0)
|
|
|
|
|
|
continue;
|
2026-04-14 15:04:46 +08:00
|
|
|
|
double dist;
|
|
|
|
|
|
if(0 == distType)
|
|
|
|
|
|
dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y, 2));
|
|
|
|
|
|
else
|
|
|
|
|
|
dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y, 2) + pow(a_seed.pt3D.z - pts[backIdx].pt3D.z, 2));
|
2026-03-19 00:43:39 +08:00
|
|
|
|
if (dist < clusterDist)
|
|
|
|
|
|
{
|
|
|
|
|
|
a_cluster.push_back(pts[backIdx]);
|
|
|
|
|
|
flags[backIdx] = -1;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
2025-07-15 21:37:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
i++;
|
|
|
|
|
|
}
|
|
|
|
|
|
return;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void sg_pointClustering(
|
|
|
|
|
|
std::vector< SVzNL3DPosition>& pts,
|
|
|
|
|
|
double clusterDist,
|
|
|
|
|
|
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
|
|
|
|
|
)
|
|
|
|
|
|
{
|
2025-09-14 21:15:44 +08:00
|
|
|
|
int ptSize = (int)pts.size();
|
2025-07-15 21:37:09 +08:00
|
|
|
|
if (ptSize == 0)
|
|
|
|
|
|
return;
|
2026-03-19 00:43:39 +08:00
|
|
|
|
|
|
|
|
|
|
std::vector<int> flags;
|
|
|
|
|
|
flags.resize(ptSize);
|
|
|
|
|
|
std::fill(flags.begin(), flags.end(), 0);
|
|
|
|
|
|
|
2025-07-15 21:37:09 +08:00
|
|
|
|
while(pts.size() > 0)
|
|
|
|
|
|
{
|
|
|
|
|
|
SVzNL3DPosition a_pt = pts[0];
|
2026-03-19 00:43:39 +08:00
|
|
|
|
flags[0] = -1;//<2F><>ֹ<EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2025-07-15 21:37:09 +08:00
|
|
|
|
//<2F>½<EFBFBD>һ<EFBFBD><D2BB>cluster
|
|
|
|
|
|
std::vector< SVzNL3DPosition> a_cluster;
|
|
|
|
|
|
a_cluster.push_back(a_pt);
|
2026-03-19 00:43:39 +08:00
|
|
|
|
//pts[0].nPointIdx = -1;
|
2025-07-15 21:37:09 +08:00
|
|
|
|
_seedClustering(
|
|
|
|
|
|
a_cluster,
|
|
|
|
|
|
pts,
|
2026-03-19 00:43:39 +08:00
|
|
|
|
flags,
|
2025-07-15 21:37:09 +08:00
|
|
|
|
clusterDist);
|
|
|
|
|
|
objClusters.push_back(a_cluster); //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
//<2F><>pts<74>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ȥ<EFBFBD><C8A5>
|
2025-09-14 21:15:44 +08:00
|
|
|
|
int m_max = (int)pts.size();
|
2025-07-15 21:37:09 +08:00
|
|
|
|
for (int m = m_max - 1; m >= 0; m--) //<2F>Ӻ<EFBFBD><D3BA><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɾ<EFBFBD><C9BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>ѭ<EFBFBD><D1AD>
|
|
|
|
|
|
{
|
2026-03-19 00:43:39 +08:00
|
|
|
|
if (flags[m] < 0)
|
|
|
|
|
|
{
|
2025-07-15 21:37:09 +08:00
|
|
|
|
pts.erase(pts.begin() + m);
|
2026-03-19 00:43:39 +08:00
|
|
|
|
flags.erase(flags.begin() + m);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
return;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//ʹ<><CAB9>SVzNL3DPosition<6F><6E>nPointIdx<64><78>ʾ2D<32><44>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>16λLine<6E><65> <20><>16λptIdx<64><78>
|
|
|
|
|
|
//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>
|
|
|
|
|
|
void wd_pointClustering_speedUp(
|
|
|
|
|
|
std::vector< SVzNL3DPosition>& pts,
|
|
|
|
|
|
int lineNum, int linePtSize, int clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double clusterDist,
|
2026-04-14 15:04:46 +08:00
|
|
|
|
int distType, //0 - 2d distance; 1- 3d distance
|
2026-03-19 00:43:39 +08:00
|
|
|
|
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
|
|
|
|
|
)
|
|
|
|
|
|
{
|
|
|
|
|
|
std::vector<std::vector<int>> indexing2D;
|
|
|
|
|
|
indexing2D.resize(lineNum);
|
|
|
|
|
|
for (int i = 0; i < lineNum; i++)
|
|
|
|
|
|
{
|
|
|
|
|
|
indexing2D[i].resize(linePtSize);
|
|
|
|
|
|
std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
|
|
|
|
|
|
}
|
|
|
|
|
|
for (int i = 0; i < (int)pts.size(); i++)
|
|
|
|
|
|
{
|
|
|
|
|
|
int line = pts[i].nPointIdx >> 16;
|
|
|
|
|
|
int ptIdx = pts[i].nPointIdx & 0x0000FFFF;
|
|
|
|
|
|
indexing2D[line][ptIdx] = i;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int ptSize = (int)pts.size();
|
|
|
|
|
|
if (ptSize == 0)
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
|
|
|
|
std::vector<int> flags;
|
|
|
|
|
|
flags.resize(ptSize);
|
|
|
|
|
|
std::fill(flags.begin(), flags.end(), 0);
|
|
|
|
|
|
|
|
|
|
|
|
int idx = 0;
|
|
|
|
|
|
while (idx < (int)pts.size())
|
|
|
|
|
|
{
|
|
|
|
|
|
SVzNL3DPosition a_pt = pts[idx];
|
|
|
|
|
|
if (flags[idx] >= 0)
|
|
|
|
|
|
{
|
|
|
|
|
|
flags[idx] = -1;//<2F><>ֹ<EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
//<2F>½<EFBFBD>һ<EFBFBD><D2BB>cluster
|
|
|
|
|
|
std::vector< SVzNL3DPosition> a_cluster;
|
|
|
|
|
|
a_cluster.push_back(a_pt);
|
|
|
|
|
|
//pts[0].nPointIdx = -1;
|
|
|
|
|
|
_seedClustering_speedUp(
|
|
|
|
|
|
a_cluster,
|
|
|
|
|
|
pts,
|
|
|
|
|
|
flags,
|
|
|
|
|
|
clusterDist,
|
2026-04-14 15:04:46 +08:00
|
|
|
|
distType,
|
2026-03-19 00:43:39 +08:00
|
|
|
|
indexing2D,
|
|
|
|
|
|
lineNum, linePtSize, clusterCheckWin);
|
2026-04-14 15:04:46 +08:00
|
|
|
|
if(a_cluster.size() >= 5)
|
|
|
|
|
|
objClusters.push_back(a_cluster); //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2025-07-15 21:37:09 +08:00
|
|
|
|
}
|
2026-03-19 00:43:39 +08:00
|
|
|
|
idx++;
|
2025-07-15 21:37:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
return;
|
|
|
|
|
|
}
|
2025-12-16 21:00:17 +08:00
|
|
|
|
|
2026-02-02 07:56:51 +08:00
|
|
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
|
2025-12-30 21:22:41 +08:00
|
|
|
|
void wd_gridPointClustering(
|
2025-12-16 21:00:17 +08:00
|
|
|
|
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
|
|
|
|
|
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
|
|
|
|
|
int clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
SSG_treeGrowParam growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
int clusterID, //<2F><>ǰCluster<65><72>ID
|
|
|
|
|
|
std::vector< SVzNL2DPoint>& a_cluster, //result
|
|
|
|
|
|
SVzNL3DRangeD& clusterRoi //roi3D
|
|
|
|
|
|
)
|
|
|
|
|
|
{
|
|
|
|
|
|
int i = 0;
|
|
|
|
|
|
int lineNum = (int)featureMask.size();
|
|
|
|
|
|
int linePtNum = (int)featureMask[0].size();
|
2026-01-26 15:57:19 +08:00
|
|
|
|
featureMask[a_cluster[0].x][a_cluster[0].y].flag = 1; //<2F><>ֹ<EFBFBD><D6B9>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
|
2025-12-16 21:00:17 +08:00
|
|
|
|
while (1)
|
|
|
|
|
|
{
|
|
|
|
|
|
if (i >= a_cluster.size())
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
|
|
SVzNL2DPoint a_seedPos = a_cluster[i];
|
2026-02-09 00:51:35 +08:00
|
|
|
|
if ((a_seedPos.x == 752) && (a_seedPos.y == 2399))
|
2025-12-16 21:00:17 +08:00
|
|
|
|
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, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
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;
|
2026-02-09 00:51:35 +08:00
|
|
|
|
if ((x == 645) && (y == 2240))
|
2025-12-16 21:00:17 +08:00
|
|
|
|
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)) //ֻ<><D6BB><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
continue;
|
|
|
|
|
|
|
2025-12-30 21:22:41 +08:00
|
|
|
|
SVzNL3DPoint& chkValue = feature3DInfo[x][y];
|
2025-12-16 21:00:17 +08:00
|
|
|
|
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)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
chk_seed.flag = 1;
|
|
|
|
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
|
|
|
|
a_cluster.push_back(new_seed);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
a_seed.clusterID = clusterID;
|
|
|
|
|
|
//<2F><><EFBFBD><EFBFBD>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;
|
2025-12-30 21:22:41 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
2026-02-02 07:56:51 +08:00
|
|
|
|
//<2F><>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD>ݵľ<DDB5><C4BE><EFBFBD>
|
|
|
|
|
|
//SSG_intPair<69><72>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|
|
|
|
|
// 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, //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ʱ<EFBFBD>ظ<EFBFBD>ѡ<EFBFBD><D1A1>
|
|
|
|
|
|
int clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
SSG_treeGrowParam growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
int clusterID, //<2F><>ǰCluster<65><72>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; //<2F><>ֹ<EFBFBD><D6B9>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
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, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
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)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
chk_seed.data_0 = 1;
|
|
|
|
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
|
|
|
|
a_cluster.push_back(new_seed);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
a_seed.idx = clusterID;
|
|
|
|
|
|
//<2F><><EFBFBD><EFBFBD>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;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
2025-12-30 21:22:41 +08:00
|
|
|
|
//ʹ<>þ<EFBFBD><C3BE><EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD><38>ͨ<EFBFBD><CDA8>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
void wd_gridPointClustering_labelling(
|
|
|
|
|
|
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
|
|
|
|
|
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
|
|
|
|
|
int clusterID, //<2F><>ǰCluster<65><72>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, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
//8<><38>ͨ
|
|
|
|
|
|
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)) //ֻ<><D6BB><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
continue;
|
|
|
|
|
|
|
|
|
|
|
|
if (0 == chk_seed.flag)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
chk_seed.flag = 1;
|
|
|
|
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
|
|
|
|
a_cluster.push_back(new_seed);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
a_seed.clusterID = clusterID;
|
|
|
|
|
|
//<2F><><EFBFBD><EFBFBD>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;
|
2026-02-02 07:56:51 +08:00
|
|
|
|
}
|
2026-02-08 22:32:36 +08:00
|
|
|
|
|
2026-02-02 07:56:51 +08:00
|
|
|
|
//ʹ<>þ<EFBFBD><C3BE><EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD><38>ͨ<EFBFBD><CDA8>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
void wd_gridPointClustering_labelling_2(
|
2026-02-08 22:32:36 +08:00
|
|
|
|
std::vector<std::vector<SVzNL3DPoint>>& srcData,
|
|
|
|
|
|
std::vector<std::vector<SSG_clusterLabel>>& labelMask,
|
2026-02-02 07:56:51 +08:00
|
|
|
|
int clusterID, //<2F><>ǰCluster<65><72>ID
|
|
|
|
|
|
std::vector< SVzNL2DPoint>& a_cluster, //result
|
2026-02-08 22:32:36 +08:00
|
|
|
|
SVzNL3DRangeD& clusterRoi //roi3D
|
2026-02-02 07:56:51 +08:00
|
|
|
|
)
|
|
|
|
|
|
{
|
|
|
|
|
|
int i = 0;
|
|
|
|
|
|
int lineNum = (int)srcData.size();
|
|
|
|
|
|
int linePtNum = (int)srcData[0].size();
|
2026-02-18 10:14:11 +08:00
|
|
|
|
|
|
|
|
|
|
SVzNL2DPoint& inputSeed = a_cluster[0];
|
|
|
|
|
|
labelMask[inputSeed.x][inputSeed.y].flag = 1; //<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2026-02-02 07:56:51 +08:00
|
|
|
|
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;
|
|
|
|
|
|
|
2026-02-08 22:32:36 +08:00
|
|
|
|
SSG_clusterLabel& a_seed = labelMask[a_seedPos.x][a_seedPos.y];
|
|
|
|
|
|
SVzNL3DPoint& a_feature3DValue = srcData[a_seedPos.x][a_seedPos.y];
|
2026-02-02 07:56:51 +08:00
|
|
|
|
if (0 == a_seed.clusterID) //clusterID == 0, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
//8<><38>ͨ
|
|
|
|
|
|
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))
|
|
|
|
|
|
{
|
2026-02-08 22:32:36 +08:00
|
|
|
|
SSG_clusterLabel& chk_seed = labelMask[x][y];
|
|
|
|
|
|
if ((chk_seed.validFlag == 0) || (chk_seed.clusterID > 0)) //ֻ<><D6BB><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2026-02-02 07:56:51 +08:00
|
|
|
|
continue;
|
|
|
|
|
|
|
|
|
|
|
|
if (0 == chk_seed.flag)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
chk_seed.flag = 1;
|
|
|
|
|
|
SVzNL2DPoint new_seed = { x, y };
|
|
|
|
|
|
a_cluster.push_back(new_seed);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
a_seed.clusterID = clusterID;
|
|
|
|
|
|
//<2F><><EFBFBD><EFBFBD>ROI
|
2026-02-08 22:32:36 +08:00
|
|
|
|
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;
|
2026-02-02 07:56:51 +08:00
|
|
|
|
i++;
|
|
|
|
|
|
}
|
|
|
|
|
|
return;
|
2026-02-08 22:32:36 +08:00
|
|
|
|
}
|