rodAndBarDetection version 1.2.4 :
根据定向盘左上点和右下点确定姿态向量
This commit is contained in:
parent
94aafeff97
commit
ab78509851
@ -175,6 +175,7 @@
|
||||
<ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_lineFeature.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_regionGrow.cpp" />
|
||||
<ClCompile Include="..\sourceCode\WD_holeDetection.cpp" />
|
||||
<ClCompile Include="..\sourceCode\WD_noiseFilter.cpp" />
|
||||
<ClCompile Include="..\sourceCode\WD_watershed.cpp" />
|
||||
</ItemGroup>
|
||||
|
||||
@ -224,16 +224,24 @@ void _outputChanneltInfo(char* fileName, std::vector<SSX_rodPoseInfo>& screwInfo
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputPlatePiseInfo(char* fileName, SSX_pointPoseInfo& centerInfo)
|
||||
void _outputPlatePiseInfo(char* fileName, SSX_platePoseInfo& centerInfo)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), normalDir_( %g, %g, %g ), xDir_( %g, %g, %g ), yDir_( %g, %g, %g )",
|
||||
centerInfo.center.x, centerInfo.center.y, centerInfo.center.z,
|
||||
centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z,
|
||||
centerInfo.xDir.x, centerInfo.xDir.y, centerInfo.xDir.z,
|
||||
centerInfo.yDir.x, centerInfo.yDir.y, centerInfo.yDir.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "定位盘: \n");
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " holeLT_(% g, % g, % g)\n", centerInfo.holeLT.x, centerInfo.holeLT.y, centerInfo.holeLT.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " holeRB_(% g, % g, % g)\n", centerInfo.holeRB.x, centerInfo.holeRB.y, centerInfo.holeRB.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " center_(% g, % g, % g)\n", centerInfo.center.x, centerInfo.center.y, centerInfo.center.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " normalDir_(% g, % g, % g)\n", centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " xDir_(% g, % g, % g)\n", centerInfo.xDir.x, centerInfo.xDir.y, centerInfo.xDir.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " yDir_(% g, % g, % g)\n", centerInfo.yDir.x, centerInfo.yDir.y, centerInfo.yDir.z);
|
||||
sw << dataStr << std::endl;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
@ -409,7 +417,7 @@ void _outputRGBDScan_RGBD(
|
||||
void _outputRGBDScan_RGBD_centerPose(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SSX_pointPoseInfo& poseInfo
|
||||
SSX_platePoseInfo& poseInfo
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
@ -477,7 +485,7 @@ void _outputRGBDScan_RGBD_centerPose(
|
||||
}
|
||||
|
||||
{
|
||||
sw << "Line_" << lineIdx << "_0_1" << std::endl;
|
||||
sw << "Line_" << lineIdx << "_0_3" << std::endl;
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 8;
|
||||
float x = (float)poseInfo.center.x;
|
||||
@ -487,6 +495,20 @@ void _outputRGBDScan_RGBD_centerPose(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
x = (float)poseInfo.holeLT.x;
|
||||
y = (float)poseInfo.holeLT.y;
|
||||
z = (float)poseInfo.holeLT.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
x = (float)poseInfo.holeRB.x;
|
||||
y = (float)poseInfo.holeRB.y;
|
||||
z = (float)poseInfo.holeRB.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
//输出法向
|
||||
size = 1;
|
||||
double len = 60;
|
||||
@ -535,6 +557,37 @@ void _outputRGBDScan_RGBD_centerPose(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
rgb = { 0, 250, 0 };
|
||||
basePt = { poseInfo.holeLT.x - len * poseInfo.xDir.x,
|
||||
poseInfo.holeLT.y - len * poseInfo.xDir.y,
|
||||
poseInfo.holeLT.z - len * poseInfo.xDir.z };
|
||||
pt2 = { poseInfo.holeLT.x + len * poseInfo.xDir.x,
|
||||
poseInfo.holeLT.y + len * poseInfo.xDir.y,
|
||||
poseInfo.holeLT.z + len * poseInfo.xDir.z };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
basePt = { poseInfo.holeRB.x - len * poseInfo.xDir.x,
|
||||
poseInfo.holeRB.y - len * poseInfo.xDir.y,
|
||||
poseInfo.holeRB.z - len * poseInfo.xDir.z };
|
||||
pt2 = { poseInfo.holeRB.x + len * poseInfo.xDir.x,
|
||||
poseInfo.holeRB.y + len * poseInfo.xDir.y,
|
||||
poseInfo.holeRB.z + len * poseInfo.xDir.z };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
@ -892,7 +945,7 @@ void locatingPlateTest(void)
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[LOCATING_PALTE_TEST_GROUP] = {
|
||||
{1,16},
|
||||
{1,17},
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
@ -902,7 +955,7 @@ void locatingPlateTest(void)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
//fidx =2;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
@ -926,7 +979,7 @@ void locatingPlateTest(void)
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
int errCode = 0;
|
||||
SSX_pointPoseInfo centerPose = sx_getLocationPlatePose(
|
||||
SSX_platePoseInfo centerPose = sx_getLocationPlatePose(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
& errCode);
|
||||
@ -1109,10 +1162,10 @@ void rodWeldSeamPosition_test(void)
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 5;
|
||||
growParam.yDeviation_max = 10.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.maxSkipDistance = 20.0;
|
||||
growParam.zDeviation_max = 10.0;//
|
||||
growParam.minLTypeTreeLen = 100; //mm, 螺杆长度
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
growParam.minLTypeTreeLen = 50; //mm, 螺杆长度
|
||||
growParam.minVTypeTreeLen = 50; //mm
|
||||
|
||||
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
||||
int errCode = 0;
|
||||
|
||||
@ -7,6 +7,9 @@
|
||||
//向量叉乘
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
|
||||
|
||||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle);
|
||||
|
||||
//滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
|
||||
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier(
|
||||
SVzNL3DPosition* lineData,
|
||||
@ -874,3 +877,15 @@ SG_APISHARED_EXPORT void scanLinesSmooting3x3(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridDataInput,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& smoothingData
|
||||
);
|
||||
|
||||
|
||||
SG_APISHARED_EXPORT void WD_getHoleInfo(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const double valieCommonNumRatio,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_v,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||||
std::vector<SSG_intPair>& validObjects
|
||||
);
|
||||
|
||||
@ -19,6 +19,17 @@ SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
||||
c.z = a.x * b.y - a.y * b.x;
|
||||
return c;
|
||||
}
|
||||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||||
SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle)
|
||||
{
|
||||
double sinTheta = sin(PI * angle / 180);
|
||||
double cosTheta = cos(PI * angle / 180);
|
||||
SVzNL3DPoint rotatePt;
|
||||
rotatePt.x = pt.x * cosTheta - pt.y * sinTheta;
|
||||
rotatePt.y = pt.x * sinTheta + pt.y * cosTheta;
|
||||
rotatePt.z = pt.z;
|
||||
return rotatePt;
|
||||
}
|
||||
|
||||
SVzNL3DRangeD sg_getScanDataROI(
|
||||
//¼ÆËãɨÃèROI
|
||||
|
||||
376
sourceCode/WD_holeDetection.cpp
Normal file
376
sourceCode/WD_holeDetection.cpp
Normal file
@ -0,0 +1,376 @@
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include <vector>
|
||||
|
||||
void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
|
||||
{
|
||||
if (a_pt.z > 1E-4)
|
||||
{
|
||||
if (roi.left < 0)
|
||||
{
|
||||
roi.left = a_pt.x;
|
||||
roi.right = a_pt.x;
|
||||
roi.left = a_pt.y;
|
||||
roi.right = a_pt.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.left > a_pt.x)
|
||||
roi.left = a_pt.x;
|
||||
if (roi.right < a_pt.x)
|
||||
roi.right = a_pt.x;
|
||||
if (roi.top > a_pt.y)
|
||||
roi.top = a_pt.y;
|
||||
if (roi.bottom < a_pt.y)
|
||||
roi.bottom = a_pt.y;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _updateRenge(SVzNLRange& range, int idx)
|
||||
{
|
||||
if (range.nMin < 0)
|
||||
{
|
||||
range.nMin = idx;
|
||||
range.nMax = idx;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (range.nMin > idx)
|
||||
range.nMin = idx;
|
||||
if (range.nMax < idx)
|
||||
range.nMax = idx;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
bool _checkExist(int id, std::vector<int>& buff)
|
||||
{
|
||||
for (int i = 0; i < (int)buff.size(); i++)
|
||||
{
|
||||
if (id == buff[i])
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void _searchNeighbours(
|
||||
int selfId, int chkExtening,
|
||||
int sLineIdx, int eLineIdx,
|
||||
SVzNLRange ptIdxRange,
|
||||
std::vector<std::vector<int>>& treeMask,
|
||||
std::vector<int>& neighbours)
|
||||
{
|
||||
int lineNum = (int)treeMask.size();
|
||||
int ptNum = treeMask[0].size();
|
||||
for (int line = sLineIdx - chkExtening; line <= eLineIdx + chkExtening; line++)
|
||||
{
|
||||
if ((line >= 0) && (line < lineNum))
|
||||
{
|
||||
for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++)
|
||||
{
|
||||
if ((ptIdx >= 0) && (ptIdx < ptNum))
|
||||
{
|
||||
if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId))
|
||||
{
|
||||
bool isExist = _checkExist(treeMask[line][ptIdx], neighbours);
|
||||
if (false == isExist)
|
||||
neighbours.push_back(treeMask[line][ptIdx]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void WD_getHoleInfo(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const double valieCommonNumRatio,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_v,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||||
std::vector<SSG_intPair>& validObjects
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
|
||||
std::vector<std::vector<int>> pointMask;
|
||||
pointMask.resize(lineNum);
|
||||
|
||||
//提取空白线段特征（孔特征）
|
||||
std::vector<std::vector<SWD_segFeature>> holeGaps;
|
||||
//提取线段端点特征
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 1047)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
pointMask[line].resize(lineData.size());
|
||||
std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0
|
||||
//滤波，滤除异常点
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||||
|
||||
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
|
||||
std::vector<SWD_segFeature> line_gaps;
|
||||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||||
{
|
||||
int ptIdx_1 = segs[i].start;
|
||||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||||
SWD_segFeature a_gap;
|
||||
a_gap.lineIdx = line;
|
||||
a_gap.startPtIdx = ptIdx_1;
|
||||
a_gap.endPtIdx = ptIdx_2;
|
||||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||||
line_gaps.push_back(a_gap);
|
||||
}
|
||||
holeGaps.push_back(line_gaps);
|
||||
}
|
||||
//特征生长
|
||||
wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);
|
||||
|
||||
//生成水平扫描
|
||||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||||
hLines_raw.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
hLines_raw[i].resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
hLines_raw[j][line] = scanLines[line][j];
|
||||
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
}
|
||||
}
|
||||
//水平arc特征提取
|
||||
std::vector<std::vector<SWD_segFeature>> holeGaps_h;
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
if (line == 974)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
|
||||
//滤波，滤除异常点
|
||||
int ptNum = (int)lineData.size();
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||||
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||||
|
||||
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
|
||||
std::vector<SWD_segFeature> line_gaps;
|
||||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||||
{
|
||||
int ptIdx_1 = segs[i].start;
|
||||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||||
SWD_segFeature a_gap;
|
||||
a_gap.lineIdx = line;
|
||||
a_gap.startPtIdx = ptIdx_1;
|
||||
a_gap.endPtIdx = ptIdx_2;
|
||||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||||
line_gaps.push_back(a_gap);
|
||||
}
|
||||
holeGaps_h.push_back(line_gaps);
|
||||
}
|
||||
//特征生长
|
||||
wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);
|
||||
|
||||
//创建Tree所在孔洞的Mask
|
||||
std::vector<std::vector<int>> treeMask_v;
|
||||
treeMask_v.resize(lineNum);
|
||||
std::vector<std::vector<int>> treeMask_h;
|
||||
treeMask_h.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
treeMask_v[i].resize(linePtNum);
|
||||
std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1);
|
||||
treeMask_h[i].resize(linePtNum);
|
||||
std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1);
|
||||
}
|
||||
//标注
|
||||
std::vector<SSG_treeInfo> treeInfo_v;
|
||||
treeInfo_v.resize(segTrees_v.size());
|
||||
for (int i = 0; i < (int)segTrees_v.size(); i++)
|
||||
{
|
||||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||||
treeInfo_v[i].treeIdx = i;
|
||||
treeInfo_v[i].sLineIdx = a_tree.sLineIdx;
|
||||
treeInfo_v[i].eLineIdx = a_tree.eLineIdx;
|
||||
treeInfo_v[i].vTreeFlag = 0;
|
||||
treeInfo_v[i].treeType = 0;
|
||||
treeInfo_v[i].roi = { -1, -1, -1, -1 };
|
||||
int nullPtSize = 0;
|
||||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||||
SVzNLRange ptIdxRange = { -1, -1 };
|
||||
if (a_tree.treeNodes.size() > 0)
|
||||
{
|
||||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||||
treeMask_v[a_seg.lineIdx][n] = i;
|
||||
|
||||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||||
//scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01;
|
||||
//scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01;
|
||||
}
|
||||
treeInfo_v[i].treeType = nullPtSize;
|
||||
treeInfo_v[i].roi = roi;
|
||||
treeInfo_v[i].ptIdxRange = ptIdxRange;
|
||||
}
|
||||
}
|
||||
std::vector<SSG_treeInfo> treeInfo_h;
|
||||
treeInfo_h.resize(segTrees_h.size());
|
||||
for (int i = 0; i < (int)segTrees_h.size(); i++)
|
||||
{
|
||||
SWD_segFeatureTree& a_tree = segTrees_h[i];
|
||||
treeInfo_h[i].treeIdx = i;
|
||||
treeInfo_h[i].sLineIdx = a_tree.sLineIdx;
|
||||
treeInfo_h[i].eLineIdx = a_tree.eLineIdx;
|
||||
treeInfo_h[i].vTreeFlag = 0;
|
||||
treeInfo_h[i].treeType = 0;
|
||||
treeInfo_h[i].roi = { -1, -1, -1, -1 };
|
||||
int nullPtSize = 0;
|
||||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||||
SVzNLRange ptIdxRange = { -1, -1 };
|
||||
if (a_tree.treeNodes.size() > 0)
|
||||
{
|
||||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||||
treeMask_h[n][a_seg.lineIdx] = i;
|
||||
|
||||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||||
_updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||||
_updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||||
//scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||||
//scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||||
}
|
||||
treeInfo_h[i].treeType = nullPtSize;
|
||||
treeInfo_h[i].roi = roi;
|
||||
treeInfo_h[i].ptIdxRange = ptIdxRange;
|
||||
}
|
||||
}
|
||||
//水平和垂直目标合并
|
||||
int vTreeSize = (int)segTrees_v.size();
|
||||
int hTreeSize = (int)segTrees_h.size();
|
||||
std::vector<std::vector<int>> treeHVInfo; //统计垂直和水平的tree的位置信息
|
||||
treeHVInfo.resize(vTreeSize);
|
||||
for (int i = 0; i < vTreeSize; i++)
|
||||
{
|
||||
treeHVInfo[i].resize(hTreeSize);
|
||||
std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0);
|
||||
}
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||||
{
|
||||
int idx_v = treeMask_v[line][ptIdx];
|
||||
int idx_h = treeMask_h[line][ptIdx];
|
||||
if ((idx_v >= 0) && (idx_h >= 0))
|
||||
treeHVInfo[idx_v][idx_h]++;
|
||||
}
|
||||
}
|
||||
|
||||
//生成候选目标
|
||||
std::vector<SSG_intPair> objects;
|
||||
for (int i = 0; i < vTreeSize; i++)
|
||||
{
|
||||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||||
if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
|
||||
int kkk = 1;
|
||||
|
||||
int totalSize_v = treeInfo_v[i].treeType;
|
||||
int commonSize = 0;
|
||||
int hTreeIdx = -1;
|
||||
for (int j = 0; j < hTreeSize; j++)
|
||||
{
|
||||
if (commonSize < treeHVInfo[i][j])
|
||||
{
|
||||
hTreeIdx = j;
|
||||
commonSize = treeHVInfo[i][j];
|
||||
}
|
||||
}
|
||||
if (hTreeIdx >= 0)
|
||||
{
|
||||
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
|
||||
int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
|
||||
int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
|
||||
if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
|
||||
{
|
||||
SSG_intPair a_obj;
|
||||
a_obj.data_0 = i;
|
||||
a_obj.data_1 = hTreeIdx;
|
||||
a_obj.idx = commonSize;
|
||||
objects.push_back(a_obj);
|
||||
treeInfo_v[i].data = commonSize;
|
||||
treeInfo_h[hTreeIdx].data = commonSize;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//滤除相邻。每个目标保留一个
|
||||
for (int i = 0; i < (int)objects.size(); i++)
|
||||
{
|
||||
int vTreeIdx = objects[i].data_0;
|
||||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||||
continue;
|
||||
|
||||
std::vector<int> neighbours;
|
||||
_searchNeighbours(vTreeIdx, 3,
|
||||
treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx,
|
||||
treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours);
|
||||
|
||||
int bestIdx = vTreeIdx;
|
||||
int maxValue = treeInfo_v[vTreeIdx].data;
|
||||
if (neighbours.size() > 0)
|
||||
{
|
||||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||||
{
|
||||
int idx = neighbours[j];
|
||||
if (maxValue < treeInfo_v[idx].data)
|
||||
{
|
||||
maxValue = treeInfo_v[idx].data;
|
||||
bestIdx = idx;
|
||||
}
|
||||
}
|
||||
if (bestIdx != vTreeIdx)
|
||||
treeInfo_v[vTreeIdx].vTreeFlag = -1;
|
||||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||||
{
|
||||
int idx = neighbours[j];
|
||||
if (bestIdx != idx)
|
||||
treeInfo_v[idx].vTreeFlag = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < (int)objects.size(); i++)
|
||||
{
|
||||
int vTreeIdx = objects[i].data_0;
|
||||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||||
continue;
|
||||
|
||||
validObjects.push_back(objects[i]);
|
||||
}
|
||||
}
|
||||
@ -12,7 +12,8 @@
|
||||
//version 1.2.1 : 增加了定位盘中心完整姿态输出
|
||||
//version 1.2.2 : 调整了定位盘中心姿态定义：法向（Z向）：向右；Y：向下
|
||||
//version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量
|
||||
std::string m_strVersion = "1.2.3";
|
||||
//version 1.2.4 : 根据定向盘左上点和右下点确定姿态向量
|
||||
std::string m_strVersion = "1.2.4";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -718,16 +719,52 @@ void _searchPlusCornerPeaks(
|
||||
}
|
||||
}
|
||||
|
||||
void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
|
||||
{
|
||||
if (a_pt.z > 1E-4)
|
||||
{
|
||||
if (roi.zRange.max < 0)
|
||||
{
|
||||
roi.xRange.min = a_pt.x;
|
||||
roi.xRange.max = a_pt.x;
|
||||
roi.yRange.min = a_pt.y;
|
||||
roi.yRange.max = a_pt.y;
|
||||
roi.zRange.min = a_pt.z;
|
||||
roi.zRange.max = a_pt.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.xRange.min > a_pt.x)
|
||||
roi.xRange.min = a_pt.x;
|
||||
if (roi.xRange.max < a_pt.x)
|
||||
roi.xRange.max = a_pt.x;
|
||||
if (roi.yRange.min > a_pt.y)
|
||||
roi.yRange.min = a_pt.y;
|
||||
if (roi.yRange.max < a_pt.y)
|
||||
roi.yRange.max = a_pt.y;
|
||||
if (roi.zRange.min > a_pt.z)
|
||||
roi.zRange.min = a_pt.z;
|
||||
if (roi.zRange.max < a_pt.z)
|
||||
roi.zRange.max = a_pt.z;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//计算定位盘中心点位姿
|
||||
SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
SSX_platePoseInfo sx_getLocationPlatePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
SSX_pointPoseInfo resultPose;
|
||||
SSX_platePoseInfo resultPose;
|
||||
resultPose.center = { 0, 0, 0 };
|
||||
resultPose.normalDir = { 0, 0, 0 };
|
||||
resultPose.holeLT = { 0, 0, 0 };
|
||||
resultPose.holeRB = { 0, 0, 0 };
|
||||
resultPose.xDir = { 0, 0, 0 };
|
||||
resultPose.yDir = { 0, 0, 0 };
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
{
|
||||
@ -960,10 +997,19 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
}
|
||||
//计算面参数: z = Ax + By + C
|
||||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
//std::vector<cv::Point3f> out_inliers;
|
||||
//Plane res = ransacFitPlane( Points3ds, out_inliers );
|
||||
#if 1
|
||||
std::vector<cv::Point3f> out_inliers;
|
||||
Plane res = ransacFitPlane( Points3ds, out_inliers );
|
||||
if (res.C < 0)
|
||||
{
|
||||
res.A = -res.A;
|
||||
res.B = -res.B;
|
||||
res.C = -res.C;
|
||||
res.D = -res.D;
|
||||
}
|
||||
#else
|
||||
Plane res = robustFitPlane(Points3ds);
|
||||
|
||||
#endif
|
||||
//std::vector<double> res;
|
||||
//vzCaculateLaserPlane(Points3ds, res);
|
||||
//计算投影向量
|
||||
@ -972,11 +1018,14 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);
|
||||
|
||||
//投影
|
||||
double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C);
|
||||
std::vector<cv::Point3f> projectPoints3ds;
|
||||
projectPoints3ds.resize(Points3ds.size());
|
||||
double sum_x = 0, sum_y = 0;
|
||||
double sum_x = 0, sum_y = 0, sum_z = 0;
|
||||
double sumZConter = 0;
|
||||
for (int i = 0; i < (int)Points3ds.size(); i++)
|
||||
{
|
||||
double distToPlane = abs(res.A * Points3ds[i].x + res.B * Points3ds[i].y + res.C * Points3ds[i].z + res.D) / normDataPlane;
|
||||
double x = Points3ds[i].x * poseR.planeCalib[0] + Points3ds[i].y * poseR.planeCalib[1] + Points3ds[i].z * poseR.planeCalib[2];
|
||||
double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5];
|
||||
double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8];
|
||||
@ -986,6 +1035,13 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
polarPoints[i].z = z;
|
||||
sum_x += x;
|
||||
sum_y += y;
|
||||
|
||||
if (distToPlane < 2.0)
|
||||
{
|
||||
sum_z += z;
|
||||
sumZConter++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
@ -1000,7 +1056,7 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
//计算质心
|
||||
double center_x = sum_x / (double)Points3ds.size();
|
||||
double center_y = sum_y / (double)Points3ds.size();
|
||||
|
||||
double center_z = sum_z / (double)sumZConter;
|
||||
//计算极坐标的R和Theta
|
||||
for (int pi = 0; pi < (int)polarPoints.size(); pi++)
|
||||
{
|
||||
@ -1024,6 +1080,224 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
double minCutAngleTh = 30;
|
||||
_searchPlusCornerPeaks(dirCornerAngles, minCutAngleTh, cornerPlusPeaks);
|
||||
|
||||
#if 1
|
||||
int cornerIdx_LT = -1;
|
||||
int cornerIdx_RB = -1;
|
||||
for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
|
||||
{
|
||||
if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90))
|
||||
cornerIdx_LT = i;
|
||||
if ((cornerPlusPeaks[i].point.angle > 180) && (cornerPlusPeaks[i].point.angle < 270))
|
||||
cornerIdx_RB = i;
|
||||
}
|
||||
if ((cornerIdx_LT < 0) || (cornerIdx_RB < 0))
|
||||
{
|
||||
*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
|
||||
return resultPose;
|
||||
}
|
||||
SSG_dirCornerAngle& corner_LT = cornerPlusPeaks[cornerIdx_LT];
|
||||
SSG_dirCornerAngle& corner_RB = cornerPlusPeaks[cornerIdx_RB];
|
||||
|
||||
//Z切割
|
||||
//内部参数
|
||||
double zTh_min = center_z - 1.0;
|
||||
double zTh_max = center_z + 5.0;
|
||||
std::vector< std::vector<SVzNL3DPosition>> zCutLines;
|
||||
zCutLines.resize(scanLines.size());
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
//行处理
|
||||
zCutLines[line].resize(linePtNum);
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
zCutLines[line][j] = scanLines[line][j];
|
||||
if ((scanLines[line][j].pt3D.z < zTh_min) || (scanLines[line][j].pt3D.z > zTh_max))
|
||||
zCutLines[line][j].pt3D = { 0.0, 0.0, 0.0 };
|
||||
}
|
||||
}
|
||||
//孔处理
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
std::vector<SWD_segFeatureTree> holeSegTrees_v;
|
||||
std::vector<SWD_segFeatureTree> holeSegTrees_h;
|
||||
SSG_lineSegParam hole_lineSegPara;
|
||||
hole_lineSegPara.distScale = 3.0;
|
||||
hole_lineSegPara.segGapTh_y = 15.0; //
|
||||
hole_lineSegPara.segGapTh_z = 0.0; //z方向间隔大于10mm认为是分段
|
||||
SSG_outlierFilterParam hole_filterParam;
|
||||
hole_filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
hole_filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam hole_growParam;
|
||||
hole_growParam.maxLineSkipNum = 2;
|
||||
hole_growParam.yDeviation_max = 4.0;
|
||||
hole_growParam.maxSkipDistance = 2.0;
|
||||
hole_growParam.zDeviation_max = 10.0;//
|
||||
hole_growParam.minLTypeTreeLen = 2.0; //mm
|
||||
hole_growParam.minVTypeTreeLen = 2.0; //mm
|
||||
double valieCommonNumRatio = 0.25;
|
||||
WD_getHoleInfo(
|
||||
zCutLines,
|
||||
hole_lineSegPara,
|
||||
hole_filterParam,
|
||||
hole_growParam,
|
||||
valieCommonNumRatio,
|
||||
holeSegTrees_v,
|
||||
holeSegTrees_h,
|
||||
validObjects
|
||||
);
|
||||
|
||||
//寻找左上孔和右下孔
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
for (int j = 0; j < (int)scanLines[i].size(); j++)
|
||||
zCutLines[i][j].nPointIdx = 0; //清零
|
||||
}
|
||||
//生成聚类信息，
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||||
for (int i = 0; i < (int)validObjects.size(); i++)
|
||||
{
|
||||
std::vector< SVzNL2DPoint> a_cluster;
|
||||
SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };
|
||||
|
||||
int vTreeIdx = validObjects[i].data_0;
|
||||
int hTreeIdx = validObjects[i].data_1;
|
||||
for (int m = 0; m < (int)holeSegTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = holeSegTrees_v[vTreeIdx].treeNodes[m];
|
||||
if (zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||||
{
|
||||
zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||||
scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 3; // 0x01;
|
||||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||||
}
|
||||
if (zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||||
{
|
||||
zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||||
scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 3; // 0x01;
|
||||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||||
}
|
||||
}
|
||||
for (int m = 0; m < (int)holeSegTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = holeSegTrees_h[hTreeIdx].treeNodes[m];
|
||||
if (zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||||
{
|
||||
zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02;
|
||||
scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02;
|
||||
SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, zCutLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||||
}
|
||||
if (zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||||
{
|
||||
zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02;
|
||||
scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02;
|
||||
SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, zCutLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||||
}
|
||||
}
|
||||
clusters.push_back(a_cluster);
|
||||
clustersRoi3D.push_back(a_roi3D);
|
||||
}
|
||||
|
||||
//聚类结果分析:搜索距左上和右下点最后的目标
|
||||
//内部参数
|
||||
double minHoleSize = 10.0;
|
||||
double maxHoleSize = 24.0;
|
||||
double maxCornerHoleDistance = 40; //角点到孔的最大距离
|
||||
int clusterSize = (int)clusters.size();
|
||||
int clusterIdx_LT = -1;
|
||||
double minDistLT = -1;
|
||||
int clusterIdx_RB = -1;
|
||||
double minDistRB = -1;
|
||||
for (int i = 0; i < clusterSize; i++)
|
||||
{
|
||||
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
|
||||
double L = a_roi.xRange.max - a_roi.xRange.min;
|
||||
double W = a_roi.yRange.max - a_roi.yRange.min;
|
||||
double cx = (a_roi.xRange.max + a_roi.xRange.min) / 2;
|
||||
double cy = (a_roi.yRange.max + a_roi.yRange.min) / 2;
|
||||
|
||||
if ((L > minHoleSize) && (L < maxHoleSize) && (W > minHoleSize) && (W < maxHoleSize))
|
||||
{
|
||||
double dist_1 = sqrt(pow(corner_LT.point.x - cx, 2) + pow(corner_LT.point.y - cy, 2));
|
||||
if ((minDistLT < 0) || (minDistLT > dist_1))
|
||||
{
|
||||
minDistLT = dist_1;
|
||||
clusterIdx_LT = i;
|
||||
}
|
||||
|
||||
double dist_2 = sqrt(pow(corner_RB.point.x - cx, 2) + pow(corner_RB.point.y - cy, 2));
|
||||
if ((minDistRB < 0) || (minDistRB > dist_2))
|
||||
{
|
||||
minDistRB = dist_2;
|
||||
clusterIdx_RB = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
if( (clusterIdx_LT < 0) || (clusterIdx_RB < 0) || (minDistLT > maxCornerHoleDistance) || (minDistRB > maxCornerHoleDistance))
|
||||
{
|
||||
*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
//目标圆拟合
|
||||
std::vector<SVzNL3DPoint> pointArrayLT;
|
||||
int clusterPtSizeLT = (int)clusters[clusterIdx_LT].size();
|
||||
for (int i = 0; i < clusterPtSizeLT; i++)
|
||||
{
|
||||
SVzNL2DPoint a_pos = clusters[clusterIdx_LT][i];
|
||||
int lineIdx = a_pos.x;
|
||||
int ptIdx = a_pos.y;
|
||||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||||
pointArrayLT.push_back(a_pt3d);
|
||||
}
|
||||
//圆拟合
|
||||
SVzNL3DPoint centerLT;
|
||||
double radiusLT;
|
||||
double err = fitCircleByLeastSquare(pointArrayLT, centerLT, radiusLT);
|
||||
centerLT.z = center_z;
|
||||
|
||||
std::vector<SVzNL3DPoint> pointArrayRB;
|
||||
int clusterPtSizeRB = (int)clusters[clusterIdx_RB].size();
|
||||
for (int i = 0; i < clusterPtSizeRB; i++)
|
||||
{
|
||||
SVzNL2DPoint a_pos = clusters[clusterIdx_RB][i];
|
||||
int lineIdx = a_pos.x;
|
||||
int ptIdx = a_pos.y;
|
||||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||||
pointArrayRB.push_back(a_pt3d);
|
||||
}
|
||||
//圆拟合
|
||||
SVzNL3DPoint centerRB;
|
||||
double radiusRB;
|
||||
err = fitCircleByLeastSquare(pointArrayRB, centerRB, radiusRB);
|
||||
centerRB.z = center_z;
|
||||
|
||||
SVzNL3DPoint refVec = { (centerRB.x-centerLT.x)/2, (centerRB.y - centerLT.y)/2, center_z};
|
||||
double angleToHorizon = -46; //两孔连线与水平线夹角46度
|
||||
double angleToCenter = 3; //两孔连线中点与定位盘中心夹角3度
|
||||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||||
SVzNL3DPoint rVec_1 = wd_rotate2D(refVec, angleToCenter);
|
||||
center_x = rVec_1.x + centerLT.x;
|
||||
center_y = rVec_1.y + centerLT.y;
|
||||
//center_z = center_z + 34.0;
|
||||
SVzNL3DPoint xDir = wd_rotate2D(refVec, angleToHorizon);
|
||||
double normData = sqrt(pow(xDir.x, 2) + pow(xDir.y, 2));
|
||||
xDir.x = xDir.x / normData;
|
||||
xDir.y = xDir.y / normData;
|
||||
xDir.z = 0;
|
||||
|
||||
resultPose.holeLT = centerLT;
|
||||
resultPose.holeRB = centerRB;
|
||||
|
||||
center_z += 33.8; //定位盘中心点深33.8
|
||||
#else
|
||||
//生成直线段数据
|
||||
int cornerIdx_0 = -1;
|
||||
int cornerIdx_1 = -1;
|
||||
@ -1084,7 +1358,7 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
lineFitting(lineFittingPts, &_k, &_b);
|
||||
double normData = sqrt(pow(_k, 2) + 1);
|
||||
SVzNL3DPoint xDir = { 1.0/normData, _k/normData, 0.0 };
|
||||
|
||||
|
||||
//计算4个点
|
||||
SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R };
|
||||
SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R };
|
||||
@ -1166,7 +1440,8 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
}
|
||||
}
|
||||
double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
|
||||
resultPose.center = { center_x, center_y, center_z };
|
||||
#endif
|
||||
resultPose.center = { center_x, center_y, center_z };
|
||||
resultPose.xDir = { 0.0, 0.0, -1.0 }; // { 1.0, 0.0, 0 };
|
||||
//resultPose.yDir = { 0.0, 1.0, 0 };
|
||||
resultPose.normalDir = xDir; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
|
||||
@ -1200,6 +1475,16 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8];
|
||||
resultPose.yDir = { x, y, z };
|
||||
|
||||
x = resultPose.holeLT.x * poseR.invRMatrix[0] + resultPose.holeLT.y * poseR.invRMatrix[1] + resultPose.holeLT.z * poseR.invRMatrix[2];
|
||||
y = resultPose.holeLT.x * poseR.invRMatrix[3] + resultPose.holeLT.y * poseR.invRMatrix[4] + resultPose.holeLT.z * poseR.invRMatrix[5];
|
||||
z = resultPose.holeLT.x * poseR.invRMatrix[6] + resultPose.holeLT.y * poseR.invRMatrix[7] + resultPose.holeLT.z * poseR.invRMatrix[8];
|
||||
resultPose.holeLT = { x, y, z };
|
||||
|
||||
x = resultPose.holeRB.x * poseR.invRMatrix[0] + resultPose.holeRB.y * poseR.invRMatrix[1] + resultPose.holeRB.z * poseR.invRMatrix[2];
|
||||
y = resultPose.holeRB.x * poseR.invRMatrix[3] + resultPose.holeRB.y * poseR.invRMatrix[4] + resultPose.holeRB.z * poseR.invRMatrix[5];
|
||||
z = resultPose.holeRB.x * poseR.invRMatrix[6] + resultPose.holeRB.y * poseR.invRMatrix[7] + resultPose.holeRB.z * poseR.invRMatrix[8];
|
||||
resultPose.holeRB = { x, y, z };
|
||||
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
|
||||
@ -17,7 +17,9 @@ typedef struct
|
||||
SVzNL3DPoint xDir;
|
||||
SVzNL3DPoint yDir;
|
||||
SVzNL3DPoint normalDir; //法向向量
|
||||
}SSX_pointPoseInfo; //
|
||||
SVzNL3DPoint holeLT;
|
||||
SVzNL3DPoint holeRB;
|
||||
}SSX_platePoseInfo; //
|
||||
|
||||
typedef struct
|
||||
{
|
||||
@ -71,7 +73,7 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
|
||||
int* errCode);
|
||||
|
||||
//计算定位盘中心点位姿
|
||||
SG_APISHARED_EXPORT SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
int* errCode);
|
||||
|
||||
@ -159,32 +159,6 @@ double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint see
|
||||
return (zSum / hist);
|
||||
}
|
||||
|
||||
void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
|
||||
{
|
||||
if (a_pt.z > 1E-4)
|
||||
{
|
||||
if (roi.left < 0)
|
||||
{
|
||||
roi.left = a_pt.x;
|
||||
roi.right = a_pt.x;
|
||||
roi.left = a_pt.y;
|
||||
roi.right = a_pt.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.left > a_pt.x)
|
||||
roi.left = a_pt.x;
|
||||
if (roi.right < a_pt.x)
|
||||
roi.right = a_pt.x;
|
||||
if (roi.top > a_pt.y)
|
||||
roi.top = a_pt.y;
|
||||
if (roi.bottom < a_pt.y)
|
||||
roi.bottom = a_pt.y;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
|
||||
{
|
||||
if (a_pt.z > 1E-4)
|
||||
@ -217,6 +191,33 @@ void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
|
||||
return;
|
||||
}
|
||||
|
||||
#if 0
|
||||
void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
|
||||
{
|
||||
if (a_pt.z > 1E-4)
|
||||
{
|
||||
if (roi.left < 0)
|
||||
{
|
||||
roi.left = a_pt.x;
|
||||
roi.right = a_pt.x;
|
||||
roi.left = a_pt.y;
|
||||
roi.right = a_pt.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.left > a_pt.x)
|
||||
roi.left = a_pt.x;
|
||||
if (roi.right < a_pt.x)
|
||||
roi.right = a_pt.x;
|
||||
if (roi.top > a_pt.y)
|
||||
roi.top = a_pt.y;
|
||||
if (roi.bottom < a_pt.y)
|
||||
roi.bottom = a_pt.y;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _updateRenge(SVzNLRange& range, int idx)
|
||||
{
|
||||
if (range.nMin < 0)
|
||||
@ -274,39 +275,7 @@ void _searchNeighbours(
|
||||
return;
|
||||
}
|
||||
|
||||
bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
|
||||
{
|
||||
return a.center.x < b.center.x;
|
||||
}
|
||||
void _getYTopLine(
|
||||
std::vector< WD_workpieceInfo>& workpieceSrc,
|
||||
std::vector< WD_workpieceInfo>& firstLine,
|
||||
std::vector< WD_workpieceInfo>& restWorkpiece,
|
||||
double yLayerTh)
|
||||
{
|
||||
//搜索Y最小值
|
||||
if (workpieceSrc.size() == 0)
|
||||
return;
|
||||
|
||||
double minY = workpieceSrc[0].center.y;
|
||||
for (int i = 1; i < (int)workpieceSrc.size(); i++)
|
||||
{
|
||||
if (minY > workpieceSrc[i].center.y)
|
||||
minY = workpieceSrc[i].center.y;
|
||||
}
|
||||
double topLayerTh = minY + yLayerTh;
|
||||
for (int i = 0; i < (int)workpieceSrc.size(); i++)
|
||||
{
|
||||
if (workpieceSrc[i].center.y < topLayerTh)
|
||||
firstLine.push_back(workpieceSrc[i]);
|
||||
else
|
||||
restWorkpiece.push_back(workpieceSrc[i]);
|
||||
}
|
||||
std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
|
||||
return;
|
||||
}
|
||||
|
||||
void wd_getHoleInfo(
|
||||
void WD_getHoleInfo(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
@ -595,6 +564,40 @@ void wd_getHoleInfo(
|
||||
validObjects.push_back(objects[i]);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
|
||||
{
|
||||
return a.center.x < b.center.x;
|
||||
}
|
||||
void _getYTopLine(
|
||||
std::vector< WD_workpieceInfo>& workpieceSrc,
|
||||
std::vector< WD_workpieceInfo>& firstLine,
|
||||
std::vector< WD_workpieceInfo>& restWorkpiece,
|
||||
double yLayerTh)
|
||||
{
|
||||
//搜索Y最小值
|
||||
if (workpieceSrc.size() == 0)
|
||||
return;
|
||||
|
||||
double minY = workpieceSrc[0].center.y;
|
||||
for (int i = 1; i < (int)workpieceSrc.size(); i++)
|
||||
{
|
||||
if (minY > workpieceSrc[i].center.y)
|
||||
minY = workpieceSrc[i].center.y;
|
||||
}
|
||||
double topLayerTh = minY + yLayerTh;
|
||||
for (int i = 0; i < (int)workpieceSrc.size(); i++)
|
||||
{
|
||||
if (workpieceSrc[i].center.y < topLayerTh)
|
||||
firstLine.push_back(workpieceSrc[i]);
|
||||
else
|
||||
restWorkpiece.push_back(workpieceSrc[i]);
|
||||
}
|
||||
std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
|
||||
return;
|
||||
}
|
||||
|
||||
SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData)
|
||||
{
|
||||
if (listData.size() == 0)
|
||||
@ -698,7 +701,7 @@ void wd_workpieceHolePositioning(
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
double valieCommonNumRatio = 0.25;
|
||||
wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
|
||||
WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
|
||||
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user