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

bagThreadPositioning version 1.1.0 :

Browse Source 
添加了标定Mark检测，添加了相对于标定柱的坐标输出，添加了相机调平功能
同时包含电机定子抓取的算法开发的过程代码
This commit is contained in:
jerryzeng 2026-02-08 22:32:36 +08:00
parent 94aaf725f9
commit 09de9f850d
10 changed files with 952 additions and 626 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
bagThreadPositioning_Export/bagThreadPositioning_test.cpp
View File

@ -99,6 +99,54 @@ bool checkGridFormat(std::vector<std::vector< SVzNL3DPosition>>& scanData)
return true; return true;
} }
void downSampleGridData(std::vector<std::vector< SVzNL3DPosition>>& scanData, int dwnSampleRatio, std::vector<std::vector< SVzNL3DPosition>>& sampleData)
{
int lines = (int)scanData.size();
for (int i = 0; i < lines; i++)
{
if ((i % dwnSampleRatio) == 0)
sampleData.push_back(scanData[i]);
}
return;
}
int counterValidPts(std::vector< SVzNL3DPosition>& a_line)
{
int ptNum = (int)a_line.size();
int validNum = 0;
for (int i = 0; i < ptNum; i++)
{
if (a_line[i].pt3D.z > 1e-4)
validNum++;
}
return validNum;
}
void removeNullLines(std::vector<std::vector< SVzNL3DPosition>>& scanData)
{
if (scanData.size() == 0)
return;
int lineNum = (int)scanData.size();
int startLineIdx = -1;
int endLineIdx = 0;
for (int i = 0; i < lineNum; i++)
{
int validPtNum = counterValidPts(scanData[i]);
if (validPtNum > 0)
{
if (startLineIdx < 0)
startLineIdx = i;
endLineIdx = i;
}
}
if( (endLineIdx < lineNum-1) && (endLineIdx > 0))
scanData.erase(scanData.begin() + endLineIdx+1, scanData.end());
if (startLineIdx > 0)
scanData.erase(scanData.begin(), scanData.begin() + startLineIdx);
return;
}
void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData) void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
{ {
std::ifstream inputFile(fileName); std::ifstream inputFile(fileName);
@ -252,7 +300,8 @@ void _outputChanneltInfo(char* fileName, std::vector<SSX_bagThreadInfo>& threadI
void _outputRGBDScan_RGBD( void _outputRGBDScan_RGBD(
char* fileName, char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines, std::vector<std::vector<SVzNL3DPosition>>& scanLines,
std::vector<SSX_bagThreadInfo>& threadInfo std::vector<SSX_bagThreadInfo>& threadInfo,
std::vector< SVzNL3DPoint>& markPoints
) )
{ {
int lineNum = (int)scanLines.size(); int lineNum = (int)scanLines.size();
@ -306,6 +355,16 @@ void _outputRGBDScan_RGBD(
rgb = { 250, 0, 0 }; rgb = { 250, 0, 0 };
size = 3; size = 3;
} }
else if (pt3D->nPointIdx == 3)
{
rgb = { 250, 250, 0 };
size = 3;
}
else if (pt3D->nPointIdx == 4)
{
rgb = { 0, 250, 250 };
size = 3;
}
else //if (pt3D->nPointIdx == 0) else //if (pt3D->nPointIdx == 0)
{ {
rgb = { 200, 200, 200 }; rgb = { 200, 200, 200 };
@ -322,8 +381,9 @@ void _outputRGBDScan_RGBD(
if (objNum > 0) if (objNum > 0)
{ {
sw << "Line_" << lineIdx << "_0_" << (objNum*2) << std::endl; int markNum = (int)markPoints.size();
size = 8; sw << "Line_" << lineIdx << "_0_" << (objNum*2 + markNum) << std::endl;
size = 10;
for (int i = 0; i < objNum; i++) for (int i = 0; i < objNum; i++)
{ {
rgb = { 250, 0, 0 }; rgb = { 250, 0, 0 };
@ -342,6 +402,16 @@ void _outputRGBDScan_RGBD(
sw << "{0,0}-{0,0}-"; sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
} }
for (int i = 0; i < markNum; i++)
{
rgb = { 250, 0, 0 };
float x = (float)markPoints[i].x;
float y = (float)markPoints[i].y;
float z = (float)markPoints[i].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//多输出一个，修正显示工具bug //多输出一个，修正显示工具bug
rgb = { 250, 0, 0 }; rgb = { 250, 0, 0 };
float x = (float)threadInfo[0].threadPos.x; float x = (float)threadInfo[0].threadPos.x;
@ -387,34 +457,41 @@ void _outputRGBDScan_RGBD(
#define CONVERT_TO_GRID 0 #define CONVERT_TO_GRID 0
#define BAG_THREAD_POSITIONING 1 #define BAG_THREAD_POSITIONING 1
#define TEST_GROUP 1 #define TEST_GROUP 3
int main() int main()
{ {
const char* dataPath[TEST_GROUP] = { const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/", //0 "F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/", //0
"F:/ShangGu/项目/吕宁项目/袋子拆线/现场数据/", //1
"F:/ShangGu/项目/吕宁项目/袋子拆线/BJDGS/", //2
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
{1,13}, {1,13},
{2,5},
{8,9}
}; };
const char* ver = wd_bagThreadPositioningVersion(); const char* ver = wd_bagThreadPositioningVersion();
printf("ver:%s\n", ver); printf("ver:%s\n", ver);
#if CONVERT_TO_GRID #if CONVERT_TO_GRID
int convertGrp = 0; int convertGrp = 2;
for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) for (int fidx = 11; fidx <= 11; fidx++)
{ {
char _scan_file[256]; char _scan_file[256];
sprintf_s(_scan_file, "%s袋子_%d.txt", dataPath[convertGrp], fidx); sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanData; std::vector<std::vector< SVzNL3DPosition>> scanData;
double lineStep = 0.096; double lineStep = 0.032;
double baseZ = 350.0; double baseZ = 350.0;
vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData); vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData);
bool isGridData = checkGridFormat(scanData); bool isGridData = checkGridFormat(scanData);
int gridFormat = (true == isGridData) ? 1 : 0; int gridFormat = (true == isGridData) ? 1 : 0;
printf("%s: 栅格格式=%d\n", _scan_file, gridFormat); printf("%s: 栅格格式=%d\n", _scan_file, gridFormat);
removeNullLines(scanData);
//std::vector<std::vector< SVzNL3DPosition>> sampleData;
//downSampleGridData(scanData, 4, sampleData);
//将数据恢复为按扫描线存储格式 //将数据恢复为按扫描线存储格式
sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx); sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx);
_outputScanDataFile(_scan_file, scanData,0, 0, 0); _outputScanDataFile(_scan_file, scanData,0, 0, 0);
@ -423,7 +500,7 @@ int main()
#endif #endif
#if BAG_THREAD_POSITIONING #if BAG_THREAD_POSITIONING
for (int grp = 0; grp < TEST_GROUP; grp++) for (int grp = 1; grp <= 1; grp++)
{ {
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{ {
@ -436,10 +513,10 @@ int main()
long t1 = (long)GetTickCount64();//统计时间 long t1 = (long)GetTickCount64();//统计时间
SSG_cornerParam cornerParam; SSG_cornerParam cornerParam;
cornerParam.cornerTh = 70; cornerParam.cornerTh = 90;
cornerParam.scale = 1.5; //袋子缝的尺度1.5mm比较合适 cornerParam.scale = 4; //袋子缝的尺度1.5mm比较合适
cornerParam.minEndingGap = 1.0; // cornerParam.minEndingGap = 1.0; //
cornerParam.minEndingGap_z = 1.0; cornerParam.minEndingGap_z = 3.5;
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
cornerParam.jumpCornerTh_2 = 60; cornerParam.jumpCornerTh_2 = 60;
@ -456,33 +533,57 @@ int main()
SSG_treeGrowParam growParam; SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = -1; //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance growParam.maxLineSkipNum = -1; //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance
growParam.maxSkipDistance = 5.0; growParam.maxSkipDistance = 5.0;
growParam.yDeviation_max = 5.0; growParam.yDeviation_max = 1.0;
growParam.zDeviation_max = 5.0;// growParam.zDeviation_max = 1.0;//
growParam.minLTypeTreeLen = 10; //mm, growParam.minLTypeTreeLen = 10; //mm,
growParam.minVTypeTreeLen = 10; //mm growParam.minVTypeTreeLen = 10; //mm
SSX_ScanInfo scanInfo; SSX_ScanInfo scanInfo;
scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝
scanInfo.scanFromThreadHead = false; //true:线袋子线缝头部开始扫描。 scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。
scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度 scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置 scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
scanInfo.mark_diameter = 6.0; //mark外径
scanInfo.mark_height = 3.5;//mark高
scanInfo.mark_distance = 53.0; //两个Mark的距离
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
poseCalibPara.planeCalib[0] = 1.0;
poseCalibPara.planeCalib[1] = 0.0;
poseCalibPara.planeCalib[2] = 0.0;
poseCalibPara.planeCalib[3] = 0.0;
poseCalibPara.planeCalib[4] = 1.0;
poseCalibPara.planeCalib[5] = 0.0;
poseCalibPara.planeCalib[6] = 0.0;
poseCalibPara.planeCalib[7] = 0.0;
poseCalibPara.planeCalib[8] = 1.0;
poseCalibPara.planeHeight = 2600.0;
for (int i = 0; i < 9; i++)
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
int errCode = 0; int errCode = 0;
std::vector<SSX_bagThreadInfo> bagThreadInfo; std::vector<SSX_bagThreadInfo> bagThreadInfo;
std::vector<SSX_bagThreadInfo> bagThreadInfo_relative; //相对于Mark的坐标
std::vector<SVzNL3DPoint> output_markCenter;
wd_bagThreadPositioning( wd_bagThreadPositioning(
scanLines, scanLines,
scanInfo, //true:激光线平行线缝；false:激光线垂直线缝 scanInfo, //true:激光线平行线缝；false:激光线垂直线缝
poseCalibPara,
filterParam, //噪点过滤参数 filterParam, //噪点过滤参数
cornerParam, //V型特征参数 cornerParam, //V型特征参数
raisedFeatureParam,//线尾凸起参数 raisedFeatureParam,//线尾凸起参数
growParam, //特征生长参数 growParam, //特征生长参数
bagThreadInfo, bagThreadInfo,
bagThreadInfo_relative,
output_markCenter,
&errCode); &errCode);
long t2 = (long)GetTickCount64(); long t2 = (long)GetTickCount64();
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
//输出测试结果 //输出测试结果
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
_outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo); _outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo, output_markCenter);
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
_outputChanneltInfo(_scan_file, bagThreadInfo); _outputChanneltInfo(_scan_file, bagThreadInfo);
} }

1
baseAlgorithm/baseAlgorithm.vcxproj
View File

@ -169,6 +169,7 @@
<ClInclude Include="..\sourceCode\SG_errCode.h" /> <ClInclude Include="..\sourceCode\SG_errCode.h" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClCompile Include="..\sourceCode\dataFitting.cpp" />
<ClCompile Include="..\sourceCode\SG_baseFunc.cpp" /> <ClCompile Include="..\sourceCode\SG_baseFunc.cpp" />
<ClCompile Include="..\sourceCode\SG_clustering.cpp" /> <ClCompile Include="..\sourceCode\SG_clustering.cpp" />
<ClCompile Include="..\sourceCode\SG_featureGrow.cpp" /> <ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />

33
sourceCode/SG_baseAlgo_Export.h
View File

@ -192,6 +192,14 @@ SG_APISHARED_EXPORT void wd_getLineRaisedFeature(
std::vector<SWD_segFeature>& line_raisedFeatures //凸起 std::vector<SWD_segFeature>& line_raisedFeatures //凸起
); );
//提取指定跳变高度的特征点
SG_APISHARED_EXPORT void wd_getSpecifiedHeightJumping(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
SVzNLRangeD jumpHeight, //跳变高度参数
std::vector<SSG_basicFeature1D>& line_jumpFeatures //跳变
);
//直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线 //直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线
SG_APISHARED_EXPORT void wd_surfaceLineSegment( SG_APISHARED_EXPORT void wd_surfaceLineSegment(
std::vector< SVzNL3DPosition>& lineData, std::vector< SVzNL3DPosition>& lineData,
@ -502,6 +510,15 @@ SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
//计算角度差值，在0-180度范围 //计算角度差值，在0-180度范围
SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2); SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2);
/**
* @brief 计算从向量a到向量b的**有方向旋转角**（范围：-π ~ π）
* @param a 源向量
* @param b 目标向量
* @param rotAngle 输出：旋转角（弧度），逆时针为正，顺时针为负
* @return true：计算成功，false：零向量（失败）
*/
SG_APISHARED_EXPORT bool calcRotateAngle(const SVzNL2DPointD& a, const SVzNL2DPointD& b, double& rotAngle);
//计算直线交点 //计算直线交点
SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs( SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs(
double a1, double b1, double c1, double a1, double b1, double c1,
@ -686,6 +703,15 @@ SG_APISHARED_EXPORT void wd_gridPointClustering_labelling(
SVzNLRect& clusterRoi //roi2D SVzNLRect& clusterRoi //roi2D
); );
//使用聚类方法完成8连通连通域分析:适应不同的数据结构
SG_APISHARED_EXPORT void wd_gridPointClustering_labelling_2(
std::vector<std::vector<SVzNL3DPoint>>& srcData,
std::vector<std::vector<SSG_clusterLabel>>& labelMask,
int clusterID, //当前Cluster的ID
std::vector< SVzNL2DPoint>& a_cluster, //result
SVzNL3DRangeD& clusterRoi //roi3D
);
//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值 //对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
SG_APISHARED_EXPORT void pointClout2DProjection( SG_APISHARED_EXPORT void pointClout2DProjection(
std::vector< std::vector<SVzNL3DPosition>>& gridScanData, std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
@ -700,16 +726,15 @@ SG_APISHARED_EXPORT void pointClout2DProjection(
); );
//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值 //对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
SG_APISHARED_EXPORT void pointClout2DQuantization( SG_APISHARED_EXPORT void pointCloud2DQuantization(
std::vector< std::vector<SVzNL3DPosition>>& gridScanData, std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
SVzNLRangeD x_range, SVzNLRangeD x_range,
SVzNLRangeD y_range, SVzNLRangeD y_range,
double scale, double scale,
double cuttingGrndZ,
int edgeSkip, int edgeSkip,
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值 double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
std::vector<std::vector<double>> quantiData, //量化数据，初始化为一个极大值1e+6 std::vector<std::vector<SVzNL3DPoint>>& quantiData, //量化数据，初始化为一个极大值1e+6
std::vector<std::vector<SVzNL2DPoint>> backIndexing //标记坐标索引，用于回找3D坐标 std::vector<std::vector<SVzNL2DPoint>>& backIndexing //标记坐标索引，用于回找3D坐标
); );
//分水岭算法 //分水岭算法

19
sourceCode/SG_baseDataType.h
View File

@ -39,6 +39,12 @@ typedef struct
SWD3DPoint point; SWD3DPoint point;
}SWDIndexing3DPoint; }SWDIndexing3DPoint;
typedef struct
{
int start;
int end;
}SWD_Interval; //Çø¼ä
typedef struct typedef struct
{ {
bool validFlag; //指示结果是否有效 bool validFlag; //指示结果是否有效
@ -55,6 +61,12 @@ typedef enum
keSG_PoseSorting_T2B_L2R, ///从上到下，从左到右排序 keSG_PoseSorting_T2B_L2R, ///从上到下，从左到右排序
} ESG_poseSortingMode; } ESG_poseSortingMode;
typedef enum
{
KeWD_Mask_ValidPt = 0,
KeWD_Mask_NullPt,
}EWD_maskMode;
typedef struct typedef struct
{ {
int data_0; int data_0;
@ -62,6 +74,13 @@ typedef struct
int idx; int idx;
}SSG_intPair; }SSG_intPair;
typedef struct
{
int flag;
int validFlag;
int clusterID;
}SSG_clusterLabel;
typedef struct typedef struct
{ {
int featurType; int featurType;

462
sourceCode/SG_baseFunc.cpp
View File

@ -9,6 +9,8 @@
#include <unordered_map> #include <unordered_map>
#include <Eigen/dense> #include <Eigen/dense>
const double EPS = 1e-10;
SVzNL3DRangeD sg_getScanDataROI( SVzNL3DRangeD sg_getScanDataROI(
//计算扫描ROI //计算扫描ROI
SVzNL3DLaserLine* laser3DPoints, SVzNL3DLaserLine* laser3DPoints,
@ -276,277 +278,6 @@ SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition
return para; return para;
} }
void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
{
//最小二乘拟合直线参数
double xx_sum = 0;
double x_sum = 0;
double y_sum = 0;
double xy_sum = 0;
int num = 0;
for (int i = 0; i < inliers.size(); i++)
{
x_sum += inliers[i].x; //x的累加和
y_sum += inliers[i].y; //y的累加和
xx_sum += inliers[i].x * inliers[i].x; //x的平方累加和
xy_sum += inliers[i].x * inliers[i].y; //x，y的累加和
num++;
}
*_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //根据公式求解k
*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
}
//拟合成通用直线方程，包括垂直
void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c)
{
//判断是否为垂直
int dataSize = (int)inliers.size();
if (dataSize <2)
return;
double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
std::vector< SVzNL3DPoint> fittingData;
if (deltaX < deltaY)
{
//x=ky+b 拟合
for (int i = 0; i < dataSize; i++)
{
SVzNL3DPoint a_fitPt;
a_fitPt.x = inliers[i].y;
a_fitPt.y = inliers[i].x;
a_fitPt.z = inliers[i].z;
fittingData.push_back(a_fitPt);
}
double k = 0, b = 0;
lineFitting(fittingData, &k, &b);
//ax+by+c
*_a = 1.0;
*_b = -k;
*_c = -b;
}
else
{
//y = kx+b拟合
double k = 0, b = 0;
lineFitting(inliers, &k, &b);
//ax+by+c
*_a = k;
*_b = -1;
*_c = b;
}
return;
}
//圆最小二乘拟合
double fitCircleByLeastSquare(
const std::vector<SVzNL3DPoint>& pointArray,
SVzNL3DPoint& center,
double& radius)
{
int N = pointArray.size();
if (N < 3) {
return std::numeric_limits<double>::max();
}
double sumX = 0.0;
double sumY = 0.0;
double sumX2 = 0.0;
double sumY2 = 0.0;
double sumX3 = 0.0;
double sumY3 = 0.0;
double sumXY = 0.0;
double sumXY2 = 0.0;
double sumX2Y = 0.0;
for (int pId = 0; pId < N; ++pId) {
sumX += pointArray[pId].x;
sumY += pointArray[pId].y;
double x2 = pointArray[pId].x * pointArray[pId].x;
double y2 = pointArray[pId].y * pointArray[pId].y;
sumX2 += x2;
sumY2 += y2;
sumX3 += x2 * pointArray[pId].x;
sumY3 += y2 * pointArray[pId].y;
sumXY += pointArray[pId].x * pointArray[pId].y;
sumXY2 += pointArray[pId].x * y2;
sumX2Y += x2 * pointArray[pId].y;
}
double C, D, E, G, H;
double a, b, c;
C = N * sumX2 - sumX * sumX;
D = N * sumXY - sumX * sumY;
E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
G = N * sumY2 - sumY * sumY;
H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;
a = (H * D - E * G) / (C * G - D * D);
b = (H * C - E * D) / (D * D - G * C);
c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;
center.x = -a / 2.0;
center.y = -b / 2.0;
radius = sqrt(a * a + b * b - 4 * c) / 2.0;
double err = 0.0;
double e;
double r2 = radius * radius;
for (int pId = 0; pId < N; ++pId) {
e = pow(pointArray[pId].x - center.x,2) + pow(pointArray[pId].y - center.y, 2) - r2;
if (e > err) {
err = e;
}
}
return err;
}
#if 0
bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
double& a, double& b, double& c,
double& mse, double& max_err)
{
// 校验点集数量（至少3个点才能拟合抛物线）
int n = points.size();
if (n < 3) {
return false;
}
// 初始化各项求和参数
double sum_x = 0.0, sum_x2 = 0.0, sum_x3 = 0.0, sum_x4 = 0.0;
double sum_y = 0.0, sum_xy = 0.0, sum_x2y = 0.0;
// 计算各项求和
for (const auto& p : points) {
double x = p.x;
double y = p.y;
double x2 = x * x;
double x3 = x2 * x;
double x4 = x3 * x;
sum_x += x;
sum_x2 += x2;
sum_x3 += x3;
sum_x4 += x4;
sum_y += y;
sum_xy += x * y;
sum_x2y += x2 * y;
}
// 构建线性方程组 M * [a,b,c]^T = N
// M矩阵：3x3
double M[3][3] = {
{sum_x4, sum_x3, sum_x2},
{sum_x3, sum_x2, sum_x},
{sum_x2, sum_x, (double)n}
};
// N向量：3x1
double N[3] = { sum_x2y, sum_xy, sum_y };
// 高斯消元法求解线性方程组（3元一次方程组）
// 步骤1：将M转化为上三角矩阵
for (int i = 0; i < 3; i++) {
// 选主元（避免除数为0）
int pivot = i;
for (int j = i; j < 3; j++) {
if (fabs(M[j][i]) > fabs(M[pivot][i])) {
pivot = j;
}
}
// 交换行
std::swap(M[i], M[pivot]);
std::swap(N[i], N[pivot]);
// 归一化主元行
double div = M[i][i];
if (fabs(div) < 1e-10) {
return false;
}
for (int j = i; j < 3; j++) {
M[i][j] /= div;
}
N[i] /= div;
// 消去下方行
for (int j = i + 1; j < 3; j++) {
double factor = M[j][i];
for (int k = i; k < 3; k++) {
M[j][k] -= factor * M[i][k];
}
N[j] -= factor * N[i];
}
}
// 步骤2：回代求解
c = N[2];
b = N[1] - M[1][2] * c;
a = N[0] - M[0][1] * b - M[0][2] * c;
// 计算拟合误差
mse = 0.0;
max_err = 0.0;
for (const auto& p : points) {
double y_fit = a * p.x * p.x + b * p.x + c;
double err = y_fit - p.y;
double err_abs = fabs(err);
mse += err * err;
if (err_abs > max_err) {
max_err = err_abs;
}
}
mse /= n; // 均方误差
return true;
}
#endif
//抛物线最小二乘拟合 y=ax^2 + bx + c
bool leastSquareParabolaFitEigen(
const std::vector<cv::Point2d>& points,
double& a, double& b, double& c,
double& mse, double& max_err)
{
int n = points.size();
if (n < 3) {
return false;
}
// 构建系数矩阵A和目标向量B
Eigen::MatrixXd A(n, 3);
Eigen::VectorXd B(n);
for (int i = 0; i < n; i++) {
double x = points[i].x;
A(i, 0) = x * x;
A(i, 1) = x;
A(i, 2) = 1.0;
B(i) = points[i].y;
}
// 最小二乘求解：Ax = B，直接调用Eigen的求解器
Eigen::Vector3d coeffs = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
a = coeffs(0);
b = coeffs(1);
c = coeffs(2);
// 计算误差
mse = 0.0;
max_err = 0.0;
for (const auto& p : points) {
double y_fit = a * p.x * p.x + b * p.x + c;
double err = y_fit - p.y;
double err_abs = fabs(err);
mse += err * err;
if (err_abs > max_err) {
max_err = err_abs;
}
}
mse /= n;
return true;
}
//计算Z均值 //计算Z均值
double computeMeanZ(std::vector< SVzNL3DPoint>& pts) double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
{ {
@ -653,6 +384,67 @@ double computeXOYVertexAngle(SVzNL3DPoint p0, SVzNL3DPoint p1, SVzNL3DPoint p2)
return angle; return angle;
} }
// 计算向量的模长
double vecNorm(const SVzNL2DPointD& v) {
return sqrt(v.x * v.x + v.y * v.y);
}
// 向量归一化（单位向量），返回是否成功（零向量返回false）
bool vecNormalize(SVzNL2DPointD& v) {
double norm = vecNorm(v);
if (norm < EPS) { // 零向量，无法归一化
return false;
}
v.x /= norm;
v.y /= norm;
return true;
}
// 计算两个向量的点积
double vecDot(const SVzNL2DPointD& a, const SVzNL2DPointD& b) {
return a.x * b.x + a.y * b.y;
}
// 计算两个向量的2D叉积（标量值）
double vecCross(const SVzNL2DPointD& a, const SVzNL2DPointD& b) {
return a.x * b.y - a.y * b.x;
}
/**
* @brief 计算从向量a到向量b的**有方向旋转角**（范围：-π ~ π）
* @param a 源向量
* @param b 目标向量
* @param rotAngle 输出：旋转角（弧度），逆时针为正，顺时针为负
* @return true：计算成功，false：零向量（失败）
*/
bool calcRotateAngle(const SVzNL2DPointD& a, const SVzNL2DPointD& b, double& rotAngle) {
SVzNL2DPointD aNorm = a;
SVzNL2DPointD bNorm = b;
// 归一化两个向量，零向量直接返回失败
if (!vecNormalize(aNorm) || !vecNormalize(bNorm)) {
std::cerr << "Error: 输入为零向量，无法计算旋转角！" << std::endl;
return false;
}
// 计算点积并钳位（避免浮点精度导致超出[-1,1]）
double dot = vecDot(aNorm, bNorm);
if (dot < -1.0 + EPS)
dot = -1.0 + EPS;
if (dot > 1.0 - EPS)
dot = 1.0 - EPS;
// 点积求无方向夹角（0 ~ π）
double angle = acos(dot);
// 叉积判断旋转方向
double cross = vecCross(aNorm, bNorm);
if (cross < -EPS) { // 顺时针，角度取负
rotAngle = -angle;
}
else { // 逆时针/共线，角度取正
rotAngle = angle;
}
return true;
}
double computePtDistToLine(double x0, double y0, double a, double b, double c) double computePtDistToLine(double x0, double y0, double a, double b, double c)
{ {
double tmp = sqrt(pow(a, 2) + pow(b, 2)); double tmp = sqrt(pow(a, 2) + pow(b, 2));
@ -1193,54 +985,6 @@ void SG_TwoPassLabel(
} }
#endif #endif
//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
void vzCaculateLaserPlane(std::vector<cv::Point3f> Points3ds, std::vector<double>& res)
{
//最小二乘法拟合平面
//获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反
//系数矩阵
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
//
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
//结果矩阵
cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
for (int i = 0; i < Points3ds.size(); i++)
{
x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
xi += (double)Points3ds[i].x;
yi += (double)Points3ds[i].y;
zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
zi += (double)Points3ds[i].z;
}
A.at<double>(0, 0) = x2;
A.at<double>(1, 0) = xiyi;
A.at<double>(2, 0) = xi;
A.at<double>(0, 1) = xiyi;
A.at<double>(1, 1) = y2;
A.at<double>(2, 1) = yi;
A.at<double>(0, 2) = xi;
A.at<double>(1, 2) = yi;
A.at<double>(2, 2) = (double)((int)Points3ds.size());
B.at<double>(0, 0) = zixi;
B.at<double>(1, 0) = ziyi;
B.at<double>(2, 0) = zi;
//计算平面系数
X = A.inv() * B;
//A
res.push_back(X.at<double>(0, 0));
//B
res.push_back(X.at<double>(1, 0));
//Z的系数为-1
res.push_back(-1.0);
//C
res.push_back(X.at<double>(2, 0));
return;
}
// 函数：从平面法向量计算欧拉角（ZYX顺序） // 函数：从平面法向量计算欧拉角（ZYX顺序）
SSG_EulerAngles planeNormalToEuler(double A, double B, double C) { SSG_EulerAngles planeNormalToEuler(double A, double B, double C) {
@ -3063,18 +2807,16 @@ void pointClout2DProjection(
} }
} }
#if 0
//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值 //对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
void pointClout2DQuantization( void pointCloud2DQuantization(
std::vector< std::vector<SVzNL3DPosition>>& gridScanData, std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
SVzNLRangeD x_range, SVzNLRangeD x_range,
SVzNLRangeD y_range, SVzNLRangeD y_range,
double scale, double scale,
double cuttingGrndZ,
int edgeSkip, int edgeSkip,
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值 double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
std::vector<std::vector<double>> quantiData, //量化数据，初始化为一个极大值1e+6 std::vector<std::vector<SVzNL3DPoint>>& quantiData, //量化数据，初始化为一个极大值1e+6
std::vector<std::vector<SVzNL2DPoint>> backIndexing //标记坐标索引，用于回找3D坐标 std::vector<std::vector<SVzNL2DPoint>>& backIndexing //标记坐标索引，用于回找3D坐标
) )
{ {
int lineNum = (int)gridScanData.size(); int lineNum = (int)gridScanData.size();
@ -3086,12 +2828,15 @@ void pointClout2DQuantization(
int y_rows = (int)((y_range.max - y_range.min) / scale) + 1 + edgeSkip * 2; int y_rows = (int)((y_range.max - y_range.min) / scale) + 1 + edgeSkip * 2;
quantiData.resize(x_cols); quantiData.resize(x_cols);
backIndexing.resize(x_cols); backIndexing.resize(x_cols);
double quantiXStart = x_range.min - edgeSkip * scale;
double quantiYStart = y_range.min - edgeSkip * scale;
for (int i = 0; i < x_cols; i++) for (int i = 0; i < x_cols; i++)
{ {
quantiData[i].resize(y_rows); quantiData[i].resize(y_rows);
std::fill(quantiData[i].begin(), quantiData[i].end(), 0); for (int j = 0; j < y_rows; j++)
quantiData[i][j] = {i * scale + quantiXStart + scale/2, j * scale + quantiYStart + scale / 2 , 0};
backIndexing[i].resize(y_rows); backIndexing[i].resize(y_rows);
std::fill(backIndexing[i].begin(), backIndexing[i].end(), 0); std::fill(backIndexing[i].begin(), backIndexing[i].end(), SVzNL2DPoint{0,0});
} }
int nPointCnt = (int)gridScanData[0].size(); int nPointCnt = (int)gridScanData[0].size();
@ -3104,8 +2849,6 @@ void pointClout2DQuantization(
SVzNL3DPosition* pt3D = &gridScanData[line][i]; SVzNL3DPosition* pt3D = &gridScanData[line][i];
if (pt3D->pt3D.z < 1e-4) if (pt3D->pt3D.z < 1e-4)
continue; continue;
if ((cuttingGrndZ > 0) && (pt3D->pt3D.z > cuttingGrndZ))
continue;
double x = pt3D->pt3D.x; double x = pt3D->pt3D.x;
double y = pt3D->pt3D.y; double y = pt3D->pt3D.y;
int px = (int)(x - x_range.min) / scale + edgeSkip; int px = (int)(x - x_range.min) / scale + edgeSkip;
@ -3122,7 +2865,7 @@ void pointClout2DQuantization(
{ {
SVzNL2DPoint v2i = { line, i }; SVzNL2DPoint v2i = { line, i };
backIndexing[px][px] = v2i; backIndexing[px][px] = v2i;
quantiData[px][py] = pt3D->pt3D.z; quantiData[px][py].z = pt3D->pt3D.z;
//垂直插值 //垂直插值
if (prePt) if (prePt)
{ {
@ -3148,7 +2891,7 @@ void pointClout2DQuantization(
k2 = 1.0 - k1; k2 = 1.0 - k1;
} }
double inter_z = k1 * pt3D->pt3D.z + k2 * prePt->pt3D.z; double inter_z = k1 * pt3D->pt3D.z + k2 * prePt->pt3D.z;
quantiData[interPts[m].x][interPts[m].y] = inter_z; quantiData[interPts[m].x][interPts[m].y].z = inter_z;
} }
} }
} }
@ -3168,7 +2911,7 @@ void pointClout2DQuantization(
double pre_value = -1; double pre_value = -1;
for (int x = 0; x < cols; x++) for (int x = 0; x < cols; x++)
{ {
double value = quantiData[x][y]; double value = quantiData[x][y].z;
if (value > 1e-4) if (value > 1e-4)
{ {
if (pre_x >= 0) if (pre_x >= 0)
@ -3182,7 +2925,7 @@ void pointClout2DQuantization(
double k1 = ((double)(m - pre_x)) / ((double)x_diff); double k1 = ((double)(m - pre_x)) / ((double)x_diff);
double k2 = 1.0 - k1; double k2 = 1.0 - k1;
double inter_z = k1 * value + k2 * pre_value; double inter_z = k1 * value + k2 * pre_value;
quantiData[x][y] = inter_z; quantiData[x][y].z = inter_z;
} }
} }
} }
@ -3192,7 +2935,6 @@ void pointClout2DQuantization(
} }
} }
} }
#endif
//对空间两组对应点计算旋转平移矩阵 //对空间两组对应点计算旋转平移矩阵
// Eigen库实现 // Eigen库实现
@ -3373,60 +3115,6 @@ void WD_EulerRpyToDirVectors(double rpy[3],std::vector<cv::Point3d>& dirVectors)
} }
/**
* @brief 空间直线最小二乘拟合
* @param points 输入的三维点集（至少2个点，否则无意义）
* @param center 输出：拟合直线的质心（基准点P0）
* @param direction 输出：拟合直线的方向向量v（单位化）
* @return 拟合是否成功（点集有效返回true）
*/
bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint& center, SVzNL3DPoint& direction)
{
// 检查点集有效性
if (points.size() < 2) {
std::cerr << "Error: 点集数量必须大于等于2！" << std::endl;
return false;
}
int n = points.size();
Eigen::MatrixXd A(n, 3); // 点集矩阵：每行一个点的(x,y,z)
// 1. 计算质心（center）
double cx = 0.0, cy = 0.0, cz = 0.0;
for (const auto& p : points) {
cx += p.x;
cy += p.y;
cz += p.z;
A.row(points.size() - n) << p.x, p.y, p.z; // 填充点集矩阵
n--;
}
cx /= points.size();
cy /= points.size();
cz /= points.size();
center = { cx, cy, cz };
// 2. 构造去中心化的协方差矩阵（3x3）
// 关键修复：使用RowVector3d（行向量）做rowwise减法，匹配维度
Eigen::RowVector3d centroid_row(cx, cy, cz);
Eigen::MatrixXd centered = A.rowwise() - centroid_row; // 维度匹配，无报错
// 协方差矩阵计算（n-1为无偏估计，工程中也可直接用n）
Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1);
// 3. 特征值分解：求协方差矩阵的特征值和特征向量
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
if (eigensolver.info() != Eigen::Success) {
std::cerr << "Error: 特征值分解失败！" << std::endl;
return false;
}
// 最大特征值对应的特征向量即为方向向量（Eigen默认按特征值升序排列，取最后一个）
Eigen::Vector3d dir = eigensolver.eigenvectors().col(2);
// 单位化方向向量（可选，但工程中通常标准化）
dir.normalize();
direction = { dir(0), dir(1), dir(2) };
return true;
}
#if 0 #if 0
#include <iostream> #include <iostream>

26
sourceCode/SG_clustering.cpp
View File

@ -282,14 +282,14 @@ void wd_gridPointClustering_labelling(
} }
return; return;
} }
#if 0
//使用聚类方法完成8连通连通域分析 //使用聚类方法完成8连通连通域分析
void wd_gridPointClustering_labelling_2( void wd_gridPointClustering_labelling_2(
std::vector<std::vector<double>>& srcData, std::vector<std::vector<SVzNL3DPoint>>& srcData,
std::vector<std::vector<SSG_intPair>>& labelMask, std::vector<std::vector<SSG_clusterLabel>>& labelMask,
int clusterID, //当前Cluster的ID int clusterID, //当前Cluster的ID
std::vector< SVzNL2DPoint>& a_cluster, //result std::vector< SVzNL2DPoint>& a_cluster, //result
SVzNLRect& clusterRoi //roi2D SVzNL3DRangeD& clusterRoi //roi3D
) )
{ {
int i = 0; int i = 0;
@ -304,7 +304,8 @@ void wd_gridPointClustering_labelling_2(
if ((a_seedPos.x == 390) && (a_seedPos.y == 949)) if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
int kkk = 1; int kkk = 1;
SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y]; SSG_clusterLabel& a_seed = labelMask[a_seedPos.x][a_seedPos.y];
SVzNL3DPoint& a_feature3DValue = srcData[a_seedPos.x][a_seedPos.y];
if (0 == a_seed.clusterID) //clusterID == 0, 未被处理，搜索邻域 if (0 == a_seed.clusterID) //clusterID == 0, 未被处理，搜索邻域
{ {
//8连通 //8连通
@ -319,8 +320,8 @@ void wd_gridPointClustering_labelling_2(
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum)) if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
{ {
SSG_featureClusteringInfo& chk_seed = featureMask[x][y]; SSG_clusterLabel& chk_seed = labelMask[x][y];
if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0)) //Ö»¼ì²éδ¾ÛÀàµÄÌØÕ÷µã if ((chk_seed.validFlag == 0) || (chk_seed.clusterID > 0)) //Ö»¼ì²éδ¾ÛÀàµÄÌØÕ÷µã
continue; continue;
if (0 == chk_seed.flag)//防止被重复添加 if (0 == chk_seed.flag)//防止被重复添加
@ -336,12 +337,13 @@ void wd_gridPointClustering_labelling_2(
} }
a_seed.clusterID = clusterID; a_seed.clusterID = clusterID;
//更新ROI //更新ROI
clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left; clusterRoi.xRange.min = clusterRoi.xRange.min > a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.min;
clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right; clusterRoi.xRange.max = clusterRoi.xRange.max < a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.max;
clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top; clusterRoi.yRange.min = clusterRoi.yRange.min > a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.min;
clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom; clusterRoi.yRange.max = clusterRoi.yRange.max < a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.max;
clusterRoi.zRange.min = clusterRoi.zRange.min > a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.min;
clusterRoi.zRange.max = clusterRoi.zRange.max < a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.max;
i++; i++;
} }
return; return;
} }
#endif

