472 lines
14 KiB
C++
472 lines
14 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,
|
||
std::vector<int> flags,
|
||
double clusterDist)
|
||
{
|
||
int i = 0;
|
||
while (1)
|
||
{
|
||
if (i >= a_cluster.size())
|
||
break;
|
||
|
||
SVzNL3DPosition a_seed = a_cluster[i];
|
||
for (int j = 0, j_max = (int)pts.size(); j < j_max; j++)
|
||
{
|
||
if (flags[j] < 0)
|
||
continue;
|
||
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))
|
||
{
|
||
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,
|
||
int distType, //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector<int>>& indexing2D,
|
||
int lineNum, int ptSize, int clusterCheckWin) //ËÑË÷´°¿Ú
|
||
{
|
||
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;
|
||
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));
|
||
if (dist < clusterDist)
|
||
{
|
||
a_cluster.push_back(pts[backIdx]);
|
||
flags[backIdx] = -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;
|
||
|
||
std::vector<int> flags;
|
||
flags.resize(ptSize);
|
||
std::fill(flags.begin(), flags.end(), 0);
|
||
|
||
while(pts.size() > 0)
|
||
{
|
||
SVzNL3DPosition a_pt = pts[0];
|
||
flags[0] = -1;//·ÀÖ¹Öظ´±»¼ÆËã
|
||
//н¨Ò»¸öcluster
|
||
std::vector< SVzNL3DPosition> a_cluster;
|
||
a_cluster.push_back(a_pt);
|
||
//pts[0].nPointIdx = -1;
|
||
_seedClustering(
|
||
a_cluster,
|
||
pts,
|
||
flags,
|
||
clusterDist);
|
||
objClusters.push_back(a_cluster); //±£´æÒ»¸ö¾ÛÀà
|
||
//½«ptsÖд¦Àí¹ýµÄµãÈ¥³ý
|
||
int m_max = (int)pts.size();
|
||
for (int m = m_max - 1; m >= 0; m--) //´ÓºóÍùÇ°£¬ÕâÑùɾ³ý²»»áÓ°ÏìÑ»·
|
||
{
|
||
if (flags[m] < 0)
|
||
{
|
||
pts.erase(pts.begin() + m);
|
||
flags.erase(flags.begin() + m);
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//ʹÓÃSVzNL3DPositionµÄnPointIdx±íʾ2DÐÅÏ¢£¨¸ß16λLine£¬ µÍ16λptIdx£©
|
||
//ËÑË÷ʱËÑË÷ÁÚÓòÒÔ¼ÓËÙ
|
||
void wd_pointClustering_speedUp(
|
||
std::vector< SVzNL3DPosition>& pts,
|
||
int lineNum, int linePtSize, int clusterCheckWin, //ËÑË÷´°¿Ú
|
||
double clusterDist,
|
||
int distType, //0 - 2d distance; 1- 3d distance
|
||
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;//·ÀÖ¹Öظ´±»¼ÆËã
|
||
//н¨Ò»¸öcluster
|
||
std::vector< SVzNL3DPosition> a_cluster;
|
||
a_cluster.push_back(a_pt);
|
||
//pts[0].nPointIdx = -1;
|
||
_seedClustering_speedUp(
|
||
a_cluster,
|
||
pts,
|
||
flags,
|
||
clusterDist,
|
||
distType,
|
||
indexing2D,
|
||
lineNum, linePtSize, clusterCheckWin);
|
||
if(a_cluster.size() >= 5)
|
||
objClusters.push_back(a_cluster); //±£´æÒ»¸ö¾ÛÀà
|
||
}
|
||
idx++;
|
||
}
|
||
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();
|
||
|
||
SVzNL2DPoint& inputSeed = a_cluster[0];
|
||
labelMask[inputSeed.x][inputSeed.y].flag = 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_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;
|
||
} |