1506 lines
41 KiB
C++
1506 lines
41 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;
|
||
|
||
typedef struct
|
||
{
|
||
cv::RotatedRect fittingPara;
|
||
SVzNL3DPoint objCenter;
|
||
}SG_fittingInfo;
|
||
|
||
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;
|
||
}
|
||
|
||
//圆周扫描定子环
|
||
//以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;
|
||
}
|
||
|
||
//切割
|
||
void cloudPointsFilter_zRange(std::vector< std::vector<SVzNL3DPosition>>& scanLines, SVzNLRangeD zRange, std::vector< std::vector<SVzNL3DPosition>>& cutData)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
cutData.resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
int ptNum = (int)scanLines[line].size();
|
||
cutData[line].resize(ptNum);
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
SVzNL3DPosition a_pt = scanLines[line][i];
|
||
if ((a_pt.pt3D.z < zRange.min) || (a_pt.pt3D.z > zRange.max))
|
||
a_pt.pt3D.z = 0;
|
||
cutData[line][i] = a_pt;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对无点区聚类
|
||
void genCLusterMask(
|
||
std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
|
||
bool maskType_null_vldPt, //true: nullPt(z=0); false:valid Point
|
||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo)
|
||
{
|
||
int lineNum = (int)clusterSrcData.size();
|
||
if (lineNum == 0)
|
||
return;
|
||
int linePtNum = (int)clusterSrcData[0].size();
|
||
|
||
featureInfoMask.resize(lineNum);
|
||
feature3DInfo.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
featureInfoMask[i].resize(linePtNum);
|
||
feature3DInfo[i].resize(linePtNum);
|
||
}
|
||
//生成Mask
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SVzNL3DPosition>& lineData = clusterSrcData[line];
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
if( ((false == maskType_null_vldPt)&&(clusterSrcData[line][ptIdx].pt3D.z > 1e-4))||
|
||
((true == maskType_null_vldPt) && (clusterSrcData[line][ptIdx].pt3D.z < 1e-4)))
|
||
{
|
||
SSG_featureClusteringInfo a_mask;
|
||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||
a_mask.featurType = 1;
|
||
a_mask.lineIdx = line;
|
||
a_mask.ptIdx = ptIdx;
|
||
featureInfoMask[line][ptIdx] = a_mask;
|
||
feature3DInfo[line][ptIdx] = clusterSrcData[line][ptIdx].pt3D;
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//聚类:对无效点的聚类实际上是连通域处理
|
||
void cloutPointsClustering_nullPt(
|
||
std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
|
||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||
std::vector<SWD_clustersInfo>& clustersInfo)
|
||
{
|
||
int lineNum = (int)clusterSrcData.size();
|
||
if (lineNum == 0)
|
||
return;
|
||
int linePtNum = (int)clusterSrcData[0].size();
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
int clusterID = 1;
|
||
int clusterCheckWin = 5;
|
||
for (int y = 0; y < linePtNum; y++)
|
||
{
|
||
for (int x = 0; x < lineNum; x++)
|
||
{
|
||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||
SVzNL3DRangeD a_clusterRoi3D;
|
||
memset(&a_clusterRoi3D, 0, sizeof(SVzNL3DRangeD));
|
||
SVzNLRect clusterRoi2D;
|
||
clusterRoi2D.left = x;
|
||
clusterRoi2D.right = x;
|
||
clusterRoi2D.top = y;
|
||
clusterRoi2D.bottom = y;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
|
||
//使用聚类方法完成8连通连通域分析
|
||
wd_gridPointClustering_labelling(
|
||
featureInfoMask,
|
||
feature3DInfo,
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
clusterRoi2D //roi2D
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
SWD_clustersInfo a_info;
|
||
a_info.clusterIdx = clusterID;
|
||
a_info.ptSize = (int)a_cluster.size();
|
||
a_info.roi2D = clusterRoi2D;
|
||
a_info.roi3D = a_clusterRoi3D;
|
||
clustersInfo.push_back(a_info);
|
||
clusterID++;
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
void cloutPointsClustering(
|
||
std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
|
||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||
SSG_treeGrowParam growParam,
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||
std::vector<SWD_clustersInfo>& clustersInfo)
|
||
{
|
||
int lineNum = (int)clusterSrcData.size();
|
||
if (lineNum == 0)
|
||
return;
|
||
int linePtNum = (int)clusterSrcData[0].size();
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
int clusterID = 1;
|
||
int clusterCheckWin = 5;
|
||
for (int y = 0; y < linePtNum; y++)
|
||
{
|
||
for (int x = 0; x < lineNum; x++)
|
||
{
|
||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||
SVzNL3DRangeD a_clusterRoi;
|
||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
wd_gridPointClustering(
|
||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||
feature3DInfo,//double,记录坐标信息
|
||
clusterCheckWin, //搜索窗口
|
||
growParam,//聚类条件
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
SWD_clustersInfo a_info;
|
||
a_info.clusterIdx = clusterID;
|
||
a_info.ptSize = (int)a_cluster.size();
|
||
a_info.roi3D = a_clusterRoi;
|
||
clustersInfo.push_back(a_info);
|
||
clusterID++;
|
||
}
|
||
}
|
||
}
|
||
|
||
//生成点的Mask， 使用SSG_clusterLabel结构
|
||
void genValidPtLabelMask(
|
||
std::vector< std::vector<SVzNL3DPoint>>& srcData,
|
||
std::vector<std::vector<SSG_clusterLabel>>& labelMask,
|
||
EWD_maskMode maskMode)
|
||
{
|
||
int lineNum = (int)srcData.size();
|
||
labelMask.resize(lineNum);
|
||
for(int i = 0; i < lineNum; i ++)
|
||
{
|
||
int ptNum = (int)srcData[i].size();
|
||
labelMask[i].resize(ptNum);
|
||
std::fill(labelMask[i].begin(), labelMask[i].end(), SSG_clusterLabel{ 0,0,0 });
|
||
for (int j = 0; j < ptNum; j++)
|
||
{
|
||
if (KeWD_Mask_ValidPt == maskMode)
|
||
{
|
||
if (srcData[i][j].z > 1e-4)
|
||
labelMask[i][j].validFlag = 1;
|
||
}
|
||
else if(KeWD_Mask_NullPt == maskMode)
|
||
{
|
||
if (srcData[i][j].z < 1e-4)
|
||
labelMask[i][j].validFlag = 1;
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//生成点的Mask， 使用SSG_clusterLabel结构
|
||
int countQuantiSize(
|
||
std::vector< std::vector<SVzNL3DPoint>>& srcData,
|
||
EWD_maskMode maskMode)
|
||
{
|
||
int totalNum = 0;
|
||
int lineNum = (int)srcData.size();
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
int ptNum = (int)srcData[i].size();
|
||
for (int j = 0; j < ptNum; j++)
|
||
{
|
||
if (KeWD_Mask_ValidPt == maskMode)
|
||
{
|
||
if (srcData[i][j].z > 1e-4)
|
||
totalNum ++;
|
||
}
|
||
else if (KeWD_Mask_NullPt == maskMode)
|
||
{
|
||
if (srcData[i][j].z < 1e-4)
|
||
totalNum ++;
|
||
}
|
||
}
|
||
}
|
||
return totalNum;
|
||
}
|
||
|
||
//点云聚类
|
||
//SSG_intPair成员变量定义:
|
||
// data_0: flag
|
||
// data_1: valid point flag
|
||
// idx: cluster ID
|
||
void cloutPointsClustering_2(
|
||
std::vector< std::vector<SVzNL3DPoint>>& clusterSrcData,
|
||
std::vector<std::vector<SSG_clusterLabel>>& labelMask,
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||
std::vector<SWD_clustersInfo>& clustersInfo)
|
||
{
|
||
int lineNum = (int)clusterSrcData.size();
|
||
if (lineNum == 0)
|
||
return;
|
||
int linePtNum = (int)clusterSrcData[0].size();
|
||
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
int clusterID = 1;
|
||
int clusterCheckWin = 5;
|
||
for (int y = 0; y < linePtNum; y++)
|
||
{
|
||
for (int x = 0; x < lineNum; x++)
|
||
{
|
||
SSG_clusterLabel& a_seedInfo = labelMask[x][y];
|
||
if ((0 == a_seedInfo.validFlag) || (a_seedInfo.clusterID > 0)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint a_feature3DValue = clusterSrcData[x][y];
|
||
SVzNL3DRangeD a_clusterRoi;
|
||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
wd_gridPointClustering_labelling_2(
|
||
clusterSrcData,
|
||
labelMask,
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi //roi2D
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
SWD_clustersInfo a_info;
|
||
a_info.clusterIdx = clusterID;
|
||
a_info.ptSize = (int)a_cluster.size();
|
||
a_info.roi3D = a_clusterRoi;
|
||
clustersInfo.push_back(a_info);
|
||
clusterID++;
|
||
}
|
||
}
|
||
}
|
||
|
||
//根据孔洞大小过滤合格目标
|
||
void clusterSizeMatching(
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters,
|
||
std::vector<SWD_clustersInfo>& clustersInfo,
|
||
const SWD_statorParam statorParam,
|
||
std::vector<int>& validClusterIndice //合格目标的序号。此处使用序号，避免数据拷贝
|
||
)
|
||
{
|
||
double objSizeTh_min = statorParam.statorInnerD * 0.75;
|
||
double objSizeTh_max = statorParam.statorInnerD * 1.25;
|
||
int clusterNum = (int)clusters.size();
|
||
for (int i = 0; i < clusterNum; i++)
|
||
{
|
||
double xw = clustersInfo[i].roi3D.xRange.max - clustersInfo[i].roi3D.xRange.min;
|
||
double yw = clustersInfo[i].roi3D.yRange.max - clustersInfo[i].roi3D.yRange.min;
|
||
if ((xw >= objSizeTh_min) && (xw <= objSizeTh_max) && (yw >= objSizeTh_min) && (yw <= objSizeTh_max))
|
||
validClusterIndice.push_back(i);
|
||
}
|
||
return;
|
||
}
|
||
|
||
SSG_peakRgnInfo _computeObjInfo(int cluterIndex,
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters,
|
||
std::vector<SWD_clustersInfo>& clustersInfo,
|
||
std::vector<std::vector<SVzNL3DPoint>>& srcData,
|
||
const double quatiStatorR
|
||
)
|
||
{
|
||
SSG_peakRgnInfo a_obj;
|
||
memset(&a_obj, 0, sizeof(SSG_peakRgnInfo));
|
||
a_obj.pkRgnIdx = -1;
|
||
|
||
int counterNum = (int)clusters[cluterIndex].size();
|
||
if (counterNum == 0)
|
||
return a_obj;
|
||
|
||
SVzNL3DPoint centerPos = { 0, 0, 0 };
|
||
SVzNL2DPoint center2D = { 0, 0 };
|
||
|
||
for (int i = 0; i < counterNum; i++)
|
||
{
|
||
SVzNL2DPoint& a_pt = clusters[cluterIndex][i];
|
||
center2D.x += a_pt.x;
|
||
center2D.y += a_pt.y;
|
||
centerPos.x += srcData[a_pt.x][a_pt.y].x;
|
||
centerPos.y += srcData[a_pt.x][a_pt.y].y;
|
||
}
|
||
|
||
a_obj.pkRgnIdx = cluterIndex;
|
||
a_obj.pos2D.x = center2D.x / counterNum;
|
||
a_obj.pos2D.y = center2D.y / counterNum;
|
||
|
||
//计算Z：取周围的Z的最小值
|
||
int winR = (int)quatiStatorR + 1;
|
||
int xMax = srcData.size();
|
||
int yMax = srcData[0].size();
|
||
double cx = a_obj.pos2D.x;
|
||
double cy = a_obj.pos2D.y;
|
||
double minZ = DBL_MAX;
|
||
for (int x = a_obj.pos2D.x - winR; x <= a_obj.pos2D.x + winR; x++)
|
||
{
|
||
for (int y = a_obj.pos2D.y - winR; y <= a_obj.pos2D.y + winR; y++)
|
||
{
|
||
if ((x >= 0) && (x < xMax) && (y >= 0) && (y < yMax))
|
||
{
|
||
double dx = x;
|
||
double dy = y;
|
||
double dist = sqrt(pow(dx - cx, 2) + pow(dy - cy, 2));
|
||
if (dist < quatiStatorR)
|
||
{
|
||
if ((srcData[x][y].z > 1e-4) && (minZ > srcData[x][y].z))
|
||
{
|
||
minZ = srcData[x][y].z;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
a_obj.centerPos.x = centerPos.x / counterNum;
|
||
a_obj.centerPos.y = centerPos.y / counterNum;
|
||
a_obj.centerPos.z = minZ;
|
||
|
||
a_obj.objSize.dWidth = clustersInfo[cluterIndex].roi3D.xRange.max - clustersInfo[cluterIndex].roi3D.xRange.min;
|
||
a_obj.objSize.dHeight = clustersInfo[cluterIndex].roi3D.yRange.max - clustersInfo[cluterIndex].roi3D.yRange.min;
|
||
return a_obj;
|
||
}
|
||
|
||
void computeBestObject(
|
||
std::vector<SSG_peakRgnInfo>& objectRgns,
|
||
const SWD_statorParam statorParam,
|
||
SWD_statorGriperState* opState,
|
||
std::vector<SWD_statorInnerGrasper>& resultObjPositions)
|
||
{
|
||
int objNumber = (int)objectRgns.size();
|
||
//计算抓取点
|
||
//建立邻接关系,每个目标最多有6个邻接目标
|
||
std::vector< int> objLinkings;
|
||
objLinkings.resize(objNumber);//第一个不使用。目标ID从1开始，保持ID与数组下标一致
|
||
for (int i = 0; i < objNumber; i++)
|
||
{
|
||
objLinkings[i] = 0;
|
||
}
|
||
double linkPixDistTh = statorParam.statorOuterD * 1.5;
|
||
for (int i = 0; i < objNumber; i++)
|
||
{
|
||
SSG_peakRgnInfo& obj_0 = objectRgns[i];
|
||
for (int j = i + 1; j < objNumber; j++)
|
||
{
|
||
SSG_peakRgnInfo& obj_1 = objectRgns[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)
|
||
{
|
||
objLinkings[i]++;
|
||
objLinkings[j]++;
|
||
}
|
||
}
|
||
}
|
||
|
||
//确定最佳抓取点
|
||
if (opState->refPos.z > 1e-4) //根据上一次给定的位置进行抓取
|
||
{
|
||
int objID = -1;
|
||
double minDist = -1;
|
||
double refx = opState->refPos.x;
|
||
double refy = opState->refPos.y;
|
||
for (int i = 0; i < objNumber; i++) //以最近的目标作为抓取目标
|
||
{
|
||
double dist = sqrt(pow(objectRgns[i].centerPos.x - refx, 2) + pow(objectRgns[i].centerPos.y - refy, 2));
|
||
if (minDist < 0)
|
||
{
|
||
minDist = dist;
|
||
objID = i;
|
||
}
|
||
else
|
||
{
|
||
if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
objID = i;
|
||
}
|
||
}
|
||
}
|
||
SWD_statorInnerGrasper innerGrasper;
|
||
innerGrasper.objID = objID;
|
||
innerGrasper.opCenter = objectRgns[objID].centerPos;
|
||
resultObjPositions.push_back(innerGrasper);
|
||
//将其它目标列出，用于Debug，检查其它目标是否准确
|
||
for (int i = 0; i < objNumber; i++)
|
||
{
|
||
if (i != objID)
|
||
{
|
||
innerGrasper.objID = i;
|
||
innerGrasper.opCenter = objectRgns[i].centerPos;
|
||
resultObjPositions.push_back(innerGrasper);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//统计相邻目标数量
|
||
int minLinkNum = -1;
|
||
int minLindID = -1;
|
||
for (int i = 0; i < objNumber; i++)
|
||
{
|
||
|
||
if (minLinkNum < 0)
|
||
{
|
||
minLinkNum = objLinkings[i];
|
||
minLindID = i;
|
||
}
|
||
else
|
||
{
|
||
if (minLinkNum > objLinkings[i])
|
||
{
|
||
minLinkNum = objLinkings[i];
|
||
minLindID = i;
|
||
}
|
||
}
|
||
}
|
||
//将相邻目标数量最少的目标定义为抓取目标
|
||
SWD_statorInnerGrasper innerGrasper;
|
||
innerGrasper.objID = minLindID;
|
||
innerGrasper.opCenter = objectRgns[minLindID].centerPos;
|
||
resultObjPositions.push_back(innerGrasper);
|
||
//将其它目标列出，用于Debug，检查其它目标是否准确
|
||
for (int i = 0; i < objNumber; i++)
|
||
{
|
||
if (i != minLindID)
|
||
{
|
||
innerGrasper.objID = i;
|
||
innerGrasper.opCenter = objectRgns[i].centerPos;
|
||
resultObjPositions.push_back(innerGrasper);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void getDistanceTransformMaskData(
|
||
std::vector<std::vector<SVzNL3DPoint>>& quantiData,
|
||
cv::Mat& projectionData//投影量化数据，初始化为一个极大值1e+6
|
||
)
|
||
{
|
||
for (int row = 0; row < (int)quantiData.size(); row++)
|
||
{
|
||
for (int col = 0; col < (int)quantiData[row].size(); col++)
|
||
{
|
||
if(quantiData[row][col].z > 1e-4)
|
||
projectionData.at<float>(row, col) = 1e+6;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//电机定子定位。
|
||
//输出：（1）定子目标（2）隔板抓取点
|
||
//算法逻辑：找到定子的高度->设置截取Z去掉底面->
|
||
// 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
|
||
// 根据相邻和边框寻找最佳抓取目标和抓取点
|
||
//使用状态机记忆每一步操作。这种状态机适用于料筐中多层工件的按层抓取
|
||
void wd_motorStatorPosition(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const SWD_statorParam statorParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SWD_statorPositonParam algoParam,
|
||
SWD_statorGriperState* opState, //操作状态机。指示当前状态
|
||
int* errCode,
|
||
std::vector<SWD_statorInnerGrasper>& resultObjPositions)
|
||
{
|
||
int lineNum = (int)scanLinesInput.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
|
||
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算法
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
|
||
//噪点过滤
|
||
wd_noiseFilter(scanLines, algoParam.filterParam, errCode);
|
||
if (*errCode != 0)
|
||
return;
|
||
|
||
///开始数据处理
|
||
double cutZ_topLevel = groundCalibPara.planeHeight - statorParam.plateThickness - statorParam.statorHeight * 1.5;
|
||
double cutZ_btmLevel = groundCalibPara.planeHeight - statorParam.statorHeight * 0.5;
|
||
//顶层数据
|
||
std::vector< std::vector<SVzNL3DPosition>> cutData_topLevel;
|
||
cutData_topLevel.resize(lineNum);
|
||
//底层数据
|
||
std::vector< std::vector<SVzNL3DPosition>> cutData_btmLevel;
|
||
cutData_btmLevel.resize(lineNum);
|
||
|
||
int ptNum_total = 0;
|
||
int ptNum_topLevel = 0;
|
||
int ptNum_btmLevel = 0;
|
||
|
||
//使用cutZ_level进行切割
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
//top level
|
||
cutData_topLevel[line].resize(linePtNum);
|
||
std::fill(cutData_topLevel[line].begin(), cutData_topLevel[line].end(), SVzNL3DPosition{ 0,{0,0,0} });
|
||
//bottom level
|
||
cutData_btmLevel[line].resize(linePtNum);
|
||
std::fill(cutData_btmLevel[line].begin(), cutData_btmLevel[line].end(), SVzNL3DPosition{ 0,{0,0,0} });
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
ptNum_total++;
|
||
if (scanLines[line][j].pt3D.z < cutZ_topLevel)
|
||
{
|
||
cutData_topLevel[line][j] = scanLines[line][j];
|
||
ptNum_topLevel++;
|
||
}
|
||
if (scanLines[line][j].pt3D.z < cutZ_btmLevel)
|
||
{
|
||
cutData_btmLevel[line][j] = scanLines[line][j];
|
||
ptNum_btmLevel++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
double quatiScale = 1.0;
|
||
double quatiStatorR = (statorParam.statorOuterD / 2.0) / quatiScale;
|
||
int quatiEdgeSkip = 8;
|
||
double inerPolateDistTh = quatiScale * 3;
|
||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
|
||
#if 0
|
||
//对全图进行量化，作为层判断基础
|
||
std::vector<std::vector<SVzNL3DPoint>> quantiData_all;
|
||
std::vector<std::vector<SVzNL2DPoint>> backIndexing_all;
|
||
pointCloud2DQuantization(
|
||
scanLines,
|
||
roi3D.xRange,
|
||
roi3D.yRange,
|
||
quatiScale,
|
||
quatiEdgeSkip,
|
||
inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||
quantiData_all, //量化数据，初始化为一个极大值1e+6
|
||
backIndexing_all //标记坐标索引，用于回找3D坐标
|
||
);
|
||
int totalQuantiSize = countQuantiSize(quantiData_all, KeWD_Mask_ValidPt);
|
||
#endif
|
||
|
||
bool topLevelIsEmpty = false;
|
||
if ((keSG_OPERATE_BTM_LAYER == opState->opState) ||
|
||
(keSG_OPERATE_PLATE == opState->opState))
|
||
topLevelIsEmpty = true;
|
||
|
||
if ((keWD_OPERATE_Uknown == opState->opState) ||
|
||
(keSG_OPERATE_TOP_LAYER == opState->opState)) //当状态机为未知状态或处于上层抓取时，对顶层进行量化
|
||
{
|
||
//对顶层进行量化
|
||
//在量化图像上处理，否则空间处没有坐标，无法确定空间区域的面积，无法确定孔的大小
|
||
std::vector<std::vector<SVzNL3DPoint>> quantiData_topLevel;
|
||
std::vector<std::vector<SVzNL2DPoint>> backIndexing_topLevel;
|
||
pointCloud2DQuantization(
|
||
cutData_topLevel,
|
||
roi3D.xRange,
|
||
roi3D.yRange,
|
||
quatiScale,
|
||
quatiEdgeSkip,
|
||
inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||
quantiData_topLevel, //量化数据，初始化为一个极大值1e+6
|
||
backIndexing_topLevel //标记坐标索引，用于回找3D坐标
|
||
);
|
||
int topLevelQuantiSize = countQuantiSize(quantiData_topLevel, KeWD_Mask_ValidPt);
|
||
double ringArea = (pow(statorParam.statorOuterD, 2) - pow(statorParam.statorInnerD, 2)) * PI * 0.25;
|
||
if (topLevelQuantiSize < (ringArea * 0.5))
|
||
{
|
||
topLevelIsEmpty = true;
|
||
}
|
||
|
||
//顶层有点云数据
|
||
if (false == topLevelIsEmpty)
|
||
{
|
||
//生成点的Mask， 使用SSG_clusterLabel结构
|
||
std::vector<std::vector<SSG_clusterLabel>> labelMask_topLevel;
|
||
genValidPtLabelMask(quantiData_topLevel, labelMask_topLevel, KeWD_Mask_NullPt); //目的：提取孔点
|
||
//
|
||
//使用聚类方法进行连通域标记
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters;
|
||
std::vector<SWD_clustersInfo> clustersInfo;
|
||
cloutPointsClustering_2(
|
||
quantiData_topLevel,
|
||
labelMask_topLevel,
|
||
clusters, //只记录位置
|
||
clustersInfo);
|
||
|
||
if (clusters.size() == 1) //如果只有一个目标，说明整个topLevel都是空点，上层为空
|
||
{
|
||
topLevelIsEmpty = true;
|
||
}
|
||
else
|
||
{
|
||
//过滤目标
|
||
std::vector<int> validClusterIndice;//合格目标的序号。此处使用序号，避免数据拷贝
|
||
//根据大小提取目标
|
||
clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //合格目标的序号。此处使用序号，避免数据拷贝
|
||
if (validClusterIndice.size() == 0) //没有找到目标(此时有点云数据，因此报错）
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
std::vector<SSG_peakRgnInfo> objectRgns;
|
||
int objNum = (int)validClusterIndice.size();
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
//计算目标位置
|
||
SSG_peakRgnInfo a_obj = _computeObjInfo(
|
||
validClusterIndice[i],
|
||
clusters,
|
||
clustersInfo,
|
||
quantiData_topLevel,
|
||
quatiStatorR
|
||
);
|
||
if (a_obj.pkRgnIdx < 0)
|
||
continue;
|
||
|
||
objectRgns.push_back(a_obj);
|
||
}
|
||
if (objectRgns.size() == 0) //没有找到目标(此时有点云数据，因此报错）
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
|
||
return;
|
||
}
|
||
|
||
//生成结果
|
||
computeBestObject(
|
||
objectRgns,
|
||
statorParam,
|
||
opState,
|
||
resultObjPositions
|
||
);
|
||
|
||
//更新状态机
|
||
opState->refPos.x = resultObjPositions[0].opCenter.x;
|
||
opState->refPos.y = resultObjPositions[0].opCenter.y;
|
||
opState->refPos.z = resultObjPositions[0].opCenter.z;
|
||
opState->opState = keSG_OPERATE_TOP_LAYER;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//顶层为空，处理底层
|
||
if (true == topLevelIsEmpty)
|
||
{
|
||
//对底层进行处理
|
||
//在量化图像上处理，否则空间处没有坐标，无法确定空间区域的面积，无法确定孔的大小
|
||
std::vector<std::vector<SVzNL3DPoint>> quantiData_btmLevel;
|
||
std::vector<std::vector<SVzNL2DPoint>> backIndexing_btmLevel;
|
||
pointCloud2DQuantization(
|
||
cutData_btmLevel,
|
||
roi3D.xRange,
|
||
roi3D.yRange,
|
||
quatiScale,
|
||
quatiEdgeSkip,
|
||
inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||
quantiData_btmLevel, //量化数据，初始化为一个极大值1e+6
|
||
backIndexing_btmLevel //标记坐标索引，用于回找3D坐标
|
||
);
|
||
int bmtLevelQuantiSize = countQuantiSize(quantiData_btmLevel, KeWD_Mask_ValidPt);
|
||
double ringArea = (pow(statorParam.statorOuterD, 2) - pow(statorParam.statorInnerD, 2)) * PI * 0.25;
|
||
if (bmtLevelQuantiSize < (ringArea * 0.5))
|
||
{
|
||
//空框
|
||
*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
|
||
return;
|
||
}
|
||
|
||
//距离变换判断隔板
|
||
if (opState->opState != keSG_OPERATE_BTM_LAYER)
|
||
{
|
||
int maskRow = quantiData_btmLevel.size();
|
||
int maskCol = quantiData_btmLevel[0].size();
|
||
cv::Mat projectionImg(maskRow, maskCol, CV_32FC1, 0.0f); //距离变换Mask，初始化为一个极大值1e+6
|
||
getDistanceTransformMaskData(quantiData_btmLevel, projectionImg ); // 投影量化数据，初始化为一个极大值1e + 6
|
||
cv::Mat distTransform;
|
||
sg_distanceTrans(projectionImg, distTransform, 0);
|
||
#if DEBUG_OUT_IMAGE //debug
|
||
cv::Mat maskImage;
|
||
cv::normalize(projectionImg, 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 distancePeakTh = statorParam.plateH < statorParam.plateW ? statorParam.plateH : statorParam.plateW;
|
||
distancePeakTh = distancePeakTh / quatiScale;
|
||
distancePeakTh = (distancePeakTh / 2) * 0.75;
|
||
double maxDistTransformValue = 0;
|
||
for (int y = 0; y < maskRow; y++)
|
||
{
|
||
for (int x = 0; x < maskCol; x++)
|
||
{
|
||
if (distTransform.at<float>(y, x) > maxDistTransformValue)
|
||
maxDistTransformValue = distTransform.at<float>(y, x);
|
||
}
|
||
}
|
||
if (maxDistTransformValue > distancePeakTh)
|
||
{
|
||
cv::Mat bwImg = cv::Mat::zeros(maskRow, maskCol, CV_8UC1);//rows, cols
|
||
for (int row = 0; row < maskRow; row++)
|
||
{
|
||
for (int col = 0; col < maskCol; col++)
|
||
{
|
||
if (quantiData_btmLevel[row][col].z > 1e-4)
|
||
bwImg.at<uchar>(row, col) = 1;
|
||
}
|
||
}
|
||
//连通域标注
|
||
cv::Mat labImg;
|
||
std::vector<SSG_Region> labelRgns;
|
||
SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
|
||
//将最大目标的中心作为隔板抓取中心点
|
||
SSG_Region maxRgn;
|
||
memset(&maxRgn, 0, sizeof(SSG_Region));
|
||
int maxId = -1;
|
||
for (int i = 0; i < (int)labelRgns.size(); i++)
|
||
{
|
||
if (maxRgn.ptCounter < labelRgns[i].ptCounter)
|
||
{
|
||
maxRgn = labelRgns[i];
|
||
maxId = i;
|
||
}
|
||
}
|
||
if (maxId < 0)
|
||
{
|
||
//隔板提取错误
|
||
*errCode = SX_ERR_GET_INVALID_PALTE;
|
||
return;
|
||
}
|
||
|
||
double plate_cx = (double)(labelRgns[maxId].roi.right + labelRgns[maxId].roi.left) / 2;
|
||
double plate_cy = (double)(labelRgns[maxId].roi.bottom + labelRgns[maxId].roi.top) / 2;
|
||
int cx_i = (int)plate_cx;
|
||
int cy_i = (int)plate_cy;
|
||
double op_z = quantiData_btmLevel[cx_i][cy_i].z;
|
||
plate_cx = (plate_cx - quatiEdgeSkip) * quatiScale + roi3D.xRange.min;
|
||
plate_cy = (plate_cy - quatiEdgeSkip) * quatiScale + roi3D.yRange.min;
|
||
|
||
SWD_statorInnerGrasper plateGrasper;
|
||
plateGrasper.objID = 0;
|
||
plateGrasper.opCenter = { plate_cx , plate_cy, op_z , 0,0,0 };
|
||
resultObjPositions.push_back(plateGrasper);
|
||
opState->opState = keSG_OPERATE_PLATE;
|
||
opState->refPos = { 0,0,0 };
|
||
return;
|
||
}
|
||
}
|
||
|
||
//处理下层目标
|
||
//生成点的Mask， 使用SSG_clusterLabel结构
|
||
std::vector<std::vector<SSG_clusterLabel>> labelMask_btmLevel;
|
||
genValidPtLabelMask(quantiData_btmLevel, labelMask_btmLevel, KeWD_Mask_NullPt); //目的：提取孔点
|
||
//使用聚类方法进行连通域标记
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters;
|
||
std::vector<SWD_clustersInfo> clustersInfo;
|
||
cloutPointsClustering_2(
|
||
quantiData_btmLevel,
|
||
labelMask_btmLevel,
|
||
clusters, //只记录位置
|
||
clustersInfo);
|
||
|
||
if (clusters.size() == 1) //如果只有一个目标，说明整个topLevel都是空点，上层为空
|
||
{
|
||
//空框
|
||
*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
//过滤目标
|
||
std::vector<int> validClusterIndice;//合格目标的序号。此处使用序号，避免数据拷贝
|
||
//根据大小提取目标
|
||
clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //合格目标的序号。此处使用序号，避免数据拷贝
|
||
if (validClusterIndice.size() == 0) //没有找到目标(此时有点云数据，因此报错）
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
std::vector<SSG_peakRgnInfo> objectRgns;
|
||
int objNum = (int)validClusterIndice.size();
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
//计算目标位置
|
||
SSG_peakRgnInfo a_obj = _computeObjInfo(
|
||
validClusterIndice[i],
|
||
clusters,
|
||
clustersInfo,
|
||
quantiData_btmLevel,
|
||
quatiStatorR
|
||
);
|
||
if (a_obj.pkRgnIdx < 0)
|
||
continue;
|
||
|
||
objectRgns.push_back(a_obj);
|
||
}
|
||
if (objectRgns.size() == 0) //没有找到目标(此时有点云数据，因此报错）
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
|
||
return;
|
||
}
|
||
|
||
//生成结果
|
||
computeBestObject(
|
||
objectRgns,
|
||
statorParam,
|
||
opState,
|
||
resultObjPositions);
|
||
|
||
//更新状态机
|
||
opState->refPos.x = resultObjPositions[0].opCenter.x;
|
||
opState->refPos.y = resultObjPositions[0].opCenter.y;
|
||
opState->refPos.z = resultObjPositions[0].opCenter.z;
|
||
opState->opState = keSG_OPERATE_BTM_LAYER;
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
} |