workpieceHolePositioning version 1.5.4 :
添加山东本事机电轮胎定位API。
This commit is contained in:
parent
1777a0b9cf
commit
29c25997fd
@ -24,7 +24,8 @@
|
||||
//version 1.5.1 : 异物检测时，检测高度由工件最小Z向上1.5mm调整为工件最小Z向上工件高度的一半，工件倾斜时容错性能更好。
|
||||
//version 1.5.2 : 修正工件判断中的一个逻辑错误。
|
||||
//version 1.5.3 : 添加郑州微力砂轮盘和轮盘架定位API。
|
||||
std::string m_strVersion = "HolePostion 1.5.3";
|
||||
//version 1.5.4 : 添加山东本事机电轮胎定位API。
|
||||
std::string m_strVersion = "HolePostion 1.5.4";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -2597,3 +2598,518 @@ SSG_pointPose sx_getDiscRackCenterPosition(
|
||||
resultPose.pose_z = _ptRotate(resultPose.pose_z, poseR.invRMatrix);
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
int _getMaxROI(std::vector<SSG_ROIRectD>& roiList)
|
||||
{
|
||||
if (roiList.size() == 0)
|
||||
return -1;
|
||||
|
||||
int maxId = 0;
|
||||
double maxSize = (roiList[0].right - roiList[0].left) * (roiList[0].bottom - roiList[0].top);
|
||||
for (int i = 1; i < (int)roiList.size(); i++)
|
||||
{
|
||||
double size = (roiList[i].right - roiList[i].left) * (roiList[i].bottom - roiList[i].top);
|
||||
if (maxSize < size)
|
||||
{
|
||||
maxId = i;
|
||||
maxSize = size;
|
||||
}
|
||||
}
|
||||
return maxId;
|
||||
}
|
||||
|
||||
void _getFlagIntervals_v(
|
||||
std::vector<std::vector<int>>& flags,
|
||||
int index,
|
||||
const int maxSkipping,
|
||||
std::vector<SSG_RUN>& segs)
|
||||
{
|
||||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||||
int dataSize = (int)flags[index].size();
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if (flags[index][i] >=0 )
|
||||
{
|
||||
if (a_run.len < 0)
|
||||
{
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = flags[index][i];
|
||||
}
|
||||
else
|
||||
{
|
||||
int gap = i - (a_run.start + a_run.len - 1);
|
||||
if( (flags[index][i] == a_run.value) && (gap <= maxSkipping))
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
segs.push_back(a_run);
|
||||
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = a_run.value = flags[index][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (a_run.len > 0)
|
||||
segs.push_back(a_run);
|
||||
}
|
||||
|
||||
void _getFlagIntervals_h(
|
||||
std::vector<std::vector<int>>& flags,
|
||||
int index,
|
||||
const int maxSkipping,
|
||||
std::vector<SSG_RUN>& segs)
|
||||
{
|
||||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||||
int dataSize = (int)flags.size();
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if (flags[i][index] >= 0)
|
||||
{
|
||||
if (a_run.len < 0)
|
||||
{
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = flags[i][index];
|
||||
}
|
||||
else
|
||||
{
|
||||
int gap = i - (a_run.start + a_run.len - 1);
|
||||
if ((flags[i][index] == a_run.value) && (gap <= maxSkipping))
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
segs.push_back(a_run);
|
||||
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = a_run.value = flags[i][index];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (a_run.len > 0)
|
||||
segs.push_back(a_run);
|
||||
}
|
||||
|
||||
//山东本事机电轮胎中心定位
|
||||
void sx_getTireHolePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_tireParam tireParam,
|
||||
std::vector<WD_HolePositionInfo>& tirePositions,
|
||||
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 removeGroundHeight = groundCalibPara.planeHeight - tireParam.thickness * 0.1;
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{ //行处理
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
|
||||
}
|
||||
|
||||
//产生水平扫描数据
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||||
scanLines_h.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
scanLines_h[i].resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
scanLines_h[j][line] = scanLines[line][j];
|
||||
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
}
|
||||
}
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
||||
scanLines_h[line][j].nPointIdx = j;
|
||||
}
|
||||
|
||||
//算法流程：
|
||||
//1、检查垂直方向数据并去除
|
||||
//2、聚类
|
||||
//3、保留最前面目标
|
||||
//4、提取孔
|
||||
//5、拟合
|
||||
//6、计算中间坐标
|
||||
//内部参数
|
||||
SSG_cornerParam removeVertialPara = cornerPara;
|
||||
removeVertialPara.scale = 10.0;
|
||||
removeVertialPara.cornerTh = 60;
|
||||
|
||||
std::vector<std::vector<int>> flags;
|
||||
flags.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
flags[i].resize(linePtNum);
|
||||
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||||
}
|
||||
std::vector<std::vector<int>> zVertivalFlags;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 700)
|
||||
int kkk = 1;
|
||||
std::vector<int> line_verticalFlags;
|
||||
wd_getXYVertialFeature_dirAngleMethod(
|
||||
scanLines[line],
|
||||
line,
|
||||
removeVertialPara,
|
||||
line_verticalFlags
|
||||
);
|
||||
zVertivalFlags.push_back(line_verticalFlags);
|
||||
|
||||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||||
{
|
||||
if (line_verticalFlags[i] > 0)
|
||||
flags[line][i] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
if (line == 1177)
|
||||
int kkk = 1;
|
||||
std::vector<int> line_verticalFlags;
|
||||
wd_getXYVertialFeature_dirAngleMethod(
|
||||
scanLines_h[line],
|
||||
line,
|
||||
removeVertialPara,
|
||||
line_verticalFlags
|
||||
);
|
||||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||||
|
||||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||||
{
|
||||
if (line_verticalFlags[i] > 0)
|
||||
flags[i][line] = 1;
|
||||
}
|
||||
}
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
if (flags[line][j] > 0)
|
||||
{
|
||||
scanLines[line][j].pt3D.z = 0;
|
||||
scanLines_h[j][line].pt3D.z = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
//迭代一次
|
||||
SSG_lineSegParam lineSegPara;
|
||||
lineSegPara.distScale = 5.0;
|
||||
lineSegPara.segGapTh_y = 5.0;
|
||||
lineSegPara.segGapTh_z = 5.0;
|
||||
const int minSegLen = 5;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataIntervals(
|
||||
scanLines[line],
|
||||
lineSegPara,
|
||||
segs);
|
||||
for (int i = 0; i < (int)segs.size(); i++)
|
||||
{
|
||||
if (segs[i].len <= minSegLen)
|
||||
{
|
||||
int idx0 = segs[i].start;
|
||||
for (int j = 0; j < segs[i].len; j++)
|
||||
flags[line][idx0 + j] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataIntervals(
|
||||
scanLines_h[line],
|
||||
lineSegPara,
|
||||
segs);
|
||||
for (int i = 0; i < (int)segs.size(); i++)
|
||||
{
|
||||
if (segs[i].len <= minSegLen)
|
||||
{
|
||||
int idx0 = segs[i].start;
|
||||
for (int j = 0; j < segs[i].len; j++)
|
||||
flags[idx0 + j][line] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//标注
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
scanLines[line][j].nPointIdx = 0;
|
||||
}
|
||||
}
|
||||
//将垂直线段去除
|
||||
std::vector< SVzNL3DPosition> validPoints;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
if (flags[line][j] > 0)
|
||||
scanLines[line][j].pt3D.z = 0;
|
||||
|
||||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||
{
|
||||
SVzNL3DPosition a_vldPt;
|
||||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||
validPoints.push_back(a_vldPt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//聚类
|
||||
int clusterCheckWin = 5;
|
||||
double clusterDist = 5.0;
|
||||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||||
wd_pointClustering_speedUp(
|
||||
validPoints,
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
distType,
|
||||
objClusters //result
|
||||
);
|
||||
|
||||
//保留最前面的目标
|
||||
double minObjSize = tireParam.diameter * 0.8;
|
||||
double maxObjSize = tireParam.diameter * 1.5;
|
||||
const double topPlateMaxZRange = 100.0;
|
||||
std::vector<double> objMeanZ;
|
||||
std::vector<SSG_ROIRectD> objROIs;
|
||||
std::vector<SVzNLRangeD> objZRange;
|
||||
objMeanZ.resize(objClusters.size());
|
||||
objROIs.resize(objClusters.size());
|
||||
objZRange.resize(objClusters.size());
|
||||
std::vector<int> validObjIndice;
|
||||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||||
objROIs[i] = a_roi;
|
||||
double w = a_roi.right - a_roi.left;
|
||||
double h = a_roi.bottom - a_roi.top;
|
||||
if ( (w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize))
|
||||
{
|
||||
SVzNLRangeD zRange;
|
||||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||||
objMeanZ[i] = meanZ;
|
||||
objZRange[i] = zRange;
|
||||
validObjIndice.push_back(i);
|
||||
}
|
||||
else
|
||||
{
|
||||
objMeanZ[i] = 0;
|
||||
objZRange[i].max = -1.0;
|
||||
objZRange[i].min = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
if (validObjIndice.size() == 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
//标注
|
||||
//重新将flags设置为目标的mask
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||||
for (int m = 0; m < (int)validObjIndice.size(); m++)
|
||||
{
|
||||
int objIdx = validObjIndice[m];
|
||||
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
|
||||
{
|
||||
int line = objClusters[objIdx][i].nPointIdx >> 16;
|
||||
int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
|
||||
scanLinesInput[line][ptIdx].nPointIdx = 2;
|
||||
flags[line][ptIdx] = m; //indexing
|
||||
}
|
||||
}
|
||||
//提取轮廓点
|
||||
int maxSkipping = 3; //间隔小于3的空白视为连续，消除扫描拉条子的影响
|
||||
std::vector<std::vector<int>> contourMasks;
|
||||
contourMasks.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
contourMasks[i].resize(linePtNum);
|
||||
std::fill(contourMasks[i].begin(), contourMasks[i].end(), -1);
|
||||
}
|
||||
//垂直方向
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SSG_RUN> flagSegs;
|
||||
_getFlagIntervals_v(
|
||||
flags,
|
||||
line,
|
||||
maxSkipping,
|
||||
flagSegs);
|
||||
for (int j = 0; j < (int)flagSegs.size(); j++)
|
||||
{
|
||||
int idx1 = flagSegs[j].start;
|
||||
int idx2 = flagSegs[j].start + flagSegs[j].len - 1;
|
||||
contourMasks[line][idx1] = flagSegs[j].value;
|
||||
contourMasks[line][idx2] = flagSegs[j].value;
|
||||
}
|
||||
}
|
||||
//水平方向
|
||||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||||
{
|
||||
std::vector<SSG_RUN> flagSegs;
|
||||
_getFlagIntervals_h(
|
||||
flags,
|
||||
ptIdx,
|
||||
maxSkipping,
|
||||
flagSegs);
|
||||
for (int j = 0; j < (int)flagSegs.size(); j++)
|
||||
{
|
||||
int idx1 = flagSegs[j].start;
|
||||
int idx2 = flagSegs[j].start + flagSegs[j].len - 1;
|
||||
contourMasks[idx1][ptIdx] = flagSegs[j].value;
|
||||
contourMasks[idx2][ptIdx] = flagSegs[j].value;
|
||||
}
|
||||
}
|
||||
std::vector<std::vector< SVzNL3DPosition>> contourPoints;
|
||||
contourPoints.resize(validObjIndice.size());
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||||
{
|
||||
if (contourMasks[line][ptIdx] >= 0)
|
||||
{
|
||||
int cIdx = contourMasks[line][ptIdx];
|
||||
SVzNL3DPosition a_vldPt;
|
||||
a_vldPt.pt3D = scanLines[line][ptIdx].pt3D;
|
||||
a_vldPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
|
||||
contourPoints[cIdx].push_back(a_vldPt);
|
||||
}
|
||||
}
|
||||
}
|
||||
//标记
|
||||
for (int m = 0; m < (int)contourPoints.size(); m++)
|
||||
{
|
||||
for (int j = 0; j < contourPoints[m].size(); j++)
|
||||
{
|
||||
int line = contourPoints[m][j].nPointIdx >> 16;
|
||||
int ptIdx = contourPoints[m][j].nPointIdx & 0x0000FFFF;
|
||||
scanLinesInput[line][ptIdx].nPointIdx = m + 3;
|
||||
}
|
||||
}
|
||||
|
||||
//逐个计算目标姿态
|
||||
clusterDist = 10.0; //轮廓点聚类适当加大距离
|
||||
distType = 0; //0 - 2d distance; 1- 3d distance
|
||||
for (int m = 0; m < (int)contourPoints.size(); m++)
|
||||
{
|
||||
std::vector<std::vector< SVzNL3DPosition>> contourClusters; //result
|
||||
wd_pointClustering_speedUp(
|
||||
contourPoints[m],
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
distType,
|
||||
contourClusters //result
|
||||
);
|
||||
//计算ROI
|
||||
std::vector<SSG_ROIRectD> coutourROIs;
|
||||
for (int i = 0; i < (int)contourClusters.size(); i++)
|
||||
{
|
||||
SSG_ROIRectD a_roi = _getListROI(contourClusters[i]);
|
||||
coutourROIs.push_back(a_roi);
|
||||
}
|
||||
//去掉最大的目标
|
||||
int maxId = _getMaxROI(coutourROIs);
|
||||
if (maxId < 0)
|
||||
continue;
|
||||
double innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left;
|
||||
double innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top;
|
||||
while ((innerHole_w > minObjSize) || (innerHole_h > minObjSize))
|
||||
{
|
||||
coutourROIs[maxId].right = 0;
|
||||
coutourROIs[maxId].left = 0;
|
||||
coutourROIs[maxId].bottom = 0;
|
||||
coutourROIs[maxId].top = 0;
|
||||
//继续找最大的目标，若大于门限，视为目标
|
||||
maxId = _getMaxROI(coutourROIs);
|
||||
if (maxId < 0)
|
||||
continue;
|
||||
|
||||
innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left;
|
||||
innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top;
|
||||
}
|
||||
|
||||
if ((innerHole_w > tireParam.diameter * 0.4) && (innerHole_h > tireParam.diameter * 0.4))
|
||||
{
|
||||
//计算姿态
|
||||
SVzNLRangeD holezRange;
|
||||
double meanZ = _getListMeanZ(contourClusters[maxId], holezRange);
|
||||
//拟合圆心
|
||||
//圆最小二乘拟合
|
||||
SVzNL3DPoint center = { 0.0, 0.0, 0.0 };
|
||||
double radius = 0;
|
||||
double errValue = fitCircleByLeastSquare_2( contourClusters[maxId], center, radius);
|
||||
if ((radius > tireParam.diameter * 0.2) && (radius < minObjSize * 0.5))
|
||||
{
|
||||
WD_HolePositionInfo a_holeInfo;
|
||||
a_holeInfo.center = { center.x, center.y, meanZ };
|
||||
a_holeInfo.normDir = { 0.0, 0.0, 1.0 };
|
||||
tirePositions.push_back(a_holeInfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (tirePositions.size() == 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
//旋转回原坐标系
|
||||
for (int i = 0; i < (int)tirePositions.size(); i++)
|
||||
{
|
||||
tirePositions[i].center = _ptRotate(tirePositions[i].center, groundCalibPara.invRMatrix);
|
||||
tirePositions[i].normDir = _ptRotate(tirePositions[i].normDir, groundCalibPara.invRMatrix);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@ -27,6 +27,12 @@ typedef struct
|
||||
SVzNL3DPoint x_dir; //x方向向量（归一化）
|
||||
}WD_workpieceInfo;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double diameter; //轮胎直径
|
||||
double thickness; //轮胎宽度
|
||||
}WD_tireParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint center;
|
||||
@ -79,3 +85,12 @@ SG_APISHARED_EXPORT SSG_pointPose sx_getDiscRackCenterPosition(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& src_scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
int* errCode);
|
||||
|
||||
//山东本事机电轮胎中心定位
|
||||
SG_APISHARED_EXPORT void sx_getTireHolePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_tireParam tireParam,
|
||||
std::vector<WD_HolePositionInfo>& tirePositions,
|
||||
int* errCode);
|
||||
@ -593,7 +593,8 @@ void _outputRGBDResult_RGBD_2(
|
||||
void _outputRGBDResult_HoleInfo(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< WD_HolePositionInfo>& holePositioning)
|
||||
std::vector< WD_HolePositionInfo>& holePositioning,
|
||||
const double dirLen)
|
||||
{
|
||||
std::vector<SVzNL3DPosition> objects;
|
||||
int objNumber = (int)holePositioning.size();
|
||||
@ -642,7 +643,12 @@ void _outputRGBDResult_HoleInfo(
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int flag = pt3D->nPointIdx & 0xffff;
|
||||
if (flag > 0)
|
||||
if (flag == 2)
|
||||
{
|
||||
rgb = { 0, 0, 200 };
|
||||
size = 2;
|
||||
}
|
||||
else if (flag > 2)
|
||||
{
|
||||
rgb = objColor[flag % 8]; // { 255, 97, 0 };
|
||||
size = 5;
|
||||
@ -681,12 +687,12 @@ void _outputRGBDResult_HoleInfo(
|
||||
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 };
|
||||
dirPt_1 = { holePositioning[i].center.x - holePositioning[i].normDir.x * dirLen,
|
||||
holePositioning[i].center.y - holePositioning[i].normDir.y * dirLen,
|
||||
holePositioning[i].center.z - holePositioning[i].normDir.z * dirLen };
|
||||
dirPt_2 = { holePositioning[i].center.x + holePositioning[i].normDir.x * dirLen,
|
||||
holePositioning[i].center.y + holePositioning[i].normDir.y * dirLen,
|
||||
holePositioning[i].center.z + holePositioning[i].normDir.z * dirLen };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << dirPt_1.x << "," << dirPt_1.y << "," << dirPt_1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
@ -1156,8 +1162,9 @@ void HuaHang_holePosition_test(void)
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
double dirLen = 20;
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDResult_HoleInfo(_scan_file, scanLines, holePositioning);
|
||||
_outputRGBDResult_HoleInfo(_scan_file, scanLines, holePositioning, dirLen);
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputHoleInfo(_scan_file, holePositioning);
|
||||
}
|
||||
@ -1283,28 +1290,177 @@ void Weili_grindingDiscRackPosition_test(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
//山东本事机电轮胎定位
|
||||
#define BENSHI_COMPUTE_CALIB_PARA 0
|
||||
#define BENSHI_HOLE_POSITION 1
|
||||
|
||||
#define BENSHI_TIRE_TEST_GROUP 1
|
||||
void Benshi_tirePosition_test(void)
|
||||
{
|
||||
const char* dataPath[BENSHI_TIRE_TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/山东本事机电轮胎抓取/数据/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = {
|
||||
{1,19},
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if BENSHI_COMPUTE_CALIB_PARA
|
||||
int cvtGrp = 0;
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "%s调平.txt", dataPath[cvtGrp]);
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
|
||||
|
||||
lineNum = (int)scanData.size();
|
||||
if (scanData.size() > 0)
|
||||
{
|
||||
SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData);
|
||||
//结果进行验证
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanData[i], calibPara.planeCalib, -1);
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
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[cvtGrp]);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
|
||||
#if 0
|
||||
for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[cvtGrp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
lineNum = (int)scanLines.size();
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);
|
||||
}
|
||||
sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
|
||||
}
|
||||
#endif
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if BENSHI_HOLE_POSITION
|
||||
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
SSG_planeCalibPara groundCalibPara;
|
||||
//初始化成单位阵
|
||||
groundCalibPara.planeCalib[0] = 1.0;
|
||||
groundCalibPara.planeCalib[1] = 0.0;
|
||||
groundCalibPara.planeCalib[2] = 0.0;
|
||||
groundCalibPara.planeCalib[3] = 0.0;
|
||||
groundCalibPara.planeCalib[4] = 1.0;
|
||||
groundCalibPara.planeCalib[5] = 0.0;
|
||||
groundCalibPara.planeCalib[6] = 0.0;
|
||||
groundCalibPara.planeCalib[7] = 0.0;
|
||||
groundCalibPara.planeCalib[8] = 1.0;
|
||||
groundCalibPara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
groundCalibPara = _readCalibPara(calibFile);
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =17;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.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_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 5.0;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
WD_tireParam tireParam;
|
||||
tireParam.diameter = 500;
|
||||
tireParam.thickness = 140;
|
||||
|
||||
int errCode = 0;
|
||||
std::vector<WD_HolePositionInfo> tirePositions;
|
||||
sx_getTireHolePose(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
groundCalibPara,
|
||||
tireParam,
|
||||
tirePositions,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
double dirLen = 200;
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDResult_HoleInfo(_scan_file, scanLines, tirePositions, dirLen);
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputHoleInfo(_scan_file, tirePositions);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSG_测试_拓普发孔定位 = 0,
|
||||
keSG_测试_华航孔定位,
|
||||
keSG_测试_微力砂轮盘定位,
|
||||
keSG_测试_微力砂轮架子定位,
|
||||
keSG_拓普发孔定位 = 0,
|
||||
keSG_华航孔定位,
|
||||
keSG_微力砂轮盘定位,
|
||||
keSG_微力砂轮架子定位,
|
||||
keSG_山东本事轮胎定位,
|
||||
} ESG_testMode;
|
||||
|
||||
int main()
|
||||
{
|
||||
//ESG_testMode testMode = keSG_测试_拓普发孔定位;
|
||||
//ESG_testMode testMode = keSG_测试_华航孔定位;
|
||||
//ESG_testMode testMode = keSG_测试_微力砂轮盘定位;
|
||||
ESG_testMode testMode = keSG_测试_微力砂轮架子定位;
|
||||
//ESG_testMode testMode = keSG_拓普发孔定位;
|
||||
//ESG_testMode testMode = keSG_华航孔定位;
|
||||
//ESG_testMode testMode = keSG_微力砂轮盘定位;
|
||||
//ESG_testMode testMode = keSG_微力砂轮架子定位;
|
||||
ESG_testMode testMode = keSG_山东本事轮胎定位;
|
||||
|
||||
|
||||
if (keSG_测试_拓普发孔定位 == testMode)
|
||||
if (keSG_拓普发孔定位 == testMode)
|
||||
TuoPuFa_holePosition_test();
|
||||
else if (keSG_测试_华航孔定位 == testMode)
|
||||
else if (keSG_华航孔定位 == testMode)
|
||||
HuaHang_holePosition_test();
|
||||
else if (keSG_测试_微力砂轮盘定位 == testMode)
|
||||
else if (keSG_微力砂轮盘定位 == testMode)
|
||||
Weili_grindingDiscHolePosition_test();
|
||||
else if (keSG_测试_微力砂轮架子定位 == testMode)
|
||||
else if (keSG_微力砂轮架子定位 == testMode)
|
||||
Weili_grindingDiscRackPosition_test();
|
||||
else if (keSG_山东本事轮胎定位 == testMode)
|
||||
Benshi_tirePosition_test();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user