8
sourceCode/SG_errCode.h
View File

@ -8,6 +8,8 @@
#define SG_ERR_INVLD_SORTING_MODE -1005 #define SG_ERR_INVLD_SORTING_MODE -1005
#define SG_ERR_INVLD_Q_SCALE -1006 #define SG_ERR_INVLD_Q_SCALE -1006
#define SG_ERR_ZERO_OBJECTS -1007 #define SG_ERR_ZERO_OBJECTS -1007
#define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008
#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009
//BQ_workpiece //BQ_workpiece
#define SX_ERR_INVLD_VTREE_NUM -2001 #define SX_ERR_INVLD_VTREE_NUM -2001
@ -21,7 +23,8 @@
//定子抓取 //定子抓取
#define SX_ERR_INVLID_CUTTING_Z -2101 #define SX_ERR_INVLID_CUTTING_Z -2101
#define SX_ERR_ZERO_OBJ -2102 #define SX_ERR_ZERO_OBJ_TOPLAYER -2102
#define SX_ERR_ZERO_OBJ_BTMLAYER -2103
//拆包 //拆包
#define SX_BAG_TRAY_EMPTY -2201 #define SX_BAG_TRAY_EMPTY -2201
@ -29,3 +32,6 @@
//汽车轮眉高度测量 //汽车轮眉高度测量
#define SX_ERR_INVALID_ARC -2301 #define SX_ERR_INVALID_ARC -2301
//ÌÇ´ü×Ó²ðÏß
#define SX_ERR_NO_MARK -2401

