1225 lines
33 KiB
C++
1225 lines
33 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "motorStatorPosition_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
|
||
#define DEBUG_OUT_IMAGE 1
|
||
|
||
typedef struct
|
||
{
|
||
int objID;
|
||
cv::Point2f pos;
|
||
double angle;
|
||
}SSG_objAngleInfo;
|
||
typedef struct
|
||
{
|
||
bool isSide;
|
||
double sideDist;
|
||
}SSG_objSideInfo;
|
||
typedef struct
|
||
{
|
||
int objID;
|
||
cv::Point2f objPos;
|
||
SVzNL3DPoint objPos3D;
|
||
std::vector< SSG_objAngleInfo> neighbours;
|
||
SSG_objSideInfo sideInfo[4]; //对应L，R，T，P
|
||
}SSG_statorNeighbourInfo;
|
||
|
||
typedef struct
|
||
{
|
||
int ID;
|
||
int linkNum;
|
||
int neighborID[6];
|
||
}SSG_hexagonNeighbour;
|
||
|
||
std::string m_strVersion = "1.0.0";
|
||
const char* wd_particleSegVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void wd_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SSG_objSideInfo _getSideX(
|
||
int cx,
|
||
int cy,
|
||
cv::Mat& zSliceData,
|
||
bool dirLeft)
|
||
{
|
||
int edge_x = -1;
|
||
SSG_objSideInfo sideInfo;
|
||
sideInfo.isSide = false;
|
||
sideInfo.sideDist = -1;
|
||
|
||
if (true == dirLeft)
|
||
{
|
||
for (int m = cx; m >= 0; m--)
|
||
{
|
||
if (zSliceData.at<uchar>(cy, m) > 0)
|
||
edge_x = m;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
for (int m = cx; m < zSliceData.cols; m++)
|
||
{
|
||
if (zSliceData.at<uchar>(cy, m) > 0)
|
||
edge_x = m;
|
||
}
|
||
}
|
||
if(edge_x >= 0)
|
||
{
|
||
sideInfo.isSide = true;
|
||
sideInfo.sideDist = edge_x;
|
||
}
|
||
return sideInfo;
|
||
}
|
||
|
||
SSG_objSideInfo _getSideY(
|
||
int cx,
|
||
int cy,
|
||
cv::Mat& zSliceData,
|
||
bool dirUp)
|
||
{
|
||
int edge_y = -1;
|
||
SSG_objSideInfo sideInfo;
|
||
sideInfo.isSide = false;
|
||
sideInfo.sideDist = -1;
|
||
|
||
if (true == dirUp)
|
||
{
|
||
for (int m = cy; m >= 0; m--)
|
||
{
|
||
if (zSliceData.at<uchar>(m, cx) > 0)
|
||
edge_y = m;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
for (int m = cy; m < zSliceData.rows; m++)
|
||
{
|
||
if (zSliceData.at<uchar>(m, cx) > 0)
|
||
edge_y = m;
|
||
}
|
||
}
|
||
if (edge_y >= 0)
|
||
{
|
||
sideInfo.isSide = true;
|
||
sideInfo.sideDist = edge_y;
|
||
}
|
||
sideInfo.isSide = true;
|
||
return sideInfo;
|
||
}
|
||
|
||
void _getNeighbouringInfo(
|
||
const SWD_statorParam positionParam,
|
||
cv::Mat& zSliceData, //用于寻找边界（边框）
|
||
std::vector<SG_fittingInfo>& Objects, //目标位置
|
||
std::vector< SSG_statorNeighbourInfo>& neighbouringInfo //邻接关系
|
||
)
|
||
{
|
||
double searchWin = positionParam.statorOuterD * 2.5;
|
||
int objNum = Objects.size();
|
||
neighbouringInfo.resize(objNum);
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
SG_fittingInfo* a_obj = &Objects[i];
|
||
SSG_statorNeighbourInfo* obj_info = &neighbouringInfo[i];
|
||
obj_info->objID = i;
|
||
obj_info->objPos3D = a_obj->objCenter;
|
||
obj_info->objPos = a_obj->fittingPara.center;
|
||
bool checkSide[4];
|
||
for (int m = 0; m < 4; m++)
|
||
checkSide[m] = true;
|
||
for (int j = 0; j < objNum; j++)
|
||
{
|
||
if (j != i)
|
||
{
|
||
//计算中心点距离
|
||
double dist = sqrt(pow(a_obj->fittingPara.center.x - Objects[j].fittingPara.center.x, 2) + pow(a_obj->fittingPara.center.y - Objects[j].fittingPara.center.y, 2));
|
||
if (dist < searchWin)
|
||
{
|
||
SSG_objAngleInfo a_neighbour;
|
||
a_neighbour.objID = j;
|
||
a_neighbour.pos = Objects[j].fittingPara.center;
|
||
//计算角度
|
||
double angle = atan2(Objects[j].fittingPara.center.y - a_obj->fittingPara.center.y, Objects[j].fittingPara.center.x - a_obj->fittingPara.center.x);
|
||
angle = angle * 180.0 / PI; //转变成角度
|
||
if (angle < 0)
|
||
angle += 360; //转变成0-360度范围
|
||
a_neighbour.angle = angle;
|
||
if ((angle > 345) || (angle < 15))
|
||
checkSide[0] = false;
|
||
else if ((angle > 75) && (angle < 105))
|
||
checkSide[1] = false;
|
||
else if ((angle > 165) && (angle < 195))
|
||
checkSide[2] = false;
|
||
else if ((angle > 255) && (angle < 285))
|
||
checkSide[3] = false;
|
||
obj_info->neighbours.push_back(a_neighbour);
|
||
}
|
||
}
|
||
}
|
||
//分析neighbour情况，判断是否要检查side情况
|
||
int cx = (int)a_obj->fittingPara.center.x;
|
||
int cy = (int)a_obj->fittingPara.center.y;
|
||
if (false == checkSide[0])
|
||
{
|
||
obj_info->sideInfo[0].isSide = false;
|
||
obj_info->sideInfo[0].sideDist = -1;
|
||
}
|
||
else
|
||
{
|
||
SSG_objSideInfo sideInfo = _getSideX(cx, cy, zSliceData, true);
|
||
obj_info->sideInfo[0] = sideInfo;
|
||
}
|
||
|
||
if (false == checkSide[1])
|
||
{
|
||
obj_info->sideInfo[1].isSide = false;
|
||
obj_info->sideInfo[1].sideDist = -1;
|
||
}
|
||
else
|
||
{
|
||
SSG_objSideInfo sideInfo = _getSideX(cx, cy, zSliceData, false);
|
||
obj_info->sideInfo[1] = sideInfo;
|
||
}
|
||
|
||
if (false == checkSide[2])
|
||
{
|
||
obj_info->sideInfo[2].isSide = false;
|
||
obj_info->sideInfo[2].sideDist = -1;
|
||
}
|
||
else
|
||
{
|
||
SSG_objSideInfo sideInfo = _getSideY(cx, cy, zSliceData, true);
|
||
obj_info->sideInfo[2] = sideInfo;
|
||
}
|
||
|
||
if (false == checkSide[3])
|
||
{
|
||
obj_info->sideInfo[3].isSide = false;
|
||
obj_info->sideInfo[3].sideDist = -1;
|
||
}
|
||
else
|
||
{
|
||
SSG_objSideInfo sideInfo = _getSideY(cx, cy, zSliceData, false);
|
||
obj_info->sideInfo[3] = sideInfo;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _getPtsApart120(cv::Point2f center, double R, double angle, cv::Point2f* testPts)
|
||
{
|
||
double theta_0 = angle * PI / 180;
|
||
double theta_120 = angle + 120;
|
||
if (theta_120 > 360)
|
||
theta_120 = theta_120 - 360;
|
||
theta_120 = theta_120 * PI / 180;
|
||
double theta_240 = angle + 240;
|
||
if (theta_240 > 360)
|
||
theta_240 = theta_240 - 360;
|
||
theta_240 = theta_240 *PI / 180;
|
||
|
||
testPts[0].x = center.x + R * cos(theta_0);
|
||
testPts[0].y = center.y - R * sin(theta_0);
|
||
testPts[1].x = center.x + R * cos(theta_120);
|
||
testPts[1].y = center.y - R * sin(theta_120);
|
||
testPts[2].x = center.x + R * cos(theta_240);
|
||
testPts[2].y = center.y - R * sin(theta_240);
|
||
}
|
||
|
||
double _getMinDist(
|
||
SSG_statorNeighbourInfo& neighbouringInfo,
|
||
cv::Point2f* testPts
|
||
)
|
||
{
|
||
double min_dist = -1;
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
cv::Point2f* a_pt = &testPts[i];
|
||
for (int j = 0; j < neighbouringInfo.neighbours.size(); j++)
|
||
{
|
||
cv::Point2f* n_pt = &(neighbouringInfo.neighbours[j].pos);
|
||
double dist = sqrt(pow(a_pt->x - n_pt->x, 2) + pow(a_pt->y - n_pt->y, 2));
|
||
if (min_dist < 0)
|
||
min_dist = dist;
|
||
else if (min_dist > dist)
|
||
min_dist = dist;
|
||
}
|
||
if (true == neighbouringInfo.sideInfo[0].isSide)
|
||
{
|
||
double dist = a_pt->x - neighbouringInfo.sideInfo[0].sideDist;//外边界
|
||
dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
|
||
if (dist < 0)
|
||
dist = 0;
|
||
if (min_dist < 0)
|
||
min_dist = dist;
|
||
else if (min_dist > dist)
|
||
min_dist = dist;
|
||
}
|
||
if (true == neighbouringInfo.sideInfo[1].isSide)
|
||
{
|
||
double dist = neighbouringInfo.sideInfo[1].sideDist - a_pt->x;//外边界
|
||
dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
|
||
if (dist < 0)
|
||
dist = 0;
|
||
if (min_dist < 0)
|
||
min_dist = dist;
|
||
else if (min_dist > dist)
|
||
min_dist = dist;
|
||
}
|
||
if (true == neighbouringInfo.sideInfo[2].isSide)
|
||
{
|
||
double dist = a_pt->y - neighbouringInfo.sideInfo[2].sideDist;//外边界
|
||
dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
|
||
if (dist < 0)
|
||
dist = 0;
|
||
if (min_dist < 0)
|
||
min_dist = dist;
|
||
else if (min_dist > dist)
|
||
min_dist = dist;
|
||
}
|
||
if (true == neighbouringInfo.sideInfo[3].isSide)
|
||
{
|
||
double dist = neighbouringInfo.sideInfo[3].sideDist - a_pt->y;//外边界
|
||
dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
|
||
if (dist < 0)
|
||
dist = 0;
|
||
if (min_dist < 0)
|
||
min_dist = dist;
|
||
else if (min_dist > dist)
|
||
min_dist = dist;
|
||
}
|
||
}
|
||
return min_dist;
|
||
}
|
||
|
||
void _computeGripperPose(
|
||
SSG_statorNeighbourInfo& neighbouringInfo,
|
||
double gripperR,
|
||
double* opAngle,
|
||
double* obstacleDist)
|
||
{
|
||
//根据相邻目标数量确定抓取点
|
||
//圆周扫描获取距离相邻目标最小距离最大的点
|
||
double searchStepping = 0.5; //搜索精度为0.5度
|
||
int steps = (int)(360.0 / searchStepping);
|
||
double max_dist = -1;
|
||
double max_angle = -1;
|
||
for (int i = 0; i < steps; i++)
|
||
{
|
||
double angle = i * searchStepping;
|
||
cv::Point2f testPts[3];
|
||
_getPtsApart120(neighbouringInfo.objPos, gripperR, angle, testPts);
|
||
//计算最小距离，相邻目标和边界统一在一起计算
|
||
double min_dist = _getMinDist(neighbouringInfo, testPts);
|
||
//获取最小距离的最大值，对应的角度为操作角度
|
||
if (max_dist < 0)
|
||
{
|
||
max_dist = min_dist;
|
||
max_angle = angle;
|
||
}
|
||
else if(max_dist < min_dist)
|
||
{
|
||
max_dist = min_dist;
|
||
max_angle = angle;
|
||
}
|
||
}
|
||
*opAngle = max_angle;
|
||
*obstacleDist = max_dist;
|
||
return;
|
||
}
|
||
|
||
bool compareByNeighbourNumber(const SSG_hexagonNeighbour& a, const SSG_hexagonNeighbour& b)
|
||
{
|
||
return a.linkNum < b.linkNum;
|
||
}
|
||
typedef struct
|
||
{
|
||
cv::RotatedRect fittingPara;
|
||
SVzNL3DPoint objCenter;
|
||
}SG_fittingInfo;
|
||
|
||
//圆周扫描定子环
|
||
//以scanCenter为扫描中心，圆周扫描从scanR1到scanR2的圆形区域
|
||
void circileScan(
|
||
cv::Mat& scanImg, //扫描对象
|
||
int scanCenterX, int scanCenterY, //扫描中心
|
||
double scanR1, double scanR2, //扫描区域
|
||
std::vector<SVzNL2DPoint>& peakPts //扫描结果
|
||
)
|
||
{
|
||
//以1度间隔扫描
|
||
double scale = 1.0;
|
||
for (int i = 0; i < 360; i++)
|
||
{
|
||
double angle = (double)i * scale * PI / 180;
|
||
double sinTheta = sin(angle);
|
||
double cosTheta = cos(angle);
|
||
int x0 = scanCenterX + (int)(scanR1 * cosTheta);
|
||
int y0 = scanCenterY - (int)(scanR1 * sinTheta);
|
||
int x1 = scanCenterX + (int)(scanR2 * cosTheta);
|
||
int y1 = scanCenterY - (int)(scanR2 * sinTheta);
|
||
|
||
//Bresenham算法
|
||
std::vector<SVzNL2DPoint> pts;
|
||
drawLine(x0, y0, x1, y1, pts);
|
||
//搜索峰值
|
||
float peakValue = 0;
|
||
SVzNL2DPoint peakPos = { 0,0 };
|
||
for (int m = 0, m_max = (int)pts.size(); m < m_max; m++)
|
||
{
|
||
if (peakValue < scanImg.at<float>(pts[m].y, pts[m].x))
|
||
{
|
||
peakValue = scanImg.at<float>(pts[m].y, pts[m].x);
|
||
peakPos = pts[m];
|
||
}
|
||
}
|
||
peakPts.push_back(peakPos);
|
||
}
|
||
}
|
||
|
||
//对定子外圈抓取的三爪抓手计算旋转位置。
|
||
//三爪对称，分别在30,150,270度。根据抓取角度，计算最小旋转角度
|
||
double _computeGrasperRotateAngle(double grasperAngle)
|
||
{
|
||
double rotateAngle = 0;
|
||
if ((grasperAngle >= 330) || (grasperAngle < 90)) //丫第一个枝位置30度
|
||
{
|
||
if (grasperAngle < 90)
|
||
rotateAngle = grasperAngle - 30;
|
||
else
|
||
rotateAngle = grasperAngle - 360 - 30;
|
||
}
|
||
else if ((grasperAngle >= 90) && (grasperAngle < 210))//丫第二个枝位置150度
|
||
rotateAngle = grasperAngle - 150;
|
||
else
|
||
rotateAngle = grasperAngle - 270;
|
||
return rotateAngle;
|
||
}
|
||
|
||
//计算两个角的间隔（绝对值），结果在0-180范围
|
||
double _computeAbsAngleInterval(double angle_0, double angle_1)
|
||
{
|
||
double diff = angle_0 < angle_1 ? (angle_1 - angle_0) : (angle_0 - angle_1);
|
||
if (diff > 180)
|
||
diff = 360 - diff;
|
||
return diff;
|
||
}
|
||
|
||
//计算角度与相邻目标的角度间隔，以其中最小值为结果
|
||
double _computeObjAngleInterval(double angle_0, std::vector<double>& linkObjAngles)
|
||
{
|
||
if (linkObjAngles.size() == 0)
|
||
return -1;
|
||
double minInterval = _computeAbsAngleInterval(angle_0, linkObjAngles[0]);
|
||
for (int i = 1, i_max = (int)linkObjAngles.size(); i < i_max; i++)
|
||
{
|
||
double angleInterval = _computeAbsAngleInterval(angle_0, linkObjAngles[i]);
|
||
if (minInterval > angleInterval)
|
||
minInterval = angleInterval;
|
||
}
|
||
return minInterval;
|
||
}
|
||
|
||
//计算目标最佳抓取角度。抓手为三爪设计，间隔120度。
|
||
//扫描与相邻目标最大的角度间隔点。此位置为最佳抓取位置
|
||
double _scanBestGrasperAngle(std::vector<double>& linkObjAngles, double* angleDistance)
|
||
{
|
||
*angleDistance = 0;
|
||
if (linkObjAngles.size() == 0)
|
||
return -1;
|
||
//以0.1度间隔扫描120度范围
|
||
double maxAngleDist = -1;
|
||
double bestAngleIdx = -1;
|
||
//粗定位，精确到1度
|
||
for (int i = 0; i < 120; i++)
|
||
{
|
||
double angle_0 = (double)i;
|
||
double angle_1 = angle_0 + 120;
|
||
if (angle_1 >= 360)
|
||
angle_1 = angle_1 - 360;
|
||
double angle_2 = angle_0 + 240;
|
||
if (angle_2 >= 360)
|
||
angle_2 = angle_2 - 360;
|
||
|
||
double angleInterval_0 = _computeObjAngleInterval(angle_0, linkObjAngles);
|
||
double angleInterval_1 = _computeObjAngleInterval(angle_1, linkObjAngles);
|
||
double angleInterval_2 = _computeObjAngleInterval(angle_2, linkObjAngles);
|
||
double angleInterval = angleInterval_0 < angleInterval_1 ? angleInterval_0 : angleInterval_1;
|
||
angleInterval = angleInterval < angleInterval_2 ? angleInterval : angleInterval_2;
|
||
|
||
if (maxAngleDist < 0)
|
||
{
|
||
maxAngleDist = angleInterval;
|
||
bestAngleIdx = (double)i;
|
||
}
|
||
else
|
||
{
|
||
if (maxAngleDist < angleInterval)
|
||
{
|
||
maxAngleDist = angleInterval;
|
||
bestAngleIdx = (double)i;
|
||
}
|
||
}
|
||
}
|
||
if (bestAngleIdx < 0)
|
||
return -1;
|
||
//精定位， 精确到0.1度
|
||
maxAngleDist = -1;
|
||
double bestAngle = bestAngleIdx;
|
||
for (int i = -10; i <= 10; i++)
|
||
{
|
||
double angle_0 = (double)i * 0.1 + bestAngleIdx;
|
||
double angle_1 = angle_0 + 120;
|
||
if (angle_1 >= 360)
|
||
angle_1 = angle_1 - 360;
|
||
double angle_2 = angle_0 + 240;
|
||
if (angle_2 >= 360)
|
||
angle_2 = angle_2 - 360;
|
||
|
||
double angleInterval_0 = _computeObjAngleInterval(angle_0, linkObjAngles);
|
||
double angleInterval_1 = _computeObjAngleInterval(angle_1, linkObjAngles);
|
||
double angleInterval_2 = _computeObjAngleInterval(angle_2, linkObjAngles);
|
||
double angleInterval = angleInterval_0 < angleInterval_1 ? angleInterval_0 : angleInterval_1;
|
||
angleInterval = angleInterval < angleInterval_2 ? angleInterval : angleInterval_2;
|
||
|
||
if (maxAngleDist < 0)
|
||
{
|
||
maxAngleDist = angleInterval;
|
||
bestAngle = angle_0;
|
||
}
|
||
else
|
||
{
|
||
if (maxAngleDist < angleInterval)
|
||
{
|
||
maxAngleDist = angleInterval;
|
||
bestAngle = angle_0;
|
||
}
|
||
}
|
||
}
|
||
*angleDistance = maxAngleDist;
|
||
if (bestAngle < 0)
|
||
bestAngle = bestAngle + 360;
|
||
|
||
return bestAngle;
|
||
}
|
||
|
||
bool computeOuterGraspPoint(
|
||
int objID,
|
||
double outerR,
|
||
std::vector< SSG_peakRgnInfo>& objects,
|
||
std::vector< SSG_hexagonNeighbour>& objLinkings,
|
||
SWD_statorOuterGrasper& graspPoint,
|
||
double* angleDistance
|
||
)
|
||
{
|
||
//R = 1.155outR
|
||
SSG_hexagonNeighbour& a_link = objLinkings[objID];
|
||
std::vector<int> linkIDs;
|
||
for (int i = 0; i < 6; i++)
|
||
{
|
||
if (a_link.neighborID[i] >= 0)
|
||
{
|
||
linkIDs.push_back(a_link.neighborID[i]);
|
||
}
|
||
}
|
||
if (linkIDs.size() != a_link.linkNum)
|
||
return false;
|
||
|
||
//计算抓取位置
|
||
if (a_link.linkNum == 0) //默认抓取位置
|
||
{
|
||
graspPoint.objID = objID;
|
||
graspPoint.grasperAngle = 0;
|
||
graspPoint.rotateAngle = 0;
|
||
graspPoint.graspR = 1.2 * outerR;
|
||
*angleDistance = 120;
|
||
return true;
|
||
}
|
||
else if (a_link.linkNum == 1) //与目标连线方向偏离60度
|
||
{
|
||
int linkID = linkIDs[0];
|
||
SSG_peakRgnInfo& obj = objects[objID];
|
||
SSG_peakRgnInfo& nObj = objects[linkID];
|
||
double dy = nObj.pos2D.y - obj.pos2D.y;
|
||
double dx = nObj.pos2D.x - obj.pos2D.x;
|
||
double dist = sqrt(pow(dx, 2) + pow(dy, 2));
|
||
double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360度
|
||
double grasperAngle = angle + 60;
|
||
if (grasperAngle >= 360)
|
||
grasperAngle = grasperAngle - 360;
|
||
double rotateAngle = _computeGrasperRotateAngle(grasperAngle);
|
||
graspPoint.grasperAngle = grasperAngle;
|
||
graspPoint.rotateAngle = rotateAngle;
|
||
graspPoint.graspR = 1.2 * outerR;
|
||
graspPoint.objID = objID;
|
||
*angleDistance = 60;
|
||
return true;
|
||
}
|
||
else if ( (a_link.linkNum == 2) || (a_link.linkNum == 3)) //目标有两个或3个相邻目标
|
||
{
|
||
std::vector<double> linkObjAngles;
|
||
for (int i = 0, i_max = (int)linkIDs.size(); i < i_max; i++)
|
||
{
|
||
int linkID = linkIDs[i];
|
||
SSG_peakRgnInfo& obj = objects[objID];
|
||
SSG_peakRgnInfo& nObj = objects[linkID];
|
||
double dy = nObj.pos2D.y - obj.pos2D.y;
|
||
double dx = nObj.pos2D.x - obj.pos2D.x;
|
||
double dist = sqrt(pow(dx, 2) + pow(dy, 2));
|
||
double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360度
|
||
linkObjAngles.push_back(angle);
|
||
}
|
||
double grasperAngle = _scanBestGrasperAngle(linkObjAngles, angleDistance);
|
||
double rotateAngle = _computeGrasperRotateAngle(grasperAngle);
|
||
graspPoint.grasperAngle = grasperAngle;
|
||
graspPoint.rotateAngle = rotateAngle;
|
||
graspPoint.graspR = 1.155 * outerR;
|
||
graspPoint.objID = objID;
|
||
return true;
|
||
}
|
||
else
|
||
return false;
|
||
}
|
||
|
||
//电机定子定位。
|
||
//算法逻辑：找到定子的高度->设置截取Z去掉底面->
|
||
// 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
|
||
// 根据相邻和边框寻找最佳抓取目标和抓取点
|
||
void wd_motorStatorPosition(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SWD_statorParam statorParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SWD_statorPositonParam algoParam,
|
||
SWD_nextOpParam* refPos, //上一次给出的参考位置，同时输出下一次的参考位置
|
||
int* errCode,
|
||
std::vector<SWD_motorStatorPosition>& resultObjPositions,
|
||
SWD_statorOuterGrasper& resultGraspPos
|
||
)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
//噪点过滤
|
||
wd_noiseFilter(scanLines, algoParam.filterParam, errCode);
|
||
if (*errCode != 0)
|
||
return;
|
||
|
||
///开始数据处理
|
||
double statorRingWidth = (statorParam.statorOuterD - statorParam.statorInnerD) / 2;
|
||
if (refPos->cuttingZ < 0) //提取特征：定子上半部的环。根据提取的环判断定子高度
|
||
{
|
||
//垂直数据处理
|
||
std::vector<std::vector<SWD_segFeature>> all_vLineArcs;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (i == 202)
|
||
int k = 1;
|
||
//提取定子环
|
||
std::vector<SWD_segFeature> line_ringArcs;
|
||
wd_getRingArcFeature(
|
||
scanLines[i],
|
||
i,
|
||
algoParam.cornerParam,
|
||
statorRingWidth, //定子的环宽度
|
||
line_ringArcs //环
|
||
);
|
||
all_vLineArcs.push_back(line_ringArcs); //空行也加入，保证能按行号索引
|
||
}
|
||
//特征生长，用生长长度滤除虚假的特征
|
||
//垂直方向特征生长（相机扫描方向）
|
||
std::vector<SWD_segFeatureTree> v_seg_trees;
|
||
//特征生长
|
||
wd_getSegFeatureGrowingTrees(
|
||
all_vLineArcs,
|
||
v_seg_trees,
|
||
algoParam.growParam);
|
||
//提取环高度
|
||
if (v_seg_trees.size() > 0)
|
||
{
|
||
int treeSize = (int)v_seg_trees.size();
|
||
double z_sum = 0;
|
||
for (int m = 0; m < treeSize; m++)
|
||
{
|
||
z_sum += v_seg_trees[m].tree_value;
|
||
}
|
||
refPos->cuttingZ = z_sum / (double)treeSize + statorParam.statorHeight / 2;
|
||
}
|
||
}
|
||
if (refPos->cuttingZ < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLID_CUTTING_Z;
|
||
return;
|
||
}
|
||
//使用 refPos->cuttingZ 分割
|
||
//投影到2D
|
||
SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);// 统计整个视野大小
|
||
double scale;
|
||
if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0))
|
||
{
|
||
*errCode = SG_ERR_INVLD_Q_SCALE;
|
||
return;
|
||
}
|
||
if (pntCloudPara.scale_x < pntCloudPara.scale_y)
|
||
scale = pntCloudPara.scale_x;
|
||
else
|
||
scale = pntCloudPara.scale_y;
|
||
|
||
double inerPolateDistTh = scale * 10; //插值门限， 两间距离大于此阈值不插值
|
||
int edgeSkip = 2;
|
||
int maskX = (int)(pntCloudPara.xRange.max - pntCloudPara.xRange.min) / scale + 1;
|
||
int maskY = (int)(pntCloudPara.yRange.max - pntCloudPara.yRange.min) / scale + 1;
|
||
if ((maskX < 16) || (maskY < 16))
|
||
return;
|
||
maskY = maskY + edgeSkip * 2;
|
||
maskX = maskX + edgeSkip * 2;
|
||
cv::Mat projectionImg(maskY, maskX, CV_32FC1, 0.0f); //距离变换Mask，初始化为一个极大值1e+6
|
||
cv::Mat projectionIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); //坐标索引
|
||
pointClout2DProjection(
|
||
scanLines,
|
||
pntCloudPara.xRange,
|
||
pntCloudPara.yRange,
|
||
scale,
|
||
refPos->cuttingZ,
|
||
edgeSkip,
|
||
inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||
projectionImg,//投影量化数据，初始化为一个极大值1e+6
|
||
projectionIndexing //标记坐标索引，用于回找3D坐标
|
||
);
|
||
//距离变换
|
||
cv::Mat distTransform;
|
||
sg_distanceTrans(projectionImg, distTransform, 0);
|
||
#if OUTPUT_DEBUG //debug
|
||
cv::Mat maskImage;
|
||
cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
|
||
cv::imwrite("distTransformMask.png", maskImage);
|
||
cv::Mat dtImage;
|
||
cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
|
||
cv::Mat dtImage_color;
|
||
cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR);
|
||
cv::imwrite("distTransform.png", dtImage_color);
|
||
#endif
|
||
//提取目标，并计算粗略中心点
|
||
//对距离变换值进行截取，使目标不相边。
|
||
double distanceTh = (statorParam.statorOuterD - statorParam.statorInnerD) / 8;
|
||
cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols
|
||
for (int y = 0; y < maskY; y++)
|
||
{
|
||
for (int x = 0; x < maskX; x++)
|
||
{
|
||
if(distTransform.at<float>(y,x) > distanceTh)
|
||
bwImg.at<uchar>(y, x) = 1;
|
||
}
|
||
}
|
||
//连通域标注
|
||
cv::Mat labImg;
|
||
std::vector<SSG_Region> labelRgns;
|
||
SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
|
||
//根据大小提取目标
|
||
double objPixSize = statorParam.statorOuterD / scale;
|
||
int objSizeTh1 = (int)(objPixSize * 0.75);
|
||
int objSizeTh2 = (int)(objPixSize * 1.1);
|
||
std::vector<SSG_Region> objectRgns;
|
||
for (int i = 0, i_max = (int)labelRgns.size(); i < i_max; i++)
|
||
{
|
||
int width = labelRgns[i].roi.right - labelRgns[i].roi.left;
|
||
int height = labelRgns[i].roi.bottom - labelRgns[i].roi.top;
|
||
if ((width > objSizeTh1) && (width < objSizeTh2) &&
|
||
(height > objSizeTh1) && (height < objSizeTh2))
|
||
objectRgns.push_back(labelRgns[i]);
|
||
}
|
||
// 以中心点进行圆周扫描，并精确确定中心点
|
||
double scanR1 = (statorParam.statorInnerD / 2) * 0.75;
|
||
double scanR2 = (statorParam.statorOuterD / 2) * 1.25;
|
||
std::vector< SSG_peakRgnInfo> objects;
|
||
SSG_peakRgnInfo a_nullObj;
|
||
memset(&a_nullObj, 0, sizeof(SSG_peakRgnInfo));
|
||
objects.push_back(a_nullObj); //ID从1开始，保持ID与数组下标相同
|
||
int objNumber = 1;
|
||
for (int i = 0, i_max = (int)objectRgns.size(); i < i_max; i++)
|
||
{
|
||
SSG_Region& a_objRgn = objectRgns[i];
|
||
//y计算粗略中心
|
||
int x = (a_objRgn.roi.left + a_objRgn.roi.right) / 2;
|
||
int y = (a_objRgn.roi.top + a_objRgn.roi.bottom) / 2;
|
||
//圆周扫描定子环
|
||
std::vector<SVzNL2DPoint> peakPts;
|
||
circileScan(
|
||
distTransform, //扫描对象
|
||
x, y, //扫描中心
|
||
scanR1, scanR2, //扫描区域
|
||
peakPts //扫描结果
|
||
);
|
||
//计算中心（不进行拟合，使用几何中心）
|
||
double cx = 0, cy = 0, cz = 0;
|
||
int ptSize = (int)peakPts.size();
|
||
if (ptSize == 0)
|
||
continue;
|
||
|
||
for (int m = 0; m < ptSize; m++)
|
||
{
|
||
cx += (double)peakPts[m].x;
|
||
cy += (double)peakPts[m].y;
|
||
cz += projectionImg.at<float>(peakPts[m].y, peakPts[m].x);
|
||
}
|
||
cx = cx / (double)ptSize;
|
||
cy = cy / (double)ptSize;
|
||
cz = cz / (double)ptSize;
|
||
//
|
||
SSG_peakRgnInfo a_obj;
|
||
a_obj.pos2D = {(int)cy,(int)cx};
|
||
a_obj.centerPos.x = cx * scale + pntCloudPara.xRange.min;
|
||
a_obj.centerPos.y = cy * scale + pntCloudPara.yRange.min;
|
||
a_obj.centerPos.z = cz;
|
||
a_obj.pkRgnIdx = objNumber;
|
||
objNumber++;
|
||
objects.push_back(a_obj);
|
||
}
|
||
if (objNumber < 2)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ;
|
||
return;
|
||
}
|
||
//输出检测到的目标位置
|
||
for (int i = 1; i < objNumber; i++)
|
||
{
|
||
SWD_motorStatorPosition a_obj;
|
||
a_obj.objR = statorParam.statorOuterD / 2;
|
||
a_obj.opCenter.x = objects[i].centerPos.x;
|
||
a_obj.opCenter.y = objects[i].centerPos.y;
|
||
a_obj.opCenter.z = objects[i].centerPos.z;
|
||
resultObjPositions.push_back(a_obj);
|
||
}
|
||
|
||
//计算抓取点
|
||
//建立邻接关系,每个目标最多有6个邻接目标
|
||
std::vector< SSG_hexagonNeighbour> objLinkings;
|
||
objLinkings.resize(objNumber);//第一个不使用。目标ID从1开始，保持ID与数组下标一致
|
||
for (int i = 0; i < objNumber; i++)
|
||
{
|
||
objLinkings[i].ID = i;
|
||
objLinkings[i].linkNum = 0;
|
||
for (int j = 0; j < 6; j++)
|
||
objLinkings[i].neighborID[j] = -1;
|
||
}
|
||
objLinkings[0].linkNum = INT_MAX;
|
||
double linkPixDistTh = (statorParam.statorOuterD * 1.5) / scale;
|
||
for (int i = 1; i < objNumber; i++)
|
||
{
|
||
SSG_peakRgnInfo& obj_0 = objects[i];
|
||
for (int j = i + 1; j < objNumber; j++)
|
||
{
|
||
SSG_peakRgnInfo& obj_1 = objects[j];
|
||
double dy = obj_1.pos2D.y - obj_0.pos2D.y;
|
||
double dx = obj_1.pos2D.x - obj_0.pos2D.x;
|
||
double dist = sqrt(pow(dx, 2) + pow(dy, 2));
|
||
if (dist < linkPixDistTh)
|
||
{
|
||
double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360度
|
||
//60度为一扇区，0-59为扇区0， 60-119为扇区1
|
||
int angleIndx = (int)(angle+ 30.0)/ 60; //此处加30度，防止目标处于0度左右引起扇区归属临界。兼容定子水平和垂直排列
|
||
int angleIndx_1 = (angleIndx + 3) % 6;
|
||
objLinkings[i].neighborID[angleIndx] = j;
|
||
objLinkings[j].neighborID[angleIndx_1] = i;
|
||
}
|
||
}
|
||
}
|
||
|
||
//确定最佳抓取点
|
||
int objID = -1;
|
||
bool validGrasper = false;
|
||
if (refPos->refPos.z > 1e-4) //根据上一次给定的位置进行抓取
|
||
{
|
||
double minDist = -1;
|
||
int refx = (refPos->refPos.x - pntCloudPara.xRange.min) / scale;
|
||
int refy = (refPos->refPos.y - pntCloudPara.yRange.min) / scale;
|
||
for (int i = 1; i < objNumber; i++) //以最近的目标作为抓取目标
|
||
{
|
||
double dist = sqrt(pow(objects[i].pos2D.x-refx, 2) + pow(objects[i].pos2D.y - refy, 2));
|
||
if (minDist < 0)
|
||
{
|
||
minDist = dist;
|
||
objID = i;
|
||
}
|
||
else
|
||
{
|
||
if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
objID = i;
|
||
}
|
||
}
|
||
}
|
||
//计算抓取点
|
||
SWD_statorOuterGrasper outerGrasper;
|
||
double angleDistance;
|
||
bool validGrasper = computeOuterGraspPoint(
|
||
objID,
|
||
statorParam.statorOuterD / 2,
|
||
objects,
|
||
objLinkings,
|
||
outerGrasper,
|
||
&angleDistance
|
||
);
|
||
resultGraspPos = outerGrasper;
|
||
|
||
}
|
||
else
|
||
{
|
||
//统计相邻目标数量
|
||
int minLinkNum = -1;
|
||
for (int i = 1; i < objNumber; i++)
|
||
{
|
||
objLinkings[i].linkNum = 0;
|
||
for (int j = 0; j < 6; j++)
|
||
{
|
||
if (objLinkings[i].neighborID[j] >= 0)
|
||
objLinkings[i].linkNum++;
|
||
}
|
||
if (minLinkNum < 0)
|
||
minLinkNum = objLinkings[i].linkNum;
|
||
else
|
||
{
|
||
if (minLinkNum > objLinkings[i].linkNum)
|
||
minLinkNum = objLinkings[i].linkNum;
|
||
}
|
||
}
|
||
|
||
std::vector<int> objCandidates;
|
||
for (int i = 1; i < objNumber; i++)
|
||
{
|
||
if (minLinkNum = objLinkings[i].linkNum)
|
||
objCandidates.push_back(i);
|
||
}
|
||
|
||
//std::sort(objLinkings.begin(), objLinkings.end(), compareByNeighbourNumber);
|
||
double maxAngleInterval = -1;
|
||
SWD_statorOuterGrasper bestGrasper;
|
||
for (int i = 0, i_max=(int)objCandidates.size(); i < i_max; i++)
|
||
{
|
||
int id = objCandidates[i];
|
||
//计算抓取点
|
||
SWD_statorOuterGrasper outerGrasper;
|
||
double angleDistance;
|
||
bool validGrasper = computeOuterGraspPoint(
|
||
id,
|
||
statorParam.statorOuterD / 2,
|
||
objects,
|
||
objLinkings,
|
||
outerGrasper,
|
||
&angleDistance
|
||
);
|
||
if (true == validGrasper)
|
||
{
|
||
if (maxAngleInterval < 0)
|
||
{
|
||
maxAngleInterval = angleDistance;
|
||
bestGrasper = outerGrasper;
|
||
}
|
||
else
|
||
{
|
||
if (maxAngleInterval < angleDistance)
|
||
{
|
||
maxAngleInterval = angleDistance;
|
||
bestGrasper = outerGrasper;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
resultGraspPos = bestGrasper;
|
||
}
|
||
//给出下一个参考抓取点
|
||
int candiID = -1;
|
||
int candiLinkNum = -1;
|
||
for (int i = 0; i < 6; i++)
|
||
{
|
||
int neighborID = objLinkings[objID].neighborID[i];
|
||
if (neighborID >= 0)
|
||
{
|
||
int linkNum = objLinkings[neighborID].linkNum;
|
||
if (candiLinkNum < 0)
|
||
{
|
||
candiID = neighborID;
|
||
candiLinkNum = linkNum;
|
||
}
|
||
else
|
||
{
|
||
if (candiLinkNum > linkNum)
|
||
{
|
||
candiID = neighborID;
|
||
candiLinkNum = linkNum;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (candiID >= 0)
|
||
{
|
||
refPos->refPos.x = objects[candiID].centerPos.x;
|
||
refPos->refPos.y = objects[candiID].centerPos.y;
|
||
refPos->refPos.z = objects[candiID].centerPos.z;
|
||
}
|
||
else
|
||
{
|
||
refPos->refPos.x = 0;
|
||
refPos->refPos.y = 0;
|
||
refPos->refPos.z = -1;
|
||
}
|
||
return;
|
||
|
||
#if 0
|
||
//Z方向统计。粒度为1mm
|
||
int zHistSize = (int)(z_range.max - z_range.min) + 1;
|
||
std::vector<int> zHist;
|
||
zHist.resize(zHistSize);
|
||
int totalPtNum = 0;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
|
||
{
|
||
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
|
||
if (pt3D->pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
totalPtNum++;
|
||
int zPos = (int)(pt3D->pt3D.z - z_range.min);
|
||
zHist[zPos] ++;
|
||
}
|
||
}
|
||
//取定子顶面：取5mm统计区间，zHist数量大于总数量的10%
|
||
std::vector<int> sumHist;
|
||
sumHist.resize(zHistSize);
|
||
for (int i = 0; i < zHistSize; i++)
|
||
{
|
||
int data = 0;
|
||
for (int j = i - 2; j < i + 2; j++)
|
||
{
|
||
if ((j >= 0) && (j < zHistSize))
|
||
data += zHist[j];
|
||
}
|
||
sumHist[i] = data;
|
||
}
|
||
//取第一个大于大于10%的极值点
|
||
int dataTh = totalPtNum / 10;
|
||
int maxPos = -1;
|
||
for (int i = 1; i < zHistSize-1; i++)
|
||
{
|
||
int preData = sumHist[i-1];
|
||
int data = sumHist[i];
|
||
int nxtData = sumHist[i+1];
|
||
if ((data > preData) && (data > nxtData) && (data > dataTh))
|
||
{
|
||
maxPos = i;
|
||
break;
|
||
}
|
||
}
|
||
if (maxPos < 0)
|
||
return;
|
||
|
||
//取定子顶面截面
|
||
double zSliceTop = (double)maxPos - 10 + z_range.min;
|
||
double zSliceBtm = (double)maxPos + 8 + z_range.min;
|
||
|
||
//在XOY面上投影
|
||
//距离变换Mask，以1mm为量化尺度
|
||
int maskX = (int)(roi3D.xRange.max - roi3D.xRange.min) + 1;
|
||
int maskY = (int)(roi3D.yRange.max - roi3D.yRange.min) + 1;
|
||
cv::Mat zSliceData = cv::Mat::zeros(maskY, maskX, CV_8U);
|
||
cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 1e+6); //距离变换Mask，初始化为一个极大值1e+6
|
||
//标记坐标索引，用于距离变换后回找坐标
|
||
cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); //坐标索引
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
|
||
{
|
||
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
|
||
if ( (pt3D->pt3D.z < zSliceTop) || (pt3D->pt3D.z > zSliceBtm))
|
||
continue;
|
||
double x = pt3D->pt3D.x;
|
||
double y = pt3D->pt3D.y;
|
||
|
||
int px = (int)(x - roi3D.xRange.min);
|
||
int py = (int)(y - roi3D.yRange.min);
|
||
cv::Vec2i v2i = { line, i };
|
||
zSliceData.at<uchar>(py, px) = (uchar)255;
|
||
distTranformIndexing.at<cv::Vec2i>(py, px) = v2i;
|
||
distTranformMask.at<float>(py, px) = 0;
|
||
}
|
||
}
|
||
cv::Mat zSliceData_origin = zSliceData.clone();
|
||
|
||
//距离变换
|
||
cv::Mat distTransform;
|
||
sg_distanceTrans(distTranformMask, distTransform, 0);
|
||
#if DEBUG_OUT_IMAGE //debug
|
||
cv::Mat dtImage;
|
||
cv::normalize(distTranformMask, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
|
||
cv::imwrite("distTransform.png", dtImage);
|
||
//cv::normalize(zSliceData, dtImage, 64, 255, cv::NORM_MINMAX, CV_8U);
|
||
cv::imwrite("zSliceImage.png", zSliceData);
|
||
#endif
|
||
|
||
//对zSliceData进行处理：（1）填充（2）腐蚀（3）连通域处理得到目标（4）椭圆拟合得到中心点
|
||
cv::Mat invertSliceData;
|
||
cv::bitwise_not(zSliceData, invertSliceData);
|
||
#if DEBUG_OUT_IMAGE
|
||
cv::imwrite("zSliceImage_invert.png", invertSliceData);
|
||
#endif
|
||
//连通域
|
||
cv::Mat labels, centroids, stats;
|
||
int nccomps = connectedComponentsWithStats(invertSliceData, labels, stats, centroids, 4);
|
||
for (int i = 1; i < nccomps; i++)
|
||
{
|
||
int size = stats.at<int>(i, cv::CC_STAT_AREA);
|
||
if (size < 20)
|
||
{
|
||
int roi_x = stats.at<int>(i, cv::CC_STAT_LEFT);
|
||
int roi_y = stats.at<int>(i, cv::CC_STAT_TOP);
|
||
int roi_w = stats.at<int>(i, cv::CC_STAT_WIDTH);
|
||
int roi_h = stats.at<int>(i, cv::CC_STAT_HEIGHT);
|
||
//对小于20的区域填充
|
||
for (int row = roi_y; row < roi_y+roi_h; row++)
|
||
{
|
||
for (int col = roi_x; col < roi_x+roi_w; col++)
|
||
{
|
||
if (labels.at<int>(row, col) == i)
|
||
zSliceData.at<uchar>(row, col) = (uchar)255;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#if DEBUG_OUT_IMAGE
|
||
cv::imwrite("zSliceImage_filled.png", zSliceData);
|
||
#endif
|
||
|
||
//腐蚀
|
||
cv::Mat test = cv::Mat::zeros(64, 64, CV_8UC1);
|
||
cv::rectangle(test, cv::Rect(30, 30, 5, 5), 255, -1);
|
||
|
||
cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
|
||
cv::Mat zSliceData_erode;
|
||
cv::erode(zSliceData, zSliceData_erode, element, cv::Point(-1,-1),2);
|
||
#if DEBUG_OUT_IMAGE
|
||
cv::imwrite("zSliceImage_erode.png", zSliceData_erode);
|
||
#endif
|
||
//提取目标
|
||
nccomps = connectedComponentsWithStats(zSliceData_erode, labels, stats, centroids, 4);
|
||
//目标大小的门限
|
||
int obj_size_min = (int)(positionParam.statorInnerD * 0.75);
|
||
int obj_size_max = (int)(positionParam.statorOuterD * 1.25);
|
||
std::vector<SG_fittingInfo> fittingObs;
|
||
for (int i = 1; i < nccomps; i++)
|
||
{
|
||
int roi_x = stats.at<int>(i, cv::CC_STAT_LEFT);
|
||
int roi_y = stats.at<int>(i, cv::CC_STAT_TOP);
|
||
int roi_w = stats.at<int>(i, cv::CC_STAT_WIDTH);
|
||
int roi_h = stats.at<int>(i, cv::CC_STAT_HEIGHT);
|
||
if( (roi_w > obj_size_min) && (roi_w < obj_size_max) && (roi_h > obj_size_min) && (roi_h < obj_size_max))
|
||
{
|
||
//合格的目标
|
||
//取轮廓点
|
||
std::vector<cv::Point > contourPts;
|
||
for (int row = roi_y; row < roi_y + roi_h; row++)
|
||
{
|
||
for (int col = roi_x; col < roi_x + roi_w; col++)
|
||
{
|
||
if (labels.at<int>(row, col) == i)
|
||
contourPts.push_back(cv::Point(col, row));
|
||
}
|
||
}
|
||
cv::RotatedRect ellipse = cv::fitEllipse(contourPts);
|
||
SG_fittingInfo a_fitting;
|
||
a_fitting.fittingPara = ellipse;
|
||
//根据轮廓点计算高度Z
|
||
double z_sum = 0;
|
||
double z_counter = 0;
|
||
for (int m = 0; m < contourPts.size(); m++)
|
||
{
|
||
cv::Vec2i ptIndexing = distTranformIndexing.at<cv::Vec2i>(contourPts[m].y, contourPts[m].x);
|
||
int line = ptIndexing[0];
|
||
int ptIdx = ptIndexing[1];
|
||
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[ptIdx];
|
||
if (pt3D->pt3D.z < 1e-4)
|
||
continue;
|
||
z_sum += pt3D->pt3D.z;
|
||
z_counter++;
|
||
}
|
||
if (z_counter > 0)
|
||
{
|
||
a_fitting.objCenter.x = ellipse.center.x + roi3D.xRange.min;
|
||
a_fitting.objCenter.y = ellipse.center.y + roi3D.yRange.min;
|
||
z_sum = z_sum / z_counter;
|
||
a_fitting.objCenter.z = z_sum;
|
||
a_fitting.fittingPara = ellipse;
|
||
fittingObs.push_back(a_fitting);
|
||
}
|
||
|
||
}
|
||
}
|
||
#if DEBUG_OUT_IMAGE
|
||
cv::Mat fittingImage;
|
||
cv::cvtColor(zSliceData_origin, fittingImage, cv::COLOR_GRAY2BGR);
|
||
for(int i = 0; i < fittingObs.size(); i ++)
|
||
cv::ellipse(fittingImage, fittingObs[i].fittingPara, cv::Scalar(0, 255, 0), 2); // 在原图上绘制椭圆
|
||
cv::imwrite("zSliceImage_fitting.png", fittingImage);
|
||
#endif
|
||
//根据邻接关系确定最佳抓取点
|
||
std::vector< SSG_statorNeighbourInfo> neighbouringInfo;
|
||
_getNeighbouringInfo(
|
||
positionParam,
|
||
zSliceData_origin, //用于寻找边界（边框）
|
||
fittingObs, //目标位置
|
||
neighbouringInfo //邻接关系
|
||
);
|
||
|
||
//根据邻接关系确定抓取点
|
||
std::vector< SSG_motorStatorPosition> grabPoses; //小于等于3个相邻目标的抓取点（按距离排序）
|
||
std::vector< SSG_motorStatorPosition> grabPoses_2; //大于3个相邻目标的抓取点（按数量排序）
|
||
//grabPoses.resize(neighbouringInfo.size());
|
||
for (int i = 0; i < neighbouringInfo.size(); i++)
|
||
{
|
||
double opAngle = -1;
|
||
double obstacleDist = -1;
|
||
_computeGripperPose(neighbouringInfo[i], positionParam.gripperR, &opAngle, &obstacleDist);
|
||
SSG_motorStatorPosition opPos;
|
||
memset(&opPos, 0, sizeof(SSG_motorStatorPosition));
|
||
opPos.neighbourNum = neighbouringInfo[i].neighbours.size();
|
||
opPos.obstacleDist = obstacleDist;
|
||
opPos.opAngle = opAngle;
|
||
opPos.opCenter = neighbouringInfo[i].objPos3D;
|
||
if (opPos.neighbourNum <= 3)
|
||
grabPoses.push_back(opPos);
|
||
else
|
||
grabPoses_2.push_back(opPos);
|
||
}
|
||
|
||
//选取抓取点：（1）周边邻居最少（2）距离边的距离最大
|
||
std::sort(grabPoses.begin(), grabPoses.end(),compareByNeighbourDist);
|
||
std::sort(grabPoses_2.begin(), grabPoses_2.end(), compareByNeighbourNum);
|
||
//合并
|
||
grabPoses.insert(grabPoses.end(), grabPoses_2.begin(), grabPoses_2.end());
|
||
return;
|
||
#endif
|
||
} |