workpieceHolePositioning version 1.5.5 : 山东本事机电轮胎定位。（1）添加了轮胎内径大小的输出， （2）按高度排序。最上面的目标排在第一个。
This commit is contained in:
parent
693471b311
commit
14ba0f0939
@ -20,6 +20,9 @@ SG_APISHARED_EXPORT float vec3_computeVecAngle(const SVzNL3DPoint& a, const SVzN
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint wd_ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9]);
|
||||
//二维点旋转
|
||||
SG_APISHARED_EXPORT SVzNL2DPointD wd_pt2dRotate(SVzNL2DPointD pt2D_in, double rotateAngle);
|
||||
//计算总体均值和总体标准差
|
||||
SG_APISHARED_EXPORT void CalcStats(const std::vector<double>& data, SSG_meanVar& result);
|
||||
|
||||
/**
|
||||
* @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
|
||||
* （v 必须在平面内，自动使用简化版罗德里格斯）
|
||||
|
||||
@ -463,6 +463,8 @@ typedef struct
|
||||
bool end_zRising;//终点z上跳标志
|
||||
}SSG_RUN_EX;
|
||||
|
||||
|
||||
//统计信息：均值和标准差（开根号）
|
||||
typedef struct
|
||||
{
|
||||
double mean;
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <corecrt_math_defines.h>
|
||||
#endif // __WIN32
|
||||
|
||||
#include <numeric>
|
||||
#include <cmath>
|
||||
#include <unordered_map>
|
||||
#include <Eigen/dense>
|
||||
@ -89,6 +90,25 @@ SVzNL2DPointD wd_pt2dRotate(SVzNL2DPointD pt2D_in, double rotateAngle)
|
||||
return result;
|
||||
}
|
||||
|
||||
//计算总体均值和总体标准差
|
||||
void CalcStats(const std::vector<double>& data, SSG_meanVar& result)
|
||||
{
|
||||
size_t n = (int)data.size();
|
||||
if (n == 0)
|
||||
return;
|
||||
|
||||
double sum = std::accumulate(data.begin(), data.end(), 0.0);
|
||||
double mean = sum / n;
|
||||
|
||||
double sq = 0.0;
|
||||
for (double v : data) sq += (v - mean) * (v - mean);
|
||||
sq = sq / n;
|
||||
|
||||
result.mean = mean;
|
||||
result.var = sqrt(sq);
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
|
||||
* （v 必须在平面内，自动使用简化版罗德里格斯）
|
||||
|
||||
@ -25,7 +25,8 @@
|
||||
//version 1.5.2 : 修正工件判断中的一个逻辑错误。
|
||||
//version 1.5.3 : 添加郑州微力砂轮盘和轮盘架定位API。
|
||||
//version 1.5.4 : 添加山东本事机电轮胎定位API。
|
||||
std::string m_strVersion = "HolePostion 1.5.4";
|
||||
//version 1.5.5 : 山东本事机电轮胎定位。（1）添加了轮胎内径大小的输出， （2）按高度排序。最上面的目标排在第一个。
|
||||
std::string m_strVersion = "HolePostion 1.5.5";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -2698,12 +2699,16 @@ void _getFlagIntervals_h(
|
||||
segs.push_back(a_run);
|
||||
}
|
||||
|
||||
bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b)
|
||||
{
|
||||
return a.center.z < b.center.z;
|
||||
}
|
||||
//山东本事机电轮胎中心定位
|
||||
void sx_getTireHolePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_tireParam tireParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_tireParam tireParam,
|
||||
std::vector<WD_HolePositionInfo>& tirePositions,
|
||||
int* errCode)
|
||||
{
|
||||
@ -3093,10 +3098,13 @@ void _getFlagIntervals_h(
|
||||
WD_HolePositionInfo a_holeInfo;
|
||||
a_holeInfo.center = { center.x, center.y, meanZ };
|
||||
a_holeInfo.normDir = { 0.0, 0.0, 1.0 };
|
||||
a_holeInfo.holeR = radius;
|
||||
tirePositions.push_back(a_holeInfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
//按高度排序
|
||||
std::sort(tirePositions.begin(), tirePositions.end(), _compareByTireZValue);
|
||||
|
||||
if (tirePositions.size() == 0)
|
||||
{
|
||||
@ -3113,3 +3121,439 @@ void _getFlagIntervals_h(
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
//模板检查
|
||||
void _templateChecking(
|
||||
std::vector<std::vector<double>>& quantiData,
|
||||
int ref_x, int ref_y,
|
||||
std::vector<std::vector<int>>& templateMask,
|
||||
std::vector<double>& circleZ,
|
||||
std::vector<double>& centerZ)
|
||||
{
|
||||
int templateX = (int)templateMask.size();
|
||||
int templateY = (int)templateMask[0].size();
|
||||
for (int x = 0; x < templateX; x++)
|
||||
{
|
||||
for (int y = 0; y < templateY; y++)
|
||||
{
|
||||
if (templateMask[x][y] == 0)
|
||||
continue;
|
||||
int maskX = x + ref_x;
|
||||
int maskY = y + ref_y;
|
||||
if (quantiData[maskX][maskY] < 1e-4)
|
||||
continue;
|
||||
|
||||
if (templateMask[x][y] == 1)
|
||||
circleZ.push_back(quantiData[maskX][maskY]);
|
||||
else if (templateMask[x][y] == 2)
|
||||
centerZ.push_back(quantiData[maskX][maskY]);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
//宁波海瑞马定子芯中心定位
|
||||
void sx_getRotorCorePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_rotorAppParam rotorParam,
|
||||
std::vector<WD_HolePositionInfo>& rotorPositions,
|
||||
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 - rotorParam.removeGroundHOffset;
|
||||
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、计算中间坐标
|
||||
//内部参数
|
||||
double maxDistTh = 5.0;
|
||||
double minSegLen = 2.0;
|
||||
SSG_cornerParam removeVertialPara = cornerPara;
|
||||
removeVertialPara.scale = 5.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_perSeg_dirAngleMethod(
|
||||
scanLines[line],
|
||||
line,
|
||||
maxDistTh,
|
||||
minSegLen,
|
||||
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_perSeg_dirAngleMethod(
|
||||
scanLines[line],
|
||||
line,
|
||||
maxDistTh,
|
||||
minSegLen,
|
||||
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;
|
||||
int minSegSize = 1;
|
||||
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 <= minSegSize)
|
||||
{
|
||||
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 <= minSegSize)
|
||||
{
|
||||
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)
|
||||
{
|
||||
scanLinesInput[line][j].pt3D.z = 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 minObj_L = rotorParam.basket_L * 0.8;
|
||||
double maxObj_L = rotorParam.basket_L * 1.25;
|
||||
double minObj_W = rotorParam.basket_W * 0.8;
|
||||
double maxObj_W = rotorParam.basket_W * 1.25;
|
||||
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;
|
||||
double len = w < h ? h : w;
|
||||
double width = w > h ? h : w;
|
||||
if ((len > minObj_L) && (width > minObj_W) && (len < maxObj_L) && (width < maxObj_W))
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
//最前面的目标为筐子
|
||||
int basketClusterIdx = validObjIndice[0];
|
||||
for (int i = 1; i < (int)validObjIndice.size(); i++)
|
||||
{
|
||||
int idx = validObjIndice[i];
|
||||
if (objMeanZ[basketClusterIdx] > objMeanZ[idx])
|
||||
basketClusterIdx = idx;
|
||||
}
|
||||
std::vector< SVzNL3DPosition>& basketCluster = objClusters[basketClusterIdx];
|
||||
//提取筐子的四个角点，用于确定目标所在的边
|
||||
|
||||
//生成投影图，方便使用模板
|
||||
const double quantiScale = 1.0; //
|
||||
SSG_ROIRectD& basketROI = objROIs[basketClusterIdx];
|
||||
int quantiW = (basketROI.right - basketROI.left) / quantiScale + 1;
|
||||
if (quantiW % 2 == 1)
|
||||
quantiW += 1;
|
||||
int quantiH = (basketROI.bottom - basketROI.top) / quantiScale + 1;
|
||||
if (quantiH % 2 == 1)
|
||||
quantiH += 1;
|
||||
|
||||
std::vector<std::vector<double>> quantiData;
|
||||
quantiData.resize(quantiW);
|
||||
for (int i = 0; i < quantiW; i++)
|
||||
{
|
||||
quantiData[i].resize(quantiH);
|
||||
std::fill(quantiData[i].begin(), quantiData[i].end(), -1);
|
||||
}
|
||||
//量化
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
if ((scanLines[line][j].pt3D.z > 1e-4) &&
|
||||
(scanLines[line][j].pt3D.x >= basketROI.left) && (scanLines[line][j].pt3D.x < basketROI.right) &&
|
||||
(scanLines[line][j].pt3D.y >= basketROI.top) && (scanLines[line][j].pt3D.y < basketROI.bottom))
|
||||
{
|
||||
int quantiX = (scanLines[line][j].pt3D.x - basketROI.left) / quantiScale;
|
||||
int quantiY = (scanLines[line][j].pt3D.y - basketROI.top) / quantiScale;
|
||||
quantiData[quantiX][quantiY] = scanLines[line][j].pt3D.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
//生成模板
|
||||
double template_minR = rotorParam.rotorDiameter - 2.0;
|
||||
double template_maxR = rotorParam.rotorDiameter + 2.0;
|
||||
double template_centerR = 10.0;
|
||||
int template_pixR = (int)(template_maxR / quantiScale + 1);
|
||||
int template_W = template_pixR * 2 + 1;
|
||||
|
||||
double template_cx = (double)template_pixR * quantiScale;
|
||||
double template_cy = template_cx;
|
||||
std::vector<std::vector<int>> templateMask;
|
||||
templateMask.resize(template_W);
|
||||
for (int i = 0; i < template_W; i++)
|
||||
templateMask[i].resize(template_W);
|
||||
for (int i = 0; i < template_W; i++)
|
||||
{
|
||||
for (int j = 0; j < template_W; j++)
|
||||
{
|
||||
double x = (double)i * quantiScale;
|
||||
double y = (double)j * quantiScale;
|
||||
double dist = sqrt(pow(x - template_cx, 2) + pow(y - template_cy, 2));
|
||||
if ((dist >= template_minR) && (dist <= template_maxR))
|
||||
templateMask[i][j] = 1;
|
||||
else if (dist <= template_centerR)
|
||||
templateMask[i][j] = 2;
|
||||
}
|
||||
}
|
||||
if ((template_W >= quantiW) || (template_W >= quantiH))
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return;
|
||||
}
|
||||
|
||||
//搜索目标
|
||||
std::vector<std::vector<SSG_meanVar>> statsCircle;
|
||||
std::vector<std::vector<SSG_meanVar>> statsCenter;
|
||||
statsCircle.resize(quantiW);
|
||||
statsCenter.resize(quantiW);
|
||||
for (int i = 0; i < quantiW; i++)
|
||||
{
|
||||
statsCircle[i].resize(quantiH);
|
||||
statsCenter[i].resize(quantiH);
|
||||
}
|
||||
|
||||
int searchStart_X = template_pixR;
|
||||
int searchEnd_X = quantiW - 1 - template_pixR;
|
||||
int searchStart_Y = template_pixR;
|
||||
int searchEnd_Y = quantiH - 1 - template_pixR;
|
||||
for (int x = searchStart_X; x < searchEnd_X; x++)
|
||||
{
|
||||
for (int y = searchStart_Y; y < searchEnd_Y; y++)
|
||||
{
|
||||
std::vector<double> circleZ;
|
||||
std::vector<double> centerZ;
|
||||
//模板检查
|
||||
_templateChecking(
|
||||
quantiData,
|
||||
x - template_pixR, y - template_pixR,
|
||||
templateMask,
|
||||
circleZ,
|
||||
centerZ);
|
||||
//计算均值和方差
|
||||
SSG_meanVar satasInfo_circle = { 0.0, 0.0 };
|
||||
CalcStats(circleZ, satasInfo_circle);
|
||||
statsCircle[x][y] = satasInfo_circle;
|
||||
|
||||
SSG_meanVar satasInfo_center = { 0.0, 0.0 };
|
||||
CalcStats(centerZ, satasInfo_center);
|
||||
statsCenter[x][y] = satasInfo_center;
|
||||
}
|
||||
}
|
||||
|
||||
//搜索方差极小值点, circle和center的方差累加后的极小值
|
||||
//两步搜索法：先搜索5x5的，再搜索模块大小
|
||||
std::vector< SSG_2DValueI> varPeaks_5x5;
|
||||
for (int i = 0; i < template_W; i++)
|
||||
{
|
||||
for (int j = 0; j < template_W; j++)
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//旋转回原坐标系
|
||||
|
||||
for (int i = 0; i < (int)rotorPositions.size(); i++)
|
||||
{
|
||||
rotorPositions[i].center = _ptRotate(rotorPositions[i].center, groundCalibPara.invRMatrix);
|
||||
rotorPositions[i].normDir = _ptRotate(rotorPositions[i].normDir, groundCalibPara.invRMatrix);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
@ -37,8 +37,17 @@ typedef struct
|
||||
{
|
||||
SVzNL3DPoint center;
|
||||
SVzNL3DPoint normDir; //方向向量（归一化）
|
||||
double holeR;
|
||||
}WD_HolePositionInfo;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double rotorDiameter;
|
||||
double removeGroundHOffset; //去除时的高度偏置，用于设置架子高度
|
||||
double basket_L; //零件筐-长
|
||||
double basket_W; //零件筐-宽
|
||||
}WD_rotorAppParam;
|
||||
|
||||
//读版本号
|
||||
SG_APISHARED_EXPORT const char* wd_workpieceHolePositioningVersion(void);
|
||||
|
||||
@ -93,4 +102,13 @@ SG_APISHARED_EXPORT void sx_getTireHolePose(
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_tireParam tireParam,
|
||||
std::vector<WD_HolePositionInfo>& tirePositions,
|
||||
int* errCode);
|
||||
|
||||
//宁波海瑞马定子芯中心定位
|
||||
SG_APISHARED_EXPORT void sx_getRotorCorePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_rotorAppParam rotorParam,
|
||||
std::vector<WD_HolePositionInfo>& rotorPositions,
|
||||
int* errCode);
|
||||
@ -172,7 +172,7 @@ void _outputHoleInfo(char* fileName, std::vector< WD_HolePositionInfo>& holePosi
|
||||
int number = (int)holePositioning.size();
|
||||
for (int i = 0; i < number; i++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "孔_%d", i + 1);
|
||||
sprintf_s(dataStr, 250, "孔_%d: R=%g", i + 1, holePositioning[i].holeR);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z);
|
||||
sw << dataStr << std::endl;
|
||||
@ -673,7 +673,10 @@ void _outputRGBDResult_HoleInfo(
|
||||
size = 10;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
rgb = {255, 0, 0};
|
||||
if (i == 0)
|
||||
rgb = { 255,255,0 };
|
||||
else
|
||||
rgb = {0, 0, 255};
|
||||
float x = (float)objects[i].pt3D.x;
|
||||
float y = (float)objects[i].pt3D.y;
|
||||
float z = (float)objects[i].pt3D.z;
|
||||
@ -1436,6 +1439,7 @@ void Benshi_tirePosition_test(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSG_拓普发孔定位 = 0,
|
||||
@ -1443,8 +1447,159 @@ typedef enum
|
||||
keSG_微力砂轮盘定位,
|
||||
keSG_微力砂轮架子定位,
|
||||
keSG_山东本事轮胎定位,
|
||||
keSG_宁波海瑞马转子芯定位,
|
||||
} ESG_testMode;
|
||||
|
||||
//山东本事机电轮胎定位
|
||||
#define HAIRUIMA_COMPUTE_CALIB_PARA 0
|
||||
#define HAIRUIMA_HOLE_POSITION 1
|
||||
|
||||
#define HAIRUIMA_TIRE_TEST_GROUP 1
|
||||
void HaiRuiMa_rotorCorePosition_test(void)
|
||||
{
|
||||
const char* dataPath[BENSHI_TIRE_TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/数据/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = {
|
||||
{1,83},
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if HAIRUIMA_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_Jl26C477.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);
|
||||
}
|
||||
printf("%s: processing...\n", _scan_file);
|
||||
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 HAIRUIMA_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_Jl26C477.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_rotorAppParam rotorParam;
|
||||
rotorParam.rotorDiameter = 76;
|
||||
rotorParam.removeGroundHOffset = 100.0; //去除时的高度偏置，用于设置架子高度
|
||||
rotorParam.basket_L = 770;
|
||||
rotorParam.basket_W = 580;
|
||||
int errCode = 0;
|
||||
std::vector<WD_HolePositionInfo> rotorPositions;
|
||||
sx_getRotorCorePose(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
groundCalibPara,
|
||||
rotorParam,
|
||||
rotorPositions,
|
||||
&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, rotorPositions, dirLen);
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputHoleInfo(_scan_file, rotorPositions);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
//ESG_testMode testMode = keSG_拓普发孔定位;
|
||||
@ -1452,6 +1607,7 @@ int main()
|
||||
//ESG_testMode testMode = keSG_微力砂轮盘定位;
|
||||
//ESG_testMode testMode = keSG_微力砂轮架子定位;
|
||||
ESG_testMode testMode = keSG_山东本事轮胎定位;
|
||||
//ESG_testMode testMode = keSG_宁波海瑞马转子芯定位;
|
||||
|
||||
if (keSG_拓普发孔定位 == testMode)
|
||||
TuoPuFa_holePosition_test();
|
||||
@ -1463,4 +1619,6 @@ int main()
|
||||
Weili_grindingDiscRackPosition_test();
|
||||
else if (keSG_山东本事轮胎定位 == testMode)
|
||||
Benshi_tirePosition_test();
|
||||
else if (keSG_宁波海瑞马转子芯定位 == testMode)
|
||||
HaiRuiMa_rotorCorePosition_test();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user