47
sourceCode/SG_lineFeature.cpp
View File

@ -4574,6 +4574,53 @@ void wd_getLineRaisedFeature(
} }
} }
//提取指定跳变高度的特征点
void wd_getSpecifiedHeightJumping(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
SVzNLRangeD jumpHeight, //跳变高度参数
std::vector<SSG_basicFeature1D>& line_jumpFeatures //跳变
)
{
int dataSize = (int)lineData.size();
double pre_z = 0;
int preIdx = -1;
for (int i = 0; i < dataSize; i++)
{
if (i == 1850)
int kkk = 1;
lineData[i].nPointIdx = i; //重新编序号
if (lineData[i].pt3D.z > 1e-4)
{
if(preIdx >= 0)
{
double z_diff = abs(lineData[i].pt3D.z - pre_z);
if ( (z_diff >= jumpHeight.min) && (z_diff <= jumpHeight.max))
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
if (lineData[i].pt3D.z < pre_z)
{
a_feature.jumpPos = lineData[i].pt3D;
a_feature.jumpPos2D = { lineIdx, i };
}
else
{
a_feature.jumpPos = lineData[preIdx].pt3D;
a_feature.jumpPos2D = { lineIdx, preIdx };
}
line_jumpFeatures.push_back(a_feature);
}
}
preIdx = i;
pre_z = lineData[i].pt3D.z;
}
}
}
//使用端点直线，检查点到直线的距离，大于门限的分割 //使用端点直线，检查点到直线的距离，大于门限的分割
void split( void split(
SSG_RUN a_run, SSG_RUN a_run,

754
sourceCode/bagThreadPositioning.cpp
View File

@ -6,21 +6,132 @@
#include <limits> #include <limits>
//version 1.0.0 : base version release to customer //version 1.0.0 : base version release to customer
std::string m_strVersion = "1.0.0"; //version 1.1.0 : 添加了标定Mark检测，添加了相对于标定柱的坐标输出，添加了相机调平功能
std::string m_strVersion = "1.1.0";
const char* wd_bagThreadPositioningVersion(void) const char* wd_bagThreadPositioningVersion(void)
{ {
return m_strVersion.c_str(); 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 #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( void wd_bagThreadPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道 const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道
const SSG_planeCalibPara groundCalibPara,
const SSG_outlierFilterParam filterParam, //噪点过滤参数 const SSG_outlierFilterParam filterParam, //噪点过滤参数
const SSG_cornerParam cornerPara, //V型特征参数 const SSG_cornerParam cornerPara, //V型特征参数
const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数 const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
const SSG_treeGrowParam growParam, //特征生长参数 const SSG_treeGrowParam growParam, //特征生长参数
std::vector<SSX_bagThreadInfo>& bagThreadInfo, std::vector<SSX_bagThreadInfo>& bagThreadInfo,
std::vector<SSX_bagThreadInfo>& bagThreadInfo_relative, //相对于Mark的输出
std::vector<SVzNL3DPoint>& output_markCenter, //输出Mark位置信息
int* errCode) int* errCode)
{ {
*errCode = 0; *errCode = 0;
@ -30,6 +141,16 @@ void wd_bagThreadPositioning(
*errCode = SG_ERR_3D_DATA_NULL; *errCode = SG_ERR_3D_DATA_NULL;
return; 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(); int linePtNum = (int)scanLines[0].size();
//判断数据格式是否为grid。算法只能处理grid数据格式 //判断数据格式是否为grid。算法只能处理grid数据格式
@ -48,6 +169,13 @@ void wd_bagThreadPositioning(
return; return;
} }
//调平。
for (int i = 0; i < lineNum; i++)
{ //行处理
//调平，去除地面
wd_bagThread_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
}
//生成水平扫描数据 //生成水平扫描数据
//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速 //统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
double ptInterval = 0; double ptInterval = 0;
@ -106,16 +234,25 @@ void wd_bagThreadPositioning(
} }
lineInterval = lineInterval / lineIntervalNum; 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; double vCornerScale = cornerPara.scale * 4;
std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures; std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
std::vector<std::vector<SWD_segFeature>> raisedFeatures; std::vector<std::vector<SWD_segFeature>> raisedFeatures;
if (false == scanInfo.isHorizonScan) //现场要求垂直扫描
//if (false == scanInfo.isHorizonScan) //
{ {
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
int validVCornerSCale = (int)(vCornerScale / ptInterval); int validVCornerSCale = (int)(vCornerScale / ptInterval);
//垂直扫描检测V型槽和线尾 //垂直扫描检测V型槽和线尾
for (int line = 0; line < lineNum; line++) for (int line = 0; line < lineNum; line++)
{ {
if ((line == 577) || (line == 932)) if ((line == 750) || (line == 905))
int kkk = 1; int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line]; std::vector<SVzNL3DPosition>& lineData = scanLines[line];
//滤波，滤除异常点 //滤波，滤除异常点
@ -163,67 +300,266 @@ void wd_bagThreadPositioning(
line_raisedFeatures //凸起 line_raisedFeatures //凸起
); );
raisedFeatures.push_back(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 0 #if 1
//水平扫描检测袋子边 //水平扫描检测标定柱
std::vector<std::vector<SSG_basicFeature1D>> jumpdFeatures; std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
#endif
}
else
{
int validVCornerSCale = (int)(vCornerScale / lineInterval);
//水平扫描检测V型槽和线尾
for (int line = 0; line < lineNum_h; line++) for (int line = 0; line < lineNum_h; line++)
{ {
if (line == 329) if ((line == 750) || (line == 905))
int kkk = 1; int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = data_lines_h[line]; std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
//滤波，滤除异常点 //滤波，滤除异常点
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam); sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
//提取V型槽 //提取标定柱跳变特征
std::vector<SSG_basicFeature1D> line_cornerFeatures; std::vector<SSG_basicFeature1D> line_jumpFearures;
std::vector<SSG_RUN_EX> segs; wd_getSpecifiedHeightJumping(
int dataSize = (int)lineData.size();
wd_getLineCorerFeature_accelerate(
lineData, lineData,
line, line,
cornerPara, jumpHeight, //跳变高度参数
lineInterval, line_jumpFearures //跳变
segs,
line_cornerFeatures //拐点
); );
//检查V型特征两侧，不能有跳变 jumpFeatures_h.push_back(line_jumpFearures);
std::vector<SSG_basicFeature1D> valid_cornerFeatures; }
int vCornerSize = (int)line_cornerFeatures.size();
int segSize = (int)segs.size(); //对标定柱进行聚类
for (int m = 0; m < vCornerSize; m++) 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++)
{ {
int cornerPos = line_cornerFeatures[m].jumpPos2D.y; featureInfoMask[line].resize(linePtNum);
for (int n = 0; n < segSize; n++) 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 segEnd = segs[n].start + segs[n].len - 1; int lineJumpNum = (int)jumpFeatures_v[line].size();
if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd)) for(int j = 0; j <lineJumpNum; j++)
{ {
int skip_1 = cornerPos - segs[n].start; SSG_basicFeature1D& a_jump = jumpFeatures_v[line][j];
int skip_2 = segEnd - cornerPos; SSG_featureClusteringInfo a_feature;
if ((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale)) a_feature.featurType = a_jump.featureType;
valid_cornerFeatures.push_back(line_cornerFeatures[m]); a_feature.featureIdx_v = j;
break; 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 };
} }
} }
} }
cornerFeatures.push_back(valid_cornerFeatures); //聚类
//提取凸起段 std::vector<std::vector< SVzNL2DPoint>> markClusters; //只记录位置
std::vector<SWD_segFeature> line_raisedFeatures; std::vector<SWD_clustersInfo> markClustersInfo;
wd_getLineRaisedFeature( SSG_treeGrowParam markGrowParam;
lineData, markGrowParam.yDeviation_max = 1.0;//生长时相邻特征最大的Y偏差
line, markGrowParam.zDeviation_max = 1.0; //生长时相邻特征最大的Z偏差
raisedFeaturePara, //凸起参数 markGrowParam.maxLineSkipNum = 5; //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
line_raisedFeatures //凸起 markGrowParam.maxSkipDistance = 1.0; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
); markGrowParam.minLTypeTreeLen = 5.0; //生长树最少的节点数目。小于此数目的生长树被移除
raisedFeatures.push_back(line_raisedFeatures); 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
} }
//特征生长 //特征生长
@ -338,7 +674,7 @@ void wd_bagThreadPositioning(
} }
scanLines[lineIdx][ptIdx].nPointIdx = 1; scanLines[lineIdx][ptIdx].nPointIdx = 1;
} }
} //显示线头
if (threadTailTreeIdx >= 0) if (threadTailTreeIdx >= 0)
{ {
int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size(); int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
@ -365,9 +701,93 @@ void wd_bagThreadPositioning(
} }
} }
} }
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个） //提取针脚(提取4个）
std::vector<SVzNLRect> stitchROIs; std::vector<SVzNLRect> stitchROIs;
int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size(); 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; int pre_idx = -1;
for (int i = 0; i < nodeSize; i++) for (int i = 0; i < nodeSize; i++)
@ -383,160 +803,158 @@ void wd_bagThreadPositioning(
double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval); double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
if (width > scanInfo.stitchWidth) if (width > scanInfo.stitchWidth)
{ {
SVzNLRect a_stitch; //搜索Z值最高点
memset(&a_stitch, 0, sizeof(SVzNLRect)); int searchStart = pre_node.jumpPos2D.x - startLineIdx;
if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x) 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++)
{ {
a_stitch.left = a_node.jumpPos2D.x; if (baseLinePoints[m].z > 1e-4)
a_stitch.right = pre_node.jumpPos2D.x; {
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 else
{ validStitch.push_back(-1);
a_stitch.left = pre_node.jumpPos2D.x;
a_stitch.right = a_node.jumpPos2D.x;
}
if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y)
{
a_stitch.top = a_node.jumpPos2D.y;
a_stitch.bottom = pre_node.jumpPos2D.y;
} }
else else
{ validStitch.push_back(-1);
a_stitch.top = pre_node.jumpPos2D.y;
a_stitch.bottom = a_node.jumpPos2D.y;
} }
stitchROIs.push_back(a_stitch);
if (stitchROIs.size() >= 4) //以 （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; break;
} }
} }
}
pre_idx = nodeIdx;
}
//提取针脚位置
if (stitchROIs.size() == 0)
{
*errCode = SG_ERR_ZERO_OBJECTS;
return;
}
//建立node下标与扫描线序号的索引
std::vector<int> backIndexing;
int indexingSize = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
backIndexing.resize(indexingSize);
for (int i = 0; i < indexingSize; i++)
backIndexing[i] = -1;
for (int i = 0; i < nodeSize; i++)
{
int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx;
backIndexing[indexingIdx] = i;
}
//搜索下刀位置
SVzNL3DPoint operatePos = {0,0, -1};
if (stitchLineIdx >= 0)
{
int opDist_lines; int opDist_lines;
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描 if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
opDist_lines = (int)(scanInfo.operateDist / lineInterval); opDist_lines = (int)(scanInfo.operateDist / lineInterval);
else else
opDist_lines = (int)(scanInfo.operateDist / ptInterval); opDist_lines = (int)(scanInfo.operateDist / ptInterval);
for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++) int op_centerLine = stitchLineIdx + opDist_lines;
for (int m = 0; m < opDist_lines; m++) //搜索范围也设为opDist_lines
{ {
//搜索Z值最高点 int index_1 = op_centerLine + m;
SVzNLRect& a_stitch = stitchROIs[i]; if (index_1 < totalLines)
SVzNL3DPoint stitchPos = { 0, 0, -1 };
for (int j = a_stitch.left + 1; j < a_stitch.right; j++)
{ {
SVzNL3DPoint linePeak = { 0, 0, -1 }; if (cornerNodeLineIndice[index_1] > 0)
for (int m = a_stitch.top; m <= a_stitch.bottom; m++)
{ {
SVzNL3DPoint a_pt; int nodeIndex = cornerNodeLineIndice[index_1];
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描 operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
a_pt = scanLines[j][m].pt3D; break;
else }
a_pt = scanLines[m][j].pt3D; }
if (a_pt.z > 1e-4) int index_2 = op_centerLine - m;
if (index_2 >= 0)
{ {
if (linePeak.z < 0) if (cornerNodeLineIndice[index_2] > 0)
linePeak = a_pt;
else
{ {
if (linePeak.z > a_pt.z) int nodeIndex = cornerNodeLineIndice[index_2];
linePeak = a_pt; operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
break;
} }
} }
} }
if (linePeak.z > 1e-4) if ( (stitchLineIdx >= 0) && (operatePos.z > 1e-4))
{ {
if (stitchPos.z < 0)
stitchPos = linePeak;
else
{
if (stitchPos.z > linePeak.z)
stitchPos = linePeak;
}
}
}
//搜索下刀位置
if (stitchPos.z > 1e-4)
{
int op_centerLine;
int searchStart, searchEnd;
if (true == scanInfo.scanFromThreadHead)
{
op_centerLine = a_stitch.right + opDist_lines;
searchStart = a_stitch.right;
searchEnd = a_stitch.right + opDist_lines * 2;
if (searchEnd > cornerGrowTrees[objTreeIdx].eLineIdx)
searchEnd = cornerGrowTrees[objTreeIdx].eLineIdx;
}
else
{
op_centerLine = a_stitch.left - opDist_lines;
searchEnd = a_stitch.left;
searchStart = a_stitch.left - opDist_lines * 2;
if (searchStart < cornerGrowTrees[objTreeIdx].sLineIdx)
searchStart = cornerGrowTrees[objTreeIdx].sLineIdx;
}
//寻找最合适点
int best_idx = -1;
int minLineDist = INT_MAX;
for (int j = searchStart; j <= searchEnd; j++)
{
int indexingIdx = j - cornerGrowTrees[objTreeIdx].sLineIdx;
if (backIndexing[indexingIdx] >= 0)
{
int lineDist = j - op_centerLine;
if (lineDist < 0)
lineDist = -lineDist;
if (minLineDist > lineDist)
{
minLineDist = lineDist;
best_idx = backIndexing[indexingIdx];
}
}
}
if (best_idx >= 0)
{
int op_lineIdx, op_ptIdx;
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
{
op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
}
else
{
op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
}
SSX_bagThreadInfo a_stitchInfo; SSX_bagThreadInfo a_stitchInfo;
memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo)); memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
a_stitchInfo.threadPos = stitchPos; a_stitchInfo.threadPos = stitchPos;
a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D; a_stitchInfo.operatePos = operatePos;
a_stitchInfo.rotateAngle = 0; a_stitchInfo.rotateAngle = 0;
bagThreadInfo.push_back(a_stitchInfo); 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; return;
} }
#endif #endif

