rodAndBarDetection version 1.2.3 :
根据定向盘轮廓的直线段确定向右的向量
This commit is contained in:
parent
96938e61b5
commit
94aafeff97
@ -2122,6 +2122,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
featurePoints,
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
0, //2D distance
|
||||
objClusters //result
|
||||
);
|
||||
//取最大的聚类作为轮廓
|
||||
|
||||
@ -4,6 +4,8 @@
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
//ÏòÁ¿²æ³Ë
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
|
||||
|
||||
//滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
|
||||
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier(
|
||||
|
||||
@ -11,6 +11,15 @@
|
||||
|
||||
const double EPS = 1e-10;
|
||||
|
||||
SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
||||
{
|
||||
SVzNL3DPoint c;
|
||||
c.x = a.y * b.z - a.z * b.y;
|
||||
c.y = a.z * b.x - a.x * b.z;
|
||||
c.z = a.x * b.y - a.y * b.x;
|
||||
return c;
|
||||
}
|
||||
|
||||
SVzNL3DRangeD sg_getScanDataROI(
|
||||
//¼ÆËãɨÃèROI
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
|
||||
@ -44,3 +44,6 @@
|
||||
//拓普发工件定位
|
||||
#define SX_ERR_UNKNOWN_OBJECT -2501
|
||||
|
||||
//配天工件定位
|
||||
#define SX_ERR_UNKNOWN_PLATE_DIR -2601
|
||||
|
||||
|
||||
@ -11,7 +11,8 @@
|
||||
//version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
|
||||
//version 1.2.1 : 增加了定位盘中心完整姿态输出
|
||||
//version 1.2.2 : 调整了定位盘中心姿态定义：法向（Z向）：向右；Y：向下
|
||||
std::string m_strVersion = "1.2.2";
|
||||
//version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量
|
||||
std::string m_strVersion = "1.2.3";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -481,6 +482,242 @@ SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
|
||||
return roi;
|
||||
}
|
||||
|
||||
bool _compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||||
return a.angle < b.angle;
|
||||
}
|
||||
|
||||
//计算封闭序列（首尾相连）在XOY平面内前向角和后向角以及拐角。 前向角：序号+； 后向角：序号-
|
||||
void _computeClosedPntListDirCorners(std::vector<SWD_polarPt>& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles)
|
||||
{
|
||||
int pntSize = (int)polarPoints.size();
|
||||
//std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 });
|
||||
dirCornerAngles.resize(pntSize);
|
||||
for (int i = 0; i < pntSize; i++)
|
||||
{
|
||||
if (i == 68)
|
||||
int kkk = 1;
|
||||
memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle));
|
||||
SWD_polarPt& a_polarPt = polarPoints[i];
|
||||
//前向寻找
|
||||
int minus_i = -1;
|
||||
for (int loop = i - 1; loop >= i - pntSize; loop--)
|
||||
{
|
||||
int j = loop >= 0 ? loop : (loop + pntSize);
|
||||
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
|
||||
pow(polarPoints[i].y - polarPoints[j].y, 2));
|
||||
if (dist >= scale)
|
||||
{
|
||||
minus_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//后向寻找
|
||||
int plus_i = -1;
|
||||
for (int loop = i + 1; loop < i + pntSize; loop++)
|
||||
{
|
||||
int j = loop % pntSize;
|
||||
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
|
||||
pow(polarPoints[i].y - polarPoints[j].y, 2));
|
||||
if (dist >= scale)
|
||||
{
|
||||
plus_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
if ((minus_i >= 0) && (plus_i >= 0))
|
||||
{
|
||||
double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI;
|
||||
double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI;
|
||||
dirCornerAngles[i].forwardAngle = forwardAngle;
|
||||
dirCornerAngles[i].backwardAngle = backwardAngle;
|
||||
|
||||
double corner = forwardAngle - backwardAngle;
|
||||
if (corner < -180)
|
||||
corner += 360;
|
||||
else if (corner > 180)
|
||||
corner = corner - 360;
|
||||
|
||||
dirCornerAngles[i].flag = 0;
|
||||
//过滤掉由于极角和方向角相近产生的轮廓点位序错误导致的corner错误
|
||||
double polarAngle = dirCornerAngles[i].point.angle;
|
||||
double angleDiff1 = abs(polarAngle - dirCornerAngles[i].forwardAngle);
|
||||
if (angleDiff1 > 180)
|
||||
angleDiff1 = 360 - angleDiff1;
|
||||
double angleDiff2 = abs(polarAngle - dirCornerAngles[i].backwardAngle);
|
||||
if (angleDiff2 > 180)
|
||||
angleDiff2 = 360 - angleDiff2;
|
||||
if ((angleDiff1 < 10) || (angleDiff2 < 10)) //过滤掉边与极线方向相近导致的波动
|
||||
{
|
||||
if (dirCornerAngles[i].corner > 160) //乱序产生角度错误
|
||||
{
|
||||
dirCornerAngles[i].flag = -1;
|
||||
}
|
||||
}
|
||||
|
||||
dirCornerAngles[i].corner = corner; //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||||
dirCornerAngles[i].pntIdx = i;
|
||||
dirCornerAngles[i].forward_pntIdx = plus_i;
|
||||
dirCornerAngles[i].backward_pntIdx = minus_i;
|
||||
dirCornerAngles[i].point = polarPoints[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||||
void _searchPlusCornerPeaks(
|
||||
std::vector< SSG_dirCornerAngle>& corners,
|
||||
double cutAngleTh,
|
||||
std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
|
||||
)
|
||||
{
|
||||
std::vector<SSG_dirCornerAngle> peakCorners;
|
||||
int cornerSize = (int)corners.size();
|
||||
//搜索拐角极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
SSG_dirCornerAngle* pre_data = NULL;
|
||||
for (int i = 0, i_max = cornerSize; i < i_max; i++)
|
||||
{
|
||||
if (i == 451)
|
||||
int kkk = 1;
|
||||
SSG_dirCornerAngle* curr_data = &corners[i];
|
||||
if (curr_data->flag < 0)
|
||||
continue;
|
||||
|
||||
if (NULL == pre_data)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double cornerDiff = curr_data->corner - pre_data->corner;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (cornerDiff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
if (pre_data->corner > cutAngleTh) //截角门限，滤除点波动导致的角度变化
|
||||
peakCorners.push_back(*pre_data);
|
||||
_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: //下降
|
||||
if (cornerDiff > 0) // 上升
|
||||
_state = 1;
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
//注意：最后一个不处理，为基座位置
|
||||
|
||||
//极小值点（峰顶）
|
||||
//极值比较，在尺度窗口下寻找局部极值点
|
||||
for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++)
|
||||
{
|
||||
bool isPeak = true;
|
||||
SSG_dirCornerAngle& a_dirAngle = peakCorners[i];
|
||||
int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx;
|
||||
if (curr_dist1 < 0)
|
||||
curr_dist1 += cornerSize;
|
||||
int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx;
|
||||
if (curr_dist2 < 0)
|
||||
curr_dist2 += cornerSize;
|
||||
|
||||
//minus方向寻找
|
||||
int minus_i = i;
|
||||
while (1)
|
||||
{
|
||||
minus_i--;
|
||||
if (minus_i < 0)
|
||||
minus_i += i_max;
|
||||
SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i];
|
||||
int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx;
|
||||
if (pntDist_0 < 0)
|
||||
pntDist_0 += cornerSize;
|
||||
if (pntDist_0 > curr_dist1)
|
||||
break;
|
||||
|
||||
if (a_dirAngle.corner < minus_dirAngle.corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//plus方向寻找
|
||||
int plus_i = i;
|
||||
while (1)
|
||||
{
|
||||
plus_i++;
|
||||
if (plus_i >= i_max)
|
||||
plus_i = 0;
|
||||
SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i];
|
||||
int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx;
|
||||
if (pntDist_0 < 0)
|
||||
pntDist_0 += cornerSize;
|
||||
if (pntDist_0 > curr_dist2)
|
||||
break;
|
||||
|
||||
if (a_dirAngle.corner < plus_dirAngle.corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
{
|
||||
double corner_curr = a_dirAngle.corner;
|
||||
double corner_1 = corners[a_dirAngle.forward_pntIdx].corner;
|
||||
double corner_2 = corners[a_dirAngle.backward_pntIdx].corner;
|
||||
double diff_1 = corner_curr - corner_1;
|
||||
double diff_2 = corner_curr - corner_2;
|
||||
if ((diff_1 > corner_curr / 4) && (diff_2 > corner_curr / 4))
|
||||
{
|
||||
double polarAngle = a_dirAngle.point.angle;
|
||||
double angleDiff1 = abs(polarAngle - a_dirAngle.forwardAngle);
|
||||
if (angleDiff1 > 180)
|
||||
angleDiff1 = 360 - angleDiff1;
|
||||
double angleDiff2 = abs(polarAngle - a_dirAngle.backwardAngle);
|
||||
if (angleDiff2 > 180)
|
||||
angleDiff2 = 360 - angleDiff2;
|
||||
if ((angleDiff1 < 10) || (angleDiff2 < 10)) //过滤掉边与极线方向相近导致的波动
|
||||
{
|
||||
if (a_dirAngle.corner < 160) //乱序产生角度错误
|
||||
{
|
||||
a_dirAngle.flag = 0;
|
||||
cornerPlusPeaks.push_back(a_dirAngle);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
a_dirAngle.flag = 0;
|
||||
cornerPlusPeaks.push_back(a_dirAngle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//计算定位盘中心点位姿
|
||||
SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
@ -706,10 +943,20 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
|
||||
//拟合平面
|
||||
std::vector<cv::Point3f> Points3ds;
|
||||
std::vector<SWD_polarPt> polarPoints;
|
||||
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
|
||||
Points3ds.push_back(a_pt);
|
||||
SWD_polarPt a_polarPt;
|
||||
a_polarPt.lineIdx = objClusters[objIdx][i].nPointIdx >> 16;
|
||||
a_polarPt.ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
|
||||
a_polarPt.R = 0;
|
||||
a_polarPt.angle = 0;
|
||||
a_polarPt.x = objClusters[objIdx][i].pt3D.x;
|
||||
a_polarPt.y = objClusters[objIdx][i].pt3D.y;
|
||||
a_polarPt.z = objClusters[objIdx][i].pt3D.z;
|
||||
polarPoints.push_back(a_polarPt);
|
||||
}
|
||||
//计算面参数: z = Ax + By + C
|
||||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
@ -734,6 +981,9 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5];
|
||||
double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8];
|
||||
projectPoints3ds[i] = cv::Point3f(x, y, z);
|
||||
polarPoints[i].x = x;
|
||||
polarPoints[i].y = y;
|
||||
polarPoints[i].z = z;
|
||||
sum_x += x;
|
||||
sum_y += y;
|
||||
}
|
||||
@ -751,6 +1001,90 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
double center_x = sum_x / (double)Points3ds.size();
|
||||
double center_y = sum_y / (double)Points3ds.size();
|
||||
|
||||
//计算极坐标的R和Theta
|
||||
for (int pi = 0; pi < (int)polarPoints.size(); pi++)
|
||||
{
|
||||
double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
|
||||
angle = (angle / PI) * 180 + 180.0;
|
||||
double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
|
||||
polarPoints[pi].R = R;
|
||||
polarPoints[pi].angle = angle;
|
||||
}
|
||||
//按角度大小排序
|
||||
std::sort(polarPoints.begin(), polarPoints.end(), _compareByAngle);
|
||||
for (int pi = 0; pi < (int)polarPoints.size(); pi++)
|
||||
polarPoints[pi].cptIndex = pi; // index
|
||||
|
||||
//计算有序边缘点的角度变化 2026.03.06
|
||||
std::vector< SSG_dirCornerAngle> dirCornerAngles;
|
||||
double dirAngleScale = 20.0;
|
||||
_computeClosedPntListDirCorners(polarPoints, dirAngleScale, dirCornerAngles);
|
||||
//提取方向角拐点(正）极值
|
||||
std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
|
||||
double minCutAngleTh = 30;
|
||||
_searchPlusCornerPeaks(dirCornerAngles, minCutAngleTh, cornerPlusPeaks);
|
||||
|
||||
//生成直线段数据
|
||||
int cornerIdx_0 = -1;
|
||||
int cornerIdx_1 = -1;
|
||||
for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
|
||||
{
|
||||
if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90))
|
||||
cornerIdx_0 = i;
|
||||
if ((cornerPlusPeaks[i].point.angle > 90) && (cornerPlusPeaks[i].point.angle < 180))
|
||||
cornerIdx_1 = i;
|
||||
}
|
||||
if ((cornerIdx_0 < 0) && (cornerIdx_1 < 0))
|
||||
{
|
||||
if (objIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
|
||||
return resultPose;
|
||||
}
|
||||
}
|
||||
std::vector<SVzNL3DPoint> lineFittingPts;
|
||||
double endingRemoveDist = 10.0;
|
||||
double validFittingLen = 100;
|
||||
if (cornerIdx_0 >= 0)
|
||||
{
|
||||
int startIdx = cornerPlusPeaks[cornerIdx_0].pntIdx;
|
||||
double sx = polarPoints[startIdx].x;
|
||||
double sy = polarPoints[startIdx].y;
|
||||
for (int m = startIdx; m < (int)polarPoints.size(); m++)
|
||||
{
|
||||
double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2));
|
||||
if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist)))
|
||||
{
|
||||
SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
|
||||
lineFittingPts.push_back(a_fitPt);
|
||||
}
|
||||
if (dist > (validFittingLen + endingRemoveDist))
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int startIdx = cornerPlusPeaks[cornerIdx_1].pntIdx;
|
||||
double sx = polarPoints[startIdx].x;
|
||||
double sy = polarPoints[startIdx].y;
|
||||
for (int m = startIdx; m >= 0; m--)
|
||||
{
|
||||
double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2));
|
||||
if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist)))
|
||||
{
|
||||
SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
|
||||
lineFittingPts.push_back(a_fitPt);
|
||||
}
|
||||
if (dist > (validFittingLen + endingRemoveDist))
|
||||
break;
|
||||
}
|
||||
}
|
||||
//直线拟合
|
||||
double _k, _b;
|
||||
lineFitting(lineFittingPts, &_k, &_b);
|
||||
double normData = sqrt(pow(_k, 2) + 1);
|
||||
SVzNL3DPoint xDir = { 1.0/normData, _k/normData, 0.0 };
|
||||
|
||||
//计算4个点
|
||||
SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R };
|
||||
SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R };
|
||||
@ -834,8 +1168,12 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
|
||||
double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
|
||||
resultPose.center = { center_x, center_y, center_z };
|
||||
resultPose.xDir = { 0.0, 0.0, -1.0 }; // { 1.0, 0.0, 0 };
|
||||
resultPose.yDir = { 0.0, 1.0, 0 };
|
||||
resultPose.normalDir = { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
|
||||
//resultPose.yDir = { 0.0, 1.0, 0 };
|
||||
resultPose.normalDir = xDir; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
|
||||
//叉乘出y;
|
||||
//向量叉乘
|
||||
resultPose.yDir = vec3_cross(resultPose.normalDir, resultPose.xDir);
|
||||
|
||||
//旋转回去
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user