rodAndBarDetection version 1.2.7 :
(1)根据实际点云修正了螺杆定位算法（2）定位盘位姿计算改回RANSAC
This commit is contained in:
parent
4fc5410114
commit
5df08483c9
@ -77,6 +77,8 @@ if(OPENCV_VERSION STREQUAL "320")
|
||||
ADD_LIBRARY(workpieceHolePositioning SHARED sourceCode/workpieceHolePositioning.cpp)
|
||||
|
||||
# ADD_LIBRARY(motorStatorPosition SHARED sourceCode/motorStatorPosition.cpp)
|
||||
|
||||
ADD_LIBRARY(steelPipeDetection SHARED sourceCode/steelPipeDetection.cpp)
|
||||
|
||||
elseif(OPENCV_VERSION STREQUAL "480")
|
||||
ADD_LIBRARY(binocularMarkCam SHARED sourceCode/binocularMarkCam.cpp
|
||||
|
||||
@ -227,6 +227,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "YouJiang_sheetPositioning_t
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B} = {BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "steelPipeDetection", "steelPipeDetection\steelPipeDetection.vcxproj", "{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "steelPipeDetection_test", "steelPipeDetection_test\steelPipeDetection_test.vcxproj", "{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d} = {8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
@ -571,6 +582,22 @@ Global
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}.Release|x64.Build.0 = Release|x64
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}.Release|x86.ActiveCfg = Release|Win32
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}.Release|x86.Build.0 = Release|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x64.Build.0 = Debug|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x86.Build.0 = Debug|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x64.ActiveCfg = Release|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x64.Build.0 = Release|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x86.ActiveCfg = Release|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x86.Build.0 = Release|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x64.Build.0 = Debug|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x86.Build.0 = Debug|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x64.ActiveCfg = Release|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x64.Build.0 = Release|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x86.ActiveCfg = Release|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x86.Build.0 = Release|Win32
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Debug|x64.Build.0 = Debug|x64
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
|
||||
@ -333,9 +333,13 @@ void _outputRGBDScan_RGBD(
|
||||
else if (pt3D->nPointIdx == 2)
|
||||
{
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 5;
|
||||
size = 3;
|
||||
}
|
||||
else if (pt3D->nPointIdx == 4)
|
||||
{
|
||||
rgb = { 250, 250, 0 };
|
||||
size = 6;
|
||||
}
|
||||
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
@ -889,27 +893,31 @@ void _outputRGBDScan_RGBD_weldSeam(
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define SCREW_TEST_GROUP 1
|
||||
#define SCREW_TEST_GROUP 2
|
||||
void screwTest(void)
|
||||
{
|
||||
const char* dataPath[SCREW_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云2/上方两根/", //1
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[SCREW_TEST_GROUP] = {
|
||||
{1,4},
|
||||
{1,4},{1,30}
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 0; grp < SCREW_TEST_GROUP; grp++)
|
||||
for (int grp = 1; grp < SCREW_TEST_GROUP; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =7;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
if(1 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C299.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
@ -920,7 +928,13 @@ void screwTest(void)
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
double rodDiameter = 10.0;
|
||||
double rodDiameter;
|
||||
if (grp == 0)
|
||||
rodDiameter = 10.0;
|
||||
else
|
||||
rodDiameter = 28.0; //现场螺杆直径28mm
|
||||
|
||||
//double rodDiameter = 10.0;
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
@ -1223,8 +1237,8 @@ void rodWeldSeamPosition_test(void)
|
||||
|
||||
int main()
|
||||
{
|
||||
#if 0 //螺杆定位测试
|
||||
#if 0
|
||||
#if 1 //螺杆定位测试
|
||||
#if 1
|
||||
screwTest();
|
||||
#else
|
||||
locatingPlateTest();
|
||||
|
||||
@ -166,6 +166,14 @@ SG_APISHARED_EXPORT void wd_lineDataSegment_dist(
|
||||
const int minSegSize
|
||||
);
|
||||
|
||||
//根据距离连续性分段
|
||||
SG_APISHARED_EXPORT void wd_lineDataSegment_dist_2(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const double maxDistTh,
|
||||
const int minSegSize
|
||||
);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_lineDataSegment_zDist(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
@ -205,6 +213,29 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod(
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的圆环的上半段弧。
|
||||
/// seg端点：z距离大于门限
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐段计算前向角和后向角
|
||||
/// （2）搜索同方向的拐角连续段
|
||||
/// （2）搜索Z极值，
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
///
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const int minSegSize,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||||
SG_APISHARED_EXPORT void wd_searchCornerPeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
|
||||
@ -3878,6 +3878,60 @@ void wd_lineDataSegment_dist(
|
||||
}
|
||||
}
|
||||
|
||||
//根据距离连续性分段
|
||||
void wd_lineDataSegment_dist_2(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const double maxDistTh,
|
||||
const int minSegSize
|
||||
)
|
||||
{
|
||||
int dataSize = (int)lineData.size();
|
||||
int runIdx = 1;
|
||||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||||
double pre_z = 0;
|
||||
double pre_y = 0;
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if (a_run.len < 0)
|
||||
{
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
double dist = sqrt(pow(lineData[i].pt3D.y - pre_y, 2) + pow(lineData[i].pt3D.z - pre_z, 2));
|
||||
if (dist < maxDistTh)
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_run.value = runIdx;
|
||||
runIdx++;
|
||||
if (a_run.len >= minSegSize)
|
||||
segs.push_back(a_run);
|
||||
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
}
|
||||
pre_y = lineData[i].pt3D.y;
|
||||
pre_z = lineData[i].pt3D.z;
|
||||
}
|
||||
}
|
||||
if (a_run.len > 0)
|
||||
{
|
||||
if (a_run.len >= minSegSize)
|
||||
segs.push_back(a_run);
|
||||
}
|
||||
}
|
||||
|
||||
//根据Z连续性分段
|
||||
void wd_lineDataSegment_zDist(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
@ -3959,6 +4013,18 @@ void _computeDirAngle_perSeg(
|
||||
)
|
||||
{
|
||||
ptDirAngles.resize(vldPts.size());
|
||||
for (int i = 0; i < (int)vldPts.size(); i++)
|
||||
{
|
||||
ptDirAngles[i].pntIdx = -1;
|
||||
ptDirAngles[i].type = -1;
|
||||
ptDirAngles[i].forwardPntIdx = 0;
|
||||
ptDirAngles[i].backwardPntIdx = 0;
|
||||
ptDirAngles[i].forwardAngle = 0;
|
||||
ptDirAngles[i].backwardAngle = 0;
|
||||
ptDirAngles[i].corner = 0;
|
||||
ptDirAngles[i].forwardDiffZ = 0;
|
||||
ptDirAngles[i].backwardDiffZ = 0;
|
||||
}
|
||||
//逐段进行
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
@ -3992,24 +4058,89 @@ void _computeDirAngle_perSeg(
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
if ((pre_i < 0) || (post_i < 0))
|
||||
{
|
||||
ptDirAngles[i].pntIdx = -1;
|
||||
ptDirAngles[i].forwardPntIdx = 0;
|
||||
ptDirAngles[i].backwardPntIdx = 0;
|
||||
ptDirAngles[i].forwardAngle = 0;
|
||||
ptDirAngles[i].backwardAngle = 0;
|
||||
ptDirAngles[i].corner = 0;
|
||||
ptDirAngles[i].forwardDiffZ = 0;
|
||||
ptDirAngles[i].backwardDiffZ = 0;
|
||||
}
|
||||
else
|
||||
if ((pre_i >= 0) && (post_i >= 0))
|
||||
{
|
||||
double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
|
||||
double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||||
ptDirAngles[i].pntIdx = i;
|
||||
ptDirAngles[i].type = segIdx;
|
||||
ptDirAngles[i].forwardPntIdx = post_i;
|
||||
ptDirAngles[i].backwardPntIdx = pre_i;
|
||||
ptDirAngles[i].forwardAngle = forwardAngle;
|
||||
ptDirAngles[i].backwardAngle = backwardAngle;
|
||||
ptDirAngles[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||||
ptDirAngles[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
|
||||
ptDirAngles[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//逐段计算方向角
|
||||
void _computeDirAngle_perSeg_2(
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector< SSG_pntDirAngle>& ptDirAngles
|
||||
)
|
||||
{
|
||||
ptDirAngles.resize(vldPts.size());
|
||||
for (int i = 0; i < (int)vldPts.size(); i++)
|
||||
{
|
||||
ptDirAngles[i].pntIdx = -1;
|
||||
ptDirAngles[i].type = -1;
|
||||
ptDirAngles[i].forwardPntIdx = 0;
|
||||
ptDirAngles[i].backwardPntIdx = 0;
|
||||
ptDirAngles[i].forwardAngle = 0;
|
||||
ptDirAngles[i].backwardAngle = 0;
|
||||
ptDirAngles[i].corner = 0;
|
||||
ptDirAngles[i].forwardDiffZ = 0;
|
||||
ptDirAngles[i].backwardDiffZ = 0;
|
||||
}
|
||||
|
||||
//逐段进行
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
//前向寻找
|
||||
int pre_i = -1;
|
||||
for (int j = i - 1; j >= vPtIdxStart; j--)
|
||||
{
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
{
|
||||
pre_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//后向寻找
|
||||
int post_i = -1;
|
||||
for (int j = i + 1; j <= vPtIdxEnd; j++)
|
||||
{
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
{
|
||||
post_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
if ((pre_i >= 0) && (post_i >= 0))
|
||||
{
|
||||
double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
|
||||
double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||||
ptDirAngles[i].pntIdx = i;
|
||||
ptDirAngles[i].type = segIdx;
|
||||
ptDirAngles[i].forwardPntIdx = post_i;
|
||||
ptDirAngles[i].backwardPntIdx = pre_i;
|
||||
ptDirAngles[i].forwardAngle = forwardAngle;
|
||||
@ -4286,18 +4417,21 @@ bool _chkRodArcFeature(
|
||||
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
|
||||
if (dist > searchWin)
|
||||
break;
|
||||
if (ptDirAngles[i].pntIdx >= 0)
|
||||
{
|
||||
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
|
||||
if (validCorner < 0)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
validCorner = ptDirAngles[i].type;
|
||||
}
|
||||
else if (realPkCorner > flatAngle)
|
||||
{
|
||||
|
||||
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
|
||||
if( validCorner < 0)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
validCorner = 0;
|
||||
}
|
||||
else if (realPkCorner > flatAngle)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
@ -4313,19 +4447,24 @@ bool _chkRodArcFeature(
|
||||
if (dist > searchWin)
|
||||
break;
|
||||
|
||||
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
|
||||
if (validCorner < 0)
|
||||
if (ptDirAngles[i].pntIdx >= 0)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
validCorner = 0;
|
||||
}
|
||||
else if (realPkCorner > flatAngle)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
|
||||
if (validCorner < 0)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
validCorner = ptDirAngles[i].type;
|
||||
}
|
||||
else if (realPkCorner > flatAngle)
|
||||
{
|
||||
realPkCorner = flatAngle;
|
||||
realPkIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (validCorner < 0)
|
||||
return false;
|
||||
|
||||
if ((ptDirAngles[realPkIdx].forwardAngle > 0) && (ptDirAngles[realPkIdx].backwardAngle < 0) &&
|
||||
(realPkCorner <10) &&(ptDirAngles[realPkIdx].corner < -arcPerPointCornerMinValue)) //前向角为正，后向角为负，两者值基本相等（<10), 拐角为负，绝对值大于10度
|
||||
@ -4334,8 +4473,11 @@ bool _chkRodArcFeature(
|
||||
int pre_idx = -1;
|
||||
for (int i = realPkIdx - 1; i >= 0; i--)
|
||||
{
|
||||
if (ptDirAngles[i].type < 0)
|
||||
if ((ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0))
|
||||
continue;
|
||||
else if (validCorner != ptDirAngles[i].type)
|
||||
break;
|
||||
|
||||
if (ptDirAngles[i].corner < -arcPerPointCornerMinValue)
|
||||
pre_idx = i;
|
||||
else
|
||||
@ -4345,8 +4487,11 @@ bool _chkRodArcFeature(
|
||||
int post_idx = -1;
|
||||
for (int i = realPkIdx + 1; i < (int)ptDirAngles.size(); i++)
|
||||
{
|
||||
if (ptDirAngles[i].type < 0)
|
||||
if ((ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0))
|
||||
continue;
|
||||
else if (validCorner != ptDirAngles[i].type)
|
||||
break;
|
||||
|
||||
if (ptDirAngles[i].corner < -arcPerPointCornerMinValue)
|
||||
post_idx = i;
|
||||
else
|
||||
@ -4428,6 +4573,79 @@ void wd_getRodArcFeature_peakCornerMethod(
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的圆环的上半段弧。
|
||||
/// seg端点：z距离大于门限
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐段计算前向角和后向角
|
||||
/// （2）搜索同方向的拐角连续段
|
||||
/// （2）搜索Z极值，
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
///
|
||||
/// </summary>
|
||||
void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const int minSegSize,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
)
|
||||
{
|
||||
double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
|
||||
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
|
||||
|
||||
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);
|
||||
|
||||
int dataSize = (int)lineData.size();
|
||||
//搜索z极值。
|
||||
std::vector< SSG_basicFeature1D> localZMax;
|
||||
std::vector< SSG_basicFeature1D> localZMin;
|
||||
sg_getLineLocalPeaks_2(lineData, lineIdx, peakChkWin, localZMax, localZMin);
|
||||
|
||||
//逐个极小值点判断
|
||||
int peakNum = (int)localZMin.size();
|
||||
for (int i = 0; i < peakNum; i++)
|
||||
{
|
||||
int pkIdx = localZMin[i].jumpPos2D.y;
|
||||
SSG_intPair a_rodPos;
|
||||
bool isArc = _chkRodArcFeature(
|
||||
pkIdx, cornerPara.scale,
|
||||
ptDirAngles, lineData,
|
||||
arcPerPointCornerMinValue, arcTotalCornerMinValue,
|
||||
a_rodPos);
|
||||
if (true == isArc)
|
||||
{
|
||||
SWD_rodArcFeature a_rodArc;
|
||||
memset(&a_rodArc, 0, sizeof(SWD_rodArcFeature));
|
||||
a_rodArc.lineIdx = lineIdx;
|
||||
a_rodArc.startPtIdx = a_rodPos.data_0;
|
||||
a_rodArc.endPtIdx = a_rodPos.data_1;
|
||||
a_rodArc.peakPtIdx = a_rodPos.idx;
|
||||
a_rodArc.startPt = lineData[a_rodPos.data_0].pt3D;
|
||||
a_rodArc.endPt = lineData[a_rodPos.data_1].pt3D;
|
||||
a_rodArc.peakPt = lineData[a_rodPos.idx].pt3D;
|
||||
a_rodArc.featureValue = ptDirAngles[a_rodPos.idx].corner;
|
||||
line_rodArcs.push_back(a_rodArc);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||||
void wd_searchCornerPeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
|
||||
@ -15,7 +15,8 @@
|
||||
//version 1.2.4 : 根据定向盘左上点和右下点确定姿态向量
|
||||
//version 1.2.5 : 添加筑裕钢筋焊缝提取API
|
||||
//version 1.2.6 : 定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高
|
||||
std::string m_strVersion = "1.2.6";
|
||||
//version 1.2.7 : (1)根据实际点云修正了螺杆定位算法（2）定位盘位姿计算改回RANSAC
|
||||
std::string m_strVersion = "1.2.7";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -74,7 +75,7 @@ SVzNL3DPoint getArcPeak(
|
||||
|
||||
SVzNL3DPoint getArcPeak_parabolaFitting(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SWD_segFeature& a_arcFeature,
|
||||
SWD_rodArcFeature& a_arcFeature,
|
||||
SVzNL2DPoint& arcPos)
|
||||
{
|
||||
std::vector<cv::Point2d> points;
|
||||
@ -99,7 +100,7 @@ SVzNL3DPoint getArcPeak_parabolaFitting(
|
||||
mse, max_err);
|
||||
double yP = -b / (2 * a);
|
||||
//寻找与yP最近的点作为Peak点
|
||||
SVzNL3DPoint arcPeak;
|
||||
SVzNL3DPoint arcPeak = {0, 0, 0};
|
||||
double minDist = -1;
|
||||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||||
{
|
||||
@ -131,6 +132,8 @@ void xoyROIProjection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const double* rtMatrix,
|
||||
SSG_ROIRectD& roi_xoy,
|
||||
int objClusterId,
|
||||
std::vector<std::vector<int>>& clusterIdMask,
|
||||
std::vector<SVzNL3DPoint>& projectPoints
|
||||
)
|
||||
{
|
||||
@ -144,6 +147,10 @@ void xoyROIProjection(
|
||||
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
||||
if (a_pt.z < 1e-4)
|
||||
continue;
|
||||
int cId = clusterIdMask[line][i];
|
||||
if (cId != objClusterId)
|
||||
continue;
|
||||
|
||||
double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
|
||||
double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
|
||||
double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
|
||||
@ -223,18 +230,83 @@ void rodAarcFeatueDetection(
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 827)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
|
||||
// Filter outliers from line data
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
// Extract rod arc features
|
||||
std::vector<SWD_rodArcFeature> line_rodArcs;
|
||||
wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs);
|
||||
wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
lineData,
|
||||
line,
|
||||
rodDiameter/4,
|
||||
rodDiameter / 8,
|
||||
rodDiameter / 2,
|
||||
cornerPara,
|
||||
line_rodArcs //环
|
||||
);
|
||||
//wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs);
|
||||
arcFeatures.push_back(line_rodArcs);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//所有点计算点云ROI: vecotr格式, （不过滤z小于0的点）
|
||||
SVzNL3DRangeD _getPointCloudROI(std::vector<SVzNL3DPoint>& scanData)
|
||||
{
|
||||
SVzNL3DRangeD roi;
|
||||
roi.xRange = { 0, -1 };
|
||||
roi.yRange = { 0, -1 };
|
||||
roi.zRange = { 0, -1 };
|
||||
|
||||
int nPositionCnt = (int)scanData.size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPoint& pt3D = scanData[i];
|
||||
if (roi.xRange.max < roi.xRange.min)
|
||||
{
|
||||
roi.xRange.min = pt3D.x;
|
||||
roi.xRange.max = pt3D.x;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.xRange.min > pt3D.x)
|
||||
roi.xRange.min = pt3D.x;
|
||||
if (roi.xRange.max < pt3D.x)
|
||||
roi.xRange.max = pt3D.x;
|
||||
}
|
||||
//y
|
||||
if (roi.yRange.max < roi.yRange.min)
|
||||
{
|
||||
roi.yRange.min = pt3D.y;
|
||||
roi.yRange.max = pt3D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.yRange.min > pt3D.y)
|
||||
roi.yRange.min = pt3D.y;
|
||||
if (roi.yRange.max < pt3D.y)
|
||||
roi.yRange.max = pt3D.y;
|
||||
}
|
||||
//z
|
||||
if (roi.zRange.max < roi.zRange.min)
|
||||
{
|
||||
roi.zRange.min = pt3D.z;
|
||||
roi.zRange.max = pt3D.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.zRange.min > pt3D.z)
|
||||
roi.zRange.min = pt3D.z;
|
||||
if (roi.zRange.max < pt3D.z)
|
||||
roi.zRange.max = pt3D.z;
|
||||
}
|
||||
}
|
||||
|
||||
return roi;
|
||||
}
|
||||
|
||||
//计算螺杆端部中心点位姿
|
||||
void sx_hexHeadScrewMeasure(
|
||||
@ -273,6 +345,51 @@ void rodAarcFeatueDetection(
|
||||
return;
|
||||
}
|
||||
|
||||
//聚类
|
||||
std::vector<SVzNL3DPosition> clusterDataSrc;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||
{
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||
a_pt.pt3D = scanLines[line][j].pt3D;
|
||||
clusterDataSrc.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
int clusterCheckWin = 5;
|
||||
int clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
|
||||
std::vector<std::vector< SVzNL3DPosition>> objClusters;
|
||||
wd_pointClustering_speedUp(
|
||||
clusterDataSrc,
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
1, //0 - 2d distance; 1- 3d distance
|
||||
objClusters //result
|
||||
);
|
||||
//生成ClusterMask
|
||||
int clusterSize = (int)objClusters.size();
|
||||
std::vector<std::vector<int>> clusterIdMask;
|
||||
clusterIdMask.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
clusterIdMask[i].resize(linePtNum);
|
||||
std::fill(clusterIdMask[i].begin(), clusterIdMask[i].end(), -1);
|
||||
}
|
||||
for (int i = 0; i < clusterSize; i++)
|
||||
{
|
||||
for (int j = 0; j < (int)objClusters[i].size(); j++)
|
||||
{
|
||||
int line = objClusters[i][j].nPointIdx >> 16;
|
||||
int ptIdx = objClusters[i][j].nPointIdx & 0x0000FFFF;
|
||||
clusterIdMask[line][ptIdx] = i;
|
||||
}
|
||||
}
|
||||
|
||||
//生成扫描数据副本
|
||||
std::vector< std::vector<SVzNL3DPosition>> data_lines;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
@ -368,17 +485,23 @@ void rodAarcFeatueDetection(
|
||||
}
|
||||
}
|
||||
|
||||
//确定目标
|
||||
//确定目标，以及目标所有的类
|
||||
//计算ROI，z最小的生长树为目标
|
||||
std::vector<SVzNL3DRangeD> objROIs;
|
||||
std::vector<int> objCluster;
|
||||
for (int i = 0; i < objNum; 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_MIN;
|
||||
a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = DBL_MIN;
|
||||
a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = DBL_MIN;
|
||||
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;
|
||||
|
||||
std::vector<int> clusterHist;
|
||||
clusterHist.resize(clusterSize);
|
||||
std::fill(clusterHist.begin(), clusterHist.end(), 0);
|
||||
|
||||
int nodeNum = (int)rodArcTrees[i].treeNodes.size();
|
||||
for (int j = 0; j < nodeNum; j++)
|
||||
{
|
||||
@ -388,126 +511,207 @@ void rodAarcFeatueDetection(
|
||||
{
|
||||
int lineIdx = rodArcTrees[i].treeNodes[j].lineIdx;
|
||||
int ptIdx = rodArcTrees[i].treeNodes[j].peakPtIdx;
|
||||
SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D;
|
||||
peak = scanLines[lineIdx][ptIdx].pt3D;
|
||||
int clusterId = clusterIdMask[lineIdx][ptIdx];
|
||||
if (clusterId >= 0)
|
||||
clusterHist[clusterId]++;
|
||||
}
|
||||
else
|
||||
{
|
||||
int ptIdx = rodArcTrees[i].treeNodes[j].lineIdx;
|
||||
int lineIdx = rodArcTrees[i].treeNodes[j].peakPtIdx;
|
||||
SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D;
|
||||
peak = scanLines[lineIdx][ptIdx].pt3D;
|
||||
int clusterId = clusterIdMask[lineIdx][ptIdx];
|
||||
if (clusterId >= 0)
|
||||
clusterHist[clusterId]++;
|
||||
}
|
||||
if (peak.z > 1e-4)
|
||||
{
|
||||
a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x);
|
||||
a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x);
|
||||
a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y);
|
||||
a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y);
|
||||
a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z);
|
||||
a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z);
|
||||
}
|
||||
a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x);
|
||||
a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x);
|
||||
a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y);
|
||||
a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y);
|
||||
a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z);
|
||||
a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z);
|
||||
}
|
||||
int objClusterId = 0;
|
||||
for (int m = 1; m < clusterSize; m++)
|
||||
{
|
||||
if (clusterHist[objClusterId] < clusterHist[m])
|
||||
objClusterId = m;
|
||||
}
|
||||
objROIs.push_back(a_roi3D);
|
||||
objCluster.push_back(objClusterId);
|
||||
}
|
||||
#if 0
|
||||
//逐个目标处理
|
||||
|
||||
//选择Z最小的作为目标
|
||||
int objIdx = -1;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
//空间直线拟合
|
||||
std::vector<SVzNL3DPoint> fitPoints;
|
||||
std::vector<SVzNL2DPoint> fit2DPos;
|
||||
int nodeSize = (int)growTrees[i].treeNodes.size();
|
||||
for (int j = 0; j < nodeSize; j++)
|
||||
if (objIdx < 0)
|
||||
objIdx = i;
|
||||
else if (objROIs[objIdx].zRange.min > objROIs[i].zRange.min)
|
||||
objIdx = i;
|
||||
}
|
||||
|
||||
if(objIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
int resultObjClusterId = objCluster[objIdx];
|
||||
//提取轴向
|
||||
//空间直线拟合
|
||||
std::vector<SVzNL3DPoint> fitPoints;
|
||||
std::vector<SVzNL2DPoint> fit2DPos;
|
||||
int nodeSize = (int)rodArcTrees[objIdx].treeNodes.size();
|
||||
for (int j = 0; j < nodeSize; j++)
|
||||
{
|
||||
SVzNL2DPoint arcPos;
|
||||
SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, rodArcTrees[objIdx].treeNodes[j], arcPos);
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
SVzNL2DPoint arcPos;
|
||||
SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||||
//SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||||
fitPoints.push_back(a_pt);
|
||||
fit2DPos.push_back(arcPos);
|
||||
}
|
||||
if (fitPoints.size() < 27)
|
||||
continue;
|
||||
//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
|
||||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10);
|
||||
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10);
|
||||
else
|
||||
{
|
||||
arcPos = { arcPos.y, arcPos.x };
|
||||
a_pt = {a_pt.y, a_pt.x, a_pt.z};
|
||||
fitPoints.push_back(a_pt);
|
||||
fit2DPos.push_back(arcPos);
|
||||
}
|
||||
}
|
||||
if (fitPoints.size() < 20)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
//去除头50mm， 尾5个点，防止在端部和根部扫描时的数据有干扰
|
||||
SVzNL3DPoint pt_0 = fitPoints[0];
|
||||
SVzNL3DPoint pt_last = fitPoints.back();
|
||||
double headLenToRemove = 40;
|
||||
if (pt_0.z < pt_last.z)
|
||||
{
|
||||
int eraseIdx = -1;
|
||||
for (int i = 1; i < (int)fitPoints.size(); i++)
|
||||
{
|
||||
double dist = sqrt(pow(fitPoints[i].x - pt_0.x, 2) + pow(fitPoints[i].y - pt_0.y, 2) + pow(fitPoints[i].z - pt_0.z, 2));
|
||||
if (dist > headLenToRemove)
|
||||
{
|
||||
eraseIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + eraseIdx);
|
||||
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + eraseIdx);
|
||||
fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
|
||||
fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end());
|
||||
//置标志
|
||||
for (int j = 0; j < (int)fit2DPos.size(); j++)
|
||||
}
|
||||
else
|
||||
{
|
||||
int eraseIdx = -1;
|
||||
for (int i = (int)fitPoints.size()-1; i >= 0; i--)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
double dist = sqrt(pow(fitPoints[i].x - pt_last.x, 2) + pow(fitPoints[i].y - pt_last.y, 2) + pow(fitPoints[i].z - pt_last.z, 2));
|
||||
if (dist > headLenToRemove)
|
||||
{
|
||||
lineIdx = fit2DPos[j].x;
|
||||
ptIdx = fit2DPos[j].y;
|
||||
eraseIdx = i;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
lineIdx = fit2DPos[j].y;
|
||||
ptIdx = fit2DPos[j].x;
|
||||
}
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
int eraseNum = (int)fitPoints.size() - eraseIdx;
|
||||
fitPoints.erase(fitPoints.end() - eraseNum, fitPoints.end());
|
||||
fit2DPos.erase(fit2DPos.end() - eraseNum, fit2DPos.end());
|
||||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5);
|
||||
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 5);
|
||||
}
|
||||
//置标志
|
||||
for (int j = 0; j < (int)fit2DPos.size(); j++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
lineIdx = fit2DPos[j].x;
|
||||
ptIdx = fit2DPos[j].y;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 4;
|
||||
}
|
||||
|
||||
//拟合
|
||||
SVzNL3DPoint P0_center, P1_dir;
|
||||
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
|
||||
if (false == result)
|
||||
continue;
|
||||
//投影
|
||||
//计算旋转向量
|
||||
SVzNL3DPoint vector1 = P1_dir;
|
||||
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
|
||||
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||||
//
|
||||
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
|
||||
SSG_ROIRectD roi_xoy;
|
||||
roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D范围
|
||||
roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D范围
|
||||
roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D范围
|
||||
roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2D范围
|
||||
//拟合
|
||||
SVzNL3DPoint P0_center, P1_dir;
|
||||
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
|
||||
if (false == result)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
//投影
|
||||
//计算旋转向量
|
||||
SVzNL3DPoint vector1 = P1_dir;
|
||||
SVzNL3DPoint vector2 = { 0, 0, 1.0 };
|
||||
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||||
//
|
||||
SVzNL3DPoint test_center = { P0_center.x, P0_center.y + rodDiameter / 2, P0_center.z - 100 };
|
||||
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
|
||||
SVzNL3DPoint test_center_rotate = _ptRotate(test_center, rotatePara.planeCalib);
|
||||
SSG_ROIRectD roi_xoy;
|
||||
roi_xoy.left = P0_rotate.x - rodDiameter* 2.0; //2D范围
|
||||
roi_xoy.right = P0_rotate.x + rodDiameter * 2.0; //2D范围
|
||||
roi_xoy.top = P0_rotate.y - rodDiameter * 2.0; //2D范围
|
||||
roi_xoy.bottom = P0_rotate.y + rodDiameter * 2.0; //2D范围
|
||||
|
||||
#if 1
|
||||
std::vector< SVzNL3DPoint> verifyData;
|
||||
for (int m = 0; m < (int)fitPoints.size(); m++)
|
||||
{
|
||||
SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
|
||||
verifyData.push_back(rPt);
|
||||
}
|
||||
#endif
|
||||
std::vector< SVzNL3DPoint> roiProjectionData;
|
||||
xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||||
//取端面
|
||||
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||||
SVzNLRangeD cutZRange;
|
||||
cutZRange.min = zRange.min;
|
||||
cutZRange.max = zRange.min + 5.0; //5mm的端面
|
||||
std::vector<SVzNL3DPoint> surfacePoints;
|
||||
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
|
||||
//计算中心点
|
||||
SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
|
||||
SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints);
|
||||
projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
|
||||
projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
|
||||
projectionCenter.z = zRange.min;
|
||||
//旋转回原坐标系
|
||||
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
||||
//生成Rod信息
|
||||
SSX_rodPoseInfo a_rod;
|
||||
a_rod.center = surfaceCenter;
|
||||
a_rod.axialDir = P1_dir;
|
||||
screwInfo.push_back(a_rod);
|
||||
}
|
||||
if (true == isHorizonScan)
|
||||
std::vector< SVzNL3DPoint> verifyData;
|
||||
for (int m = 0; m < (int)fitPoints.size(); m++)
|
||||
{
|
||||
int objNum = (int)screwInfo.size();
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
double tmp = screwInfo[i].center.x;
|
||||
screwInfo[i].center.x = screwInfo[i].center.y;
|
||||
screwInfo[i].center.y = tmp;
|
||||
tmp = screwInfo[i].axialDir.x;
|
||||
screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
|
||||
screwInfo[i].axialDir.y = tmp;
|
||||
//screwInfo[i].rotateAngle += 90;
|
||||
}
|
||||
SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
|
||||
verifyData.push_back(rPt);
|
||||
}
|
||||
#endif
|
||||
std::vector< SVzNL3DPoint> roiProjectionData;
|
||||
xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, resultObjClusterId, clusterIdMask, roiProjectionData);
|
||||
|
||||
//取端面
|
||||
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||||
SVzNLRangeD cutZRange;
|
||||
cutZRange.min = zRange.min;
|
||||
cutZRange.max = zRange.min + 5.0; //5mm的端面
|
||||
std::vector<SVzNL3DPoint> surfacePoints;
|
||||
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
|
||||
//计算中心点
|
||||
SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
|
||||
SVzNL3DRangeD roi3D = _getPointCloudROI(surfacePoints);
|
||||
projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
|
||||
projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
|
||||
projectionCenter.z = zRange.min;
|
||||
//迭代搜索：搜索projectionCenter为中心5mm内z最大的的点为中心点
|
||||
double searchR = 5.0;
|
||||
int centerIdx = -1;
|
||||
for (int i = 0; i < (int)surfacePoints.size(); i++)
|
||||
{
|
||||
double dist = sqrt(pow(surfacePoints[i].x - projectionCenter.x, 2) + pow(surfacePoints[i].y - projectionCenter.y, 2));
|
||||
if (dist < searchR)
|
||||
{
|
||||
if (centerIdx < 0)
|
||||
centerIdx = i;
|
||||
else if(surfacePoints[centerIdx].z < surfacePoints[i].z)
|
||||
centerIdx = i;
|
||||
}
|
||||
}
|
||||
if(centerIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
//旋转回原坐标系
|
||||
SVzNL3DPoint surfaceCenter = _ptRotate(surfacePoints[centerIdx], rotatePara.invRMatrix);
|
||||
//生成Rod信息
|
||||
SSX_rodPoseInfo a_rod;
|
||||
a_rod.center = surfaceCenter;
|
||||
a_rod.axialDir = P1_dir;
|
||||
screwInfo.push_back(a_rod);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -1051,7 +1255,7 @@ SSX_platePoseInfo sx_getLocationPlatePose(
|
||||
}
|
||||
//计算面参数: z = Ax + By + C
|
||||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
#if 0
|
||||
#if 1
|
||||
std::vector<cv::Point3f> out_inliers;
|
||||
Plane res = ransacFitPlane( Points3ds, out_inliers );
|
||||
if (res.C < 0)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user