19
sourceCode/bagThreadPositioning_Export.h
View File

@ -16,20 +16,39 @@ typedef struct
{ {
bool isHorizonScan; bool isHorizonScan;
bool scanFromThreadHead; bool scanFromThreadHead;
double camInstallAngle; //相机安装时与台面夹角
double stitchWidth; //针脚最小宽度，用于过滤虚假的针脚 double stitchWidth; //针脚最小宽度，用于过滤虚假的针脚
double operateDist; //下刀位置距针脚距离 double operateDist; //下刀位置距针脚距离
double mark_height; //标定柱高度
double mark_diameter; //标定柱外径
double mark_distance; //两个标定柱的距离
}SSX_ScanInfo; }SSX_ScanInfo;
//读版本号 //读版本号
SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void); SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void);
//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara wd_bagThread_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
//相机姿态调平，并去除地面
SG_APISHARED_EXPORT void wd_bagThread_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH);
//线头位置检测定位 //线头位置检测定位
SG_APISHARED_EXPORT void wd_bagThreadPositioning( SG_APISHARED_EXPORT void wd_bagThreadPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道 const SSX_ScanInfo scanInfo, //true:激光线平行槽道；false:激光线垂直槽道
const SSG_planeCalibPara groundCalibPara,
const SSG_outlierFilterParam filterParam, //噪点过滤参数 const SSG_outlierFilterParam filterParam, //噪点过滤参数
const SSG_cornerParam cornerPara, //V型特征参数 const SSG_cornerParam cornerPara, //V型特征参数
const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数 const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
const SSG_treeGrowParam growParam, //特征生长参数 const SSG_treeGrowParam growParam, //特征生长参数
std::vector<SSX_bagThreadInfo>& bagThreadInfo, std::vector<SSX_bagThreadInfo>& bagThreadInfo,
std::vector<SSX_bagThreadInfo>& bagThreadInfo_relative, //相对于Mark的坐标
std::vector<SVzNL3DPoint>& output_markCenter, //输出Mark位置信息
int* errCode); int* errCode);