rodAndBarDetection version 1.3.8 :
新的螺杆定位算法，使用PCA方法确定螺杆轴向
This commit is contained in:
parent
8ddf2db090
commit
965d82389c
@ -471,9 +471,10 @@ void _outputRGBDScan_RGBD(
|
|||||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||||
|
|
||||||
//输出法向
|
//输出法向
|
||||||
|
rgb = { 250, 255, 0 };
|
||||||
size = 1;
|
size = 1;
|
||||||
double len1 = 30;
|
double len1 = 30;
|
||||||
double len2 = 200;
|
double len2 = 300;
|
||||||
lineIdx = 0;
|
lineIdx = 0;
|
||||||
for (int i = 0; i < objNum; i++)
|
for (int i = 0; i < objNum; i++)
|
||||||
{
|
{
|
||||||
@ -998,11 +999,11 @@ void screwTest(void)
|
|||||||
const char* ver = wd_rodAndBarDetectionVersion();
|
const char* ver = wd_rodAndBarDetectionVersion();
|
||||||
printf("ver:%s\n", ver);
|
printf("ver:%s\n", ver);
|
||||||
|
|
||||||
for (int grp = 9; grp < SCREW_TEST_GROUP; grp++)
|
for (int grp = 3; grp <= 6; grp++)
|
||||||
{
|
{
|
||||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||||
{
|
{
|
||||||
//fidx =3;
|
//fidx =2;
|
||||||
char _scan_file[256];
|
char _scan_file[256];
|
||||||
|
|
||||||
if(0 == grp)
|
if(0 == grp)
|
||||||
@ -1052,9 +1053,9 @@ void screwTest(void)
|
|||||||
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
||||||
int errCode = 0;
|
int errCode = 0;
|
||||||
std::vector<SSX_rodPoseInfo> screwInfo;
|
std::vector<SSX_rodPoseInfo> screwInfo;
|
||||||
sx_hexHeadScrewMeasure(
|
sx_hexHeadScrewMeasure_PCA(
|
||||||
scanLines,
|
scanLines,
|
||||||
isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
//isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||||
cornerParam,
|
cornerParam,
|
||||||
filterParam,
|
filterParam,
|
||||||
growParam,
|
growParam,
|
||||||
@ -1743,9 +1744,9 @@ typedef enum
|
|||||||
|
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
//ESG_testMode testMode = keSG_测试_配天螺杆定位;
|
ESG_testMode testMode = keSG_测试_配天螺杆定位;
|
||||||
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
|
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
|
||||||
ESG_testMode testMode = keSG_测试_配天新定位盘定位;
|
//ESG_testMode testMode = keSG_测试_配天新定位盘定位;
|
||||||
//ESG_testMode testMode = keSG_测试_棒材抓取;
|
//ESG_testMode testMode = keSG_测试_棒材抓取;
|
||||||
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
||||||
|
|
||||||
|
|||||||
@ -308,6 +308,17 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod(
|
|||||||
const SSG_cornerParam cornerPara,
|
const SSG_cornerParam cornerPara,
|
||||||
std::vector<int>& xyVerticalFlags //环
|
std::vector<int>& xyVerticalFlags //环
|
||||||
);
|
);
|
||||||
|
/// <summary>
|
||||||
|
/// 提取激光线上的与XY平面水平的特征（水平段）
|
||||||
|
/// </summary>
|
||||||
|
SG_APISHARED_EXPORT void wd_getXYHorizontalFeature_dirAngleMethod(
|
||||||
|
std::vector< SVzNL3DPosition>& lineData,
|
||||||
|
int lineIdx,
|
||||||
|
const double maxDistTh,
|
||||||
|
const double minSegSize,
|
||||||
|
const SSG_cornerParam cornerPara,
|
||||||
|
std::vector<int>& xyHorizontalFlags
|
||||||
|
);
|
||||||
|
|
||||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||||
/// 使用平均点距进行加速
|
/// 使用平均点距进行加速
|
||||||
@ -799,6 +810,13 @@ SG_APISHARED_EXPORT Plane ransacFitPlane(
|
|||||||
int stop_no_improve = 250 // 连续多少次无提升就提前退出
|
int stop_no_improve = 250 // 连续多少次无提升就提前退出
|
||||||
);
|
);
|
||||||
|
|
||||||
|
// 输入：3D点云 std::vector<Eigen::Vector3d>
|
||||||
|
// 输出：axis 轴向单位向量, centroid 点云中心
|
||||||
|
SG_APISHARED_EXPORT void computeCylinderAxisFromIncompletePCA(
|
||||||
|
const std::vector<SVzNL3DPosition>& points,
|
||||||
|
SVzNL3DPoint& vec_axis,
|
||||||
|
SVzNL3DPoint& vec_centroid);
|
||||||
|
|
||||||
//计算一个平面调平参数。
|
//计算一个平面调平参数。
|
||||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||||
@ -884,6 +902,19 @@ SG_APISHARED_EXPORT void wd_pointClustering_speedUp(
|
|||||||
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||||
);
|
);
|
||||||
|
|
||||||
|
//对一个给定的聚类（已经有点）继续在一个点云中聚类
|
||||||
|
//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
|
||||||
|
//搜索时搜索邻域以加速
|
||||||
|
SG_APISHARED_EXPORT void wd_clusterGrowing_speedUp(
|
||||||
|
std::vector< SVzNL3DPosition>& pts,
|
||||||
|
std::vector< SVzNL3DPosition >& a_cluster,
|
||||||
|
SVzNL3DRangeD& growingROI, //聚类范围，用于加速
|
||||||
|
int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
|
||||||
|
double clusterDist,
|
||||||
|
int distType, //0 - 2d distance; 1- 3d distance
|
||||||
|
std::vector< SVzNL3DPosition >& added_points
|
||||||
|
);
|
||||||
|
|
||||||
//基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
|
//基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
|
||||||
//使用vector构成2维结构体数组
|
//使用vector构成2维结构体数组
|
||||||
SG_APISHARED_EXPORT void wd_gridPointClustering(
|
SG_APISHARED_EXPORT void wd_gridPointClustering(
|
||||||
|
|||||||
@ -138,6 +138,10 @@ void wd_pointClustering_speedUp(
|
|||||||
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||||
)
|
)
|
||||||
{
|
{
|
||||||
|
int ptSize = (int)pts.size();
|
||||||
|
if (ptSize == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
std::vector<std::vector<int>> indexing2D;
|
std::vector<std::vector<int>> indexing2D;
|
||||||
indexing2D.resize(lineNum);
|
indexing2D.resize(lineNum);
|
||||||
for (int i = 0; i < lineNum; i++)
|
for (int i = 0; i < lineNum; i++)
|
||||||
@ -152,10 +156,6 @@ void wd_pointClustering_speedUp(
|
|||||||
indexing2D[line][ptIdx] = i;
|
indexing2D[line][ptIdx] = i;
|
||||||
}
|
}
|
||||||
|
|
||||||
int ptSize = (int)pts.size();
|
|
||||||
if (ptSize == 0)
|
|
||||||
return;
|
|
||||||
|
|
||||||
std::vector<int> flags;
|
std::vector<int> flags;
|
||||||
flags.resize(ptSize);
|
flags.resize(ptSize);
|
||||||
std::fill(flags.begin(), flags.end(), 0);
|
std::fill(flags.begin(), flags.end(), 0);
|
||||||
@ -187,6 +187,126 @@ void wd_pointClustering_speedUp(
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//对一个给定的聚类（已经有点）继续在一个点云中聚类
|
||||||
|
//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
|
||||||
|
//搜索时搜索邻域以加速
|
||||||
|
void wd_clusterGrowing_speedUp(
|
||||||
|
std::vector< SVzNL3DPosition>& pts,
|
||||||
|
std::vector< SVzNL3DPosition >& a_cluster,
|
||||||
|
SVzNL3DRangeD& growingROI, //聚类范围，用于加速
|
||||||
|
int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
|
||||||
|
double clusterDist,
|
||||||
|
int distType, //0 - 2d distance; 1- 3d distance
|
||||||
|
std::vector< SVzNL3DPosition >& added_points
|
||||||
|
)
|
||||||
|
{
|
||||||
|
int ptSize = (int)pts.size();
|
||||||
|
if (ptSize == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
//构建索引
|
||||||
|
std::vector<int> flags;
|
||||||
|
flags.resize(pts.size());
|
||||||
|
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;
|
||||||
|
flags[i] = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
//将已经聚类的点置标记
|
||||||
|
for (int i = 0; i < (int)a_cluster.size(); i++)
|
||||||
|
{
|
||||||
|
int line = a_cluster[i].nPointIdx >> 16;
|
||||||
|
int ptIdx = a_cluster[i].nPointIdx & 0x0000FFFF;
|
||||||
|
int backIdx = indexing2D[line][ptIdx];
|
||||||
|
flags[backIdx] = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
//在ROI中取出未被聚类的点（候选的检查点）
|
||||||
|
std::vector<int> toGrowPtIndice;
|
||||||
|
for (int i = 0; i < (int)pts.size(); i++)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition a_pt = pts[i];
|
||||||
|
if (a_pt.pt3D.z < 1e-4)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
int line = a_pt.nPointIdx >> 16;
|
||||||
|
int ptIdx = a_pt.nPointIdx & 0x0000FFFF;
|
||||||
|
if ((a_pt.pt3D.x > growingROI.xRange.min) && (a_pt.pt3D.x < growingROI.xRange.max) &&
|
||||||
|
(a_pt.pt3D.y > growingROI.yRange.min) && (a_pt.pt3D.y < growingROI.yRange.max) &&
|
||||||
|
(a_pt.pt3D.z > growingROI.zRange.min) && (a_pt.pt3D.z < growingROI.zRange.max) &&
|
||||||
|
(flags[i] == 0))
|
||||||
|
toGrowPtIndice.push_back(i);
|
||||||
|
}
|
||||||
|
if (toGrowPtIndice.size() == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
//在toGrowPts中寻找新的种子
|
||||||
|
std::vector< SVzNL3DPosition > new_seeds;
|
||||||
|
for (int i = 0; i < (int)toGrowPtIndice.size(); i++)
|
||||||
|
{
|
||||||
|
int indexing = toGrowPtIndice[i];
|
||||||
|
SVzNL3DPosition a_pt = pts[indexing];
|
||||||
|
int pt_line = a_pt.nPointIdx >> 16;
|
||||||
|
int pt_idx = a_pt.nPointIdx & 0x0000FFFF;
|
||||||
|
if (flags[indexing] != 0) //防止重复检查
|
||||||
|
continue;
|
||||||
|
|
||||||
|
for (int line = pt_line - clusterCheckWin; line <= pt_line + clusterCheckWin; line++)
|
||||||
|
{
|
||||||
|
if ((line >= 0) && (line < lineNum))
|
||||||
|
{
|
||||||
|
for (int ptIdx = pt_idx - clusterCheckWin; ptIdx <= pt_idx + clusterCheckWin; ptIdx++)
|
||||||
|
{
|
||||||
|
if ((ptIdx >= 0) && (ptIdx < ptSize))
|
||||||
|
{
|
||||||
|
int backIdx = indexing2D[line][ptIdx];
|
||||||
|
if (backIdx < 0)
|
||||||
|
continue;
|
||||||
|
if (flags[backIdx] < 0) //已有聚类中的点
|
||||||
|
{
|
||||||
|
double dist;
|
||||||
|
if (0 == distType)
|
||||||
|
dist = sqrt(pow(a_pt.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_pt.pt3D.y - pts[backIdx].pt3D.y, 2));
|
||||||
|
else
|
||||||
|
dist = sqrt(pow(a_pt.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_pt.pt3D.y - pts[backIdx].pt3D.y, 2) + pow(a_pt.pt3D.z - pts[backIdx].pt3D.z, 2));
|
||||||
|
if (dist < clusterDist)
|
||||||
|
{
|
||||||
|
new_seeds.push_back(pts[backIdx]);
|
||||||
|
flags[indexing] = -1; //聚类种子
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (new_seeds.size() == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
_seedClustering_speedUp(
|
||||||
|
new_seeds,
|
||||||
|
pts,
|
||||||
|
flags,
|
||||||
|
clusterDist,
|
||||||
|
distType,
|
||||||
|
indexing2D,
|
||||||
|
lineNum, linePtSize, clusterCheckWin);
|
||||||
|
|
||||||
|
added_points.insert(added_points.end(), new_seeds.begin(), new_seeds.end());
|
||||||
|
a_cluster.insert(a_cluster.end(), new_seeds.begin(), new_seeds.end());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
//对特征点的聚类
|
//对特征点的聚类
|
||||||
void wd_gridPointClustering(
|
void wd_gridPointClustering(
|
||||||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
||||||
|
|||||||
@ -5706,12 +5706,7 @@ void wd_getLineCorerFeature(
|
|||||||
/// nPointIdx被重新定义成Feature类型
|
/// nPointIdx被重新定义成Feature类型
|
||||||
/// 算法流程：
|
/// 算法流程：
|
||||||
/// （1）逐点计算前向角和后向角
|
/// （1）逐点计算前向角和后向角
|
||||||
/// （2）搜索同方向的拐角连续段
|
/// （2）去除前身角和后向角大于门限的点
|
||||||
/// （2）搜索Z极值，
|
|
||||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
|
||||||
/// （3）搜索正拐角的极大值。
|
|
||||||
/// （4）判断拐角是否为跳变
|
|
||||||
///
|
|
||||||
/// </summary>
|
/// </summary>
|
||||||
void wd_getXYVertialFeature_dirAngleMethod(
|
void wd_getXYVertialFeature_dirAngleMethod(
|
||||||
std::vector< SVzNL3DPosition>& lineData,
|
std::vector< SVzNL3DPosition>& lineData,
|
||||||
@ -5741,6 +5736,94 @@ void wd_getXYVertialFeature_dirAngleMethod(
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提取激光线上的与XY平面水平的特征（水平段）
|
||||||
|
/// seg端点：z距离大于门限
|
||||||
|
/// nPointIdx被重新定义成Feature类型
|
||||||
|
/// 算法流程：
|
||||||
|
/// （1）逐点计算前向角和后向角
|
||||||
|
/// （2）去除前向角和后向角小于门限的点
|
||||||
|
/// </summary>
|
||||||
|
void wd_getXYHorizontalFeature_dirAngleMethod(
|
||||||
|
std::vector< SVzNL3DPosition>& lineData,
|
||||||
|
int lineIdx,
|
||||||
|
const double maxDistTh,
|
||||||
|
const double minSegSize,
|
||||||
|
const SSG_cornerParam cornerPara,
|
||||||
|
std::vector<int>& xyHorizontalFlags
|
||||||
|
)
|
||||||
|
{
|
||||||
|
if (lineIdx == 562)
|
||||||
|
int kkk = 1;
|
||||||
|
double maxHorizontalAngle = cornerPara.cornerTh; //arc上每个点的转角最大值
|
||||||
|
|
||||||
|
xyHorizontalFlags.resize(lineData.size());
|
||||||
|
std::fill(xyHorizontalFlags.begin(), xyHorizontalFlags.end(), 0);
|
||||||
|
|
||||||
|
//根据z连续性分段
|
||||||
|
std::vector<SSG_RUN> segs;
|
||||||
|
wd_lineDataSegment_dist_2(
|
||||||
|
lineData,
|
||||||
|
segs,
|
||||||
|
maxDistTh,
|
||||||
|
minSegSize
|
||||||
|
);
|
||||||
|
//计算前向角和后向角
|
||||||
|
std::vector< SSG_pntDirAngle> ptDirAngles;
|
||||||
|
_computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles);
|
||||||
|
|
||||||
|
for (int i = 0; i < (int)ptDirAngles.size(); i++)
|
||||||
|
{
|
||||||
|
if ( (ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0))
|
||||||
|
continue;
|
||||||
|
|
||||||
|
if ((abs(ptDirAngles[i].backwardAngle) < maxHorizontalAngle) && (abs(ptDirAngles[i].forwardAngle) < maxHorizontalAngle))
|
||||||
|
xyHorizontalFlags[i] = 1;
|
||||||
|
}
|
||||||
|
//检查边缘点
|
||||||
|
for (int i = 1; i < (int)ptDirAngles.size() - 1; i++)
|
||||||
|
{
|
||||||
|
if ((xyHorizontalFlags[i - 1] == 0) && (xyHorizontalFlags[i] == 1))
|
||||||
|
{
|
||||||
|
int sIdx = ptDirAngles[i].backwardPntIdx;
|
||||||
|
bool isEdge = true;
|
||||||
|
for (int j = sIdx; j < i; j++)
|
||||||
|
{
|
||||||
|
if (ptDirAngles[j].pntIdx >= 0)
|
||||||
|
{
|
||||||
|
isEdge = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (true == isEdge)
|
||||||
|
{
|
||||||
|
for (int j = sIdx; j < i; j++)
|
||||||
|
xyHorizontalFlags[j] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if ((xyHorizontalFlags[i + 1] == 0) && (xyHorizontalFlags[i] == 1))
|
||||||
|
{
|
||||||
|
int eIdx = ptDirAngles[i].forwardPntIdx;
|
||||||
|
bool isEdge = true;
|
||||||
|
for (int j = i+1; j <= eIdx; j++)
|
||||||
|
{
|
||||||
|
if (ptDirAngles[j].pntIdx >= 0)
|
||||||
|
{
|
||||||
|
isEdge = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (true == isEdge)
|
||||||
|
{
|
||||||
|
for (int j = i+1; j <= eIdx; j++)
|
||||||
|
xyHorizontalFlags[j] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||||
/// 使用平均点距进行加速
|
/// 使用平均点距进行加速
|
||||||
/// nPointIdx被重新定义成Feature类型
|
/// nPointIdx被重新定义成Feature类型
|
||||||
|
|||||||
@ -817,3 +817,63 @@ Plane ransacFitPlane(
|
|||||||
|
|
||||||
return best_plane;
|
return best_plane;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 输入：3D点云 std::vector<Eigen::Vector3d>
|
||||||
|
// 输出：axis 轴向单位向量, centroid 点云中心
|
||||||
|
void computeCylinderAxisFromIncompletePCA(
|
||||||
|
const std::vector<SVzNL3DPosition>& src_points,
|
||||||
|
SVzNL3DPoint& vec_axis,
|
||||||
|
SVzNL3DPoint& vec_centroid)
|
||||||
|
{
|
||||||
|
vec_axis = { 0.0, 0.0, 0.0 };
|
||||||
|
vec_centroid = { 0.0, 0.0, 0.0 };
|
||||||
|
|
||||||
|
if (src_points.empty()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<Eigen::Vector3d> points;
|
||||||
|
for (int i = 0; i < (int)src_points.size(); i++)
|
||||||
|
points.emplace_back(src_points[i].pt3D.x, src_points[i].pt3D.y, src_points[i].pt3D.z);
|
||||||
|
|
||||||
|
Eigen::Vector3d axis, centroid;
|
||||||
|
// --------------------------
|
||||||
|
// 1. 计算质心 + 中心化
|
||||||
|
// --------------------------
|
||||||
|
centroid.setZero();
|
||||||
|
for (const auto& p : points) centroid += p;
|
||||||
|
centroid /= points.size();
|
||||||
|
|
||||||
|
std::vector<Eigen::Vector3d> centered_pts;
|
||||||
|
centered_pts.reserve(points.size());
|
||||||
|
for (const auto& p : points) centered_pts.emplace_back(p - centroid);
|
||||||
|
|
||||||
|
// --------------------------
|
||||||
|
// 2. 构建 3x3 协方差矩阵
|
||||||
|
// --------------------------
|
||||||
|
Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
|
||||||
|
for (const auto& p : centered_pts) cov += p * p.transpose();
|
||||||
|
cov /= centered_pts.size() - 1;
|
||||||
|
|
||||||
|
// --------------------------
|
||||||
|
// 3. 特征值分解
|
||||||
|
// --------------------------
|
||||||
|
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);
|
||||||
|
Eigen::Vector3d eig_vals = solver.eigenvalues(); // 升序排列
|
||||||
|
Eigen::Matrix3d eig_vecs = solver.eigenvectors();
|
||||||
|
|
||||||
|
// --------------------------
|
||||||
|
// 关键：残缺圆柱 → 取【最大】特征值对应的向量 = 轴向
|
||||||
|
// --------------------------
|
||||||
|
axis = eig_vecs.col(2); // eigenvalues 升序，[2]最大
|
||||||
|
axis.normalize();
|
||||||
|
|
||||||
|
vec_axis.x = axis[0];
|
||||||
|
vec_axis.y = axis[1];
|
||||||
|
vec_axis.z = axis[2];
|
||||||
|
|
||||||
|
vec_centroid.x = centroid[0];
|
||||||
|
vec_centroid.y = centroid[1];
|
||||||
|
vec_centroid.z = centroid[2];
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|||||||
@ -28,7 +28,8 @@
|
|||||||
//version 1.3.5 : 新的定位盘中心测量功能占将float运算改成double ,测试PC和3588差异
|
//version 1.3.5 : 新的定位盘中心测量功能占将float运算改成double ,测试PC和3588差异
|
||||||
//version 1.3.6 : 新的定位盘中心测量功能：优化聚类前的垂直点去除算法，保证聚类结果正确
|
//version 1.3.6 : 新的定位盘中心测量功能：优化聚类前的垂直点去除算法，保证聚类结果正确
|
||||||
//version 1.3.7 : 新的定位盘中心测量功能：进一步优化了内部参数，优化了垂直点去除效果
|
//version 1.3.7 : 新的定位盘中心测量功能：进一步优化了内部参数，优化了垂直点去除效果
|
||||||
std::string m_strVersion = "1.3.7";
|
//version 1.3.8 : 新的螺杆定位算法，使用PCA方法确定螺杆轴向
|
||||||
|
std::string m_strVersion = "1.3.8";
|
||||||
const char* wd_rodAndBarDetectionVersion(void)
|
const char* wd_rodAndBarDetectionVersion(void)
|
||||||
{
|
{
|
||||||
return m_strVersion.c_str();
|
return m_strVersion.c_str();
|
||||||
@ -912,6 +913,623 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SWD3DPointPostion>& scanData)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//PCA方法计算螺杆端部中心点位姿
|
||||||
|
//相对于sx_hexHeadScrewMeasure()，算法上（1）去除了水平段（2）使用PCA方法计算轴向
|
||||||
|
void sx_hexHeadScrewMeasure_PCA(
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
//bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||||
|
const SSG_cornerParam cornerPara,
|
||||||
|
const SSG_outlierFilterParam filterParam,
|
||||||
|
const SSG_treeGrowParam growParam,
|
||||||
|
double rodDiameter,
|
||||||
|
std::vector<SSX_rodPoseInfo>& screwInfo,
|
||||||
|
int* errCode)
|
||||||
|
{
|
||||||
|
*errCode = 0;
|
||||||
|
int lineNum = (int)scanLines.size();
|
||||||
|
if (lineNum == 0)
|
||||||
|
{
|
||||||
|
*errCode = SG_ERR_3D_DATA_NULL;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int linePtNum = (int)scanLines[0].size();
|
||||||
|
|
||||||
|
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||||||
|
bool isGridData = true;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
if (linePtNum != (int)scanLines[line].size())
|
||||||
|
{
|
||||||
|
isGridData = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (false == isGridData)//数据不是网格格式
|
||||||
|
{
|
||||||
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
//产生数据Copy和水平扫描数据
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>> scanLines_copy;
|
||||||
|
scanLines_copy.resize(scanLines.size());
|
||||||
|
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||||||
|
scanLines_h.resize(linePtNum);
|
||||||
|
for (int i = 0; i < linePtNum; i++)
|
||||||
|
scanLines_h[i].resize(lineNum);
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
scanLines_copy[line].insert(scanLines_copy[line].end(), scanLines[line].begin(), scanLines[line].end());
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
{
|
||||||
|
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||||
|
scanLines_h[j][line] = scanLines[line][j];
|
||||||
|
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||||
|
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (int line = 0; line < linePtNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
||||||
|
scanLines_h[line][j].nPointIdx = j;
|
||||||
|
}
|
||||||
|
|
||||||
|
//算法流程：
|
||||||
|
//1、检查水平方向数据并去除
|
||||||
|
//2、聚类
|
||||||
|
//3、保留最前面目标
|
||||||
|
//内部参数
|
||||||
|
SSG_cornerParam removeHorizonPara = cornerPara;
|
||||||
|
removeHorizonPara.scale = 5.0;
|
||||||
|
removeHorizonPara.cornerTh = 45;
|
||||||
|
double maxDistTh = 10.0;
|
||||||
|
double minSegSize = 3.0; //小于3mm的segment长度被过滤掉
|
||||||
|
|
||||||
|
std::vector<std::vector<int>> flags;
|
||||||
|
flags.resize(lineNum);
|
||||||
|
for (int i = 0; i < lineNum; i++)
|
||||||
|
{
|
||||||
|
flags[i].resize(linePtNum);
|
||||||
|
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||||||
|
}
|
||||||
|
std::vector<std::vector<int>> zHorizonFlags;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
if (line == 248)
|
||||||
|
int kkk = 1;
|
||||||
|
std::vector<int> line_horizontalFlags;
|
||||||
|
wd_getXYHorizontalFeature_dirAngleMethod(
|
||||||
|
scanLines_copy[line],
|
||||||
|
line,
|
||||||
|
maxDistTh,
|
||||||
|
minSegSize,
|
||||||
|
removeHorizonPara,
|
||||||
|
line_horizontalFlags
|
||||||
|
);
|
||||||
|
zHorizonFlags.push_back(line_horizontalFlags);
|
||||||
|
|
||||||
|
for (int i = 0; i < (int)line_horizontalFlags.size(); i++)
|
||||||
|
{
|
||||||
|
if (line_horizontalFlags[i] > 0)
|
||||||
|
flags[line][i] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
std::vector<std::vector<int>> zHorizonFlags_h;
|
||||||
|
for (int line = 0; line < linePtNum; line++)
|
||||||
|
{
|
||||||
|
if (line == 1177)
|
||||||
|
int kkk = 1;
|
||||||
|
std::vector<int> line_horizontalFlags;
|
||||||
|
wd_getXYHorizontalFeature_dirAngleMethod(
|
||||||
|
scanLines_h[line],
|
||||||
|
line,
|
||||||
|
removeHorizonPara,
|
||||||
|
line_horizontalFlags
|
||||||
|
);
|
||||||
|
zHorizonFlags_h.push_back(line_horizontalFlags);
|
||||||
|
|
||||||
|
for (int i = 0; i < (int)line_horizontalFlags.size(); i++)
|
||||||
|
{
|
||||||
|
if (line_horizontalFlags[i] > 0)
|
||||||
|
flags[i][line] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//去除操作
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
{
|
||||||
|
if (flags[line][j] > 0)
|
||||||
|
{
|
||||||
|
scanLines_copy[line][j].pt3D.z = 0;
|
||||||
|
scanLines_h[j][line].pt3D.z = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//迭代一次
|
||||||
|
SSG_lineSegParam lineSegPara;
|
||||||
|
lineSegPara.distScale = 10.0;
|
||||||
|
lineSegPara.segGapTh_y = 10.0;
|
||||||
|
lineSegPara.segGapTh_z = 10.0;
|
||||||
|
const int minSegLen = 5;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
std::vector<SSG_RUN> segs;
|
||||||
|
wd_getLineDataIntervals(
|
||||||
|
scanLines_copy[line],
|
||||||
|
lineSegPara,
|
||||||
|
segs);
|
||||||
|
for (int i = 0; i < (int)segs.size(); i++)
|
||||||
|
{
|
||||||
|
if (segs[i].len <= minSegLen)
|
||||||
|
{
|
||||||
|
int idx0 = segs[i].start;
|
||||||
|
for (int j = 0; j < segs[i].len; j++)
|
||||||
|
flags[line][idx0 + j] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (int line = 0; line < linePtNum; line++)
|
||||||
|
{
|
||||||
|
std::vector<SSG_RUN> segs;
|
||||||
|
wd_getLineDataIntervals(
|
||||||
|
scanLines_h[line],
|
||||||
|
lineSegPara,
|
||||||
|
segs);
|
||||||
|
for (int i = 0; i < (int)segs.size(); i++)
|
||||||
|
{
|
||||||
|
if (segs[i].len <= minSegLen)
|
||||||
|
{
|
||||||
|
int idx0 = segs[i].start;
|
||||||
|
for (int j = 0; j < segs[i].len; j++)
|
||||||
|
flags[idx0 + j][line] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//标注
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
scanLines_copy[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||||
|
}
|
||||||
|
//将垂直线段去除
|
||||||
|
std::vector< SVzNL3DPosition> validPoints;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
{
|
||||||
|
if (flags[line][j] > 0)
|
||||||
|
scanLines_copy[line][j].pt3D.z = 0;
|
||||||
|
|
||||||
|
if (scanLines_copy[line][j].pt3D.z > 1e-4)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition a_vldPt;
|
||||||
|
a_vldPt.pt3D = scanLines_copy[line][j].pt3D;
|
||||||
|
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||||
|
validPoints.push_back(a_vldPt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//聚类
|
||||||
|
//内部参数
|
||||||
|
double minObjSize_w = 150;
|
||||||
|
double minScrewLen = 50;
|
||||||
|
|
||||||
|
int clusterCheckWin = 5;
|
||||||
|
double clusterDist = 2.5;
|
||||||
|
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||||||
|
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||||||
|
wd_pointClustering_speedUp(
|
||||||
|
validPoints,
|
||||||
|
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||||
|
clusterDist,
|
||||||
|
distType,
|
||||||
|
objClusters //result
|
||||||
|
);
|
||||||
|
|
||||||
|
//使用cluster的ROI信息过滤目标，将Z最小的符合要求的目标判断为中间的螺杆
|
||||||
|
int clusterSize = (int)objClusters.size();
|
||||||
|
std::vector<SVzNL3DRangeD> objROIs;
|
||||||
|
for (int i = 0; i < clusterSize; i++)
|
||||||
|
{
|
||||||
|
// Initialize min and max values
|
||||||
|
// Calculate X, Y and Z ranges
|
||||||
|
SVzNL3DRangeD a_roi3D;
|
||||||
|
a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = -DBL_MAX;
|
||||||
|
a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = -DBL_MAX;
|
||||||
|
a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = -DBL_MAX;
|
||||||
|
|
||||||
|
int nodeNum = (int)objClusters[i].size();
|
||||||
|
for (int j = 0; j < nodeNum; j++)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition& a_pt = objClusters[i][j];
|
||||||
|
|
||||||
|
if (a_pt.pt3D.z > 1e-4)
|
||||||
|
{
|
||||||
|
a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, a_pt.pt3D.x);
|
||||||
|
a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, a_pt.pt3D.x);
|
||||||
|
a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, a_pt.pt3D.y);
|
||||||
|
a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, a_pt.pt3D.y);
|
||||||
|
a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, a_pt.pt3D.z);
|
||||||
|
a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, a_pt.pt3D.z);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
objROIs.push_back(a_roi3D);
|
||||||
|
}
|
||||||
|
std::vector<int> objCluster;
|
||||||
|
for (int i = 0; i < clusterSize; i++)
|
||||||
|
{
|
||||||
|
double x_width = objROIs[i].xRange.max - objROIs[i].xRange.min;
|
||||||
|
double y_width = objROIs[i].yRange.max - objROIs[i].yRange.min;
|
||||||
|
double z_width = objROIs[i].zRange.max - objROIs[i].zRange.min;
|
||||||
|
if ((x_width < rodDiameter * 3.0) && (y_width < rodDiameter*3.0) && (z_width > minScrewLen) && (objClusters[i].size() > 100))
|
||||||
|
objCluster.push_back(i);
|
||||||
|
}
|
||||||
|
|
||||||
|
//取最前面的
|
||||||
|
int targetClusterID = -1;
|
||||||
|
for (int i = 0; i < objCluster.size(); i++)
|
||||||
|
{
|
||||||
|
int clusterIdx = objCluster[i];
|
||||||
|
if (targetClusterID < 0)
|
||||||
|
targetClusterID = clusterIdx;
|
||||||
|
else if(objROIs[targetClusterID].zRange.min > objROIs[clusterIdx].zRange.min)
|
||||||
|
targetClusterID = clusterIdx;
|
||||||
|
}
|
||||||
|
if(targetClusterID < 0)
|
||||||
|
{
|
||||||
|
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
//进行PCA前，去除螺杆根部，防止影响PCA精度
|
||||||
|
double zMax = objROIs[targetClusterID].zRange.max - 30; //去除螺杆根部影响
|
||||||
|
double zMin = objROIs[targetClusterID].zRange.min + 30; //去除螺杆根部影响
|
||||||
|
std::vector< SVzNL3DPosition > PCA_points;
|
||||||
|
for (int i = 0; i < objClusters[targetClusterID].size(); i++)
|
||||||
|
{
|
||||||
|
if ((objClusters[targetClusterID][i].pt3D.z < zMax) && (objClusters[targetClusterID][i].pt3D.z > zMin))
|
||||||
|
PCA_points.push_back(objClusters[targetClusterID][i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
SVzNL3DPoint vec_axis, vec_centroid;
|
||||||
|
//PCA计算轴向量
|
||||||
|
computeCylinderAxisFromIncompletePCA(
|
||||||
|
PCA_points,
|
||||||
|
vec_axis,
|
||||||
|
vec_centroid);
|
||||||
|
if (vec_axis.z < 0) //确定唯一方向
|
||||||
|
vec_axis = { -vec_axis.x, -vec_axis.y, -vec_axis.z };
|
||||||
|
|
||||||
|
//投影
|
||||||
|
//计算旋转向量
|
||||||
|
SVzNL3DPoint vector1 = vec_axis;
|
||||||
|
SVzNL3DPoint vector2 = { 0, 0, 1.0 };
|
||||||
|
SSG_planeCalibPara rotatePara = wd_computeRTMatrix(vector1, vector2);
|
||||||
|
|
||||||
|
///此处考虑到倾斜情况下最前面的螺杆和正投影下会有不同，需要迭代一下
|
||||||
|
//迭代一次，确定正确的螺杆
|
||||||
|
std::vector<std::vector< SVzNL3DPosition>> rotate_objClusters; //result
|
||||||
|
rotate_objClusters.resize(objClusters.size());
|
||||||
|
std::vector<SVzNL3DRangeD> rotate_objROIs;
|
||||||
|
for (int i = 0; i < clusterSize; i++)
|
||||||
|
{
|
||||||
|
rotate_objClusters[i].resize(objClusters[i].size());
|
||||||
|
// Initialize min and max values
|
||||||
|
// Calculate X, Y and Z ranges
|
||||||
|
SVzNL3DRangeD a_roi3D;
|
||||||
|
a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = -DBL_MAX;
|
||||||
|
a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = -DBL_MAX;
|
||||||
|
a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = -DBL_MAX;
|
||||||
|
|
||||||
|
int nodeNum = (int)objClusters[i].size();
|
||||||
|
for (int j = 0; j < nodeNum; j++)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition& a_pt = objClusters[i][j];
|
||||||
|
|
||||||
|
if (a_pt.pt3D.z > 1e-4)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition rotate_pt;
|
||||||
|
rotate_pt.nPointIdx = a_pt.nPointIdx;
|
||||||
|
rotate_pt.pt3D = _translatePoint(a_pt.pt3D, rotatePara.planeCalib);
|
||||||
|
rotate_objClusters[i][j] = rotate_pt;
|
||||||
|
|
||||||
|
a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, rotate_pt.pt3D.x);
|
||||||
|
a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, rotate_pt.pt3D.x);
|
||||||
|
a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, rotate_pt.pt3D.y);
|
||||||
|
a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, rotate_pt.pt3D.y);
|
||||||
|
a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, rotate_pt.pt3D.z);
|
||||||
|
a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, rotate_pt.pt3D.z);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
rotate_objROIs.push_back(a_roi3D);
|
||||||
|
}
|
||||||
|
//重新确定Z最小的目标
|
||||||
|
//取最前面的
|
||||||
|
targetClusterID = -1;
|
||||||
|
for (int i = 0; i < objCluster.size(); i++)
|
||||||
|
{
|
||||||
|
int clusterIdx = objCluster[i];
|
||||||
|
if (targetClusterID < 0)
|
||||||
|
targetClusterID = clusterIdx;
|
||||||
|
else if (rotate_objROIs[targetClusterID].zRange.min > rotate_objROIs[clusterIdx].zRange.min)
|
||||||
|
targetClusterID = clusterIdx;
|
||||||
|
}
|
||||||
|
if (targetClusterID < 0)
|
||||||
|
{
|
||||||
|
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
//进行PCA前，去除螺杆根部，防止影响PCA精度
|
||||||
|
zMax = objROIs[targetClusterID].zRange.max - 30.0; //去除螺杆根部影响
|
||||||
|
zMin = objROIs[targetClusterID].zRange.min + 30.0; //去除螺杆根部影响
|
||||||
|
PCA_points.clear();
|
||||||
|
for (int i = 0; i < objClusters[targetClusterID].size(); i++)
|
||||||
|
{
|
||||||
|
if ((objClusters[targetClusterID][i].pt3D.z < zMax) && (objClusters[targetClusterID][i].pt3D.z > zMin))
|
||||||
|
PCA_points.push_back(objClusters[targetClusterID][i]);
|
||||||
|
}
|
||||||
|
//重新使用PCA方法计算轴向，此处使用旋转前数据
|
||||||
|
computeCylinderAxisFromIncompletePCA(
|
||||||
|
PCA_points,
|
||||||
|
vec_axis,
|
||||||
|
vec_centroid);
|
||||||
|
if (vec_axis.z < 0) //确定唯一方向
|
||||||
|
vec_axis = { -vec_axis.x, -vec_axis.y, -vec_axis.z };
|
||||||
|
|
||||||
|
//生成原始数据的去零点的点云数据
|
||||||
|
std::vector< SVzNL3DPosition> raw_validPoints;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
{
|
||||||
|
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||||
|
{
|
||||||
|
SVzNL3DPosition a_vldPt;
|
||||||
|
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||||||
|
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||||
|
raw_validPoints.push_back(a_vldPt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2、补充完整端面数据
|
||||||
|
//在未旋转的点云中继续生长（端面可能在去除水平点中被去除）
|
||||||
|
SVzNL3DRangeD growingROI = objROIs[targetClusterID];
|
||||||
|
growingROI.xRange.min -= rodDiameter;
|
||||||
|
growingROI.xRange.max += rodDiameter;
|
||||||
|
growingROI.yRange.min -= rodDiameter;
|
||||||
|
growingROI.yRange.max += rodDiameter;
|
||||||
|
growingROI.zRange.max = (growingROI.zRange.max + growingROI.zRange.min) / 2; //从Z的中间向外生长
|
||||||
|
growingROI.zRange.min -= rodDiameter;
|
||||||
|
std::vector< SVzNL3DPosition>& screw_cluster = objClusters[targetClusterID];
|
||||||
|
std::vector< SVzNL3DPosition> added_points;
|
||||||
|
wd_clusterGrowing_speedUp(
|
||||||
|
raw_validPoints,
|
||||||
|
screw_cluster,
|
||||||
|
growingROI, //聚类范围，用于加速
|
||||||
|
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||||
|
clusterDist,
|
||||||
|
distType, //0 - 2d distance; 1- 3d distance
|
||||||
|
added_points
|
||||||
|
);
|
||||||
|
#if 1
|
||||||
|
for (int i = 0; i < (int)added_points.size(); i++)
|
||||||
|
{
|
||||||
|
int line = added_points[i].nPointIdx >> 16;
|
||||||
|
int ptIdx = added_points[i].nPointIdx & 0x0000FFFF;
|
||||||
|
scanLines_copy[line][ptIdx].pt3D = added_points[i].pt3D; //恢复
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
SVzNL3DPoint rotate_centroid = _ptRotate(vec_centroid, rotatePara.planeCalib);
|
||||||
|
std::vector< SWD3DPointPostion> roiProjectionData;
|
||||||
|
//投影，提取ROI内的数据
|
||||||
|
for (int i = 0; i < (int)objClusters[targetClusterID].size(); i++)
|
||||||
|
{
|
||||||
|
SVzNL3DPoint a_pt = objClusters[targetClusterID][i].pt3D;
|
||||||
|
if (a_pt.z < 1e-4)
|
||||||
|
continue;
|
||||||
|
int line = objClusters[targetClusterID][i].nPointIdx >> 16;
|
||||||
|
int ptIdx = objClusters[targetClusterID][i].nPointIdx & 0x0000FFFF;
|
||||||
|
double x = a_pt.x * rotatePara.planeCalib[0] + a_pt.y * rotatePara.planeCalib[1] + a_pt.z * rotatePara.planeCalib[2];
|
||||||
|
double y = a_pt.x * rotatePara.planeCalib[3] + a_pt.y * rotatePara.planeCalib[4] + a_pt.z * rotatePara.planeCalib[5];
|
||||||
|
double z = a_pt.x * rotatePara.planeCalib[6] + a_pt.y * rotatePara.planeCalib[7] + a_pt.z * rotatePara.planeCalib[8];
|
||||||
|
if (z <= rotate_centroid.z)
|
||||||
|
{
|
||||||
|
SWD3DPointPostion projectPt;
|
||||||
|
projectPt.lineIdx = line;
|
||||||
|
projectPt.ptIdx = ptIdx;
|
||||||
|
projectPt.point.x = x;
|
||||||
|
projectPt.point.y = y;
|
||||||
|
projectPt.point.z = z;
|
||||||
|
roiProjectionData.push_back(projectPt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool dirInverting = false;
|
||||||
|
//取端面
|
||||||
|
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||||||
|
SVzNLRangeD cutZRange;
|
||||||
|
if (false == dirInverting)
|
||||||
|
{
|
||||||
|
cutZRange.min = zRange.min;
|
||||||
|
cutZRange.max = zRange.min + 10.0; //5mm的端面
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
cutZRange.max = zRange.max;
|
||||||
|
cutZRange.min = zRange.max - 10.0; //5mm的端面
|
||||||
|
}
|
||||||
|
std::vector<SWD3DPointPostion> surfacePoints;
|
||||||
|
std::vector<std::vector<int>>addrMapping;
|
||||||
|
addrMapping.resize(scanLines.size());
|
||||||
|
for (int i = 0; i < (int)scanLines.size(); i++)
|
||||||
|
{
|
||||||
|
addrMapping[i].resize(scanLines[i].size());
|
||||||
|
std::fill(addrMapping[i].begin(), addrMapping[i].end(), -1);
|
||||||
|
}
|
||||||
|
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints, addrMapping);
|
||||||
|
//计算中心点
|
||||||
|
SWD3DPointPostion projectionCenter;// = getXoYCentroid(surfacePoints);
|
||||||
|
SVzNL3DRangeD roi3D = _getPointCloudROI(surfacePoints);
|
||||||
|
//计算XY平面上的质心
|
||||||
|
double sum_x = 0, sum_y = 0;
|
||||||
|
int sum_size = 0;
|
||||||
|
for (int i = 0; i < (int)surfacePoints.size(); i++)
|
||||||
|
{
|
||||||
|
if (surfacePoints[i].point.z > 1e-4)
|
||||||
|
{
|
||||||
|
sum_x += surfacePoints[i].point.x;
|
||||||
|
sum_y += surfacePoints[i].point.y;
|
||||||
|
sum_size++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (sum_size == 0)
|
||||||
|
{
|
||||||
|
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
projectionCenter.lineIdx = -1;
|
||||||
|
projectionCenter.ptIdx = -1;
|
||||||
|
projectionCenter.point.x = sum_x / sum_size; // (roi3D.xRange.min + roi3D.xRange.max) / 2;
|
||||||
|
projectionCenter.point.y = sum_y / sum_size; // (roi3D.yRange.min + roi3D.yRange.max) / 2;
|
||||||
|
projectionCenter.point.z = zRange.min;
|
||||||
|
//迭代搜索：搜索projectionCenter为中心5mm内z最大的的点为中心点
|
||||||
|
double searchR = 5.0;
|
||||||
|
int centerIdx = -1;
|
||||||
|
double maxZ = -1;
|
||||||
|
for (int i = 0; i < (int)surfacePoints.size(); i++)
|
||||||
|
{
|
||||||
|
double dist = sqrt(pow(surfacePoints[i].point.x - projectionCenter.point.x, 2) + pow(surfacePoints[i].point.y - projectionCenter.point.y, 2));
|
||||||
|
if (dist < searchR)
|
||||||
|
{
|
||||||
|
if (centerIdx < 0)
|
||||||
|
{
|
||||||
|
centerIdx = i;
|
||||||
|
maxZ = surfacePoints[i].point.z;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
|
||||||
|
if (((false == dirInverting) && (surfacePoints[centerIdx].point.z < surfacePoints[i].point.z)) ||
|
||||||
|
((true == dirInverting) && (surfacePoints[centerIdx].point.z > surfacePoints[i].point.z)))
|
||||||
|
{
|
||||||
|
centerIdx = i;
|
||||||
|
maxZ = surfacePoints[i].point.z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (centerIdx < 0)
|
||||||
|
{
|
||||||
|
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
int centerIdx_test = addrMapping[surfacePoints[centerIdx].lineIdx][surfacePoints[centerIdx].ptIdx];
|
||||||
|
//迭代一次
|
||||||
|
projectionCenter.lineIdx = surfacePoints[centerIdx].lineIdx;
|
||||||
|
projectionCenter.ptIdx = surfacePoints[centerIdx].ptIdx;
|
||||||
|
int sLine = projectionCenter.lineIdx - 5;
|
||||||
|
if (sLine < 0)
|
||||||
|
sLine = 0;
|
||||||
|
int eLine = projectionCenter.lineIdx + 5;
|
||||||
|
if (eLine >= (int)scanLines.size())
|
||||||
|
eLine = (int)scanLines.size() - 1;
|
||||||
|
int sPtIdx = projectionCenter.ptIdx - 5;
|
||||||
|
if (sPtIdx < 0)
|
||||||
|
sPtIdx = 0;
|
||||||
|
int ePtIdx = projectionCenter.ptIdx + 5;
|
||||||
|
if (ePtIdx >= (int)scanLines[0].size())
|
||||||
|
ePtIdx = (int)scanLines[0].size() - 1;
|
||||||
|
|
||||||
|
int objLine = -1;
|
||||||
|
int objPtIdx = -1;
|
||||||
|
maxZ = -1;
|
||||||
|
for (int line = sLine; line <= eLine; line++)
|
||||||
|
{
|
||||||
|
for (int ptIdx = sPtIdx; ptIdx <= ePtIdx; ptIdx++)
|
||||||
|
{
|
||||||
|
int idx_center = addrMapping[line][ptIdx];
|
||||||
|
if (idx_center < 0)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
int sL = line - 1;
|
||||||
|
if (sL < 0)
|
||||||
|
sL = 0;
|
||||||
|
int eL = line + 1;
|
||||||
|
if (eL >= (int)scanLines.size())
|
||||||
|
eL = (int)scanLines.size() - 1;
|
||||||
|
int sPt = ptIdx - 1;
|
||||||
|
if (sPt < 0)
|
||||||
|
sPt = 0;
|
||||||
|
int ePt = ptIdx + 1;
|
||||||
|
if (ePt >= (int)scanLines[0].size())
|
||||||
|
ePt = (int)scanLines[0].size() - 1;
|
||||||
|
int size = 0;
|
||||||
|
double sumZ = 0;
|
||||||
|
for (int i = sL; i <= eL; i++)
|
||||||
|
{
|
||||||
|
for (int j = sPt; j <= ePt; j++)
|
||||||
|
{
|
||||||
|
int idx = addrMapping[i][j];
|
||||||
|
if (idx >= 0)
|
||||||
|
{
|
||||||
|
if (roiProjectionData[idx].point.z > 1e-4)
|
||||||
|
{
|
||||||
|
sumZ += roiProjectionData[idx].point.z;
|
||||||
|
size++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (size > 0)
|
||||||
|
{
|
||||||
|
sumZ = sumZ / size;
|
||||||
|
if (maxZ < 0)
|
||||||
|
{
|
||||||
|
maxZ = sumZ;
|
||||||
|
objLine = line;
|
||||||
|
objPtIdx = ptIdx;
|
||||||
|
}
|
||||||
|
else if (((false == dirInverting) && (maxZ < sumZ)) || ((true == dirInverting) && (maxZ > sumZ)))
|
||||||
|
{
|
||||||
|
maxZ = sumZ;
|
||||||
|
objLine = line;
|
||||||
|
objPtIdx = ptIdx;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if ((objLine >= 0) && (objPtIdx >= 0))
|
||||||
|
centerIdx = addrMapping[objLine][objPtIdx];
|
||||||
|
//旋转回原坐标系
|
||||||
|
SVzNL3DPoint surfaceCenter = _ptRotate(roiProjectionData[centerIdx].point, rotatePara.invRMatrix);
|
||||||
|
//生成Rod信息
|
||||||
|
SSX_rodPoseInfo a_rod;
|
||||||
|
a_rod.center = surfaceCenter;
|
||||||
|
a_rod.axialDir = vec_axis;
|
||||||
|
screwInfo.push_back(a_rod);
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
//自制scanlines_copy数据，用于测试
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
scanLines[line][j].pt3D = scanLines_copy[line][j].pt3D;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
|
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
|
||||||
{
|
{
|
||||||
if (listData.size() == 0)
|
if (listData.size() == 0)
|
||||||
|
|||||||
@ -84,6 +84,18 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
|
|||||||
std::vector<SSX_rodPoseInfo>& screwInfo,
|
std::vector<SSX_rodPoseInfo>& screwInfo,
|
||||||
int* errCode);
|
int* errCode);
|
||||||
|
|
||||||
|
//PCA方法计算螺杆端部中心点位姿
|
||||||
|
//相对于sx_hexHeadScrewMeasure()，算法上（1）去除了水平段（2）使用PCA方法计算轴向
|
||||||
|
SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure_PCA(
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
//bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||||
|
const SSG_cornerParam cornerPara,
|
||||||
|
const SSG_outlierFilterParam filterParam,
|
||||||
|
const SSG_treeGrowParam growParam,
|
||||||
|
double rodDiameter,
|
||||||
|
std::vector<SSX_rodPoseInfo>& screwInfo,
|
||||||
|
int* errCode);
|
||||||
|
|
||||||
//计算定位盘中心点位姿
|
//计算定位盘中心点位姿
|
||||||
SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose(
|
SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose(
|
||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user