1289 lines
33 KiB
C++
1289 lines
33 KiB
C++
|
#include <vector>
|
|||
|
|
#include "SG_baseDataType.h"
|
|||
|
|
#include "SG_baseAlgo_Export.h"
|
|||
|
|
#include "hybridPosePositioning_Export.h"
|
|||
|
|
#include <opencv2/opencv.hpp>
|
|||
|
|
#include <limits>
|
|||
|
|
|
|||
|
|
#define _DEBUG_OUTPUT
|
|||
|
|
|
|||
|
|
//version 1.0.0 : base version release to customer
|
|||
|
|
//version 1.1.0 : <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>滮<EFBFBD>汾
|
|||
|
|
std::string m_strVersion = "HybridPositioning 1.1.0";
|
|||
|
|
const char* wd_hybridPositioningVersion(void)
|
|||
|
|
{
|
|||
|
|
return m_strVersion.c_str();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>е<EFBFBD><D0B5><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ҫ<EFBFBD>Ե<EFBFBD><D4B5><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>գ<EFBFBD><D5A3><EFBFBD><EFBFBD><EFBFBD><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_getGroundCalibPara(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
|||
|
|
{
|
|||
|
|
return sg_getPlaneCalibPara2(scanLines);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽʱ<C6BD><CAB1>̬<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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SVzNL3DPosition _computeMinZPoint(std::vector<SVzNL3DPosition>& points)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition peak;
|
|||
|
|
peak.nPointIdx = 0;
|
|||
|
|
peak.pt3D = { 0.0, 0.0, 0.0 };
|
|||
|
|
for (int i = 0; i < (int)points.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (points[i].pt3D.z < 1e-4)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (peak.pt3D.z < 1e-4)
|
|||
|
|
peak = points[i];
|
|||
|
|
else if (peak.pt3D.z > points[i].pt3D.z)
|
|||
|
|
peak = points[i];
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
return peak;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SVzNL3DPoint _computeCentroid(std::vector<SVzNL3DPosition>& points)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
|
|||
|
|
int counter = 0;
|
|||
|
|
for (int i = 0; i < (int)points.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (points[i].pt3D.z < 1e-4)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
counter++;
|
|||
|
|
centroid.x += points[i].pt3D.x;
|
|||
|
|
centroid.y += points[i].pt3D.y;
|
|||
|
|
centroid.z += points[i].pt3D.z;
|
|||
|
|
}
|
|||
|
|
centroid.x = centroid.x / counter;
|
|||
|
|
centroid.y = centroid.y / counter;
|
|||
|
|
centroid.z = centroid.z / counter;
|
|||
|
|
return centroid;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int _get2DRegion(SVzNLPositionD& a_pt2D3D, std::vector<WD_objArea2D>& objROIs)
|
|||
|
|
{
|
|||
|
|
for (int i = 0; i < (int)objROIs.size(); i++)
|
|||
|
|
{
|
|||
|
|
if ((a_pt2D3D.ptLeft2D.x >= objROIs[i].roi.left) && (a_pt2D3D.ptLeft2D.x <= objROIs[i].roi.right) &&
|
|||
|
|
(a_pt2D3D.ptLeft2D.y >= objROIs[i].roi.top) && (a_pt2D3D.ptLeft2D.y <= objROIs[i].roi.bottom))
|
|||
|
|
return i;
|
|||
|
|
}
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b)
|
|||
|
|
{
|
|||
|
|
return a.pt3D.z < b.pt3D.z;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
void _genPolarScanData(
|
|||
|
|
std::vector<SVzNL3DPosition>& points,
|
|||
|
|
const double angleScale,
|
|||
|
|
const SVzNL3DPoint polarCener,
|
|||
|
|
std::vector<std::vector<SWD_polarPt>>& polarScanData)
|
|||
|
|
{
|
|||
|
|
int polarLines = (int)(360.0 / angleScale + 0.5);
|
|||
|
|
polarScanData.resize(polarLines);
|
|||
|
|
|
|||
|
|
int dataSize = (int)points.size();
|
|||
|
|
for (int i = 0; i < dataSize; i++)
|
|||
|
|
{
|
|||
|
|
int line = points[i].nPointIdx >> 16;
|
|||
|
|
int ptIdx = points[i].nPointIdx & 0x0000FFFF;
|
|||
|
|
SVzNL3DPoint& a_pt = points[i].pt3D;
|
|||
|
|
|
|||
|
|
double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
|
|||
|
|
angle = (angle / PI) * 180 + 180.0;
|
|||
|
|
double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));
|
|||
|
|
|
|||
|
|
int angleLine = (int)(angle / angleScale + 0.5);
|
|||
|
|
angleLine = angleLine % (int)polarScanData.size();
|
|||
|
|
|
|||
|
|
SWD_polarPt a_polarPt;
|
|||
|
|
a_polarPt.lineIdx = line;
|
|||
|
|
a_polarPt.ptIdx = ptIdx;
|
|||
|
|
a_polarPt.x = a_pt.x;
|
|||
|
|
a_polarPt.y = a_pt.y;
|
|||
|
|
a_polarPt.z = a_pt.z;
|
|||
|
|
a_polarPt.R = R;
|
|||
|
|
a_polarPt.angle = angle;
|
|||
|
|
polarScanData[angleLine].push_back(a_polarPt);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, ͬ<><CDAC>Բ<EFBFBD><D4B2><EFBFBD>ṹ
|
|||
|
|
void _genPolarScanData_2(
|
|||
|
|
std::vector<SVzNL3DPosition>& points,
|
|||
|
|
const double radiusScale,
|
|||
|
|
const SVzNL3DPoint polarCener,
|
|||
|
|
std::vector<std::vector<SWD_polarPt>>& polarScanData)
|
|||
|
|
{
|
|||
|
|
std::vector< SWD_polarPt> polarPoints;
|
|||
|
|
int dataSize = (int)points.size();
|
|||
|
|
double rMax = 0;
|
|||
|
|
for (int i = 0; i < dataSize; i++)
|
|||
|
|
{
|
|||
|
|
int line = points[i].nPointIdx >> 16;
|
|||
|
|
int ptIdx = points[i].nPointIdx & 0x0000FFFF;
|
|||
|
|
SVzNL3DPoint& a_pt = points[i].pt3D;
|
|||
|
|
|
|||
|
|
double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
|
|||
|
|
angle = (angle / PI) * 180 + 180.0;
|
|||
|
|
double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));
|
|||
|
|
rMax = rMax < R ? R : rMax;
|
|||
|
|
|
|||
|
|
SWD_polarPt a_polarPt;
|
|||
|
|
a_polarPt.lineIdx = line;
|
|||
|
|
a_polarPt.ptIdx = ptIdx;
|
|||
|
|
a_polarPt.x = a_pt.x;
|
|||
|
|
a_polarPt.y = a_pt.y;
|
|||
|
|
a_polarPt.z = a_pt.z;
|
|||
|
|
a_polarPt.R = R;
|
|||
|
|
a_polarPt.angle = angle;
|
|||
|
|
|
|||
|
|
polarPoints.push_back(a_polarPt);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int circleNum = (int)(rMax / radiusScale) + 1;
|
|||
|
|
polarScanData.resize(circleNum);
|
|||
|
|
for (int i = 0; i < (int)polarPoints.size(); i++)
|
|||
|
|
{
|
|||
|
|
double r = polarPoints[i].R;
|
|||
|
|
int idx = r / radiusScale;
|
|||
|
|
polarScanData[idx].push_back(polarPoints[i]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) {
|
|||
|
|
return a.R < b.R;
|
|||
|
|
}
|
|||
|
|
bool compareByPolarScanAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
|||
|
|
return a.angle < b.angle;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
WD_workpieceInfo _computeWorkpiecePose(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
|||
|
|
{
|
|||
|
|
WD_workpieceInfo a_pose;
|
|||
|
|
memset(&a_pose, 0, sizeof(WD_workpieceInfo));
|
|||
|
|
|
|||
|
|
int lineNum = (int)scanLines.size();
|
|||
|
|
int linePtNum = (int)scanLines[0].size();
|
|||
|
|
|
|||
|
|
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
|||
|
|
hLines.resize(linePtNum);
|
|||
|
|
for (int i = 0; i < linePtNum; i++)
|
|||
|
|
hLines[i].resize(lineNum);
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
hLines[j][line] = scanLines[line][j];
|
|||
|
|
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
|||
|
|
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><>ֱ<EFBFBD><D6B1>ˮƽɨ<C6BD>裬<EFBFBD><E8A3AC><EFBFBD><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
void wd_computeDirAngle_wholeLine2(
|
|||
|
|
std::vector< SVzNL3DPosition>&line_data,
|
|||
|
|
const double steppingScale,
|
|||
|
|
const double invalidScale, //<2F><><EFBFBD><EFBFBD><EFBFBD>˳߶ȣ<DFB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>Ч
|
|||
|
|
std::vector< SSG_pntDirAngle>&ptDirAngles
|
|||
|
|
);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><CDB9>
|
|||
|
|
return a_pose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
void wd_HRM_RotorCorePositioning(
|
|||
|
|
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
|||
|
|
std::vector<WD_objArea2D>& objROIs,
|
|||
|
|
const SSG_planeCalibPara groundCalibPara,
|
|||
|
|
std::vector< WD_workpieceInfo>& workpiecePositions,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
|
|||
|
|
if (objROIs.size() == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SX_ERR_ZERO_2D_OBJECTS;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#if 0
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
{ //<2F>д<EFBFBD><D0B4><EFBFBD>
|
|||
|
|
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
int lineNum = (int)scanLinesInput.size();
|
|||
|
|
int linePtNum = (int)scanLinesInput[0].size();
|
|||
|
|
int maxU = 0; //2Dͼ<44><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Col
|
|||
|
|
int maxV = 0; //2Dͼ<44><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Row
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
|
|||
|
|
{
|
|||
|
|
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU;
|
|||
|
|
maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>3d<33>Ķ<EFBFBD>Ӧ<EFBFBD><D3A6>
|
|||
|
|
const int imgCols = maxU;
|
|||
|
|
const int imgRows = maxV;
|
|||
|
|
std::vector<std::vector<SWDIndexingVzPoint>> mappingTable; //<2F><>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
mappingTable.resize(imgCols); //<2F><>ɨ<EFBFBD><C9A8><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>Ӧ
|
|||
|
|
for (int i = 0; i < imgCols; i++)
|
|||
|
|
mappingTable[i].resize(imgRows);
|
|||
|
|
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
|||
|
|
{
|
|||
|
|
scanLinesInput[line][ptIdx].nPointIdx = 0;
|
|||
|
|
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
SWDIndexingVzPoint indexingPt;
|
|||
|
|
indexingPt.lineIdx = line;
|
|||
|
|
indexingPt.ptIdx = ptIdx;
|
|||
|
|
indexingPt.point = scanLinesInput[line][ptIdx].pt3D;
|
|||
|
|
int u = scanLinesInput[line][ptIdx].ptLeft2D.x;
|
|||
|
|
int v = scanLinesInput[line][ptIdx].ptLeft2D.y;
|
|||
|
|
mappingTable[u][v] = indexingPt;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int objNum = (int)objROIs.size();
|
|||
|
|
for (int idx = 0; idx < objNum; idx++)
|
|||
|
|
{
|
|||
|
|
WD_objArea2D& obj_roi = objROIs[idx];
|
|||
|
|
int L = (int)(obj_roi.roi.left + 0.5);
|
|||
|
|
int R = (int)(obj_roi.roi.right + 0.5);
|
|||
|
|
int T = (int)(obj_roi.roi.top + 0.5);
|
|||
|
|
int B = (int)(obj_roi.roi.bottom + 0.5);
|
|||
|
|
|
|||
|
|
//ͳ<><CDB3>ROI<4F>е<EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>ߺ<EFBFBD>PtIdx<64><78>Χ
|
|||
|
|
SVzNLRange roiLineIndice = { INT_MAX, 0 };
|
|||
|
|
SVzNLRange roiPtIndice = { INT_MAX, 0 };
|
|||
|
|
for (int x = L; x <= R; x++)
|
|||
|
|
{
|
|||
|
|
for (int y = T; y <= B; y++)
|
|||
|
|
{
|
|||
|
|
if (mappingTable[x][y].point.z > 1e-4)
|
|||
|
|
{
|
|||
|
|
int lineIdx = mappingTable[x][y].lineIdx;
|
|||
|
|
int ptIdx = mappingTable[x][y].ptIdx;
|
|||
|
|
scanLinesInput[lineIdx][ptIdx].nPointIdx = idx + 1;
|
|||
|
|
|
|||
|
|
roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin;
|
|||
|
|
roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax;
|
|||
|
|
roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin;
|
|||
|
|
roiPtIndice.nMax = roiPtIndice.nMax < ptIdx ? ptIdx : roiPtIndice.nMax;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ROI<4F>е<EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int roiLines = roiLineIndice.nMax - roiLineIndice.nMin + 1;
|
|||
|
|
int roiLinePtNum = roiPtIndice.nMax - roiPtIndice.nMin + 1;
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>> roiScanLines;
|
|||
|
|
roiScanLines.resize(roiLines);
|
|||
|
|
for (int line = 0; line < roiLines; line++)
|
|||
|
|
roiScanLines[line].resize(roiLinePtNum);
|
|||
|
|
for (int x = L; x <= R; x++)
|
|||
|
|
{
|
|||
|
|
for (int y = T; y <= B; y++)
|
|||
|
|
{
|
|||
|
|
if (mappingTable[x][y].point.z > 1e-4)
|
|||
|
|
{
|
|||
|
|
int lineIdx = mappingTable[x][y].lineIdx - roiLineIndice.nMin;
|
|||
|
|
int ptIdx = mappingTable[x][y].ptIdx - roiPtIndice.nMin;
|
|||
|
|
roiScanLines[lineIdx][ptIdx].pt3D = mappingTable[x][y].point;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>жϹ<D0B6><CFB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD><E0A3AC><EFBFBD>㹤<EFBFBD><E3B9A4><EFBFBD><EFBFBD>̬
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void wd_HRM_TaperedWorkpiecePositioning(
|
|||
|
|
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
|||
|
|
std::vector<WD_objArea2D>& objROIs,
|
|||
|
|
const SSG_planeCalibPara groundCalibPara,
|
|||
|
|
std::vector< WD_workpieceInfo>& workpiecePositions,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
|
|||
|
|
if (objROIs.size() == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SX_ERR_ZERO_2D_OBJECTS;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<std::vector<SVzNL3DPosition>> rgnPoints;
|
|||
|
|
rgnPoints.resize(objROIs.size());
|
|||
|
|
for (int line = 0; line < (int)scanLinesInput.size(); line++)
|
|||
|
|
{
|
|||
|
|
for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
|
|||
|
|
{
|
|||
|
|
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
int rgnIdx = _get2DRegion(scanLinesInput[line][ptIdx], objROIs);
|
|||
|
|
if (rgnIdx >= 0)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition a_rgnPt;
|
|||
|
|
a_rgnPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
|
|||
|
|
a_rgnPt.pt3D = scanLinesInput[line][ptIdx].pt3D;
|
|||
|
|
rgnPoints[rgnIdx].push_back(a_rgnPt);
|
|||
|
|
scanLinesInput[line][ptIdx].nPointIdx = rgnIdx + 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ͳ<><CDB3>ÿ<EFBFBD><C3BF>region<6F><6E><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>
|
|||
|
|
std::vector< SVzNL3DPosition> rgnPeaks;
|
|||
|
|
for (int i = 0; i < (int)rgnPoints.size(); i++)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition peakPoint = _computeMinZPoint(rgnPoints[i]);
|
|||
|
|
SVzNL3DPosition a_peak;
|
|||
|
|
a_peak.nPointIdx = i;
|
|||
|
|
a_peak.pt3D = peakPoint.pt3D;
|
|||
|
|
rgnPeaks.push_back(a_peak);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::sort(rgnPeaks.begin(), rgnPeaks.end(), _compareByZValue);
|
|||
|
|
for (int i = 0; i < (int)rgnPeaks.size(); i++)
|
|||
|
|
{
|
|||
|
|
WD_workpieceInfo a_obj;
|
|||
|
|
memset(&a_obj, 0, sizeof(WD_workpieceInfo));
|
|||
|
|
a_obj.center = rgnPeaks[i].pt3D;
|
|||
|
|
workpiecePositions.push_back(a_obj);
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
|
|||
|
|
{
|
|||
|
|
if (listData.size() == 0)
|
|||
|
|
return { 0,0,0,0 };
|
|||
|
|
SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y };
|
|||
|
|
for (int i = 0; i < (int)listData.size(); i++)
|
|||
|
|
{
|
|||
|
|
roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
|
|||
|
|
roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
|
|||
|
|
roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
|
|||
|
|
roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
|
|||
|
|
}
|
|||
|
|
return roi;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
|
|||
|
|
{
|
|||
|
|
if (listData.size() == 0)
|
|||
|
|
return 0;
|
|||
|
|
double meanZ = 0;
|
|||
|
|
zRange.max = -1;
|
|||
|
|
zRange.min = 0;
|
|||
|
|
for (int i = 0; i < (int)listData.size(); i++)
|
|||
|
|
{
|
|||
|
|
meanZ += listData[i].pt3D.z;
|
|||
|
|
if (zRange.max < 0)
|
|||
|
|
{
|
|||
|
|
zRange.max = listData[i].pt3D.z;
|
|||
|
|
zRange.min = listData[i].pt3D.z;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max;
|
|||
|
|
zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
meanZ = meanZ / (double)listData.size();
|
|||
|
|
return meanZ;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>תʱ <20><> > 0 <20><>˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD>תʱ <20><> < 0
|
|||
|
|
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
|
|||
|
|
{
|
|||
|
|
return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Ͽ<EFBFBD><CFBF><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD>ȡ<EFBFBD>Ͽ<EFBFBD><CFBF>ߴ硢<DFB4>Ͽ<EFBFBD><CFBF><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>Ͽ<EFBFBD><CFBF><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
#if 0
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>Ͽ<EFBFBD>
|
|||
|
|
WD_HRM_BinInfo wd_HRM_getBinSize(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
|||
|
|
const SSG_cornerParam cornerPara,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
WD_HRM_BinInfo resultPose;
|
|||
|
|
memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));
|
|||
|
|
|
|||
|
|
int lineNum = (int)scanLines.size();
|
|||
|
|
if (lineNum == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_3D_DATA_NULL;
|
|||
|
|
return resultPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int linePtNum = (int)scanLines[0].size();
|
|||
|
|
|
|||
|
|
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
|
|||
|
|
bool isGridData = true;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
if (linePtNum != (int)scanLines[line].size())
|
|||
|
|
{
|
|||
|
|
isGridData = false;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
|||
|
|
return resultPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
|||
|
|
scanLines_h.resize(linePtNum);
|
|||
|
|
for (int i = 0; i < linePtNum; i++)
|
|||
|
|
scanLines_h[i].resize(lineNum);
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
scanLines_h[j][line] = scanLines[line][j];
|
|||
|
|
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
|||
|
|
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int line = 0; line < linePtNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
|||
|
|
scanLines_h[line][j].nPointIdx = j;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>㷨<EFBFBD><E3B7A8><EFBFBD>̣<EFBFBD>
|
|||
|
|
//1<><31><EFBFBD><EFBFBD><EFBFBD>鴹ֱ<E9B4B9><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD>ȥ<EFBFBD><C8A5>
|
|||
|
|
//2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//3<><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
|
|||
|
|
//4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
SSG_cornerParam removeVertialPara = cornerPara;
|
|||
|
|
removeVertialPara.scale = 3.0;
|
|||
|
|
removeVertialPara.cornerTh = 60;
|
|||
|
|
|
|||
|
|
std::vector<std::vector<int>> flags;
|
|||
|
|
flags.resize(lineNum);
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
{
|
|||
|
|
flags[i].resize(linePtNum);
|
|||
|
|
std::fill(flags[i].begin(), flags[i].end(), 0);
|
|||
|
|
}
|
|||
|
|
std::vector<std::vector<int>> zVertivalFlags;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 700)
|
|||
|
|
int kkk = 1;
|
|||
|
|
std::vector<int> line_verticalFlags;
|
|||
|
|
wd_getXYVertialFeature_dirAngleMethod(
|
|||
|
|
scanLines[line],
|
|||
|
|
line,
|
|||
|
|
removeVertialPara,
|
|||
|
|
line_verticalFlags
|
|||
|
|
);
|
|||
|
|
zVertivalFlags.push_back(line_verticalFlags);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (line_verticalFlags[i] > 0)
|
|||
|
|
flags[line][i] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<std::vector<int>> zVertivalFlags_h;
|
|||
|
|
for (int line = 0; line < linePtNum; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 1177)
|
|||
|
|
int kkk = 1;
|
|||
|
|
std::vector<int> line_verticalFlags;
|
|||
|
|
wd_getXYVertialFeature_dirAngleMethod(
|
|||
|
|
scanLines_h[line],
|
|||
|
|
line,
|
|||
|
|
removeVertialPara,
|
|||
|
|
line_verticalFlags
|
|||
|
|
);
|
|||
|
|
zVertivalFlags_h.push_back(line_verticalFlags);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (line_verticalFlags[i] > 0)
|
|||
|
|
flags[i][line] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
if (flags[line][j] > 0)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].pt3D.z = 0;
|
|||
|
|
scanLines_h[j][line].pt3D.z = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
|||
|
|
SSG_lineSegParam lineSegPara;
|
|||
|
|
lineSegPara.distScale = 5.0;
|
|||
|
|
lineSegPara.segGapTh_y = 5.0;
|
|||
|
|
lineSegPara.segGapTh_z = 5.0;
|
|||
|
|
const int minSegLen = 5;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_RUN> segs;
|
|||
|
|
wd_getLineDataIntervals(
|
|||
|
|
scanLines[line],
|
|||
|
|
lineSegPara,
|
|||
|
|
segs);
|
|||
|
|
for (int i = 0; i < (int)segs.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (segs[i].len <= minSegLen)
|
|||
|
|
{
|
|||
|
|
int idx0 = segs[i].start;
|
|||
|
|
for (int j = 0; j < segs[i].len; j++)
|
|||
|
|
flags[line][idx0 + j] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int line = 0; line < linePtNum; line++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_RUN> segs;
|
|||
|
|
wd_getLineDataIntervals(
|
|||
|
|
scanLines_h[line],
|
|||
|
|
lineSegPara,
|
|||
|
|
segs);
|
|||
|
|
for (int i = 0; i < (int)segs.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (segs[i].len <= minSegLen)
|
|||
|
|
{
|
|||
|
|
int idx0 = segs[i].start;
|
|||
|
|
for (int j = 0; j < segs[i].len; j++)
|
|||
|
|
flags[idx0 + j][line] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ע
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD>߶<EFBFBD>ȥ<EFBFBD><C8A5>
|
|||
|
|
std::vector< SVzNL3DPosition> validPoints;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
if (flags[line][j] > 0)
|
|||
|
|
scanLines[line][j].pt3D.z = 0;
|
|||
|
|
|
|||
|
|
if (scanLines[line][j].pt3D.z > 1e-4)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition a_vldPt;
|
|||
|
|
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
|||
|
|
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
|||
|
|
validPoints.push_back(a_vldPt);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>
|
|||
|
|
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//double minObjSize_w = 150;
|
|||
|
|
//double minObjSize_h = 150;
|
|||
|
|
|
|||
|
|
int clusterCheckWin = 5;
|
|||
|
|
double clusterDist = 5.0;
|
|||
|
|
int distType = 1; //0 - 2d distance; 1- 3d distance
|
|||
|
|
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
|||
|
|
wd_pointClustering_speedUp(
|
|||
|
|
validPoints,
|
|||
|
|
lineNum, linePtNum, clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
clusterDist,
|
|||
|
|
distType,
|
|||
|
|
objClusters //result
|
|||
|
|
);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
|
|||
|
|
std::vector<double> objMeanZ;
|
|||
|
|
std::vector<SVzNLRangeD> objZRange;
|
|||
|
|
objMeanZ.resize(objClusters.size());
|
|||
|
|
objZRange.resize(objClusters.size());
|
|||
|
|
int maxSizeId = -1;
|
|||
|
|
double maxSize = 0;
|
|||
|
|
for (int i = 0; i < (int)objClusters.size(); i++)
|
|||
|
|
{
|
|||
|
|
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
|||
|
|
double w = a_roi.right - a_roi.left;
|
|||
|
|
double h = a_roi.bottom - a_roi.top;
|
|||
|
|
double size = w * h;
|
|||
|
|
|
|||
|
|
SVzNLRangeD zRange;
|
|||
|
|
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
|||
|
|
objMeanZ[i] = meanZ;
|
|||
|
|
objZRange[i] = zRange;
|
|||
|
|
|
|||
|
|
if (maxSize < size)
|
|||
|
|
{
|
|||
|
|
maxSize = size;
|
|||
|
|
maxSizeId = i;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0>Ͳ<EFBFBD><CDB2><EFBFBD><EFBFBD>ı<EFBFBD>Ե<EFBFBD><D4B5>ɨ<EFBFBD><C9A8>ë<EFBFBD><C3AB>
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
std::vector< SVzNL3DPosition>& bottomCluster = objClusters[maxSizeId];
|
|||
|
|
//<2F><>ע
|
|||
|
|
//<2F><><EFBFBD>½<EFBFBD>flags<67><73><EFBFBD><EFBFBD>ΪĿ<CEAA><C4BF><EFBFBD><EFBFBD>mask
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
std::fill(flags[i].begin(), flags[i].end(), -1);
|
|||
|
|
for (int i = 0; i < (int)bottomCluster.size(); i++)
|
|||
|
|
{
|
|||
|
|
int line = bottomCluster[i].nPointIdx >> 16;
|
|||
|
|
int ptIdx = bottomCluster[i].nPointIdx & 0x0000FFFF;
|
|||
|
|
scanLines[line][ptIdx].nPointIdx = 2;
|
|||
|
|
flags[line][ptIdx] = i; //indexing
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ʹ<><CAB9>PCA<43><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>
|
|||
|
|
SVzNL3DPoint vec_normal, vec_centroid;
|
|||
|
|
|
|||
|
|
computePlaneNormalByPCA(
|
|||
|
|
bottomCluster,
|
|||
|
|
vec_normal,
|
|||
|
|
vec_centroid);
|
|||
|
|
|
|||
|
|
//ͶӰ
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
if (vec_normal.z < 0)
|
|||
|
|
vec_normal = { -vec_normal.x, -vec_normal.y, -vec_normal.z };
|
|||
|
|
resultPose.center = vec_centroid;
|
|||
|
|
resultPose.bottomNormal = vec_normal;
|
|||
|
|
|
|||
|
|
|
|||
|
|
return resultPose;
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ͽ<EFBFBD><CFBF><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><D7BC><EFBFBD>й滮
|
|||
|
|
WD_HRM_BinInfo wd_HRM_getBinSize(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
|||
|
|
const SSG_planeCalibPara calibPara,
|
|||
|
|
const double binHeight, //<2F>Ͽ<EFBFBD><CFBF>߶<EFBFBD>
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
WD_HRM_BinInfo resultPose;
|
|||
|
|
memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));
|
|||
|
|
|
|||
|
|
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
SVzNLRangeD binTopSliceRange = {calibPara.planeHeight- binHeight-5.0, calibPara.planeHeight - binHeight + 5.0}; //<2F>Ͽ<EFBFBD><CFBF>߶<EFBFBD>Z<EFBFBD><5A><EFBFBD>з<EFBFBD>Χ
|
|||
|
|
|
|||
|
|
int lineNum = (int)scanLines.size();
|
|||
|
|
if (lineNum == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_3D_DATA_NULL;
|
|||
|
|
return resultPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int linePtNum = (int)scanLines[0].size();
|
|||
|
|
|
|||
|
|
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
|
|||
|
|
bool isGridData = true;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
if (linePtNum != (int)scanLines[line].size())
|
|||
|
|
{
|
|||
|
|
isGridData = false;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
|||
|
|
return resultPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//<2F><>ƽ
|
|||
|
|
|
|||
|
|
//Z<><5A><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SVzNL3DPosition> zSliceData;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].nPointIdx = 0;
|
|||
|
|
if ((scanLines[line][j].pt3D.z > binTopSliceRange.min) && (scanLines[line][j].pt3D.z < binTopSliceRange.max))
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition a_pt;
|
|||
|
|
a_pt.nPointIdx = (line << 16) | j & 0xffff;
|
|||
|
|
a_pt.pt3D = scanLines[line][j].pt3D;
|
|||
|
|
zSliceData.push_back(a_pt);
|
|||
|
|
scanLines[line][j].nPointIdx = 1; //<2F><>ע
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SVzNLRangeD dataZRange;
|
|||
|
|
double zSliceZ = _getListMeanZ(zSliceData, dataZRange);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// <20><>С<EFBFBD><D0A1><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD>
|
|||
|
|
std::vector<cv::Point2f> points;
|
|||
|
|
for (int i = 0; i < (int)zSliceData.size(); i++)
|
|||
|
|
{
|
|||
|
|
cv::Point2f a_pt = cv::Point2f(zSliceData[i].pt3D.x, zSliceData[i].pt3D.y);
|
|||
|
|
points.push_back(a_pt);
|
|||
|
|
}
|
|||
|
|
cv::RotatedRect rect = minAreaRect(points);
|
|||
|
|
cv::Point2f vertices[4];
|
|||
|
|
rect.points(vertices);
|
|||
|
|
double width = rect.size.width; //ͶӰ<CDB6>Ŀ<EFBFBD><C4BF><EFBFBD>
|
|||
|
|
double height = rect.size.height;
|
|||
|
|
if (width < height)
|
|||
|
|
{
|
|||
|
|
double tmp = height;
|
|||
|
|
height = width;
|
|||
|
|
width = tmp;
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>vertices[0]<5D><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2));
|
|||
|
|
double width_diff = abs(dist_v0v3 - width);
|
|||
|
|
double pose_yaw;
|
|||
|
|
if (CV_VERSION == "3.2.0")
|
|||
|
|
{
|
|||
|
|
if (width_diff < 10.0)//width<74><68><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
pose_yaw = -rect.angle;
|
|||
|
|
}
|
|||
|
|
else //<2F><><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>
|
|||
|
|
{
|
|||
|
|
pose_yaw = -rect.angle - 90;
|
|||
|
|
if (pose_yaw < -90)
|
|||
|
|
pose_yaw = 180 + pose_yaw;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else //if (CV_VERSION == "4.8.0")
|
|||
|
|
{
|
|||
|
|
if (width_diff < 10.0) //width<74><68><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
pose_yaw = -rect.angle;
|
|||
|
|
}
|
|||
|
|
else//<2F><><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>
|
|||
|
|
{
|
|||
|
|
pose_yaw = -rect.angle + 90;
|
|||
|
|
if (pose_yaw > 90)
|
|||
|
|
pose_yaw = pose_yaw - 180;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ͽ<EFBFBD><CFBF><EFBFBD>Ϣ
|
|||
|
|
double binZ = calibPara.planeHeight - binHeight;
|
|||
|
|
double sinTheta = sin(-PI * pose_yaw / 180);
|
|||
|
|
double cosTheta = cos(-PI * pose_yaw / 180);
|
|||
|
|
resultPose.center = { rect.center.x, rect.center.y, binZ };
|
|||
|
|
resultPose.bottomNormal = { 0.0, 0.0, 1.0 };
|
|||
|
|
resultPose.x_dir = { cosTheta , sinTheta , 0.0 };
|
|||
|
|
resultPose.y_dir = vec3_cross(resultPose.bottomNormal, resultPose.x_dir); //<2F><><EFBFBD>˳<EFBFBD>y_dir
|
|||
|
|
resultPose.length = width;
|
|||
|
|
resultPose.width = height;
|
|||
|
|
resultPose.binTopZ = zSliceZ;
|
|||
|
|
resultPose.minRectVertex[0] = { vertices[0].x, vertices[0].y, binZ };
|
|||
|
|
resultPose.minRectVertex[1] = { vertices[1].x, vertices[1].y, binZ };
|
|||
|
|
resultPose.minRectVertex[2] = { vertices[2].x, vertices[2].y, binZ };
|
|||
|
|
resultPose.minRectVertex[3] = { vertices[3].x, vertices[3].y, binZ };
|
|||
|
|
|
|||
|
|
return resultPose;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
//<2F>Ͽ<EFBFBD><CFBF><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ߴ<EFBFBD>
|
|||
|
|
WD_HRM_workpieceSizeInfo wd_HRM_getWorkpieceSize(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
|||
|
|
std::vector<WD_HRM_workpieceSizeInfo>& standardWorkpieceSize,
|
|||
|
|
const SSG_cornerParam cornerPara,
|
|||
|
|
const SSG_planeCalibPara calibPara,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
WD_HRM_workpieceSizeInfo resultInfo;
|
|||
|
|
memset(&resultInfo, 0, sizeof(WD_HRM_workpieceSizeInfo));
|
|||
|
|
|
|||
|
|
int lineNum = (int)scanLines.size();
|
|||
|
|
if (lineNum == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_3D_DATA_NULL;
|
|||
|
|
return resultInfo;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int linePtNum = (int)scanLines[0].size();
|
|||
|
|
|
|||
|
|
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
|
|||
|
|
bool isGridData = true;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
if (linePtNum != (int)scanLines[line].size())
|
|||
|
|
{
|
|||
|
|
isGridData = false;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
|||
|
|
return resultInfo;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//<2F><>ƽ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
|||
|
|
scanLines_h.resize(linePtNum);
|
|||
|
|
for (int i = 0; i < linePtNum; i++)
|
|||
|
|
scanLines_h[i].resize(lineNum);
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
scanLines_h[j][line] = scanLines[line][j];
|
|||
|
|
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
|||
|
|
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int line = 0; line < linePtNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
|||
|
|
scanLines_h[line][j].nPointIdx = j;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>㷨<EFBFBD><E3B7A8><EFBFBD>̣<EFBFBD>
|
|||
|
|
//1<><31><EFBFBD><EFBFBD><EFBFBD>鴹ֱ<E9B4B9><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD>ȥ<EFBFBD><C8A5>
|
|||
|
|
//2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//3<><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
|
|||
|
|
//4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
SSG_cornerParam removeVertialPara = cornerPara;
|
|||
|
|
removeVertialPara.scale = 3.0;
|
|||
|
|
removeVertialPara.cornerTh = 60;
|
|||
|
|
|
|||
|
|
std::vector<std::vector<int>> flags;
|
|||
|
|
flags.resize(lineNum);
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
{
|
|||
|
|
flags[i].resize(linePtNum);
|
|||
|
|
std::fill(flags[i].begin(), flags[i].end(), 0);
|
|||
|
|
}
|
|||
|
|
std::vector<std::vector<int>> zVertivalFlags;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 700)
|
|||
|
|
int kkk = 1;
|
|||
|
|
std::vector<int> line_verticalFlags;
|
|||
|
|
wd_getXYVertialFeature_dirAngleMethod(
|
|||
|
|
scanLines[line],
|
|||
|
|
line,
|
|||
|
|
removeVertialPara,
|
|||
|
|
line_verticalFlags
|
|||
|
|
);
|
|||
|
|
zVertivalFlags.push_back(line_verticalFlags);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (line_verticalFlags[i] > 0)
|
|||
|
|
flags[line][i] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<std::vector<int>> zVertivalFlags_h;
|
|||
|
|
for (int line = 0; line < linePtNum; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 1177)
|
|||
|
|
int kkk = 1;
|
|||
|
|
std::vector<int> line_verticalFlags;
|
|||
|
|
wd_getXYVertialFeature_dirAngleMethod(
|
|||
|
|
scanLines_h[line],
|
|||
|
|
line,
|
|||
|
|
removeVertialPara,
|
|||
|
|
line_verticalFlags
|
|||
|
|
);
|
|||
|
|
zVertivalFlags_h.push_back(line_verticalFlags);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (line_verticalFlags[i] > 0)
|
|||
|
|
flags[i][line] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
if (flags[line][j] > 0)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].pt3D.z = 0;
|
|||
|
|
scanLines_h[j][line].pt3D.z = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
|||
|
|
SSG_lineSegParam lineSegPara;
|
|||
|
|
lineSegPara.distScale = 5.0;
|
|||
|
|
lineSegPara.segGapTh_y = 5.0;
|
|||
|
|
lineSegPara.segGapTh_z = 5.0;
|
|||
|
|
const int minSegLen = 5;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_RUN> segs;
|
|||
|
|
wd_getLineDataIntervals(
|
|||
|
|
scanLines[line],
|
|||
|
|
lineSegPara,
|
|||
|
|
segs);
|
|||
|
|
for (int i = 0; i < (int)segs.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (segs[i].len <= minSegLen)
|
|||
|
|
{
|
|||
|
|
int idx0 = segs[i].start;
|
|||
|
|
for (int j = 0; j < segs[i].len; j++)
|
|||
|
|
flags[line][idx0 + j] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int line = 0; line < linePtNum; line++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_RUN> segs;
|
|||
|
|
wd_getLineDataIntervals(
|
|||
|
|
scanLines_h[line],
|
|||
|
|
lineSegPara,
|
|||
|
|
segs);
|
|||
|
|
for (int i = 0; i < (int)segs.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (segs[i].len <= minSegLen)
|
|||
|
|
{
|
|||
|
|
int idx0 = segs[i].start;
|
|||
|
|
for (int j = 0; j < segs[i].len; j++)
|
|||
|
|
flags[idx0 + j][line] = 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ע
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD>߶<EFBFBD>ȥ<EFBFBD><C8A5>
|
|||
|
|
std::vector< SVzNL3DPosition> validPoints;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
if (flags[line][j] > 0)
|
|||
|
|
scanLines[line][j].pt3D.z = 0;
|
|||
|
|
|
|||
|
|
if (scanLines[line][j].pt3D.z > 1e-4)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition a_vldPt;
|
|||
|
|
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
|||
|
|
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
|||
|
|
validPoints.push_back(a_vldPt);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>
|
|||
|
|
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//double minObjSize_w = 150;
|
|||
|
|
//double minObjSize_h = 150;
|
|||
|
|
|
|||
|
|
int clusterCheckWin = 5;
|
|||
|
|
double clusterDist = 10.0;
|
|||
|
|
int distType = 1; //0 - 2d distance; 1- 3d distance
|
|||
|
|
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
|||
|
|
wd_pointClustering_speedUp(
|
|||
|
|
validPoints,
|
|||
|
|
lineNum, linePtNum, clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
clusterDist,
|
|||
|
|
distType,
|
|||
|
|
objClusters //result
|
|||
|
|
);
|
|||
|
|
|
|||
|
|
//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<double> objMeanZ;
|
|||
|
|
std::vector<SVzNLRangeD> objZRange;
|
|||
|
|
std::vector< SSG_ROIRectD> objROIs;
|
|||
|
|
objMeanZ.resize(objClusters.size());
|
|||
|
|
objZRange.resize(objClusters.size());
|
|||
|
|
objROIs.resize(objClusters.size());
|
|||
|
|
int maxSizeId = -1;
|
|||
|
|
double maxSize = 0;
|
|||
|
|
for (int i = 0; i < (int)objClusters.size(); i++)
|
|||
|
|
{
|
|||
|
|
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
|||
|
|
objROIs[i] = a_roi;
|
|||
|
|
|
|||
|
|
SVzNLRangeD zRange;
|
|||
|
|
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
|||
|
|
objMeanZ[i] = meanZ;
|
|||
|
|
objZRange[i] = zRange;
|
|||
|
|
|
|||
|
|
double w = a_roi.right - a_roi.left;
|
|||
|
|
double h = a_roi.bottom - a_roi.top;
|
|||
|
|
double size = w * h;
|
|||
|
|
if (maxSize < size)
|
|||
|
|
{
|
|||
|
|
maxSize = size;
|
|||
|
|
maxSizeId = i;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ȡ<><C8A1><EFBFBD><EFBFBD>ROI<4F>ڵ<EFBFBD>Ŀ<EFBFBD><C4BF>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
|
|||
|
|
SSG_ROIRectD& layerBoardROI = objROIs[maxSizeId];
|
|||
|
|
double layerBoardZ = objMeanZ[maxSizeId];
|
|||
|
|
int workpieceClusterId = -1;
|
|||
|
|
for (int i = 0; i< (int)objClusters.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (i == maxSizeId)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
SSG_ROIRectD& a_roi = objROIs[i];
|
|||
|
|
double obj_z = objMeanZ[i];
|
|||
|
|
if ((a_roi.left > layerBoardROI.left) && (a_roi.right < layerBoardROI.right) &&
|
|||
|
|
(a_roi.top > layerBoardROI.top) && (a_roi.bottom < layerBoardROI.bottom) && (obj_z < layerBoardZ))
|
|||
|
|
{
|
|||
|
|
if (workpieceClusterId < 0)
|
|||
|
|
workpieceClusterId = i;
|
|||
|
|
else if (objClusters[workpieceClusterId].size() < objClusters[i].size())
|
|||
|
|
workpieceClusterId = i;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(workpieceClusterId <0)
|
|||
|
|
{
|
|||
|
|
*errCode = SX_ERR_ZERO_OBJECTS;
|
|||
|
|
return resultInfo;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector< SVzNL3DPosition>& layerBoardCluster = objClusters[maxSizeId];
|
|||
|
|
std::vector< SVzNL3DPosition>& workpieceCluster = objClusters[workpieceClusterId];
|
|||
|
|
double workpieceHeight = layerBoardZ - objZRange[workpieceClusterId].min;
|
|||
|
|
//<2F><>ע
|
|||
|
|
for (int i = 0; i < (int)layerBoardCluster.size(); i++)
|
|||
|
|
{
|
|||
|
|
int line = layerBoardCluster[i].nPointIdx >> 16;
|
|||
|
|
int ptIdx = layerBoardCluster[i].nPointIdx & 0x0000FFFF;
|
|||
|
|
scanLines[line][ptIdx].nPointIdx = 1;
|
|||
|
|
}
|
|||
|
|
for (int i = 0; i < (int)workpieceCluster.size(); i++)
|
|||
|
|
{
|
|||
|
|
int line = workpieceCluster[i].nPointIdx >> 16;
|
|||
|
|
int ptIdx = workpieceCluster[i].nPointIdx & 0x0000FFFF;
|
|||
|
|
scanLines[line][ptIdx].nPointIdx = 2;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>㹤<EFBFBD><E3B9A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>Բ
|
|||
|
|
std::vector<cv::Point2f> points_2d;
|
|||
|
|
for (int i = 0; i < (int)workpieceCluster.size(); i++)
|
|||
|
|
{
|
|||
|
|
cv::Point2f a_pt = cv::Point2f(workpieceCluster[i].pt3D.x, workpieceCluster[i].pt3D.y);
|
|||
|
|
points_2d.push_back(a_pt);
|
|||
|
|
}
|
|||
|
|
cv::Point2f center;
|
|||
|
|
float r;
|
|||
|
|
cv::minEnclosingCircle(points_2d, center, r);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><D7BC><EFBFBD><EFBFBD>
|
|||
|
|
double bestError = 0;
|
|||
|
|
int bestId = -1;
|
|||
|
|
for (int i = 0; i < (int)standardWorkpieceSize.size(); i++)
|
|||
|
|
{
|
|||
|
|
double err = abs(standardWorkpieceSize[i].workpieceHeight - workpieceHeight) + abs(standardWorkpieceSize[i].workpieceRadius - r);
|
|||
|
|
if (bestId < 0)
|
|||
|
|
{
|
|||
|
|
bestId = i;
|
|||
|
|
bestError = err;
|
|||
|
|
}
|
|||
|
|
else if (bestError > err)
|
|||
|
|
{
|
|||
|
|
bestId = i;
|
|||
|
|
bestError = err;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>
|
|||
|
|
resultInfo.center = { center.x, center.y, layerBoardZ };
|
|||
|
|
resultInfo.layerZValue = layerBoardZ;
|
|||
|
|
resultInfo.workpieceHeight = standardWorkpieceSize[bestId].workpieceHeight;
|
|||
|
|
resultInfo.workpieceRadius = standardWorkpieceSize[bestId].workpieceRadius;
|
|||
|
|
return resultInfo;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void planningFromCenter_1D(int num, double interval, double centerValue, std::vector<double>& pos)
|
|||
|
|
{
|
|||
|
|
pos.resize(num);
|
|||
|
|
double halfInterval = interval / 2;
|
|||
|
|
if (num % 2 == 1) //<2F><><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
int centerIdx = num / 2;
|
|||
|
|
pos[centerIdx] = centerValue;
|
|||
|
|
int j = 1;
|
|||
|
|
for (int idx = centerIdx - 1; idx >= 0; idx--)
|
|||
|
|
{
|
|||
|
|
pos[idx] = centerValue - j * interval;
|
|||
|
|
j++;
|
|||
|
|
}
|
|||
|
|
j = 1;
|
|||
|
|
for (int idx = centerIdx + 1; idx < num; idx++)
|
|||
|
|
{
|
|||
|
|
pos[idx] = centerValue + j * interval;
|
|||
|
|
j++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
int j = 0;
|
|||
|
|
int halfSize = num / 2;
|
|||
|
|
for (int idx = halfSize - 1; idx >= 0; idx--)
|
|||
|
|
{
|
|||
|
|
pos[idx] = centerValue - halfInterval - j * interval;
|
|||
|
|
j++;
|
|||
|
|
}
|
|||
|
|
j = 0;
|
|||
|
|
for (int idx = halfSize; idx < num; idx++)
|
|||
|
|
{
|
|||
|
|
pos[idx] = centerValue + halfInterval + j * interval;
|
|||
|
|
j++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Ͽ<EFBFBD><CFBF><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ù滮
|
|||
|
|
void wd_HRM_PlanBinPlacement(
|
|||
|
|
const WD_HRM_BinInfo binInfo,
|
|||
|
|
const SSG_size2D realBoardSize, //ʵ<>ʵ<EFBFBD><CAB5>а<EFBFBD><D0B0><EFBFBD>С
|
|||
|
|
const WD_HRM_workpieceSizeInfo workpieceInfo,
|
|||
|
|
const SSG_planeCalibPara calibPara,
|
|||
|
|
const double guardingInterval, //<2F><><EFBFBD><EFBFBD><EFBFBD>빤<EFBFBD><EBB9A4><EFBFBD>ı<EFBFBD><C4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector< WD_workpieceInfo>& planningPositions,
|
|||
|
|
int* out_rows,
|
|||
|
|
int* out_cols,
|
|||
|
|
int* isLastLayere)
|
|||
|
|
{
|
|||
|
|
double diamter = workpieceInfo.workpieceRadius * 2 + guardingInterval;
|
|||
|
|
double L = realBoardSize.width;// binInfo.length - guardingToSide * 2 + guardingInterval;
|
|||
|
|
double W = realBoardSize.height; //binInfo.width - guardingToSide * 2 + guardingInterval;
|
|||
|
|
|
|||
|
|
int cols = (int)(L / diamter);
|
|||
|
|
int rows = (int)(W / diamter);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><D7BC><EFBFBD><EFBFBD>λ<EFBFBD>ü<EFBFBD><C3BC><EFBFBD>
|
|||
|
|
std::vector<double> rowPos;
|
|||
|
|
planningFromCenter_1D(rows, diamter, 0, rowPos);
|
|||
|
|
std::vector<double> colPos;
|
|||
|
|
planningFromCenter_1D(cols, diamter, 0, colPos);
|
|||
|
|
|
|||
|
|
int objNum = rows * cols;
|
|||
|
|
planningPositions.resize(objNum);
|
|||
|
|
for (int row = 0; row < rows; row++)
|
|||
|
|
{
|
|||
|
|
for(int col = 0; col < cols; col++)
|
|||
|
|
{
|
|||
|
|
WD_workpieceInfo a_pos;
|
|||
|
|
a_pos.center = { colPos[col], rowPos[row], workpieceInfo.layerZValue };
|
|||
|
|
a_pos.value = workpieceInfo.workpieceRadius;
|
|||
|
|
a_pos.workpieceType = 1;
|
|||
|
|
a_pos.z_dir = { 0, 0, 1.0 };
|
|||
|
|
a_pos.y_dir = { 0, 0, 0 };
|
|||
|
|
a_pos.x_dir = { 0, 0, 0 };
|
|||
|
|
planningPositions[row * cols + col] = a_pos;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ת
|
|||
|
|
double cosTheta = binInfo.x_dir.x;
|
|||
|
|
double sinTheta = binInfo.x_dir.y;
|
|||
|
|
for (int i = 0; i < (int)planningPositions.size(); i++)
|
|||
|
|
{
|
|||
|
|
cv::Point2f a_pt2D = cv::Point2f(planningPositions[i].center.x, planningPositions[i].center.y);
|
|||
|
|
a_pt2D = _rotate2D(a_pt2D, sinTheta, cosTheta);
|
|||
|
|
planningPositions[i].center.x = a_pt2D.x + binInfo.center.x;
|
|||
|
|
planningPositions[i].center.y = a_pt2D.y + binInfo.center.y;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
double resiH = workpieceInfo.layerZValue - binInfo.binTopZ - workpieceInfo.workpieceHeight;
|
|||
|
|
if (resiH < workpieceInfo.workpieceHeight)
|
|||
|
|
*isLastLayere = 1;
|
|||
|
|
else
|
|||
|
|
*isLastLayere = 0;
|
|||
|
|
*out_rows = rows;
|
|||
|
|
*out_cols = cols;
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>ϵ
|
|||
|
|
for (int i = 0; i < (int)planningPositions.size(); i++)
|
|||
|
|
{
|
|||
|
|
planningPositions[i].center = wd_ptRotate(planningPositions[i].center, calibPara.invRMatrix);
|
|||
|
|
planningPositions[i].x_dir = wd_ptRotate(planningPositions[i].x_dir, calibPara.invRMatrix);
|
|||
|
|
planningPositions[i].y_dir = wd_ptRotate(planningPositions[i].y_dir, calibPara.invRMatrix);
|
|||
|
|
planningPositions[i].z_dir = wd_ptRotate(planningPositions[i].z_dir, calibPara.invRMatrix);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return;
|
|||
|
|
}
|