jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/sourceCode/motorStatorPosition.cpp

1225 lines
33 KiB
C++
Raw Normal View History

source code
2025-06-08 10:46:41 +08:00
#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]; //<2F><>ӦL<D3A6><4C>R<EFBFBD><52>T<EFBFBD><54>P
}SSG_statorNeighbourInfo;
合并Mark检测
2025-12-05 23:08:46 +08:00
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();
}
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara wd_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
return sg_getPlaneCalibPara2(scanLines);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
source code
2025-06-08 10:46:41 +08:00
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;
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
return sideInfo;
source code
2025-06-08 10:46:41 +08:00
}
void _getNeighbouringInfo(
合并Mark检测
2025-12-05 23:08:46 +08:00
const SWD_statorParam positionParam,
source code
2025-06-08 10:46:41 +08:00
cv::Mat& zSliceData, //<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD>ұ߽磨<DFBD>߿<EFBFBD><DFBF><EFBFBD>
std::vector<SG_fittingInfo>& Objects, //Ŀ<><C4BF>λ<EFBFBD><CEBB>
std::vector< SSG_statorNeighbourInfo>& neighbouringInfo //<2F>ڽӹ<DABD>ϵ
)
{
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)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
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; //ת<><D7AA><EFBFBD>ɽǶ<C9BD>
if (angle < 0)
angle += 360; //ת<><D7AA><EFBFBD><EFBFBD>0-360<36>ȷ<EFBFBD>Χ
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);
}
}
}
//<2F><><EFBFBD><EFBFBD>neighbour<75><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>side<64><65><EFBFBD><EFBFBD>
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;//<2F><><EFBFBD>߽<EFBFBD>
dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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;//<2F><><EFBFBD>߽<EFBFBD>
dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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;//<2F><><EFBFBD>߽<EFBFBD>
dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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;//<2F><><EFBFBD>߽<EFBFBD>
dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>ץȡ<D7A5><C8A1>
//Բ<><D4B2>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
double searchStepping = 0.5; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0.5<EFBFBD><EFBFBD>
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);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ͱ߽<CDB1>ͳһ<CDB3><D2BB>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double min_dist = _getMinDist(neighbouringInfo, testPts);
//<2F><>ȡ<EFBFBD><C8A1>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>Ӧ<EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
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;
}
合并Mark检测
2025-12-05 23:08:46 +08:00
bool compareByNeighbourNumber(const SSG_hexagonNeighbour& a, const SSG_hexagonNeighbour& b)
source code
2025-06-08 10:46:41 +08:00
{
合并Mark检测
2025-12-05 23:08:46 +08:00
return a.linkNum < b.linkNum;
source code
2025-06-08 10:46:41 +08:00
}
typedef struct
{
cv::RotatedRect fittingPara;
SVzNL3DPoint objCenter;
}SG_fittingInfo;
合并Mark检测
2025-12-05 23:08:46 +08:00
//Բ<><D4B2>ɨ<EFBFBD>趨<EFBFBD>ӻ<EFBFBD>
//<2F><>scanCenterΪɨ<CEAA><C9A8><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>Բ<EFBFBD><D4B2>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>scanR1<52><31>scanR2<52><32>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void circileScan(
cv::Mat& scanImg, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int scanCenterX, int scanCenterY, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double scanR1, double scanR2, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SVzNL2DPoint>& peakPts //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
)
{
//<2F><>1<EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>ɨ<EFBFBD><C9A8>
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<61>㷨
std::vector<SVzNL2DPoint> pts;
drawLine(x0, y0, x1, y1, pts);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
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);
}
}
//<2F>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>Ȧץȡ<D7A5><C8A1><EFBFBD><EFBFBD>צץ<D7A6>ּ<EFBFBD><D6BC><EFBFBD><EFBFBD><EFBFBD>תλ<D7AA>á<EFBFBD>
//<2F><>צ<EFBFBD>Գƣ<D4B3><C6A3>ֱ<EFBFBD><D6B1><EFBFBD>30,150,270<37>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ת<EFBFBD>Ƕ<EFBFBD>
double _computeGrasperRotateAngle(double grasperAngle)
{
double rotateAngle = 0;
if ((grasperAngle >= 330) || (grasperAngle < 90)) //Ѿ<><D1BE>һ<EFBFBD><D2BB>֦λ<D6A6><CEBB>30<33><30>
{
if (grasperAngle < 90)
rotateAngle = grasperAngle - 30;
else
rotateAngle = grasperAngle - 360 - 30;
}
else if ((grasperAngle >= 90) && (grasperAngle < 210))//Ѿ<>ڶ<EFBFBD><DAB6><EFBFBD>֦λ<D6A6><CEBB>150<35><30>
rotateAngle = grasperAngle - 150;
else
rotateAngle = grasperAngle - 270;
return rotateAngle;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵļ<C7B5><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0-180<38><30>Χ
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ĽǶȼ<C7B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>СֵΪ<D6B5><CEAA><EFBFBD><EFBFBD>
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;
}
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5>Ƕȡ<C7B6>ץ<EFBFBD><D7A5>Ϊ<EFBFBD><CEAA>צ<EFBFBD><D7A6><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD>120<32>ȡ<EFBFBD>
//ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĽǶȼ<C7B6><C8BC><EFBFBD><EFBFBD>㡣<EFBFBD><E3A1A3>λ<EFBFBD><CEBB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ץȡλ<C8A1><CEBB>
double _scanBestGrasperAngle(std::vector<double>& linkObjAngles, double* angleDistance)
{
*angleDistance = 0;
if (linkObjAngles.size() == 0)
return -1;
//<2F><>0.1<EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><EFBFBD>120<EFBFBD>ȷ<EFBFBD>Χ
double maxAngleDist = -1;
double bestAngleIdx = -1;
//<2F>ֶ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>1<EFBFBD><31>
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;
//<2F><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB> <20><>ȷ<EFBFBD><C8B7>0.1<EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD>ץȡλ<C8A1><CEBB>
if (a_link.linkNum == 0) //Ĭ<><C4AC>ץȡλ<C8A1><CEBB>
{
graspPoint.objID = objID;
graspPoint.grasperAngle = 0;
graspPoint.rotateAngle = 0;
graspPoint.graspR = 1.2 * outerR;
*angleDistance = 120;
return true;
}
else if (a_link.linkNum == 1) //<2F><>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ƫ<EFBFBD><C6AB>60<36><30>
{
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<36><30>
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)) //Ŀ<><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
{
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<36><30>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӷ<EFBFBD>λ<EFBFBD><CEBB>
//<2F>㷨<EFBFBD>߼<EFBFBD><DFBC><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>ӵĸ߶<C4B8>-><3E><><EFBFBD>ý<EFBFBD>ȡZȥ<5A><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>->
// ͶӰ<CDB6><D3B0>ע<EFBFBD><D7A2><EFBFBD><EFBFBD>ʱ<EFBFBD>߿<EFBFBD>ҲͬʱͶӰ<CDB6><D3B0>-><3E><><EFBFBD><EFBFBD><EFBFBD>任-><3E><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>->
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںͱ߿<CDB1>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡĿ<C8A1><C4BF><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
void wd_motorStatorPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_statorParam statorParam,
const SSG_planeCalibPara groundCalibPara,
const SWD_statorPositonParam algoParam,
SWD_nextOpParam* refPos, //<2F><>һ<EFBFBD>θ<EFBFBD><CEB8><EFBFBD><EFBFBD>IJο<C4B2>λ<EFBFBD>ã<EFBFBD>ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>εIJο<C4B2>λ<EFBFBD><CEBB>
source code
2025-06-08 10:46:41 +08:00
int* errCode,
合并Mark检测
2025-12-05 23:08:46 +08:00
std::vector<SWD_motorStatorPosition>& resultObjPositions,
SWD_statorOuterGrasper& resultGraspPos
source code
2025-06-08 10:46:41 +08:00
)
{
合并Mark检测
2025-12-05 23:08:46 +08:00
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_noiseFilter(scanLines, algoParam.filterParam, errCode);
if (*errCode != 0)
return;
///<2F><>ʼ<EFBFBD><CABC><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
double statorRingWidth = (statorParam.statorOuterD - statorParam.statorInnerD) / 2;
if (refPos->cuttingZ < 0) //<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϰ벿<CFB0>Ļ<EFBFBD><C4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>Ļ<EFBFBD><C4BB>ж϶<D0B6><CFB6>Ӹ߶<D3B8>
{
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
std::vector<std::vector<SWD_segFeature>> all_vLineArcs;
for (int i = 0; i < lineNum; i++)
{
if (i == 202)
int k = 1;
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>ӻ<EFBFBD>
std::vector<SWD_segFeature> line_ringArcs;
wd_getRingArcFeature(
scanLines[i],
i,
algoParam.cornerParam,
statorRingWidth, //<2F><><EFBFBD>ӵĻ<D3B5><C4BB><EFBFBD><EFBFBD><EFBFBD>
line_ringArcs //<2F><>
);
all_vLineArcs.push_back(line_ringArcs); //<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD>跽<EFBFBD><E8B7BD><EFBFBD><EFBFBD>
std::vector<SWD_segFeatureTree> v_seg_trees;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_getSegFeatureGrowingTrees(
all_vLineArcs,
v_seg_trees,
algoParam.growParam);
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>߶<EFBFBD>
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;
}
//ʹ<><CAB9> refPos->cuttingZ <20>ָ<EFBFBD>
//ͶӰ<CDB6><D3B0>2D
SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);// ͳ<><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0>С
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;
source code
2025-06-08 10:46:41 +08:00
合并Mark检测
2025-12-05 23:08:46 +08:00
double inerPolateDistTh = scale * 10; //<2F><>ֵ<EFBFBD><D6B5><EFBFBD>ޣ<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
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); //<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
cv::Mat projectionIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pointClout2DProjection(
scanLines,
pntCloudPara.xRange,
pntCloudPara.yRange,
scale,
refPos->cuttingZ,
edgeSkip,
inerPolateDistTh, //<2F><>ֵ<EFBFBD><D6B5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
projectionImg,//ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
projectionIndexing //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD>3D<33><44><EFBFBD><EFBFBD>
);
//<2F><><EFBFBD><EFBFBD><EFBFBD>任
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
//<2F><>ȡĿ<C8A1>꣬<EFBFBD><EAA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
//<2F>Ծ<EFBFBD><D4BE><EFBFBD><EFBFBD>任ֵ<E4BBBB><D6B5><EFBFBD>н<EFBFBD>ȡ<EFBFBD><C8A1>ʹĿ<CAB9>겻<EFBFBD><EAB2BB><EFBFBD>ߡ<EFBFBD>
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;
}
}
//<2F><>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>ע
cv::Mat labImg;
std::vector<SSG_Region> labelRgns;
SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
//<2F><><EFBFBD>ݴ<EFBFBD>С<EFBFBD><D0A1>ȡĿ<C8A1><C4BF>
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]);
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2>ɨ<EFBFBD>裬<EFBFBD><E8A3AC><EFBFBD><EFBFBD>ȷȷ<C8B7><C8B7><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
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<49><44>1<EFBFBD><31>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>±<EFBFBD><C2B1><EFBFBD>ͬ
int objNumber = 1;
for (int i = 0, i_max = (int)objectRgns.size(); i < i_max; i++)
{
SSG_Region& a_objRgn = objectRgns[i];
//y<><79><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int x = (a_objRgn.roi.left + a_objRgn.roi.right) / 2;
int y = (a_objRgn.roi.top + a_objRgn.roi.bottom) / 2;
//Բ<><D4B2>ɨ<EFBFBD>趨<EFBFBD>ӻ<EFBFBD>
std::vector<SVzNL2DPoint> peakPts;
circileScan(
distTransform, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
x, y, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
scanR1, scanR2, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
peakPts //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD>ʹ<EFBFBD>ü<EFBFBD><C3BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⵽<EFBFBD><E2B5BD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>
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);
}
//<2F><><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
//<2F><><EFBFBD><EFBFBD><EFBFBD>ڽӹ<DABD>ϵ,ÿ<><C3BF>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>6<EFBFBD><36><EFBFBD>ڽ<EFBFBD>Ŀ<EFBFBD><C4BF>
std::vector< SSG_hexagonNeighbour> objLinkings;
objLinkings.resize(objNumber);//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ʹ<EFBFBD>á<EFBFBD>Ŀ<EFBFBD><C4BF>ID<49><44>1<EFBFBD><31>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>±<EFBFBD>һ<EFBFBD><D2BB>
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<36><30>
//60<36><30>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0-59Ϊ<39><CEAA><EFBFBD><EFBFBD>0<EFBFBD><30> 60-119Ϊ<39><CEAA><EFBFBD><EFBFBD>1
int angleIndx = (int)(angle+ 30.0)/ 60; //<2F>˴<EFBFBD><CBB4><EFBFBD>30<33>ȣ<EFBFBD><C8A3><EFBFBD>ֹĿ<D6B9>괦<EFBFBD><EAB4A6>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٽ硣<D9BD><E7A1A3><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>
int angleIndx_1 = (angleIndx + 3) % 6;
objLinkings[i].neighborID[angleIndx] = j;
objLinkings[j].neighborID[angleIndx_1] = i;
}
}
}
//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
int objID = -1;
bool validGrasper = false;
if (refPos->refPos.z > 1e-4) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>θ<EFBFBD><CEB8><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ý<EFBFBD><C3BD><EFBFBD>ץȡ
{
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++) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ΪץȡĿ<C8A1><C4BF>
{
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;
}
}
}
//<2F><><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
SWD_statorOuterGrasper outerGrasper;
double angleDistance;
bool validGrasper = computeOuterGraspPoint(
objID,
statorParam.statorOuterD / 2,
objects,
objLinkings,
outerGrasper,
&angleDistance
);
resultGraspPos = outerGrasper;
}
else
{
//ͳ<><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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];
//<2F><><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ο<EFBFBD>ץȡ<D7A5><C8A1>
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
source code
2025-06-08 10:46:41 +08:00
//Z<><5A><EFBFBD><EFBFBD>ͳ<EFBFBD>ơ<EFBFBD><C6A1><EFBFBD><EFBFBD><EFBFBD>Ϊ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] ++;
}
}
//ȡ<><C8A1><EFBFBD>Ӷ<EFBFBD><D3B6>棺ȡ5mmͳ<6D><CDB3><EFBFBD><EFBFBD><EFBFBD>䣬zHist<73><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
//ȡ<><C8A1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>10%<25>ļ<EFBFBD>ֵ<EFBFBD><D6B5>
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;
//ȡ<><C8A1><EFBFBD>Ӷ<EFBFBD><D3B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double zSliceTop = (double)maxPos - 10 + z_range.min;
double zSliceBtm = (double)maxPos + 8 + z_range.min;
//<2F><>XOY<4F><59><EFBFBD><EFBFBD>ͶӰ
//<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>1mmΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>
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); //<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ھ<EFBFBD><DABE><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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();
//<2F><><EFBFBD><EFBFBD><EFBFBD>任
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
//<2F><>zSliceData<74><61><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>䣨2<E4A3A8><32><EFBFBD><EFBFBD>ʴ<EFBFBD><CAB4>3<EFBFBD><33><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>Ŀ<EFBFBD>꣨4<EAA3A8><34><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>ϵõ<CFB5><C3B5><EFBFBD><EFBFBD>ĵ<EFBFBD>
cv::Mat invertSliceData;
cv::bitwise_not(zSliceData, invertSliceData);
#if DEBUG_OUT_IMAGE
cv::imwrite("zSliceImage_invert.png", invertSliceData);
#endif
//<2F><>ͨ<EFBFBD><CDA8>
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);
//<2F><>С<EFBFBD><D0A1>20<32><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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
//<2F><>ʴ
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
//<2F><>ȡĿ<C8A1><C4BF>
nccomps = connectedComponentsWithStats(zSliceData_erode, labels, stats, centroids, 4);
//Ŀ<><C4BF><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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))
{
//<2F>ϸ<EFBFBD><CFB8><EFBFBD>Ŀ<EFBFBD><C4BF>
//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>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); // <20><>ԭͼ<D4AD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD>Բ
cv::imwrite("zSliceImage_fitting.png", fittingImage);
#endif
//<2F><><EFBFBD><EFBFBD><EFBFBD>ڽӹ<DABD>ϵȷ<CFB5><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
std::vector< SSG_statorNeighbourInfo> neighbouringInfo;
_getNeighbouringInfo(
positionParam,
zSliceData_origin, //<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD>ұ߽磨<DFBD>߿<EFBFBD><DFBF><EFBFBD>
fittingObs, //Ŀ<><C4BF>λ<EFBFBD><CEBB>
neighbouringInfo //<2F>ڽӹ<DABD>ϵ
);
//<2F><><EFBFBD><EFBFBD><EFBFBD>ڽӹ<DABD>ϵȷ<CFB5><C8B7>ץȡ<D7A5><C8A1>
std::vector< SSG_motorStatorPosition> grabPoses; //С<>ڵ<EFBFBD><DAB5><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ץȡ<D7A5>㣨<EFBFBD><E3A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector< SSG_motorStatorPosition> grabPoses_2; //<2F><><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ץȡ<D7A5>㣨<EFBFBD><E3A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//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);
}
//ѡȡץȡ<D7A5>㣺<EFBFBD><E3A3BA>1<EFBFBD><31><EFBFBD>ܱ<EFBFBD><DCB1>ھ<EFBFBD><DABE><EFBFBD><EFBFBD>٣<EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵľ<DFB5><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::sort(grabPoses.begin(), grabPoses.end(),compareByNeighbourDist);
std::sort(grabPoses_2.begin(), grabPoses_2.end(), compareByNeighbourNum);
//<2F>ϲ<EFBFBD>
grabPoses.insert(grabPoses.end(), grabPoses_2.begin(), grabPoses_2.end());
return;
合并Mark检测
2025-12-05 23:08:46 +08:00
#endif
source code
2025-06-08 10:46:41 +08:00
}
Reference in New Issue Copy Permalink