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

291 lines
8.6 KiB
C++
Raw Normal View History

YouJiang_sheetPositioning v1.0.0 初始版本
2026-03-01 21:16:05 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "YouJiang_sheetPositioning_Export.h"
#include <opencv2/opencv.hpp>
#define DEBUG_OUT_IMAGE 1
std::string m_strVersion = "1.0.0";
const char* wd_sheetPositionVersion(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_sheetPosition_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_sheetPosition_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
//<2F><>Ƭ<EFBFBD><C6AC>λ
//ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>ƥ<EFBFBD>䣺<EFBFBD><E4A3BA>1<EFBFBD><31><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƥ<EFBFBD>䱡Ƭ
void wd_YouJiang_getSheetPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_sheetTemplateParam templateParam,
const SSG_planeCalibPara groundCalibPara,
const SWD_sheetAlgoParam algoParam,
SWD_gripState* opState, //<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>ָʾ<D6B8><CABE>ǰ״̬
int* errCode,
std::vector<SWD_sheetGrasper>& resultObjPositions)
{
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
bool isGridData = true;
int linePtNum = (int)scanLines[0].size();
for (int i = 0; i < lineNum; i++)
{
if (linePtNum != (int)scanLines[i].size())
isGridData = false;//<2F>д<EFBFBD><D0B4><EFBFBD>
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_sheetPosition_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
//<2F><>ȡ<EFBFBD><C8A1>Ƭ<EFBFBD>ϵ<EFBFBD>Բ<EFBFBD><D4B2>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_basicFeature1D>> lineJumpFeatures_v;
for (int line = 0; line < lineNum; line++)
{
if (line == 76)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
if (linePtNum != (int)lineData.size())
isGridData = false;
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, algoParam.filterParam);
std::vector<SSG_basicFeature1D> line_jumpFeatures;
int dataSize = (int)lineData.size();
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>Jumping<6E><67><EFBFBD><EFBFBD>
sg_getLineLJumpFeature_cornerMethod(
&lineData[0],
dataSize,
line,
algoParam.cornerParam,
true, //ȡ<><C8A1><EFBFBD><EFBFBD>
line_jumpFeatures);
lineJumpFeatures_v.push_back(line_jumpFeatures);
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
hLines_raw.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
hLines_raw[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_raw[j][line] = scanLines[line][j];
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
//ˮƽjump<6D><70><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_basicFeature1D>> lineJumpFeatures_h;
int lineNum_h_raw = (int)hLines_raw.size();
for (int line = 0; line < lineNum_h_raw; line++)
{
if (line == 522)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
int ptNum = (int)lineData.size();
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, algoParam.filterParam);
std::vector<SSG_basicFeature1D> line_jumpFeatures;
int dataSize = (int)lineData.size();
/// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>Jumping<6E><67><EFBFBD><EFBFBD>
sg_getLineLJumpFeature_cornerMethod(
&lineData[0],
dataSize,
line,
algoParam.cornerParam,
true, //ȡ<><C8A1><EFBFBD><EFBFBD>
line_jumpFeatures);
lineJumpFeatures_h.push_back(line_jumpFeatures);
}
//<2F><>ע
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
featureInfoMask.resize(lineNum);
feature3DInfo.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
featureInfoMask[i].resize(lineNum_h_raw);
feature3DInfo[i].resize(lineNum_h_raw);
}
//<2F><>ֱ<EFBFBD><D6B1>ע
for (int line = 0; line < lineNum; line++)
{
if (line == 390)
int kkk = 1;
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = lineJumpFeatures_v[line];
for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
{
int px = a_lineJumpFeature[m].jumpPos2D.x;
int py = a_lineJumpFeature[m].jumpPos2D.y;
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
a_featureInfo.clusterID = 0;
a_featureInfo.featurType = 1;
a_featureInfo.featureIdx_v = m;
a_featureInfo.featureIdx_h = 0;
a_featureInfo.lineIdx = px;
a_featureInfo.ptIdx = py;
a_featureInfo.flag = 0;
SVzNL3DPoint& a_feature3D = feature3DInfo[px][py];
a_feature3D = a_lineJumpFeature[m].jumpPos;
scanLines[px][py].nPointIdx = 1;
}
}
//ˮƽ<CBAE><C6BD>ע
for (int line = 0; line < lineNum_h_raw; line++)
{
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = lineJumpFeatures_h[line];
for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
{
int py = a_lineJumpFeature[m].jumpPos2D.x;
int px = a_lineJumpFeature[m].jumpPos2D.y;
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
if (a_featureInfo.featurType == 0)
{
a_featureInfo.clusterID = 0;
a_featureInfo.lineIdx = px;
a_featureInfo.ptIdx = py;
a_featureInfo.flag = 0;
}
a_featureInfo.featurType += 2;
a_featureInfo.featureIdx_h = m;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[px][py];
a_feature3DValue = { a_lineJumpFeature[m].jumpPos.y, a_lineJumpFeature[m].jumpPos.x, a_lineJumpFeature[m].jumpPos.z };
scanLines[px][py].nPointIdx += 2;
}
}
//<2F><><EFBFBD><EFBFBD>
//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
std::vector<std::vector< SVzNL2DPoint>> clusters; //ֻ<><D6BB>¼λ<C2BC><CEBB>
std::vector<SVzNL3DRangeD> clustersRoi3D;
int clusterID = 1;
int clusterCheckWin = 3;
for (int y = 0; y < lineNum_h_raw; y++)
{
for (int x = 0; x < lineNum; x++)
{
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
continue;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
SVzNL3DRangeD a_clusterRoi;
a_clusterRoi.xRange.min = a_feature3DValue.x;
a_clusterRoi.xRange.max = a_feature3DValue.x;
a_clusterRoi.yRange.min = a_feature3DValue.y;
a_clusterRoi.yRange.max = a_feature3DValue.y;
a_clusterRoi.zRange.min = a_feature3DValue.z;
a_clusterRoi.zRange.max = a_feature3DValue.z;
SVzNL2DPoint a_seedPos = { x, y };
std::vector< SVzNL2DPoint> a_cluster;
a_cluster.push_back(a_seedPos);
wd_gridPointClustering(
featureInfoMask,//int<6E><74><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>clusterID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>flag
feature3DInfo,//double,<2C><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
algoParam.growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
clusterID, //<2F><>ǰCluster<65><72>ID
a_cluster, //result
a_clusterRoi
);
clusters.push_back(a_cluster);
clustersRoi3D.push_back(a_clusterRoi);
clusterID++;
}
}
int clusterNum = (int)clusters.size();
//ͶӰ<CDB6>ͱ<EFBFBD>ע<EFBFBD><D7A2>ͶӰ<CDB6><D3B0>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>ƥ<EFBFBD><C6A5>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1mmΪ<6D><CEAA>λ.<2E><>¼clusterID
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
double projectionScale = 1.0; //<2F><>1mmΪͶӰ<CDB6>߶<EFBFBD>
int projecction_xw = (int)((roi3D.xRange.max - roi3D.xRange.min) / projectionScale) + 1;
int projecction_yh = (int)((roi3D.yRange.max - roi3D.yRange.min) / projectionScale) + 1;
std::vector<std::vector<int>> projectionClusters;
projectionClusters.resize(projecction_xw);
for (int i = 0; i < projecction_yh; i++)
projectionClusters[i].resize(projecction_yh);
for (int i = 0; i < clusterNum; i++)
{
std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
for (int j = 0; j < (int)clusters.size(); j++)
{
SVzNL2DPoint& a_pt = a_cluster[i];
SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
int px = (int)((a_pt3d.pt3D.x - roi3D.xRange.min) / projectionScale);
int py = (int)((a_pt3d.pt3D.y - roi3D.yRange.min) / projectionScale);
projectionClusters[px][py] = i + 1; //clusterID = index + 1
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>ݴ<EFBFBD>Сȷ<D0A1><C8B7>Բ<EFBFBD><D4B2>
std::vector<SVzNL3DPoint> clustersCenter;
clustersCenter.resize(clusters.size());
std::vector<int> validIndice;
double diameter_th = templateParam.holeDiameter * 0.1;
for (int i = 0; i < clusterNum; i++)
{
double xw = clustersRoi3D[i].xRange.max - clustersRoi3D[i].xRange.min;
double yh = clustersRoi3D[i].yRange.max - clustersRoi3D[i].yRange.min;
double xw_diff = abs(xw - templateParam.holeDiameter);
double yh_diff = abs(yh - templateParam.holeDiameter);
if ((xw_diff < diameter_th) && (yh_diff < diameter_th))
{
validIndice.push_back(i);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
}
}
//Բ<><D4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
}
Reference in New Issue Copy Permalink