422 lines
12 KiB
C++
422 lines
12 KiB
C++
#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();
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_sheetPosition_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void wd_sheetPosition_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
//提取边缘封闭目标的边缘特征，输出聚类簇
|
||
void wd_computeClosedEdgeClusters(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SWD_sheetAlgoParam algoParam,
|
||
int* errCode,
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||
std::vector<SVzNL3DRangeD>& clustersRoi3D)
|
||
{
|
||
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;//行处理
|
||
//调平，去除地面
|
||
wd_sheetPosition_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
//提取薄片上的圆孔
|
||
//垂直跳变特征提取
|
||
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;
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, algoParam.filterParam);
|
||
|
||
std::vector<SSG_basicFeature1D> line_jumpFeatures;
|
||
int dataSize = (int)lineData.size();
|
||
/// 提取激光线上的Jumping特征
|
||
sg_getLineLJumpFeature_cornerMethod(
|
||
&lineData[0],
|
||
dataSize,
|
||
line,
|
||
algoParam.cornerParam,
|
||
true, //取上沿
|
||
line_jumpFeatures);
|
||
lineJumpFeatures_v.push_back(line_jumpFeatures);
|
||
}
|
||
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
//生成水平扫描
|
||
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; //将原始数据的序列清0（会转义使用）
|
||
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特征提取
|
||
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];
|
||
//滤波，滤除异常点
|
||
int ptNum = (int)lineData.size();
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, algoParam.filterParam);
|
||
|
||
std::vector<SSG_basicFeature1D> line_jumpFeatures;
|
||
int dataSize = (int)lineData.size();
|
||
/// 提取激光线上的Jumping特征
|
||
sg_getLineLJumpFeature_cornerMethod(
|
||
&lineData[0],
|
||
dataSize,
|
||
line,
|
||
algoParam.cornerParam,
|
||
true, //取上沿
|
||
line_jumpFeatures);
|
||
lineJumpFeatures_h.push_back(line_jumpFeatures);
|
||
}
|
||
//标注
|
||
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);
|
||
}
|
||
|
||
//垂直标注
|
||
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;
|
||
}
|
||
}
|
||
//水平标注
|
||
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;
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
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)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||
SVzNL3DRangeD a_clusterRoi;
|
||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
wd_gridPointClustering(
|
||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||
feature3DInfo,//double,记录坐标信息
|
||
clusterCheckWin, //搜索窗口
|
||
algoParam.growParam,//聚类条件
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
clustersRoi3D.push_back(a_clusterRoi);
|
||
clusterID++;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
//薄片定位
|
||
//使用语义模块匹配：（1）提取特征（2）基于特征匹配薄片
|
||
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, //操作状态机。指示当前状态
|
||
int* errCode,
|
||
std::vector<SWD_sheetGrasper>& resultObjPositions)
|
||
{
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
|
||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||
//提取边缘封闭目标的边缘特征，输出聚类簇
|
||
wd_computeClosedEdgeClusters(
|
||
scanLines,
|
||
groundCalibPara,
|
||
algoParam,
|
||
errCode,
|
||
clusters, //只记录位置
|
||
clustersRoi3D);
|
||
|
||
int clusterNum = (int)clusters.size();
|
||
//投影和标注。投影的目的是用于模板匹配时的特征处理。以1mm为单位.记录clusterID
|
||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
|
||
double projectionScale = 1.0; //以1mm为投影尺度
|
||
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)a_cluster.size(); j++)
|
||
{
|
||
SVzNL2DPoint& a_pt = a_cluster[j];
|
||
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);
|
||
if ((px < 0) || (px >= projecction_xw) || (py < 0) || (py >= projecction_yh))
|
||
int kkk = 1;
|
||
projectionClusters[px][py] = i + 1; //clusterID = index + 1
|
||
}
|
||
}
|
||
|
||
//聚类结果分析
|
||
//根据大小确定圆孔
|
||
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);
|
||
//计算中心点
|
||
|
||
}
|
||
}
|
||
//圆孔配对
|
||
|
||
|
||
|
||
//确定正反和抓取点
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
typedef struct
|
||
{
|
||
cv::RotatedRect rect;
|
||
cv::Point2f vertices[4];
|
||
}_RectParam;
|
||
//薄片按键高度测量
|
||
void wd_computeSheetKeyHeigth(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SWD_sheetKeyParam sheetKeyParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SWD_sheetAlgoParam algoParam,
|
||
SWD_gripState* opState, //操作状态机。指示当前状态
|
||
int* errCode,
|
||
std::vector<SWD_sheetGrasper>& resultObjPositions)
|
||
{
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
|
||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||
//提取边缘封闭目标的边缘特征，输出聚类簇
|
||
wd_computeClosedEdgeClusters(
|
||
scanLines,
|
||
groundCalibPara,
|
||
algoParam,
|
||
errCode,
|
||
clusters, //只记录位置
|
||
clustersRoi3D);
|
||
int clusterNum = (int)clusters.size();
|
||
|
||
//聚类结果分析
|
||
//根据大小确定薄片
|
||
const double sameSizeTh_w = sheetKeyParam.sheetSize.width * 0.05;
|
||
const double sameSizeTh_h = sheetKeyParam.sheetSize.height * 0.05;
|
||
const double zAreaWin = 5;
|
||
|
||
std::vector<_RectParam> clustersRect;
|
||
clustersRect.resize(clusters.size());
|
||
#if 1
|
||
std::vector<int> validIndice;
|
||
|
||
for (int i = 0; i < clusterNum; i++)
|
||
{
|
||
std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
|
||
if (a_cluster.size() < 4)
|
||
continue;
|
||
//计算最小外接矩
|
||
std::vector<cv::Point2f> points;
|
||
SVzNLRect roi = { -1,-1,-1,-1 };
|
||
for (int j = 0; j < (int)a_cluster.size(); j++)
|
||
{
|
||
SVzNL2DPoint& a_pt = a_cluster[j];
|
||
SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
|
||
//生成ROI
|
||
if (roi.left < 0)
|
||
roi = { a_pt.x ,a_pt.x , a_pt.y, a_pt.y };
|
||
else
|
||
{
|
||
if (roi.left > a_pt.x)
|
||
roi.left = a_pt.x;
|
||
if (roi.right < a_pt.x)
|
||
roi.right = a_pt.x;
|
||
if (roi.top > a_pt.y)
|
||
roi.top = a_pt.y;
|
||
if (roi.bottom < a_pt.y)
|
||
roi.bottom = a_pt.y;
|
||
}
|
||
float x = (float)a_pt3d.pt3D.x;
|
||
float y = (float)a_pt3d.pt3D.y;
|
||
points.push_back(cv::Point2f(x, y));
|
||
}
|
||
if (points.size() == 0)
|
||
continue;
|
||
|
||
// 最小外接矩形
|
||
cv::RotatedRect rect = minAreaRect(points);
|
||
cv::Point2f vertices[4];
|
||
rect.points(vertices);
|
||
double width = rect.size.width; //投影的宽和高， 对应袋子的长和宽
|
||
double height = rect.size.height;
|
||
if (width < height)
|
||
{
|
||
double tmp = height;
|
||
height = width;
|
||
width = tmp;
|
||
}
|
||
|
||
int cx = (roi.left + roi.right) / 2;
|
||
int cy = (roi.top + roi.bottom) / 2;
|
||
//判别
|
||
double w_diff = abs(width - sheetKeyParam.sheetSize.width);
|
||
double h_diff = abs(height - sheetKeyParam.sheetSize.height);
|
||
if ((w_diff < sameSizeTh_w) && (h_diff < sameSizeTh_h))
|
||
{
|
||
//寻找到目标
|
||
//取中心点，以中心点附近区域计算Z
|
||
int cx = (roi.left + roi.right) / 2;
|
||
int cy = (roi.top + roi.bottom) / 2;
|
||
SVzNLRect meanZRoi = { cx - zAreaWin , cx + zAreaWin , cy - zAreaWin , cy + zAreaWin };
|
||
double meanZ = computeROIMeanZ(scanLines, meanZRoi);
|
||
//以Z值作为门限进行切割
|
||
double cutZ = meanZ - sheetKeyParam.nominalKeyHeight * 0.75;
|
||
int labelID = i + 100; //ID从100开始
|
||
computeROIZCutLabel(scanLines, roi, cutZ, labelID);
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
} |