964 lines
30 KiB
C++
964 lines
30 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "bagThreadPositioning_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
//version 1.1.0 : 添加了标定Mark检测，添加了相对于标定柱的坐标输出，添加了相机调平功能
|
||
std::string m_strVersion = "1.1.0";
|
||
const char* wd_bagThreadPositioningVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_bagThread_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void wd_bagThread_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
#if 1
|
||
|
||
SVzNL3DPosition _findClosestPoint(std::vector<SVzNL3DPosition>& lineData, double a, double b, double c, double* distMin)
|
||
{
|
||
SVzNL3DPosition bestPt = { -1, { 0, 0, -1 } };
|
||
|
||
double mod = sqrt(a * a + b * b);
|
||
a = a / mod;
|
||
b = b / mod;
|
||
c = c / mod;
|
||
double minDist = DBL_MAX;
|
||
for (int i = 0; i < (int)lineData.size(); i++)
|
||
{
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
double dist = abs(a * lineData[i].pt3D.x + b * lineData[i].pt3D.y + c);
|
||
if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
bestPt = lineData[i];
|
||
bestPt.nPointIdx = i;
|
||
}
|
||
}
|
||
}
|
||
*distMin = minDist;
|
||
return bestPt;
|
||
}
|
||
|
||
//聚类
|
||
void cloutPointsClustering(
|
||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||
SSG_treeGrowParam growParam,
|
||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||
std::vector<SWD_clustersInfo>& clustersInfo)
|
||
{
|
||
int lineNum = (int)feature3DInfo.size();
|
||
if (lineNum == 0)
|
||
return;
|
||
int linePtNum = (int)feature3DInfo[0].size();
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
int clusterID = 1;
|
||
int clusterCheckWin = 5;
|
||
for (int y = 0; y < linePtNum; y++)
|
||
{
|
||
for (int x = 0; x < lineNum; x++)
|
||
{
|
||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||
SVzNL3DRangeD a_clusterRoi;
|
||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
wd_gridPointClustering(
|
||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||
feature3DInfo,//double,记录坐标信息
|
||
clusterCheckWin, //搜索窗口
|
||
growParam,//聚类条件
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
SWD_clustersInfo a_info;
|
||
a_info.clusterIdx = clusterID;
|
||
a_info.ptSize = (int)a_cluster.size();
|
||
a_info.roi3D = a_clusterRoi;
|
||
clustersInfo.push_back(a_info);
|
||
clusterID++;
|
||
}
|
||
}
|
||
}
|
||
|
||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||
SVzNL3DPoint rotateXoY(SVzNL3DPoint& pt, double sinTheta, double cosTheta)
|
||
{
|
||
return (SVzNL3DPoint{ (pt.x * cosTheta - pt.y * sinTheta), (pt.x * sinTheta + pt.y * cosTheta), pt.z });
|
||
}
|
||
|
||
//线头位置检测定位
|
||
void wd_bagThreadPositioning(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SSG_outlierFilterParam filterParam, //噪点过滤参数
|
||
const SSG_cornerParam cornerPara, //V型特征参数
|
||
const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
|
||
const SSG_treeGrowParam growParam, //特征生长参数
|
||
std::vector<SSX_bagThreadInfo>& bagThreadInfo,
|
||
std::vector<SSX_bagThreadInfo>& bagThreadInfo_relative, //相对于Mark的输出
|
||
std::vector<SVzNL3DPoint>& output_markCenter, //输出Mark位置信息
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
if (true == scanInfo.isHorizonScan)
|
||
{
|
||
*errCode = SG_ERR_LASER_DIR_NOT_SUPPORTED;
|
||
return;
|
||
}
|
||
if (false == scanInfo.scanFromThreadHead)
|
||
{
|
||
*errCode = SG_ERR_SCAN_DIR_NOT_SUPPORTED;
|
||
return;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
//调平。
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_bagThread_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
|
||
//生成水平扫描数据
|
||
//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
|
||
double ptInterval = 0;
|
||
int ptIntevalNum = 0;
|
||
std::vector< std::vector<SVzNL3DPosition>> data_lines_h; //水平扫描数据
|
||
data_lines_h.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
data_lines_h[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
//scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
data_lines_h[j][line] = scanLines[line][j];
|
||
data_lines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
data_lines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
if (j > 0)
|
||
{
|
||
if ((scanLines[line][j - 1].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z > 1e-4))
|
||
{
|
||
ptInterval += abs(scanLines[line][j].pt3D.y - scanLines[line][j - 1].pt3D.y);
|
||
ptIntevalNum++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if(ptIntevalNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
ptInterval = ptInterval / ptIntevalNum;
|
||
int lineNum_h = linePtNum;
|
||
int linePtNum_h = (int)data_lines_h[0].size();
|
||
double lineInterval = 0;
|
||
int lineIntervalNum = 0;
|
||
for (int line = 0; line< lineNum_h; line++)
|
||
{
|
||
for (int j = 0, j_max = (int)data_lines_h[line].size(); j < j_max; j++)
|
||
{
|
||
data_lines_h[line][j].nPointIdx = j;
|
||
if (j > 0)
|
||
{
|
||
if ((data_lines_h[line][j - 1].pt3D.z > 1e-4) && (data_lines_h[line][j].pt3D.z > 1e-4))
|
||
{
|
||
lineInterval += abs(data_lines_h[line][j].pt3D.y - data_lines_h[line][j - 1].pt3D.y);
|
||
lineIntervalNum++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (lineIntervalNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
lineInterval = lineInterval / lineIntervalNum;
|
||
|
||
SVzNLRangeD jumpHeight = { scanInfo.mark_height-1.0, scanInfo.mark_height+1.0};
|
||
double markRotateAngle = 0;
|
||
SVzNL3DPoint markCenter = { 0,0,0 };
|
||
std::vector< SVzNL2DPoint> debug_markOrigin;
|
||
std::vector< SVzNL2DPoint> debug_markXDir;
|
||
|
||
double vCornerScale = cornerPara.scale * 4;
|
||
std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
|
||
std::vector<std::vector<SWD_segFeature>> raisedFeatures;
|
||
//现场要求垂直扫描
|
||
//if (false == scanInfo.isHorizonScan) //
|
||
{
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
|
||
|
||
int validVCornerSCale = (int)(vCornerScale / ptInterval);
|
||
//垂直扫描检测V型槽和线尾
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if ((line == 750) || (line == 905))
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
//提取V型槽
|
||
std::vector<SSG_basicFeature1D> line_cornerFeatures;
|
||
std::vector<SSG_RUN_EX> segs;
|
||
int dataSize = (int)lineData.size();
|
||
wd_getLineCorerFeature_accelerate(
|
||
lineData,
|
||
line,
|
||
cornerPara,
|
||
ptInterval,
|
||
segs,
|
||
line_cornerFeatures //拐点
|
||
);
|
||
//检查V型特征两侧，不能有跳变
|
||
std::vector<SSG_basicFeature1D> valid_cornerFeatures;
|
||
int vCornerSize = (int)line_cornerFeatures.size();
|
||
int segSize = (int)segs.size();
|
||
for (int m = 0; m < vCornerSize; m++)
|
||
{
|
||
int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
|
||
for (int n = 0; n < segSize; n++)
|
||
{
|
||
int segEnd = segs[n].start + segs[n].len - 1;
|
||
if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
|
||
{
|
||
int skip_1 = cornerPos - segs[n].start;
|
||
int skip_2 = segEnd - cornerPos;
|
||
if((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
|
||
valid_cornerFeatures.push_back(line_cornerFeatures[m]);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
cornerFeatures.push_back(valid_cornerFeatures);
|
||
|
||
//提取凸起段
|
||
std::vector<SWD_segFeature> line_raisedFeatures;
|
||
wd_getLineRaisedFeature(
|
||
lineData,
|
||
line,
|
||
raisedFeaturePara, //凸起参数
|
||
line_raisedFeatures //凸起
|
||
);
|
||
raisedFeatures.push_back(line_raisedFeatures);
|
||
|
||
//提取标定柱跳变特征
|
||
std::vector<SSG_basicFeature1D> line_jumpFearures;
|
||
wd_getSpecifiedHeightJumping(
|
||
lineData,
|
||
line,
|
||
jumpHeight, //跳变高度参数
|
||
line_jumpFearures //跳变
|
||
);
|
||
jumpFeatures_v.push_back(line_jumpFearures);
|
||
}
|
||
#if 1
|
||
//水平扫描检测标定柱
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
|
||
for (int line = 0; line < lineNum_h; line++)
|
||
{
|
||
if ((line == 750) || (line == 905))
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
|
||
//提取标定柱跳变特征
|
||
std::vector<SSG_basicFeature1D> line_jumpFearures;
|
||
wd_getSpecifiedHeightJumping(
|
||
lineData,
|
||
line,
|
||
jumpHeight, //跳变高度参数
|
||
line_jumpFearures //跳变
|
||
);
|
||
jumpFeatures_h.push_back(line_jumpFearures);
|
||
}
|
||
|
||
//对标定柱进行聚类
|
||
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
|
||
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
|
||
featureInfoMask.resize(lineNum);
|
||
feature3DInfo.resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
featureInfoMask[line].resize(linePtNum);
|
||
std::fill(featureInfoMask[line].begin(), featureInfoMask[line].end(), SSG_featureClusteringInfo{ 0,0,0,0,0,0,0 });
|
||
feature3DInfo[line].resize(linePtNum);
|
||
std::fill(feature3DInfo[line].begin(), feature3DInfo[line].end(), SVzNL3DPoint{ 0,0,0 });
|
||
}
|
||
//垂直
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
int lineJumpNum = (int)jumpFeatures_v[line].size();
|
||
for(int j = 0; j <lineJumpNum; j++)
|
||
{
|
||
SSG_basicFeature1D& a_jump = jumpFeatures_v[line][j];
|
||
SSG_featureClusteringInfo a_feature;
|
||
a_feature.featurType = a_jump.featureType;
|
||
a_feature.featureIdx_v = j;
|
||
a_feature.featureIdx_h = -1;
|
||
a_feature.clusterID = 0;
|
||
a_feature.flag = 0;
|
||
a_feature.lineIdx = line;
|
||
a_feature.ptIdx = a_jump.jumpPos2D.y;
|
||
|
||
int ptIdx = a_jump.jumpPos2D.y;
|
||
featureInfoMask[line][ptIdx] = a_feature;
|
||
feature3DInfo[line][ptIdx] = a_jump.jumpPos;
|
||
}
|
||
}
|
||
//水平
|
||
for (int line = 0; line < lineNum_h; line++)
|
||
{
|
||
if (line == 1067)
|
||
int kkk = 1;
|
||
int lineJumpNum = (int)jumpFeatures_h[line].size();
|
||
for (int j = 0; j < lineJumpNum; j++)
|
||
{
|
||
SSG_basicFeature1D& a_jump = jumpFeatures_h[line][j];
|
||
SSG_featureClusteringInfo a_feature;
|
||
a_feature.featurType = a_jump.featureType;
|
||
a_feature.featureIdx_v = -1;
|
||
a_feature.featureIdx_h = j;
|
||
a_feature.clusterID = 0;
|
||
a_feature.flag = 0;
|
||
a_feature.lineIdx = a_jump.jumpPos2D.y;
|
||
a_feature.ptIdx = line;
|
||
|
||
int ptIdx = a_jump.jumpPos2D.y;
|
||
if (featureInfoMask[ptIdx][line].featurType == 0) //不重复添加
|
||
{
|
||
featureInfoMask[ptIdx][line] = a_feature;
|
||
feature3DInfo[ptIdx][line] = { a_jump.jumpPos.y, a_jump.jumpPos.x, a_jump.jumpPos.z };
|
||
}
|
||
}
|
||
}
|
||
//聚类
|
||
std::vector<std::vector< SVzNL2DPoint>> markClusters; //只记录位置
|
||
std::vector<SWD_clustersInfo> markClustersInfo;
|
||
SSG_treeGrowParam markGrowParam;
|
||
markGrowParam.yDeviation_max = 1.0;//生长时相邻特征最大的Y偏差
|
||
markGrowParam.zDeviation_max = 1.0; //生长时相邻特征最大的Z偏差
|
||
markGrowParam.maxLineSkipNum = 5; //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
|
||
markGrowParam.maxSkipDistance = 1.0; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
|
||
markGrowParam.minLTypeTreeLen = 5.0; //生长树最少的节点数目。小于此数目的生长树被移除
|
||
markGrowParam.minVTypeTreeLen = 5.0; //生长树最少的节点数目。小于此数目的生长树被移除
|
||
cloutPointsClustering(
|
||
featureInfoMask,
|
||
feature3DInfo,
|
||
markGrowParam,
|
||
markClusters, //只记录位置
|
||
markClustersInfo);
|
||
|
||
//判断标定柱是否检出
|
||
int clusterNum = (int)markClustersInfo.size();
|
||
if (clusterNum < 2)
|
||
{
|
||
*errCode = SX_ERR_NO_MARK;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
std::vector<int> validMarks;
|
||
for (int i = 0; i < clusterNum; i++)
|
||
{
|
||
double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min;
|
||
double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min;
|
||
if ((w >= scanInfo.mark_diameter - 0.5) && (w <= scanInfo.mark_diameter + 1.0) &&
|
||
(h >= scanInfo.mark_diameter - 0.5) && (h <= scanInfo.mark_diameter + 1.0))
|
||
{
|
||
validMarks.push_back(i);
|
||
}
|
||
}
|
||
|
||
int mark_O_idx = -1, mark_x_idx = -1;
|
||
if (validMarks.size() < 2)
|
||
{
|
||
*errCode = SX_ERR_NO_MARK;
|
||
return;
|
||
}
|
||
else if (validMarks.size() == 2)
|
||
{
|
||
int mark0Idx = validMarks[0];
|
||
int mark1Idx = validMarks[1];
|
||
double cy_1 = (markClustersInfo[mark0Idx].roi3D.yRange.max + markClustersInfo[mark0Idx].roi3D.yRange.min) / 2;
|
||
double cy_2 = (markClustersInfo[mark1Idx].roi3D.yRange.max + markClustersInfo[mark1Idx].roi3D.yRange.min) / 2;
|
||
if (cy_1 < cy_2)
|
||
{
|
||
mark_O_idx = mark0Idx;
|
||
mark_x_idx = mark1Idx;
|
||
}
|
||
else
|
||
{
|
||
mark_O_idx = mark1Idx;
|
||
mark_x_idx = mark0Idx;
|
||
}
|
||
}
|
||
else// if (validMarks.size() > 2)
|
||
{
|
||
//搜索距离最近的一组
|
||
SSG_intPair obj_pair = {-1,-1,-1};
|
||
double best_dist = -1;
|
||
for (int i = 0; i < (int)validMarks.size(); i++)
|
||
{
|
||
int vldIdx0 = validMarks[i];
|
||
double cx_1 = (markClustersInfo[vldIdx0].roi3D.xRange.max + markClustersInfo[vldIdx0].roi3D.xRange.min) / 2;
|
||
double cy_1 = (markClustersInfo[vldIdx0].roi3D.yRange.max + markClustersInfo[vldIdx0].roi3D.yRange.min) / 2;
|
||
for (int j = i + 1; j < (int)validMarks.size(); j++)
|
||
{
|
||
int vldIdx1 = validMarks[j];
|
||
double cx_2 = (markClustersInfo[vldIdx1].roi3D.xRange.max + markClustersInfo[vldIdx1].roi3D.xRange.min) / 2;
|
||
double cy_2 = (markClustersInfo[vldIdx1].roi3D.yRange.max + markClustersInfo[vldIdx1].roi3D.yRange.min) / 2;
|
||
double dist = sqrt(pow(cx_1 - cx_2, 2) + pow(cy_1 - cy_2, 2));
|
||
if (best_dist < 0)
|
||
{
|
||
best_dist = dist;
|
||
obj_pair.data_0 = vldIdx0;
|
||
obj_pair.data_1 = vldIdx1;
|
||
}
|
||
else
|
||
{
|
||
double dist_diff1 = abs(best_dist - scanInfo.mark_distance);
|
||
double dist_diff2 = abs(dist - scanInfo.mark_distance);
|
||
if (dist_diff1 > dist_diff2)
|
||
{
|
||
best_dist = dist;
|
||
obj_pair.data_0 = vldIdx0;
|
||
obj_pair.data_1 = vldIdx1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (obj_pair.data_0 < 0)
|
||
{
|
||
*errCode = SX_ERR_NO_MARK;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
double cy_1 = (markClustersInfo[obj_pair.data_0].roi3D.yRange.max + markClustersInfo[obj_pair.data_0].roi3D.yRange.min) / 2;
|
||
double cy_2 = (markClustersInfo[obj_pair.data_1].roi3D.yRange.max + markClustersInfo[obj_pair.data_1].roi3D.yRange.min) / 2;
|
||
if (cy_1 < cy_2)
|
||
{
|
||
mark_O_idx = obj_pair.data_0;
|
||
mark_x_idx = obj_pair.data_1;
|
||
}
|
||
else
|
||
{
|
||
mark_O_idx = obj_pair.data_1;
|
||
mark_x_idx = obj_pair.data_0;
|
||
}
|
||
}
|
||
}
|
||
//圆拟合计算圆心，以点Z值均值作为圆心的Z值
|
||
std::vector<SVzNL3DPoint> fittingData_0;
|
||
double meanZ_0 = 0;
|
||
for (int i = 0; i < markClusters[mark_O_idx].size(); i++)
|
||
{
|
||
SVzNL2DPoint& a_pt2D = markClusters[mark_O_idx][i];
|
||
SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y];
|
||
meanZ_0 += a_pt3D.z;
|
||
fittingData_0.push_back(a_pt3D);
|
||
}
|
||
meanZ_0 = meanZ_0 / (double)markClusters[mark_O_idx].size();
|
||
SVzNL3DPoint mark0_center;
|
||
double mark0_radius;
|
||
double fitErr1 = fitCircleByLeastSquare(
|
||
fittingData_0,
|
||
mark0_center,
|
||
mark0_radius);
|
||
mark0_center.z = meanZ_0;
|
||
|
||
std::vector<SVzNL3DPoint> fittingData_1;
|
||
double meanZ_1 = 0;
|
||
for (int i = 1; i < markClusters[mark_x_idx].size(); i++)
|
||
{
|
||
SVzNL2DPoint& a_pt2D = markClusters[mark_x_idx][i];
|
||
SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y];
|
||
meanZ_1 += a_pt3D.z;
|
||
fittingData_1.push_back(a_pt3D);
|
||
}
|
||
meanZ_1 = meanZ_1 / (double)markClusters[mark_x_idx].size();
|
||
SVzNL3DPoint mark1_center;
|
||
double mark1_radius;
|
||
double fitErr2 = fitCircleByLeastSquare(
|
||
fittingData_1,
|
||
mark1_center,
|
||
mark1_radius);
|
||
mark1_center.z = meanZ_1;
|
||
|
||
output_markCenter.push_back(mark0_center);
|
||
output_markCenter.push_back(mark1_center);
|
||
debug_markOrigin.insert(debug_markOrigin.end(), markClusters[mark_O_idx].begin(), markClusters[mark_O_idx].end());
|
||
debug_markXDir.insert(debug_markXDir.end(), markClusters[mark_x_idx].begin(), markClusters[mark_x_idx].end());
|
||
|
||
markCenter = mark0_center;
|
||
//计算旋转角
|
||
SVzNL2DPointD a = { mark1_center.x - mark0_center.x, mark1_center.y - mark0_center.y };
|
||
SVzNL2DPointD b = { 1, 0 };
|
||
double rotAngle = 0;
|
||
bool validRotate = calcRotateAngle(a, b, rotAngle);
|
||
markRotateAngle = rotAngle;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
//特征生长
|
||
std::vector< SSG_featureTree> cornerGrowTrees;
|
||
sg_cornerFeaturesGrowing(
|
||
cornerFeatures,
|
||
cornerGrowTrees,
|
||
growParam);
|
||
std::vector<SWD_segFeatureTree> raisedFeatureGrowTrees;
|
||
wd_getSegFeatureGrowingTrees(
|
||
raisedFeatures,
|
||
raisedFeatureGrowTrees,
|
||
growParam);
|
||
|
||
if (cornerGrowTrees.size() == 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
|
||
int threadTailTreeIdx = -1; //线尾所在的tree
|
||
if (raisedTreeNum == 1)
|
||
threadTailTreeIdx = 0;
|
||
else if (raisedTreeNum > 1)
|
||
{
|
||
//取最长的tree作为线尾所在的tree
|
||
int maxLines = 0;
|
||
threadTailTreeIdx = 0;
|
||
for (int i = 0; i < raisedTreeNum; i++)
|
||
{
|
||
int treeLines = raisedFeatureGrowTrees[i].eLineIdx - raisedFeatureGrowTrees[i].sLineIdx;
|
||
if (maxLines < treeLines)
|
||
{
|
||
maxLines = treeLines;
|
||
threadTailTreeIdx = i;
|
||
}
|
||
}
|
||
}
|
||
int cornerTreeNum = (int)cornerGrowTrees.size();
|
||
int objTreeIdx = -1;
|
||
if (threadTailTreeIdx < 0)
|
||
{
|
||
int maxLines = 0;
|
||
//取最长的tree作为线缝所有的生长树
|
||
for (int i = 0; i < cornerTreeNum; i++)
|
||
{
|
||
int treeLines = cornerGrowTrees[i].eLineIdx - cornerGrowTrees[i].sLineIdx;
|
||
if (maxLines < treeLines)
|
||
{
|
||
maxLines = treeLines;
|
||
objTreeIdx = i;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//取与线尾所在tree最近的tree作为线缝所在的生长树,计算2D距离，所以不需要区分水平和垂直扫描方式
|
||
|
||
SVzNL3DPoint pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].startPt;
|
||
SVzNL3DPoint pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].endPt;
|
||
SVzNL3DPoint tail_end_1 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
|
||
pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().startPt;
|
||
pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().endPt;
|
||
SVzNL3DPoint tail_end_2 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
|
||
double minDist = DBL_MAX;
|
||
for (int i = 0; i < cornerTreeNum; i++)
|
||
{
|
||
//端点最近的距离为两个tree间的距离
|
||
SVzNL3DPoint end_1 = cornerGrowTrees[i].treeNodes[0].jumpPos;
|
||
SVzNL3DPoint end_2 = cornerGrowTrees[i].treeNodes.back().jumpPos;
|
||
int lineIdx_2 = cornerGrowTrees[i].eLineIdx;
|
||
double dist_11 = sqrt(pow(tail_end_1.x - end_1.x, 2) + pow(tail_end_1.y - end_1.y, 2));
|
||
double dist_12 = sqrt(pow(tail_end_1.x - end_2.x, 2) + pow(tail_end_1.y - end_2.y, 2));
|
||
double dist_21 = sqrt(pow(tail_end_2.x - end_1.x, 2) + pow(tail_end_2.y - end_1.y, 2));
|
||
double dist_22 = sqrt(pow(tail_end_2.x - end_2.x, 2) + pow(tail_end_2.y - end_2.y, 2));
|
||
double dist = dist_11 < dist_12 ? dist_11 : dist_12;
|
||
dist = dist < dist_21 ? dist : dist_21;
|
||
dist = dist < dist_22 ? dist : dist_22;
|
||
if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
objTreeIdx = i;
|
||
}
|
||
}
|
||
}
|
||
if(objTreeIdx < 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//显示
|
||
//将原始数据的序列清0，转义使用
|
||
for (int line = 0; line < lineNum; line++)
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
//置标志，用于debug
|
||
{
|
||
int nodeNum = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int lineIdx, ptIdx;
|
||
if (false == scanInfo.isHorizonScan)
|
||
{
|
||
lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
|
||
ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
|
||
}
|
||
else
|
||
{
|
||
lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
|
||
ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
|
||
}
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||
}
|
||
//显示线头
|
||
if (threadTailTreeIdx >= 0)
|
||
{
|
||
int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int lineIdx, ptIdx;
|
||
if (false == scanInfo.isHorizonScan)
|
||
{
|
||
lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
|
||
for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
|
||
{
|
||
ptIdx = m;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
|
||
for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
|
||
{
|
||
lineIdx = m;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
for (int i = 0; i < (int)debug_markOrigin.size(); i++)
|
||
{
|
||
int lineIdx = debug_markOrigin[i].x;
|
||
int ptIdx = debug_markOrigin[i].y;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 3;
|
||
}
|
||
for (int i = 0; i < (int)debug_markXDir.size(); i++)
|
||
{
|
||
int lineIdx = debug_markXDir[i].x;
|
||
int ptIdx = debug_markXDir[i].y;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 4;
|
||
}
|
||
}
|
||
//提取针脚(提取4个）
|
||
std::vector<SVzNLRect> stitchROIs;
|
||
int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
|
||
|
||
//重新确定线缝中每条扫描线拐点位置。使用分段最小二乘来优化
|
||
double lenFittingScale = 10.0;//
|
||
int lenFittingScale_lines = (int)(lenFittingScale / lineInterval);
|
||
|
||
int totalLines = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
|
||
int num_intervals = totalLines / lenFittingScale_lines; //此处剩余部分不再处理，因为只需要取前3个线头
|
||
std::vector<std::vector<SVzNL3DPoint>> segPoints;
|
||
segPoints.resize(num_intervals);
|
||
std::vector<int> cornerNodeLineIndice;
|
||
cornerNodeLineIndice.resize(totalLines);
|
||
std::fill(cornerNodeLineIndice.begin(), cornerNodeLineIndice.end(), -1);
|
||
int startLineIdx = cornerGrowTrees[objTreeIdx].sLineIdx;
|
||
for (int i = 0; i < nodeSize; i++)
|
||
{
|
||
SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[i];
|
||
int lineDiff = a_node.jumpPos2D.x - startLineIdx;
|
||
cornerNodeLineIndice[lineDiff] = i;
|
||
int interval_id = lineDiff / lenFittingScale_lines;
|
||
if (interval_id < num_intervals)
|
||
segPoints[interval_id].push_back(a_node.jumpPos);
|
||
}
|
||
//分段直线拟合
|
||
std::vector<SVzNL3DPoint> baseLinePoints; //线缝上每条扫描线的点
|
||
std::vector<SVzNL2DPoint> baseLinePtPos;
|
||
for (int i = 0; i < num_intervals; i++)
|
||
{
|
||
std::vector<SVzNL3DPoint> fittingPoints;
|
||
if ((i == 0) || (i == (num_intervals - 1)))
|
||
{
|
||
fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end());
|
||
}
|
||
else
|
||
{
|
||
//使用3段数据拟合
|
||
fittingPoints.insert(fittingPoints.end(), segPoints[i-1].begin(), segPoints[i-1].end());
|
||
fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end());
|
||
fittingPoints.insert(fittingPoints.end(), segPoints[i+1].begin(), segPoints[i+1].end());
|
||
}
|
||
//局部拟合
|
||
double a, b, c; //方程：ax+by+c=0;
|
||
lineFitting_abc(fittingPoints, &a, &b, &c);
|
||
|
||
int segStartLineIdx = i * lenFittingScale_lines + startLineIdx;
|
||
for (int j = 0; j < lenFittingScale_lines; j++)
|
||
{
|
||
int idx = i * lenFittingScale_lines + j;
|
||
if (cornerNodeLineIndice[idx] >= 0)
|
||
{
|
||
int nodeIdx = cornerNodeLineIndice[idx];
|
||
SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];
|
||
baseLinePtPos.push_back(a_node.jumpPos2D);
|
||
baseLinePoints.push_back(a_node.jumpPos);
|
||
}
|
||
else
|
||
{
|
||
double dist = DBL_MAX;
|
||
SVzNL3DPosition bestPoint = _findClosestPoint(scanLines[segStartLineIdx + j], a, b, c, &dist);
|
||
SVzNL2DPoint pos = { segStartLineIdx + j , bestPoint.nPointIdx };
|
||
if (dist > ptInterval * 2)
|
||
{
|
||
bestPoint.pt3D = { 0, 0, -1 };
|
||
pos.y = -1;
|
||
}
|
||
baseLinePtPos.push_back(pos);
|
||
baseLinePoints.push_back(bestPoint.pt3D);
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
//提取针脚范围
|
||
int pre_idx = -1;
|
||
for (int i = 0; i < nodeSize; i++)
|
||
{
|
||
int nodeIdx = (true == scanInfo.scanFromThreadHead) ? i : (nodeSize - 1 - i);
|
||
SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];
|
||
if(pre_idx >= 0)
|
||
{
|
||
SSG_basicFeature1D& pre_node = cornerGrowTrees[objTreeIdx].treeNodes[pre_idx];
|
||
int line_diff = a_node.jumpPos2D.x < pre_node.jumpPos2D.x ? (pre_node.jumpPos2D.x - a_node.jumpPos2D.x) : (a_node.jumpPos2D.x - pre_node.jumpPos2D.x);
|
||
if (line_diff > 2)
|
||
{
|
||
double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
|
||
if (width > scanInfo.stitchWidth)
|
||
{
|
||
//搜索Z值最高点
|
||
int searchStart = pre_node.jumpPos2D.x - startLineIdx;
|
||
int searchEnd = a_node.jumpPos2D.x - startLineIdx;
|
||
|
||
int mean_Lines = (int)(0.5 / lineInterval) + 1; //取1mm的距离上的Z平均值，用于搞噪
|
||
int validMeanLinesTh = mean_Lines - 5;
|
||
std::vector<double> validStitch;
|
||
std::vector<int> validStitchIndice;
|
||
|
||
//计算平均高度和高度极值
|
||
double sticthPeak = DBL_MAX;
|
||
int debug_pkPos = -1;
|
||
for (int m = searchStart + 1; m < searchEnd; m++)
|
||
{
|
||
if (baseLinePoints[m].z > 1e-4)
|
||
{
|
||
double sumZ = 0;
|
||
int sumZ_counter = 0;
|
||
for (int n = m - mean_Lines; n <= m+ mean_Lines; n++)
|
||
{
|
||
if ((n > searchStart) && (n < searchEnd))
|
||
{
|
||
if (baseLinePoints[n].z > 1e-4)
|
||
{
|
||
sumZ += baseLinePoints[n].z;
|
||
sumZ_counter++;
|
||
}
|
||
}
|
||
}
|
||
validStitchIndice.push_back(m);
|
||
if (sumZ_counter > validMeanLinesTh)
|
||
{
|
||
double meanZ = sumZ / (double)sumZ_counter;
|
||
validStitch.push_back(meanZ);
|
||
|
||
if (sticthPeak > meanZ)
|
||
{
|
||
sticthPeak = meanZ;
|
||
debug_pkPos = m;
|
||
}
|
||
}
|
||
else
|
||
validStitch.push_back(-1);
|
||
}
|
||
else
|
||
validStitch.push_back(-1);
|
||
}
|
||
|
||
//以 （sticthPeak+0.5)为门限，取中间点
|
||
double stitchPeakTh = sticthPeak + 0.5;
|
||
int validStart = -1, validEnd = 0;
|
||
for (int n = 0; n < (int)validStitch.size(); n++)
|
||
{
|
||
if ((validStitch[n] > 1e-4) &&(validStitch[n] < stitchPeakTh))
|
||
{
|
||
if (validStart < 0)
|
||
validStart = n;
|
||
validEnd = n;
|
||
}
|
||
}
|
||
int refCenterLine = (validStart + validEnd)/2 + searchStart + 1;
|
||
SVzNL3DPoint stitchPos = { 0, 0, DBL_MAX };
|
||
int stitchLineIdx = -1;
|
||
int searchRng = (validEnd - validStart) / 2;
|
||
for (int m = 0; m < searchRng; m++) //搜索范围也设为opDist_lines
|
||
{
|
||
int index_1 = refCenterLine + m;
|
||
if (baseLinePoints[index_1].z > 1e-4)
|
||
{
|
||
stitchLineIdx = index_1;
|
||
stitchPos = baseLinePoints[index_1];
|
||
break;
|
||
}
|
||
int index_2 = refCenterLine - m;
|
||
if (baseLinePoints[index_2].z > 1e-4)
|
||
{
|
||
stitchLineIdx = index_2;
|
||
stitchPos = baseLinePoints[index_2];
|
||
break;
|
||
}
|
||
}
|
||
|
||
//搜索下刀位置
|
||
SVzNL3DPoint operatePos = {0,0, -1};
|
||
if (stitchLineIdx >= 0)
|
||
{
|
||
int opDist_lines;
|
||
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
|
||
opDist_lines = (int)(scanInfo.operateDist / lineInterval);
|
||
else
|
||
opDist_lines = (int)(scanInfo.operateDist / ptInterval);
|
||
|
||
int op_centerLine = stitchLineIdx + opDist_lines;
|
||
for (int m = 0; m < opDist_lines; m++) //搜索范围也设为opDist_lines
|
||
{
|
||
int index_1 = op_centerLine + m;
|
||
if (index_1 < totalLines)
|
||
{
|
||
if (cornerNodeLineIndice[index_1] > 0)
|
||
{
|
||
int nodeIndex = cornerNodeLineIndice[index_1];
|
||
operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
|
||
break;
|
||
}
|
||
}
|
||
int index_2 = op_centerLine - m;
|
||
if (index_2 >= 0)
|
||
{
|
||
if (cornerNodeLineIndice[index_2] > 0)
|
||
{
|
||
int nodeIndex = cornerNodeLineIndice[index_2];
|
||
operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if ( (stitchLineIdx >= 0) && (operatePos.z > 1e-4))
|
||
{
|
||
SSX_bagThreadInfo a_stitchInfo;
|
||
memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
|
||
a_stitchInfo.threadPos = stitchPos;
|
||
a_stitchInfo.operatePos = operatePos;
|
||
a_stitchInfo.rotateAngle = 0;
|
||
bagThreadInfo.push_back(a_stitchInfo);
|
||
if (bagThreadInfo.size() >= 4)
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
pre_idx = nodeIdx;
|
||
}
|
||
//输出
|
||
if (bagThreadInfo.size() == 0)
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
|
||
//计算为相对坐标
|
||
double cosTheta = cos(markRotateAngle);
|
||
double sinTheta = sin(markRotateAngle);
|
||
bagThreadInfo_relative.resize(bagThreadInfo.size());
|
||
for (int i = 0; i < (int)bagThreadInfo.size(); i++)
|
||
{
|
||
SSX_bagThreadInfo& a_stitch = bagThreadInfo[i];
|
||
SVzNL3DPoint a_pt = { a_stitch.threadPos.x - markCenter.x, a_stitch.threadPos.y - markCenter.y, a_stitch.threadPos.z - markCenter.z };
|
||
SVzNL3DPoint rot_pt = rotateXoY(a_pt, sinTheta, cosTheta);
|
||
bagThreadInfo_relative[i].threadPos = rot_pt;
|
||
a_pt = { a_stitch.operatePos.x - markCenter.x, a_stitch.operatePos.y - markCenter.y, a_stitch.operatePos.z - markCenter.z };
|
||
rot_pt = rotateXoY(a_pt, sinTheta, cosTheta);
|
||
bagThreadInfo_relative[i].operatePos = rot_pt;
|
||
bagThreadInfo_relative[i].rotateAngle = 0;
|
||
}
|
||
return;
|
||
}
|
||
#endif
|
||
|
||
|
||
|