workpieceHolePositioning version 1.3.0 :
(1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
This commit is contained in:
parent
6d8e6bb9b9
commit
248f555ada
@ -106,6 +106,12 @@ SG_APISHARED_EXPORT void wd_getLineDataIntervals(
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
std::vector<SSG_RUN>& segs);
|
||||
|
||||
//提取Gap段
|
||||
SG_APISHARED_EXPORT void wd_getLineDataNullIntervals(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
std::vector<SSG_RUN>& segs);
|
||||
|
||||
// 最小点集数量（小于此数无法拟合直线）
|
||||
const int MIN_POINT_COUNT = 3;
|
||||
//使用端点直线，检查点到直线的距离，大于门限的分割
|
||||
@ -318,6 +324,12 @@ SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees(
|
||||
std::vector<SWD_segFeatureTree>& feature_trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//segment特征生长, 相信seg间要保证有重叠区
|
||||
SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees_2(
|
||||
std::vector<std::vector<SWD_segFeature>>& all_lineFeatures,
|
||||
std::vector<SWD_segFeatureTree>& feature_trees,
|
||||
const SSG_treeGrowParam growParam);
|
||||
|
||||
//对rodArcfeature进行生长
|
||||
SG_APISHARED_EXPORT void wd_getRodArcFeatureGrowingTrees(
|
||||
std::vector<std::vector<SWD_rodArcFeature>>& all_lineFeatures,
|
||||
|
||||
@ -342,6 +342,18 @@ typedef struct
|
||||
int angleChkScalePos; //仅用于加速angleCheck的速度
|
||||
}SSG_gapFeatureTree;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int treeState;
|
||||
int treeType;
|
||||
int sLineIdx;
|
||||
int eLineIdx;
|
||||
double tree_value;
|
||||
SSG_ROIRectD roi;
|
||||
std::vector< SWD_segFeature> treeNodes;
|
||||
int angleChkScalePos; //仅用于加速angleCheck的速度
|
||||
}SSG_segFeatureTree;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int treeState;
|
||||
@ -380,7 +392,9 @@ typedef struct
|
||||
int treeType;
|
||||
int sLineIdx;
|
||||
int eLineIdx;
|
||||
int data;
|
||||
SSG_ROIRectD roi;
|
||||
SVzNLRange ptIdxRange;
|
||||
}SSG_treeInfo;
|
||||
|
||||
typedef struct
|
||||
|
||||
@ -146,6 +146,40 @@ bool _segFeatureGrowing(SWD_segFeature& a_feature, const int lineIdx, std::vecto
|
||||
return false;
|
||||
}
|
||||
|
||||
//将segment feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||||
bool _segFeatureGrowing_2(SWD_segFeature& a_feature, const int lineIdx, std::vector<SWD_segFeatureTree>& trees, SSG_treeGrowParam growParam)
|
||||
{
|
||||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||||
{
|
||||
SWD_segFeatureTree& a_tree = trees[i];
|
||||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||||
continue;
|
||||
//检查生长点
|
||||
SWD_segFeature last_node = a_tree.treeNodes.back();
|
||||
if (last_node.lineIdx == a_feature.lineIdx) //x为lineIdx，同一条扫描线上的不进行生长
|
||||
continue;
|
||||
|
||||
//判断生长点
|
||||
double y_diff_1 = abs(a_feature.startPt.y - last_node.startPt.y);
|
||||
double y_diff_2 = abs(a_feature.endPt.y - last_node.endPt.y);
|
||||
double z_diff_1 = abs(a_feature.startPt.z - last_node.startPt.z);
|
||||
double z_diff_2 = abs(a_feature.endPt.z - last_node.endPt.z);
|
||||
int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
|
||||
double x_diff = (abs(a_feature.startPt.x - last_node.startPt.x) + abs(a_feature.endPt.x - last_node.endPt.x)) / 2;
|
||||
if ((y_diff_1 < growParam.yDeviation_max) && (y_diff_2 < growParam.yDeviation_max) &&
|
||||
(z_diff_1 < growParam.zDeviation_max) && (z_diff_2 < growParam.zDeviation_max) &&
|
||||
(a_feature.endPt. y >= last_node.startPt.y) && (last_node.endPt.y >= a_feature.startPt.y)&& //重叠
|
||||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
|
||||
{
|
||||
a_tree.eLineIdx = lineIdx;
|
||||
a_tree.treeNodes.push_back(a_feature);
|
||||
a_tree.tree_value += (a_feature.startPt.z + a_feature.endPt.z) / 2;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
//将segment feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||||
bool _rodArcFeatureGrowing(SWD_rodArcFeature& a_feature, const int lineIdx, std::vector<SWD_rodArcFeatureTree>& trees, const SSG_treeGrowParam growParam)
|
||||
{
|
||||
@ -732,6 +766,76 @@ void wd_getSegFeatureGrowingTrees(
|
||||
|
||||
}
|
||||
|
||||
//segment特征生长, 相信seg间要保证有重叠区
|
||||
void wd_getSegFeatureGrowingTrees_2(
|
||||
std::vector<std::vector<SWD_segFeature>>& all_lineFeatures,
|
||||
std::vector<SWD_segFeatureTree>& feature_trees,
|
||||
const SSG_treeGrowParam growParam)
|
||||
{
|
||||
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 1047)
|
||||
int kkk = 1;
|
||||
std::vector<SWD_segFeature>& a_lineFeatures = all_lineFeatures[i];
|
||||
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
|
||||
{
|
||||
SWD_segFeature& a_feature = a_lineFeatures[j];
|
||||
bool isMatched = _segFeatureGrowing_2(a_feature, i, feature_trees, growParam);
|
||||
if (false == isMatched)
|
||||
{
|
||||
//新的生长树
|
||||
SWD_segFeatureTree a_newTree;
|
||||
a_newTree.treeNodes.push_back(a_feature);
|
||||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||||
a_newTree.sLineIdx = a_feature.lineIdx;
|
||||
a_newTree.eLineIdx = a_feature.lineIdx;
|
||||
a_newTree.tree_value = (a_feature.startPt.z + a_feature.endPt.z) / 2;
|
||||
feature_trees.push_back(a_newTree);
|
||||
}
|
||||
}
|
||||
//检查停止生长的树,加速。
|
||||
//将生长节点为1的生长树移除
|
||||
int lineIdx = i;
|
||||
int m_max = (int)feature_trees.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
if (TREE_STATE_ALIVE == feature_trees[m].treeState)
|
||||
{
|
||||
int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().lineIdx);
|
||||
if (line_diff > 0)
|
||||
{
|
||||
SWD_segFeature* first_node = &(feature_trees[m].treeNodes[0]);
|
||||
SWD_segFeature* last_node = &(feature_trees[m].treeNodes[feature_trees[m].treeNodes.size() - 1]);
|
||||
double len = (abs(first_node->startPt.x - last_node->startPt.x) + abs(first_node->endPt.x - last_node->endPt.x)) / 2;
|
||||
int tree_lines = feature_trees[m].eLineIdx - feature_trees[m].sLineIdx;
|
||||
if (tree_lines > 0)
|
||||
{
|
||||
double mean_stepping = len / tree_lines;
|
||||
double x_skip = mean_stepping * line_diff;
|
||||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||||
((growParam.maxLineSkipNum <= 0) && (x_skip > growParam.maxSkipDistance)) ||
|
||||
(i == i_max - 1))
|
||||
{
|
||||
feature_trees[m].treeState = TREE_STATE_DEAD;
|
||||
if (len <= growParam.minLTypeTreeLen)
|
||||
feature_trees.erase(feature_trees.begin() + m);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//将node为1的tree去除。生长时可能无法去除node数量为1的tree
|
||||
int m_max = (int)feature_trees.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
int tree_lines = feature_trees[m].eLineIdx - feature_trees[m].sLineIdx;
|
||||
if (tree_lines == 0)
|
||||
feature_trees.erase(feature_trees.begin() + m);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void wd_getRodArcFeatureGrowingTrees(
|
||||
std::vector<std::vector<SWD_rodArcFeature>>& all_lineFeatures,
|
||||
std::vector<SWD_rodArcFeatureTree>& feature_trees,
|
||||
|
||||
@ -254,6 +254,47 @@ void wd_getLineDataIntervals(
|
||||
segs.push_back(a_run);
|
||||
}
|
||||
|
||||
//提取空点段。与wd_getLineDataIntervals（）正好相反
|
||||
void wd_getLineDataNullIntervals(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
std::vector<SSG_RUN>& segs)
|
||||
{
|
||||
int runIdx = 1;
|
||||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||||
SVzNL3DPosition preData = { -1, {0, 0, 0} };
|
||||
int dataSize = (int)lineData.size();
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z < 1e-4)
|
||||
{
|
||||
//若前一个不为空，RUN的起点
|
||||
if ( (preData.nPointIdx >= 0) && (preData.pt3D.z > 1e-4))
|
||||
{
|
||||
a_run.start = i-1;
|
||||
a_run.len = 2;
|
||||
a_run.value = i;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//若前一个为空，RUN的终点
|
||||
if ((preData.nPointIdx >= 0) && (preData.pt3D.z < 1e-4) &&(a_run.start >= 0))
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
a_run.value = i;
|
||||
//判断长度
|
||||
double gap = abs(lineData[i].pt3D.y - lineData[a_run.start].pt3D.y);
|
||||
if(gap < lineSegPara.segGapTh_y)
|
||||
segs.push_back(a_run);
|
||||
a_run = { 0, -1, 0 };
|
||||
}
|
||||
|
||||
}
|
||||
preData = lineData[i];
|
||||
}
|
||||
}
|
||||
|
||||
//滤除离群点：z跳变门限方法
|
||||
void sg_lineDataRemoveOutlier_changeOriginData(
|
||||
SVzNL3DPosition* lineData,
|
||||
|
||||
@ -9,7 +9,8 @@
|
||||
//version 1.0.2 : 添加了工件姿态（欧拉角输出)
|
||||
//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量
|
||||
//version 1.2.0 : 算法完成了6轴验证
|
||||
std::string m_strVersion = "1.2.0";
|
||||
//version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
|
||||
std::string m_strVersion = "1.3.0";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -152,6 +153,152 @@ 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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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_workpieceHolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
@ -177,6 +324,9 @@ void wd_workpieceHolePositioning(
|
||||
|
||||
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)//数据不是网格格式
|
||||
{
|
||||
@ -236,11 +386,12 @@ void wd_workpieceHolePositioning(
|
||||
std::vector<std::vector<int>> pointMask;
|
||||
pointMask.resize(lineNum);
|
||||
|
||||
std::vector<SVzNL3DPoint> endingPoints;
|
||||
//提取空白线段特征（孔特征）
|
||||
std::vector<std::vector<SWD_segFeature>> holeGaps;
|
||||
//提取线段端点特征
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 1677)
|
||||
if (line == 1047)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
@ -250,21 +401,28 @@ void wd_workpieceHolePositioning(
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataIntervals(
|
||||
lineData,
|
||||
lineSegPara,
|
||||
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 = segs[i].start;
|
||||
endingPoints.push_back(lineData[ptIdx].pt3D);
|
||||
pointMask[line][ptIdx] = 1; //防止重复
|
||||
ptIdx = segs[i].start + segs[i].len - 1;
|
||||
endingPoints.push_back(lineData[ptIdx].pt3D);
|
||||
pointMask[line][ptIdx] = 1;
|
||||
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);
|
||||
}
|
||||
//特征生长
|
||||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||||
wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);
|
||||
|
||||
//生成水平扫描
|
||||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||||
@ -282,8 +440,8 @@ void wd_workpieceHolePositioning(
|
||||
}
|
||||
}
|
||||
//水平arc特征提取
|
||||
int lineNum_h_raw = (int)hLines_raw.size();
|
||||
for (int line = 0; line < lineNum_h_raw; line++)
|
||||
std::vector<std::vector<SWD_segFeature>> holeGaps_h;
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
if (line == 974)
|
||||
int kkk = 1;
|
||||
@ -293,107 +451,263 @@ void wd_workpieceHolePositioning(
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||||
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataIntervals(
|
||||
lineData,
|
||||
lineSegPara,
|
||||
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 = segs[i].start;
|
||||
if (pointMask[ptIdx][line] == 0) //防止点重复
|
||||
{
|
||||
SVzNL3DPoint an_ending;
|
||||
an_ending.x = lineData[ptIdx].pt3D.y;
|
||||
an_ending.y = lineData[ptIdx].pt3D.x;
|
||||
an_ending.z = lineData[ptIdx].pt3D.z;
|
||||
endingPoints.push_back(an_ending);
|
||||
pointMask[ptIdx][line] = 1;
|
||||
}
|
||||
ptIdx = segs[i].start + segs[i].len - 1;
|
||||
if (pointMask[ptIdx][line] == 0) //防止点重复
|
||||
{
|
||||
SVzNL3DPoint an_ending;
|
||||
an_ending.x = lineData[ptIdx].pt3D.y;
|
||||
an_ending.y = lineData[ptIdx].pt3D.x;
|
||||
an_ending.z = lineData[ptIdx].pt3D.z;
|
||||
endingPoints.push_back(an_ending);
|
||||
pointMask[ptIdx][line] = 1;
|
||||
}
|
||||
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);
|
||||
}
|
||||
//特征生长
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);
|
||||
|
||||
//标注
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
|
||||
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
|
||||
featureInfoMask.resize(lineNum);
|
||||
feature3DInfo.resize(lineNum);
|
||||
//创建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++)
|
||||
{
|
||||
featureInfoMask[i].resize(lineNum_h_raw);
|
||||
feature3DInfo[i].resize(lineNum_h_raw);
|
||||
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() > 2)
|
||||
{
|
||||
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() > 2)
|
||||
{
|
||||
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++)
|
||||
{
|
||||
std::vector<int>& a_lineMask = pointMask[line];
|
||||
for (int m = 0; m < lineNum_h_raw; m++)
|
||||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||||
{
|
||||
if (a_lineMask[m] > 0)
|
||||
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])
|
||||
{
|
||||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[line][m];
|
||||
a_featureInfo.clusterID = 0;
|
||||
a_featureInfo.featurType = 1;
|
||||
a_featureInfo.featureIdx_v = 0;
|
||||
a_featureInfo.featureIdx_h = 0;
|
||||
a_featureInfo.lineIdx = line;
|
||||
a_featureInfo.ptIdx = m;
|
||||
a_featureInfo.flag = 0;
|
||||
feature3DInfo[line][m] = scanLines[line][m].pt3D;
|
||||
hTreeIdx = j;
|
||||
commonSize = treeHVInfo[i][j];
|
||||
}
|
||||
}
|
||||
if (hTreeIdx >= 0)
|
||||
{
|
||||
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
|
||||
if ((commonSize > totalSize_h / 4) && (commonSize > totalSize_v / 4))
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
//聚类
|
||||
//采用迭代思想，回归思路进行高效聚类
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||||
int clusterID = 1;
|
||||
int clusterCheckWin = 5;
|
||||
for (int y = 0; y < lineNum_h_raw; y++)
|
||||
|
||||
//滤除相邻。每个目标保留一个
|
||||
for (int i = 0; i < (int)objects.size(); i++)
|
||||
{
|
||||
for (int x = 0; x < lineNum; x++)
|
||||
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)
|
||||
{
|
||||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||||
continue;
|
||||
|
||||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||||
SVzNL3DRangeD a_clusterRoi;
|
||||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||||
|
||||
SVzNL2DPoint a_seedPos = { x, y };
|
||||
std::vector< SVzNL2DPoint> a_cluster;
|
||||
a_cluster.push_back(a_seedPos);
|
||||
wd_gridPointClustering(
|
||||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||||
feature3DInfo,//double,记录坐标信息
|
||||
clusterCheckWin, //搜索窗口
|
||||
growParam,//聚类条件
|
||||
clusterID, //当前Cluster的ID
|
||||
a_cluster, //result
|
||||
a_clusterRoi
|
||||
);
|
||||
clusters.push_back(a_cluster);
|
||||
clustersRoi3D.push_back(a_clusterRoi);
|
||||
clusterID++;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
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]);
|
||||
}
|
||||
|
||||
//生成聚类信息，
|
||||
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)segTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
|
||||
if (scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||||
{
|
||||
scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||||
}
|
||||
if (scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||||
{
|
||||
scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||||
}
|
||||
}
|
||||
for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||||
{
|
||||
SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
|
||||
if (scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||||
{
|
||||
scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||||
SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||||
}
|
||||
if (scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||||
{
|
||||
scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||||
SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
|
||||
a_cluster.push_back(a_pos);
|
||||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||||
}
|
||||
}
|
||||
clusters.push_back(a_cluster);
|
||||
clustersRoi3D.push_back(a_roi3D);
|
||||
}
|
||||
|
||||
//聚类结果分析
|
||||
std::vector<int> validCluserIndexing;
|
||||
int clusterSize = (int)clusters.size();
|
||||
@ -402,8 +716,8 @@ void wd_workpieceHolePositioning(
|
||||
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;
|
||||
if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2) &&
|
||||
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2))
|
||||
if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) &&
|
||||
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5))
|
||||
validCluserIndexing.push_back(i);
|
||||
}
|
||||
//生成结果
|
||||
@ -419,9 +733,8 @@ void wd_workpieceHolePositioning(
|
||||
for (int i = 0; i < clusterPtSize; i++)
|
||||
{
|
||||
SVzNL2DPoint a_pos = clusters[clusterIdx][i];
|
||||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y];
|
||||
int lineIdx = a_featureInfo.lineIdx;
|
||||
int ptIdx = a_featureInfo.ptIdx;
|
||||
int lineIdx = a_pos.x;
|
||||
int ptIdx = a_pos.y;
|
||||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||||
if (minZ > a_pt3d.z)
|
||||
minZ = a_pt3d.z;
|
||||
@ -440,6 +753,7 @@ void wd_workpieceHolePositioning(
|
||||
//分割
|
||||
//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
|
||||
double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
|
||||
std::vector< WD_workpieceInfo> allWorkpiece;
|
||||
for (int objIdx = 0; objIdx < objectSize; objIdx++)
|
||||
{
|
||||
if (holes[objIdx].radius < 0)
|
||||
@ -518,11 +832,40 @@ void wd_workpieceHolePositioning(
|
||||
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
|
||||
a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z + 20 };
|
||||
|
||||
workpiecePositioning.push_back(a_workpiece);
|
||||
allWorkpiece.push_back(a_workpiece);
|
||||
}
|
||||
int workpieceNum = (int)allWorkpiece.size();
|
||||
if (workpieceNum == 0)
|
||||
return;
|
||||
|
||||
//排序
|
||||
//z方向排序
|
||||
std::vector< WD_workpieceInfo> zSortWorkpiece;
|
||||
double minZ = allWorkpiece[0].center.z;
|
||||
for (int i = 1; i < workpieceNum; i++)
|
||||
{
|
||||
if (minZ > allWorkpiece[i].center.z)
|
||||
minZ = allWorkpiece[i].center.z;
|
||||
}
|
||||
double topLayerTh = minZ + workpiecePara.H / 2;
|
||||
for (int i = 0; i < workpieceNum; i++)
|
||||
{
|
||||
if (allWorkpiece[i].center.z < topLayerTh)
|
||||
zSortWorkpiece.push_back(allWorkpiece[i]);
|
||||
}
|
||||
//水平方向排序
|
||||
while (zSortWorkpiece.size() > 0)
|
||||
{
|
||||
std::vector< WD_workpieceInfo> firstLine;
|
||||
std::vector< WD_workpieceInfo> restWorkpiece;
|
||||
_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpiecePara.yLen / 2);
|
||||
workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end());
|
||||
zSortWorkpiece.clear();
|
||||
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
|
||||
}
|
||||
|
||||
int workpieceNum = (int)workpiecePositioning.size();
|
||||
//旋转回去
|
||||
workpieceNum = (int)workpiecePositioning.size();
|
||||
for (int i = 0; i < workpieceNum; i++)
|
||||
{
|
||||
SVzNL3DPoint rpt;
|
||||
|
||||
@ -11,6 +11,9 @@ typedef struct
|
||||
double holeDiameter; //ż×Öąžś
|
||||
double holeDist_L; //ż×źäžŕ_ł¤
|
||||
double holeDist_W; //ż×źäžŕ_żí
|
||||
double xLen; //x方向长， 用于结果排序
|
||||
double yLen; //y方向长， 用于结果排序
|
||||
double H;
|
||||
}WD_workpieceHoleParam;
|
||||
|
||||
typedef struct
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -276,6 +276,9 @@ void _outputRGBDResult_RGBD(
|
||||
for (int i = 0; i < objNumber; i++)
|
||||
{
|
||||
SVzNL3DPosition a_objPt;
|
||||
a_objPt.pt3D = workpiecePositioning[i].center;
|
||||
objects.push_back(a_objPt);
|
||||
|
||||
int holeNumber = (int)workpiecePositioning[i].holes.size();
|
||||
for (int j = 0; j < holeNumber; j++)
|
||||
{
|
||||
@ -283,8 +286,6 @@ void _outputRGBDResult_RGBD(
|
||||
a_objPt.pt3D = workpiecePositioning[i].holes[j];
|
||||
objects.push_back(a_objPt);
|
||||
}
|
||||
a_objPt.pt3D = workpiecePositioning[i].center;
|
||||
objects.push_back(a_objPt);
|
||||
}
|
||||
|
||||
int lineNum = (int)scanLines.size();
|
||||
@ -324,13 +325,10 @@ void _outputRGBDResult_RGBD(
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int featureType_v = pt3D->nPointIdx & 0xff;
|
||||
int featureType_h = featureType_v >> 4;
|
||||
featureType_v &= 0x0f;
|
||||
|
||||
if (pt3D->nPointIdx == 1)
|
||||
int flag = pt3D->nPointIdx & 0xffff;
|
||||
if (flag > 0)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
rgb = objColor[flag % 8]; // { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
@ -353,6 +351,10 @@ void _outputRGBDResult_RGBD(
|
||||
size = 10;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
if (i == 0)
|
||||
size = 20;
|
||||
else
|
||||
size = 10;
|
||||
int colorIdx = objects[i].nPointIdx % 8;
|
||||
rgb = objColor[colorIdx];
|
||||
float x = (float)objects[i].pt3D.x;
|
||||
@ -362,14 +364,6 @@ void _outputRGBDResult_RGBD(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//修正显示软件bug
|
||||
rgb = objColor[1];
|
||||
float x = (float)objects[0].pt3D.x;
|
||||
float y = (float)objects[0].pt3D.y;
|
||||
float z = (float)objects[0].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;
|
||||
@ -412,16 +406,6 @@ void _outputRGBDResult_RGBD(
|
||||
lineIdx++;
|
||||
}
|
||||
}
|
||||
SVzNL3DPoint dirPt = { workpiecePositioning[0].center.x + workpiecePositioning[0].y_dir.x * 10,
|
||||
workpiecePositioning[0].center.y + workpiecePositioning[0].y_dir.y * 10,
|
||||
workpiecePositioning[0].center.z + workpiecePositioning[0].y_dir.z * 10 };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << workpiecePositioning[0].center.x << "," << workpiecePositioning[0].center.y << "," << workpiecePositioning[0].center.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
sw.close();
|
||||
}
|
||||
@ -439,18 +423,20 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T)
|
||||
}
|
||||
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_HOLE 0
|
||||
#define TEST_COMPUTE_RT 1
|
||||
#define TEST_GROUP 1
|
||||
#define TEST_COMPUTE_HOLE 1
|
||||
#define TEST_COMPUTE_RT 0
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云2/", //1
|
||||
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{6,6},
|
||||
{6,6}, {1, 15}
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
@ -560,8 +546,9 @@ int main()
|
||||
}
|
||||
#endif
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
int cvtGrp = 1;
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "%sLaserData_ground2.txt", dataPath[0]);
|
||||
sprintf_s(_calib_datafile, "%s1470mm调平.txt", dataPath[cvtGrp]);
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
@ -585,18 +572,18 @@ int main()
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[0]);
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvtGrp]);
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[0]);
|
||||
sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[cvtGrp]);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
|
||||
#if 0
|
||||
for (int fidx = fileIdx[0].nMin; fidx <= fileIdx[0].nMax; fidx++)
|
||||
#if 1
|
||||
for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[0], fidx);
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[cvtGrp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
@ -607,7 +594,7 @@ int main()
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);
|
||||
}
|
||||
sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[0], fidx);
|
||||
sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
|
||||
}
|
||||
@ -617,7 +604,7 @@ int main()
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_HOLE
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
for (int grp = 1; grp <= 1; grp++)
|
||||
{
|
||||
SSG_planeCalibPara groundCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -651,23 +638,26 @@ int main()
|
||||
|
||||
SSG_lineSegParam lineSegPara;
|
||||
lineSegPara.distScale = 3.0;
|
||||
lineSegPara.segGapTh_y = 3.0; //y方向间隔大于5mm认为是分段
|
||||
lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段
|
||||
lineSegPara.segGapTh_y = 15.0; //
|
||||
lineSegPara.segGapTh_z = 0.0; //z方向间隔大于10mm认为是分段
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 10.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
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_workpieceInfo> workpiecePositioning;
|
||||
wd_workpieceHolePositioning(
|
||||
|
||||
@ -42,13 +42,13 @@
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user