workpieceHolePositioning version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法，使用圆扫描提取轮廓点。
This commit is contained in:
parent
14ba0f0939
commit
4001bb5c55
@ -319,9 +319,8 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod(
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getXYVertialFeature_perSeg_dirAngleMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const double minSegSize,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<int>& xyVerticalFlags
|
||||
);
|
||||
@ -338,6 +337,16 @@ SG_APISHARED_EXPORT void wd_getXYHorizontalFeature_dirAngleMethod(
|
||||
std::vector<int>& xyHorizontalFlags
|
||||
);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_getRimConvexFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
int lineIdx,
|
||||
const SWDRimEdgeParam rimPara_1,
|
||||
const SWDRimEdgeParam rimPara_2,
|
||||
const SVzNLRangeD rimWidth,
|
||||
std::vector<SSG_RUN>& rimFeatures
|
||||
);
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
@ -723,6 +732,14 @@ SG_APISHARED_EXPORT double fitCircleByLeastSquare_3(
|
||||
const std::vector<SVzNL2DPointD>& pointArray,
|
||||
SVzNL2DPointD& center,
|
||||
double& radius);
|
||||
// 加权迭代最小二乘圆拟合（Huber权重或Tukey权重）
|
||||
SG_APISHARED_EXPORT double fitCircleRobust(
|
||||
const std::vector<SVzNL3DPosition>& points,
|
||||
SVzNL3DPoint& robustCenter,
|
||||
double& robustRadius,
|
||||
double outlierThreshold = 2.5, // 标准差倍数
|
||||
int maxIter = 20,
|
||||
double minPointsRatio = 0.5);
|
||||
|
||||
//抛物线最小二乘拟合 y=ax^2 + bx + c
|
||||
SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
|
||||
@ -819,7 +836,7 @@ SG_APISHARED_EXPORT void SG_TwoPassLabel(
|
||||
//计算面参数: z = Ax + By + C
|
||||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
SG_APISHARED_EXPORT void vzCaculateLaserPlane(
|
||||
std::vector<cv::Point3d> Points3ds,
|
||||
std::vector<cv::Point3d>& Points3ds,
|
||||
std::vector<double>& res);
|
||||
|
||||
SG_APISHARED_EXPORT Plane robustFitPlane(
|
||||
|
||||
@ -41,6 +41,17 @@ typedef struct
|
||||
double z;
|
||||
}SWD3DPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double cx, cy, r;
|
||||
}SWD_Circle;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double rimEdgeAngleTh;
|
||||
double rimCornerTh;
|
||||
}SWDRimEdgeParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int lineIdx;
|
||||
|
||||
@ -5946,9 +5946,8 @@ void wd_getXYVertialFeature_dirAngleMethod(
|
||||
/// </summary>
|
||||
void wd_getXYVertialFeature_perSeg_dirAngleMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const double minSegSize,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<int>& xyVerticalFlags
|
||||
)
|
||||
@ -5960,14 +5959,7 @@ void wd_getXYVertialFeature_perSeg_dirAngleMethod(
|
||||
xyVerticalFlags.resize(lineData.size());
|
||||
std::fill(xyVerticalFlags.begin(), xyVerticalFlags.end(), 0);
|
||||
|
||||
//根据z连续性分段
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_lineDataSegment_dist_2(
|
||||
lineData,
|
||||
segs,
|
||||
maxDistTh,
|
||||
minSegSize
|
||||
);
|
||||
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles;
|
||||
_computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles);
|
||||
@ -6112,6 +6104,147 @@ void wd_getXYHorizontalFeature_dirAngleMethod(
|
||||
return;
|
||||
}
|
||||
|
||||
//提取边沿特征
|
||||
void wd_getRimConvexFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
int lineIdx,
|
||||
const SWDRimEdgeParam rimPara_1,
|
||||
const SWDRimEdgeParam rimPara_2,
|
||||
const SVzNLRangeD rimWidth,
|
||||
std::vector<SSG_RUN>& rimFeatures
|
||||
)
|
||||
{
|
||||
//双尺度检测，分别用于缓变和剧变（对应step为2和1）
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles_1;
|
||||
ptDirAngles_1.resize(lineData.size());
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles_2;
|
||||
ptDirAngles_2.resize(lineData.size());
|
||||
for (int i = 0; i < (int)lineData.size(); i++)
|
||||
{
|
||||
ptDirAngles_1[i].pntIdx = -1;
|
||||
ptDirAngles_1[i].type = -1;
|
||||
ptDirAngles_1[i].forwardPntIdx = 0;
|
||||
ptDirAngles_1[i].backwardPntIdx = 0;
|
||||
ptDirAngles_1[i].forwardAngle = 0;
|
||||
ptDirAngles_1[i].backwardAngle = 0;
|
||||
ptDirAngles_1[i].corner = 0;
|
||||
ptDirAngles_1[i].forwardDiffZ = 0;
|
||||
ptDirAngles_1[i].backwardDiffZ = 0;
|
||||
|
||||
ptDirAngles_2[i].pntIdx = -1;
|
||||
ptDirAngles_2[i].type = -1;
|
||||
ptDirAngles_2[i].forwardPntIdx = 0;
|
||||
ptDirAngles_2[i].backwardPntIdx = 0;
|
||||
ptDirAngles_2[i].forwardAngle = 0;
|
||||
ptDirAngles_2[i].backwardAngle = 0;
|
||||
ptDirAngles_2[i].corner = 0;
|
||||
ptDirAngles_2[i].forwardDiffZ = 0;
|
||||
ptDirAngles_2[i].backwardDiffZ = 0;
|
||||
}
|
||||
//逐段进行
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
std::vector<int> ptIndice;
|
||||
for (int i = vPtIdxStart; i < vPtIdxEnd; i++)
|
||||
{
|
||||
ptDirAngles_1[i].type = segIdx;
|
||||
ptDirAngles_2[i].type = segIdx;
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
ptIndice.push_back(i);
|
||||
}
|
||||
|
||||
int vldNum = (int)ptIndice.size();
|
||||
for (int idx = 1; idx < vldNum-1; idx++)
|
||||
{
|
||||
int i = ptIndice[idx];
|
||||
int pre_i = ptIndice[idx-1];
|
||||
int post_i = ptIndice[idx+1];
|
||||
|
||||
//计算拐角
|
||||
double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y);
|
||||
double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||||
ptDirAngles_1[i].pntIdx = i;
|
||||
ptDirAngles_1[i].forwardPntIdx = post_i;
|
||||
ptDirAngles_1[i].backwardPntIdx = pre_i;
|
||||
ptDirAngles_1[i].forwardAngle = forwardAngle;
|
||||
ptDirAngles_1[i].backwardAngle = backwardAngle;
|
||||
ptDirAngles_1[i].corner = (forwardAngle - backwardAngle);
|
||||
ptDirAngles_1[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z;
|
||||
ptDirAngles_1[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z;
|
||||
}
|
||||
|
||||
for (int idx = 2; idx < vldNum - 2; idx++)
|
||||
{
|
||||
int i = ptIndice[idx];
|
||||
int pre_i = ptIndice[idx - 2];
|
||||
int post_i = ptIndice[idx + 2];
|
||||
|
||||
//计算拐角
|
||||
double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y);
|
||||
double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||||
ptDirAngles_2[i].pntIdx = i;
|
||||
ptDirAngles_2[i].forwardPntIdx = post_i;
|
||||
ptDirAngles_2[i].backwardPntIdx = pre_i;
|
||||
ptDirAngles_2[i].forwardAngle = forwardAngle;
|
||||
ptDirAngles_2[i].backwardAngle = backwardAngle;
|
||||
ptDirAngles_2[i].corner = (forwardAngle - backwardAngle);
|
||||
ptDirAngles_2[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z;
|
||||
ptDirAngles_2[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z;
|
||||
}
|
||||
|
||||
//分段搜索拐点
|
||||
int pre_start_pos = -1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
//搜索开始
|
||||
if ((i == vPtIdxStart) ||
|
||||
((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].backwardAngle < -rimPara_1.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_1.rimCornerTh)) ||
|
||||
((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].backwardAngle < -rimPara_2.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_2.rimCornerTh)) ||
|
||||
((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].backwardAngle < -rimPara_1.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_1.rimCornerTh)) ||
|
||||
((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].backwardAngle < -rimPara_2.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_2.rimCornerTh)))
|
||||
pre_start_pos = i;
|
||||
|
||||
//搜索结束
|
||||
if ((i == vPtIdxEnd) ||
|
||||
((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].forwardAngle > rimPara_1.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_1.rimCornerTh)) ||
|
||||
((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].forwardAngle > rimPara_2.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_2.rimCornerTh)) ||
|
||||
((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].forwardAngle > rimPara_1.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_1.rimCornerTh)) ||
|
||||
((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].forwardAngle > rimPara_2.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_2.rimCornerTh)))
|
||||
{
|
||||
if (pre_start_pos >= 0)
|
||||
{
|
||||
if ((ptDirAngles_1[pre_start_pos].pntIdx >= 0) || (ptDirAngles_1[i].pntIdx >= 0) ||
|
||||
(ptDirAngles_2[pre_start_pos].pntIdx >= 0) || (ptDirAngles_2[i].pntIdx >= 0))
|
||||
{
|
||||
double width = sqrt(pow(lineData[pre_start_pos].pt3D.x - lineData[i].pt3D.x, 2) +
|
||||
pow(lineData[pre_start_pos].pt3D.y - lineData[i].pt3D.y, 2) +
|
||||
pow(lineData[pre_start_pos].pt3D.z - lineData[i].pt3D.z, 2));
|
||||
if ((width >= rimWidth.min) && (width <= rimWidth.max))
|
||||
{
|
||||
//一个rim
|
||||
SSG_RUN a_rim;
|
||||
a_rim.start = pre_start_pos;
|
||||
a_rim.len = i - pre_start_pos + 1;
|
||||
a_rim.curveLen = width;
|
||||
a_rim.value = segIdx;
|
||||
rimFeatures.push_back(a_rim);
|
||||
pre_start_pos = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
|
||||
@ -319,6 +319,122 @@ double fitCircleByLeastSquare_3(
|
||||
return err;
|
||||
}
|
||||
|
||||
// 计算点到圆的几何距离（带符号）
|
||||
double geometricDistance(const SVzNL3DPosition& p, const SVzNL3DPoint& center, const double R) {
|
||||
return std::sqrt((p.pt3D.x - center.x) * (p.pt3D.x - center.x) + (p.pt3D.y - center.y) * (p.pt3D.y - center.y)) - R;
|
||||
}
|
||||
|
||||
// ========== 计算几何误差（点到圆的距离） ==========
|
||||
double calcGeometricError(const SVzNL3DPosition& p, const SWD_Circle& c) {
|
||||
double dx = p.pt3D.x - c.cx;
|
||||
double dy = p.pt3D.y - c.cy;
|
||||
return fabs(sqrt(dx * dx + dy * dy) - c.r);
|
||||
}
|
||||
|
||||
// ========== 计算所有点的误差统计 ==========
|
||||
void calcErrorStats(const std::vector<SVzNL3DPosition>& points, const SWD_Circle& c,
|
||||
std::vector<double>& errors, double& mean, double& stddev)
|
||||
{
|
||||
int n = points.size();
|
||||
errors.resize(n);
|
||||
mean = 0;
|
||||
for (int i = 0; i < n; ++i) {
|
||||
errors[i] = calcGeometricError(points[i], c);
|
||||
mean += errors[i];
|
||||
}
|
||||
mean /= n;
|
||||
|
||||
stddev = 0;
|
||||
for (int i = 0; i < n; ++i) {
|
||||
double d = errors[i] - mean;
|
||||
stddev += d * d;
|
||||
}
|
||||
stddev = sqrt(stddev / n);
|
||||
}
|
||||
|
||||
// ========== 主函数：迭代剔除离群点 + 最小二乘拟合 ==========
|
||||
double fitCircleRobust(
|
||||
const std::vector<SVzNL3DPosition>& points,
|
||||
SVzNL3DPoint& robustCenter,
|
||||
double& robustRadius,
|
||||
double outlierThreshold, // 标准差倍数
|
||||
int maxIter,
|
||||
double minPointsRatio)
|
||||
{
|
||||
if (points.size() < 3) return 0;
|
||||
|
||||
// 用所有点初始化
|
||||
std::vector<SVzNL3DPosition> workingPoints = points;
|
||||
SVzNL3DPoint c0;
|
||||
double r0;
|
||||
double errVal = fitCircleByLeastSquare_2(workingPoints, c0, r0);
|
||||
SWD_Circle bestCircle = { c0.x, c0.y, r0 };
|
||||
|
||||
double bestMeanError = std::numeric_limits<double>::max();
|
||||
std::vector<SVzNL3DPosition> bestPoints = workingPoints;
|
||||
|
||||
for (int iter = 0; iter < maxIter; ++iter) {
|
||||
// 计算当前拟合圆的误差统计
|
||||
std::vector<double> errors;
|
||||
double mean, stddev;
|
||||
calcErrorStats(workingPoints, bestCircle, errors, mean, stddev);
|
||||
|
||||
// 检查收敛：如果平均误差不再显著改善
|
||||
if (mean >= bestMeanError * 0.99 && iter > 0)
|
||||
break;
|
||||
bestMeanError = mean;
|
||||
|
||||
// 如果标准差太小，说明已经收敛
|
||||
if (stddev < 1e-8) break;
|
||||
|
||||
// 剔除离群点：误差 > mean + outlierThreshold * stddev
|
||||
double threshold = mean + outlierThreshold * stddev;
|
||||
std::vector<SVzNL3DPosition> filteredPoints;
|
||||
filteredPoints.reserve(workingPoints.size());
|
||||
|
||||
for (size_t i = 0; i < workingPoints.size(); ++i) {
|
||||
if (errors[i] <= threshold) {
|
||||
filteredPoints.push_back(workingPoints[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// 如果剩余点太少，停止剔除
|
||||
if (filteredPoints.size() < points.size() * minPointsRatio ||
|
||||
filteredPoints.size() < 3) {
|
||||
break;
|
||||
}
|
||||
|
||||
// 用剩余点重新拟合
|
||||
SVzNL3DPoint center;
|
||||
double radius;
|
||||
errVal = fitCircleByLeastSquare_2(filteredPoints, center, radius);
|
||||
SWD_Circle newCircle = { center.x, center.y, radius };
|
||||
|
||||
// 检查新圆是否更优（用平均误差衡量）
|
||||
double newMean = 0;
|
||||
for (const auto& p : filteredPoints) {
|
||||
newMean += calcGeometricError(p, newCircle);
|
||||
}
|
||||
newMean /= filteredPoints.size();
|
||||
|
||||
if (newMean < bestMeanError) {
|
||||
bestCircle = newCircle;
|
||||
workingPoints = filteredPoints;
|
||||
bestPoints = filteredPoints;
|
||||
}
|
||||
else {
|
||||
// 新圆不如旧圆好，停止迭代
|
||||
break;
|
||||
}
|
||||
}
|
||||
robustCenter.x = bestCircle.cx;
|
||||
robustCenter.y = bestCircle.cy;
|
||||
robustCenter.z = 0;
|
||||
robustRadius = bestCircle.r;
|
||||
return bestMeanError;
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
|
||||
double& a, double& b, double& c,
|
||||
@ -464,7 +580,7 @@ bool leastSquareParabolaFitEigen(
|
||||
|
||||
//计算面参数: z = Ax + By + C
|
||||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
void vzCaculateLaserPlane(std::vector<cv::Point3d> Points3ds, std::vector<double>& res)
|
||||
void vzCaculateLaserPlane(std::vector<cv::Point3d>& Points3ds, std::vector<double>& res)
|
||||
{
|
||||
//最小二乘法拟合平面
|
||||
//获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -33,6 +33,13 @@ typedef struct
|
||||
double thickness; //轮胎宽度
|
||||
}WD_tireParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double len; //工件长
|
||||
double width; //工件宽度
|
||||
double thickness;
|
||||
}WD_JiuruiWorkpieceParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint center;
|
||||
@ -43,6 +50,7 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
double rotorDiameter;
|
||||
double rotorH;
|
||||
double removeGroundHOffset; //去除时的高度偏置，用于设置架子高度
|
||||
double basket_L; //零件筐-长
|
||||
double basket_W; //零件筐-宽
|
||||
@ -112,3 +120,12 @@ SG_APISHARED_EXPORT void sx_getRotorCorePose(
|
||||
const WD_rotorAppParam rotorParam,
|
||||
std::vector<WD_HolePositionInfo>& rotorPositions,
|
||||
int* errCode);
|
||||
|
||||
//北京玖瑞工件定位
|
||||
SG_APISHARED_EXPORT void Jiurui_getWorkpiecePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const WD_JiuruiWorkpieceParam workpieceParam,
|
||||
std::vector<SSG_pointPose>& tirePositions,
|
||||
int* errCode);
|
||||
@ -635,11 +635,22 @@ void _outputRGBDResult_HoleInfo(
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
std::vector< SVzNL3DPosition> vlPts;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
if (scanLines[line][i].pt3D.z > 1e-4)
|
||||
vlPts.push_back(scanLines[line][i]);
|
||||
}
|
||||
|
||||
linePtNum = (int)vlPts.size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
SVzNL3DPosition* pt3D = &vlPts[i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int flag = pt3D->nPointIdx & 0xffff;
|
||||
@ -1306,7 +1317,7 @@ void Benshi_tirePosition_test(void)
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = {
|
||||
{1,19},
|
||||
{1,34},
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
@ -1394,7 +1405,7 @@ void Benshi_tirePosition_test(void)
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =17;
|
||||
//fidx =18;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
@ -1439,18 +1450,7 @@ void Benshi_tirePosition_test(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSG_拓普发孔定位 = 0,
|
||||
keSG_华航孔定位,
|
||||
keSG_微力砂轮盘定位,
|
||||
keSG_微力砂轮架子定位,
|
||||
keSG_山东本事轮胎定位,
|
||||
keSG_宁波海瑞马转子芯定位,
|
||||
} ESG_testMode;
|
||||
|
||||
//山东本事机电轮胎定位
|
||||
//海瑞马
|
||||
#define HAIRUIMA_COMPUTE_CALIB_PARA 0
|
||||
#define HAIRUIMA_HOLE_POSITION 1
|
||||
|
||||
@ -1572,7 +1572,8 @@ void HaiRuiMa_rotorCorePosition_test(void)
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
WD_rotorAppParam rotorParam;
|
||||
rotorParam.rotorDiameter = 76;
|
||||
rotorParam.rotorDiameter = 72;
|
||||
rotorParam.rotorH = 40.0;
|
||||
rotorParam.removeGroundHOffset = 100.0; //去除时的高度偏置，用于设置架子高度
|
||||
rotorParam.basket_L = 770;
|
||||
rotorParam.basket_W = 580;
|
||||
@ -1599,7 +1600,166 @@ void HaiRuiMa_rotorCorePosition_test(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
#define JIURUI_COMPUTE_CALIB_PARA 0
|
||||
#define JIURUI_HOLE_POSITION 1
|
||||
|
||||
#define JIURUI_TIRE_TEST_GROUP 2
|
||||
void JiuRui_workpiecePosition_test(void)
|
||||
{
|
||||
const char* dataPath[JIURUI_TIRE_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/北京玖瑞/工件识别/工件数据/1367mm/",//0
|
||||
"F:/ShangGu/项目/冠钦项目/北京玖瑞/工件识别/工件数据/距离工件1183mm/", //1
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[JIURUI_TIRE_TEST_GROUP] = {
|
||||
{1,8},{1,6}
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if JIURUI_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 JIURUI_HOLE_POSITION
|
||||
for (int grp = 0; grp <= 1; 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_Hi229229.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_JiuruiWorkpieceParam workpieceParam;
|
||||
workpieceParam.len = 320;
|
||||
workpieceParam.width = 140;
|
||||
workpieceParam.thickness = 48;
|
||||
|
||||
int errCode = 0;
|
||||
std::vector<SSG_pointPose> workpiecePositions;
|
||||
Jiurui_getWorkpiecePose(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
groundCalibPara,
|
||||
workpieceParam,
|
||||
workpiecePositions,
|
||||
&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);
|
||||
SSG_pointPose a_pose;
|
||||
memset(&a_pose, 0, sizeof(SSG_pointPose));
|
||||
if (workpiecePositions.size() > 0)
|
||||
a_pose = workpiecePositions[0];
|
||||
_outputRGBDResult_RGBD_2(_scan_file, scanLines, a_pose);
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputHoleInfo_3(_scan_file, a_pose);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSG_拓普发孔定位 = 0,
|
||||
keSG_华航孔定位,
|
||||
keSG_微力砂轮盘定位,
|
||||
keSG_微力砂轮架子定位,
|
||||
keSG_山东本事轮胎定位,
|
||||
keSG_宁波海瑞马转子芯定位,
|
||||
keSG_北京玖瑞工件定位,
|
||||
} ESG_testMode;
|
||||
int main()
|
||||
{
|
||||
//ESG_testMode testMode = keSG_拓普发孔定位;
|
||||
@ -1608,6 +1768,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();
|
||||
@ -1621,4 +1782,6 @@ int main()
|
||||
Benshi_tirePosition_test();
|
||||
else if (keSG_宁波海瑞马转子芯定位 == testMode)
|
||||
HaiRuiMa_rotorCorePosition_test();
|
||||
else if (keSG_北京玖瑞工件定位 == testMode)
|
||||
JiuRui_workpiecePosition_test();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user