Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib
This commit is contained in:
杰仔
commit
7cd7508c61
@ -48,13 +48,13 @@
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
|
||||
@ -139,6 +139,12 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
|
||||
const SSG_cornerParam cornerPara,
|
||||
SSG_lineFeature* line_features);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_computeDirAngle_wholeLine(
|
||||
std::vector< SVzNL3DPosition>& line_data,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector< SSG_pntDirAngle>& ptDirAngles
|
||||
);
|
||||
|
||||
/// 提取激光线上的圆环的上半段弧。宽度由圆环的宽度确定
|
||||
/// seg端点：z距离大于门限
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
|
||||
@ -3969,7 +3969,7 @@ void _computeDirAngle_perSeg(
|
||||
}
|
||||
|
||||
//计算方向角(不分段）
|
||||
void _computeDirAngle_wholeLine(
|
||||
void wd_computeDirAngle_wholeLine(
|
||||
std::vector< SVzNL3DPosition>& line_data,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector< SSG_pntDirAngle>& ptDirAngles
|
||||
@ -4309,7 +4309,7 @@ void wd_getRodArcFeature_peakCornerMethod(
|
||||
double arcTotalCornerMinValue = 45; //整个Arc的转角最小值
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles;
|
||||
_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles);
|
||||
wd_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles);
|
||||
|
||||
int dataSize = (int)lineData.size();
|
||||
//搜索z极值。
|
||||
|
||||
@ -10,7 +10,8 @@
|
||||
//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量
|
||||
//version 1.2.0 : 算法完成了6轴验证
|
||||
//version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
|
||||
std::string m_strVersion = "1.3.0";
|
||||
//version 1.4.0 : 添加了华航孔定位功能
|
||||
std::string m_strVersion = "1.4.0";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -299,89 +300,19 @@ void _getYTopLine(
|
||||
std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
|
||||
return;
|
||||
}
|
||||
//工件孔定位
|
||||
void wd_workpieceHolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const WD_workpieceHoleParam workpiecePara,
|
||||
|
||||
void wd_getHoleInfo(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
std::vector< WD_workpieceInfo>& workpiecePositioning,
|
||||
int* errCode)
|
||||
std::vector<SWD_segFeatureTree>& segTrees_v,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||||
std::vector<SSG_intPair>& validObjects
|
||||
)
|
||||
{
|
||||
*errCode = 0;
|
||||
|
||||
int lineNum = (int)scanLinesInput.size();
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
scanLines.resize(lineNum);
|
||||
int linePtNum = (int)scanLinesInput[0].size();
|
||||
bool isGridData = true;
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (linePtNum != (int)scanLinesInput[i].size())
|
||||
isGridData = false;
|
||||
|
||||
scanLines[i].resize(scanLinesInput[i].size());
|
||||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||||
|
||||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||||
}
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return;
|
||||
}
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{ //行处理
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||||
}
|
||||
|
||||
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
|
||||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines);
|
||||
SVzNLRect roi2D;
|
||||
roi2D.left = (int)roi3D.xRange.min;
|
||||
roi2D.right = (int)roi3D.xRange.max;
|
||||
roi2D.top = (int)roi3D.yRange.min;
|
||||
roi2D.bottom = (int)roi3D.yRange.max;
|
||||
int quanti_X = roi2D.right - roi2D.left + 1;
|
||||
int quanti_Y = roi2D.bottom - roi2D.top + 1;
|
||||
std::vector<std::vector<double>> quantiValue;
|
||||
std::vector<std::vector<int>> quantiHist;
|
||||
quantiValue.resize(quanti_X);
|
||||
quantiHist.resize(quanti_X);
|
||||
for (int i = 0; i < quanti_X; i++)
|
||||
{
|
||||
quantiValue[i].resize(quanti_Y);
|
||||
std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
|
||||
quantiHist[i].resize(quanti_Y);
|
||||
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
|
||||
}
|
||||
//以1mm尺度量化
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
|
||||
if (a_pt.z > 1e-4)
|
||||
{
|
||||
int qx = (int)a_pt.x - roi2D.left;
|
||||
int qy = (int)a_pt.y - roi2D.top;
|
||||
quantiValue[qx][qy] += a_pt.z;
|
||||
quantiHist[qx][qy] += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int ix = 0; ix < quanti_X; ix++)
|
||||
{
|
||||
for (int iy = 0; iy < quanti_Y; iy++)
|
||||
{
|
||||
if (quantiHist[ix][iy] > 0)
|
||||
quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
|
||||
}
|
||||
}
|
||||
int lineNum = (int)scanLines.size();
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
|
||||
std::vector<std::vector<int>> pointMask;
|
||||
pointMask.resize(lineNum);
|
||||
@ -421,7 +352,6 @@ void wd_workpieceHolePositioning(
|
||||
holeGaps.push_back(line_gaps);
|
||||
}
|
||||
//特征生长
|
||||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||||
wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);
|
||||
|
||||
//生成水平扫描
|
||||
@ -471,7 +401,6 @@ void wd_workpieceHolePositioning(
|
||||
holeGaps_h.push_back(line_gaps);
|
||||
}
|
||||
//特征生长
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);
|
||||
|
||||
//创建Tree所在孔洞的Mask
|
||||
@ -497,9 +426,9 @@ void wd_workpieceHolePositioning(
|
||||
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};
|
||||
treeInfo_v[i].roi = { -1, -1, -1, -1 };
|
||||
int nullPtSize = 0;
|
||||
SSG_ROIRectD roi = {-1, -1, -1, -1};
|
||||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||||
SVzNLRange ptIdxRange = { -1, -1 };
|
||||
if (a_tree.treeNodes.size() > 2)
|
||||
{
|
||||
@ -648,7 +577,7 @@ void wd_workpieceHolePositioning(
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
|
||||
for (int i = 0; i < (int)objects.size(); i++)
|
||||
{
|
||||
int vTreeIdx = objects[i].data_0;
|
||||
@ -657,6 +586,96 @@ void wd_workpieceHolePositioning(
|
||||
|
||||
validObjects.push_back(objects[i]);
|
||||
}
|
||||
}
|
||||
|
||||
//工件孔定位-拓普发工件孔定位
|
||||
void wd_workpieceHolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const WD_workpieceHoleParam workpiecePara,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
std::vector< WD_workpieceInfo>& workpiecePositioning,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
|
||||
int lineNum = (int)scanLinesInput.size();
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
scanLines.resize(lineNum);
|
||||
int linePtNum = (int)scanLinesInput[0].size();
|
||||
bool isGridData = true;
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (linePtNum != (int)scanLinesInput[i].size())
|
||||
isGridData = false;
|
||||
|
||||
scanLines[i].resize(scanLinesInput[i].size());
|
||||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||||
|
||||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||||
}
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return;
|
||||
}
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{ //行处理
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||||
}
|
||||
|
||||
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
|
||||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines);
|
||||
SVzNLRect roi2D;
|
||||
roi2D.left = (int)roi3D.xRange.min;
|
||||
roi2D.right = (int)roi3D.xRange.max;
|
||||
roi2D.top = (int)roi3D.yRange.min;
|
||||
roi2D.bottom = (int)roi3D.yRange.max;
|
||||
int quanti_X = roi2D.right - roi2D.left + 1;
|
||||
int quanti_Y = roi2D.bottom - roi2D.top + 1;
|
||||
std::vector<std::vector<double>> quantiValue;
|
||||
std::vector<std::vector<int>> quantiHist;
|
||||
quantiValue.resize(quanti_X);
|
||||
quantiHist.resize(quanti_X);
|
||||
for (int i = 0; i < quanti_X; i++)
|
||||
{
|
||||
quantiValue[i].resize(quanti_Y);
|
||||
std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
|
||||
quantiHist[i].resize(quanti_Y);
|
||||
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
|
||||
}
|
||||
//以1mm尺度量化
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
|
||||
if (a_pt.z > 1e-4)
|
||||
{
|
||||
int qx = (int)a_pt.x - roi2D.left;
|
||||
int qy = (int)a_pt.y - roi2D.top;
|
||||
quantiValue[qx][qy] += a_pt.z;
|
||||
quantiHist[qx][qy] += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int ix = 0; ix < quanti_X; ix++)
|
||||
{
|
||||
for (int iy = 0; iy < quanti_Y; iy++)
|
||||
{
|
||||
if (quantiHist[ix][iy] > 0)
|
||||
quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects);
|
||||
|
||||
//生成聚类信息，
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||||
@ -921,4 +940,445 @@ void wd_workpieceHolePositioning(
|
||||
}
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int flag;
|
||||
SWDIndexing3DPoint zMaxPos;
|
||||
SVzNLRect peakROI;
|
||||
double pkValue;
|
||||
}_zMaxInfo;
|
||||
|
||||
bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_v,
|
||||
std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_h,
|
||||
SVzNLRect& roi
|
||||
)
|
||||
{
|
||||
int lineNum = (int)pntDirAngles_v.size();
|
||||
int ptNum = (int)pntDirAngles_h.size();
|
||||
|
||||
//向左搜索
|
||||
int left = -1;
|
||||
for (int line = seedPos.x; line >= 0; line--)
|
||||
{
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0))
|
||||
{
|
||||
left = line;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向右搜索
|
||||
int right = -1;
|
||||
for (int line = seedPos.x; line < lineNum; line++)
|
||||
{
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0))
|
||||
{
|
||||
right = line;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向上搜索
|
||||
int top = -1;
|
||||
for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--)
|
||||
{
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0))
|
||||
{
|
||||
top = ptIdx;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向下搜索
|
||||
int bottom = -1;
|
||||
for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++)
|
||||
{
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0))
|
||||
{
|
||||
bottom = ptIdx;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if ((left < 0) || (right < 0) || (top < 0) || (bottom < 0))
|
||||
return false;
|
||||
roi.left = left;
|
||||
roi.right = right;
|
||||
roi.top = top;
|
||||
roi.bottom = bottom;
|
||||
return true;
|
||||
}
|
||||
|
||||
SVzNLRect _mergeROI(SVzNLRect roi_1, SVzNLRect roi_2)
|
||||
{
|
||||
SVzNLRect roi;
|
||||
roi.left = roi_1.left < roi_2.left ? roi_1.left : roi_2.left;
|
||||
roi.right = roi_1.right > roi_2.right ? roi_1.right : roi_2.right;
|
||||
roi.top = roi_1.top < roi_2.top ? roi_1.top : roi_2.top;
|
||||
roi.bottom = roi_1.bottom > roi_2.bottom ? roi_1.bottom : roi_2.bottom;
|
||||
return roi;
|
||||
}
|
||||
|
||||
//单个孔或凹坑定位-华航孔定位
|
||||
void wd_HolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_cornerParam cornerParam,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
std::vector< WD_HolePositionInfo>& holePositioning,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
|
||||
int lineNum = (int)scanLinesInput.size();
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
scanLines.resize(lineNum);
|
||||
int linePtNum = (int)scanLinesInput[0].size();
|
||||
bool isGridData = true;
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (linePtNum != (int)scanLinesInput[i].size())
|
||||
isGridData = false;
|
||||
|
||||
scanLines[i].resize(scanLinesInput[i].size());
|
||||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||||
|
||||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||||
}
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return;
|
||||
}
|
||||
|
||||
//内部参数
|
||||
double zPeakScale = 25.0; //计算ZPeak时的尺度
|
||||
double minZPeakHeight = 2.5; //最小的凹坑深度
|
||||
double planeInlierDistTh = 1.0; //平面点距离平面的距离。超出此距离被判别为离群点
|
||||
|
||||
//计算dirAngle
|
||||
std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_v;
|
||||
std::vector<std::vector<SSG_basicFeature1D>> zMaxPeaks_v;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 1047)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
//滤波，滤除异常点
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
|
||||
std::vector< SSG_pntDirAngle> line_ptDirAngles;
|
||||
wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles);
|
||||
pntDirAngles_v.push_back(line_ptDirAngles);
|
||||
|
||||
std::vector< SSG_basicFeature1D> localZMax;
|
||||
std::vector< SSG_basicFeature1D> localZMin;
|
||||
sg_getLineLocalPeaks_2( lineData, line, zPeakScale, localZMax, localZMin);
|
||||
zMaxPeaks_v.push_back(localZMax);
|
||||
}
|
||||
|
||||
//生成水平扫描
|
||||
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特征提取
|
||||
//创建水平ZMax的Mask
|
||||
std::vector<std::vector<int>> zMaxMask_h;
|
||||
zMaxMask_h.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
zMaxMask_h[i].resize(linePtNum);
|
||||
std::fill(zMaxMask_h[i].begin(), zMaxMask_h[i].end(), 0);
|
||||
}
|
||||
|
||||
std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_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_pntDirAngle> line_ptDirAngles;
|
||||
wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles);
|
||||
pntDirAngles_h.push_back(line_ptDirAngles);
|
||||
|
||||
std::vector< SSG_basicFeature1D> localZMax;
|
||||
std::vector< SSG_basicFeature1D> localZMin;
|
||||
sg_getLineLocalPeaks_2(lineData, line, zPeakScale, localZMax, localZMin);
|
||||
for (int j = 0; j < (int)localZMax.size(); j++)
|
||||
{
|
||||
int idx_line = localZMax[j].jumpPos2D.y;
|
||||
int idx_pt = localZMax[j].jumpPos2D.x;
|
||||
zMaxMask_h[idx_line][idx_pt] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
//判断真正的ZMax并记录
|
||||
//（1）真正的ZMax一定是同时为X方向的ZMax和Y方向的ZMax.(2)满足一定的Z差值
|
||||
std::vector<_zMaxInfo> objPeaks;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SSG_basicFeature1D>& a_lineZMax = zMaxPeaks_v[line];
|
||||
for (int j = 0; j < (int)a_lineZMax.size(); j++)
|
||||
{
|
||||
int idx_line = a_lineZMax[j].jumpPos2D.x;
|
||||
int idx_pt = a_lineZMax[j].jumpPos2D.y;
|
||||
if (zMaxMask_h[idx_line][idx_pt] > 0) //在X和Y方向均为极大值
|
||||
{
|
||||
//判断ROI
|
||||
SVzNLRect a_roi;
|
||||
bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, a_roi);
|
||||
if (true == validPk)
|
||||
{
|
||||
double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z;
|
||||
meanZ += scanLines[a_roi.right][idx_pt].pt3D.z;
|
||||
meanZ += scanLines[idx_line][a_roi.top].pt3D.z;
|
||||
meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z;
|
||||
meanZ = meanZ / 4.0;
|
||||
double z_diff = a_lineZMax[j].jumpPos.z - meanZ;
|
||||
if (z_diff > minZPeakHeight)
|
||||
{
|
||||
_zMaxInfo a_info;
|
||||
a_info.flag = 0;
|
||||
a_info.peakROI = a_roi;
|
||||
a_info.zMaxPos.lineIdx = a_lineZMax[j].jumpPos2D.x;
|
||||
a_info.zMaxPos.ptIdx = a_lineZMax[j].jumpPos2D.y;
|
||||
a_info.zMaxPos.point = { a_lineZMax[j].jumpPos.x, a_lineZMax[j].jumpPos.y, a_lineZMax[j].jumpPos.z };
|
||||
a_info.pkValue = a_lineZMax[j].jumpPos.z;
|
||||
objPeaks.push_back(a_info);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
int pkSize = (int)objPeaks.size();
|
||||
for (int i = 0; i < pkSize; i++)
|
||||
{
|
||||
_zMaxInfo a_info = objPeaks[i];
|
||||
if (a_info.flag < 0)
|
||||
continue;
|
||||
|
||||
for (int j = i + 1; j < pkSize; j++)
|
||||
{
|
||||
if (objPeaks[j].flag < 0)
|
||||
continue;
|
||||
|
||||
if ((a_info.peakROI.right >= objPeaks[j].peakROI.left) && (objPeaks[j].peakROI.right >= a_info.peakROI.left) &&
|
||||
(a_info.peakROI.bottom >= objPeaks[j].peakROI.top) && (objPeaks[j].peakROI.bottom >= a_info.peakROI.top)) //重叠
|
||||
{
|
||||
a_info.peakROI = _mergeROI(a_info.peakROI, objPeaks[j].peakROI);
|
||||
if (a_info.pkValue < objPeaks[j].pkValue)
|
||||
{
|
||||
a_info.pkValue = objPeaks[j].pkValue;
|
||||
a_info.zMaxPos = objPeaks[j].zMaxPos;
|
||||
objPeaks[j].flag = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
objPeaks[i] = a_info;
|
||||
}
|
||||
//逐个目标提取
|
||||
int contourWin = 4; //取周围3行和3列的点
|
||||
for (int peakIdx = 0; peakIdx < pkSize; peakIdx++)
|
||||
{
|
||||
if (objPeaks[peakIdx].flag < 0)
|
||||
continue;
|
||||
|
||||
SVzNLRect& a_roi = objPeaks[peakIdx].peakROI;
|
||||
//提取孔周围点
|
||||
SVzNLRect extend_roi = { a_roi.left - contourWin, a_roi.right + contourWin, a_roi.top - contourWin , a_roi.bottom + contourWin };
|
||||
if (extend_roi.left < 0) extend_roi.left = 0;
|
||||
if (extend_roi.right >= lineNum) extend_roi.right = lineNum - 1;
|
||||
if (extend_roi.top < 0) extend_roi.top = 0;
|
||||
if (extend_roi.bottom >= linePtNum) extend_roi.bottom = linePtNum - 1;
|
||||
|
||||
std::vector<cv::Point3f> Points3ds;
|
||||
//左
|
||||
for (int line = extend_roi.left; line <= a_roi.left-1; line++)
|
||||
{
|
||||
for (int j = a_roi.top; j <= a_roi.bottom; j++)
|
||||
{
|
||||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f(scanLines[line][j].pt3D.x , scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
|
||||
Points3ds.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
//右
|
||||
for (int line = a_roi.right+1; line <=extend_roi.right; line++)
|
||||
{
|
||||
for (int j = a_roi.top; j <= a_roi.bottom; j++)
|
||||
{
|
||||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
|
||||
Points3ds.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
//上
|
||||
for (int ptIdx = extend_roi.top; ptIdx <= a_roi.top-1; ptIdx++)
|
||||
{
|
||||
for (int j = a_roi.left; j <= a_roi.right; j++)
|
||||
{
|
||||
if (scanLines[j][ptIdx].pt3D.z > 1e-4)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z);
|
||||
Points3ds.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
//下
|
||||
for (int ptIdx = a_roi.bottom+1; ptIdx < extend_roi.bottom; ptIdx++)
|
||||
{
|
||||
for (int j = a_roi.left; j <= a_roi.right; j++)
|
||||
{
|
||||
if (scanLines[j][ptIdx].pt3D.z > 1e-4)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z);
|
||||
Points3ds.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
//计算面参数: z = Ax + By + C , res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
std::vector<double> res;
|
||||
vzCaculateLaserPlane(Points3ds, res);
|
||||
double normValue = sqrt(pow(res[0], 2) + pow(res[1], 2) + pow(res[2],2));
|
||||
double norm_A = res[0] / normValue;
|
||||
double norm_B = res[1] / normValue;
|
||||
double norm_C = res[2] / normValue;
|
||||
double norm_D = res[3] / normValue;
|
||||
//生成ROI scanLines
|
||||
int roi_lineNum = extend_roi.right - extend_roi.left + 1;
|
||||
int roi_ptNum = extend_roi.bottom - extend_roi.top + 1;
|
||||
std::vector< std::vector<SVzNL3DPosition>> roi_scanLines;
|
||||
roi_scanLines.resize(roi_lineNum);
|
||||
for (int j = 0; j < roi_lineNum; j++)
|
||||
roi_scanLines[j].resize(roi_ptNum);
|
||||
|
||||
//生成面上点
|
||||
for (int line = extend_roi.left; line <= extend_roi.right; line++)
|
||||
{
|
||||
int roi_line = line - extend_roi.left;
|
||||
for (int ptIdx = extend_roi.top; ptIdx <= extend_roi.bottom; ptIdx++)
|
||||
{
|
||||
int roi_ptIdx = ptIdx - extend_roi.top;
|
||||
roi_scanLines[roi_line][roi_ptIdx] = scanLines[line][ptIdx];
|
||||
//最外圈不进行处理
|
||||
if ((line != extend_roi.left) && (line != extend_roi.right) && (ptIdx != extend_roi.top) && (ptIdx != extend_roi.bottom))
|
||||
{
|
||||
if (scanLines[line][ptIdx].pt3D.z > 1e-4)
|
||||
{
|
||||
SVzNL3DPoint& a_pt3D = scanLines[line][ptIdx].pt3D;
|
||||
double dist = abs(norm_A * a_pt3D.x + norm_B * a_pt3D.y + norm_C * a_pt3D.z + norm_D);
|
||||
if (dist > planeInlierDistTh)
|
||||
roi_scanLines[roi_line][roi_ptIdx].pt3D = { 0, 0, 0 };
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects);
|
||||
if (validObjects.size() > 0)
|
||||
{
|
||||
SSG_intPair a_hvPair = validObjects[0];
|
||||
if (validObjects.size() > 1)
|
||||
{
|
||||
for (int j = 1; j < (int)validObjects.size(); j++)
|
||||
{
|
||||
if (validObjects[j].idx < validObjects[j].idx)
|
||||
a_hvPair = validObjects[j];
|
||||
}
|
||||
}
|
||||
//生成Contour
|
||||
std::vector<SVzNL3DPoint> cluster_pointArray;
|
||||
int vTreeIdx = a_hvPair.data_0;
|
||||
int hTreeIdx = a_hvPair.data_1;
|
||||
for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
|
||||
if (roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||||
{
|
||||
roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D;
|
||||
cluster_pointArray.push_back(a_pos);
|
||||
}
|
||||
if (roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||||
{
|
||||
roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D;
|
||||
cluster_pointArray.push_back(a_pos);
|
||||
}
|
||||
}
|
||||
for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
|
||||
if (roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||||
{
|
||||
roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D;
|
||||
cluster_pointArray.push_back(a_pos);
|
||||
}
|
||||
if (roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||||
{
|
||||
roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D;
|
||||
cluster_pointArray.push_back(a_pos);
|
||||
}
|
||||
}
|
||||
//计算重心
|
||||
#if 0
|
||||
SVzNL3DPoint center;
|
||||
double radius;
|
||||
double err = fitCircleByLeastSquare(cluster_pointArray, center, radius);
|
||||
#endif
|
||||
if (cluster_pointArray.size() > 0)
|
||||
{
|
||||
SVzNL3DPoint center = { 0, 0, 0 };
|
||||
int ptSize = (int)cluster_pointArray.size();
|
||||
for (int m = 0; m < ptSize; m++)
|
||||
{
|
||||
center.x += cluster_pointArray[m].x;
|
||||
center.y += cluster_pointArray[m].y;
|
||||
center.z += cluster_pointArray[m].z;
|
||||
}
|
||||
center.x = center.x / ptSize;
|
||||
center.y = center.y / ptSize;
|
||||
center.z = center.z / ptSize;
|
||||
//求在平面上的垂足
|
||||
double t = -(center.x * norm_A + center.y * norm_B + center.z * norm_C + norm_D);
|
||||
SVzNL3DPoint realCenter;
|
||||
realCenter.x = center.x + t * norm_A;
|
||||
realCenter.y = center.y + t * norm_B;
|
||||
realCenter.z = center.z + t * norm_C;
|
||||
WD_HolePositionInfo a_hole;
|
||||
a_hole.center = realCenter;
|
||||
a_hole.normDir = { norm_A, norm_B, norm_C };
|
||||
holePositioning.push_back(a_hole);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
return;
|
||||
}
|
||||
@ -27,6 +27,12 @@ typedef struct
|
||||
SVzNL3DPoint x_dir; //x方向向量（归一化）
|
||||
}WD_workpieceInfo;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint center;
|
||||
SVzNL3DPoint normDir; //方向向量（归一化）
|
||||
}WD_HolePositionInfo;
|
||||
|
||||
//读版本号
|
||||
SG_APISHARED_EXPORT const char* wd_workpieceHolePositioningVersion(void);
|
||||
|
||||
@ -41,7 +47,7 @@ SG_APISHARED_EXPORT void wd_lineDataR(
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
//工件孔定位
|
||||
//工件孔定位-拓普发工件孔定位
|
||||
SG_APISHARED_EXPORT void wd_workpieceHolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const WD_workpieceHoleParam workpiecePara,
|
||||
@ -51,3 +57,13 @@ SG_APISHARED_EXPORT void wd_workpieceHolePositioning(
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
std::vector< WD_workpieceInfo>& workpiecePositioning,
|
||||
int* errCode);
|
||||
|
||||
//单个孔或凹坑定位-华航孔定位
|
||||
SG_APISHARED_EXPORT void wd_HolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_cornerParam cornerParam,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
std::vector< WD_HolePositionInfo>& holePositioning,
|
||||
int* errCode);
|
||||
@ -164,6 +164,24 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputHoleInfo(char* fileName, std::vector< WD_HolePositionInfo>& holePositioning)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
|
||||
int number = (int)holePositioning.size();
|
||||
for (int i = 0; i < number; i++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "孔_%d", i + 1);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 50, " center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 50, " norm_dir: (%g, %g, %g)", holePositioning[i].normDir.x, holePositioning[i].normDir.y, holePositioning[i].normDir.z);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
@ -410,6 +428,117 @@ void _outputRGBDResult_RGBD(
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputRGBDResult_HoleInfo(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< WD_HolePositionInfo>& holePositioning)
|
||||
{
|
||||
std::vector<SVzNL3DPosition> objects;
|
||||
int objNumber = (int)holePositioning.size();
|
||||
for (int i = 0; i < objNumber; i++)
|
||||
{
|
||||
SVzNL3DPosition a_objPt;
|
||||
a_objPt.pt3D = holePositioning[i].center;
|
||||
objects.push_back(a_objPt);
|
||||
}
|
||||
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = (objNumber == 0) ? lineNum : (lineNum + 1);
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int flag = pt3D->nPointIdx & 0xffff;
|
||||
if (flag > 0)
|
||||
{
|
||||
rgb = objColor[flag % 8]; // { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
int linePtNum = (int)objects.size();
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
size = 10;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
rgb = {255, 0, 0};
|
||||
float x = (float)objects[i].pt3D.x;
|
||||
float y = (float)objects[i].pt3D.y;
|
||||
float z = (float)objects[i].pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//输出方向线条
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 2;
|
||||
for (int i = 0; i < objNumber; i++)
|
||||
{
|
||||
SVzNL3DPoint dirPt_1, dirPt_2;
|
||||
dirPt_1 = { holePositioning[i].center.x - holePositioning[i].normDir.x * 20,
|
||||
holePositioning[i].center.y - holePositioning[i].normDir.y * 20,
|
||||
holePositioning[i].center.z - holePositioning[i].normDir.z * 20 };
|
||||
dirPt_2 = { holePositioning[i].center.x + holePositioning[i].normDir.x * 10,
|
||||
holePositioning[i].center.y + holePositioning[i].normDir.y * 10,
|
||||
holePositioning[i].center.z + holePositioning[i].normDir.z * 10 };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << dirPt_1.x << "," << dirPt_1.y << "," << dirPt_1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPt_2.x << "," << dirPt_2.y << "," << dirPt_2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
|
||||
SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T)
|
||||
{
|
||||
SVzNL3DPoint result;
|
||||
@ -425,17 +554,17 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T)
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_HOLE 1
|
||||
#define TEST_COMPUTE_RT 0
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
#define TPF_TEST_GROUP 2
|
||||
//拓普发工件孔定位（工件定位)
|
||||
void TuoPuFa_holePosition_test(void)
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
const char* dataPath[TPF_TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云2/", //1
|
||||
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
SVzNLRange fileIdx[TPF_TEST_GROUP] = {
|
||||
{6,6}, {1, 15}
|
||||
};
|
||||
|
||||
@ -681,13 +810,92 @@ int main()
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||
//华航孔定位
|
||||
#define HH_TEST_GROUP 4
|
||||
void HuaHang_holePosition_test(void)
|
||||
{
|
||||
const char* dataPath[HH_TEST_GROUP] = {
|
||||
|
||||
// 入门使用技巧:
|
||||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||
// 3. 使用输出窗口查看生成输出和其他消息
|
||||
// 4. 使用错误列表窗口查看错误
|
||||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据1/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/1/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/2/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/3/", //1
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[HH_TEST_GROUP] = {
|
||||
{1,15}, {1, 21},{1,19}, {1,9}
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if TEST_COMPUTE_HOLE
|
||||
for (int grp = 1; grp <= 3; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
SSG_lineSegParam lineSegPara;
|
||||
lineSegPara.distScale = 25.0;
|
||||
lineSegPara.segGapTh_y = 15.0; //
|
||||
lineSegPara.segGapTh_z = 0.0; //z方向间隔大于10mm认为是分段
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 5.0;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 2;
|
||||
growParam.yDeviation_max = 4.0;
|
||||
growParam.maxSkipDistance = 0.0;
|
||||
growParam.zDeviation_max = 10.0;//
|
||||
growParam.minLTypeTreeLen = 2.0; //mm
|
||||
growParam.minVTypeTreeLen = 2.0; //mm
|
||||
WD_workpieceHoleParam workpiecePara;
|
||||
workpiecePara.workpieceType = 0;
|
||||
workpiecePara.holeDiameter = 6.0; //
|
||||
workpiecePara.holeDist_W = 32.0;
|
||||
workpiecePara.holeDist_L = 40.0;
|
||||
workpiecePara.xLen = 44;
|
||||
workpiecePara.yLen = 70;
|
||||
workpiecePara.H = 47;
|
||||
int errCode = 0;
|
||||
std::vector< WD_HolePositionInfo> holePositioning;
|
||||
wd_HolePositioning(scanLines, lineSegPara, cornerParam, filterParam, growParam, holePositioning, &errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDResult_HoleInfo(_scan_file, scanLines, holePositioning);
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputHoleInfo(_scan_file, holePositioning);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
#if 0
|
||||
TuoPuFa_holePosition_test();
|
||||
#else
|
||||
HuaHang_holePosition_test();
|
||||
#endif
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user