Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib
This commit is contained in:
杰仔
commit
753b3b0b5e
@ -99,6 +99,54 @@ bool checkGridFormat(std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
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)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
@ -252,7 +300,8 @@ void _outputChanneltInfo(char* fileName, std::vector<SSX_bagThreadInfo>& threadI
|
||||
void _outputRGBDScan_RGBD(
|
||||
char* fileName,
|
||||
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();
|
||||
@ -306,6 +355,16 @@ void _outputRGBDScan_RGBD(
|
||||
rgb = { 250, 0, 0 };
|
||||
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)
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
@ -322,8 +381,9 @@ void _outputRGBDScan_RGBD(
|
||||
|
||||
if (objNum > 0)
|
||||
{
|
||||
sw << "Line_" << lineIdx << "_0_" << (objNum*2) << std::endl;
|
||||
size = 8;
|
||||
int markNum = (int)markPoints.size();
|
||||
sw << "Line_" << lineIdx << "_0_" << (objNum*2 + markNum) << std::endl;
|
||||
size = 10;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
rgb = { 250, 0, 0 };
|
||||
@ -342,6 +402,16 @@ void _outputRGBDScan_RGBD(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
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
|
||||
rgb = { 250, 0, 0 };
|
||||
float x = (float)threadInfo[0].threadPos.x;
|
||||
@ -387,34 +457,41 @@ void _outputRGBDScan_RGBD(
|
||||
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define BAG_THREAD_POSITIONING 1
|
||||
#define TEST_GROUP 1
|
||||
#define TEST_GROUP 3
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/", //0
|
||||
"F:/ShangGu/项目/吕宁项目/袋子拆线/现场数据/", //1
|
||||
"F:/ShangGu/项目/吕宁项目/袋子拆线/BJDGS/", //2
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,13},
|
||||
{2,5},
|
||||
{8,9}
|
||||
};
|
||||
|
||||
const char* ver = wd_bagThreadPositioningVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
int convertGrp = 0;
|
||||
for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++)
|
||||
int convertGrp = 2;
|
||||
for (int fidx = 11; fidx <= 11; fidx++)
|
||||
{
|
||||
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;
|
||||
double lineStep = 0.096;
|
||||
double lineStep = 0.032;
|
||||
double baseZ = 350.0;
|
||||
vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData);
|
||||
bool isGridData = checkGridFormat(scanData);
|
||||
int gridFormat = (true == isGridData) ? 1 : 0;
|
||||
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);
|
||||
_outputScanDataFile(_scan_file, scanData,0, 0, 0);
|
||||
@ -423,7 +500,7 @@ int main()
|
||||
#endif
|
||||
|
||||
#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++)
|
||||
{
|
||||
@ -436,10 +513,10 @@ int main()
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 70;
|
||||
cornerParam.scale = 1.5; //袋子缝的尺度1.5mm比较合适
|
||||
cornerParam.cornerTh = 90;
|
||||
cornerParam.scale = 4; //袋子缝的尺度1.5mm比较合适
|
||||
cornerParam.minEndingGap = 1.0; //
|
||||
cornerParam.minEndingGap_z = 1.0;
|
||||
cornerParam.minEndingGap_z = 3.5;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
@ -456,33 +533,57 @@ int main()
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = -1; //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance
|
||||
growParam.maxSkipDistance = 5.0;
|
||||
growParam.yDeviation_max = 5.0;
|
||||
growParam.zDeviation_max = 5.0;//
|
||||
growParam.yDeviation_max = 1.0;
|
||||
growParam.zDeviation_max = 1.0;//
|
||||
growParam.minLTypeTreeLen = 10; //mm,
|
||||
growParam.minVTypeTreeLen = 10; //mm
|
||||
|
||||
SSX_ScanInfo scanInfo;
|
||||
scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝
|
||||
scanInfo.scanFromThreadHead = false; //true:线袋子线缝头部开始扫描。
|
||||
scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。
|
||||
scanInfo.stitchWidth = 1.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;
|
||||
std::vector<SSX_bagThreadInfo> bagThreadInfo;
|
||||
std::vector<SSX_bagThreadInfo> bagThreadInfo_relative; //相对于Mark的坐标
|
||||
std::vector<SVzNL3DPoint> output_markCenter;
|
||||
wd_bagThreadPositioning(
|
||||
scanLines,
|
||||
scanInfo, //true:激光线平行线缝；false:激光线垂直线缝
|
||||
poseCalibPara,
|
||||
filterParam, //噪点过滤参数
|
||||
cornerParam, //V型特征参数
|
||||
raisedFeatureParam,//线尾凸起参数
|
||||
growParam, //特征生长参数
|
||||
bagThreadInfo,
|
||||
bagThreadInfo_relative,
|
||||
output_markCenter,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
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);
|
||||
_outputChanneltInfo(_scan_file, bagThreadInfo);
|
||||
}
|
||||
|
||||
@ -169,6 +169,7 @@
|
||||
<ClInclude Include="..\sourceCode\SG_errCode.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\dataFitting.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_baseFunc.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_clustering.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />
|
||||
|
||||
@ -192,6 +192,14 @@ SG_APISHARED_EXPORT void wd_getLineRaisedFeature(
|
||||
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法作了简化，以起点终点直线代替拟合直线
|
||||
SG_APISHARED_EXPORT void wd_surfaceLineSegment(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
@ -502,6 +510,15 @@ SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
|
||||
//计算角度差值，在0-180度范围
|
||||
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(
|
||||
double a1, double b1, double c1,
|
||||
@ -686,6 +703,15 @@ SG_APISHARED_EXPORT void wd_gridPointClustering_labelling(
|
||||
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平面上的投影量化，并对量化产生的空白点进行插值
|
||||
SG_APISHARED_EXPORT void pointClout2DProjection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
|
||||
@ -700,16 +726,15 @@ SG_APISHARED_EXPORT void pointClout2DProjection(
|
||||
);
|
||||
|
||||
//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
|
||||
SG_APISHARED_EXPORT void pointClout2DQuantization(
|
||||
SG_APISHARED_EXPORT void pointCloud2DQuantization(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
|
||||
SVzNLRangeD x_range,
|
||||
SVzNLRangeD y_range,
|
||||
double scale,
|
||||
double cuttingGrndZ,
|
||||
int edgeSkip,
|
||||
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||||
std::vector<std::vector<double>> quantiData, //量化数据，初始化为一个极大值1e+6
|
||||
std::vector<std::vector<SVzNL2DPoint>> backIndexing //标记坐标索引，用于回找3D坐标
|
||||
std::vector<std::vector<SVzNL3DPoint>>& quantiData, //量化数据，初始化为一个极大值1e+6
|
||||
std::vector<std::vector<SVzNL2DPoint>>& backIndexing //标记坐标索引，用于回找3D坐标
|
||||
);
|
||||
|
||||
//分水岭算法
|
||||
|
||||
@ -39,6 +39,12 @@ typedef struct
|
||||
SWD3DPoint point;
|
||||
}SWDIndexing3DPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int start;
|
||||
int end;
|
||||
}SWD_Interval; //Çø¼ä
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool validFlag; //指示结果是否有效
|
||||
@ -55,6 +61,12 @@ typedef enum
|
||||
keSG_PoseSorting_T2B_L2R, ///从上到下，从左到右排序
|
||||
} ESG_poseSortingMode;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
KeWD_Mask_ValidPt = 0,
|
||||
KeWD_Mask_NullPt,
|
||||
}EWD_maskMode;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int data_0;
|
||||
@ -62,6 +74,13 @@ typedef struct
|
||||
int idx;
|
||||
}SSG_intPair;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int flag;
|
||||
int validFlag;
|
||||
int clusterID;
|
||||
}SSG_clusterLabel;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int featurType;
|
||||
|
||||
@ -9,6 +9,8 @@
|
||||
#include <unordered_map>
|
||||
#include <Eigen/dense>
|
||||
|
||||
const double EPS = 1e-10;
|
||||
|
||||
SVzNL3DRangeD sg_getScanDataROI(
|
||||
//计算扫描ROI
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
@ -276,277 +278,6 @@ SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition
|
||||
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均值
|
||||
double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
|
||||
{
|
||||
@ -653,6 +384,67 @@ double computeXOYVertexAngle(SVzNL3DPoint p0, SVzNL3DPoint p1, SVzNL3DPoint p2)
|
||||
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 tmp = sqrt(pow(a, 2) + pow(b, 2));
|
||||
@ -1193,54 +985,6 @@ void SG_TwoPassLabel(
|
||||
}
|
||||
#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顺序）
|
||||
SSG_EulerAngles planeNormalToEuler(double A, double B, double C) {
|
||||
@ -3063,18 +2807,16 @@ void pointClout2DProjection(
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
|
||||
void pointClout2DQuantization(
|
||||
void pointCloud2DQuantization(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
|
||||
SVzNLRangeD x_range,
|
||||
SVzNLRangeD y_range,
|
||||
double scale,
|
||||
double cuttingGrndZ,
|
||||
int edgeSkip,
|
||||
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||||
std::vector<std::vector<double>> quantiData, //量化数据，初始化为一个极大值1e+6
|
||||
std::vector<std::vector<SVzNL2DPoint>> backIndexing //标记坐标索引，用于回找3D坐标
|
||||
std::vector<std::vector<SVzNL3DPoint>>& quantiData, //量化数据，初始化为一个极大值1e+6
|
||||
std::vector<std::vector<SVzNL2DPoint>>& backIndexing //标记坐标索引，用于回找3D坐标
|
||||
)
|
||||
{
|
||||
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;
|
||||
quantiData.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++)
|
||||
{
|
||||
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);
|
||||
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();
|
||||
@ -3104,8 +2849,6 @@ void pointClout2DQuantization(
|
||||
SVzNL3DPosition* pt3D = &gridScanData[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
if ((cuttingGrndZ > 0) && (pt3D->pt3D.z > cuttingGrndZ))
|
||||
continue;
|
||||
double x = pt3D->pt3D.x;
|
||||
double y = pt3D->pt3D.y;
|
||||
int px = (int)(x - x_range.min) / scale + edgeSkip;
|
||||
@ -3122,7 +2865,7 @@ void pointClout2DQuantization(
|
||||
{
|
||||
SVzNL2DPoint v2i = { line, i };
|
||||
backIndexing[px][px] = v2i;
|
||||
quantiData[px][py] = pt3D->pt3D.z;
|
||||
quantiData[px][py].z = pt3D->pt3D.z;
|
||||
//垂直插值
|
||||
if (prePt)
|
||||
{
|
||||
@ -3148,7 +2891,7 @@ void pointClout2DQuantization(
|
||||
k2 = 1.0 - k1;
|
||||
}
|
||||
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;
|
||||
for (int x = 0; x < cols; x++)
|
||||
{
|
||||
double value = quantiData[x][y];
|
||||
double value = quantiData[x][y].z;
|
||||
if (value > 1e-4)
|
||||
{
|
||||
if (pre_x >= 0)
|
||||
@ -3182,7 +2925,7 @@ void pointClout2DQuantization(
|
||||
double k1 = ((double)(m - pre_x)) / ((double)x_diff);
|
||||
double k2 = 1.0 - k1;
|
||||
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库实现
|
||||
@ -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
|
||||
#include <iostream>
|
||||
|
||||
@ -282,14 +282,14 @@ void wd_gridPointClustering_labelling(
|
||||
}
|
||||
return;
|
||||
}
|
||||
#if 0
|
||||
|
||||
//使用聚类方法完成8连通连通域分析
|
||||
void wd_gridPointClustering_labelling_2(
|
||||
std::vector<std::vector<double>>& srcData,
|
||||
std::vector<std::vector<SSG_intPair>>& labelMask,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& srcData,
|
||||
std::vector<std::vector<SSG_clusterLabel>>& labelMask,
|
||||
int clusterID, //当前Cluster的ID
|
||||
std::vector< SVzNL2DPoint>& a_cluster, //result
|
||||
SVzNLRect& clusterRoi //roi2D
|
||||
SVzNL3DRangeD& clusterRoi //roi3D
|
||||
)
|
||||
{
|
||||
int i = 0;
|
||||
@ -304,7 +304,8 @@ void wd_gridPointClustering_labelling_2(
|
||||
if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
|
||||
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, 未被处理，搜索邻域
|
||||
{
|
||||
//8连通
|
||||
@ -319,8 +320,8 @@ void wd_gridPointClustering_labelling_2(
|
||||
|
||||
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
|
||||
{
|
||||
SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
|
||||
if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0)) //Ö»¼ì²éδ¾ÛÀàµÄÌØÕ÷µã
|
||||
SSG_clusterLabel& chk_seed = labelMask[x][y];
|
||||
if ((chk_seed.validFlag == 0) || (chk_seed.clusterID > 0)) //Ö»¼ì²éδ¾ÛÀàµÄÌØÕ÷µã
|
||||
continue;
|
||||
|
||||
if (0 == chk_seed.flag)//防止被重复添加
|
||||
@ -336,12 +337,13 @@ void wd_gridPointClustering_labelling_2(
|
||||
}
|
||||
a_seed.clusterID = clusterID;
|
||||
//更新ROI
|
||||
clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left;
|
||||
clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right;
|
||||
clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top;
|
||||
clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom;
|
||||
clusterRoi.xRange.min = clusterRoi.xRange.min > a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.min;
|
||||
clusterRoi.xRange.max = clusterRoi.xRange.max < a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.max;
|
||||
clusterRoi.yRange.min = clusterRoi.yRange.min > a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.min;
|
||||
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++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@ -8,6 +8,8 @@
|
||||
#define SG_ERR_INVLD_SORTING_MODE -1005
|
||||
#define SG_ERR_INVLD_Q_SCALE -1006
|
||||
#define SG_ERR_ZERO_OBJECTS -1007
|
||||
#define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008
|
||||
#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009
|
||||
|
||||
//BQ_workpiece
|
||||
#define SX_ERR_INVLD_VTREE_NUM -2001
|
||||
@ -21,7 +23,8 @@
|
||||
|
||||
//定子抓取
|
||||
#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
|
||||
@ -29,3 +32,6 @@
|
||||
//汽车轮眉高度测量
|
||||
#define SX_ERR_INVALID_ARC -2301
|
||||
|
||||
//ÌÇ´ü×Ó²ðÏß
|
||||
#define SX_ERR_NO_MARK -2401
|
||||
|
||||
|
||||
@ -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(
|
||||
SSG_RUN a_run,
|
||||
|
||||
@ -6,21 +6,132 @@
|
||||
#include <limits>
|
||||
|
||||
//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)
|
||||
{
|
||||
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;
|
||||
@ -30,6 +141,16 @@ void wd_bagThreadPositioning(
|
||||
*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数据格式
|
||||
@ -48,6 +169,13 @@ void wd_bagThreadPositioning(
|
||||
return;
|
||||
}
|
||||
|
||||
//调平。
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{ //行处理
|
||||
//调平，去除地面
|
||||
wd_bagThread_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||||
}
|
||||
|
||||
//生成水平扫描数据
|
||||
//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
|
||||
double ptInterval = 0;
|
||||
@ -106,16 +234,25 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
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)
|
||||
//现场要求垂直扫描
|
||||
//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 == 577) || (line == 932))
|
||||
if ((line == 750) || (line == 905))
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
//滤波，滤除异常点
|
||||
@ -163,67 +300,266 @@ void wd_bagThreadPositioning(
|
||||
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
|
||||
//水平扫描检测袋子边
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpdFeatures;
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
int validVCornerSCale = (int)(vCornerScale / lineInterval);
|
||||
//水平扫描检测V型槽和线尾
|
||||
#if 1
|
||||
//水平扫描检测标定柱
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
if (line == 329)
|
||||
if ((line == 750) || (line == 905))
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
|
||||
//滤波，滤除异常点
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
|
||||
//提取V型槽
|
||||
std::vector<SSG_basicFeature1D> line_cornerFeatures;
|
||||
std::vector<SSG_RUN_EX> segs;
|
||||
int dataSize = (int)lineData.size();
|
||||
wd_getLineCorerFeature_accelerate(
|
||||
//提取标定柱跳变特征
|
||||
std::vector<SSG_basicFeature1D> line_jumpFearures;
|
||||
wd_getSpecifiedHeightJumping(
|
||||
lineData,
|
||||
line,
|
||||
cornerPara,
|
||||
lineInterval,
|
||||
segs,
|
||||
line_cornerFeatures //拐点
|
||||
jumpHeight, //跳变高度参数
|
||||
line_jumpFearures //跳变
|
||||
);
|
||||
//检查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++)
|
||||
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++)
|
||||
{
|
||||
int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
|
||||
for (int n = 0; n < segSize; n++)
|
||||
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) //不重复添加
|
||||
{
|
||||
int segEnd = segs[n].start + segs[n].len - 1;
|
||||
if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
|
||||
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 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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
cornerFeatures.push_back(valid_cornerFeatures);
|
||||
//提取凸起段
|
||||
std::vector<SWD_segFeature> line_raisedFeatures;
|
||||
wd_getLineRaisedFeature(
|
||||
lineData,
|
||||
line,
|
||||
raisedFeaturePara, //凸起参数
|
||||
line_raisedFeatures //凸起
|
||||
);
|
||||
raisedFeatures.push_back(line_raisedFeatures);
|
||||
//圆拟合计算圆心，以点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,36 +674,120 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
}
|
||||
}
|
||||
if (threadTailTreeIdx >= 0)
|
||||
{
|
||||
int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
|
||||
for (int j = 0; j < nodeNum; j++)
|
||||
//显示线头
|
||||
if (threadTailTreeIdx >= 0)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == scanInfo.isHorizonScan)
|
||||
int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
|
||||
for (int j = 0; j < nodeNum; j++)
|
||||
{
|
||||
lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
|
||||
for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
|
||||
int lineIdx, ptIdx;
|
||||
if (false == scanInfo.isHorizonScan)
|
||||
{
|
||||
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;
|
||||
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++)
|
||||
@ -383,160 +803,158 @@ void wd_bagThreadPositioning(
|
||||
double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
|
||||
if (width > scanInfo.stitchWidth)
|
||||
{
|
||||
SVzNLRect a_stitch;
|
||||
memset(&a_stitch, 0, sizeof(SVzNLRect));
|
||||
if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x)
|
||||
//搜索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++)
|
||||
{
|
||||
a_stitch.left = a_node.jumpPos2D.x;
|
||||
a_stitch.right = pre_node.jumpPos2D.x;
|
||||
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);
|
||||
}
|
||||
else
|
||||
|
||||
//以 （sticthPeak+0.5)为门限，取中间点
|
||||
double stitchPeakTh = sticthPeak + 0.5;
|
||||
int validStart = -1, validEnd = 0;
|
||||
for (int n = 0; n < (int)validStitch.size(); n++)
|
||||
{
|
||||
a_stitch.left = pre_node.jumpPos2D.x;
|
||||
a_stitch.right = a_node.jumpPos2D.x;
|
||||
if ((validStitch[n] > 1e-4) &&(validStitch[n] < stitchPeakTh))
|
||||
{
|
||||
if (validStart < 0)
|
||||
validStart = n;
|
||||
validEnd = n;
|
||||
}
|
||||
}
|
||||
if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y)
|
||||
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
|
||||
{
|
||||
a_stitch.top = a_node.jumpPos2D.y;
|
||||
a_stitch.bottom = pre_node.jumpPos2D.y;
|
||||
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;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
//搜索下刀位置
|
||||
SVzNL3DPoint operatePos = {0,0, -1};
|
||||
if (stitchLineIdx >= 0)
|
||||
{
|
||||
a_stitch.top = pre_node.jumpPos2D.y;
|
||||
a_stitch.bottom = a_node.jumpPos2D.y;
|
||||
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;
|
||||
}
|
||||
}
|
||||
stitchROIs.push_back(a_stitch);
|
||||
if (stitchROIs.size() >= 4)
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
pre_idx = nodeIdx;
|
||||
}
|
||||
//提取针脚位置
|
||||
if (stitchROIs.size() == 0)
|
||||
{
|
||||
//输出
|
||||
if (bagThreadInfo.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++)
|
||||
|
||||
//计算为相对坐标
|
||||
double cosTheta = cos(markRotateAngle);
|
||||
double sinTheta = sin(markRotateAngle);
|
||||
bagThreadInfo_relative.resize(bagThreadInfo.size());
|
||||
for (int i = 0; i < (int)bagThreadInfo.size(); i++)
|
||||
{
|
||||
int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx;
|
||||
backIndexing[indexingIdx] = 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;
|
||||
}
|
||||
|
||||
int opDist_lines;
|
||||
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
|
||||
opDist_lines = (int)(scanInfo.operateDist / lineInterval);
|
||||
else
|
||||
opDist_lines = (int)(scanInfo.operateDist / ptInterval);
|
||||
|
||||
for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++)
|
||||
{
|
||||
//搜索Z值最高点
|
||||
SVzNLRect& a_stitch = stitchROIs[i];
|
||||
SVzNL3DPoint stitchPos = { 0, 0, -1 };
|
||||
for (int j = a_stitch.left + 1; j < a_stitch.right; j++)
|
||||
{
|
||||
SVzNL3DPoint linePeak = { 0, 0, -1 };
|
||||
for (int m = a_stitch.top; m <= a_stitch.bottom; m++)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
if (false == scanInfo.isHorizonScan) //垂直于线缝扫描
|
||||
a_pt = scanLines[j][m].pt3D;
|
||||
else
|
||||
a_pt = scanLines[m][j].pt3D;
|
||||
if (a_pt.z > 1e-4)
|
||||
{
|
||||
if (linePeak.z < 0)
|
||||
linePeak = a_pt;
|
||||
else
|
||||
{
|
||||
if (linePeak.z > a_pt.z)
|
||||
linePeak = a_pt;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (linePeak.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;
|
||||
memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
|
||||
a_stitchInfo.threadPos = stitchPos;
|
||||
a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D;
|
||||
a_stitchInfo.rotateAngle = 0;
|
||||
bagThreadInfo.push_back(a_stitchInfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -16,20 +16,39 @@ typedef struct
|
||||
{
|
||||
bool isHorizonScan;
|
||||
bool scanFromThreadHead;
|
||||
double camInstallAngle; //相机安装时与台面夹角
|
||||
double stitchWidth; //针脚最小宽度，用于过滤虚假的针脚
|
||||
double operateDist; //下刀位置距针脚距离
|
||||
double mark_height; //标定柱高度
|
||||
double mark_diameter; //标定柱外径
|
||||
double mark_distance; //两个标定柱的距离
|
||||
}SSX_ScanInfo;
|
||||
|
||||
//读版本号
|
||||
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(
|
||||
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);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user