hybridPosePositioning
version 1.3.0 : 添加了海瑞马转子钢芯定位
This commit is contained in:
parent
408a2c32c1
commit
fe7811b407
@ -673,7 +673,8 @@ void _outputRGBDResult_XYZ_RGBD_binInfo(
|
||||
void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector<WD_workpieceInfo>& poseInfo)
|
||||
std::vector<WD_workpieceInfo>& poseInfo,
|
||||
double dirLen)
|
||||
{
|
||||
int objNumber = 0;
|
||||
if (poseInfo.size()>0)
|
||||
@ -769,7 +770,7 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
|
||||
//输出方向线条
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 2;
|
||||
double dirLen = 500;
|
||||
//double dirLen = 500;
|
||||
for (int m = 0; m < (int)poseInfo.size(); m++)
|
||||
{
|
||||
SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen,
|
||||
@ -2111,7 +2112,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 0;
|
||||
//fidx = 3;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNLPositionD>> scanLines;
|
||||
@ -2152,11 +2153,24 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
else
|
||||
printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode);
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult/debug_%d_data.txt", dataPath[grp], fidx);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions);
|
||||
sprintf_s(calibFile, "%sresult/LaserLine%d_workpiece_info.txt", dataPath[grp], fidx);
|
||||
_outputWorkpieceInfo(calibFile, workpiecePositions);
|
||||
|
||||
//旋转到调平坐标系
|
||||
int workpieceNum = (int)workpiecePositions.size();
|
||||
for (int i = 0; i < workpieceNum; i++)
|
||||
{
|
||||
SVzNL3DPoint rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.planeCalib);
|
||||
workpiecePositions[i].center = rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.planeCalib);
|
||||
workpiecePositions[i].y_dir = rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.planeCalib);
|
||||
workpiecePositions[i].z_dir = rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.planeCalib);
|
||||
workpiecePositions[i].x_dir = rpt;
|
||||
}
|
||||
sprintf_s(_scan_file, "%sresult/result_%d.txt", dataPath[grp], fidx);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, 50);
|
||||
#if 0
|
||||
sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx);
|
||||
double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; //
|
||||
@ -2364,7 +2378,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void)
|
||||
a_pose.value = workpieceInfo.workpieceRadius;
|
||||
a_pose.z_dir = { 0, 0, 1.0 };
|
||||
poseInfo.push_back(a_pose);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, 500);
|
||||
sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx);
|
||||
_outputWorkpieceInfo(_scan_file, workpieceInfo);
|
||||
|
||||
@ -2501,8 +2515,8 @@ typedef enum
|
||||
int main()
|
||||
{
|
||||
//ESG_testMode testMode = keSG_2D3D定位_海瑞马_地面调平;
|
||||
ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件;
|
||||
//ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯;
|
||||
//ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件;
|
||||
ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯;
|
||||
//ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛料筐定位;
|
||||
//ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛工件尺寸测量;
|
||||
//ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛位置规划;
|
||||
|
||||
@ -10,7 +10,8 @@
|
||||
//version 1.0.0 : base version release to customer
|
||||
//version 1.1.0 : 海瑞马码垛规划版本
|
||||
//version 1.2.0 : 添加了海瑞马锥形工件定位版本（不包含二次定位）
|
||||
std::string m_strVersion = "HybridPositioning 1.1.0";
|
||||
//version 1.3.0 : 添加了海瑞马转子钢芯定位
|
||||
std::string m_strVersion = "HybridPositioning 1.3.0";
|
||||
const char* wd_hybridPositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -348,15 +349,21 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
flags[i].resize(linePtNum);
|
||||
|
||||
//内部参数
|
||||
double topSeg_minWidth = 5.0; //
|
||||
double topSeg_maxWidth = 20.0;
|
||||
|
||||
SSG_lineSegParam filterSegPara;
|
||||
filterSegPara.distScale = 0;
|
||||
filterSegPara.segGapTh_y = DBL_MAX;
|
||||
filterSegPara.segGapTh_y = topSeg_maxWidth;
|
||||
filterSegPara.segGapTh_z = workpieceParam.height / 2;
|
||||
|
||||
//垂直
|
||||
std::vector<std::vector<SWD_indexingSeg>> vScanLineSegs;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 83)
|
||||
int kkk = 1;
|
||||
//分段
|
||||
std::vector<SSG_RUN> simpleSegs;
|
||||
wd_getLineDataIntervals(scanLines[line], filterSegPara, simpleSegs);
|
||||
@ -430,6 +437,8 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
exSegs.push_back(a_seg);
|
||||
}
|
||||
}
|
||||
if (exSegs.size() == 0)
|
||||
return a_pose;
|
||||
//去除低的段：如果两个段的meanZ相差达到工件高度的一半，视为低的段
|
||||
int highestSeg = 0;
|
||||
for (int m = 1; m < (int)exSegs.size(); m++)
|
||||
@ -562,10 +571,11 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
SVzNL3DPoint centerPoint = scanLines[centerPosition.lineIdx][centerPosition.ptIdx].pt3D;
|
||||
std::vector< SWD_indexingSeg> segsTop;
|
||||
//生成工件顶面上的点: 宽度大于一个门限（滤噪）；边界点不能是边界
|
||||
double minWidth = 5.0; //
|
||||
//垂直扫描处理
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 83)
|
||||
int kkk = 1;
|
||||
int mostLeftSegId = -1; //对于垂直扫描，实际上是最上面
|
||||
int mostRightSegId = -1;//对于垂直扫描，实际上是最下面
|
||||
for (int i = 0; i < (int)vScanLineSegs[line].size(); i++)
|
||||
@ -577,10 +587,12 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
double width = abs(scanLines[line][idx1].pt3D.y - scanLines[line][idx2].pt3D.y);
|
||||
SVzNL3DPoint segCenter = exSeg.centerPoint.pt3D;
|
||||
double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
|
||||
if ( (width > minWidth) && (distToCircleCenter < workpieceParam.radius))
|
||||
if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
|
||||
{
|
||||
if (mostLeftSegId < 0)
|
||||
if ((mostLeftSegId < 0) && (idx2 < centerPosition.ptIdx) &&(idx1 > 0 ))
|
||||
mostLeftSegId = i;
|
||||
|
||||
if( (idx1 > centerPosition.ptIdx) && (idx2 < linePtNum - 1))
|
||||
mostRightSegId = i;
|
||||
}
|
||||
}
|
||||
@ -627,10 +639,12 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
double width = abs(hLines[line][idx1].pt3D.y - hLines[line][idx2].pt3D.y);
|
||||
SVzNL3DPoint segCenter = a_seg.centerPoint.pt3D;
|
||||
double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
|
||||
if ( (width > minWidth) && (distToCircleCenter < workpieceParam.radius))
|
||||
if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
|
||||
{
|
||||
if (mostLeftSegId < 0)
|
||||
if ((mostLeftSegId < 0) && (idx2 < centerPosition.lineIdx) && (idx1 > 0))
|
||||
mostLeftSegId = i;
|
||||
|
||||
if( (idx1 > centerPosition.lineIdx) && (idx2 < lineNum - 1))
|
||||
mostRightSegId = i;
|
||||
}
|
||||
}
|
||||
@ -664,25 +678,6 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
|
||||
}
|
||||
}
|
||||
//Éú³É±ê×¢ÐèÒªµÄÐÅÏ¢
|
||||
for (int i = 0; i < (int)segsTop.size(); i++)
|
||||
{
|
||||
SWD_indexingSeg& a_seg = segsTop[i];
|
||||
if (a_seg.hScanFlag == 0)
|
||||
{
|
||||
SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx };
|
||||
centerPtIndice.push_back(pos_center);
|
||||
SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx };
|
||||
outsideEdgePtIndice.push_back(pos_edge);
|
||||
}
|
||||
else
|
||||
{
|
||||
SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx};
|
||||
centerPtIndice.push_back(pos_center);
|
||||
SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx};
|
||||
outsideEdgePtIndice.push_back(pos_edge);
|
||||
}
|
||||
}
|
||||
|
||||
//聚类
|
||||
int clusterCheckWin = 5;
|
||||
@ -754,15 +749,48 @@ WD_workpieceInfo _computeWorkpiecePose(
|
||||
for (int m = 0; m < nodeSize; m++)
|
||||
pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
|
||||
}
|
||||
|
||||
SVzNL3DPoint center;
|
||||
double radius;
|
||||
double err = fitCircleByLeastSquare(pointArray,center,radius);
|
||||
double err = fitCircleByLeastSquare(pointArray, center, radius);
|
||||
|
||||
//迭代一次
|
||||
pointArray.clear();
|
||||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
|
||||
int nodeSize = (int)a_cluster.size();
|
||||
for (int m = 0; m < nodeSize; m++)
|
||||
{
|
||||
SWD_indexingSeg& a_seg = a_cluster[m];
|
||||
double r1 = sqrt(pow(center.x - a_seg.outsideEdgePoint.pt3D.x, 2) + pow(center.y - a_seg.outsideEdgePoint.pt3D.y, 2));
|
||||
if (r1 < workpieceParam.radius * 1.05)
|
||||
{
|
||||
pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
|
||||
//生成标注需要的信息
|
||||
if (a_seg.hScanFlag == 0)
|
||||
{
|
||||
SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx };
|
||||
centerPtIndice.push_back(pos_center);
|
||||
SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx };
|
||||
outsideEdgePtIndice.push_back(pos_edge);
|
||||
}
|
||||
else
|
||||
{
|
||||
SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx };
|
||||
centerPtIndice.push_back(pos_center);
|
||||
SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx };
|
||||
outsideEdgePtIndice.push_back(pos_edge);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
err = fitCircleByLeastSquare(pointArray, center, radius);
|
||||
a_pose.center = { center.x, center.y, objZ };
|
||||
a_pose.value = radius;
|
||||
a_pose.z_dir = { 0.0, 0.0, 1.0 };
|
||||
a_pose.y_dir = { 0.0, 0.0, 0.0 };
|
||||
a_pose.z_dir = { 0.0, 0.0, 0.0 };
|
||||
a_pose.x_dir = { 0.0, 0.0, 0.0 };
|
||||
return a_pose;
|
||||
}
|
||||
|
||||
@ -771,6 +799,38 @@ typedef struct
|
||||
std::vector<SWDScanPosition> mappings;//每个2D点可能对应多个3D点
|
||||
}mapping2DTo3D;
|
||||
|
||||
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_HRM_RotorCorePositioning(
|
||||
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
||||
std::vector<WD_objArea2D>& objROIs,
|
||||
@ -978,8 +1038,8 @@ void wd_HRM_RotorCorePositioning(
|
||||
}
|
||||
}
|
||||
|
||||
int objNum = 1; // (int)objROIs.size();
|
||||
|
||||
int objNum = (int)objROIs.size();
|
||||
std::vector<WD_workpieceInfo> allWorkpiece;
|
||||
for (int idx = 0; idx < objNum; idx++)
|
||||
{
|
||||
WD_objArea2D& obj_roi = objROIs[idx];
|
||||
@ -1009,7 +1069,8 @@ void wd_HRM_RotorCorePositioning(
|
||||
int ptIdx = a_mapping.mappings[m].ptIdx;
|
||||
if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
|
||||
{
|
||||
scanLinesInput[lineIdx][ptIdx].nPointIdx = idx + 1;
|
||||
scanLinesInput[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
|
||||
scanLinesInput[lineIdx][ptIdx].nPointIdx |= (idx + 1);
|
||||
roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin;
|
||||
roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax;
|
||||
roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin;
|
||||
@ -1041,7 +1102,8 @@ void wd_HRM_RotorCorePositioning(
|
||||
int kkk = 1;
|
||||
if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
|
||||
{
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = idx + 1;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx |= idx + 1;
|
||||
int roiLineIdx = lineIdx - roiLineIndice.nMin;
|
||||
int roiPtIdx = ptIdx - roiPtIndice.nMin;
|
||||
roiScanLines[roiLineIdx][roiPtIdx].nPointIdx = 0;
|
||||
@ -1062,6 +1124,7 @@ void wd_HRM_RotorCorePositioning(
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//判断工件种类，计算工件姿态
|
||||
std::vector<SWDScanPosition> outsideEdgePtIndice;
|
||||
std::vector<SWDScanPosition> centerPtIndice;
|
||||
@ -1073,6 +1136,8 @@ void wd_HRM_RotorCorePositioning(
|
||||
centerPtIndice);
|
||||
|
||||
#ifdef _OUTPUT_DEBUG_DATA
|
||||
if (a_workpiece.center.z > 1e-4)
|
||||
{
|
||||
//标注
|
||||
for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++)
|
||||
{
|
||||
@ -1086,12 +1151,55 @@ void wd_HRM_RotorCorePositioning(
|
||||
int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin;
|
||||
scanLines[line][ptIdx].nPointIdx |= 0x20000;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (a_workpiece.center.z > 1e-4)
|
||||
workpiecePositions.push_back(a_workpiece);
|
||||
allWorkpiece.push_back(a_workpiece);
|
||||
|
||||
}
|
||||
if (allWorkpiece.size() == 0)
|
||||
return;
|
||||
|
||||
//排序
|
||||
//搜索最高目标
|
||||
double highest_z = allWorkpiece[0].center.z;
|
||||
for (int i = 1; i < (int)allWorkpiece.size(); i++)
|
||||
highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z;
|
||||
//z方向排序
|
||||
std::vector< WD_workpieceInfo> zSortWorkpiece;
|
||||
double topLayerTh = highest_z + workpieceParam.height * 0.75;
|
||||
for (int i = 0; i < (int)allWorkpiece.size(); 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, workpieceParam.radius);
|
||||
workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end());
|
||||
zSortWorkpiece.clear();
|
||||
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
|
||||
}
|
||||
|
||||
//旋转回去
|
||||
int workpieceNum = (int)workpiecePositions.size();
|
||||
for (int i = 0; i < workpieceNum; i++)
|
||||
{
|
||||
SVzNL3DPoint rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
|
||||
workpiecePositions[i].center = rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
|
||||
workpiecePositions[i].y_dir = rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
|
||||
workpiecePositions[i].z_dir = rpt;
|
||||
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
|
||||
workpiecePositions[i].x_dir = rpt;
|
||||
}
|
||||
|
||||
#ifdef _OUTPUT_DEBUG_DATA
|
||||
debugScanLines.resize(scanLines.size());
|
||||
for (int line = 0; line < (int)scanLines.size(); line++)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user